Biomechanical Joint Model  Author: Anderson Maciel

---

comematrix.cpp

Go to the documentation of this file.

/***************************************************************************
 *   Copyright (C) 2002 by Anderson Maciel                                 *
 *   andi.maciel@gmail.com                                                 *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 **************************************************************************/
#include    <algebra/comematrix.h>
#include        <algebra/comevector3d.h>
#include        <algebra/comevertex3d.h>
#include    <math.h>


COME_Matrix::COME_Matrix( void ){

        // set the identity matrix
        data[0][1] = data[0][2] = data[0][3] =
        data[1][0] = data[1][2] = data[1][3] =
        data[2][0] = data[2][1] = data[2][3] =
        data[3][0] = data[3][1] = data[3][2] = 0.0;
        data[0][0] = data[1][1] = data[2][2] = data[3][3] = 1.0;

}

COME_Matrix::COME_Matrix( double fLines[] ){

        for( int l = 0; l < 4; l++ ){
                for( int c = 0; c < 4; c++ ){
                        data[l][c] = fLines[l*4+c];
                }
        }
}

COME_Matrix::COME_Matrix( double fm44[][4] ){

    for( int l = 0; l < 4; l++ ){
        for( int c = 0; c < 4; c++ ){
                data[l][c] = fm44[l][c];
        }
    }
}


COME_Matrix::~COME_Matrix( void ){
        // do nothing
}

double
COME_Matrix::getValueAt( int iL, int iC ) const{

        return data[iL][iC];
}

void
COME_Matrix::setValueAt( int iL, int iC, double fValue ){

        data[iL][iC] = fValue;
}

void
COME_Matrix::setMatrix( const COME_Matrix& matrixN ){

        for( int l = 0; l < 4; l++ ){
        for( int c = 0; c < 4; c++ ){
                data[l][c] = matrixN.getValueAt( l, c );
        }
    }
}

const double**
COME_Matrix::getMatrixD( void ){

        double **M;
        M = new double*[4];

  for( int l = 0; l < 4; l++ ){
        M[l] = new double[4];
        for( int c = 0; c < 4; c++ ){
        M[l][c] = data[c][l];
    }
  }
  return (const double**) M;

}

void
COME_Matrix::getMatrixD( double **M ){
        
  for( int l = 0; l < 4; l++ ){
        for( int c = 0; c < 4; c++ ){
        M[l][c] = data[c][l];
    }
  }


}

double*
COME_Matrix::getMatrixVD( void ){

        double *M = new double[16];

        for( int l = 0; l < 4; l++ ){
                for( int c = 0; c < 4; c++ ){
                        M[l*4+c] = data[l][c];
                }
        }
        return M;
}


// Calculate inverse matrix
//
//   -1

//  A  =____1__ adjoint A
//       det A
//
COME_Matrix
COME_Matrix :: getInverse( void ){

        COME_Matrix Inv;
        double fDet;

        // calculate the adjoint matrix
        Inv = getAdjoint();

        // calculate the 4x4 determinent
        // if the determinent is zero, then the inverse matrix is not unique
        fDet = Inv.getDeterminent();
        if (fabs(fDet) < 0.0001){
                printf("Non-singular matrix, no inverse!");
                return Inv;
        }
        // scale the adjoint matrix to get the inverse
        Inv = Inv * ( 1/fDet );
        
        return Inv;
}

// Make the transposed matrix by changing columns and rows
COME_Matrix
COME_Matrix :: getTransposed( void ){

        double result [4][4];

        for( int l = 0; l < 4; l++ ){
                for( int c = 0; c < 4; c++ ){
                        result[l][c] = data[c][l];
                }
        }
        COME_Matrix trans( result );
        
        return trans;
}


// calculate the adjoint of a 4x4 matrix
//
// Let a  denote the minor determinant of matrix A obtained by
//     ij
// deleting the ith row and jth column from A.
//             i+j
// let b  = (-1)   a
//     ij          ji
// The matrix B = (b  ) is the adjoint of A
//                  ij

