/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/include/matvec3D.h

Go to the documentation of this file.

//######################################################################
//
// GraspIt!
// Copyright (C) 2002-2009  Columbia University in the City of New York.
// All rights reserved.
//
// GraspIt! 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 3 of the License, or
// (at your option) any later version.
//
// GraspIt! 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 GraspIt!.  If not, see <http://www.gnu.org/licenses/>.
//
// Author(s):  Andrew T. Miller and Matei T. Ciocarlie
//
// $Id: matvec3D.h,v 1.17 2009/04/21 14:53:08 cmatei Exp $
//
//######################################################################

#ifndef _MATVEC_H_
#include <math.h>
#include <string.h>
#include <iostream>
#include <stack>

#include <Inventor/SbLinear.h>
#include <Inventor/nodes/SoTransform.h>

class vec3;
class position;
class mat3;
class Quaternion;
class transf;

#define MACHINE_ZERO 1.0e-9
#define resabs 1.0e-5

typedef double col_Mat4[4][4];


class vec3 {

  double vec[3];

public:
  vec3() {}

  vec3(const vec3& v) {set(v);}

  vec3(double v[3])   {set(v);}

  vec3(const SbVec3f& v) {set(v);}

  vec3(double x,double y,double z) {set(x,y,z);}

  const double& operator[](int i) const {return vec[i];}

  double& operator[](int i) {return vec[i];}

  void set(const vec3& v) {vec[0] = v[0]; vec[1] = v[1]; vec[2] = v[2];}
  
  void set(double v[3])   {vec[0] = v[0]; vec[1] = v[1]; vec[2] = v[2];}

  void set(double x,double y,double z) {vec[0] = x; vec[1] = y; vec[2] = z;}

  inline double len() const;
  inline double len_sq() const;

  inline void set(const SbVec3f &v);  
  inline void toSbVec3f(SbVec3f &v) const;
  inline SbVec3f toSbVec3f() const;

  // input output

  inline vec3 const& operator=(vec3 const& v);
  inline vec3 const& operator*=(double s);
  //  inline vec3 const& operator*=(mat3 const& m);
  //inline vec3 const& operator*=(transf const& t);
  inline vec3 const& operator+=(vec3 const& v);
  inline vec3 const& operator-=(vec3 const& v);
  inline vec3 const& operator/=(double s);
  inline bool operator==(vec3 const& v) const;
  inline bool operator<(vec3 const& v) const;

  double x() const {return vec[0];}

  double y() const {return vec[1];}

  double z() const {return vec[2];}

  double& x() {return vec[0];}

  double& y() {return vec[1];}

  double& z() {return vec[2];}  


  void perpVectors(vec3 &v1,vec3 &v2); 

  /*  friend double operator%(position const& p, vec3 const& v);
      friend double operator%(vec3 const& v, position const& p);*/

  friend inline double operator%(vec3 const& v1, vec3 const& v2);  // dot prod
  friend inline double operator%(position const& p, vec3 const& v);
  friend inline double operator%(vec3 const& v, position const& p);
  friend inline vec3   operator*(double s, vec3 const& v);
  friend inline vec3   operator*(vec3 const& v, double s);
  friend inline vec3   operator*(mat3 const& m, vec3 const& v);

  friend inline vec3   operator*(vec3 const& v, transf const& tr);
  friend inline vec3   operator>(vec3 const& v, transf const& tr);
  friend inline vec3   operator*(vec3 const& v1, vec3 const& v2);  // cross
  friend inline vec3   operator+(vec3 const& v1, vec3 const& v2);
  friend inline vec3   operator-(vec3 const& v1, vec3 const& v2);
  friend inline vec3   operator-(vec3 const& v);  
  friend inline vec3   operator/(vec3 const& v, double s);
  friend inline vec3   normalise(vec3 const& v);

  friend std::istream &operator>>(std::istream &is, vec3 &v);
  friend std::ostream &operator<<(std::ostream &os, const vec3 &v);

  static const vec3 ZERO;

  static const vec3 X;

  static const vec3 Y;

  static const vec3 Z;
};



class position {

  double pos[3];

public:

  position() {}

  position(const position& p) {set(p);}

  position(double p[3])       {set(p);}

