UnitQuaternion UnitQuaternion::operator * (const UnitQuaternion& p) const {
//From http://en.wikipedia.org/wiki/Quaternion#Quaternions_and_the_geometry_of_R3
double realPart = real() * p.real() - arma::dot(imaginary(), p.imaginary());
arma::vec3 imaginaryPart = arma::cross(imaginary(), p.imaginary())
+ p.real() * imaginary()
+ real() * p.imaginary();
return UnitQuaternion(realPart, imaginaryPart);
}
UnitQuaternion UnitQuaternion::i() const {
UnitQuaternion qi = *this;
// take the congugate, as it is equal to the inverse when a unit vector
qi.imaginary() *= -1;
return qi;
}
arma::vec3 UnitQuaternion::rotateVector(const arma::vec3& v) const {
UnitQuaternion vRotated = *this * UnitQuaternion(v) * i();
return vRotated.imaginary();
}