COME_Matrix
COME_Matrix::getAdjoint(){

        COME_Matrix     A;
        double          a1, a2, a3, a4, b1, b2, b3, b4,
                        c1, c2, c3, c4, d1, d2, d3, d4;

        // Assign to individual variable names to aid selecting correct values
        a1 = data[0][0]; b1 = data[0][1];
        c1 = data[0][2]; d1 = data[0][3];

        a2 = data[1][0]; b2 = data[1][1];
        c2 = data[1][2]; d2 = data[1][3];

        a3 = data[2][0]; b3 = data[2][1];
        c3 = data[2][2]; d3 = data[2][3];

        a4 = data[3][0]; b4 = data[3][1];
        c4 = data[3][2]; d4 = data[3][3];

        // Row column labeling reversed since we transpose rows & columns
        A.setValueAt( 0, 0,  getDet33(b2, b3, b4, c2, c3, c4, d2, d3, d4) );
        A.setValueAt( 1, 0, -getDet33(a2, a3, a4, c2, c3, c4, d2, d3, d4) );
        A.setValueAt( 2, 0,  getDet33(a2, a3, a4, b2, b3, b4, d2, d3, d4) );
        A.setValueAt( 3, 0, -getDet33(a2, a3, a4, b2, b3, b4, c2, c3, c4) );

        A.setValueAt( 0, 1, -getDet33(b1, b3, b4, c1, c3, c4, d1, d3, d4) );
        A.setValueAt( 1, 1,  getDet33(a1, a3, a4, c1, c3, c4, d1, d3, d4) );
        A.setValueAt( 2, 1, -getDet33(a1, a3, a4, b1, b3, b4, d1, d3, d4) );
        A.setValueAt( 3, 1,  getDet33(a1, a3, a4, b1, b3, b4, c1, c3, c4) );

        A.setValueAt( 0, 2,  getDet33(b1, b2, b4, c1, c2, c4, d1, d2, d4) );
        A.setValueAt( 1, 2, -getDet33(a1, a2, a4, c1, c2, c4, d1, d2, d4) );
        A.setValueAt( 2, 2,  getDet33(a1, a2, a4, b1, b2, b4, d1, d2, d4) );
        A.setValueAt( 3, 2, -getDet33(a1, a2, a4, b1, b2, b4, c1, c2, c4) );

        A.setValueAt( 0, 3, -getDet33(b1, b2, b3, c1, c2, c3, d1, d2, d3) );
        A.setValueAt( 1, 3,  getDet33(a1, a2, a3, c1, c2, c3, d1, d2, d3) );
        A.setValueAt( 2, 3, -getDet33(a1, a2, a3, b1, b2, b3, d1, d2, d3) );
        A.setValueAt( 3, 3,  getDet33(a1, a2, a3, b1, b2, b3, c1, c2, c3) );

        return A;
}


// calculate the determinent of a 4x4 matrix
double
COME_Matrix::getDeterminent( void ){

        double ans, a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;


        // assign to individual variable names to aid selecting correct elements
        a1 = data[0][0]; b1 = data[0][1];
        c1 = data[0][2]; d1 = data[0][3];

        a2 = data[1][0]; b2 = data[1][1];
        c2 = data[1][2]; d2 = data[1][3];

        a3 = data[2][0]; b3 = data[2][1];
        c3 = data[2][2]; d3 = data[2][3];

        a4 = data[3][0]; b4 = data[3][1];
        c4 = data[3][2]; d4 = data[3][3];

        ans =   a1 * getDet33(b2, b3, b4, c2, c3, c4, d2, d3, d4) -
                b1 * getDet33(a2, a3, a4, c2, c3, c4, d2, d3, d4) +
                c1 * getDet33(a2, a3, a4, b2, b3, b4, d2, d3, d4) -
                d1 * getDet33(a2, a3, a4, b2, b3, b4, c2, c3, c4);

        return ans;
}

//
// calcule the determinent of a 3x3 matrix in the form
//
//      | a1, b1, c1 |
//      | a2, b2, c2 |
//      | a3, b3, c3 |
//
double
COME_Matrix::getDet33(double a1, double a2, double a3, double b1, double b2, double b3,
double c1, double c2, double c3){

        double ans;
        double aux1, aux2, aux3;

        aux1 = getDet22(b2, b3, c2, c3);
        aux2 = getDet22(a2, a3, c2, c3);
        aux3 = getDet22(a2, a3, b2, b3);
        ans = a1 * aux1 - b1 * aux2 + c1 * aux3;

        return ans;
}


