void ExpMapQuaternion::exponential(OutputType& q, const ConstRefVec& v)
{
mnf_assert(v.size() == 3 && "Increment for expMap must be of size 3");
double n2 = v.squaredNorm(); // (theta)^2 (in Grassia)
mnf_assert(sqrt(n2) < M_PI && "Increment for expMap must be of norm at most pi");
double s; // sin(theta/2)/theta in Grassia
if (n2 < prec)
{
toQuat(q.data()).w() = 1 + (-1 + n2 / 48)*(n2/8);// cos(theta/2) in Grassia
s = (1+(-1+0.0125*n2)*n2/24)/2;
}
else
{
double t = sqrt(n2); // theta (in Grassia)
toQuat(q.data()).w() = cos(0.5*t);
s = sin(0.5*t) / t;
}
toQuat(q.data()).vec() = s*v;
}
void ExpMapQuaternion::logarithm(RefVec out, const OutputType& v)
{
const toConstQuat vQ(v.data());
double n2 = vQ.vec().squaredNorm();
double n = sqrt(n2);
if (n < prec && vQ.w()!=0 )
out = (2/vQ.w())*vQ.vec();
else
out = atan2(2 * n * vQ.w(), vQ.w() * vQ.w() - n2) / n * vQ.vec();
}
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)
}