  position(const SbVec3f& p)  {set(p);}

  position(double x,double y,double z) {set(x,y,z);}
    
  const double& operator[](int i) const {return pos[i];}

  double& operator[](int i) {return pos[i];}

  void set(const position& p) {pos[0] = p[0]; pos[1] = p[1]; pos[2] = p[2];}

  void set(double p[3])       {pos[0] = p[0]; pos[1] = p[1]; pos[2] = p[2];}

  void set(double x,double y,double z) {pos[0] = x; pos[1] = y; pos[2] = z;}

  void set(const vec3 &v){pos[0] = v.x(); pos[1] = v.y(); pos[2] = v.z();}

  void get(double p[3]) const {p[0] = pos[0]; p[1] = pos[1]; p[2] = pos[2];}

  inline void set(const SbVec3f &v);
  inline void toSbVec3f(SbVec3f &v) const;
  inline SbVec3f toSbVec3f() const;

  // input output

  inline position const& operator=(position const& p);
  //  inline position const& operator*=(mat3 const& m);
  //  inline position const& operator*=(transf const& tr);
  inline position const& operator+=(vec3 const& v);
  inline position const& operator-=(vec3 const& v);
  inline bool operator==(position const& p) const;

  double x() const {return pos[0];}

  double y() const {return pos[1];}

  double z() const {return pos[2];}

  double& x() {return pos[0];}

  double& y() {return pos[1];}

  double& z() {return pos[2];}  

  friend inline double     operator%(position const& p, vec3 const& v);  // dot
  friend inline double     operator%(vec3 const& v, position const& p);  // dot
  //  friend inline position   operator*(mat3 const& m, position const& p);
  inline friend position   operator*(double d, position const& p);
  inline friend position   operator*(position const& p, transf const& tr);
  friend inline position   operator+(position const& p, vec3 const& v);
  friend inline position   operator-(position const& p, vec3 const& v);
  friend inline position   operator+(vec3 const& v, position const& p);
  friend inline vec3       operator-(position const& p1, position const& p2);
  friend inline position   operator+(position const& p1, position const& p2);

  friend std::istream&       operator>>(std::istream &is, position &p);
  friend std::ostream&       operator<<(std::ostream &os, const position &p); 

  static const position ORIGIN;
};



class mat3 {
  
  double mat[9];

public:

  mat3() {}

  mat3(const double M[9]) {set(M);}

  mat3(const Quaternion &q) {set(q);}

  mat3(const vec3 &v1, const vec3 &v2, const vec3 &v3) {set(v1,v2,v3);}

  void set(const double M[9]) {memcpy(mat,M,sizeof(double)*9);}

  inline void set(const Quaternion &q);
  inline void set(const vec3 &v1, const vec3 &v2, const vec3 &v3);

  inline void setCrossProductMatrix(const vec3 &v);

  void ToEulerAngles(double &roll,double &pitch, double &yaw) const;

  const double& element(int i,int j) const {return mat[j*3+i];}

        double& element(int i,int j)       {return mat[j*3+i];}

  const double& operator[](int i) const {return mat[i];}
        double& operator[](int i)       {return mat[i];}

        vec3    row(int i) const {return vec3(mat[i],mat[i+3],mat[i+6]);}
  
  inline        double        determinant() const;

  inline        mat3          transpose() const;
                mat3          inverse() const;

                mat3 const&   operator*=(mat3 const& m);
                 friend vec3          operator*(mat3 const& m,  vec3 const& v);
  //     friend position      operator*(mat3 const& m,  position const& p);
             friend vec3          operator*(vec3 const& v,  mat3 const& m);
         friend mat3          operator*(mat3 const& m1, mat3 const& m2);
  inline friend mat3          operator*(double const& s,  mat3 const& m);
  inline mat3 const& operator+=(mat3 const& v);

  friend std::istream&     operator>>(std::istream &is, mat3 &m);
  friend std::ostream&     operator<<(std::ostream &os, const mat3 &m);  


  static const mat3 ZERO;
  
  static const mat3 IDENTITY;  
};


class Quaternion {
public:
  double w, x, y, z;
  
  Quaternion () : w(1.0),x(0.0),y(0.0),z(0.0) {}

