C++ (Cpp) Mat3::invert Examples

Example #1

File: Spatial.cpp Project: BackupTheBerlios/artbody-svn

void SpatialMat::inertiaInvert(const SpatialMat &I)
{
  Mat3 Binv;
  Mat3 M, N;  /* temp storage */

  
  Binv.invert(I._B);
  Binv.negate();
  M.mult(I._D, Binv);
  N.mult(M, I._A);
  N.add(I._C);
  _B.invert(N);

  M.mult(_B, I._D);
  _A.mult(M, Binv);
  
  _D.xpose(_A);

  M.mult(I._A, _A);
  M.xrow().x -= 1.0; 
  M.yrow().y -= 1.0; 
  M.zrow().z -= 1.0;
  _C.mult(Binv, M);
  
  //Mat3 detI;

  //detI.mult(I._A, I._D);
  //M.mult(I._B, I._C);
  //detI.sub(M);
  //detI.invert();

  //_A.mult(detI, I._D);
  //_B.mult(detI, I._B);
  //_B.negate();
  //_C.mult(detI, I._C);
  //_C.negate();
  //_D.mult(detI, I._A);

  return;
}