Biomechanical Joint Model  Author: Anderson Maciel

---

implicit/LinAlg/quat.h

Go to the documentation of this file.

#ifndef AMAURY_QUAT_H
#define AMAURY_QUAT_H


#define  QUAT_X  0
#define  QUAT_Y  1
#define  QUAT_Z  2
#define  QUAT_W  3

#include "vec.h"
#include "hmat.h"

namespace LinAlg{

 
class QUAT : public VEC
{
public:
   QUAT(const QUAT &Q) : VEC(Q) {};
   explicit QUAT(const REAL& value = 0) : VEC(4, value) {};
   explicit QUAT(const VEC &V) : VEC(V) {};
   explicit QUAT(const char *s) : VEC(4,s) {};
   explicit QUAT(const float *v) : VEC(v,4) {};
   QUAT(const HMAT &M);
   QUAT(REAL theta, const VEC &N) : VEC(4) 
   { 
      assert(N.dim()==3);
      VEC T(N); T.normalize();
      REAL s = sin(theta/2);
      v_[QUAT_W]=cos(theta/2); 
      v_[QUAT_X]=s*T[0]; 
      v_[QUAT_Y]=s*T[1]; 
      v_[QUAT_Z]=s*T[2];
   }; 
 
   void set(const Vector &Q) 
   {
           v_[QUAT_W]= Q[3];
           v_[QUAT_X]= Q[0];
           v_[QUAT_Y]= Q[1];
       v_[QUAT_Z]= Q[2];
   };

   void inverse();
   void conjugate();
   operator HMAT() const;
   friend QUAT operator*(const QUAT &A, const QUAT &B);

   static void slerp (const QUAT &left, const QUAT &right, REAL u, QUAT &result);
   static REAL dist(const QUAT &left, const QUAT &right) { return acosf(dot_prod(left,right)); }
};
 
 inline void QUAT::inverse(){
   REAL qn = norm();
   
   v_[QUAT_W] =  v_[QUAT_W] / qn;
   v_[QUAT_X] = -v_[QUAT_X] / qn;
   v_[QUAT_Y] = -v_[QUAT_Y] / qn;
   v_[QUAT_Z] = -v_[QUAT_Z] / qn;
 }
 
 inline void QUAT::conjugate(){
     v_[QUAT_X] = -v_[QUAT_X];
     v_[QUAT_Y] = -v_[QUAT_Y];
     v_[QUAT_Z] = -v_[QUAT_Z];
 }
 

}
#endif

---