/// <summary>
/// Creates a new Quaternion from the given yaw, pitch, and roll, in radians.
/// </summary>
/// <param name="yaw">The yaw angle, in radians, around the Y-axis.</param>
/// <param name="pitch">The pitch angle, in radians, around the X-axis.</param>
/// <param name="roll">The roll angle, in radians, around the Z-axis.</param>
/// <returns></returns>
public static Quaternion CreateFromYawPitchRoll(Single yaw, Single pitch, Single roll)
{
// Roll first, about axis the object is facing, then
// pitch upward, then yaw to face into the new heading
Single sr, cr, sp, cp, sy, cy;
Single halfRoll = roll * 0.5f;
sr = MathF.Sin(halfRoll);
cr = MathF.Cos(halfRoll);
Single halfPitch = pitch * 0.5f;
sp = MathF.Sin(halfPitch);
cp = MathF.Cos(halfPitch);
Single halfYaw = yaw * 0.5f;
sy = MathF.Sin(halfYaw);
cy = MathF.Cos(halfYaw);
Quaternion result;
result.X = cy * sp * cr + sy * cp * sr;
result.Y = sy * cp * cr - cy * sp * sr;
result.Z = cy * cp * sr - sy * sp * cr;
result.W = cy * cp * cr + sy * sp * sr;
return(result);
}
/// <summary>
/// Creates a rotation matrix using the given rotation in radians and a center point.
/// </summary>
/// <param name="radians">The amount of rotation, in radians.</param>
/// <param name="centerPoint">The center point.</param>
/// <returns>A rotation matrix.</returns>
public static Matrix3x2 CreateRotation(Single radians, Vector2 centerPoint)
{
Matrix3x2 result;
radians = MathF.IEEERemainder(radians, MathF.PI * 2);
Single c, s;
if (radians > -RotationEpsilon && radians < RotationEpsilon)
{
// Exact case for zero rotation.
c = 1;
s = 0;
}
else if (radians > MathF.PI / 2 - RotationEpsilon && radians < MathF.PI / 2 + RotationEpsilon)
{
// Exact case for 90 degree rotation.
c = 0;
s = 1;
}
else if (radians < -MathF.PI + RotationEpsilon || radians > MathF.PI - RotationEpsilon)
{
// Exact case for 180 degree rotation.
c = -1;
s = 0;
}
else if (radians > -MathF.PI / 2 - RotationEpsilon && radians < -MathF.PI / 2 + RotationEpsilon)
{
// Exact case for 270 degree rotation.
c = 0;
s = -1;
}
else
{
// Arbitrary rotation.
c = MathF.Cos(radians);
s = MathF.Sin(radians);
}
Single x = centerPoint.X * (1 - c) + centerPoint.Y * s;
Single y = centerPoint.Y * (1 - c) - centerPoint.X * s;
// [ c s ]
// [ -s c ]
// [ x y ]
result.M11 = c;
result.M12 = s;
result.M21 = -s;
result.M22 = c;
result.M31 = x;
result.M32 = y;
return(result);
}
public void QuaternionCreateFromAxisAngleTest1()
{
Vector3 axis = new Vector3();
Single angle = MathHelper.ToRadians(-30.0f);
Single cos = (Single)Math.Cos(angle / 2.0f);
Quaternion actual = Quaternion.CreateFromAxisAngle(axis, angle);
Assert.True(actual.X == 0.0f && actual.Y == 0.0f && actual.Z == 0.0f && MathHelper.Equal(cos, actual.W)
, "Quaternion.CreateFromAxisAngle did not return the expected value.");
}
public Single Length()
{
if (Vector.IsHardwareAccelerated)
{
Single ls = Vector2.Dot(this, this);
return(MathF.Sqrt(ls));
}
else
{
Single ls = X * X + Y * Y;
return(MathF.Sqrt(ls));
}
}
/// <summary>
/// Creates a rotation matrix using the given rotation in radians.
/// </summary>
/// <param name="radians">The amount of rotation, in radians.</param>
/// <returns>A rotation matrix.</returns>
public static Matrix3x2 CreateRotation(Single radians)
{
Matrix3x2 result;
radians = MathF.IEEERemainder(radians, MathF.PI * 2);
Single c, s;
if (radians > -RotationEpsilon && radians < RotationEpsilon)
{
// Exact case for zero rotation.
c = 1;
s = 0;
}
else if (radians > MathF.PI / 2 - RotationEpsilon && radians < MathF.PI / 2 + RotationEpsilon)
{
// Exact case for 90 degree rotation.
c = 0;
s = 1;
}
else if (radians < -MathF.PI + RotationEpsilon || radians > MathF.PI - RotationEpsilon)
{
// Exact case for 180 degree rotation.
c = -1;
s = 0;
}
else if (radians > -MathF.PI / 2 - RotationEpsilon && radians < -MathF.PI / 2 + RotationEpsilon)
{
// Exact case for 270 degree rotation.
c = 0;
s = -1;
}
else
{
// Arbitrary rotation.
c = MathF.Cos(radians);
s = MathF.Sin(radians);
}
// [ c s ]
// [ -s c ]
// [ 0 0 ]
result.M11 = c;
result.M12 = s;
result.M21 = -s;
result.M22 = c;
result.M31 = 0.0f;
result.M32 = 0.0f;
return(result);
}
public static Vector3 Normalize(Vector3 value)
{
if (Vector.IsHardwareAccelerated)
{
Single length = value.Length();
return(value / length);
}
else
{
Single ls = value.X * value.X + value.Y * value.Y + value.Z * value.Z;
Single invLength = Single.One / MathF.Sqrt(ls);
return(new Vector3(value.X * invLength, value.Y * invLength, value.Z * invLength));
}
}
/// <summary>
/// Creates a Quaternion from a normalized vector axis and an angle to rotate about the vector.
/// </summary>
/// <param name="axis">The unit vector to rotate around.
/// This vector must be normalized before calling this function or the resulting Quaternion will be incorrect.</param>
/// <param name="angle">The angle, in radians, to rotate around the vector.</param>
/// <returns>The created Quaternion.</returns>
public static Quaternion CreateFromAxisAngle(Vector3 axis, Single angle)
{
Quaternion ans;
Single halfAngle = angle * 0.5f;
Single s = MathF.Sin(halfAngle);
Single c = MathF.Cos(halfAngle);
ans.X = axis.X * s;
ans.Y = axis.Y * s;
ans.Z = axis.Z * s;
ans.W = c;
return(ans);
}
/// <summary>
/// Divides each component of the Quaternion by the length of the Quaternion.
/// </summary>
/// <param name="value">The source Quaternion.</param>
/// <returns>The normalized Quaternion.</returns>
public static Quaternion Normalize(Quaternion value)
{
Quaternion ans;
Single ls = value.X * value.X + value.Y * value.Y + value.Z * value.Z + value.W * value.W;
Single invNorm = 1.0f / MathF.Sqrt(ls);
ans.X = value.X * invNorm;
ans.Y = value.Y * invNorm;
ans.Z = value.Z * invNorm;
ans.W = value.W * invNorm;
return(ans);
}
/// <summary>
/// Creates a Quaternion from the given rotation matrix.
/// </summary>
/// <param name="matrix">The rotation matrix.</param>
/// <returns>The created Quaternion.</returns>
public static Quaternion CreateFromRotationMatrix(Matrix4x4 matrix)
{
Single trace = matrix.M11 + matrix.M22 + matrix.M33;
Quaternion q = new Quaternion();
if (trace > 0.0f)
{
Single s = MathF.Sqrt(trace + 1.0f);
q.W = s * 0.5f;
s = 0.5f / s;
q.X = (matrix.M23 - matrix.M32) * s;
q.Y = (matrix.M31 - matrix.M13) * s;
q.Z = (matrix.M12 - matrix.M21) * s;
}
else
{
if (matrix.M11 >= matrix.M22 && matrix.M11 >= matrix.M33)
{
Single s = MathF.Sqrt(1.0f + matrix.M11 - matrix.M22 - matrix.M33);
Single invS = 0.5f / s;
q.X = 0.5f * s;
q.Y = (matrix.M12 + matrix.M21) * invS;
q.Z = (matrix.M13 + matrix.M31) * invS;
q.W = (matrix.M23 - matrix.M32) * invS;
}
else if (matrix.M22 > matrix.M33)
{
Single s = MathF.Sqrt(1.0f + matrix.M22 - matrix.M11 - matrix.M33);
Single invS = 0.5f / s;
q.X = (matrix.M21 + matrix.M12) * invS;
q.Y = 0.5f * s;
q.Z = (matrix.M32 + matrix.M23) * invS;
q.W = (matrix.M31 - matrix.M13) * invS;
}
else
{
Single s = MathF.Sqrt(1.0f + matrix.M33 - matrix.M11 - matrix.M22);
Single invS = 0.5f / s;
q.X = (matrix.M31 + matrix.M13) * invS;
q.Y = (matrix.M32 + matrix.M23) * invS;
q.Z = 0.5f * s;
q.W = (matrix.M12 - matrix.M21) * invS;
}
}
return(q);
}
/// <summary>
/// Creates a skew matrix from the given angles in radians.
/// </summary>
/// <param name="radiansX">The X angle, in radians.</param>
/// <param name="radiansY">The Y angle, in radians.</param>
/// <returns>A skew matrix.</returns>
public static Matrix3x2 CreateSkew(Single radiansX, Single radiansY)
{
Matrix3x2 result;
Single xTan = MathF.Tan(radiansX);
Single yTan = MathF.Tan(radiansY);
result.M11 = 1.0f;
result.M12 = yTan;
result.M21 = xTan;
result.M22 = 1.0f;
result.M31 = 0.0f;
result.M32 = 0.0f;
return(result);
}
public static Vector2 Normalize(Vector2 value)
{
if (Vector.IsHardwareAccelerated)
{
Single length = value.Length();
return(value / length);
}
else
{
Single ls = value.X * value.X + value.Y * value.Y;
Single invNorm = Single.One / MathF.Sqrt(ls);
return(new Vector2(
value.X * invNorm,
value.Y * invNorm));
}
}
public static Single Distance(Vector2 value1, Vector2 value2)
{
if (Vector.IsHardwareAccelerated)
{
Vector2 difference = value1 - value2;
Single ls = Vector2.Dot(difference, difference);
return(MathF.Sqrt(ls));
}
else
{
Single dx = value1.X - value2.X;
Single dy = value1.Y - value2.Y;
Single ls = dx * dx + dy * dy;
return(MathF.Sqrt(ls));
}
}
/// <summary>
/// Interpolates between two quaternions, using spherical linear interpolation.
/// </summary>
/// <param name="quaternion1">The first source Quaternion.</param>
/// <param name="quaternion2">The second source Quaternion.</param>
/// <param name="amount">The relative weight of the second source Quaternion in the interpolation.</param>
/// <returns>The interpolated Quaternion.</returns>
public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, Single amount)
{
Single t = amount;
Single cosOmega = quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y +
quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W;
bool flip = false;
if (cosOmega < 0.0f)
{
flip = true;
cosOmega = -cosOmega;
}
Single s1, s2;
if (cosOmega > (1.0f - SlerpEpsilon))
{
// Too close, do straight linear interpolation.
s1 = 1.0f - t;
s2 = (flip) ? -t : t;
}
else
{
Single omega = MathF.Acos(cosOmega);
Single invSinOmega = 1 / MathF.Sin(omega);
s1 = MathF.Sin((1.0f - t) * omega) * invSinOmega;
s2 = (flip)
? -MathF.Sin(t * omega) * invSinOmega
: MathF.Sin(t * omega) * invSinOmega;
}
Quaternion ans;
ans.X = s1 * quaternion1.X + s2 * quaternion2.X;
ans.Y = s1 * quaternion1.Y + s2 * quaternion2.Y;
ans.Z = s1 * quaternion1.Z + s2 * quaternion2.Z;
ans.W = s1 * quaternion1.W + s2 * quaternion2.W;
return(ans);
}
/// <summary>
/// Creates a skew matrix from the given angles in radians and a center point.
/// </summary>
/// <param name="radiansX">The X angle, in radians.</param>
/// <param name="radiansY">The Y angle, in radians.</param>
/// <param name="centerPoint">The center point.</param>
/// <returns>A skew matrix.</returns>
public static Matrix3x2 CreateSkew(Single radiansX, Single radiansY, Vector2 centerPoint)
{
Matrix3x2 result;
Single xTan = MathF.Tan(radiansX);
Single yTan = MathF.Tan(radiansY);
Single tx = -centerPoint.Y * xTan;
Single ty = -centerPoint.X * yTan;
result.M11 = 1.0f;
result.M12 = yTan;
result.M21 = xTan;
result.M22 = 1.0f;
result.M31 = tx;
result.M32 = ty;
return(result);
}
public static Vector4 Normalize(Vector4 vector)
{
if (Vector.IsHardwareAccelerated)
{
Single length = vector.Length();
return(vector / length);
}
else
{
Single ls = vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z + vector.W * vector.W;
Single invNorm = 1.0f / MathF.Sqrt(ls);
return(new Vector4(
vector.X * invNorm,
vector.Y * invNorm,
vector.Z * invNorm,
vector.W * invNorm));
}
}
public static Single Distance(Vector4 value1, Vector4 value2)
{
if (Vector.IsHardwareAccelerated)
{
Vector4 difference = value1 - value2;
Single ls = Vector4.Dot(difference, difference);
return(MathF.Sqrt(ls));
}
else
{
Single dx = value1.X - value2.X;
Single dy = value1.Y - value2.Y;
Single dz = value1.Z - value2.Z;
Single dw = value1.W - value2.W;
Single ls = dx * dx + dy * dy + dz * dz + dw * dw;
return(MathF.Sqrt(ls));
}
}
/// <summary>
/// Linearly interpolates between two quaternions.
/// </summary>
/// <param name="quaternion1">The first source Quaternion.</param>
/// <param name="quaternion2">The second source Quaternion.</param>
/// <param name="amount">The relative weight of the second source Quaternion in the interpolation.</param>
/// <returns>The interpolated Quaternion.</returns>
public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, Single amount)
{
Single t = amount;
Single t1 = 1.0f - t;
Quaternion r = new Quaternion();
Single dot = quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y +
quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W;
if (dot >= 0.0f)
{
r.X = t1 * quaternion1.X + t * quaternion2.X;
r.Y = t1 * quaternion1.Y + t * quaternion2.Y;
r.Z = t1 * quaternion1.Z + t * quaternion2.Z;
r.W = t1 * quaternion1.W + t * quaternion2.W;
}
else
{
r.X = t1 * quaternion1.X - t * quaternion2.X;
r.Y = t1 * quaternion1.Y - t * quaternion2.Y;
r.Z = t1 * quaternion1.Z - t * quaternion2.Z;
r.W = t1 * quaternion1.W - t * quaternion2.W;
}
// Normalize it.
Single ls = r.X * r.X + r.Y * r.Y + r.Z * r.Z + r.W * r.W;
Single invNorm = 1.0f / MathF.Sqrt(ls);
r.X *= invNorm;
r.Y *= invNorm;
r.Z *= invNorm;
r.W *= invNorm;
return(r);
}
