/// <summary>
/// Rotate this vector about the given axis angle.
/// </summary>
/// <param name="wAxis">The axis to rotate about. Must be unit length.</param>
/// <param name="angle">The angle of rotation.</param>
/// <returns></returns>
public Vector3 rotate(Vector3 wAxis, float angle)
{
// wAxis must be a unit lenght vector
Vector3 o = wAxis * wAxis.dot(ref this);
Vector3 x = this - o;
Vector3 y;
y = wAxis.cross(ref this);
return(o + x * (float)System.Math.Cos(angle) + y * (float)System.Math.Sin(angle));
}
/// <summary>
/// Find the shortest arc between two vectors.
/// </summary>
/// <param name="v0">Source vector.</param>
/// <param name="v1">Destination vector.</param>
/// <returns>The quaternion with the shortest arc.</returns>
public static Quaternion shortestArcQuat(ref Vector3 v0, ref Vector3 v1)
{
Vector3 c = v0.cross(ref v1);
float d = v0.dot(ref v1);
if (d < -1.0 + FLT_EPSILON)
{
return(new Quaternion(0.0f, 1.0f, 0.0f, 0.0f)); // just pick any vector
}
float s = (float)System.Math.Sqrt((1.0f + d) * 2.0f);
float rs = 1.0f / s;
return(new Quaternion(c.x * rs, c.y * rs, c.z * rs, s * 0.5f).normalized());
}
/// <summary>
/// Compute the shortest arc quaternion with a fixed yaw axis.
/// </summary>
/// <param name="direction">The direction to face. Must be normalized.</param>
/// <param name="yawFixedAxis">The axis to fix as yaw.</param>
/// <returns>The quaternion that will get to this orientation.</returns>
public static Quaternion shortestArcQuatFixedYaw(ref Vector3 direction, ref Vector3 yawFixedAxis)
{
Vector3 xVec = yawFixedAxis.cross(ref direction);
xVec.normalize();
Vector3 yVec = direction.cross(ref xVec);
yVec.normalize();
Quaternion targetWorldOrientation = new Quaternion();
targetWorldOrientation.fromAxes(xVec, yVec, direction);
return(targetWorldOrientation);
}