  Quaternion (double W, double X, double Y, double Z) {set(W,X,Y,Z);}

  Quaternion (const mat3& R) {set(R);}

  Quaternion (const double& angle, const vec3 &axis) {set(angle,axis);}

  void set(const Quaternion& Q)
    {w = Q.w; x = Q.x; y = Q.y; z = Q.z;}

  void set(double W, double X, double Y, double Z)
    {w = W; x = X; y = Y; z = Z; normalise();}

  void set(const mat3& R);
  void set(const double& angle, const vec3 &axis);
  void set(const SbRotation &SbRot);

  void ToRotationMatrix (mat3& m) const;  
  void ToAngleAxis (double& angle, vec3& axis) const;
  inline SbRotation toSbRotation() const;

  // arithmetic operations
  inline Quaternion& operator= (const Quaternion& q);
  inline Quaternion operator+ (const Quaternion& q) const;
  inline Quaternion operator- (const Quaternion& q) const;
  inline Quaternion operator* (const Quaternion& q) const;
  //inline Quaternion operator* (double c) const;
  inline Quaternion operator- () const;
  inline double     operator% (const Quaternion& q) const;  // dot product 
  //  inline friend Quaternion operator* (double c, const Quaternion& q);
  inline bool operator==(Quaternion const& q) const;

  // functions of a quaternion
  inline double norm () const;  // squared-length of quaternion
  inline Quaternion inverse () const;  // apply to unit quaternion
  
  // rotation of a vector by a quaternion
  inline vec3 operator* (const vec3& v) const;
  
  // rotation of a position by a quaternion
  inline position operator* (const position& pt) const;
  
  // spherical linear interpolation
  static Quaternion Slerp (double t, const Quaternion& p,
                           const Quaternion& q);
  
  // setup for spherical quadratic interpolation
  static void Intermediate (const Quaternion& q0, const Quaternion& q1,
                            const Quaternion& q2, Quaternion& a, Quaternion& b);
  
  // spherical quadratic interpolation
  static Quaternion Squad (double t, const Quaternion& p,
                           const Quaternion& a, const Quaternion& b, const Quaternion& q);
  
  inline void normalise();
  
  friend std::istream& operator>>(std::istream &is, Quaternion &q);
  friend std::ostream& operator<<(std::ostream &os, const Quaternion &q);
  
  static const Quaternion IDENTITY;

};


class transf {

  mat3 R;

  vec3 t;

  Quaternion rot;

public:

  transf() {R = mat3::IDENTITY; t = vec3::ZERO; rot = Quaternion::IDENTITY;}

  transf(const Quaternion& r, const vec3& d) {set(r,d);}

  transf(const mat3& r, const vec3& d) {set(r,d);}

  transf(const SoTransform *IVt) {set(IVt);}

  void set(const Quaternion& r, const vec3& d) {
    rot = r; t = d; rot.ToRotationMatrix(R);
  }
  
  void set(const mat3& r, const vec3& d) {rot.set(r); t = d; R = r;}

  inline void set(const SoTransform *IVt);

  const Quaternion& rotation() const {return rot;}

  const mat3& affine() const {return R;}

  const vec3& translation() const {return t;}

  inline void toSoTransform(SoTransform *IVt) const;
  inline void tocol_Mat4(col_Mat4 colTran) const;
  inline void toColMajorMatrix(double t[][4]) const;
  inline void toRowMajorMatrix(double t[][4]) const;
  void jacobian(double jac[]);
  
  inline transf          inverse() const;
  
  inline transf const&   operator=(transf const& tr);
  inline bool operator==(transf const& tr) const;
  
  inline bool operator!=(transf const& tr) const {return !operator==(tr);}


  //  inline transf const&   operator*=(transf const& tr);

  inline friend transf   operator*(transf const& tr1,  transf const& tr2);
  inline friend vec3     operator*(vec3 const& v, transf const& tr);
  inline friend vec3     operator>(vec3 const& v, transf const& tr);
  inline friend position operator*(position const& p, transf const& tr);


  inline friend transf translate_transf(const vec3& v);
  inline friend transf rotate_transf(double angle, const vec3& axis);
  inline friend transf coordinate_transf(const position& origin,
                                         const vec3& xaxis, const vec3& yaxis);
  inline friend transf rotXYZ(double, double, double);

