Biomechanical Joint Model  Author: Anderson Maciel

---

hmat.cpp

Go to the documentation of this file.

#include "hmat.h"


namespace LinAlg{

  VEC HMAT::operator*(const VEC &V) const
  {
    assert (V.dim()==3);
    static VEC tmp(4, 1);
    tmp.insert(V);
    return this->MAT::operator*(tmp);
  }
  
  void HMAT::makeVecRotVec(HMAT &mat, const VEC &v0, const VEC &v1)
  {
    static VEC absv0(3), leastCoordAxis(3);
    
    REAL cosTheta = dot_prod(v0, v1);
    VEC  axis = cross_prod(v0, v1);
    REAL sinTheta = axis.norm();
    
    if (fabs(sinTheta) > 1E-7)
      {
        axis /= sinTheta;
      }
    else if (cosTheta < 0.0)
      {
        absv0 = v0; abs(absv0);
        leastCoordAxis = 0;
        leastCoordAxis[absv0[0]<=absv0[1] ? absv0[0]<=absv0[2] ? 0 : 2
                      : absv0[1]<=absv0[2] ? 1 : 2] = 1;
        axis = cross_prod(v0, leastCoordAxis);
        axis.normalize();
      }
    else
      {
        mat.setIdt();
        return;
      }
    
    REAL x = axis[0];
    REAL y = axis[1];
    REAL z = axis[2];
    
    REAL t = 1.0f - cosTheta;
    
    mat(0,3) = 0.0; // zero translation
    mat(1,3) = 0.0;
    mat(2,3) = 0.0;
    mat(0,0) = t * x * x + cosTheta;
    mat(1,0) = t * x * y + sinTheta * z;
    mat(2,0) = t * x * z - sinTheta * y;
    mat(0,1) = t * y * x - sinTheta * z;
    mat(1,1) = t * y * y + cosTheta;
    mat(2,1) = t * y * z + sinTheta * x;
    mat(0,2) = t * z * x + sinTheta * y;
    mat(1,2) = t * z * y - sinTheta * x;
    mat(2,2) = t * z * z + cosTheta;
  }

}

---