UnitQuaternion UnitQuaternion::slerp(const UnitQuaternion& p, const double& t) {
// See http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/
// Where qa = *this and qb = p
double cosHalfTheta = kW() * p.kW() + kX() * p.kX() + kY() * p.kY() + kZ() * p.kZ();
// If qa=qb or qa=-qb then theta = 0 and we can return qa
if (std::abs(cosHalfTheta) >= 1.0) {
return *this;
}
double halfTheta = utility::math::angle::acos_clamped(cosHalfTheta);
double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
// If theta = 180 degrees then result is not fully defined
// We could rotate around any axis normal to qa or qb
if (std::abs(sinHalfTheta) < 0.001) {
return *this * 0.5 + p * 0.5;
}
// Interpolate
double ratioA = std::sin((1 - t) * halfTheta) / sinHalfTheta;
double ratioB = std::sin(t * halfTheta) / sinHalfTheta;
return *this * ratioA + p * ratioB;
}
Rotation3D::Rotation(const UnitQuaternion& q) {
// quaternion to rotation conversion
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/
// http://en.wikipedia.org/wiki/Rotation_group_SO(3)#Quaternions_of_unit_norm
*this << 1 - 2 * q.kY() * q.kY() - 2 * q.kZ() * q.kZ() << 2 * q.kX() * q.kY() - 2 * q.kZ() * q.kW()
<< 2 * q.kX() * q.kZ() + 2 * q.kY() * q.kW() << arma::endr
<< 2 * q.kX() * q.kY() + 2 * q.kZ() * q.kW() << 1 - 2 * q.kX() * q.kX() - 2 * q.kZ() * q.kZ()
<< 2 * q.kY() * q.kZ() - 2 * q.kX() * q.kW() << arma::endr
<< 2 * q.kX() * q.kZ() - 2 * q.kY() * q.kW() << 2 * q.kY() * q.kZ() + 2 * q.kX() * q.kW()
<< 1 - 2 * q.kX() * q.kX() - 2 * q.kY() * q.kY();
}