  friend std::istream& operator>>(std::istream &is, transf &tr);
  friend std::ostream& operator<<(std::ostream &os, const transf &tr);

  static const transf IDENTITY;
};


class FlockTransf{
private:
        transf mount, mountInv;
        transf flockBaseInv, objBase;

public:
        void identity();
        void setMount(transf t){mount = t; mountInv = t.inverse();}
        void setFlockBase(transf t){flockBaseInv = t.inverse();}
        void setObjectBase(transf t){objBase = t;}
        transf get(transf t) const;
        transf getAbsolute(transf t) const;
        transf get2(transf t) const;
        transf getMount() const {return mount;}
};

//                       vec3  functions

void
vec3::set(const SbVec3f &v)
{
  vec[0] = (double) v[0];
  vec[1] = (double) v[1];
  vec[2] = (double) v[2];
}

void
vec3::toSbVec3f(SbVec3f &v) const
{
  v[0] = (float) vec[0];
  v[1] = (float) vec[1];
  v[2] = (float) vec[2];
}

SbVec3f
vec3::toSbVec3f() const
{
  return SbVec3f((float) vec[0],(float) vec[1],(float) vec[2]);
}

double
operator%(vec3 const& v1, vec3 const& v2)
{
  return v1.vec[0]*v2.vec[0] + v1.vec[1]*v2.vec[1] + v1.vec[2]*v2.vec[2];
}

vec3
operator*(double s, vec3 const& v)
{
  return vec3(s*v.vec[0],s*v.vec[1],s*v.vec[2]);
}

vec3
operator*(vec3 const& v, double s)
{
  return vec3(s*v.vec[0],s*v.vec[1],s*v.vec[2]);
}

double
vec3::len() const
{
  return sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]);
}

double
vec3::len_sq() const
{
  return vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2];
}

vec3 const&
vec3::operator=(vec3 const& v)
{
  vec[0] = v[0]; vec[1] = v[1]; vec[2] = v[2];
  return *this;
}

vec3 const&
vec3::operator*=(double s)
{
  vec[0] *= s;   vec[1] *= s;   vec[2] *= s;
  return *this;
}

/* 
// Not used right now.
vec3 const&
vec3::operator*=(mat3 const& m)
{
  double v[3];
  memcpy(v,vec,3*sizeof(double));

  vec[0] = m[0] * v[0] + m[3] * v[1] + m[6] * v[2];
  vec[1] = m[1] * v[0] + m[4] * v[1] + m[7] * v[2];
  vec[2] = m[2] * v[0] + m[5] * v[1] + m[8] * v[2];
  return *this;
}
*/

vec3 const&
vec3::operator+=(vec3 const& v)
{
  vec[0] += v[0]; vec[1] += v[1]; vec[2] += v[2];
  return *this;
}

vec3 const&
vec3::operator-=(vec3 const& v)
{
  vec[0] -= v[0]; vec[1] -= v[1]; vec[2] -= v[2];
  return *this;
}

vec3 const&
vec3::operator/=(double s)
{
  vec[0] /= s;   vec[1] /= s;   vec[2] /= s;
  return *this;
}

bool
vec3::operator==(vec3 const& v) const
{
  if ((*this - v).len_sq() < resabs * resabs)
    return true;
  return false;
}

bool
vec3::operator<(vec3 const& v) const
{
  double v0diff,v1diff,v2diff;
  v2diff = vec[2]-v[2];  
  if (v2diff < -resabs) return true;
  else if (v2diff < resabs) {
    v1diff = vec[1]-v[1];
    if (v1diff < -resabs) return true;
    else if (v1diff < resabs) {
      v0diff = vec[0]-v[0];
      if (v0diff < -resabs) return true;      
    }
  }
  return false;
}

double
operator%(position const& p, vec3 const& v)
{
  return p.pos[0]*v.vec[0] + p.pos[1]*v.vec[1] + p.pos[2]*v.vec[2];
}

double
operator%(vec3 const& v, position const& p)
{
  return p.pos[0]*v.vec[0] + p.pos[1]*v.vec[1] + p.pos[2]*v.vec[2];
}