//
// calculate the determinent of a 2x2 matrix
//
double
COME_Matrix::getDet22(double a, double b, double c, double d){

        return (a * d - b * c);
}


// Return the result of x.T
//
COME_Matrix*
COME_Matrix::multiplyScalar( double x ){

        COME_Matrix *M = new COME_Matrix();

        for( int l = 0; l < 4; l++ ){
                for( int c = 0; c < 4; c++ ){
                        M->setValueAt( l, c, x * data[l][c] );
                }
        }
        return M;
}

// Return the result of T.B
// POTENTIAL BUG: Allocating matrix that can be never released
//
COME_Matrix*
COME_Matrix::multiply( COME_Matrix B ){

        COME_Matrix *M = new COME_Matrix();

        M->setValueAt( 0, 0,    data[0][0]*B.getValueAt( 0, 0 )+
                                data[0][1]*B.getValueAt( 1, 0 )+
                                data[0][2]*B.getValueAt( 2, 0 )+
                                data[0][3]*B.getValueAt( 3, 0 ) );
        M->setValueAt( 0, 1,    data[0][0]*B.getValueAt( 0, 1 )+
                                data[0][1]*B.getValueAt( 1, 1 )+
                                data[0][2]*B.getValueAt( 2, 1 )+
                                data[0][3]*B.getValueAt( 3, 1 ) );
        M->setValueAt( 0, 2,    data[0][0]*B.getValueAt( 0, 2 )+
                                data[0][1]*B.getValueAt( 1, 2 )+
                                data[0][2]*B.getValueAt( 2, 2 )+
                                data[0][3]*B.getValueAt( 3, 2 ) );
        M->setValueAt( 0, 3,    data[0][0]*B.getValueAt( 0, 3 )+
                                data[0][1]*B.getValueAt( 1, 3 )+
                                data[0][2]*B.getValueAt( 2, 3 )+
                                data[0][3]*B.getValueAt( 3, 3 ) );

        M->setValueAt( 1, 0,    data[1][0]*B.getValueAt( 0, 0 )+
                                data[1][1]*B.getValueAt( 1, 0 )+
                                data[1][2]*B.getValueAt( 2, 0 )+
                                data[1][3]*B.getValueAt( 3, 0 ) );
        M->setValueAt( 1, 1,    data[1][0]*B.getValueAt( 0, 1 )+
                                data[1][1]*B.getValueAt( 1, 1 )+
                                data[1][2]*B.getValueAt( 2, 1 )+
                                data[1][3]*B.getValueAt( 3, 1 ) );
        M->setValueAt( 1, 2,    data[1][0]*B.getValueAt( 0, 2 )+
                                data[1][1]*B.getValueAt( 1, 2 )+
                                data[1][2]*B.getValueAt( 2, 2 )+
                                data[1][3]*B.getValueAt( 3, 2 ) );
        M->setValueAt( 1, 3,    data[1][0]*B.getValueAt( 0, 3 )+
                                data[1][1]*B.getValueAt( 1, 3 )+
                                data[1][2]*B.getValueAt( 2, 3 )+
                                data[1][3]*B.getValueAt( 3, 3 ) );

        M->setValueAt( 2, 0,    data[2][0]*B.getValueAt( 0, 0 )+
                                data[2][1]*B.getValueAt( 1, 0 )+
                                data[2][2]*B.getValueAt( 2, 0 )+
                                data[2][3]*B.getValueAt( 3, 0 ) );
        M->setValueAt( 2, 1,    data[2][0]*B.getValueAt( 0, 1 )+
                                data[2][1]*B.getValueAt( 1, 1 )+
                                data[2][2]*B.getValueAt( 2, 1 )+
                                data[2][3]*B.getValueAt( 3, 1 ) );
        M->setValueAt( 2, 2,    data[2][0]*B.getValueAt( 0, 2 )+
                                data[2][1]*B.getValueAt( 1, 2 )+
                                data[2][2]*B.getValueAt( 2, 2 )+
                                data[2][3]*B.getValueAt( 3, 2 ) );
        M->setValueAt( 2, 3,    data[2][0]*B.getValueAt( 0, 3 )+
                                data[2][1]*B.getValueAt( 1, 3 )+
                                data[2][2]*B.getValueAt( 2, 3 )+
                                data[2][3]*B.getValueAt( 3, 3 ) );

        M->setValueAt( 3, 0,    data[3][0]*B.getValueAt( 0, 0 )+
                                data[3][1]*B.getValueAt( 1, 0 )+
                                data[3][2]*B.getValueAt( 2, 0 )+
                                data[3][3]*B.getValueAt( 3, 0 ) );
        M->setValueAt( 3, 1,    data[3][0]*B.getValueAt( 0, 1 )+
                                data[3][1]*B.getValueAt( 1, 1 )+
                                data[3][2]*B.getValueAt( 2, 1 )+
                                data[3][3]*B.getValueAt( 3, 1 ) );
        M->setValueAt( 3, 2,    data[3][0]*B.getValueAt( 0, 2 )+
                                data[3][1]*B.getValueAt( 1, 2 )+
                                data[3][2]*B.getValueAt( 2, 2 )+
                                data[3][3]*B.getValueAt( 3, 2 ) );
        M->setValueAt( 3, 3,    data[3][0]*B.getValueAt( 0, 3 )+
                                data[3][1]*B.getValueAt( 1, 3 )+
                                data[3][2]*B.getValueAt( 2, 3 )+
                                data[3][3]*B.getValueAt( 3, 3 ) );

        return M;
}

