diff --git a/Source/OpenTK/Math/Matrix3x4.cs b/Source/OpenTK/Math/Matrix3x4.cs new file mode 100644 index 00000000..8a9a88f1 --- /dev/null +++ b/Source/OpenTK/Math/Matrix3x4.cs @@ -0,0 +1,755 @@ +using System; +using System.Runtime.InteropServices; + +using OpenTK; + +namespace TopHat.Mathematics +{ + /// + /// Represents a 3x4 Matrix + /// + [Serializable] + [StructLayout(LayoutKind.Sequential)] + public struct Matrix3x4 : IEquatable + { + #region Fields + + /// + /// Top row of the matrix + /// + public Vector4 Row0; + + /// + /// 2nd row of the matrix + /// + public Vector4 Row1; + + /// + /// Bottom row of the matrix + /// + public Vector4 Row2; + + /// + /// The identity matrix + /// + public static Matrix3x4 Identity = new Matrix3x4(Vector4.UnitX, Vector4.UnitY, Vector4.UnitZ); + + #endregion + + #region Constructors + + /// + /// Constructs a new instance. + /// + /// Top row of the matrix + /// Second row of the matrix + /// Bottom row of the matrix + public Matrix3x4(Vector4 row0, Vector4 row1, Vector4 row2) + { + Row0 = row0; + Row1 = row1; + Row2 = row2; + } + + /// + /// Constructs a new instance. + /// + /// First item of the first row of the matrix. + /// Second item of the first row of the matrix. + /// Third item of the first row of the matrix. + /// Fourth item of the first row of the matrix. + /// First item of the second row of the matrix. + /// Second item of the second row of the matrix. + /// Third item of the second row of the matrix. + /// Fourth item of the second row of the matrix. + /// First item of the third row of the matrix. + /// Second item of the third row of the matrix. + /// Third item of the third row of the matrix. + /// First item of the third row of the matrix. + public Matrix3x4( + float m00, float m01, float m02, float m03, + float m10, float m11, float m12, float m13, + float m20, float m21, float m22, float m23) + { + Row0 = new Vector4(m00, m01, m02, m03); + Row1 = new Vector4(m10, m11, m12, m13); + Row2 = new Vector4(m20, m21, m22, m23); + } + + #endregion + + #region Public Members + + #region Properties + + /// + /// Gets the first column of this matrix. + /// + public Vector3 Column0 + { + get { return new Vector3(Row0.X, Row1.X, Row2.X); } + } + + /// + /// Gets the second column of this matrix. + /// + public Vector3 Column1 + { + get { return new Vector3(Row0.Y, Row1.Y, Row2.Y); } + } + + /// + /// Gets the third column of this matrix. + /// + public Vector3 Column2 + { + get { return new Vector3(Row0.Z, Row1.Z, Row2.Z); } + } + + /// + /// Gets the fourth column of this matrix. + /// + public Vector3 Column3 + { + get { return new Vector3(Row0.W, Row1.W, Row2.W); } + } + + /// + /// Gets or sets the value at row 1, column 1 of this instance. + /// + public float M11 { get { return Row0.X; } set { Row0.X = value; } } + + /// + /// Gets or sets the value at row 1, column 2 of this instance. + /// + public float M12 { get { return Row0.Y; } set { Row0.Y = value; } } + + /// + /// Gets or sets the value at row 1, column 3 of this instance. + /// + public float M13 { get { return Row0.Z; } set { Row0.Z = value; } } + + /// + /// Gets or sets the value at row 1, column 4 of this instance. + /// + public float M14 { get { return Row0.W; } set { Row0.W = value; } } + + /// + /// Gets or sets the value at row 2, column 1 of this instance. + /// + public float M21 { get { return Row1.X; } set { Row1.X = value; } } + + /// + /// Gets or sets the value at row 2, column 2 of this instance. + /// + public float M22 { get { return Row1.Y; } set { Row1.Y = value; } } + + /// + /// Gets or sets the value at row 2, column 3 of this instance. + /// + public float M23 { get { return Row1.Z; } set { Row1.Z = value; } } + + /// + /// Gets or sets the value at row 2, column 4 of this instance. + /// + public float M24 { get { return Row1.W; } set { Row1.W = value; } } + + /// + /// Gets or sets the value at row 3, column 1 of this instance. + /// + public float M31 { get { return Row2.X; } set { Row2.X = value; } } + + /// + /// Gets or sets the value at row 3, column 2 of this instance. + /// + public float M32 { get { return Row2.Y; } set { Row2.Y = value; } } + + /// + /// Gets or sets the value at row 3, column 3 of this instance. + /// + public float M33 { get { return Row2.Z; } set { Row2.Z = value; } } + + /// + /// Gets or sets the value at row 3, column 4 of this instance. + /// + public float M34 { get { return Row2.W; } set { Row2.W = value; } } + + #endregion + + #region Instance + + #region public void Invert() + + public void Invert() + { + this = Matrix3x4.Invert(this); + } + + #endregion + + #endregion + + #region Static + + #region CreateFromAxisAngle + + /// + /// Build a rotation matrix from the specified axis/angle rotation. + /// + /// The axis to rotate about. + /// Angle in radians to rotate counter-clockwise (looking in the direction of the given axis). + /// A matrix instance. + public static void CreateFromAxisAngle(Vector3 axis, float angle, out Matrix3x4 result) + { + axis.Normalize(); + float axisX = axis.X, axisY = axis.Y, axisZ = axis.Z; + + float cos = (float)System.Math.Cos(angle); + float sin = (float)System.Math.Sin(angle); + float t = 1.0f - cos; + + float tXX = t * axisX * axisX, + tXY = t * axisX * axisY, + tXZ = t * axisX * axisZ, + tYY = t * axisY * axisY, + tYZ = t * axisY * axisZ, + tZZ = t * axisZ * axisZ; + + float sinX = sin * axisX, + sinY = sin * axisY, + sinZ = sin * axisZ; + + result.Row0.X = tXX + cos; + result.Row0.Y = tXY - sinZ; + result.Row0.Z = tXZ + sinY; + result.Row0.W = 0; + result.Row1.X = tXY + sinZ; + result.Row1.Y = tYY + cos; + result.Row1.Z = tYZ - sinX; + result.Row1.W = 0; + result.Row2.X = tXZ - sinY; + result.Row2.Y = tYZ + sinX; + result.Row2.Z = tZZ + cos; + result.Row2.W = 0; + } + + /// + /// Build a rotation matrix from the specified axis/angle rotation. + /// + /// The axis to rotate about. + /// Angle in radians to rotate counter-clockwise (looking in the direction of the given axis). + /// A matrix instance. + public static Matrix3x4 CreateFromAxisAngle(Vector3 axis, float angle) + { + Matrix3x4 result; + CreateFromAxisAngle(axis, angle, out result); + return result; + } + + #endregion + + #region CreateFromQuaternion + + /// + /// Builds a rotation matrix from a quaternion. + /// + /// The quaternion to rotate by. + /// A matrix instance. + public static void CreateFromQuaternion(ref Quaternion q, out Matrix3x4 result) + { + float x = q.X, y = q.Y, z = q.Z, w = q.W, + tx = 2 * x, ty = 2 * y, tz = 2 * z, + txx = tx * x, tyy = ty * y, tzz = tz * z, + txy = tx * y, txz = tx * z, tyz = ty * z, + txw = tx * w, tyw = ty * w, tzw = tz * w; + + result.Row0.X = 1f - (tyy + tzz); + result.Row0.Y = txy + tzw; + result.Row0.Z = txz - tyw; + result.Row0.W = 0f; + result.Row1.X = txy - tzw; + result.Row1.Y = 1f - (txx + tzz); + result.Row1.Z = tyz + txw; + result.Row1.W = 0f; + result.Row2.X = txz + tyw; + result.Row2.Y = tyz - txw; + result.Row2.Z = 1f - (txx + tyy); + result.Row2.W = 0f; + + /*Vector3 axis; + float angle; + q.ToAxisAngle(out axis, out angle); + CreateFromAxisAngle(axis, angle, out result);*/ + } + + /// + /// Builds a rotation matrix from a quaternion. + /// + /// The quaternion to rotate by. + /// A matrix instance. + public static Matrix3x4 CreateFromQuaternion(Quaternion q) + { + Matrix3x4 result; + CreateFromQuaternion(ref q, out result); + return result; + } + + #endregion + + #region CreateRotation[XYZ] + + /// + /// Builds a rotation matrix for a rotation around the x-axis. + /// + /// The counter-clockwise angle in radians. + /// The resulting Matrix4 instance. + public static void CreateRotationX(float angle, out Matrix3x4 result) + { + float cos = (float)System.Math.Cos(angle); + float sin = (float)System.Math.Sin(angle); + + result = Identity; + result.Row1.Y = cos; + result.Row1.Z = sin; + result.Row2.Y = -sin; + result.Row2.Z = cos; + } + + /// + /// Builds a rotation matrix for a rotation around the x-axis. + /// + /// The counter-clockwise angle in radians. + /// The resulting Matrix4 instance. + public static Matrix3x4 CreateRotationX(float angle) + { + Matrix3x4 result; + CreateRotationX(angle, out result); + return result; + } + + /// + /// Builds a rotation matrix for a rotation around the y-axis. + /// + /// The counter-clockwise angle in radians. + /// The resulting Matrix4 instance. + public static void CreateRotationY(float angle, out Matrix3x4 result) + { + float cos = (float)System.Math.Cos(angle); + float sin = (float)System.Math.Sin(angle); + + result = Identity; + result.Row0.X = cos; + result.Row0.Z = -sin; + result.Row2.X = sin; + result.Row2.Z = cos; + } + + /// + /// Builds a rotation matrix for a rotation around the y-axis. + /// + /// The counter-clockwise angle in radians. + /// The resulting Matrix4 instance. + public static Matrix3x4 CreateRotationY(float angle) + { + Matrix3x4 result; + CreateRotationY(angle, out result); + return result; + } + + /// + /// Builds a rotation matrix for a rotation around the z-axis. + /// + /// The counter-clockwise angle in radians. + /// The resulting Matrix4 instance. + public static void CreateRotationZ(float angle, out Matrix3x4 result) + { + float cos = (float)System.Math.Cos(angle); + float sin = (float)System.Math.Sin(angle); + + result = Identity; + result.Row0.X = cos; + result.Row0.Y = sin; + result.Row1.X = -sin; + result.Row1.Y = cos; + } + + /// + /// Builds a rotation matrix for a rotation around the z-axis. + /// + /// The counter-clockwise angle in radians. + /// The resulting Matrix4 instance. + public static Matrix3x4 CreateRotationZ(float angle) + { + Matrix3x4 result; + CreateRotationZ(angle, out result); + return result; + } + + #endregion + + #region CreateTranslation + + /// + /// Creates a translation matrix. + /// + /// X translation. + /// Y translation. + /// Z translation. + /// The resulting Matrix4 instance. + public static void CreateTranslation(float x, float y, float z, out Matrix3x4 result) + { + result = Identity; + result.Row0.W = x; + result.Row1.W = y; + result.Row2.W = z; + } + + /// + /// Creates a translation matrix. + /// + /// The translation vector. + /// The resulting Matrix4 instance. + public static void CreateTranslation(ref Vector3 vector, out Matrix3x4 result) + { + result = Identity; + result.Row0.W = vector.X; + result.Row1.W = vector.Y; + result.Row2.W = vector.Z; + } + + /// + /// Creates a translation matrix. + /// + /// X translation. + /// Y translation. + /// Z translation. + /// The resulting Matrix4 instance. + public static Matrix3x4 CreateTranslation(float x, float y, float z) + { + Matrix3x4 result; + CreateTranslation(x, y, z, out result); + return result; + } + + /// + /// Creates a translation matrix. + /// + /// The translation vector. + /// The resulting Matrix4 instance. + public static Matrix3x4 CreateTranslation(Vector3 vector) + { + Matrix3x4 result; + CreateTranslation(vector.X, vector.Y, vector.Z, out result); + return result; + } + + #endregion + + #region CreateScale + + /// + /// Build a scaling matrix + /// + /// Single scale factor for x,y and z axes + /// A scaling matrix + public static Matrix3x4 CreateScale(float scale) + { + return CreateScale(scale, scale, scale); + } + + /// + /// Build a scaling matrix + /// + /// Scale factors for x,y and z axes + /// A scaling matrix + public static Matrix3x4 CreateScale(Vector3 scale) + { + return CreateScale(scale.X, scale.Y, scale.Z); + } + + /// + /// Build a scaling matrix + /// + /// Scale factor for x-axis + /// Scale factor for y-axis + /// Scale factor for z-axis + /// A scaling matrix + public static Matrix3x4 CreateScale(float x, float y, float z) + { + Matrix3x4 result = Identity; + result.Row0.X = x; + result.Row1.Y = y; + result.Row2.Z = z; + return result; + } + + #endregion + + #region Multiply Functions + + /// + /// Multiplies two instances. + /// + /// The left operand of the multiplication. + /// The right operand of the multiplication. + /// A new instance that is the result of the multiplication + public static Matrix3 Mult(Matrix3x4 left, Matrix4x3 right) + { + Matrix3 result; + Mult(ref left, ref right, out result); + return result; + } + + /// + /// Multiplies two instances. + /// + /// The left operand of the multiplication. + /// The right operand of the multiplication. + /// A new instance that is the result of the multiplication + public static void Mult(ref Matrix3x4 left, ref Matrix4x3 right, out Matrix3 result) + { + float lM11 = left.Row0.X, lM12 = left.Row0.Y, lM13 = left.Row0.Z, lM14 = left.Row0.W, + lM21 = left.Row1.X, lM22 = left.Row1.Y, lM23 = left.Row1.Z, lM24 = left.Row1.W, + lM31 = left.Row2.X, lM32 = left.Row2.Y, lM33 = left.Row2.Z, lM34 = left.Row2.W, + rM11 = right.Row0.X, rM12 = right.Row0.Y, rM13 = right.Row0.Z, + rM21 = right.Row1.X, rM22 = right.Row1.Y, rM23 = right.Row1.Z, + rM31 = right.Row2.X, rM32 = right.Row2.Y, rM33 = right.Row2.Z, + rM41 = right.Row3.X, rM42 = right.Row3.Y, rM43 = right.Row3.Z; + + result.Row0.X = (lM11 * rM11) + (lM12 * rM21) + (lM13 * rM31) + (lM14 * rM41); + result.Row0.Y = (lM11 * rM12) + (lM12 * rM22) + (lM13 * rM32) + (lM14 * rM42); + result.Row0.Z = (lM11 * rM13) + (lM12 * rM23) + (lM13 * rM33) + (lM14 * rM43); + result.Row1.X = (lM21 * rM11) + (lM22 * rM21) + (lM23 * rM31) + (lM24 * rM41); + result.Row1.Y = (lM21 * rM12) + (lM22 * rM22) + (lM23 * rM32) + (lM24 * rM42); + result.Row1.Z = (lM21 * rM13) + (lM22 * rM23) + (lM23 * rM33) + (lM24 * rM43); + result.Row2.X = (lM31 * rM11) + (lM32 * rM21) + (lM33 * rM31) + (lM34 * rM41); + result.Row2.Y = (lM31 * rM12) + (lM32 * rM22) + (lM33 * rM32) + (lM34 * rM42); + result.Row2.Z = (lM31 * rM13) + (lM32 * rM23) + (lM33 * rM33) + (lM34 * rM43); + } + + public static Matrix3x4 Mult(Matrix3x4 left, Matrix3x4 right) + { + Matrix3x4 result; + Mult(ref left, ref right, out result); + return result; + } + + public static void Mult(ref Matrix3x4 left, ref Matrix3x4 right, out Matrix3x4 result) + { + float lM11 = left.Row0.X, lM12 = left.Row0.Y, lM13 = left.Row0.Z, lM14 = left.Row0.W, + lM21 = left.Row1.X, lM22 = left.Row1.Y, lM23 = left.Row1.Z, lM24 = left.Row1.W, + lM31 = left.Row2.X, lM32 = left.Row2.Y, lM33 = left.Row2.Z, lM34 = left.Row2.W, + rM11 = right.Row0.X, rM12 = right.Row0.Y, rM13 = right.Row0.Z, rM14 = right.Row0.W, + rM21 = right.Row1.X, rM22 = right.Row1.Y, rM23 = right.Row1.Z, rM24 = right.Row1.W, + rM31 = right.Row2.X, rM32 = right.Row2.Y, rM33 = right.Row2.Z, rM34 = right.Row2.W; + + result.Row0.X = (lM11 * rM11) + (lM12 * rM21) + (lM13 * rM31); + result.Row0.Y = (lM11 * rM12) + (lM12 * rM22) + (lM13 * rM32); + result.Row0.Z = (lM11 * rM13) + (lM12 * rM23) + (lM13 * rM33); + result.Row0.W = (lM11 * rM14) + (lM12 * rM24) + (lM13 * rM34) + lM14; + result.Row1.X = (lM21 * rM11) + (lM22 * rM21) + (lM23 * rM31); + result.Row1.Y = (lM21 * rM12) + (lM22 * rM22) + (lM23 * rM32); + result.Row1.Z = (lM21 * rM13) + (lM22 * rM23) + (lM23 * rM33); + result.Row1.W = (lM21 * rM14) + (lM22 * rM24) + (lM23 * rM34) + lM24; + result.Row2.X = (lM31 * rM11) + (lM32 * rM21) + (lM33 * rM31); + result.Row2.Y = (lM31 * rM12) + (lM32 * rM22) + (lM33 * rM32); + result.Row2.Z = (lM31 * rM13) + (lM32 * rM23) + (lM33 * rM33); + result.Row2.W = (lM31 * rM14) + (lM32 * rM24) + (lM33 * rM34) + lM34; + + /*result.Row0 = (right.Row0 * lM11 + right.Row1 * lM12 + right.Row2 * lM13); + result.Row0.W += lM14; + + result.Row1 = (right.Row0 * lM21 + right.Row1 * lM22 + right.Row2 * lM23); + result.Row1.W += lM24; + + result.Row2 = (right.Row0 * lM31 + right.Row1 * lM32 + right.Row2 * lM33); + result.Row2.W += lM34;*/ + } + + public static Matrix3x4 Mult(Matrix3x4 left, float right) + { + Matrix3x4 result; + Mult(ref left, right, out result); + return result; + } + + public static void Mult(ref Matrix3x4 left, float right, out Matrix3x4 result) + { + result.Row0 = left.Row0 * right; + result.Row1 = left.Row1 * right; + result.Row2 = left.Row2 * right; + } + + #endregion + + #region Add Functions + + public static Matrix3x4 Add(Matrix3x4 left, Matrix3x4 right) + { + Matrix3x4 result; + Add(ref left, ref right, out result); + return result; + } + + public static void Add(ref Matrix3x4 left, ref Matrix3x4 right, out Matrix3x4 result) + { + result.Row0 = left.Row0 + right.Row0; + result.Row1 = left.Row1 + right.Row1; + result.Row2 = left.Row2 + right.Row2; + } + + #endregion + + #region Subtract Functions + + public static Matrix3x4 Subtract(Matrix3x4 left, Matrix3x4 right) + { + Matrix3x4 result; + Subtract(ref left, ref right, out result); + return result; + } + + public static void Subtract(ref Matrix3x4 left, ref Matrix3x4 right, out Matrix3x4 result) + { + result.Row0 = left.Row0 - right.Row0; + result.Row1 = left.Row1 - right.Row1; + result.Row2 = left.Row2 - right.Row2; + } + + #endregion + + #region Invert Functions + + public static Matrix3x4 Invert(Matrix3x4 mat) + { + Matrix3x4 result; + Invert(ref mat, out result); + return result; + } + + public static void Invert(ref Matrix3x4 mat, out Matrix3x4 result) + { + Matrix3 inverseRotation = new Matrix3(mat.Column0, mat.Column1, mat.Column2); + inverseRotation.Row0 /= inverseRotation.Row0.LengthSquared; + inverseRotation.Row1 /= inverseRotation.Row1.LengthSquared; + inverseRotation.Row2 /= inverseRotation.Row2.LengthSquared; + + Vector3 translation = new Vector3(mat.Row0.W, mat.Row1.W, mat.Row2.W); + + result.Row0 = new Vector4(inverseRotation.Row0, -Vector3.Dot(inverseRotation.Row0, translation)); + result.Row1 = new Vector4(inverseRotation.Row1, -Vector3.Dot(inverseRotation.Row1, translation)); + result.Row2 = new Vector4(inverseRotation.Row2, -Vector3.Dot(inverseRotation.Row2, translation)); + } + + #endregion + + #region Transpose + + public static Matrix4x3 Transpose(Matrix3x4 mat) + { + return new Matrix4x3(mat.Column0, mat.Column1, mat.Column2, mat.Column3); + } + + public static void Transpose(ref Matrix3x4 mat, out Matrix4x3 result) + { + result.Row0 = mat.Column0; + result.Row1 = mat.Column1; + result.Row2 = mat.Column2; + result.Row3 = mat.Column3; + } + + #endregion + + #endregion + + #region Operators + + public static Matrix3 operator *(Matrix3x4 left, Matrix4x3 right) + { + return Matrix3x4.Mult(left, right); + } + + public static Matrix3x4 operator *(Matrix3x4 left, Matrix3x4 right) + { + return Matrix3x4.Mult(left, right); + } + + public static Matrix3x4 operator *(Matrix3x4 left, float right) + { + return Matrix3x4.Mult(left, right); + } + + public static Matrix3x4 operator +(Matrix3x4 left, Matrix3x4 right) + { + return Matrix3x4.Add(left, right); + } + + public static Matrix3x4 operator -(Matrix3x4 left, Matrix3x4 right) + { + return Matrix3x4.Subtract(left, right); + } + + public static bool operator ==(Matrix3x4 left, Matrix3x4 right) + { + return left.Equals(right); + } + + public static bool operator !=(Matrix3x4 left, Matrix3x4 right) + { + return !left.Equals(right); + } + + #endregion + + #region Overrides + + #region public override string ToString() + + public override string ToString() + { + return string.Format("{0}\n{1}\n{2}", Row0, Row1, Row2); + } + + #endregion + + #region public override int GetHashCode() + + public override int GetHashCode() + { + return Row0.GetHashCode() ^ Row1.GetHashCode() ^ Row2.GetHashCode(); + } + + #endregion + + #region public override bool Equals(object obj) + + public override bool Equals(object obj) + { + if (!(obj is Matrix3x4)) + return false; + + return this.Equals((Matrix3x4)obj); + } + + #endregion + + #endregion + + #endregion + + #region IEquatable Members + + /// Indicates whether the current matrix is equal to another matrix. + /// An matrix to compare with this matrix. + /// true if the current matrix is equal to the matrix parameter; otherwise, false. + public bool Equals(Matrix3x4 other) + { + return + Row0 == other.Row0 && + Row1 == other.Row1 && + Row2 == other.Row2; + } + + #endregion + } +} diff --git a/Source/OpenTK/Math/Matrix4x3.cs b/Source/OpenTK/Math/Matrix4x3.cs new file mode 100644 index 00000000..d2715453 --- /dev/null +++ b/Source/OpenTK/Math/Matrix4x3.cs @@ -0,0 +1,760 @@ +using System; +using System.Runtime.InteropServices; + +using OpenTK; + +namespace TopHat.Mathematics +{ + /// + /// Represents a 3x4 Matrix + /// + [Serializable] + [StructLayout(LayoutKind.Sequential)] + public struct Matrix4x3 : IEquatable + { + #region Fields + + /// + /// Top row of the matrix + /// + public Vector3 Row0; + + /// + /// 2nd row of the matrix + /// + public Vector3 Row1; + + /// + /// 3rd row of the matrix + /// + public Vector3 Row2; + + /// + /// Bottom row of the matrix + /// + public Vector3 Row3; + + /// + /// The identity matrix + /// + public static Matrix4x3 Identity = new Matrix4x3(Vector3.UnitX, Vector3.UnitY, Vector3.UnitZ, Vector3.Zero); + + #endregion + + #region Constructors + + /// + /// Constructs a new instance. + /// + /// Top row of the matrix + /// Second row of the matrix + /// Third row of the matrix + /// Bottom row of the matrix + public Matrix4x3(Vector3 row0, Vector3 row1, Vector3 row2, Vector3 row3) + { + Row0 = row0; + Row1 = row1; + Row2 = row2; + Row3 = row3; + } + + /// + /// Constructs a new instance. + /// + /// First item of the first row of the matrix. + /// Second item of the first row of the matrix. + /// Third item of the first row of the matrix. + /// First item of the second row of the matrix. + /// Second item of the second row of the matrix. + /// Third item of the second row of the matrix. + /// First item of the third row of the matrix. + /// Second item of the third row of the matrix. + /// Third item of the third row of the matrix. + /// First item of the fourth row of the matrix. + /// Second item of the fourth row of the matrix. + /// Third item of the fourth row of the matrix. + public Matrix4x3( + float m00, float m01, float m02, + float m10, float m11, float m12, + float m20, float m21, float m22, + float m30, float m31, float m32) + { + Row0 = new Vector3(m00, m01, m02); + Row1 = new Vector3(m10, m11, m12); + Row2 = new Vector3(m20, m21, m22); + Row3 = new Vector3(m30, m31, m32); + } + + #endregion + + #region Public Members + + #region Properties + + /// + /// Gets the first column of this matrix. + /// + public Vector4 Column0 + { + get { return new Vector4(Row0.X, Row1.X, Row2.X, Row3.X); } + } + + /// + /// Gets the second column of this matrix. + /// + public Vector4 Column1 + { + get { return new Vector4(Row0.Y, Row1.Y, Row2.Y, Row3.Y); } + } + + /// + /// Gets the third column of this matrix. + /// + public Vector4 Column2 + { + get { return new Vector4(Row0.Z, Row1.Z, Row2.Z, Row3.Z); } + } + + /// + /// Gets or sets the value at row 1, column 1 of this instance. + /// + public float M11 { get { return Row0.X; } set { Row0.X = value; } } + + /// + /// Gets or sets the value at row 1, column 2 of this instance. + /// + public float M12 { get { return Row0.Y; } set { Row0.Y = value; } } + + /// + /// Gets or sets the value at row 1, column 3 of this instance. + /// + public float M13 { get { return Row0.Z; } set { Row0.Z = value; } } + + /// + /// Gets or sets the value at row 2, column 1 of this instance. + /// + public float M21 { get { return Row1.X; } set { Row1.X = value; } } + + /// + /// Gets or sets the value at row 2, column 2 of this instance. + /// + public float M22 { get { return Row1.Y; } set { Row1.Y = value; } } + + /// + /// Gets or sets the value at row 2, column 3 of this instance. + /// + public float M23 { get { return Row1.Z; } set { Row1.Z = value; } } + + /// + /// Gets or sets the value at row 3, column 1 of this instance. + /// + public float M31 { get { return Row2.X; } set { Row2.X = value; } } + + /// + /// Gets or sets the value at row 3, column 2 of this instance. + /// + public float M32 { get { return Row2.Y; } set { Row2.Y = value; } } + + /// + /// Gets or sets the value at row 3, column 3 of this instance. + /// + public float M33 { get { return Row2.Z; } set { Row2.Z = value; } } + + /// + /// Gets or sets the value at row 4, column 1 of this instance. + /// + public float M41 { get { return Row3.X; } set { Row3.X = value; } } + + /// + /// Gets or sets the value at row 4, column 2 of this instance. + /// + public float M42 { get { return Row3.Y; } set { Row3.Y = value; } } + + /// + /// Gets or sets the value at row 4, column 3 of this instance. + /// + public float M43 { get { return Row3.Z; } set { Row3.Z = value; } } + + #endregion + + #region Instance + + #region public void Invert() + + public void Invert() + { + this = Matrix4x3.Invert(this); + } + + #endregion + + #endregion + + #region Static + + #region CreateFromAxisAngle + + /// + /// Build a rotation matrix from the specified axis/angle rotation. + /// + /// The axis to rotate about. + /// Angle in radians to rotate counter-clockwise (looking in the direction of the given axis). + /// A matrix instance. + public static void CreateFromAxisAngle(Vector3 axis, float angle, out Matrix4x3 result) + { + axis.Normalize(); + float axisX = axis.X, axisY = axis.Y, axisZ = axis.Z; + + float cos = (float)System.Math.Cos(-angle); + float sin = (float)System.Math.Sin(-angle); + float t = 1.0f - cos; + + float tXX = t * axisX * axisX, + tXY = t * axisX * axisY, + tXZ = t * axisX * axisZ, + tYY = t * axisY * axisY, + tYZ = t * axisY * axisZ, + tZZ = t * axisZ * axisZ; + + float sinX = sin * axisX, + sinY = sin * axisY, + sinZ = sin * axisZ; + + result.Row0.X = tXX + cos; + result.Row0.Y = tXY - sinZ; + result.Row0.Z = tXZ + sinY; + result.Row1.X = tXY + sinZ; + result.Row1.Y = tYY + cos; + result.Row1.Z = tYZ - sinX; + result.Row2.X = tXZ - sinY; + result.Row2.Y = tYZ + sinX; + result.Row2.Z = tZZ + cos; + result.Row3.X = 0; + result.Row3.Y = 0; + result.Row3.Z = 0; + } + + /// + /// Build a rotation matrix from the specified axis/angle rotation. + /// + /// The axis to rotate about. + /// Angle in radians to rotate counter-clockwise (looking in the direction of the given axis). + /// A matrix instance. + public static Matrix4x3 CreateFromAxisAngle(Vector3 axis, float angle) + { + Matrix4x3 result; + CreateFromAxisAngle(axis, angle, out result); + return result; + } + + #endregion + + #region CreateFromQuaternion + + /// + /// Builds a rotation matrix from a quaternion. + /// + /// The quaternion to rotate by. + /// A matrix instance. + public static void CreateFromQuaternion(ref Quaternion q, out Matrix4x3 result) + { + float x = q.X, y = q.Y, z = q.Z, w = q.W, + tx = 2 * x, ty = 2 * y, tz = 2 * z, + txx = tx * x, tyy = ty * y, tzz = tz * z, + txy = tx * y, txz = tx * z, tyz = ty * z, + twx = w * tx, twy = w * ty, twz = w * tz; + + result.Row0.X = 1f - tyy - tzz; + result.Row0.Y = txy - twz; + result.Row0.Z = txz + twy; + result.Row1.X = txy + twz; + result.Row1.Y = 1f - txx - tzz; + result.Row1.Z = tyz - twx; + result.Row2.X = txz - twy; + result.Row2.Y = tyz + twx; + result.Row2.Z = 1f - txx - tyy; + result.Row3.X = 0; + result.Row3.Y = 0; + result.Row3.Z = 0; + + /*Vector3 axis; + float angle; + q.ToAxisAngle(out axis, out angle); + CreateFromAxisAngle(axis, angle, out result);*/ + } + + /// + /// Builds a rotation matrix from a quaternion. + /// + /// The quaternion to rotate by. + /// A matrix instance. + public static Matrix4x3 CreateFromQuaternion(Quaternion q) + { + Matrix4x3 result; + CreateFromQuaternion(ref q, out result); + return result; + } + + #endregion + + #region CreateRotation[XYZ] + + /// + /// Builds a rotation matrix for a rotation around the x-axis. + /// + /// The counter-clockwise angle in radians. + /// The resulting Matrix4 instance. + public static void CreateRotationX(float angle, out Matrix4x3 result) + { + float cos = (float)System.Math.Cos(angle); + float sin = (float)System.Math.Sin(angle); + + result = Identity; + result.Row1.Y = cos; + result.Row1.Z = sin; + result.Row2.Y = -sin; + result.Row2.Z = cos; + } + + /// + /// Builds a rotation matrix for a rotation around the x-axis. + /// + /// The counter-clockwise angle in radians. + /// The resulting Matrix4 instance. + public static Matrix4x3 CreateRotationX(float angle) + { + Matrix4x3 result; + CreateRotationX(angle, out result); + return result; + } + + /// + /// Builds a rotation matrix for a rotation around the y-axis. + /// + /// The counter-clockwise angle in radians. + /// The resulting Matrix4 instance. + public static void CreateRotationY(float angle, out Matrix4x3 result) + { + float cos = (float)System.Math.Cos(angle); + float sin = (float)System.Math.Sin(angle); + + result = Identity; + result.Row0.X = cos; + result.Row0.Z = -sin; + result.Row2.X = sin; + result.Row2.Z = cos; + } + + /// + /// Builds a rotation matrix for a rotation around the y-axis. + /// + /// The counter-clockwise angle in radians. + /// The resulting Matrix4 instance. + public static Matrix4x3 CreateRotationY(float angle) + { + Matrix4x3 result; + CreateRotationY(angle, out result); + return result; + } + + /// + /// Builds a rotation matrix for a rotation around the z-axis. + /// + /// The counter-clockwise angle in radians. + /// The resulting Matrix4 instance. + public static void CreateRotationZ(float angle, out Matrix4x3 result) + { + float cos = (float)System.Math.Cos(angle); + float sin = (float)System.Math.Sin(angle); + + result = Identity; + result.Row0.X = cos; + result.Row0.Y = sin; + result.Row1.X = -sin; + result.Row1.Y = cos; + } + + /// + /// Builds a rotation matrix for a rotation around the z-axis. + /// + /// The counter-clockwise angle in radians. + /// The resulting Matrix4 instance. + public static Matrix4x3 CreateRotationZ(float angle) + { + Matrix4x3 result; + CreateRotationZ(angle, out result); + return result; + } + + #endregion + + #region CreateTranslation + + /// + /// Creates a translation matrix. + /// + /// X translation. + /// Y translation. + /// Z translation. + /// The resulting Matrix4 instance. + public static void CreateTranslation(float x, float y, float z, out Matrix4x3 result) + { + result = Identity; + result.Row3.X = x; + result.Row3.Y = y; + result.Row3.Z = z; + } + + /// + /// Creates a translation matrix. + /// + /// The translation vector. + /// The resulting Matrix4 instance. + public static void CreateTranslation(ref Vector3 vector, out Matrix4x3 result) + { + result = Identity; + result.Row3.X = vector.X; + result.Row3.Y = vector.Y; + result.Row3.Z = vector.Z; + } + + /// + /// Creates a translation matrix. + /// + /// X translation. + /// Y translation. + /// Z translation. + /// The resulting Matrix4 instance. + public static Matrix4x3 CreateTranslation(float x, float y, float z) + { + Matrix4x3 result; + CreateTranslation(x, y, z, out result); + return result; + } + + /// + /// Creates a translation matrix. + /// + /// The translation vector. + /// The resulting Matrix4 instance. + public static Matrix4x3 CreateTranslation(Vector3 vector) + { + Matrix4x3 result; + CreateTranslation(vector.X, vector.Y, vector.Z, out result); + return result; + } + + #endregion + + #region CreateScale + + /// + /// Build a scaling matrix + /// + /// Single scale factor for x,y and z axes + /// A scaling matrix + public static Matrix4x3 CreateScale(float scale) + { + return CreateScale(scale, scale, scale); + } + + /// + /// Build a scaling matrix + /// + /// Scale factors for x,y and z axes + /// A scaling matrix + public static Matrix4x3 CreateScale(Vector3 scale) + { + return CreateScale(scale.X, scale.Y, scale.Z); + } + + /// + /// Build a scaling matrix + /// + /// Scale factor for x-axis + /// Scale factor for y-axis + /// Scale factor for z-axis + /// A scaling matrix + public static Matrix4x3 CreateScale(float x, float y, float z) + { + Matrix4x3 result = Identity; + result.Row0.X = x; + result.Row1.Y = y; + result.Row2.Z = z; + return result; + } + + #endregion + + #region Multiply Functions + + /// + /// Multiplies two instances. + /// + /// The left operand of the multiplication. + /// The right operand of the multiplication. + /// A new instance that is the result of the multiplication + public static Matrix4 Mult(Matrix4x3 left, Matrix3x4 right) + { + Matrix4 result; + Mult(ref left, ref right, out result); + return result; + } + + /// + /// Multiplies two instances. + /// + /// The left operand of the multiplication. + /// The right operand of the multiplication. + /// A new instance that is the result of the multiplication + public static void Mult(ref Matrix4x3 left, ref Matrix3x4 right, out Matrix4 result) + { + float lM11 = left.Row0.X, lM12 = left.Row0.Y, lM13 = left.Row0.Z, + lM21 = left.Row1.X, lM22 = left.Row1.Y, lM23 = left.Row1.Z, + lM31 = left.Row2.X, lM32 = left.Row2.Y, lM33 = left.Row2.Z, + lM41 = left.Row3.X, lM42 = left.Row3.Y, lM43 = left.Row3.Z, + rM11 = right.Row0.X, rM12 = right.Row0.Y, rM13 = right.Row0.Z, rM14 = right.Row0.W, + rM21 = right.Row1.X, rM22 = right.Row1.Y, rM23 = right.Row1.Z, rM24 = right.Row1.W, + rM31 = right.Row2.X, rM32 = right.Row2.Y, rM33 = right.Row2.Z, rM34 = right.Row2.W; + + result.Row0.X = (lM11 * rM11) + (lM12 * rM21) + (lM13 * rM31); + result.Row0.Y = (lM11 * rM12) + (lM12 * rM22) + (lM13 * rM32); + result.Row0.Z = (lM11 * rM13) + (lM12 * rM23) + (lM13 * rM33); + result.Row0.W = (lM11 * rM14) + (lM12 * rM24) + (lM13 * rM34); + result.Row1.X = (lM21 * rM11) + (lM22 * rM21) + (lM23 * rM31); + result.Row1.Y = (lM21 * rM12) + (lM22 * rM22) + (lM23 * rM32); + result.Row1.Z = (lM21 * rM13) + (lM22 * rM23) + (lM23 * rM33); + result.Row1.W = (lM21 * rM14) + (lM22 * rM24) + (lM23 * rM34); + result.Row2.X = (lM31 * rM11) + (lM32 * rM21) + (lM33 * rM31); + result.Row2.Y = (lM31 * rM12) + (lM32 * rM22) + (lM33 * rM32); + result.Row2.Z = (lM31 * rM13) + (lM32 * rM23) + (lM33 * rM33); + result.Row2.W = (lM31 * rM14) + (lM32 * rM24) + (lM33 * rM34); + result.Row3.X = (lM41 * rM11) + (lM42 * rM21) + (lM43 * rM31); + result.Row3.Y = (lM41 * rM12) + (lM42 * rM22) + (lM43 * rM32); + result.Row3.Z = (lM41 * rM13) + (lM42 * rM23) + (lM43 * rM33); + result.Row3.W = (lM41 * rM14) + (lM42 * rM24) + (lM43 * rM34); + } + + public static Matrix4x3 Mult(Matrix4x3 left, Matrix4x3 right) + { + Matrix4x3 result; + Mult(ref left, ref right, out result); + return result; + } + + public static void Mult(ref Matrix4x3 left, ref Matrix4x3 right, out Matrix4x3 result) + { + float lM11 = left.Row0.X, lM12 = left.Row0.Y, lM13 = left.Row0.Z, + lM21 = left.Row1.X, lM22 = left.Row1.Y, lM23 = left.Row1.Z, + lM31 = left.Row2.X, lM32 = left.Row2.Y, lM33 = left.Row2.Z, + lM41 = left.Row3.X, lM42 = left.Row3.Y, lM43 = left.Row3.Z, + rM11 = right.Row0.X, rM12 = right.Row0.Y, rM13 = right.Row0.Z, + rM21 = right.Row1.X, rM22 = right.Row1.Y, rM23 = right.Row1.Z, + rM31 = right.Row2.X, rM32 = right.Row2.Y, rM33 = right.Row2.Z, + rM41 = right.Row3.X, rM42 = right.Row3.Y, rM43 = right.Row3.Z; + + result.Row0.X = (lM11 * rM11) + (lM12 * rM21) + (lM13 * rM31) + rM41; + result.Row0.Y = (lM11 * rM12) + (lM12 * rM22) + (lM13 * rM32) + rM42; + result.Row0.Z = (lM11 * rM13) + (lM12 * rM23) + (lM13 * rM33) + rM43; + result.Row1.X = (lM21 * rM11) + (lM22 * rM21) + (lM23 * rM31) + rM41; + result.Row1.Y = (lM21 * rM12) + (lM22 * rM22) + (lM23 * rM32) + rM42; + result.Row1.Z = (lM21 * rM13) + (lM22 * rM23) + (lM23 * rM33) + rM43; + result.Row2.X = (lM31 * rM11) + (lM32 * rM21) + (lM33 * rM31) + rM41; + result.Row2.Y = (lM31 * rM12) + (lM32 * rM22) + (lM33 * rM32) + rM42; + result.Row2.Z = (lM31 * rM13) + (lM32 * rM23) + (lM33 * rM33) + rM43; + result.Row3.X = (lM41 * rM11) + (lM42 * rM21) + (lM43 * rM31) + rM41; + result.Row3.Y = (lM41 * rM12) + (lM42 * rM22) + (lM43 * rM32) + rM42; + result.Row3.Z = (lM41 * rM13) + (lM42 * rM23) + (lM43 * rM33) + rM43; + } + + public static Matrix4x3 Mult(Matrix4x3 left, float right) + { + Matrix4x3 result; + Mult(ref left, right, out result); + return result; + } + + public static void Mult(ref Matrix4x3 left, float right, out Matrix4x3 result) + { + result.Row0 = left.Row0 * right; + result.Row1 = left.Row1 * right; + result.Row2 = left.Row2 * right; + result.Row3 = left.Row3 * right; + } + + #endregion + + #region Add Functions + + public static Matrix4x3 Add(Matrix4x3 left, Matrix4x3 right) + { + Matrix4x3 result; + Add(ref left, ref right, out result); + return result; + } + + public static void Add(ref Matrix4x3 left, ref Matrix4x3 right, out Matrix4x3 result) + { + result.Row0 = left.Row0 + right.Row0; + result.Row1 = left.Row1 + right.Row1; + result.Row2 = left.Row2 + right.Row2; + result.Row3 = left.Row3 + right.Row3; + } + + #endregion + + #region Subtract Functions + + public static Matrix4x3 Subtract(Matrix4x3 left, Matrix4x3 right) + { + Matrix4x3 result; + Subtract(ref left, ref right, out result); + return result; + } + + public static void Subtract(ref Matrix4x3 left, ref Matrix4x3 right, out Matrix4x3 result) + { + result.Row0 = left.Row0 - right.Row0; + result.Row1 = left.Row1 - right.Row1; + result.Row2 = left.Row2 - right.Row2; + result.Row3 = left.Row3 - right.Row3; + } + + #endregion + + #region Invert Functions + + public static Matrix4x3 Invert(Matrix4x3 mat) + { + Matrix4x3 result; + Invert(ref mat, out result); + return result; + } + + public static void Invert(ref Matrix4x3 mat, out Matrix4x3 result) + { + Matrix3 inverseRotation = new Matrix3(mat.Column0.Xyz, mat.Column1.Xyz, mat.Column2.Xyz); + inverseRotation.Row0 /= inverseRotation.Row0.LengthSquared; + inverseRotation.Row1 /= inverseRotation.Row1.LengthSquared; + inverseRotation.Row2 /= inverseRotation.Row2.LengthSquared; + + Vector3 translation = mat.Row3; + + result.Row0 = inverseRotation.Row0; + result.Row1 = inverseRotation.Row1; + result.Row2 = inverseRotation.Row2; + result.Row3 = new Vector3(-Vector3.Dot(inverseRotation.Row0, translation), -Vector3.Dot(inverseRotation.Row1, translation), -Vector3.Dot(inverseRotation.Row2, translation)); + } + + #endregion + + #region Transpose + + /*public static Matrix3x4 Transpose(Matrix4x3 mat) + { + return new Matrix3x4(mat.Column0, mat.Column1, mat.Column2); + } + + public static void Transpose(ref Matrix4x3 mat, out Matrix4x3 result) + { + result.Row0 = mat.Column0; + result.Row1 = mat.Column1; + result.Row2 = mat.Column2; + }*/ + + #endregion + + #endregion + + #region Operators + + public static Matrix4 operator *(Matrix4x3 left, Matrix3x4 right) + { + return Matrix4x3.Mult(left, right); + } + + public static Matrix4x3 operator *(Matrix4x3 left, Matrix4x3 right) + { + return Matrix4x3.Mult(left, right); + } + + public static Matrix4x3 operator *(Matrix4x3 left, float right) + { + return Matrix4x3.Mult(left, right); + } + + public static Matrix4x3 operator +(Matrix4x3 left, Matrix4x3 right) + { + return Matrix4x3.Add(left, right); + } + + public static Matrix4x3 operator -(Matrix4x3 left, Matrix4x3 right) + { + return Matrix4x3.Subtract(left, right); + } + + public static bool operator ==(Matrix4x3 left, Matrix4x3 right) + { + return left.Equals(right); + } + + public static bool operator !=(Matrix4x3 left, Matrix4x3 right) + { + return !left.Equals(right); + } + + #endregion + + #region Overrides + + #region public override string ToString() + + public override string ToString() + { + return string.Format("{0}\n{1}\n{2}", Row0, Row1, Row2); + } + + #endregion + + #region public override int GetHashCode() + + public override int GetHashCode() + { + return Row0.GetHashCode() ^ Row1.GetHashCode() ^ Row2.GetHashCode(); + } + + #endregion + + #region public override bool Equals(object obj) + + public override bool Equals(object obj) + { + if (!(obj is Matrix4x3)) + return false; + + return this.Equals((Matrix4x3)obj); + } + + #endregion + + #endregion + + #endregion + + #region IEquatable Members + + /// Indicates whether the current matrix is equal to another matrix. + /// An matrix to compare with this matrix. + /// true if the current matrix is equal to the matrix parameter; otherwise, false. + public bool Equals(Matrix4x3 other) + { + return + Row0 == other.Row0 && + Row1 == other.Row1 && + Row2 == other.Row2 && + Row3 == other.Row3; + } + + #endregion + } +} diff --git a/Source/OpenTK/OpenTK.csproj b/Source/OpenTK/OpenTK.csproj index d7b8a614..c89bb8f1 100644 --- a/Source/OpenTK/OpenTK.csproj +++ b/Source/OpenTK/OpenTK.csproj @@ -1,4 +1,4 @@ - + Local @@ -133,6 +133,8 @@ + + @@ -766,8 +768,8 @@ - - + +