vec3
operator*(const mat3 &m, const vec3 &v)
{
  double result[3];
  result[0] = m[0] * v.vec[0] + m[3] * v.vec[1] + m[6] * v.vec[2];
  result[1] = m[1] * v.vec[0] + m[4] * v.vec[1] + m[7] * v.vec[2];
  result[2] = m[2] * v.vec[0] + m[5] * v.vec[1] + m[8] * v.vec[2];
  return vec3(result);
}


vec3
operator*(vec3 const& v1, vec3 const& v2)  // cross pr
{
  return vec3(v1.vec[1]*v2.vec[2]-v1.vec[2]*v2.vec[1],
              v1.vec[2]*v2.vec[0]-v1.vec[0]*v2.vec[2],
              v1.vec[0]*v2.vec[1]-v1.vec[1]*v2.vec[0]);
}

vec3
operator+(vec3 const& v1, vec3 const& v2)
{
  return vec3(v1.vec[0]+v2.vec[0],v1.vec[1]+v2.vec[1],v1.vec[2]+v2.vec[2]);
}


vec3
operator-(vec3 const& v1, vec3 const& v2)
{
  return vec3(v1.vec[0]-v2.vec[0],v1.vec[1]-v2.vec[1],v1.vec[2]-v2.vec[2]);
}
 

vec3
operator-(vec3 const& v)
{
  return vec3(-v.vec[0],-v.vec[1],-v.vec[2]);
}
  

vec3
operator/(vec3 const& v, double s)
{
  return vec3(v.vec[0]/s,v.vec[1]/s,v.vec[2]/s);
}

vec3
normalise(vec3 const& v)
{
  double length = v.len();
#ifdef GRASPITDBG
  if (length == 0.0)
    printf("0 length in vec3 normalise\n");
#endif
  return v/length;
}

 


//                       position inline functions

void
position::set(const SbVec3f &v)
{
  pos[0] = (double) v[0];
  pos[1] = (double) v[1];
  pos[2] = (double) v[2];
}

void
position::toSbVec3f(SbVec3f &v) const
{
  v[0] = (float) pos[0];
  v[1] = (float) pos[1];
  v[2] = (float) pos[2];
}

SbVec3f
position::toSbVec3f() const
{
  return SbVec3f((float) pos[0],(float) pos[1],(float) pos[2]);
}

position const&
position::operator=(position const& p)
{
  pos[0] = p[0]; pos[1] = p[1]; pos[2] = p[2];
  return *this;
}

/*
//not used
position const&
position::operator*=(mat3 const& m)
{
  double p[3];
  memcpy(p,pos,3*sizeof(double));

  pos[0] = m[0] * p[0] + m[3] * p[1] + m[6] * p[2];
  pos[1] = m[1] * p[0] + m[4] * p[1] + m[7] * p[2];
  pos[2] = m[2] * p[0] + m[5] * p[1] + m[8] * p[2];
  return *this;
}
*/

position const&
position::operator+=(vec3 const& v)
{
  pos[0] += v[0]; pos[1] += v[1]; pos[2] += v[2];
  return *this;
}

position const&
position::operator-=(vec3 const& v)
{
  pos[0] -= v[0]; pos[1] -= v[1]; pos[2] -= v[2];
  return *this;
}

/*
position
operator*(const mat3 &m, const position &p)
{
  double result[3];
  result[0] = m.mat[0] * p.pos[0] + m.mat[3] * p.pos[1] + m.mat[6] * p.pos[2];
  result[1] = m.mat[1] * p.pos[0] + m.mat[4] * p.pos[1] + m.mat[7] * p.pos[2];
  result[2] = m.mat[2] * p.pos[0] + m.mat[5] * p.pos[1] + m.mat[8] * p.pos[2];
  return position(result);
}
*/

position
operator+(position const& p, vec3 const& v)
{
  return position(p[0]+v[0], p[1]+v[1], p[2]+v[2]);
}

position
operator-(position const& p, vec3 const& v)
{
  return position(p[0]-v[0], p[1]-v[1], p[2]-v[2]);
}

position
operator+(vec3 const& v, position const& p)
{
  return position(p[0]+v[0], p[1]+v[1], p[2]+v[2]);
}