COME_Matrix*
COME_Matrix::subtract( COME_Matrix B ){

        COME_Matrix *M = new COME_Matrix();

        for( int l = 0; l < 4; l++ ){
                for( int c = 0; c < 4; c++ ){
                        M->setValueAt( l, c,
                                        data[l][c] - B.getValueAt( l, c ) );
                }
        }
        return M;
}

COME_Matrix*
COME_Matrix::add( COME_Matrix B ){

        COME_Matrix *M = new COME_Matrix();

        for( int l = 0; l < 4; l++ ){
                for( int c = 0; c < 4; c++ ){
                        M->setValueAt( l, c,
                                        data[l][c] + B.getValueAt( l, c ) );
                }
        }
        return M;
}

COME_Point3D*
COME_Matrix::multiply( COME_Point3D p ){

        COME_Point3D *temp = new COME_Point3D();
        double x,y,z,w;

        w =     data[3][0] * p.getX() + data[3][1] * p.getY() +
                data[3][2] * p.getZ() + data[3][3] * 1;
        x =     (data[0][0] * p.getX() + data[0][1] * p.getY() +
                data[0][2] * p.getZ() + data[0][3] * 1)/w;
        y =     (data[1][0] * p.getX() + data[1][1] * p.getY() +
                data[1][2] * p.getZ() + data[1][3] * 1)/w;
        z =     (data[2][0] * p.getX() + data[2][1] * p.getY() +
                data[2][2] * p.getZ() + data[2][3] * 1)/w;

        temp->setXYZ(x,y,z);

        return temp;
}

void
COME_Matrix::reset(){

        for( int l = 0; l < 4; l++ ){
                for( int c = 0; c < 4; c++ ){
                        data[l][c] = 0.0;
                }
        }
}

COME_Matrix&
COME_Matrix :: operator+= ( const COME_Matrix& mO ) {

        for( int l = 0; l < 4; l++ ){
        for( int c = 0; c < 4; c++ ){
                data[l][c] += mO.getValueAt( l, c );
        }
    }
        return *this ;
}

COME_Matrix
COME_Matrix :: operator+ ( const COME_Matrix& mO ) {

        double result [4][4];

        for( int l = 0; l < 4; l++ ){
        for( int c = 0; c < 4; c++ ){
                result[l][c] = data[l][c] + mO.getValueAt( l, c );
        }
    }
        COME_Matrix resultM( result );
        return resultM;
}

COME_Matrix
COME_Matrix :: operator* ( double scalar ) {

        double result [4][4];

        for( int l = 0; l < 4; l++ ){
        for( int c = 0; c < 4; c++ ){
                result[l][c] = data[l][c] * scalar;
        }
    }
        COME_Matrix resultM( result );
        return resultM;
}

