|
// Copyright (c) Microsoft. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for full license information.
using System.Globalization;
namespace System.Numerics
{
/// <summary>
/// A structure encapsulating a 4x4 matrix.
/// </summary>
public struct Matrix4x4 : IEquatable<Matrix4x4>
{
#region Public Fields
/// <summary>
/// Value at row 1, column 1 of the matrix.
/// </summary>
public float M11;
/// <summary>
/// Value at row 1, column 2 of the matrix.
/// </summary>
public float M12;
/// <summary>
/// Value at row 1, column 3 of the matrix.
/// </summary>
public float M13;
/// <summary>
/// Value at row 1, column 4 of the matrix.
/// </summary>
public float M14;
/// <summary>
/// Value at row 2, column 1 of the matrix.
/// </summary>
public float M21;
/// <summary>
/// Value at row 2, column 2 of the matrix.
/// </summary>
public float M22;
/// <summary>
/// Value at row 2, column 3 of the matrix.
/// </summary>
public float M23;
/// <summary>
/// Value at row 2, column 4 of the matrix.
/// </summary>
public float M24;
/// <summary>
/// Value at row 3, column 1 of the matrix.
/// </summary>
public float M31;
/// <summary>
/// Value at row 3, column 2 of the matrix.
/// </summary>
public float M32;
/// <summary>
/// Value at row 3, column 3 of the matrix.
/// </summary>
public float M33;
/// <summary>
/// Value at row 3, column 4 of the matrix.
/// </summary>
public float M34;
/// <summary>
/// Value at row 4, column 1 of the matrix.
/// </summary>
public float M41;
/// <summary>
/// Value at row 4, column 2 of the matrix.
/// </summary>
public float M42;
/// <summary>
/// Value at row 4, column 3 of the matrix.
/// </summary>
public float M43;
/// <summary>
/// Value at row 4, column 4 of the matrix.
/// </summary>
public float M44;
#endregion Public Fields
private static readonly Matrix4x4 _identity = new Matrix4x4
(
1f, 0f, 0f, 0f,
0f, 1f, 0f, 0f,
0f, 0f, 1f, 0f,
0f, 0f, 0f, 1f
);
/// <summary>
/// Returns the multiplicative identity matrix.
/// </summary>
public static Matrix4x4 Identity
{
get { return _identity; }
}
/// <summary>
/// Returns whether the matrix is the identity matrix.
/// </summary>
public bool IsIdentity
{
get
{
return M11 == 1f && M22 == 1f && M33 == 1f && M44 == 1f && // Check diagonal element first for early out.
M12 == 0f && M13 == 0f && M14 == 0f &&
M21 == 0f && M23 == 0f && M24 == 0f &&
M31 == 0f && M32 == 0f && M34 == 0f &&
M41 == 0f && M42 == 0f && M43 == 0f;
}
}
/// <summary>
/// Gets or sets the translation component of this matrix.
/// </summary>
public Vector3 Translation
{
get
{
return new Vector3(M41, M42, M43);
}
set
{
M41 = value.X;
M42 = value.Y;
M43 = value.Z;
}
}
/// <summary>
/// Constructs a Matrix4x4 from the given components.
/// </summary>
public Matrix4x4(float m11, float m12, float m13, float m14,
float m21, float m22, float m23, float m24,
float m31, float m32, float m33, float m34,
float m41, float m42, float m43, float m44)
{
this.M11 = m11;
this.M12 = m12;
this.M13 = m13;
this.M14 = m14;
this.M21 = m21;
this.M22 = m22;
this.M23 = m23;
this.M24 = m24;
this.M31 = m31;
this.M32 = m32;
this.M33 = m33;
this.M34 = m34;
this.M41 = m41;
this.M42 = m42;
this.M43 = m43;
this.M44 = m44;
}
/// <summary>
/// Constructs a Matrix4x4 from the given Matrix3x2.
/// </summary>
/// <param name="value">The source Matrix3x2.</param>
public Matrix4x4(Matrix3x2 value)
{
M11 = value.M11;
M12 = value.M12;
M13 = 0f;
M14 = 0f;
M21 = value.M21;
M22 = value.M22;
M23 = 0f;
M24 = 0f;
M31 = 0f;
M32 = 0f;
M33 = 1f;
M34 = 0f;
M41 = value.M31;
M42 = value.M32;
M43 = 0f;
M44 = 1f;
}
/// <summary>
/// Creates a spherical billboard that rotates around a specified object position.
/// </summary>
/// <param name="objectPosition">Position of the object the billboard will rotate around.</param>
/// <param name="cameraPosition">Position of the camera.</param>
/// <param name="cameraUpVector">The up vector of the camera.</param>
/// <param name="cameraForwardVector">The forward vector of the camera.</param>
/// <returns>The created billboard matrix</returns>
public static Matrix4x4 CreateBillboard(Vector3 objectPosition, Vector3 cameraPosition, Vector3 cameraUpVector, Vector3 cameraForwardVector)
{
const float epsilon = 1e-4f;
Vector3 zaxis = new Vector3(
objectPosition.X - cameraPosition.X,
objectPosition.Y - cameraPosition.Y,
objectPosition.Z - cameraPosition.Z);
float norm = zaxis.LengthSquared();
if (norm < epsilon)
{
zaxis = -cameraForwardVector;
}
else
{
zaxis = Vector3.Multiply(zaxis, 1.0f / (float)Math.Sqrt(norm));
}
Vector3 xaxis = Vector3.Normalize(Vector3.Cross(cameraUpVector, zaxis));
Vector3 yaxis = Vector3.Cross(zaxis, xaxis);
Matrix4x4 result;
result.M11 = xaxis.X;
result.M12 = xaxis.Y;
result.M13 = xaxis.Z;
result.M14 = 0.0f;
result.M21 = yaxis.X;
result.M22 = yaxis.Y;
result.M23 = yaxis.Z;
result.M24 = 0.0f;
result.M31 = zaxis.X;
result.M32 = zaxis.Y;
result.M33 = zaxis.Z;
result.M34 = 0.0f;
result.M41 = objectPosition.X;
result.M42 = objectPosition.Y;
result.M43 = objectPosition.Z;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a cylindrical billboard that rotates around a specified axis.
/// </summary>
/// <param name="objectPosition">Position of the object the billboard will rotate around.</param>
/// <param name="cameraPosition">Position of the camera.</param>
/// <param name="rotateAxis">Axis to rotate the billboard around.</param>
/// <param name="cameraForwardVector">Forward vector of the camera.</param>
/// <param name="objectForwardVector">Forward vector of the object.</param>
/// <returns>The created billboard matrix.</returns>
public static Matrix4x4 CreateConstrainedBillboard(Vector3 objectPosition, Vector3 cameraPosition, Vector3 rotateAxis, Vector3 cameraForwardVector, Vector3 objectForwardVector)
{
const float epsilon = 1e-4f;
const float minAngle = 1.0f - (0.1f * ((float)Math.PI / 180.0f)); // 0.1 degrees
// Treat the case when object and camera positions are too close.
Vector3 faceDir = new Vector3(
objectPosition.X - cameraPosition.X,
objectPosition.Y - cameraPosition.Y,
objectPosition.Z - cameraPosition.Z);
float norm = faceDir.LengthSquared();
if (norm < epsilon)
{
faceDir = -cameraForwardVector;
}
else
{
faceDir = Vector3.Multiply(faceDir, (1.0f / (float)Math.Sqrt(norm)));
}
Vector3 yaxis = rotateAxis;
Vector3 xaxis;
Vector3 zaxis;
// Treat the case when angle between faceDir and rotateAxis is too close to 0.
float dot = Vector3.Dot(rotateAxis, faceDir);
if (Math.Abs(dot) > minAngle)
{
zaxis = objectForwardVector;
// Make sure passed values are useful for compute.
dot = Vector3.Dot(rotateAxis, zaxis);
if (Math.Abs(dot) > minAngle)
{
zaxis = (Math.Abs(rotateAxis.Z) > minAngle) ? new Vector3(1, 0, 0) : new Vector3(0, 0, -1);
}
xaxis = Vector3.Normalize(Vector3.Cross(rotateAxis, zaxis));
zaxis = Vector3.Normalize(Vector3.Cross(xaxis, rotateAxis));
}
else
{
xaxis = Vector3.Normalize(Vector3.Cross(rotateAxis, faceDir));
zaxis = Vector3.Normalize(Vector3.Cross(xaxis, yaxis));
}
Matrix4x4 result;
result.M11 = xaxis.X;
result.M12 = xaxis.Y;
result.M13 = xaxis.Z;
result.M14 = 0.0f;
result.M21 = yaxis.X;
result.M22 = yaxis.Y;
result.M23 = yaxis.Z;
result.M24 = 0.0f;
result.M31 = zaxis.X;
result.M32 = zaxis.Y;
result.M33 = zaxis.Z;
result.M34 = 0.0f;
result.M41 = objectPosition.X;
result.M42 = objectPosition.Y;
result.M43 = objectPosition.Z;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a translation matrix.
/// </summary>
/// <param name="position">The amount to translate in each axis.</param>
/// <returns>The translation matrix.</returns>
public static Matrix4x4 CreateTranslation(Vector3 position)
{
Matrix4x4 result;
result.M11 = 1.0f;
result.M12 = 0.0f;
result.M13 = 0.0f;
result.M14 = 0.0f;
result.M21 = 0.0f;
result.M22 = 1.0f;
result.M23 = 0.0f;
result.M24 = 0.0f;
result.M31 = 0.0f;
result.M32 = 0.0f;
result.M33 = 1.0f;
result.M34 = 0.0f;
result.M41 = position.X;
result.M42 = position.Y;
result.M43 = position.Z;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a translation matrix.
/// </summary>
/// <param name="xPosition">The amount to translate on the X-axis.</param>
/// <param name="yPosition">The amount to translate on the Y-axis.</param>
/// <param name="zPosition">The amount to translate on the Z-axis.</param>
/// <returns>The translation matrix.</returns>
public static Matrix4x4 CreateTranslation(float xPosition, float yPosition, float zPosition)
{
Matrix4x4 result;
result.M11 = 1.0f;
result.M12 = 0.0f;
result.M13 = 0.0f;
result.M14 = 0.0f;
result.M21 = 0.0f;
result.M22 = 1.0f;
result.M23 = 0.0f;
result.M24 = 0.0f;
result.M31 = 0.0f;
result.M32 = 0.0f;
result.M33 = 1.0f;
result.M34 = 0.0f;
result.M41 = xPosition;
result.M42 = yPosition;
result.M43 = zPosition;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a scaling matrix.
/// </summary>
/// <param name="xScale">Value to scale by on the X-axis.</param>
/// <param name="yScale">Value to scale by on the Y-axis.</param>
/// <param name="zScale">Value to scale by on the Z-axis.</param>
/// <returns>The scaling matrix.</returns>
public static Matrix4x4 CreateScale(float xScale, float yScale, float zScale)
{
Matrix4x4 result;
result.M11 = xScale;
result.M12 = 0.0f;
result.M13 = 0.0f;
result.M14 = 0.0f;
result.M21 = 0.0f;
result.M22 = yScale;
result.M23 = 0.0f;
result.M24 = 0.0f;
result.M31 = 0.0f;
result.M32 = 0.0f;
result.M33 = zScale;
result.M34 = 0.0f;
result.M41 = 0.0f;
result.M42 = 0.0f;
result.M43 = 0.0f;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a scaling matrix with a center point.
/// </summary>
/// <param name="xScale">Value to scale by on the X-axis.</param>
/// <param name="yScale">Value to scale by on the Y-axis.</param>
/// <param name="zScale">Value to scale by on the Z-axis.</param>
/// <param name="centerPoint">The center point.</param>
/// <returns>The scaling matrix.</returns>
public static Matrix4x4 CreateScale(float xScale, float yScale, float zScale, Vector3 centerPoint)
{
Matrix4x4 result;
float tx = centerPoint.X * (1 - xScale);
float ty = centerPoint.Y * (1 - yScale);
float tz = centerPoint.Z * (1 - zScale);
result.M11 = xScale;
result.M12 = 0.0f;
result.M13 = 0.0f;
result.M14 = 0.0f;
result.M21 = 0.0f;
result.M22 = yScale;
result.M23 = 0.0f;
result.M24 = 0.0f;
result.M31 = 0.0f;
result.M32 = 0.0f;
result.M33 = zScale;
result.M34 = 0.0f;
result.M41 = tx;
result.M42 = ty;
result.M43 = tz;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a scaling matrix.
/// </summary>
/// <param name="scales">The vector containing the amount to scale by on each axis.</param>
/// <returns>The scaling matrix.</returns>
public static Matrix4x4 CreateScale(Vector3 scales)
{
Matrix4x4 result;
result.M11 = scales.X;
result.M12 = 0.0f;
result.M13 = 0.0f;
result.M14 = 0.0f;
result.M21 = 0.0f;
result.M22 = scales.Y;
result.M23 = 0.0f;
result.M24 = 0.0f;
result.M31 = 0.0f;
result.M32 = 0.0f;
result.M33 = scales.Z;
result.M34 = 0.0f;
result.M41 = 0.0f;
result.M42 = 0.0f;
result.M43 = 0.0f;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a scaling matrix with a center point.
/// </summary>
/// <param name="scales">The vector containing the amount to scale by on each axis.</param>
/// <param name="centerPoint">The center point.</param>
/// <returns>The scaling matrix.</returns>
public static Matrix4x4 CreateScale(Vector3 scales, Vector3 centerPoint)
{
Matrix4x4 result;
float tx = centerPoint.X * (1 - scales.X);
float ty = centerPoint.Y * (1 - scales.Y);
float tz = centerPoint.Z * (1 - scales.Z);
result.M11 = scales.X;
result.M12 = 0.0f;
result.M13 = 0.0f;
result.M14 = 0.0f;
result.M21 = 0.0f;
result.M22 = scales.Y;
result.M23 = 0.0f;
result.M24 = 0.0f;
result.M31 = 0.0f;
result.M32 = 0.0f;
result.M33 = scales.Z;
result.M34 = 0.0f;
result.M41 = tx;
result.M42 = ty;
result.M43 = tz;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a uniform scaling matrix that scales equally on each axis.
/// </summary>
/// <param name="scale">The uniform scaling factor.</param>
/// <returns>The scaling matrix.</returns>
public static Matrix4x4 CreateScale(float scale)
{
Matrix4x4 result;
result.M11 = scale;
result.M12 = 0.0f;
result.M13 = 0.0f;
result.M14 = 0.0f;
result.M21 = 0.0f;
result.M22 = scale;
result.M23 = 0.0f;
result.M24 = 0.0f;
result.M31 = 0.0f;
result.M32 = 0.0f;
result.M33 = scale;
result.M34 = 0.0f;
result.M41 = 0.0f;
result.M42 = 0.0f;
result.M43 = 0.0f;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a uniform scaling matrix that scales equally on each axis with a center point.
/// </summary>
/// <param name="scale">The uniform scaling factor.</param>
/// <param name="centerPoint">The center point.</param>
/// <returns>The scaling matrix.</returns>
public static Matrix4x4 CreateScale(float scale, Vector3 centerPoint)
{
Matrix4x4 result;
float tx = centerPoint.X * (1 - scale);
float ty = centerPoint.Y * (1 - scale);
float tz = centerPoint.Z * (1 - scale);
result.M11 = scale;
result.M12 = 0.0f;
result.M13 = 0.0f;
result.M14 = 0.0f;
result.M21 = 0.0f;
result.M22 = scale;
result.M23 = 0.0f;
result.M24 = 0.0f;
result.M31 = 0.0f;
result.M32 = 0.0f;
result.M33 = scale;
result.M34 = 0.0f;
result.M41 = tx;
result.M42 = ty;
result.M43 = tz;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a matrix for rotating points around the X-axis.
/// </summary>
/// <param name="radians">The amount, in radians, by which to rotate around the X-axis.</param>
/// <returns>The rotation matrix.</returns>
public static Matrix4x4 CreateRotationX(float radians)
{
Matrix4x4 result;
float c = (float)Math.Cos(radians);
float s = (float)Math.Sin(radians);
// [ 1 0 0 0 ]
// [ 0 c s 0 ]
// [ 0 -s c 0 ]
// [ 0 0 0 1 ]
result.M11 = 1.0f;
result.M12 = 0.0f;
result.M13 = 0.0f;
result.M14 = 0.0f;
result.M21 = 0.0f;
result.M22 = c;
result.M23 = s;
result.M24 = 0.0f;
result.M31 = 0.0f;
result.M32 = -s;
result.M33 = c;
result.M34 = 0.0f;
result.M41 = 0.0f;
result.M42 = 0.0f;
result.M43 = 0.0f;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a matrix for rotating points around the X-axis, from a center point.
/// </summary>
/// <param name="radians">The amount, in radians, by which to rotate around the X-axis.</param>
/// <param name="centerPoint">The center point.</param>
/// <returns>The rotation matrix.</returns>
public static Matrix4x4 CreateRotationX(float radians, Vector3 centerPoint)
{
Matrix4x4 result;
float c = (float)Math.Cos(radians);
float s = (float)Math.Sin(radians);
float y = centerPoint.Y * (1 - c) + centerPoint.Z * s;
float z = centerPoint.Z * (1 - c) - centerPoint.Y * s;
// [ 1 0 0 0 ]
// [ 0 c s 0 ]
// [ 0 -s c 0 ]
// [ 0 y z 1 ]
result.M11 = 1.0f;
result.M12 = 0.0f;
result.M13 = 0.0f;
result.M14 = 0.0f;
result.M21 = 0.0f;
result.M22 = c;
result.M23 = s;
result.M24 = 0.0f;
result.M31 = 0.0f;
result.M32 = -s;
result.M33 = c;
result.M34 = 0.0f;
result.M41 = 0.0f;
result.M42 = y;
result.M43 = z;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a matrix for rotating points around the Y-axis.
/// </summary>
/// <param name="radians">The amount, in radians, by which to rotate around the Y-axis.</param>
/// <returns>The rotation matrix.</returns>
public static Matrix4x4 CreateRotationY(float radians)
{
Matrix4x4 result;
float c = (float)Math.Cos(radians);
float s = (float)Math.Sin(radians);
// [ c 0 -s 0 ]
// [ 0 1 0 0 ]
// [ s 0 c 0 ]
// [ 0 0 0 1 ]
result.M11 = c;
result.M12 = 0.0f;
result.M13 = -s;
result.M14 = 0.0f;
result.M21 = 0.0f;
result.M22 = 1.0f;
result.M23 = 0.0f;
result.M24 = 0.0f;
result.M31 = s;
result.M32 = 0.0f;
result.M33 = c;
result.M34 = 0.0f;
result.M41 = 0.0f;
result.M42 = 0.0f;
result.M43 = 0.0f;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a matrix for rotating points around the Y-axis, from a center point.
/// </summary>
/// <param name="radians">The amount, in radians, by which to rotate around the Y-axis.</param>
/// <param name="centerPoint">The center point.</param>
/// <returns>The rotation matrix.</returns>
public static Matrix4x4 CreateRotationY(float radians, Vector3 centerPoint)
{
Matrix4x4 result;
float c = (float)Math.Cos(radians);
float s = (float)Math.Sin(radians);
float x = centerPoint.X * (1 - c) - centerPoint.Z * s;
float z = centerPoint.Z * (1 - c) + centerPoint.X * s;
// [ c 0 -s 0 ]
// [ 0 1 0 0 ]
// [ s 0 c 0 ]
// [ x 0 z 1 ]
result.M11 = c;
result.M12 = 0.0f;
result.M13 = -s;
result.M14 = 0.0f;
result.M21 = 0.0f;
result.M22 = 1.0f;
result.M23 = 0.0f;
result.M24 = 0.0f;
result.M31 = s;
result.M32 = 0.0f;
result.M33 = c;
result.M34 = 0.0f;
result.M41 = x;
result.M42 = 0.0f;
result.M43 = z;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a matrix for rotating points around the Z-axis.
/// </summary>
/// <param name="radians">The amount, in radians, by which to rotate around the Z-axis.</param>
/// <returns>The rotation matrix.</returns>
public static Matrix4x4 CreateRotationZ(float radians)
{
Matrix4x4 result;
float c = (float)Math.Cos(radians);
float s = (float)Math.Sin(radians);
// [ c s 0 0 ]
// [ -s c 0 0 ]
// [ 0 0 1 0 ]
// [ 0 0 0 1 ]
result.M11 = c;
result.M12 = s;
result.M13 = 0.0f;
result.M14 = 0.0f;
result.M21 = -s;
result.M22 = c;
result.M23 = 0.0f;
result.M24 = 0.0f;
result.M31 = 0.0f;
result.M32 = 0.0f;
result.M33 = 1.0f;
result.M34 = 0.0f;
result.M41 = 0.0f;
result.M42 = 0.0f;
result.M43 = 0.0f;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a matrix for rotating points around the Z-axis, from a center point.
/// </summary>
/// <param name="radians">The amount, in radians, by which to rotate around the Z-axis.</param>
/// <param name="centerPoint">The center point.</param>
/// <returns>The rotation matrix.</returns>
public static Matrix4x4 CreateRotationZ(float radians, Vector3 centerPoint)
{
Matrix4x4 result;
float c = (float)Math.Cos(radians);
float s = (float)Math.Sin(radians);
float x = centerPoint.X * (1 - c) + centerPoint.Y * s;
float y = centerPoint.Y * (1 - c) - centerPoint.X * s;
// [ c s 0 0 ]
// [ -s c 0 0 ]
// [ 0 0 1 0 ]
// [ x y 0 1 ]
result.M11 = c;
result.M12 = s;
result.M13 = 0.0f;
result.M14 = 0.0f;
result.M21 = -s;
result.M22 = c;
result.M23 = 0.0f;
result.M24 = 0.0f;
result.M31 = 0.0f;
result.M32 = 0.0f;
result.M33 = 1.0f;
result.M34 = 0.0f;
result.M41 = x;
result.M42 = y;
result.M43 = 0.0f;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a matrix that rotates around an arbitrary vector.
/// </summary>
/// <param name="axis">The axis to rotate around.</param>
/// <param name="angle">The angle to rotate around the given axis, in radians.</param>
/// <returns>The rotation matrix.</returns>
public static Matrix4x4 CreateFromAxisAngle(Vector3 axis, float angle)
{
// a: angle
// x, y, z: unit vector for axis.
//
// Rotation matrix M can compute by using below equation.
//
// T T
// M = uu + (cos a)( I-uu ) + (sin a)S
//
// Where:
//
// u = ( x, y, z )
//
// [ 0 -z y ]
// S = [ z 0 -x ]
// [ -y x 0 ]
//
// [ 1 0 0 ]
// I = [ 0 1 0 ]
// [ 0 0 1 ]
//
//
// [ xx+cosa*(1-xx) yx-cosa*yx-sina*z zx-cosa*xz+sina*y ]
// M = [ xy-cosa*yx+sina*z yy+cosa(1-yy) yz-cosa*yz-sina*x ]
// [ zx-cosa*zx-sina*y zy-cosa*zy+sina*x zz+cosa*(1-zz) ]
//
float x = axis.X, y = axis.Y, z = axis.Z;
float sa = (float)Math.Sin(angle), ca = (float)Math.Cos(angle);
float xx = x * x, yy = y * y, zz = z * z;
float xy = x * y, xz = x * z, yz = y * z;
Matrix4x4 result;
result.M11 = xx + ca * (1.0f - xx);
result.M12 = xy - ca * xy + sa * z;
result.M13 = xz - ca * xz - sa * y;
result.M14 = 0.0f;
result.M21 = xy - ca * xy - sa * z;
result.M22 = yy + ca * (1.0f - yy);
result.M23 = yz - ca * yz + sa * x;
result.M24 = 0.0f;
result.M31 = xz - ca * xz + sa * y;
result.M32 = yz - ca * yz - sa * x;
result.M33 = zz + ca * (1.0f - zz);
result.M34 = 0.0f;
result.M41 = 0.0f;
result.M42 = 0.0f;
result.M43 = 0.0f;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a perspective projection matrix based on a field of view, aspect ratio, and near and far view plane distances.
/// </summary>
/// <param name="fieldOfView">Field of view in the y direction, in radians.</param>
/// <param name="aspectRatio">Aspect ratio, defined as view space width divided by height.</param>
/// <param name="nearPlaneDistance">Distance to the near view plane.</param>
/// <param name="farPlaneDistance">Distance to the far view plane.</param>
/// <returns>The perspective projection matrix.</returns>
public static Matrix4x4 CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float nearPlaneDistance, float farPlaneDistance)
{
if (fieldOfView <= 0.0f || fieldOfView >= Math.PI)
throw new ArgumentOutOfRangeException("fieldOfView");
if (nearPlaneDistance <= 0.0f)
throw new ArgumentOutOfRangeException("nearPlaneDistance");
if (farPlaneDistance <= 0.0f)
throw new ArgumentOutOfRangeException("farPlaneDistance");
if (nearPlaneDistance >= farPlaneDistance)
throw new ArgumentOutOfRangeException("nearPlaneDistance");
float yScale = 1.0f / (float)Math.Tan(fieldOfView * 0.5f);
float xScale = yScale / aspectRatio;
Matrix4x4 result;
result.M11 = xScale;
result.M12 = result.M13 = result.M14 = 0.0f;
result.M22 = yScale;
result.M21 = result.M23 = result.M24 = 0.0f;
result.M31 = result.M32 = 0.0f;
result.M33 = farPlaneDistance / (nearPlaneDistance - farPlaneDistance);
result.M34 = -1.0f;
result.M41 = result.M42 = result.M44 = 0.0f;
result.M43 = nearPlaneDistance * farPlaneDistance / (nearPlaneDistance - farPlaneDistance);
return result;
}
/// <summary>
/// Creates a perspective projection matrix from the given view volume dimensions.
/// </summary>
/// <param name="width">Width of the view volume at the near view plane.</param>
/// <param name="height">Height of the view volume at the near view plane.</param>
/// <param name="nearPlaneDistance">Distance to the near view plane.</param>
/// <param name="farPlaneDistance">Distance to the far view plane.</param>
/// <returns>The perspective projection matrix.</returns>
public static Matrix4x4 CreatePerspective(float width, float height, float nearPlaneDistance, float farPlaneDistance)
{
if (nearPlaneDistance <= 0.0f)
throw new ArgumentOutOfRangeException("nearPlaneDistance");
if (farPlaneDistance <= 0.0f)
throw new ArgumentOutOfRangeException("farPlaneDistance");
if (nearPlaneDistance >= farPlaneDistance)
throw new ArgumentOutOfRangeException("nearPlaneDistance");
Matrix4x4 result;
result.M11 = 2.0f * nearPlaneDistance / width;
result.M12 = result.M13 = result.M14 = 0.0f;
result.M22 = 2.0f * nearPlaneDistance / height;
result.M21 = result.M23 = result.M24 = 0.0f;
result.M33 = farPlaneDistance / (nearPlaneDistance - farPlaneDistance);
result.M31 = result.M32 = 0.0f;
result.M34 = -1.0f;
result.M41 = result.M42 = result.M44 = 0.0f;
result.M43 = nearPlaneDistance * farPlaneDistance / (nearPlaneDistance - farPlaneDistance);
return result;
}
/// <summary>
/// Creates a customized, perspective projection matrix.
/// </summary>
/// <param name="left">Minimum x-value of the view volume at the near view plane.</param>
/// <param name="right">Maximum x-value of the view volume at the near view plane.</param>
/// <param name="bottom">Minimum y-value of the view volume at the near view plane.</param>
/// <param name="top">Maximum y-value of the view volume at the near view plane.</param>
/// <param name="nearPlaneDistance">Distance to the near view plane.</param>
/// <param name="farPlaneDistance">Distance to of the far view plane.</param>
/// <returns>The perspective projection matrix.</returns>
public static Matrix4x4 CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float nearPlaneDistance, float farPlaneDistance)
{
if (nearPlaneDistance <= 0.0f)
throw new ArgumentOutOfRangeException("nearPlaneDistance");
if (farPlaneDistance <= 0.0f)
throw new ArgumentOutOfRangeException("farPlaneDistance");
if (nearPlaneDistance >= farPlaneDistance)
throw new ArgumentOutOfRangeException("nearPlaneDistance");
Matrix4x4 result;
result.M11 = 2.0f * nearPlaneDistance / (right - left);
result.M12 = result.M13 = result.M14 = 0.0f;
result.M22 = 2.0f * nearPlaneDistance / (top - bottom);
result.M21 = result.M23 = result.M24 = 0.0f;
result.M31 = (left + right) / (right - left);
result.M32 = (top + bottom) / (top - bottom);
result.M33 = farPlaneDistance / (nearPlaneDistance - farPlaneDistance);
result.M34 = -1.0f;
result.M43 = nearPlaneDistance * farPlaneDistance / (nearPlaneDistance - farPlaneDistance);
result.M41 = result.M42 = result.M44 = 0.0f;
return result;
}
/// <summary>
/// Creates an orthographic perspective matrix from the given view volume dimensions.
/// </summary>
/// <param name="width">Width of the view volume.</param>
/// <param name="height">Height of the view volume.</param>
/// <param name="zNearPlane">Minimum Z-value of the view volume.</param>
/// <param name="zFarPlane">Maximum Z-value of the view volume.</param>
/// <returns>The orthographic projection matrix.</returns>
public static Matrix4x4 CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane)
{
Matrix4x4 result;
result.M11 = 2.0f / width;
result.M12 = result.M13 = result.M14 = 0.0f;
result.M22 = 2.0f / height;
result.M21 = result.M23 = result.M24 = 0.0f;
result.M33 = 1.0f / (zNearPlane - zFarPlane);
result.M31 = result.M32 = result.M34 = 0.0f;
result.M41 = result.M42 = 0.0f;
result.M43 = zNearPlane / (zNearPlane - zFarPlane);
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Builds a customized, orthographic projection matrix.
/// </summary>
/// <param name="left">Minimum X-value of the view volume.</param>
/// <param name="right">Maximum X-value of the view volume.</param>
/// <param name="bottom">Minimum Y-value of the view volume.</param>
/// <param name="top">Maximum Y-value of the view volume.</param>
/// <param name="zNearPlane">Minimum Z-value of the view volume.</param>
/// <param name="zFarPlane">Maximum Z-value of the view volume.</param>
/// <returns>The orthographic projection matrix.</returns>
public static Matrix4x4 CreateOrthographicOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane)
{
Matrix4x4 result;
result.M11 = 2.0f / (right - left);
result.M12 = result.M13 = result.M14 = 0.0f;
result.M22 = 2.0f / (top - bottom);
result.M21 = result.M23 = result.M24 = 0.0f;
result.M33 = 1.0f / (zNearPlane - zFarPlane);
result.M31 = result.M32 = result.M34 = 0.0f;
result.M41 = (left + right) / (left - right);
result.M42 = (top + bottom) / (bottom - top);
result.M43 = zNearPlane / (zNearPlane - zFarPlane);
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a view matrix.
/// </summary>
/// <param name="cameraPosition">The position of the camera.</param>
/// <param name="cameraTarget">The target towards which the camera is pointing.</param>
/// <param name="cameraUpVector">The direction that is "up" from the camera's point of view.</param>
/// <returns>The view matrix.</returns>
public static Matrix4x4 CreateLookAt(Vector3 cameraPosition, Vector3 cameraTarget, Vector3 cameraUpVector)
{
Vector3 zaxis = Vector3.Normalize(cameraPosition - cameraTarget);
Vector3 xaxis = Vector3.Normalize(Vector3.Cross(cameraUpVector, zaxis));
Vector3 yaxis = Vector3.Cross(zaxis, xaxis);
Matrix4x4 result;
result.M11 = xaxis.X;
result.M12 = yaxis.X;
result.M13 = zaxis.X;
result.M14 = 0.0f;
result.M21 = xaxis.Y;
result.M22 = yaxis.Y;
result.M23 = zaxis.Y;
result.M24 = 0.0f;
result.M31 = xaxis.Z;
result.M32 = yaxis.Z;
result.M33 = zaxis.Z;
result.M34 = 0.0f;
result.M41 = -Vector3.Dot(xaxis, cameraPosition);
result.M42 = -Vector3.Dot(yaxis, cameraPosition);
result.M43 = -Vector3.Dot(zaxis, cameraPosition);
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a world matrix with the specified parameters.
/// </summary>
/// <param name="position">The position of the object; used in translation operations.</param>
/// <param name="forward">Forward direction of the object.</param>
/// <param name="up">Upward direction of the object; usually [0, 1, 0].</param>
/// <returns>The world matrix.</returns>
public static Matrix4x4 CreateWorld(Vector3 position, Vector3 forward, Vector3 up)
{
Vector3 zaxis = Vector3.Normalize(-forward);
Vector3 xaxis = Vector3.Normalize(Vector3.Cross(up, zaxis));
Vector3 yaxis = Vector3.Cross(zaxis, xaxis);
Matrix4x4 result;
result.M11 = xaxis.X;
result.M12 = xaxis.Y;
result.M13 = xaxis.Z;
result.M14 = 0.0f;
result.M21 = yaxis.X;
result.M22 = yaxis.Y;
result.M23 = yaxis.Z;
result.M24 = 0.0f;
result.M31 = zaxis.X;
result.M32 = zaxis.Y;
result.M33 = zaxis.Z;
result.M34 = 0.0f;
result.M41 = position.X;
result.M42 = position.Y;
result.M43 = position.Z;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a rotation matrix from the given Quaternion rotation value.
/// </summary>
/// <param name="quaternion">The source Quaternion.</param>
/// <returns>The rotation matrix.</returns>
public static Matrix4x4 CreateFromQuaternion(Quaternion quaternion)
{
Matrix4x4 result;
float xx = quaternion.X * quaternion.X;
float yy = quaternion.Y * quaternion.Y;
float zz = quaternion.Z * quaternion.Z;
float xy = quaternion.X * quaternion.Y;
float wz = quaternion.Z * quaternion.W;
float xz = quaternion.Z * quaternion.X;
float wy = quaternion.Y * quaternion.W;
float yz = quaternion.Y * quaternion.Z;
float wx = quaternion.X * quaternion.W;
result.M11 = 1.0f - 2.0f * (yy + zz);
result.M12 = 2.0f * (xy + wz);
result.M13 = 2.0f * (xz - wy);
result.M14 = 0.0f;
result.M21 = 2.0f * (xy - wz);
result.M22 = 1.0f - 2.0f * (zz + xx);
result.M23 = 2.0f * (yz + wx);
result.M24 = 0.0f;
result.M31 = 2.0f * (xz + wy);
result.M32 = 2.0f * (yz - wx);
result.M33 = 1.0f - 2.0f * (yy + xx);
result.M34 = 0.0f;
result.M41 = 0.0f;
result.M42 = 0.0f;
result.M43 = 0.0f;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Creates a rotation matrix from the specified yaw, pitch, and roll.
/// </summary>
/// <param name="yaw">Angle of rotation, in radians, around the Y-axis.</param>
/// <param name="pitch">Angle of rotation, in radians, around the X-axis.</param>
/// <param name="roll">Angle of rotation, in radians, around the Z-axis.</param>
/// <returns>The rotation matrix.</returns>
public static Matrix4x4 CreateFromYawPitchRoll(float yaw, float pitch, float roll)
{
Quaternion q = Quaternion.CreateFromYawPitchRoll(yaw, pitch, roll);
return Matrix4x4.CreateFromQuaternion(q);
}
/// <summary>
/// Creates a Matrix that flattens geometry into a specified Plane as if casting a shadow from a specified light source.
/// </summary>
/// <param name="lightDirection">The direction from which the light that will cast the shadow is coming.</param>
/// <param name="plane">The Plane onto which the new matrix should flatten geometry so as to cast a shadow.</param>
/// <returns>A new Matrix that can be used to flatten geometry onto the specified plane from the specified direction.</returns>
public static Matrix4x4 CreateShadow(Vector3 lightDirection, Plane plane)
{
Plane p = Plane.Normalize(plane);
float dot = p.Normal.X * lightDirection.X + p.Normal.Y * lightDirection.Y + p.Normal.Z * lightDirection.Z;
float a = -p.Normal.X;
float b = -p.Normal.Y;
float c = -p.Normal.Z;
float d = -p.D;
Matrix4x4 result;
result.M11 = a * lightDirection.X + dot;
result.M21 = b * lightDirection.X;
result.M31 = c * lightDirection.X;
result.M41 = d * lightDirection.X;
result.M12 = a * lightDirection.Y;
result.M22 = b * lightDirection.Y + dot;
result.M32 = c * lightDirection.Y;
result.M42 = d * lightDirection.Y;
result.M13 = a * lightDirection.Z;
result.M23 = b * lightDirection.Z;
result.M33 = c * lightDirection.Z + dot;
result.M43 = d * lightDirection.Z;
result.M14 = 0.0f;
result.M24 = 0.0f;
result.M34 = 0.0f;
result.M44 = dot;
return result;
}
/// <summary>
/// Creates a Matrix that reflects the coordinate system about a specified Plane.
/// </summary>
/// <param name="value">The Plane about which to create a reflection.</param>
/// <returns>A new matrix expressing the reflection.</returns>
public static Matrix4x4 CreateReflection(Plane value)
{
value = Plane.Normalize(value);
float a = value.Normal.X;
float b = value.Normal.Y;
float c = value.Normal.Z;
float fa = -2.0f * a;
float fb = -2.0f * b;
float fc = -2.0f * c;
Matrix4x4 result;
result.M11 = fa * a + 1.0f;
result.M12 = fb * a;
result.M13 = fc * a;
result.M14 = 0.0f;
result.M21 = fa * b;
result.M22 = fb * b + 1.0f;
result.M23 = fc * b;
result.M24 = 0.0f;
result.M31 = fa * c;
result.M32 = fb * c;
result.M33 = fc * c + 1.0f;
result.M34 = 0.0f;
result.M41 = fa * value.D;
result.M42 = fb * value.D;
result.M43 = fc * value.D;
result.M44 = 1.0f;
return result;
}
/// <summary>
/// Calculates the determinant of the matrix.
/// </summary>
/// <returns>The determinant of the matrix.</returns>
public float GetDeterminant()
{
// | a b c d | | f g h | | e g h | | e f h | | e f g |
// | e f g h | = a | j k l | - b | i k l | + c | i j l | - d | i j k |
// | i j k l | | n o p | | m o p | | m n p | | m n o |
// | m n o p |
//
// | f g h |
// a | j k l | = a ( f ( kp - lo ) - g ( jp - ln ) + h ( jo - kn ) )
// | n o p |
//
// | e g h |
// b | i k l | = b ( e ( kp - lo ) - g ( ip - lm ) + h ( io - km ) )
// | m o p |
//
// | e f h |
// c | i j l | = c ( e ( jp - ln ) - f ( ip - lm ) + h ( in - jm ) )
// | m n p |
//
// | e f g |
// d | i j k | = d ( e ( jo - kn ) - f ( io - km ) + g ( in - jm ) )
// | m n o |
//
// Cost of operation
// 17 adds and 28 muls.
//
// add: 6 + 8 + 3 = 17
// mul: 12 + 16 = 28
float a = M11, b = M12, c = M13, d = M14;
float e = M21, f = M22, g = M23, h = M24;
float i = M31, j = M32, k = M33, l = M34;
float m = M41, n = M42, o = M43, p = M44;
float kp_lo = k * p - l * o;
float jp_ln = j * p - l * n;
float jo_kn = j * o - k * n;
float ip_lm = i * p - l * m;
float io_km = i * o - k * m;
float in_jm = i * n - j * m;
return a * (f * kp_lo - g * jp_ln + h * jo_kn) -
b * (e * kp_lo - g * ip_lm + h * io_km) +
c * (e * jp_ln - f * ip_lm + h * in_jm) -
d * (e * jo_kn - f * io_km + g * in_jm);
}
/// <summary>
/// Attempts to calculate the inverse of the given matrix. If successful, result will contain the inverted matrix.
/// </summary>
/// <param name="matrix">The source matrix to invert.</param>
/// <param name="result">If successful, contains the inverted matrix.</param>
/// <returns>True if the source matrix could be inverted; False otherwise.</returns>
public static bool Invert(Matrix4x4 matrix, out Matrix4x4 result)
{
// -1
// If you have matrix M, inverse Matrix M can compute
//
// -1 1
// M = --------- A
// det(M)
//
// A is adjugate (adjoint) of M, where,
//
// T
// A = C
//
// C is Cofactor matrix of M, where,
// i + j
// C = (-1) * det(M )
// ij ij
//
// [ a b c d ]
// M = [ e f g h ]
// [ i j k l ]
// [ m n o p ]
//
// First Row
// 2 | f g h |
// C = (-1) | j k l | = + ( f ( kp - lo ) - g ( jp - ln ) + h ( jo - kn ) )
// 11 | n o p |
//
// 3 | e g h |
// C = (-1) | i k l | = - ( e ( kp - lo ) - g ( ip - lm ) + h ( io - km ) )
// 12 | m o p |
//
// 4 | e f h |
// C = (-1) | i j l | = + ( e ( jp - ln ) - f ( ip - lm ) + h ( in - jm ) )
// 13 | m n p |
//
// 5 | e f g |
// C = (-1) | i j k | = - ( e ( jo - kn ) - f ( io - km ) + g ( in - jm ) )
// 14 | m n o |
//
// Second Row
// 3 | b c d |
// C = (-1) | j k l | = - ( b ( kp - lo ) - c ( jp - ln ) + d ( jo - kn ) )
// 21 | n o p |
//
// 4 | a c d |
// C = (-1) | i k l | = + ( a ( kp - lo ) - c ( ip - lm ) + d ( io - km ) )
// 22 | m o p |
//
// 5 | a b d |
// C = (-1) | i j l | = - ( a ( jp - ln ) - b ( ip - lm ) + d ( in - jm ) )
// 23 | m n p |
//
// 6 | a b c |
// C = (-1) | i j k | = + ( a ( jo - kn ) - b ( io - km ) + c ( in - jm ) )
// 24 | m n o |
//
// Third Row
// 4 | b c d |
// C = (-1) | f g h | = + ( b ( gp - ho ) - c ( fp - hn ) + d ( fo - gn ) )
// 31 | n o p |
//
// 5 | a c d |
// C = (-1) | e g h | = - ( a ( gp - ho ) - c ( ep - hm ) + d ( eo - gm ) )
// 32 | m o p |
//
// 6 | a b d |
// C = (-1) | e f h | = + ( a ( fp - hn ) - b ( ep - hm ) + d ( en - fm ) )
// 33 | m n p |
//
// 7 | a b c |
// C = (-1) | e f g | = - ( a ( fo - gn ) - b ( eo - gm ) + c ( en - fm ) )
// 34 | m n o |
//
// Fourth Row
// 5 | b c d |
// C = (-1) | f g h | = - ( b ( gl - hk ) - c ( fl - hj ) + d ( fk - gj ) )
// 41 | j k l |
//
// 6 | a c d |
// C = (-1) | e g h | = + ( a ( gl - hk ) - c ( el - hi ) + d ( ek - gi ) )
// 42 | i k l |
//
// 7 | a b d |
// C = (-1) | e f h | = - ( a ( fl - hj ) - b ( el - hi ) + d ( ej - fi ) )
// 43 | i j l |
//
// 8 | a b c |
// C = (-1) | e f g | = + ( a ( fk - gj ) - b ( ek - gi ) + c ( ej - fi ) )
// 44 | i j k |
//
// Cost of operation
// 53 adds, 104 muls, and 1 div.
float a = matrix.M11, b = matrix.M12, c = matrix.M13, d = matrix.M14;
float e = matrix.M21, f = matrix.M22, g = matrix.M23, h = matrix.M24;
float i = matrix.M31, j = matrix.M32, k = matrix.M33, l = matrix.M34;
float m = matrix.M41, n = matrix.M42, o = matrix.M43, p = matrix.M44;
float kp_lo = k * p - l * o;
float jp_ln = j * p - l * n;
float jo_kn = j * o - k * n;
float ip_lm = i * p - l * m;
float io_km = i * o - k * m;
float in_jm = i * n - j * m;
float a11 = +(f * kp_lo - g * jp_ln + h * jo_kn);
float a12 = -(e * kp_lo - g * ip_lm + h * io_km);
float a13 = +(e * jp_ln - f * ip_lm + h * in_jm);
float a14 = -(e * jo_kn - f * io_km + g * in_jm);
float det = a * a11 + b * a12 + c * a13 + d * a14;
if (Math.Abs(det) < float.Epsilon)
{
result = new Matrix4x4(float.NaN, float.NaN, float.NaN, float.NaN,
float.NaN, float.NaN, float.NaN, float.NaN,
float.NaN, float.NaN, float.NaN, float.NaN,
float.NaN, float.NaN, float.NaN, float.NaN);
return false;
}
float invDet = 1.0f / det;
result.M11 = a11 * invDet;
result.M21 = a12 * invDet;
result.M31 = a13 * invDet;
result.M41 = a14 * invDet;
result.M12 = -(b * kp_lo - c * jp_ln + d * jo_kn) * invDet;
result.M22 = +(a * kp_lo - c * ip_lm + d * io_km) * invDet;
result.M32 = -(a * jp_ln - b * ip_lm + d * in_jm) * invDet;
result.M42 = +(a * jo_kn - b * io_km + c * in_jm) * invDet;
float gp_ho = g * p - h * o;
float fp_hn = f * p - h * n;
float fo_gn = f * o - g * n;
float ep_hm = e * p - h * m;
float eo_gm = e * o - g * m;
float en_fm = e * n - f * m;
result.M13 = +(b * gp_ho - c * fp_hn + d * fo_gn) * invDet;
result.M23 = -(a * gp_ho - c * ep_hm + d * eo_gm) * invDet;
result.M33 = +(a * fp_hn - b * ep_hm + d * en_fm) * invDet;
result.M43 = -(a * fo_gn - b * eo_gm + c * en_fm) * invDet;
float gl_hk = g * l - h * k;
float fl_hj = f * l - h * j;
float fk_gj = f * k - g * j;
float el_hi = e * l - h * i;
float ek_gi = e * k - g * i;
float ej_fi = e * j - f * i;
result.M14 = -(b * gl_hk - c * fl_hj + d * fk_gj) * invDet;
result.M24 = +(a * gl_hk - c * el_hi + d * ek_gi) * invDet;
result.M34 = -(a * fl_hj - b * el_hi + d * ej_fi) * invDet;
result.M44 = +(a * fk_gj - b * ek_gi + c * ej_fi) * invDet;
return true;
}
struct CanonicalBasis
{
public Vector3 Row0;
public Vector3 Row1;
public Vector3 Row2;
};
[System.Security.SecuritySafeCritical]
struct VectorBasis
{
public unsafe Vector3* Element0;
public unsafe Vector3* Element1;
public unsafe Vector3* Element2;
}
/// <summary>
/// Attempts to extract the scale, translation, and rotation components from the given scale/rotation/translation matrix.
/// If successful, the out parameters will contained the extracted values.
/// </summary>
/// <param name="matrix">The source matrix.</param>
/// <param name="scale">The scaling component of the transformation matrix.</param>
/// <param name="rotation">The rotation component of the transformation matrix.</param>
/// <param name="translation">The translation component of the transformation matrix</param>
/// <returns>True if the source matrix was successfully decomposed; False otherwise.</returns>
[System.Security.SecuritySafeCritical]
public static bool Decompose(Matrix4x4 matrix, out Vector3 scale, out Quaternion rotation, out Vector3 translation)
{
bool result = true;
unsafe
{
fixed (Vector3* scaleBase = &scale)
{
float* pfScales = (float*)scaleBase;
const float EPSILON = 0.0001f;
float det;
VectorBasis vectorBasis;
Vector3** pVectorBasis = (Vector3**)&vectorBasis;
Matrix4x4 matTemp = Matrix4x4.Identity;
CanonicalBasis canonicalBasis = new CanonicalBasis();
Vector3* pCanonicalBasis = &canonicalBasis.Row0;
canonicalBasis.Row0 = new Vector3(1.0f, 0.0f, 0.0f);
canonicalBasis.Row1 = new Vector3(0.0f, 1.0f, 0.0f);
canonicalBasis.Row2 = new Vector3(0.0f, 0.0f, 1.0f);
translation = new Vector3(
matrix.M41,
matrix.M42,
matrix.M43);
pVectorBasis[0] = (Vector3*)&matTemp.M11;
pVectorBasis[1] = (Vector3*)&matTemp.M21;
pVectorBasis[2] = (Vector3*)&matTemp.M31;
*(pVectorBasis[0]) = new Vector3(matrix.M11, matrix.M12, matrix.M13);
*(pVectorBasis[1]) = new Vector3(matrix.M21, matrix.M22, matrix.M23);
*(pVectorBasis[2]) = new Vector3(matrix.M31, matrix.M32, matrix.M33);
scale.X = pVectorBasis[0]->Length();
scale.Y = pVectorBasis[1]->Length();
scale.Z = pVectorBasis[2]->Length();
uint a, b, c;
#region Ranking
float x = pfScales[0], y = pfScales[1], z = pfScales[2];
if (x < y)
{
if (y < z)
{
a = 2;
b = 1;
c = 0;
}
else
{
a = 1;
if (x < z)
{
b = 2;
c = 0;
}
else
{
b = 0;
c = 2;
}
}
}
else
{
if (x < z)
{
a = 2;
b = 0;
c = 1;
}
else
{
a = 0;
if (y < z)
{
b = 2;
c = 1;
}
else
{
b = 1;
c = 2;
}
}
}
#endregion
if (pfScales[a] < EPSILON)
{
*(pVectorBasis[a]) = pCanonicalBasis[a];
}
*pVectorBasis[a] = Vector3.Normalize(*pVectorBasis[a]);
if (pfScales[b] < EPSILON)
{
uint cc;
float fAbsX, fAbsY, fAbsZ;
fAbsX = (float)Math.Abs(pVectorBasis[a]->X);
fAbsY = (float)Math.Abs(pVectorBasis[a]->Y);
fAbsZ = (float)Math.Abs(pVectorBasis[a]->Z);
#region Ranking
if (fAbsX < fAbsY)
{
if (fAbsY < fAbsZ)
{
cc = 0;
}
else
{
if (fAbsX < fAbsZ)
{
cc = 0;
}
else
{
cc = 2;
}
}
}
else
{
if (fAbsX < fAbsZ)
{
cc = 1;
}
else
{
if (fAbsY < fAbsZ)
{
cc = 1;
}
else
{
cc = 2;
}
}
}
#endregion
*pVectorBasis[b] = Vector3.Cross(*pVectorBasis[a], *(pCanonicalBasis + cc));
}
*pVectorBasis[b] = Vector3.Normalize(*pVectorBasis[b]);
if (pfScales[c] < EPSILON)
{
*pVectorBasis[c] = Vector3.Cross(*pVectorBasis[a], *pVectorBasis[b]);
}
*pVectorBasis[c] = Vector3.Normalize(*pVectorBasis[c]);
det = matTemp.GetDeterminant();
// use Kramer's rule to check for handedness of coordinate system
if (det < 0.0f)
{
// switch coordinate system by negating the scale and inverting the basis vector on the x-axis
pfScales[a] = -pfScales[a];
*pVectorBasis[a] = -(*pVectorBasis[a]);
det = -det;
}
det -= 1.0f;
det *= det;
if ((EPSILON < det))
{
// Non-SRT matrix encountered
rotation = Quaternion.Identity;
result = false;
}
else
{
// generate the quaternion from the matrix
rotation = Quaternion.CreateFromRotationMatrix(matTemp);
}
}
}
return result;
}
/// <summary>
/// Transforms the given matrix by applying the given Quaternion rotation.
/// </summary>
/// <param name="value">The source matrix to transform.</param>
/// <param name="rotation">The rotation to apply.</param>
/// <returns>The transformed matrix.</returns>
public static Matrix4x4 Transform(Matrix4x4 value, Quaternion rotation)
{
// Compute rotation matrix.
float x2 = rotation.X + rotation.X;
float y2 = rotation.Y + rotation.Y;
float z2 = rotation.Z + rotation.Z;
float wx2 = rotation.W * x2;
float wy2 = rotation.W * y2;
float wz2 = rotation.W * z2;
float xx2 = rotation.X * x2;
float xy2 = rotation.X * y2;
float xz2 = rotation.X * z2;
float yy2 = rotation.Y * y2;
float yz2 = rotation.Y * z2;
float zz2 = rotation.Z * z2;
float q11 = 1.0f - yy2 - zz2;
float q21 = xy2 - wz2;
float q31 = xz2 + wy2;
float q12 = xy2 + wz2;
float q22 = 1.0f - xx2 - zz2;
float q32 = yz2 - wx2;
float q13 = xz2 - wy2;
float q23 = yz2 + wx2;
float q33 = 1.0f - xx2 - yy2;
Matrix4x4 result;
// First row
result.M11 = value.M11 * q11 + value.M12 * q21 + value.M13 * q31;
result.M12 = value.M11 * q12 + value.M12 * q22 + value.M13 * q32;
result.M13 = value.M11 * q13 + value.M12 * q23 + value.M13 * q33;
result.M14 = value.M14;
// Second row
result.M21 = value.M21 * q11 + value.M22 * q21 + value.M23 * q31;
result.M22 = value.M21 * q12 + value.M22 * q22 + value.M23 * q32;
result.M23 = value.M21 * q13 + value.M22 * q23 + value.M23 * q33;
result.M24 = value.M24;
// Third row
result.M31 = value.M31 * q11 + value.M32 * q21 + value.M33 * q31;
result.M32 = value.M31 * q12 + value.M32 * q22 + value.M33 * q32;
result.M33 = value.M31 * q13 + value.M32 * q23 + value.M33 * q33;
result.M34 = value.M34;
// Fourth row
result.M41 = value.M41 * q11 + value.M42 * q21 + value.M43 * q31;
result.M42 = value.M41 * q12 + value.M42 * q22 + value.M43 * q32;
result.M43 = value.M41 * q13 + value.M42 * q23 + value.M43 * q33;
result.M44 = value.M44;
return result;
}
/// <summary>
/// Transposes the rows and columns of a matrix.
/// </summary>
/// <param name="matrix">The source matrix.</param>
/// <returns>The transposed matrix.</returns>
public static Matrix4x4 Transpose(Matrix4x4 matrix)
{
Matrix4x4 result;
result.M11 = matrix.M11;
result.M12 = matrix.M21;
result.M13 = matrix.M31;
result.M14 = matrix.M41;
result.M21 = matrix.M12;
result.M22 = matrix.M22;
result.M23 = matrix.M32;
result.M24 = matrix.M42;
result.M31 = matrix.M13;
result.M32 = matrix.M23;
result.M33 = matrix.M33;
result.M34 = matrix.M43;
result.M41 = matrix.M14;
result.M42 = matrix.M24;
result.M43 = matrix.M34;
result.M44 = matrix.M44;
return result;
}
/// <summary>
/// Linearly interpolates between the corresponding values of two matrices.
/// </summary>
/// <param name="matrix1">The first source matrix.</param>
/// <param name="matrix2">The second source matrix.</param>
/// <param name="amount">The relative weight of the second source matrix.</param>
/// <returns>The interpolated matrix.</returns>
public static Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float amount)
{
Matrix4x4 result;
// First row
result.M11 = matrix1.M11 + (matrix2.M11 - matrix1.M11) * amount;
result.M12 = matrix1.M12 + (matrix2.M12 - matrix1.M12) * amount;
result.M13 = matrix1.M13 + (matrix2.M13 - matrix1.M13) * amount;
result.M14 = matrix1.M14 + (matrix2.M14 - matrix1.M14) * amount;
// Second row
result.M21 = matrix1.M21 + (matrix2.M21 - matrix1.M21) * amount;
result.M22 = matrix1.M22 + (matrix2.M22 - matrix1.M22) * amount;
result.M23 = matrix1.M23 + (matrix2.M23 - matrix1.M23) * amount;
result.M24 = matrix1.M24 + (matrix2.M24 - matrix1.M24) * amount;
// Third row
result.M31 = matrix1.M31 + (matrix2.M31 - matrix1.M31) * amount;
result.M32 = matrix1.M32 + (matrix2.M32 - matrix1.M32) * amount;
result.M33 = matrix1.M33 + (matrix2.M33 - matrix1.M33) * amount;
result.M34 = matrix1.M34 + (matrix2.M34 - matrix1.M34) * amount;
// Fourth row
result.M41 = matrix1.M41 + (matrix2.M41 - matrix1.M41) * amount;
result.M42 = matrix1.M42 + (matrix2.M42 - matrix1.M42) * amount;
result.M43 = matrix1.M43 + (matrix2.M43 - matrix1.M43) * amount;
result.M44 = matrix1.M44 + (matrix2.M44 - matrix1.M44) * amount;
return result;
}
/// <summary>
/// Returns a new matrix with the negated elements of the given matrix.
/// </summary>
/// <param name="value">The source matrix.</param>
/// <returns>The negated matrix.</returns>
public static Matrix4x4 Negate(Matrix4x4 value)
{
Matrix4x4 result;
result.M11 = -value.M11;
result.M12 = -value.M12;
result.M13 = -value.M13;
result.M14 = -value.M14;
result.M21 = -value.M21;
result.M22 = -value.M22;
result.M23 = -value.M23;
result.M24 = -value.M24;
result.M31 = -value.M31;
result.M32 = -value.M32;
result.M33 = -value.M33;
result.M34 = -value.M34;
result.M41 = -value.M41;
result.M42 = -value.M42;
result.M43 = -value.M43;
result.M44 = -value.M44;
return result;
}
/// <summary>
/// Adds two matrices together.
/// </summary>
/// <param name="value1">The first source matrix.</param>
/// <param name="value2">The second source matrix.</param>
/// <returns>The resulting matrix.</returns>
public static Matrix4x4 Add(Matrix4x4 value1, Matrix4x4 value2)
{
Matrix4x4 result;
result.M11 = value1.M11 + value2.M11;
result.M12 = value1.M12 + value2.M12;
result.M13 = value1.M13 + value2.M13;
result.M14 = value1.M14 + value2.M14;
result.M21 = value1.M21 + value2.M21;
result.M22 = value1.M22 + value2.M22;
result.M23 = value1.M23 + value2.M23;
result.M24 = value1.M24 + value2.M24;
result.M31 = value1.M31 + value2.M31;
result.M32 = value1.M32 + value2.M32;
result.M33 = value1.M33 + value2.M33;
result.M34 = value1.M34 + value2.M34;
result.M41 = value1.M41 + value2.M41;
result.M42 = value1.M42 + value2.M42;
result.M43 = value1.M43 + value2.M43;
result.M44 = value1.M44 + value2.M44;
return result;
}
/// <summary>
/// Subtracts the second matrix from the first.
/// </summary>
/// <param name="value1">The first source matrix.</param>
/// <param name="value2">The second source matrix.</param>
/// <returns>The result of the subtraction.</returns>
public static Matrix4x4 Subtract(Matrix4x4 value1, Matrix4x4 value2)
{
Matrix4x4 result;
result.M11 = value1.M11 - value2.M11;
result.M12 = value1.M12 - value2.M12;
result.M13 = value1.M13 - value2.M13;
result.M14 = value1.M14 - value2.M14;
result.M21 = value1.M21 - value2.M21;
result.M22 = value1.M22 - value2.M22;
result.M23 = value1.M23 - value2.M23;
result.M24 = value1.M24 - value2.M24;
result.M31 = value1.M31 - value2.M31;
result.M32 = value1.M32 - value2.M32;
result.M33 = value1.M33 - value2.M33;
result.M34 = value1.M34 - value2.M34;
result.M41 = value1.M41 - value2.M41;
result.M42 = value1.M42 - value2.M42;
result.M43 = value1.M43 - value2.M43;
result.M44 = value1.M44 - value2.M44;
return result;
}
/// <summary>
/// Multiplies a matrix by another matrix.
/// </summary>
/// <param name="value1">The first source matrix.</param>
/// <param name="value2">The second source matrix.</param>
/// <returns>The result of the multiplication.</returns>
public static Matrix4x4 Multiply(Matrix4x4 value1, Matrix4x4 value2)
{
Matrix4x4 result;
// First row
result.M11 = value1.M11 * value2.M11 + value1.M12 * value2.M21 + value1.M13 * value2.M31 + value1.M14 * value2.M41;
result.M12 = value1.M11 * value2.M12 + value1.M12 * value2.M22 + value1.M13 * value2.M32 + value1.M14 * value2.M42;
result.M13 = value1.M11 * value2.M13 + value1.M12 * value2.M23 + value1.M13 * value2.M33 + value1.M14 * value2.M43;
result.M14 = value1.M11 * value2.M14 + value1.M12 * value2.M24 + value1.M13 * value2.M34 + value1.M14 * value2.M44;
// Second row
result.M21 = value1.M21 * value2.M11 + value1.M22 * value2.M21 + value1.M23 * value2.M31 + value1.M24 * value2.M41;
result.M22 = value1.M21 * value2.M12 + value1.M22 * value2.M22 + value1.M23 * value2.M32 + value1.M24 * value2.M42;
result.M23 = value1.M21 * value2.M13 + value1.M22 * value2.M23 + value1.M23 * value2.M33 + value1.M24 * value2.M43;
result.M24 = value1.M21 * value2.M14 + value1.M22 * value2.M24 + value1.M23 * value2.M34 + value1.M24 * value2.M44;
// Third row
result.M31 = value1.M31 * value2.M11 + value1.M32 * value2.M21 + value1.M33 * value2.M31 + value1.M34 * value2.M41;
result.M32 = value1.M31 * value2.M12 + value1.M32 * value2.M22 + value1.M33 * value2.M32 + value1.M34 * value2.M42;
result.M33 = value1.M31 * value2.M13 + value1.M32 * value2.M23 + value1.M33 * value2.M33 + value1.M34 * value2.M43;
result.M34 = value1.M31 * value2.M14 + value1.M32 * value2.M24 + value1.M33 * value2.M34 + value1.M34 * value2.M44;
// Fourth row
result.M41 = value1.M41 * value2.M11 + value1.M42 * value2.M21 + value1.M43 * value2.M31 + value1.M44 * value2.M41;
result.M42 = value1.M41 * value2.M12 + value1.M42 * value2.M22 + value1.M43 * value2.M32 + value1.M44 * value2.M42;
result.M43 = value1.M41 * value2.M13 + value1.M42 * value2.M23 + value1.M43 * value2.M33 + value1.M44 * value2.M43;
result.M44 = value1.M41 * value2.M14 + value1.M42 * value2.M24 + value1.M43 * value2.M34 + value1.M44 * value2.M44;
return result;
}
/// <summary>
/// Multiplies a matrix by a scalar value.
/// </summary>
/// <param name="value1">The source matrix.</param>
/// <param name="value2">The scaling factor.</param>
/// <returns>The scaled matrix.</returns>
public static Matrix4x4 Multiply(Matrix4x4 value1, float value2)
{
Matrix4x4 result;
result.M11 = value1.M11 * value2;
result.M12 = value1.M12 * value2;
result.M13 = value1.M13 * value2;
result.M14 = value1.M14 * value2;
result.M21 = value1.M21 * value2;
result.M22 = value1.M22 * value2;
result.M23 = value1.M23 * value2;
result.M24 = value1.M24 * value2;
result.M31 = value1.M31 * value2;
result.M32 = value1.M32 * value2;
result.M33 = value1.M33 * value2;
result.M34 = value1.M34 * value2;
result.M41 = value1.M41 * value2;
result.M42 = value1.M42 * value2;
result.M43 = value1.M43 * value2;
result.M44 = value1.M44 * value2;
return result;
}
/// <summary>
/// Returns a new matrix with the negated elements of the given matrix.
/// </summary>
/// <param name="value">The source matrix.</param>
/// <returns>The negated matrix.</returns>
public static Matrix4x4 operator -(Matrix4x4 value)
{
Matrix4x4 m;
m.M11 = -value.M11;
m.M12 = -value.M12;
m.M13 = -value.M13;
m.M14 = -value.M14;
m.M21 = -value.M21;
m.M22 = -value.M22;
m.M23 = -value.M23;
m.M24 = -value.M24;
m.M31 = -value.M31;
m.M32 = -value.M32;
m.M33 = -value.M33;
m.M34 = -value.M34;
m.M41 = -value.M41;
m.M42 = -value.M42;
m.M43 = -value.M43;
m.M44 = -value.M44;
return m;
}
/// <summary>
/// Adds two matrices together.
/// </summary>
/// <param name="value1">The first source matrix.</param>
/// <param name="value2">The second source matrix.</param>
/// <returns>The resulting matrix.</returns>
public static Matrix4x4 operator +(Matrix4x4 value1, Matrix4x4 value2)
{
Matrix4x4 m;
m.M11 = value1.M11 + value2.M11;
m.M12 = value1.M12 + value2.M12;
m.M13 = value1.M13 + value2.M13;
m.M14 = value1.M14 + value2.M14;
m.M21 = value1.M21 + value2.M21;
m.M22 = value1.M22 + value2.M22;
m.M23 = value1.M23 + value2.M23;
m.M24 = value1.M24 + value2.M24;
m.M31 = value1.M31 + value2.M31;
m.M32 = value1.M32 + value2.M32;
m.M33 = value1.M33 + value2.M33;
m.M34 = value1.M34 + value2.M34;
m.M41 = value1.M41 + value2.M41;
m.M42 = value1.M42 + value2.M42;
m.M43 = value1.M43 + value2.M43;
m.M44 = value1.M44 + value2.M44;
return m;
}
/// <summary>
/// Subtracts the second matrix from the first.
/// </summary>
/// <param name="value1">The first source matrix.</param>
/// <param name="value2">The second source matrix.</param>
/// <returns>The result of the subtraction.</returns>
public static Matrix4x4 operator -(Matrix4x4 value1, Matrix4x4 value2)
{
Matrix4x4 m;
m.M11 = value1.M11 - value2.M11;
m.M12 = value1.M12 - value2.M12;
m.M13 = value1.M13 - value2.M13;
m.M14 = value1.M14 - value2.M14;
m.M21 = value1.M21 - value2.M21;
m.M22 = value1.M22 - value2.M22;
m.M23 = value1.M23 - value2.M23;
m.M24 = value1.M24 - value2.M24;
m.M31 = value1.M31 - value2.M31;
m.M32 = value1.M32 - value2.M32;
m.M33 = value1.M33 - value2.M33;
m.M34 = value1.M34 - value2.M34;
m.M41 = value1.M41 - value2.M41;
m.M42 = value1.M42 - value2.M42;
m.M43 = value1.M43 - value2.M43;
m.M44 = value1.M44 - value2.M44;
return m;
}
/// <summary>
/// Multiplies a matrix by another matrix.
/// </summary>
/// <param name="value1">The first source matrix.</param>
/// <param name="value2">The second source matrix.</param>
/// <returns>The result of the multiplication.</returns>
public static Matrix4x4 operator *(Matrix4x4 value1, Matrix4x4 value2)
{
Matrix4x4 m;
// First row
m.M11 = value1.M11 * value2.M11 + value1.M12 * value2.M21 + value1.M13 * value2.M31 + value1.M14 * value2.M41;
m.M12 = value1.M11 * value2.M12 + value1.M12 * value2.M22 + value1.M13 * value2.M32 + value1.M14 * value2.M42;
m.M13 = value1.M11 * value2.M13 + value1.M12 * value2.M23 + value1.M13 * value2.M33 + value1.M14 * value2.M43;
m.M14 = value1.M11 * value2.M14 + value1.M12 * value2.M24 + value1.M13 * value2.M34 + value1.M14 * value2.M44;
// Second row
m.M21 = value1.M21 * value2.M11 + value1.M22 * value2.M21 + value1.M23 * value2.M31 + value1.M24 * value2.M41;
m.M22 = value1.M21 * value2.M12 + value1.M22 * value2.M22 + value1.M23 * value2.M32 + value1.M24 * value2.M42;
m.M23 = value1.M21 * value2.M13 + value1.M22 * value2.M23 + value1.M23 * value2.M33 + value1.M24 * value2.M43;
m.M24 = value1.M21 * value2.M14 + value1.M22 * value2.M24 + value1.M23 * value2.M34 + value1.M24 * value2.M44;
// Third row
m.M31 = value1.M31 * value2.M11 + value1.M32 * value2.M21 + value1.M33 * value2.M31 + value1.M34 * value2.M41;
m.M32 = value1.M31 * value2.M12 + value1.M32 * value2.M22 + value1.M33 * value2.M32 + value1.M34 * value2.M42;
m.M33 = value1.M31 * value2.M13 + value1.M32 * value2.M23 + value1.M33 * value2.M33 + value1.M34 * value2.M43;
m.M34 = value1.M31 * value2.M14 + value1.M32 * value2.M24 + value1.M33 * value2.M34 + value1.M34 * value2.M44;
// Fourth row
m.M41 = value1.M41 * value2.M11 + value1.M42 * value2.M21 + value1.M43 * value2.M31 + value1.M44 * value2.M41;
m.M42 = value1.M41 * value2.M12 + value1.M42 * value2.M22 + value1.M43 * value2.M32 + value1.M44 * value2.M42;
m.M43 = value1.M41 * value2.M13 + value1.M42 * value2.M23 + value1.M43 * value2.M33 + value1.M44 * value2.M43;
m.M44 = value1.M41 * value2.M14 + value1.M42 * value2.M24 + value1.M43 * value2.M34 + value1.M44 * value2.M44;
return m;
}
/// <summary>
/// Multiplies a matrix by a scalar value.
/// </summary>
/// <param name="value1">The source matrix.</param>
/// <param name="value2">The scaling factor.</param>
/// <returns>The scaled matrix.</returns>
public static Matrix4x4 operator *(Matrix4x4 value1, float value2)
{
Matrix4x4 m;
m.M11 = value1.M11 * value2;
m.M12 = value1.M12 * value2;
m.M13 = value1.M13 * value2;
m.M14 = value1.M14 * value2;
m.M21 = value1.M21 * value2;
m.M22 = value1.M22 * value2;
m.M23 = value1.M23 * value2;
m.M24 = value1.M24 * value2;
m.M31 = value1.M31 * value2;
m.M32 = value1.M32 * value2;
m.M33 = value1.M33 * value2;
m.M34 = value1.M34 * value2;
m.M41 = value1.M41 * value2;
m.M42 = value1.M42 * value2;
m.M43 = value1.M43 * value2;
m.M44 = value1.M44 * value2;
return m;
}
/// <summary>
/// Returns a boolean indicating whether the given two matrices are equal.
/// </summary>
/// <param name="value1">The first matrix to compare.</param>
/// <param name="value2">The second matrix to compare.</param>
/// <returns>True if the given matrices are equal; False otherwise.</returns>
public static bool operator ==(Matrix4x4 value1, Matrix4x4 value2)
{
return (value1.M11 == value2.M11 && value1.M22 == value2.M22 && value1.M33 == value2.M33 && value1.M44 == value2.M44 && // Check diagonal element first for early out.
value1.M12 == value2.M12 && value1.M13 == value2.M13 && value1.M14 == value2.M14 &&
value1.M21 == value2.M21 && value1.M23 == value2.M23 && value1.M24 == value2.M24 &&
value1.M31 == value2.M31 && value1.M32 == value2.M32 && value1.M34 == value2.M34 &&
value1.M41 == value2.M41 && value1.M42 == value2.M42 && value1.M43 == value2.M43);
}
/// <summary>
/// Returns a boolean indicating whether the given two matrices are not equal.
/// </summary>
/// <param name="value1">The first matrix to compare.</param>
/// <param name="value2">The second matrix to compare.</param>
/// <returns>True if the given matrices are not equal; False if they are equal.</returns>
public static bool operator !=(Matrix4x4 value1, Matrix4x4 value2)
{
return (value1.M11 != value2.M11 || value1.M12 != value2.M12 || value1.M13 != value2.M13 || value1.M14 != value2.M14 ||
value1.M21 != value2.M21 || value1.M22 != value2.M22 || value1.M23 != value2.M23 || value1.M24 != value2.M24 ||
value1.M31 != value2.M31 || value1.M32 != value2.M32 || value1.M33 != value2.M33 || value1.M34 != value2.M34 ||
value1.M41 != value2.M41 || value1.M42 != value2.M42 || value1.M43 != value2.M43 || value1.M44 != value2.M44);
}
/// <summary>
/// Returns a boolean indicating whether this matrix instance is equal to the other given matrix.
/// </summary>
/// <param name="other">The matrix to compare this instance to.</param>
/// <returns>True if the matrices are equal; False otherwise.</returns>
public bool Equals(Matrix4x4 other)
{
return (M11 == other.M11 && M22 == other.M22 && M33 == other.M33 && M44 == other.M44 && // Check diagonal element first for early out.
M12 == other.M12 && M13 == other.M13 && M14 == other.M14 &&
M21 == other.M21 && M23 == other.M23 && M24 == other.M24 &&
M31 == other.M31 && M32 == other.M32 && M34 == other.M34 &&
M41 == other.M41 && M42 == other.M42 && M43 == other.M43);
}
/// <summary>
/// Returns a boolean indicating whether the given Object is equal to this matrix instance.
/// </summary>
/// <param name="obj">The Object to compare against.</param>
/// <returns>True if the Object is equal to this matrix; False otherwise.</returns>
public override bool Equals(object obj)
{
if (obj is Matrix4x4)
{
return Equals((Matrix4x4)obj);
}
return false;
}
/// <summary>
/// Returns a String representing this matrix instance.
/// </summary>
/// <returns>The string representation.</returns>
public override string ToString()
{
CultureInfo ci = CultureInfo.CurrentCulture;
return String.Format(ci, "{{ {{M11:{0} M12:{1} M13:{2} M14:{3}}} {{M21:{4} M22:{5} M23:{6} M24:{7}}} {{M31:{8} M32:{9} M33:{10} M34:{11}}} {{M41:{12} M42:{13} M43:{14} M44:{15}}} }}",
M11.ToString(ci), M12.ToString(ci), M13.ToString(ci), M14.ToString(ci),
M21.ToString(ci), M22.ToString(ci), M23.ToString(ci), M24.ToString(ci),
M31.ToString(ci), M32.ToString(ci), M33.ToString(ci), M34.ToString(ci),
M41.ToString(ci), M42.ToString(ci), M43.ToString(ci), M44.ToString(ci));
}
/// <summary>
/// Returns the hash code for this instance.
/// </summary>
/// <returns>The hash code.</returns>
public override int GetHashCode()
{
return M11.GetHashCode() + M12.GetHashCode() + M13.GetHashCode() + M14.GetHashCode() +
M21.GetHashCode() + M22.GetHashCode() + M23.GetHashCode() + M24.GetHashCode() +
M31.GetHashCode() + M32.GetHashCode() + M33.GetHashCode() + M34.GetHashCode() +
M41.GetHashCode() + M42.GetHashCode() + M43.GetHashCode() + M44.GetHashCode();
}
}
}
|