vec3
operator-(position const& p1, position const& p2)
{
  return vec3(p1[0]-p2[0], p1[1]-p2[1], p1[2]-p2[2]);
}

position
operator+(position const& p1, position const& p2)
{
  return position(p1[0]+p2[0], p1[1]+p2[1], p1[2]+p2[2]);
}

position
operator*(double d, position const& p)
{
        return position(d*p[0], d*p[1], d*p[2]);
}

bool
position::operator==(position const& p) const
{
  if ((*this - p).len_sq() < resabs * resabs)
    return true;
  return false;
}

//                       mat3 inline functions


void
mat3::set(const Quaternion &q)
{
  double tx  = 2.0*q.x;
  double ty  = 2.0*q.y;
  double tz  = 2.0*q.z;
  double twx = tx*q.w;
  double twy = ty*q.w;
  double twz = tz*q.w;
  double txx = tx*q.x;
  double txy = ty*q.x;
  double txz = tz*q.x;
  double tyy = ty*q.y;
  double tyz = tz*q.y;
  double tzz = tz*q.z;
  
  mat[0] = 1.0-(tyy+tzz);    mat[1] = txy-twz;          mat[2] = txz+twy;
  mat[3] = txy+twz;          mat[4] = 1.0-(txx+tzz);    mat[5] = tyz-twx;
  mat[6] = txz-twy;          mat[7] = tyz+twx;          mat[8] = 1.0-(txx+tyy);
}

void
mat3::set(const vec3 &v1, const vec3 &v2, const vec3 &v3)
{
  mat[0] = v1[0]; mat[3] = v1[1]; mat[6] = v1[2];
  mat[1] = v2[0]; mat[4] = v2[1]; mat[7] = v2[2];
  mat[2] = v3[0]; mat[5] = v3[1]; mat[8] = v3[2];
}

void 
mat3::setCrossProductMatrix(const vec3 &v)
{
        mat[0]=0.0;
        mat[1]= v.z();
        mat[2]=-v.y();
  
        mat[3]=-v.z();
        mat[4]=0.0;
        mat[5]= v.x();
  
        mat[6]= v.y();
        mat[7]=-v.x();
        mat[8]=0.0;

}

double
mat3::determinant() const
{
  return  mat[0] * (mat[4] * mat[8] - mat[7] * mat[5])
        + mat[3] * (mat[7] * mat[2] - mat[1] * mat[8])
        + mat[6] * (mat[1] * mat[5] - mat[4] * mat[2]);
}

mat3
mat3::transpose() const
{
  double M[9];

  M[0] = mat[0];
  M[1] = mat[3];
  M[2] = mat[6];

  M[3] = mat[1];
  M[4] = mat[4];
  M[5] = mat[7];
  
  M[6] = mat[2];
  M[7] = mat[5];
  M[8] = mat[8];

  return mat3(M);
}

mat3
operator*(double const& s,  mat3 const& m)
{
  double M[9];

  M[0] = s*m.mat[0];
  M[1] = s*m.mat[1];
  M[2] = s*m.mat[2];
  M[3] = s*m.mat[3];
  M[4] = s*m.mat[4];
  M[5] = s*m.mat[5];
  M[6] = s*m.mat[6];
  M[7] = s*m.mat[7];
  M[8] = s*m.mat[8];
  return mat3(M);
}

mat3 const&
mat3::operator+=(mat3 const& m)
{
  mat[0] += m[0]; mat[1] += m[1]; mat[2] += m[2];
  mat[3] += m[3]; mat[4] += m[4]; mat[5] += m[5];
  mat[6] += m[6]; mat[7] += m[7]; mat[8] += m[8];
  return *this;
}

//                       Quaternion inline functions

SbRotation
Quaternion::toSbRotation() const
{
  return SbRotation((float) x,(float) y, (float) z, (float) w);
}

Quaternion&
Quaternion::operator=(const Quaternion& q)
{
  set(q);
  return *this;
}

Quaternion
Quaternion::operator+(const Quaternion& q) const
{
    return Quaternion(w+q.w,x+q.x,y+q.y,z+q.z);
}

