/// <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); }