/// <summary>
/// Gets the shortest arc quaternion to rotate this vector
/// to the destination vector.
/// </summary>
/// <remarks>
/// If you call this with a dest vector that is close to the inverse
/// of this vector, we will rotate 180 degrees around the 'fallbackAxis'
/// (if specified, or a generated axis if not) since in this case
/// ANY axis of rotation is valid.
/// </remarks>
public Quaternion GetRotationTo(Vector3 destination, Vector3 fallbackAxis)
{
// Based on Stan Melax's article in Game Programming Gems
Quaternion q = new Quaternion();
Vector3 v0 = new Vector3(this.x, this.y, this.z);
Vector3 v1 = destination;
// normalize both vectors
v0.Normalize();
v1.Normalize();
// get the cross product of the vectors
Vector3 c = v0.Cross(v1);
// If the cross product approaches zero, we get unstable because ANY axis will do
// when v0 == -v1
float d = v0.Dot(v1);
// If dot == 1, vectors are the same
if (d >= 1.0f)
{
return Quaternion.Identity;
}
if (d < (1e-6f - 1.0f))
{
if (fallbackAxis != Vector3.Zero)
// rotate 180 degrees about the fallback axis
q = Quaternion.FromAngleAxis((float)Math.PI, fallbackAxis);
else
{
// Generate an axis
Vector3 axis = Vector3.UnitX.Cross(this);
if (axis.IsZero) // pick another if colinear
axis = Vector3.UnitY.Cross(this);
axis.Normalize();
q = Quaternion.FromAngleAxis((float)Math.PI, axis);
}
}
else
{
float s = MathUtil.Sqrt( (1+d) * 2 );
float inverse = 1 / s;
q.x = c.x * inverse;
q.y = c.y * inverse;
q.z = c.z * inverse;
q.w = s * 0.5f;
q.Normalize();
}
return q;
}
/// <summary>
/// Rotates the camera about an arbitrary axis.
/// </summary>
/// <param name="quat"></param>
public void Rotate(Quaternion quat)
{
// Note the order of the multiplication
orientation = quat * orientation;
orientation.Normalize();
InvalidateView();
}