Quaternion
Quaternion::operator-(const Quaternion& q) const
{
    return Quaternion(w-q.w,x-q.x,y-q.y,z-q.z);
}

Quaternion
Quaternion::operator*(const Quaternion& q) const
{
    return Quaternion(
                w*q.w-x*q.x-y*q.y-z*q.z,
                w*q.x+x*q.w+y*q.z-z*q.y,
                w*q.y+y*q.w+z*q.x-x*q.z,
                w*q.z+z*q.w+x*q.y-y*q.x
    );
}


/*Quaternion
Quaternion::operator*(double c) const
{
    return Quaternion(c*w,c*x,c*y,c*z);
}


Quaternion
operator*(double c, const Quaternion& q)
{
    return Quaternion(c*q.w,c*q.x,c*q.y,c*q.z);
}
*/

Quaternion
Quaternion::operator-() const
{
    return Quaternion(-w,-x,-y,-z);
}

double
Quaternion::operator%(const Quaternion& q) const
{
    return w*q.w+x*q.x+y*q.y+z*q.z;
}

double
Quaternion::norm() const
{
    return w*w+x*x+y*y+z*z;
}

Quaternion
Quaternion::inverse() const
{
    // assert:  'this' is unit length
    return Quaternion(w,-x,-y,-z);
}

vec3
Quaternion::operator*(const vec3& v) const
{
    // Given a vector u = (x0,y0,z0) and a unit length quaternion
    // q = <w,x,y,z>, the vector v = (x1,y1,z1) which represents the
    // rotation of u by q is v = q*u*q^{-1} where * indicates quaternion
    // multiplication and where u is treated as the quaternion <0,x0,y0,z0>.
    // Note that q^{-1} = <w,-x,-y,-z>, so no real work is required to
    // invert q.  Now
    //
    //   q*u*q^{-1} = q*<0,x0,y0,z0>*q^{-1}
    //     = q*(x0*i+y0*j+z0*k)*q^{-1}
    //     = x0*(q*i*q^{-1})+y0*(q*j*q^{-1})+z0*(q*k*q^{-1})
    //
    // As 3-vectors, q*i*q^{-1}, q*j*q^{-1}, and 2*k*q^{-1} are the columns
    // of the rotation matrix computed in Quaternion::ToRotationMatrix.  The
    // vector v is obtained as the product of that rotation matrix with
    // vector u.  As such, the quaternion representation of a rotation
    // matrix requires less space than the matrix and more time to compute
    // the rotated vector.  Typical space-time tradeoff...

    mat3 R;
    ToRotationMatrix(R);

    vec3 result;
    result[0] = v.x()*R.element(0,0)+v.y()*R.element(1,0)+v.z()*R.element(2,0);
    result[1] = v.x()*R.element(0,1)+v.y()*R.element(1,1)+v.z()*R.element(2,1);
    result[2] = v.x()*R.element(0,2)+v.y()*R.element(1,2)+v.z()*R.element(2,2);

    return result;
}

position
Quaternion::operator*(const position& p) const
{
  mat3 R;
  ToRotationMatrix(R);
  
  position result;
  result[0] = p.x()*R.element(0,0)+p.y()*R.element(1,0)+p.z()*R.element(2,0);
  result[1] = p.x()*R.element(0,1)+p.y()*R.element(1,1)+p.z()*R.element(2,1);
  result[2] = p.x()*R.element(0,2)+p.y()*R.element(1,2)+p.z()*R.element(2,2);
  
  return result;
}

bool
Quaternion::operator==(Quaternion const& q) const
{
  if ((w-q.w)*(w-q.w)+(x-q.x)*(x-q.x)+(y-q.y)*(y-q.y)+(z-q.z)*(z-q.z) < 
      resabs * resabs)
    return true;
  return false;
}

void
Quaternion::normalise()
{
  double nm=norm();
  double invnorm;
  if (nm==0.0) {
#ifdef GRASPITDBG
    printf("0 norm in Quaternion normalise\n");
#endif  
        return;
  }
  invnorm =1.0/sqrt(nm);
  w*=invnorm; x*=invnorm; y*=invnorm; z*=invnorm;
}

//                       transf  functions

transf
transf::inverse() const
{ 
  return transf(rot.inverse(),-(rot.inverse() * t));
}

