void ExpMapQuaternion::forceOnM_(RefVec out, const ConstRefVec& in)
{
toConstQuat inQuat(in.data());
toQuat outQuat(out.data());
outQuat = inQuat;
out.normalize();
}
void ExpMapQuaternion::setZero_(RefVec out)
{
toQuat(out.data()).setIdentity();
}
void ExpMapQuaternion::retractation_(RefVec out, const ConstRefVec& x, const ConstRefVec& v)
{
OutputType q;
exponential(q,v);
toQuat(out.data()) = (toConstQuat(x.data()))*(toConstQuat(q.data())); //out = x*exp(v)
}
