//-----------------------------------------------------------------------
void Quaternion::Intermediate (const Quaternion& rkQ0,
const Quaternion& rkQ1, const Quaternion& rkQ2,
Quaternion& rkA, Quaternion& rkB)
{
// assert: q0, q1, q2 are unit quaternions
Quaternion kQ0inv = rkQ0.UnitInverse();
Quaternion kQ1inv = rkQ1.UnitInverse();
Quaternion rkP0 = kQ0inv*rkQ1;
Quaternion rkP1 = kQ1inv*rkQ2;
Quaternion kArg = 0.25*(rkP0.Log()-rkP1.Log());
Quaternion kMinusArg = -kArg;
rkA = rkQ1*kArg.Exp();
rkB = rkQ1*kMinusArg.Exp();
}
void Quaternion::SetupSquad(
const Quaternion &q0,
const Quaternion &q1,
const Quaternion &q2,
Quaternion &a,
Quaternion &b)
{
// assert: q0, q1, q2 are unit quaternions
Quaternion q0inv = q0.Conjugate();
Quaternion q1inv = q1.Conjugate();
Quaternion p0 = q0inv * q1;
Quaternion p1 = q1inv * q2;
Quaternion arg = 0.25f * (p0.Ln() - p1.Ln());
Quaternion minusArg = -arg;
a = q1 * arg.Exp();
b = q1 * minusArg.Exp();
}
Quaternion<Real>
Quaternion<Real>::GetIntermediate(const Quaternion& rkQ0, const Quaternion& rkQ1, const Quaternion& rkQ2)
{
// assert: Q0, Q1, Q2 all unit-length
Quaternion<Real> kQ1Inv = rkQ1.Conjugate();
Quaternion<Real> kP0 = kQ1Inv* rkQ0;
Quaternion<Real> kP2 = kQ1Inv* rkQ2;
Quaternion<Real> kArg = -((Real) 0.25) * (kP0.Log() + kP2.Log());
Quaternion<Real> kA = rkQ1* kArg.Exp();
return kA;
}
//----------------------------------------------------------------------------
Quaternion& Quaternion::Intermediate (const Quaternion& q0,
const Quaternion& q1, const Quaternion& q2)
{
Quaternion q1Inv = q1.Conjugate();
Quaternion p0 = q1Inv*q0;
Quaternion p2 = q1Inv*q2;
Quaternion arg = -0.25f*(p0.Log() + p2.Log());
Quaternion a = q1*arg.Exp();
*this = a;
return *this;
}