transf const&
transf::operator=(transf const& tr)
{
  rot = tr.rotation();
  R = tr.affine();
  t = tr.translation();
  return *this;
}

/*transf const&
transf::operator*=(transf const& tr)
{
  rot = tr.rotation() * rot;
  rot.ToRotationMatrix(R);
  t  += tr.translation + rot * t;
  return *this;
}
*/


void
transf::toSoTransform(SoTransform *IVt) const
{  
  IVt->rotation.setValue(rot.toSbRotation());
  IVt->translation.setValue(t.toSbVec3f());
}
void
transf::toColMajorMatrix(double mat[][4]) const
{
  for (int i=0; i<3; i++) {
    for(int j=0; j<3; j++) {
      mat[j][i] = R.element(j,i);
        }
    mat[i][3] = 0.0;
  }
  mat[3][0] = t[0];
  mat[3][1] = t[1];
  mat[3][2] = t[2];
  mat[3][3] = 1.0;
}

void
transf::toRowMajorMatrix(double mat[][4]) const
{
  for (int i=0; i<3; i++) {
    for(int j=0; j<3; j++) {
      mat[i][j] = R.element(j,i);
        }
    mat[3][i] = 0.0;
  }
  mat[0][3] = t[0];
  mat[1][3] = t[1];
  mat[2][3] = t[2];
  mat[3][3] = 1.0;
}

void
transf::tocol_Mat4(col_Mat4 colTran) const
{
  for (int j=0;j<3;j++) {
    for(int i=0;i<3;i++)
      colTran[i][j] = R.element(j,i);
    colTran[3][j] = 0.0;
  }
  colTran[0][3] = t[0];
  colTran[1][3] = t[1];
  colTran[2][3] = t[2];
  colTran[3][3] = 1.0;
}

void
transf::set(const SoTransform *IVt)
{
  float q1,q2,q3,q4,x,y,z;

  // Inventor stores the quaternion as qx,qy,qz,qw
  IVt->rotation.getValue().getValue(q2,q3,q4,q1);
  IVt->translation.getValue().getValue(x,y,z);  
  rot.w=(double)q1; rot.x=(double)q2; rot.y=(double)q3; rot.z=(double)q4;
  t[0]=(double)x; t[1]=(double)y; t[2]=(double)z;
  rot.ToRotationMatrix(R);
}

transf
operator*(const transf& tr2,  const transf& tr1)
{
  Quaternion newRot = tr1.rotation() * tr2.rotation();
  return transf(newRot, tr1.translation() + tr1.rotation() * tr2.translation());
}

vec3
operator*(const vec3& v, const transf& tr)
{
  return tr.rot * v;
}
/*returns the vector under transformation tr*/
vec3
operator>(const vec3& v, const transf& tr)
{
        return tr.rot*v + tr.t;
}


position
operator*(const position& p, const transf& tr)
{
  return tr.rot * p + tr.t;
}

bool
transf::operator==(transf const& tr) const
{
  if (tr.rot == rot && tr.t == t)
    return true;
  return false;
}

transf
translate_transf(const vec3& v)
{
  return transf(Quaternion::IDENTITY,v);
}

transf
rotate_transf(double angle, const vec3& axis)
{
  return transf(Quaternion(angle,axis),vec3::ZERO);
}

transf
coordinate_transf(const position& o, const vec3& xaxis, const vec3& yaxis)
{ 
  vec3 newxaxis = normalise(xaxis);
  vec3 newzaxis = normalise(newxaxis * yaxis);
  vec3 newyaxis = normalise(newzaxis * newxaxis);
  return transf(mat3(newxaxis,newyaxis,newzaxis),o - position::ORIGIN);
}

transf
rotXYZ(double rx, double ry, double rz)
{
        transf r;
        vec3 x(1,0,0), y(0,1,0), z(0,0,1);
        
        r = rotate_transf(rx, x);
        y = y * r.inverse();
        r = rotate_transf(ry, y) * r;
        z = z * r.inverse();
        r = rotate_transf(rz, z) * r;

        return r;
}

#define _MATVEC_H_
#endif

---

graspit
Author(s):
autogenerated on Wed Jan 25 10:58:51 2012