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);
}
}