COME_Matrix
COME_Matrix :: operator* ( const COME_Matrix& mO ) {

        
        double result [4][4];

        result[0][0] =  data[0][0]*mO.getValueAt( 0, 0 )+
                                data[0][1]*mO.getValueAt( 1, 0 )+
                                data[0][2]*mO.getValueAt( 2, 0 )+
                                data[0][3]*mO.getValueAt( 3, 0 );
        result[0][1] =  data[0][0]*mO.getValueAt( 0, 1 )+
                                data[0][1]*mO.getValueAt( 1, 1 )+
                                data[0][2]*mO.getValueAt( 2, 1 )+
                                data[0][3]*mO.getValueAt( 3, 1 );
        result[0][2] =  data[0][0]*mO.getValueAt( 0, 2 )+
                                data[0][1]*mO.getValueAt( 1, 2 )+
                                data[0][2]*mO.getValueAt( 2, 2 )+
                                data[0][3]*mO.getValueAt( 3, 2 );
        result[0][3] =  data[0][0]*mO.getValueAt( 0, 3 )+
                                data[0][1]*mO.getValueAt( 1, 3 )+
                                data[0][2]*mO.getValueAt( 2, 3 )+
                                data[0][3]*mO.getValueAt( 3, 3 );

        result[1][0] =  data[1][0]*mO.getValueAt( 0, 0 )+
                                data[1][1]*mO.getValueAt( 1, 0 )+
                                data[1][2]*mO.getValueAt( 2, 0 )+
                                data[1][3]*mO.getValueAt( 3, 0 );
        result[1][1] =  data[1][0]*mO.getValueAt( 0, 1 )+
                                data[1][1]*mO.getValueAt( 1, 1 )+
                                data[1][2]*mO.getValueAt( 2, 1 )+
                                data[1][3]*mO.getValueAt( 3, 1 );
        result[1][2] =  data[1][0]*mO.getValueAt( 0, 2 )+
                                data[1][1]*mO.getValueAt( 1, 2 )+
                                data[1][2]*mO.getValueAt( 2, 2 )+
                                data[1][3]*mO.getValueAt( 3, 2 );
        result[1][3] =  data[1][0]*mO.getValueAt( 0, 3 )+
                                data[1][1]*mO.getValueAt( 1, 3 )+
                                data[1][2]*mO.getValueAt( 2, 3 )+
                                data[1][3]*mO.getValueAt( 3, 3 );

        result[2][0] =  data[2][0]*mO.getValueAt( 0, 0 )+
                                data[2][1]*mO.getValueAt( 1, 0 )+
                                data[2][2]*mO.getValueAt( 2, 0 )+
                                data[2][3]*mO.getValueAt( 3, 0 );
        result[2][1] =  data[2][0]*mO.getValueAt( 0, 1 )+
                                data[2][1]*mO.getValueAt( 1, 1 )+
                                data[2][2]*mO.getValueAt( 2, 1 )+
                                data[2][3]*mO.getValueAt( 3, 1 );
        result[2][2] =  data[2][0]*mO.getValueAt( 0, 2 )+
                                data[2][1]*mO.getValueAt( 1, 2 )+
                                data[2][2]*mO.getValueAt( 2, 2 )+
                                data[2][3]*mO.getValueAt( 3, 2 );
        result[2][3] =  data[2][0]*mO.getValueAt( 0, 3 )+
                                data[2][1]*mO.getValueAt( 1, 3 )+
                                data[2][2]*mO.getValueAt( 2, 3 )+
                                data[2][3]*mO.getValueAt( 3, 3 );

        result[3][0] =  data[3][0]*mO.getValueAt( 0, 0 )+
                                data[3][1]*mO.getValueAt( 1, 0 )+
                                data[3][2]*mO.getValueAt( 2, 0 )+
                                data[3][3]*mO.getValueAt( 3, 0 );
        result[3][1] =  data[3][0]*mO.getValueAt( 0, 1 )+
                                data[3][1]*mO.getValueAt( 1, 1 )+
                                data[3][2]*mO.getValueAt( 2, 1 )+
                                data[3][3]*mO.getValueAt( 3, 1 );
        result[3][2] =  data[3][0]*mO.getValueAt( 0, 2 )+
                                data[3][1]*mO.getValueAt( 1, 2 )+
                                data[3][2]*mO.getValueAt( 2, 2 )+
                                data[3][3]*mO.getValueAt( 3, 2 );
        result[3][3] =  data[3][0]*mO.getValueAt( 0, 3 )+
                                data[3][1]*mO.getValueAt( 1, 3 )+
                                data[3][2]*mO.getValueAt( 2, 3 )+
                                data[3][3]*mO.getValueAt( 3, 3 );
        
        COME_Matrix resultM( result );
        return resultM;
}

