void TrfmTranslate::applyDeriv(const Dof* q, Matrix4d& m){ for(unsigned int i=0; i<mDofs.size(); i++){ if(mDofs[i] != q) m.row(i).setZero(); else m.row(i) = m.row(3); } m.row(3).setZero(); }
void TrfmTranslateZ::applyDeriv(const Dof* q, Matrix4d& m){ if(mDofs[0] != q) m = Matrix4d::Zero(); else for(int i=0; i<4; i++){ if(i==A_Z) m.row(i) = m.row(3); else m.row(i).setZero(); } }
Matrix4d Sim3:: hat(const Vector7d & v) { Matrix4d Omega; Omega.topLeftCorner<3,3>() = ScSO3::hat(v.tail<4>()); Omega.col(3).head<3>() = v.head<3>(); Omega.row(3) = Vector4d(0., 0., 0., 0.); return Omega; }
void TrfmTranslateZ::applyInvTransform(Matrix4d& m){ m.row(A_Z) -= mDofs[0]->getValue() * m.row(3); }
void TrfmTranslateY::applyTransform(Matrix4d& m){ m.row(A_Y) += mDofs[0]->getValue() * m.row(3); }
void TrfmTranslate::applyInvTransform(Matrix4d& m){ for(unsigned int i=0; i<mDofs.size(); i++) { m.row(i) -= mDofs[i]->getValue() * m.row(3); } }