/// <summary>
/// Given an object's current Euler angles and a surface normal, will calculate
/// the Euler angles necessary to rotate the object so that it's up-vector is
/// aligned with the surface normal.
/// </summary>
/// <param name="originalEulers">From an IRenderableNode.Rotation, for example.</param>
/// <param name="surfaceNormal">a unit vector that the object should be rotate-snapped to</param>
/// <param name="upAxis">y or z is up?</param>
/// <returns>The resulting angles to be assigned to IRenderableNode.Rotation</returns>
/// <remarks>
/// Note that QuatF was attempted to be used, but I could not get it to work reliably
/// with the Matrix3F.GetEulerAngles(). Numerical instability? The basis vector
/// method below works well except for when the target surface is 90 degrees different
/// than the starting up vector in which case the rotation around the up vector is lost
/// (gimbal lock) but the results are always valid in the sense that the up vector
/// is aligned with the surface normal. --Ron Little
/// </remarks>
public static Vec3F RotateToVector(Vec3F originalEulers, Vec3F surfaceNormal, AxisSystemType upAxis)
{
// get basis vectors for the current rotation
Matrix3F rotMat = new Matrix3F();
rotMat.Rotation(originalEulers);
Vec3F a1 = rotMat.XAxis;
Vec3F a2 = rotMat.YAxis;
Vec3F a3 = rotMat.ZAxis;
// calculate destination basis vectors
Vec3F b1, b2, b3;
if (upAxis == AxisSystemType.YIsUp)
{
// a2 is the current up vector. b2 is the final up vector.
// now, find either a1 or a3, whichever is most orthogonal to surface
b2 = new Vec3F(surfaceNormal);
float a1DotS = Vec3F.Dot(a1, surfaceNormal);
float a3DotS = Vec3F.Dot(a3, surfaceNormal);
if (Math.Abs(a1DotS) < Math.Abs(a3DotS))
{
b1 = new Vec3F(a1);
b3 = Vec3F.Cross(b1, b2);
b1 = Vec3F.Cross(b2, b3);
}
else
{
b3 = new Vec3F(a3);
b1 = Vec3F.Cross(b2, b3);
b3 = Vec3F.Cross(b1, b2);
}
}
else
{
// a3 is the current up vector. b3 is the final up vector.
// now, find either a1 or a2, whichever is most orthogonal to surface
b3 = new Vec3F(surfaceNormal);
float a1DotS = Vec3F.Dot(a1, surfaceNormal);
float a2DotS = Vec3F.Dot(a2, surfaceNormal);
if (Math.Abs(a1DotS) < Math.Abs(a2DotS))
{
b1 = new Vec3F(a1);
b2 = Vec3F.Cross(b3, b1);
b1 = Vec3F.Cross(b2, b3);
}
else
{
b2 = new Vec3F(a2);
b1 = Vec3F.Cross(b2, b3);
b2 = Vec3F.Cross(b3, b1);
}
}
// in theory, this isn't necessary, but in practice...
b1.Normalize();
b2.Normalize();
b3.Normalize();
// construct new rotation matrix and extract euler angles
rotMat.XAxis = b1;
rotMat.YAxis = b2;
rotMat.ZAxis = b3;
Vec3F newEulers = new Vec3F();
rotMat.GetEulerAngles(out newEulers.X, out newEulers.Y, out newEulers.Z);
return newEulers;
}
/// <summary>
/// Given an object's current Euler angles and a surface normal, will calculate
/// the Euler angles necessary to rotate the object so that it's up-vector is
/// aligned with the surface normal.
/// </summary>
/// <param name="originalEulers">From an IRenderableNode.Rotation, for example.</param>
/// <param name="surfaceNormal">a unit vector that the object should be rotate-snapped to</param>
/// <param name="upAxis">y or z is up?</param>
/// <returns>The resulting angles to be assigned to IRenderableNode.Rotation</returns>
/// <remarks>
/// Note that QuatF was attempted to be used, but I could not get it to work reliably
/// with the Matrix3F.GetEulerAngles(). Numerical instability? The basis vector
/// method below works well except for when the target surface is 90 degrees different
/// than the starting up vector in which case the rotation around the up vector is lost
/// (gimbal lock) but the results are always valid in the sense that the up vector
/// is aligned with the surface normal. --Ron Little
/// </remarks>
public static Vec3F RotateToVector(Vec3F originalEulers, Vec3F surfaceNormal, AxisSystemType upAxis)
{
// get basis vectors for the current rotation
Matrix3F rotMat = new Matrix3F();
rotMat.Rotation(originalEulers);
Vec3F a1 = rotMat.XAxis;
Vec3F a2 = rotMat.YAxis;
Vec3F a3 = rotMat.ZAxis;
// calculate destination basis vectors
Vec3F b1, b2, b3;
if (upAxis == AxisSystemType.YIsUp)
{
// a2 is the current up vector. b2 is the final up vector.
// now, find either a1 or a3, whichever is most orthogonal to surface
b2 = new Vec3F(surfaceNormal);
float a1DotS = Vec3F.Dot(a1, surfaceNormal);
float a3DotS = Vec3F.Dot(a3, surfaceNormal);
if (Math.Abs(a1DotS) < Math.Abs(a3DotS))
{
b1 = new Vec3F(a1);
b3 = Vec3F.Cross(b1, b2);
b1 = Vec3F.Cross(b2, b3);
}
else
{
b3 = new Vec3F(a3);
b1 = Vec3F.Cross(b2, b3);
b3 = Vec3F.Cross(b1, b2);
}
}
else
{
// a3 is the current up vector. b3 is the final up vector.
// now, find either a1 or a2, whichever is most orthogonal to surface
b3 = new Vec3F(surfaceNormal);
float a1DotS = Vec3F.Dot(a1, surfaceNormal);
float a2DotS = Vec3F.Dot(a2, surfaceNormal);
if (Math.Abs(a1DotS) < Math.Abs(a2DotS))
{
b1 = new Vec3F(a1);
b2 = Vec3F.Cross(b3, b1);
b1 = Vec3F.Cross(b2, b3);
}
else
{
b2 = new Vec3F(a2);
b1 = Vec3F.Cross(b2, b3);
b2 = Vec3F.Cross(b3, b1);
}
}
// in theory, this isn't necessary, but in practice...
b1.Normalize();
b2.Normalize();
b3.Normalize();
// construct new rotation matrix and extract euler angles
rotMat.XAxis = b1;
rotMat.YAxis = b2;
rotMat.ZAxis = b3;
Vec3F newEulers = new Vec3F();
rotMat.GetEulerAngles(out newEulers.X, out newEulers.Y, out newEulers.Z);
return(newEulers);
}
