Biomechanical Joint Model  Author: Anderson Maciel

---

comevector3d.cpp

Go to the documentation of this file.

/***************************************************************************
 *   Copyright (C) 2001 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 <cmath>
#include <math.h>
#include <algebra/comevector3d.h>
#include <algebra/comematrix.h>


// Description: Class "COME_Vector3D" constructor without parameter.
// Parameters.: -
// Return.....: -

COME_Vector3D::COME_Vector3D(void) {
        x = 0;
    y = 0;
    z = 0;
}

// Description: Class "COME_Vector3D" constructor with parameters.
// Parameters.: double xi (initial x value), 
//                              double yi (initial y value),
//                              double zi (initial z value).
// Return.....: -

COME_Vector3D::COME_Vector3D(const double xi, const double yi, const double zi) {
        x = xi;
    y = yi;
    z = zi;
}



// Description: Class "COME_Vector3D" constructor with parameters.
// Parameters.: COME_Vector3D (object).
// Return.....: -

COME_Vector3D::COME_Vector3D(const COME_Vector3D &v) {
        x = v.x ;
    y = v.y ;
    z = v.z ;
}

// Description: Class "COME_Vector3D" constructor with parameters.
// Parameters.: VPpoint3D (object).
// Return.....: -

COME_Vector3D::COME_Vector3D(const COME_Point3D &v) {
        x = v.x ;
    y = v.y ;
    z = v.z ;
}

// Description: Operator=
// Parameters.: COME_Vector3D (object).
// Return.....: COME_Vector3D (object).

COME_Vector3D& COME_Vector3D :: operator= ( const COME_Vector3D& v ) {
      if ( this == &v ) return ( *this ) ;
      x = v.x ;
      y = v.y ;
      z = v.z ;
      return ( *this ) ;
}


// Description: Operator=
// Parameters.: COME_Vector3D (object).
// Return.....: COME_Vector3D (object).

COME_Vector3D& COME_Vector3D :: operator= ( const COME_Point3D& p ) {
      if ( this == &p ) return ( *this ) ;
      x = p.x ;
      y = p.y ;
      z = p.z ;
      return ( *this ) ;
}



// Description: Operator==
// Parameters.: COME_Vector3D (object).
// Return.....: TRUE if the vectors are same,
//              FALSE if the vectors are not same.

int COME_Vector3D :: operator== ( const COME_Vector3D& v1 ) {
        if ( ( x == v1.x ) &&
         ( y == v1.y ) &&
         ( z == v1.z ) )
            return true;
         else
            return false;
}




// Description: Operator!=
// Parameters.: COME_Vector3D (object).
// Return.....: TRUE if the vectors are not same,
//              FALSE if the vectors are same.

int COME_Vector3D :: operator!= ( const COME_Vector3D& v ) {
        if ( ( x != v.x ) ||
         ( y != v.y ) ||
         ( z != v.z ) )
            return true;
         else
            return false;
}




// Description: Operator+
// Parameters.: COME_Point3D (object).
// Return.....: COME_Point3D (object).

COME_Point3D COME_Vector3D :: operator+ (COME_Point3D v) const{

      COME_Point3D vv(x + v.getX(), y + v.getY(), z + v.getZ());

      return ( vv );

}


// Description: Operator-
// Parameters.: COME_Point3D (object).
// Return.....: COME_Point3D (object).

COME_Point3D COME_Vector3D :: operator- (COME_Point3D v) {

      COME_Point3D vv(x - v.getX(), y - v.getY(), z - v.getZ());
      return ( vv );
}

// Description: Operator+
// Parameters.: COME_Vector3D (object).
// Return.....: COME_Vector3D (object).

COME_Vector3D COME_Vector3D :: operator+ ( const COME_Vector3D& v ) const {
      COME_Vector3D vv(x + v.x, y + v.y, z + v.z);
      return ( vv ) ;
}


// Description: Operator-
// Parameters.: COME_Vector3D (object).
// Return.....: COME_Vector3D (object).

COME_Vector3D COME_Vector3D :: operator- ( const COME_Vector3D& v ) {
      COME_Vector3D vv(x - v.x, y - v.y, z - v.z);
      return ( vv ) ;
}




// Description: Operator-
// Parameters.: -
// Return.....: COME_Vector3D (object).

COME_Vector3D COME_Vector3D :: operator- () {
      COME_Vector3D vv(- x, - y, - z);
      return ( vv ) ;
}


// Description: Operator*
// Parameters.: double c (multiplies the vector).
// Return.....: COME_Vector3D (object).

COME_Vector3D COME_Vector3D :: operator* ( const double& c ) const {
      COME_Vector3D vv(c * x, c * y, c * z);
      return ( vv ) ;
}


// Description: Operator*
// Parameters.: COME_Vector3D (multiplies the vector).
// Return.....: COME_Vector3D (object).

COME_Vector3D COME_Vector3D :: operator* ( const COME_Vector3D& v ) {
  COME_Vector3D vv(y * v.z - (z * v.y),
        z * v.x - (x * v.z),
        x * v.y - (y * v.x));
  return ( vv ) ;

}

// Description: Operator*
// Parameters.: COME_Vector3D (multiplies the vector).
// Return.....: COME_Vector3D (object).

COME_Vector3D COME_Vector3D :: operator* ( const COME_Matrix& m ) {
  
        double xl,yl,zl;

        xl =    m.getValueAt(0,0) * x + m.getValueAt(1,0) * y + m.getValueAt(2,0) * z;
        yl =    m.getValueAt(0,1) * x + m.getValueAt(1,1) * y + m.getValueAt(2,1) * z;
        zl =    m.getValueAt(0,2) * x + m.getValueAt(1,2) * y + m.getValueAt(2,2) * z;
        
        COME_Vector3D resultV( xl, yl, zl );
        return resultV;

}

// Description: Operator*
// Parameters.: COME_Vector3D (multiplies the vector).
// Return.....: COME_Vector3D (object).

COME_Vector3D COME_Vector3D :: operator* ( const COME_Point3D& v ) {
  COME_Vector3D vv(y * v.z - (z * v.y),
        z * v.x - (x * v.z),
        x * v.y - (y * v.x));
  return ( vv ) ;

}


// Description: Operator/
// Parameters.: double c (divides the vector).
// Return.....: COME_Vector3D (object).

COME_Vector3D COME_Vector3D :: operator/ ( const double& c ) {
      COME_Vector3D vv(x / c, y / c, z / c);
      return ( vv ) ;
}



// Description: Operator+=
// Parameters.: COME_Vector3D (object).
// Return.....: COME_Vector3D (object).

COME_Vector3D& COME_Vector3D :: operator+= ( const COME_Vector3D& v ) {
      x += v.x ;
      y += v.y ;
      z += v.z ;
      return *this ;
}



// Description: Operator-=
// Parameters.: COME_Vector3D (object).
// Return.....: COME_Vector3D (object).

COME_Vector3D& COME_Vector3D :: operator-= ( const COME_Vector3D& v ) {
      x -= v.x ;
      y -= v.y ;
      z -= v.z ;
      return *this ;
}


// Description: Operator*=
// Parameters.: double c (multiplies the vector).
// Return.....: COME_Vector3D (object).

COME_Vector3D& COME_Vector3D :: operator*= ( const double& c ) {
      x *= c ;
      y *= c ;
      z *= c ;
      return *this ;
}


// Description: Operator/=
// Parameters.: double c (divides the vector).
// Return.....: COME_Vector3D (object).

COME_Vector3D& COME_Vector3D :: operator/= ( const double& c ) {
      x /= c ;
      y /= c ;
      z /= c ;
      return *this ;
}


// Description: Vector module.
// Parameters.: -
// Return.....: size (module)

double COME_Vector3D :: vpModule () const {
        double size = (double) sqrt( (double) (x*x + y*y + z*z) );
        return size;
}


// Description: Vector normalization
// Parameters.: -
// Return.....: -

COME_Vector3D COME_Vector3D :: vpNormalize () {
      double l =  vpModule();
      if (l==0) return COME_Vector3D();
      x = x / l ;
      y = y / l ;
      z = z / l ;
          return COME_Vector3D( x, y, z );
}


// Description: Dot Product.
// Parameters.: COME_Vector3D& v
// Return.....: double d (dot product - angle cos * the two vectors module)

double COME_Vector3D :: vpDotProduct ( const COME_Vector3D& v ) {
      double    d ;

      d =  x * v.x + y * v.y + z * v.z;

        
      return ( d ) ;
}


// Description: Cross Product
// Parameters.: 
// Return.....: 

COME_Vector3D COME_Vector3D :: vpCrossProduct ( const COME_Vector3D& v ) {
  COME_Vector3D vv(y * v.z - (z * v.y), 
                            z * v.x - (x * v.z), 
                            x * v.y - (y * v.x));
  return ( vv ) ;
}


// Description: Set the vector value.
// Parameters.: COME_Point3D p
// Return.....: -

void COME_Vector3D :: setVector3D(COME_Point3D p) {
        x = p.getX();
        y = p.getY();
        z = p.getZ();
}


// Description: Set the vector value.
// Parameters.: double x,
//              double y,
//                      double z.
// Return.....: -

void COME_Vector3D :: setVector3D(double xx,double yy,double zz) {
        x = xx;
        y = yy;
        z = zz;
}



// Description: Get the vector value.
// Parameters.: double x,
//              double y,
//                      double z.
// Return.....: -

void COME_Vector3D :: getVector3D(double &xx,double &yy,double &zz) {
        xx = x;
        yy = y;
        zz = z;
}


// Description: Get the vector value.
// Parameters.: -
// Return.....: COME_Vector3D (object)

COME_Vector3D COME_Vector3D :: getVector3D(void) {

        return *this;

}


// Description: Get the X vector value. 
// Parameters.: -
// Return.....: double (x)

double COME_Vector3D :: getVector3DX(void) {

        return x;

}


// Description: Get the Y vector value.
// Parameters.: -
// Return.....: double (y)

double COME_Vector3D :: getVector3DY(void) {

        return y;

}



// Description: Get the Z vector value.
// Parameters.: -
// Return.....: double (z)

double COME_Vector3D :: getVector3DZ(void) {

        return z;

}

// Returns the bigger absolute value of x, y, z
double
COME_Vector3D::maxK(void) {

        //( fabs(x) > fabs(y) ) ? ( ( fabs(z) > fabs(x)) ? return z : return x ) : ( ( fabs(y) > fabs(z) ) ? return y : return z );
        if( fabs(x) > fabs(y) ){
                if( fabs(z) > fabs(x)){
                        return z;
                } else {
                        return x;
                }
        } else if( fabs(y) > fabs(z) ){
                return y;
        } else {
                return z;
        }
        return 0.0;
}


COME_Point3D
COME_Vector3D::getVectorAsPoint3D(){

        return COME_Point3D( x, y, z );
}



double
COME_Vector3D::getAngle( COME_Vector3D& v ){

        double cosine = ( this->vpDotProduct( v ) ) / ( this->vpModule() * v.vpModule() );
        if ( ( cosine >= -1.000001 ) && ( cosine <= -0.99999999999 ) ) return M_PI;
        double angle = acos( cosine );
        /*if( ( angle < - M_PI ) || ( angle > M_PI ) ){
                printf(" angulo louco ");
                angle = acos( ( this->vpDotProduct( v ) ) / ( this->vpModule() * v.vpModule() ) );
                angle = acos( -0.018 / ( 0.135 * 0.135 ) );
        }*/

        return angle;
}


COME_Matrix
COME_Vector3D :: transMult ( COME_Vector3D& v ) {

        
        double result [4][4];

        result[0][0] =  x * v.getX();
        result[0][1] =  x * v.getY();
        result[0][2] =  x * v.getZ();
        result[0][3] =  0.0;

        result[1][0] =  y * v.getX();
        result[1][1] =  y * v.getY();
        result[1][2] =  y * v.getZ();
        result[1][3] =  0.0;

        result[2][0] =  z * v.getX();
        result[2][1] =  z * v.getY();
        result[2][2] =  z * v.getZ();
        result[2][3] =  0.0;

        result[3][0] =  result[3][1] =  result[3][2] =  result[3][3] =  0.0;

        
        COME_Matrix resultM( result );
        return resultM;
}

---