COME_Vector3D
COME_Matrix :: operator* ( COME_Vector3D& vO ){
        
        double x,y,z;

        /*x =   data[0][0] * vO.getX() + data[0][1] * vO.getY() + data[0][2] * vO.getZ() + data[0][3];
        y =     data[1][0] * vO.getX() + data[1][1] * vO.getY() + data[1][2] * vO.getZ() + data[1][3];
        z =     data[2][0] * vO.getX() + data[2][1] * vO.getY() + data[2][2] * vO.getZ() + data[2][3];*/

        x =     (data[0][0] * vO.getX() + data[1][0] * vO.getY() + data[2][0] * vO.getZ() + data[3][0] * 1);
        y =     (data[0][1] * vO.getX() + data[1][1] * vO.getY() + data[2][1] * vO.getZ() + data[3][1] * 1);
        z =     (data[0][2] * vO.getX() + data[1][2] * vO.getY() + data[2][2] * vO.getZ() + data[3][2] * 1);
        
        COME_Vector3D resultV( x, y, z );
        return resultV;
}

COME_Point3D
COME_Matrix :: operator* ( const COME_Point3D& pO ) {
        
        double x,y,z;

        /*w =   data[3][0] * pO.getX() + data[3][1] * pO.getY() + data[3][2] * pO.getZ() + data[3][3] * 1;
        x =     (data[0][0] * pO.getX() + data[0][1] * pO.getY() + data[0][2] * pO.getZ() + data[0][3] * 1)/w;
        y =     (data[1][0] * pO.getX() + data[1][1] * pO.getY() + data[1][2] * pO.getZ() + data[1][3] * 1)/w;
        z =     (data[2][0] * pO.getX() + data[2][1] * pO.getY() + data[2][2] * pO.getZ() + data[2][3] * 1)/w;*/

        x =     (data[0][0] * pO.getX() + data[1][0] * pO.getY() + data[2][0] * pO.getZ() + data[3][0] * 1);
        y =     (data[0][1] * pO.getX() + data[1][1] * pO.getY() + data[2][1] * pO.getZ() + data[3][1] * 1);
        z =     (data[0][2] * pO.getX() + data[1][2] * pO.getY() + data[2][2] * pO.getZ() + data[3][2] * 1);
        
        COME_Point3D resultP( x, y, z );
        return resultP;
}

COME_Vertex3D
COME_Matrix :: operator* ( const COME_Vertex3D& vO ) {
        
        double x,y,z;

        /*w =   data[3][0] * vO.getX() + data[3][1] * vO.getY() + data[3][2] * vO.getZ() + data[3][3] * 1;
        x =     (data[0][0] * vO.getX() + data[0][1] * vO.getY() + data[0][2] * vO.getZ() + data[0][3] * 1)/w;
        y =     (data[1][0] * vO.getX() + data[1][1] * vO.getY() + data[1][2] * vO.getZ() + data[1][3] * 1)/w;
        z =     (data[2][0] * vO.getX() + data[2][1] * vO.getY() + data[2][2] * vO.getZ() + data[2][3] * 1)/w;*/

        x =     (data[0][0] * vO.getX() + data[1][0] * vO.getY() + data[2][0] * vO.getZ() + data[3][0] * 1);
        y =     (data[0][1] * vO.getX() + data[1][1] * vO.getY() + data[2][1] * vO.getZ() + data[3][1] * 1);
        z =     (data[0][2] * vO.getX() + data[1][2] * vO.getY() + data[2][2] * vO.getZ() + data[3][2] * 1);
        
        COME_Vertex3D resultV( x, y, z );
        return resultV;
}

---