public double angleShortestPath(Quaternion q)
{
double s = Math.Sqrt(length2() * q.length2());
if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
{
return(Math.Acos(dot(-q) / s) * 2.0);
}
return(Math.Acos(dot(q) / s) * 2.0);
}
public void setRotation(Quaternion q)
{
double d = q.length2();
double s = 2.0 / d;
double xs = q.x * s, ys = q.y * s, zs = q.z * s;
double wx = q.w * xs, wy = q.w * ys, wz = q.w * zs;
double xx = q.x * xs, xy = q.x * ys, xz = q.x * zs;
double yy = q.y * ys, yz = q.y * zs, zz = q.z * zs;
setValue(1.0 - (yy + zz), xy - wz, xz + wy,
xy + wz, 1.0 - (xx + zz), yz - wx,
xz - wy, yz + wx, 1.0 - (xx + yy));
}