From d4a63df42b46679ba435ca5d44fd1e1bd78ff8b1 Mon Sep 17 00:00:00 2001 From: the_fiddler Date: Sun, 20 Jan 2008 17:14:12 +0000 Subject: [PATCH] Added JTalton's double precision mathlib. --- Source/Bind/Structures/Function.cs | 32 +- Source/OpenTK/Math/Matrix3d.cs | 791 ++++++++++ Source/OpenTK/Math/Matrix4d.cs | 2345 ++++++++++++++++++++++++++++ Source/OpenTK/Math/Quaterniond.cs | 638 ++++++++ Source/OpenTK/Math/Vector2d.cs | 921 +++++++++++ Source/OpenTK/Math/Vector3d.cs | 1216 +++++++++++++++ Source/OpenTK/Math/Vector4d.cs | 1343 ++++++++++++++++ 7 files changed, 7271 insertions(+), 15 deletions(-) create mode 100644 Source/OpenTK/Math/Matrix3d.cs create mode 100644 Source/OpenTK/Math/Matrix4d.cs create mode 100644 Source/OpenTK/Math/Quaterniond.cs create mode 100644 Source/OpenTK/Math/Vector2d.cs create mode 100644 Source/OpenTK/Math/Vector3d.cs create mode 100644 Source/OpenTK/Math/Vector4d.cs diff --git a/Source/Bind/Structures/Function.cs b/Source/Bind/Structures/Function.cs index a760a6b7..cbee23c7 100644 --- a/Source/Bind/Structures/Function.cs +++ b/Source/Bind/Structures/Function.cs @@ -218,21 +218,6 @@ namespace Bind.Structures if (Parameters.HasPointerParameters) { - // Pointer overloads - foreach (Parameter p in this.Parameters) - { - if (p.WrapperType == WrapperTypes.ArrayParameter) - { - p.Reference = false; - p.Array = 0; - p.Pointer = true; - } - } - f = new Function(this); - f.CreateBody(false); - wrappers.Add(f); - new Function(f).WrapVoidPointers(wrappers); - // Array overloads foreach (Parameter p in this.Parameters) { @@ -262,6 +247,23 @@ namespace Bind.Structures f.CreateBody(false); wrappers.Add(f); new Function(f).WrapVoidPointers(wrappers); + + // Pointer overloads + // Should be last to work around Intellisense bug, where + // array overloads are not reported if there is a pointer overload. + foreach (Parameter p in this.Parameters) + { + if (p.WrapperType == WrapperTypes.ArrayParameter) + { + p.Reference = false; + p.Array = 0; + p.Pointer = true; + } + } + f = new Function(this); + f.CreateBody(false); + wrappers.Add(f); + new Function(f).WrapVoidPointers(wrappers); } else { diff --git a/Source/OpenTK/Math/Matrix3d.cs b/Source/OpenTK/Math/Matrix3d.cs new file mode 100644 index 00000000..c98c79c9 --- /dev/null +++ b/Source/OpenTK/Math/Matrix3d.cs @@ -0,0 +1,791 @@ +using System; +using System.Runtime.InteropServices; + +namespace OpenTK.Math +{ + [Serializable] + [StructLayout(LayoutKind.Sequential)] + public struct Matrix3d : IEquatable + { + #region Fields & Access + + /// Row 0, Column 0 + public double R0C0; + + /// Row 0, Column 1 + public double R0C1; + + /// Row 0, Column 2 + public double R0C2; + + /// Row 1, Column 0 + public double R1C0; + + /// Row 1, Column 1 + public double R1C1; + + /// Row 1, Column 2 + public double R1C2; + + /// Row 2, Column 0 + public double R2C0; + + /// Row 2, Column 1 + public double R2C1; + + /// Row 2, Column 2 + public double R2C2; + + /// Gets the component at the given row and column in the matrix. + /// The row of the matrix. + /// The column of the matrix. + /// The component at the given row and column in the matrix. + public double this[int row, int column] + { + get + { + switch( row ) + { + case 0: + switch (column) + { + case 0: return R0C0; + case 1: return R0C1; + case 2: return R0C2; + } + break; + + case 1: + switch (column) + { + case 0: return R1C0; + case 1: return R1C1; + case 2: return R1C2; + } + break; + + case 2: + switch (column) + { + case 0: return R2C0; + case 1: return R2C1; + case 2: return R2C2; + } + break; + } + + throw new IndexOutOfRangeException(); + } + set + { + switch( row ) + { + case 0: + switch (column) + { + case 0: R0C0 = value; return; + case 1: R0C1 = value; return; + case 2: R0C2 = value; return; + } + break; + + case 1: + switch (column) + { + case 0: R1C0 = value; return; + case 1: R1C1 = value; return; + case 2: R1C2 = value; return; + } + break; + + case 2: + switch (column) + { + case 0: R2C0 = value; return; + case 1: R2C1 = value; return; + case 2: R2C2 = value; return; + } + break; + } + + throw new IndexOutOfRangeException(); + } + } + + /// Gets the component at the index into the matrix. + /// The index into the components of the matrix. + /// The component at the given index into the matrix. + public double this[int index] + { + get + { + switch (index) + { + case 0: return R0C0; + case 1: return R0C1; + case 2: return R0C2; + case 3: return R1C0; + case 4: return R1C1; + case 5: return R1C2; + case 6: return R2C0; + case 7: return R2C1; + case 8: return R2C2; + default: throw new IndexOutOfRangeException(); + } + } + set + { + switch (index) + { + case 0: R0C0 = value; return; + case 1: R0C1 = value; return; + case 2: R0C2 = value; return; + case 3: R1C0 = value; return; + case 4: R1C1 = value; return; + case 5: R1C2 = value; return; + case 6: R2C0 = value; return; + case 7: R2C1 = value; return; + case 8: R2C2 = value; return; + default: throw new IndexOutOfRangeException(); + } + } + } + + /// Converts the matrix into an IntPtr. + /// The matrix to convert. + /// An IntPtr for the matrix. + public static explicit operator IntPtr(Matrix3d matrix) + { + unsafe + { + return (IntPtr)(&matrix.R0C0); + } + } + + /// Converts the matrix into left double*. + /// The matrix to convert. + /// A double* for the matrix. + [CLSCompliant(false)] + unsafe public static explicit operator double*(Matrix3d matrix) + { + return &matrix.R0C0; + } + + /// Converts the matrix into an array of doubles. + /// The matrix to convert. + /// An array of doubles for the matrix. + public static explicit operator double[](Matrix3d matrix) + { + return new double[9] + { + matrix.R0C0, + matrix.R0C1, + matrix.R0C2, + matrix.R1C0, + matrix.R1C1, + matrix.R1C2, + matrix.R2C0, + matrix.R2C1, + matrix.R2C2 + }; + } + + #endregion + + + #region Constructors + + /// Constructs left matrix with the same components as the given matrix. + /// The matrix whose components to copy. + public Matrix3d(ref Matrix3d matrix) + { + this.R0C0 = matrix.R0C0; + this.R0C1 = matrix.R0C1; + this.R0C2 = matrix.R0C2; + this.R1C0 = matrix.R1C0; + this.R1C1 = matrix.R1C1; + this.R1C2 = matrix.R1C2; + this.R2C0 = matrix.R2C0; + this.R2C1 = matrix.R2C1; + this.R2C2 = matrix.R2C2; + } + + /// Constructs left matrix with the given values. + /// The value for row 0 column 0. + /// The value for row 0 column 1. + /// The value for row 0 column 2. + /// The value for row 1 column 0. + /// The value for row 1 column 1. + /// The value for row 1 column 2. + /// The value for row 2 column 0. + /// The value for row 2 column 1. + /// The value for row 2 column 2. + public Matrix3d + ( + double r0c0, + double r0c1, + double r0c2, + double r1c0, + double r1c1, + double r1c2, + double r2c0, + double r2c1, + double r2c2 + ) + { + this.R0C0 = r0c0; + this.R0C1 = r0c1; + this.R0C2 = r0c2; + this.R1C0 = r1c0; + this.R1C1 = r1c1; + this.R1C2 = r1c2; + this.R2C0 = r2c0; + this.R2C1 = r2c1; + this.R2C2 = r2c2; + } + + /// Constructs left matrix from the given array of double-precision floating point numbers. + /// The array of doubles for the components of the matrix. + public Matrix3d(double[] doubleArray) + { + if (doubleArray == null || doubleArray.GetLength(0) < 9) throw new MissingFieldException(); + + this.R0C0 = doubleArray[0]; + this.R0C1 = doubleArray[1]; + this.R0C2 = doubleArray[2]; + this.R1C0 = doubleArray[3]; + this.R1C1 = doubleArray[4]; + this.R1C2 = doubleArray[5]; + this.R2C0 = doubleArray[6]; + this.R2C1 = doubleArray[7]; + this.R2C2 = doubleArray[8]; + } + + /// Constructs left matrix from the given quaternion. + /// The quaternion to use to construct the martix. + public Matrix3d(ref Quaterniond quaternion) + { + Quaterniond quaternionNormalized; + quaternion.Normalize(out quaternionNormalized); + + double xx = quaternionNormalized.X * quaternionNormalized.X; + double yy = quaternionNormalized.Y * quaternionNormalized.Y; + double zz = quaternionNormalized.Z * quaternionNormalized.Z; + double xy = quaternionNormalized.X * quaternionNormalized.Y; + double xz = quaternionNormalized.X * quaternionNormalized.Z; + double yz = quaternionNormalized.Y * quaternionNormalized.Z; + double wx = quaternionNormalized.W * quaternionNormalized.X; + double wy = quaternionNormalized.W * quaternionNormalized.Y; + double wz = quaternionNormalized.W * quaternionNormalized.Z; + + R0C0 = 1 - 2 * (yy + zz); + R0C1 = 2 * (xy - wz); + R0C2 = 2 * (xz + wy); + + R1C0 = 2 * (xy + wz); + R1C1 = 1 - 2 * (xx + zz); + R1C2 = 2 * (yz - wx); + + R2C0 = 2 * (xz - wy); + R2C1 = 2 * (yz + wx); + R2C2 = 1 - 2 * (xx + yy); + } + + #endregion + + + #region Equality + + /// 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(Matrix3d matrix) + { + return + R0C0 == matrix.R0C0 && + R0C1 == matrix.R0C1 && + R0C2 == matrix.R0C2 && + R1C0 == matrix.R1C0 && + R1C1 == matrix.R1C1 && + R1C2 == matrix.R1C2 && + R2C0 == matrix.R2C0 && + R2C1 == matrix.R2C1 && + R2C2 == matrix.R2C2; + } + + public bool Equals(ref Matrix3d matrix) + { + return + R0C0 == matrix.R0C0 && + R0C1 == matrix.R0C1 && + R0C2 == matrix.R0C2 && + R1C0 == matrix.R1C0 && + R1C1 == matrix.R1C1 && + R1C2 == matrix.R1C2 && + R2C0 == matrix.R2C0 && + R2C1 == matrix.R2C1 && + R2C2 == matrix.R2C2; + } + + public static bool Equals(ref Matrix3d left, ref Matrix3d right) + { + return + left.R0C0 == right.R0C0 && + left.R0C1 == right.R0C1 && + left.R0C2 == right.R0C2 && + left.R1C0 == right.R1C0 && + left.R1C1 == right.R1C1 && + left.R1C2 == right.R1C2 && + left.R2C0 == right.R2C0 && + left.R2C1 == right.R2C1 && + left.R2C2 == right.R2C2; + } + + public bool EqualsApprox(ref Matrix3d matrix, double tolerance) + { + return + System.Math.Abs(R0C0 - matrix.R0C0) <= tolerance && + System.Math.Abs(R0C1 - matrix.R0C1) <= tolerance && + System.Math.Abs(R0C2 - matrix.R0C2) <= tolerance && + System.Math.Abs(R1C0 - matrix.R1C0) <= tolerance && + System.Math.Abs(R1C1 - matrix.R1C1) <= tolerance && + System.Math.Abs(R1C2 - matrix.R1C2) <= tolerance && + System.Math.Abs(R2C0 - matrix.R2C0) <= tolerance && + System.Math.Abs(R2C1 - matrix.R2C1) <= tolerance && + System.Math.Abs(R2C2 - matrix.R2C2) <= tolerance; + } + + public static bool EqualsApprox(ref Matrix3d left, ref Matrix3d right, double tolerance) + { + return + System.Math.Abs(left.R0C0 - right.R0C0) <= tolerance && + System.Math.Abs(left.R0C1 - right.R0C1) <= tolerance && + System.Math.Abs(left.R0C2 - right.R0C2) <= tolerance && + System.Math.Abs(left.R1C0 - right.R1C0) <= tolerance && + System.Math.Abs(left.R1C1 - right.R1C1) <= tolerance && + System.Math.Abs(left.R1C2 - right.R1C2) <= tolerance && + System.Math.Abs(left.R2C0 - right.R2C0) <= tolerance && + System.Math.Abs(left.R2C1 - right.R2C1) <= tolerance && + System.Math.Abs(left.R2C2 - right.R2C2) <= tolerance; + } + + #endregion + + + #region Arithmetic Operators + + + /// Add left matrix to this matrix. + /// The matrix to add. + public void Add(ref Matrix3d matrix) + { + R0C0 = R0C0 + matrix.R0C0; + R0C1 = R0C1 + matrix.R0C1; + R0C2 = R0C2 + matrix.R0C2; + R1C0 = R1C0 + matrix.R1C0; + R1C1 = R1C1 + matrix.R1C1; + R1C2 = R1C2 + matrix.R1C2; + R2C0 = R2C0 + matrix.R2C0; + R2C1 = R2C1 + matrix.R2C1; + R2C2 = R2C2 + matrix.R2C2; + } + + /// Add left matrix to this matrix. + /// The matrix to add. + /// The resulting matrix of the addition. + public void Add(ref Matrix3d matrix, out Matrix3d result) + { + result.R0C0 = R0C0 + matrix.R0C0; + result.R0C1 = R0C1 + matrix.R0C1; + result.R0C2 = R0C2 + matrix.R0C2; + result.R1C0 = R1C0 + matrix.R1C0; + result.R1C1 = R1C1 + matrix.R1C1; + result.R1C2 = R1C2 + matrix.R1C2; + result.R2C0 = R2C0 + matrix.R2C0; + result.R2C1 = R2C1 + matrix.R2C1; + result.R2C2 = R2C2 + matrix.R2C2; + } + + /// Add left matrix to left matrix. + /// The matrix on the matrix side of the equation. + /// The matrix on the right side of the equation + /// The resulting matrix of the addition. + public static void Add(ref Matrix3d left, ref Matrix3d right, out Matrix3d result) + { + result.R0C0 = left.R0C0 + right.R0C0; + result.R0C1 = left.R0C1 + right.R0C1; + result.R0C2 = left.R0C2 + right.R0C2; + result.R1C0 = left.R1C0 + right.R1C0; + result.R1C1 = left.R1C1 + right.R1C1; + result.R1C2 = left.R1C2 + right.R1C2; + result.R2C0 = left.R2C0 + right.R2C0; + result.R2C1 = left.R2C1 + right.R2C1; + result.R2C2 = left.R2C2 + right.R2C2; + } + + + /// Subtract left matrix from this matrix. + /// The matrix to subtract. + public void Subtract(ref Matrix3d matrix) + { + R0C0 = R0C0 + matrix.R0C0; + R0C1 = R0C1 + matrix.R0C1; + R0C2 = R0C2 + matrix.R0C2; + R1C0 = R1C0 + matrix.R1C0; + R1C1 = R1C1 + matrix.R1C1; + R1C2 = R1C2 + matrix.R1C2; + R2C0 = R2C0 + matrix.R2C0; + R2C1 = R2C1 + matrix.R2C1; + R2C2 = R2C2 + matrix.R2C2; + } + + /// Subtract left matrix from this matrix. + /// The matrix to subtract. + /// The resulting matrix of the subtraction. + public void Subtract(ref Matrix3d matrix, out Matrix3d result) + { + result.R0C0 = R0C0 + matrix.R0C0; + result.R0C1 = R0C1 + matrix.R0C1; + result.R0C2 = R0C2 + matrix.R0C2; + result.R1C0 = R1C0 + matrix.R1C0; + result.R1C1 = R1C1 + matrix.R1C1; + result.R1C2 = R1C2 + matrix.R1C2; + result.R2C0 = R2C0 + matrix.R2C0; + result.R2C1 = R2C1 + matrix.R2C1; + result.R2C2 = R2C2 + matrix.R2C2; + } + + /// Subtract left matrix from left matrix. + /// The matrix on the matrix side of the equation. + /// The matrix on the right side of the equation + /// The resulting matrix of the subtraction. + public static void Subtract(ref Matrix3d left, ref Matrix3d right, out Matrix3d result) + { + result.R0C0 = left.R0C0 + right.R0C0; + result.R0C1 = left.R0C1 + right.R0C1; + result.R0C2 = left.R0C2 + right.R0C2; + result.R1C0 = left.R1C0 + right.R1C0; + result.R1C1 = left.R1C1 + right.R1C1; + result.R1C2 = left.R1C2 + right.R1C2; + result.R2C0 = left.R2C0 + right.R2C0; + result.R2C1 = left.R2C1 + right.R2C1; + result.R2C2 = left.R2C2 + right.R2C2; + } + + + /// Multiply left martix times this matrix. + /// The matrix to multiply. + public void Multiply(ref Matrix3d matrix) + { + double r0c0 = matrix.R0C0 * R0C0 + matrix.R0C1 * R1C0 + matrix.R0C2 * R2C0; + double r0c1 = matrix.R0C0 * R0C1 + matrix.R0C1 * R1C1 + matrix.R0C2 * R2C1; + double r0c2 = matrix.R0C0 * R0C2 + matrix.R0C1 * R1C2 + matrix.R0C2 * R2C2; + + double r1c0 = matrix.R1C0 * R0C0 + matrix.R1C1 * R1C0 + matrix.R1C2 * R2C0; + double r1c1 = matrix.R1C0 * R0C1 + matrix.R1C1 * R1C1 + matrix.R1C2 * R2C1; + double r1c2 = matrix.R1C0 * R0C2 + matrix.R1C1 * R1C2 + matrix.R1C2 * R2C2; + + R2C0 = matrix.R2C0 * R0C0 + matrix.R2C1 * R1C0 + matrix.R2C2 * R2C0; + R2C1 = matrix.R2C0 * R0C1 + matrix.R2C1 * R1C1 + matrix.R2C2 * R2C1; + R2C2 = matrix.R2C0 * R0C2 + matrix.R2C1 * R1C2 + matrix.R2C2 * R2C2; + + + R0C0 = r0c0; + R0C1 = r0c1; + R0C2 = r0c2; + + R1C0 = r1c0; + R1C1 = r1c1; + R1C2 = r1c2; + } + + /// Multiply matrix times this matrix. + /// The matrix to multiply. + /// The resulting matrix of the multiplication. + public void Multiply(ref Matrix3d matrix, out Matrix3d result) + { + result.R0C0 = matrix.R0C0 * R0C0 + matrix.R0C1 * R1C0 + matrix.R0C2 * R2C0; + result.R0C1 = matrix.R0C0 * R0C1 + matrix.R0C1 * R1C1 + matrix.R0C2 * R2C1; + result.R0C2 = matrix.R0C0 * R0C2 + matrix.R0C1 * R1C2 + matrix.R0C2 * R2C2; + result.R1C0 = matrix.R1C0 * R0C0 + matrix.R1C1 * R1C0 + matrix.R1C2 * R2C0; + result.R1C1 = matrix.R1C0 * R0C1 + matrix.R1C1 * R1C1 + matrix.R1C2 * R2C1; + result.R1C2 = matrix.R1C0 * R0C2 + matrix.R1C1 * R1C2 + matrix.R1C2 * R2C2; + result.R2C0 = matrix.R2C0 * R0C0 + matrix.R2C1 * R1C0 + matrix.R2C2 * R2C0; + result.R2C1 = matrix.R2C0 * R0C1 + matrix.R2C1 * R1C1 + matrix.R2C2 * R2C1; + result.R2C2 = matrix.R2C0 * R0C2 + matrix.R2C1 * R1C2 + matrix.R2C2 * R2C2; + } + + /// Multiply left matrix times left matrix. + /// The matrix on the matrix side of the equation. + /// The matrix on the right side of the equation + /// The resulting matrix of the multiplication. + public static void Multiply(ref Matrix3d left, ref Matrix3d right, out Matrix3d result) + { + result.R0C0 = right.R0C0 * left.R0C0 + right.R0C1 * left.R1C0 + right.R0C2 * left.R2C0; + result.R0C1 = right.R0C0 * left.R0C1 + right.R0C1 * left.R1C1 + right.R0C2 * left.R2C1; + result.R0C2 = right.R0C0 * left.R0C2 + right.R0C1 * left.R1C2 + right.R0C2 * left.R2C2; + result.R1C0 = right.R1C0 * left.R0C0 + right.R1C1 * left.R1C0 + right.R1C2 * left.R2C0; + result.R1C1 = right.R1C0 * left.R0C1 + right.R1C1 * left.R1C1 + right.R1C2 * left.R2C1; + result.R1C2 = right.R1C0 * left.R0C2 + right.R1C1 * left.R1C2 + right.R1C2 * left.R2C2; + result.R2C0 = right.R2C0 * left.R0C0 + right.R2C1 * left.R1C0 + right.R2C2 * left.R2C0; + result.R2C1 = right.R2C0 * left.R0C1 + right.R2C1 * left.R1C1 + right.R2C2 * left.R2C1; + result.R2C2 = right.R2C0 * left.R0C2 + right.R2C1 * left.R1C2 + right.R2C2 * left.R2C2; + } + + + /// Multiply matrix times this matrix. + /// The matrix to multiply. + public void Multiply(double scalar) + { + R0C0 = scalar * R0C0; + R0C1 = scalar * R0C1; + R0C2 = scalar * R0C2; + R1C0 = scalar * R1C0; + R1C1 = scalar * R1C1; + R1C2 = scalar * R1C2; + R2C0 = scalar * R2C0; + R2C1 = scalar * R2C1; + R2C2 = scalar * R2C2; + } + + /// Multiply matrix times this matrix. + /// The matrix to multiply. + /// The resulting matrix of the multiplication. + public void Multiply(double scalar, out Matrix3d result) + { + result.R0C0 = scalar * R0C0; + result.R0C1 = scalar * R0C1; + result.R0C2 = scalar * R0C2; + result.R1C0 = scalar * R1C0; + result.R1C1 = scalar * R1C1; + result.R1C2 = scalar * R1C2; + result.R2C0 = scalar * R2C0; + result.R2C1 = scalar * R2C1; + result.R2C2 = scalar * R2C2; + } + + /// Multiply left matrix times left matrix. + /// The matrix on the matrix side of the equation. + /// The matrix on the right side of the equation + /// The resulting matrix of the multiplication. + public static void Multiply(ref Matrix3d matrix, double scalar, out Matrix3d result) + { + result.R0C0 = scalar * matrix.R0C0; + result.R0C1 = scalar * matrix.R0C1; + result.R0C2 = scalar * matrix.R0C2; + result.R1C0 = scalar * matrix.R1C0; + result.R1C1 = scalar * matrix.R1C1; + result.R1C2 = scalar * matrix.R1C2; + result.R2C0 = scalar * matrix.R2C0; + result.R2C1 = scalar * matrix.R2C1; + result.R2C2 = scalar * matrix.R2C2; + } + + + #endregion + + + #region Functions + + public double Determinant + { + get + { + return R0C0 * R1C1 * R2C2 - R0C0 * R1C2 * R2C1 - R0C1 * R1C0 * R2C2 + R0C2 * R1C0 * R2C1 + R0C1 * R1C2 * R2C0 - R0C2 * R1C1 * R2C0; + } + } + + public void Transpose() + { + Functions.Swap(ref R0C1, ref R1C0); + Functions.Swap(ref R0C2, ref R2C0); + Functions.Swap(ref R1C2, ref R2C1); + } + public void Transpose(out Matrix3d result) + { + result.R0C0 = R0C0; + result.R0C1 = R1C0; + result.R0C2 = R2C0; + result.R1C0 = R0C1; + result.R1C1 = R1C1; + result.R1C2 = R2C1; + result.R2C0 = R0C2; + result.R2C1 = R1C2; + result.R2C2 = R2C2; + } + public static void Transpose(ref Matrix3d matrix, out Matrix3d result) + { + result.R0C0 = matrix.R0C0; + result.R0C1 = matrix.R1C0; + result.R0C2 = matrix.R2C0; + result.R1C0 = matrix.R0C1; + result.R1C1 = matrix.R1C1; + result.R1C2 = matrix.R2C1; + result.R2C0 = matrix.R0C2; + result.R2C1 = matrix.R1C2; + result.R2C2 = matrix.R2C2; + } + + #endregion + + + #region Transformation Functions + + public void Transform(ref Vector3d vector) + { + double x = R0C0 * vector.X + R0C1 * vector.Y + R0C2 * vector.Z; + double y = R1C0 * vector.X + R1C1 * vector.Y + R1C2 * vector.Z; + vector.Z = R2C0 * vector.X + R2C1 * vector.Y + R2C2 * vector.Z; + vector.X = x; + vector.Y = y; + } + public static void Transform(ref Matrix3d matrix, ref Vector3d vector) + { + double x = matrix.R0C0 * vector.X + matrix.R0C1 * vector.Y + matrix.R0C2 * vector.Z; + double y = matrix.R1C0 * vector.X + matrix.R1C1 * vector.Y + matrix.R1C2 * vector.Z; + vector.Z = matrix.R2C0 * vector.X + matrix.R2C1 * vector.Y + matrix.R2C2 * vector.Z; + vector.X = x; + vector.Y = y; + } + public void Transform(ref Vector3d vector, out Vector3d result) + { + result.X = R0C0 * vector.X + R0C1 * vector.Y + R0C2 * vector.Z; + result.Y = R1C0 * vector.X + R1C1 * vector.Y + R1C2 * vector.Z; + result.Z = R2C0 * vector.X + R2C1 * vector.Y + R2C2 * vector.Z; + } + public static void Transform(ref Matrix3d matrix, ref Vector3d vector, out Vector3d result) + { + result.X = matrix.R0C0 * vector.X + matrix.R0C1 * vector.Y + matrix.R0C2 * vector.Z; + result.Y = matrix.R1C0 * vector.X + matrix.R1C1 * vector.Y + matrix.R1C2 * vector.Z; + result.Z = matrix.R2C0 * vector.X + matrix.R2C1 * vector.Y + matrix.R2C2 * vector.Z; + } + + public void Rotate(double angle) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + double r0c0 = cos * R0C0 + sin * R1C0; + double r0c1 = cos * R0C1 + sin * R1C1; + double r0c2 = cos * R0C2 + sin * R1C2; + + R1C0 = cos * R1C0 - sin * R0C0; + R1C1 = cos * R1C1 - sin * R0C1; + R1C2 = cos * R1C2 - sin * R0C2; + + R0C0 = r0c0; + R0C1 = r0c1; + R0C2 = r0c2; + } + public void Rotate(double angle, out Matrix3d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.R0C0 = cos * R0C0 + sin * R1C0; + result.R0C1 = cos * R0C1 + sin * R1C1; + result.R0C2 = cos * R0C2 + sin * R1C2; + result.R1C0 = cos * R1C0 - sin * R0C0; + result.R1C1 = cos * R1C1 - sin * R0C1; + result.R1C2 = cos * R1C2 - sin * R0C2; + result.R2C0 = R2C0; + result.R2C1 = R2C1; + result.R2C2 = R2C2; + } + public static void Rotate(ref Matrix3d matrix, double angle, out Matrix3d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.R0C0 = cos * matrix.R0C0 + sin * matrix.R1C0; + result.R0C1 = cos * matrix.R0C1 + sin * matrix.R1C1; + result.R0C2 = cos * matrix.R0C2 + sin * matrix.R1C2; + result.R1C0 = cos * matrix.R1C0 - sin * matrix.R0C0; + result.R1C1 = cos * matrix.R1C1 - sin * matrix.R0C1; + result.R1C2 = cos * matrix.R1C2 - sin * matrix.R0C2; + result.R2C0 = matrix.R2C0; + result.R2C1 = matrix.R2C1; + result.R2C2 = matrix.R2C2; + } + public static void RotateMatrix(double angle, out Matrix3d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.R0C0 = cos; + result.R0C1 = sin; + result.R0C2 = 0; + result.R1C0 = -sin; + result.R1C1 = cos; + result.R1C2 = 0; + result.R2C0 = 0; + result.R2C1 = 0; + result.R2C2 = 1; + } + + public void Quaternion(out Quaterniond quaternion) + { + quaternion = new Quaterniond(ref this); + } + + #endregion + + + #region Constants + + /// The identity matrix. + public static readonly Matrix3d Identity = new Matrix3d + ( + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 + ); + + /// A matrix of all zeros. + public static readonly Matrix3d Zero = new Matrix3d + ( + 0, 0, 0, + 0, 0, 0, + 0, 0, 0 + ); + + #endregion + + + #region HashCode + + /// Returns the hash code for this instance. + /// A 32-bit signed integer that is the hash code for this instance. + public override int GetHashCode() + { + return + R0C0.GetHashCode() ^ R0C1.GetHashCode() ^ R0C2.GetHashCode() ^ + R1C0.GetHashCode() ^ R1C1.GetHashCode() ^ R1C2.GetHashCode() ^ + R2C0.GetHashCode() ^ R2C1.GetHashCode() ^ R2C2.GetHashCode(); + } + + #endregion + + + #region String + + /// Returns the fully qualified type name of this instance. + /// A System.String containing left fully qualified type name. + public override string ToString() + { + return String.Format( + "|{00}, {01}, {02}|\n" + + "|{03}, {04}, {05}|\n" + + "|{06}, {07}, {18}|\n" + + R0C0, R0C1, R0C2, + R1C0, R1C1, R1C2, + R2C0, R2C1, R2C2); + } + + #endregion + } +} diff --git a/Source/OpenTK/Math/Matrix4d.cs b/Source/OpenTK/Math/Matrix4d.cs new file mode 100644 index 00000000..a59ab383 --- /dev/null +++ b/Source/OpenTK/Math/Matrix4d.cs @@ -0,0 +1,2345 @@ +using System; +using System.Runtime.InteropServices; + +namespace OpenTK.Math +{ + [Serializable] + [StructLayout(LayoutKind.Sequential)] + public struct Matrix4d : IEquatable + { + #region Fields & Access + + /// Row 0, Column 0 + public double R0C0; + + /// Row 0, Column 1 + public double R0C1; + + /// Row 0, Column 2 + public double R0C2; + + /// Row 0, Column 3 + public double R0C3; + + /// Row 1, Column 0 + public double R1C0; + + /// Row 1, Column 1 + public double R1C1; + + /// Row 1, Column 2 + public double R1C2; + + /// Row 1, Column 3 + public double R1C3; + + /// Row 2, Column 0 + public double R2C0; + + /// Row 2, Column 1 + public double R2C1; + + /// Row 2, Column 2 + public double R2C2; + + /// Row 2, Column 3 + public double R2C3; + + /// Row 3, Column 0 + public double R3C0; + + /// Row 3, Column 1 + public double R3C1; + + /// Row 3, Column 2 + public double R3C2; + + /// Row 3, Column 3 + public double R3C3; + + /// Gets the component at the given row and column in the matrix. + /// The row of the matrix. + /// The column of the matrix. + /// The component at the given row and column in the matrix. + public double this[int row, int column] + { + get + { + switch( row ) + { + case 0: + switch (column) + { + case 0: return R0C0; + case 1: return R0C1; + case 2: return R0C2; + case 3: return R0C3; + } + break; + + case 1: + switch (column) + { + case 0: return R1C0; + case 1: return R1C1; + case 2: return R1C2; + case 3: return R1C3; + } + break; + + case 2: + switch (column) + { + case 0: return R2C0; + case 1: return R2C1; + case 2: return R2C2; + case 3: return R2C3; + } + break; + + case 3: + switch (column) + { + case 0: return R3C0; + case 1: return R3C1; + case 2: return R3C2; + case 3: return R3C3; + } + break; + } + + throw new IndexOutOfRangeException(); + } + set + { + switch( row ) + { + case 0: + switch (column) + { + case 0: R0C0 = value; return; + case 1: R0C1 = value; return; + case 2: R0C2 = value; return; + case 3: R0C3 = value; return; + } + break; + + case 1: + switch (column) + { + case 0: R1C0 = value; return; + case 1: R1C1 = value; return; + case 2: R1C2 = value; return; + case 3: R1C3 = value; return; + } + break; + + case 2: + switch (column) + { + case 0: R2C0 = value; return; + case 1: R2C1 = value; return; + case 2: R2C2 = value; return; + case 3: R2C3 = value; return; + } + break; + + case 3: + switch (column) + { + case 0: R3C0 = value; return; + case 1: R3C1 = value; return; + case 2: R3C2 = value; return; + case 3: R3C3 = value; return; + } + break; + } + + throw new IndexOutOfRangeException(); + } } + + /// Gets the component at the index into the matrix. + /// The index into the components of the matrix. + /// The component at the given index into the matrix. + public double this[int index] + { + get + { + switch (index) + { + case 0: return R0C0; + case 1: return R0C1; + case 2: return R0C2; + case 3: return R0C3; + case 4: return R1C0; + case 5: return R1C1; + case 6: return R1C2; + case 7: return R1C3; + case 8: return R2C0; + case 9: return R2C1; + case 10: return R2C2; + case 11: return R2C3; + case 12: return R3C0; + case 13: return R3C1; + case 14: return R3C2; + case 15: return R3C3; + default: throw new IndexOutOfRangeException(); + } + } + set + { + switch (index) + { + case 0: R0C0 = value; return; + case 1: R0C1 = value; return; + case 2: R0C2 = value; return; + case 3: R0C3 = value; return; + case 4: R1C0 = value; return; + case 5: R1C1 = value; return; + case 6: R1C2 = value; return; + case 7: R1C3 = value; return; + case 8: R2C0 = value; return; + case 9: R2C1 = value; return; + case 10: R2C2 = value; return; + case 11: R2C3 = value; return; + case 12: R3C0 = value; return; + case 13: R3C1 = value; return; + case 14: R3C2 = value; return; + case 15: R3C3 = value; return; + default: throw new IndexOutOfRangeException(); + } + } + } + + /// Converts the matrix into an IntPtr. + /// The matrix to convert. + /// An IntPtr for the matrix. + public static explicit operator IntPtr(Matrix4d matrix) + { + unsafe + { + return (IntPtr)(&matrix.R0C0); + } + } + + /// Converts the matrix into left double*. + /// The matrix to convert. + /// A double* for the matrix. + [CLSCompliant(false)] + unsafe public static explicit operator double*(Matrix4d matrix) + { + return &matrix.R0C0; + } + + /// Converts the matrix into an array of doubles. + /// The matrix to convert. + /// An array of doubles for the matrix. + public static explicit operator double[](Matrix4d matrix) + { + return new double[16] + { + matrix.R0C0, + matrix.R0C1, + matrix.R0C2, + matrix.R0C3, + matrix.R1C0, + matrix.R1C1, + matrix.R1C2, + matrix.R1C3, + matrix.R2C0, + matrix.R2C1, + matrix.R2C2, + matrix.R2C3, + matrix.R3C0, + matrix.R3C1, + matrix.R3C2, + matrix.R3C3 + }; + } + + #endregion + + + #region Constructors + + /// Constructs left matrix with the same components as the given matrix. + /// The matrix whose components to copy. + public Matrix4d(ref Matrix4d matrix) + { + this.R0C0 = matrix.R0C0; + this.R0C1 = matrix.R0C1; + this.R0C2 = matrix.R0C2; + this.R0C3 = matrix.R0C3; + this.R1C0 = matrix.R1C0; + this.R1C1 = matrix.R1C1; + this.R1C2 = matrix.R1C2; + this.R1C3 = matrix.R1C3; + this.R2C0 = matrix.R2C0; + this.R2C1 = matrix.R2C1; + this.R2C2 = matrix.R2C2; + this.R2C3 = matrix.R2C3; + this.R3C0 = matrix.R3C0; + this.R3C1 = matrix.R3C1; + this.R3C2 = matrix.R3C2; + this.R3C3 = matrix.R3C3; + } + + /// Constructs left matrix with the given values. + /// The value for row 0 column 0. + /// The value for row 0 column 1. + /// The value for row 0 column 2. + /// The value for row 0 column 3. + /// The value for row 1 column 0. + /// The value for row 1 column 1. + /// The value for row 1 column 2. + /// The value for row 1 column 3. + /// The value for row 2 column 0. + /// The value for row 2 column 1. + /// The value for row 2 column 2. + /// The value for row 2 column 3. + /// The value for row 3 column 0. + /// The value for row 3 column 1. + /// The value for row 3 column 2. + /// The value for row 3 column 3. + public Matrix4d + ( + double r0c0, + double r0c1, + double r0c2, + double r0c3, + double r1c0, + double r1c1, + double r1c2, + double r1c3, + double r2c0, + double r2c1, + double r2c2, + double r2c3, + double r3c0, + double r3c1, + double r3c2, + double r3c3 + ) + { + this.R0C0 = r0c0; + this.R0C1 = r0c1; + this.R0C2 = r0c2; + this.R0C3 = r0c3; + this.R1C0 = r1c0; + this.R1C1 = r1c1; + this.R1C2 = r1c2; + this.R1C3 = r1c3; + this.R2C0 = r2c0; + this.R2C1 = r2c1; + this.R2C2 = r2c2; + this.R2C3 = r2c3; + this.R3C0 = r3c0; + this.R3C1 = r3c1; + this.R3C2 = r3c2; + this.R3C3 = r3c3; + } + + /// Constructs left matrix from the given array of double-precision floating point numbers. + /// The array of doubles for the components of the matrix. + public Matrix4d(double[] doubleArray) + { + if (doubleArray == null || doubleArray.GetLength(0) < 16) throw new MissingFieldException(); + + this.R0C0 = doubleArray[0]; + this.R0C1 = doubleArray[1]; + this.R0C2 = doubleArray[2]; + this.R0C3 = doubleArray[3]; + this.R1C0 = doubleArray[4]; + this.R1C1 = doubleArray[5]; + this.R1C2 = doubleArray[6]; + this.R1C3 = doubleArray[7]; + this.R2C0 = doubleArray[8]; + this.R2C1 = doubleArray[9]; + this.R2C2 = doubleArray[10]; + this.R2C3 = doubleArray[11]; + this.R3C0 = doubleArray[12]; + this.R3C1 = doubleArray[13]; + this.R3C2 = doubleArray[14]; + this.R3C3 = doubleArray[15]; + } + + /// Constructs left matrix from the given quaternion. + /// The quaternion to use to construct the martix. + public Matrix4d(ref Quaterniond quaternion) + { + Quaterniond quaternionNormalized; + quaternion.Normalize(out quaternionNormalized); + + double xx = quaternionNormalized.X * quaternionNormalized.X; + double yy = quaternionNormalized.Y * quaternionNormalized.Y; + double zz = quaternionNormalized.Z * quaternionNormalized.Z; + double xy = quaternionNormalized.X * quaternionNormalized.Y; + double xz = quaternionNormalized.X * quaternionNormalized.Z; + double yz = quaternionNormalized.Y * quaternionNormalized.Z; + double wx = quaternionNormalized.W * quaternionNormalized.X; + double wy = quaternionNormalized.W * quaternionNormalized.Y; + double wz = quaternionNormalized.W * quaternionNormalized.Z; + + R0C0 = 1 - 2 * (yy + zz); + R0C1 = 2 * (xy - wz); + R0C2 = 2 * (xz + wy); + R0C3 = 0; + + R1C0 = 2 * (xy + wz); + R1C1 = 1 - 2 * (xx + zz); + R1C2 = 2 * (yz - wx); + R1C3 = 0; + + R2C0 = 2 * (xz - wy); + R2C1 = 2 * (yz + wx); + R2C2 = 1 - 2 * (xx + yy); + R2C3 = 0; + + R3C0 = 0; + R3C1 = 0; + R3C2 = 0; + R3C3 = 1; + } + + #endregion + + + #region Equality + + /// 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(Matrix4d matrix) + { + return + R0C0 == matrix.R0C0 && + R0C1 == matrix.R0C1 && + R0C2 == matrix.R0C2 && + R0C3 == matrix.R0C3 && + R1C0 == matrix.R1C0 && + R1C1 == matrix.R1C1 && + R1C2 == matrix.R1C2 && + R1C3 == matrix.R1C3 && + R2C0 == matrix.R2C0 && + R2C1 == matrix.R2C1 && + R2C2 == matrix.R2C2 && + R2C3 == matrix.R2C3 && + R3C0 == matrix.R3C0 && + R3C1 == matrix.R3C1 && + R3C2 == matrix.R3C2 && + R3C3 == matrix.R3C3; + + } + + public bool Equals(ref Matrix4d matrix) + { + return + R0C0 == matrix.R0C0 && + R0C1 == matrix.R0C1 && + R0C2 == matrix.R0C2 && + R0C3 == matrix.R0C3 && + R1C0 == matrix.R1C0 && + R1C1 == matrix.R1C1 && + R1C2 == matrix.R1C2 && + R1C3 == matrix.R1C3 && + R2C0 == matrix.R2C0 && + R2C1 == matrix.R2C1 && + R2C2 == matrix.R2C2 && + R2C3 == matrix.R2C3 && + R3C0 == matrix.R3C0 && + R3C1 == matrix.R3C1 && + R3C2 == matrix.R3C2 && + R3C3 == matrix.R3C3; + + } + + public static bool Equals(ref Matrix4d left, ref Matrix4d right) + { + return + left.R0C0 == right.R0C0 && + left.R0C1 == right.R0C1 && + left.R0C2 == right.R0C2 && + left.R0C3 == right.R0C3 && + left.R1C0 == right.R1C0 && + left.R1C1 == right.R1C1 && + left.R1C2 == right.R1C2 && + left.R1C3 == right.R1C3 && + left.R2C0 == right.R2C0 && + left.R2C1 == right.R2C1 && + left.R2C2 == right.R2C2 && + left.R2C3 == right.R2C3 && + left.R3C0 == right.R3C0 && + left.R3C1 == right.R3C1 && + left.R3C2 == right.R3C2 && + left.R3C3 == right.R3C3; + } + + public bool EqualsApprox(ref Matrix4d matrix, double tolerance) + { + return + System.Math.Abs(R0C0 - matrix.R0C0) <= tolerance && + System.Math.Abs(R0C1 - matrix.R0C1) <= tolerance && + System.Math.Abs(R0C2 - matrix.R0C2) <= tolerance && + System.Math.Abs(R0C3 - matrix.R0C3) <= tolerance && + System.Math.Abs(R1C0 - matrix.R1C0) <= tolerance && + System.Math.Abs(R1C1 - matrix.R1C1) <= tolerance && + System.Math.Abs(R1C2 - matrix.R1C2) <= tolerance && + System.Math.Abs(R1C3 - matrix.R1C3) <= tolerance && + System.Math.Abs(R2C0 - matrix.R2C0) <= tolerance && + System.Math.Abs(R2C1 - matrix.R2C1) <= tolerance && + System.Math.Abs(R2C2 - matrix.R2C2) <= tolerance && + System.Math.Abs(R2C3 - matrix.R2C3) <= tolerance && + System.Math.Abs(R3C0 - matrix.R3C0) <= tolerance && + System.Math.Abs(R3C1 - matrix.R3C1) <= tolerance && + System.Math.Abs(R3C2 - matrix.R3C2) <= tolerance && + System.Math.Abs(R3C3 - matrix.R3C3) <= tolerance; + } + + public static bool EqualsApprox(ref Matrix4d left, ref Matrix4d right, double tolerance) + { + return + System.Math.Abs(left.R0C0 - right.R0C0) <= tolerance && + System.Math.Abs(left.R0C1 - right.R0C1) <= tolerance && + System.Math.Abs(left.R0C2 - right.R0C2) <= tolerance && + System.Math.Abs(left.R0C3 - right.R0C3) <= tolerance && + System.Math.Abs(left.R1C0 - right.R1C0) <= tolerance && + System.Math.Abs(left.R1C1 - right.R1C1) <= tolerance && + System.Math.Abs(left.R1C2 - right.R1C2) <= tolerance && + System.Math.Abs(left.R1C3 - right.R1C3) <= tolerance && + System.Math.Abs(left.R2C0 - right.R2C0) <= tolerance && + System.Math.Abs(left.R2C1 - right.R2C1) <= tolerance && + System.Math.Abs(left.R2C2 - right.R2C2) <= tolerance && + System.Math.Abs(left.R2C3 - right.R2C3) <= tolerance && + System.Math.Abs(left.R3C0 - right.R3C0) <= tolerance && + System.Math.Abs(left.R3C1 - right.R3C1) <= tolerance && + System.Math.Abs(left.R3C2 - right.R3C2) <= tolerance && + System.Math.Abs(left.R3C3 - right.R3C3) <= tolerance; + } + + #endregion + + + #region Arithmetic Operators + + + /// Add left matrix to this matrix. + /// The matrix to add. + public void Add(ref Matrix4d matrix) + { + R0C0 = R0C0 + matrix.R0C0; + R0C1 = R0C1 + matrix.R0C1; + R0C2 = R0C2 + matrix.R0C2; + R0C3 = R0C3 + matrix.R0C3; + R1C0 = R1C0 + matrix.R1C0; + R1C1 = R1C1 + matrix.R1C1; + R1C2 = R1C2 + matrix.R1C2; + R1C3 = R1C3 + matrix.R1C3; + R2C0 = R2C0 + matrix.R2C0; + R2C1 = R2C1 + matrix.R2C1; + R2C2 = R2C2 + matrix.R2C2; + R2C3 = R2C3 + matrix.R2C3; + R3C0 = R3C0 + matrix.R3C0; + R3C1 = R3C1 + matrix.R3C1; + R3C2 = R3C2 + matrix.R3C2; + R3C3 = R3C3 + matrix.R3C3; + } + + /// Add left matrix to this matrix. + /// The matrix to add. + /// The resulting matrix of the addition. + public void Add(ref Matrix4d matrix, out Matrix4d result) + { + result.R0C0 = R0C0 + matrix.R0C0; + result.R0C1 = R0C1 + matrix.R0C1; + result.R0C2 = R0C2 + matrix.R0C2; + result.R0C3 = R0C3 + matrix.R0C3; + result.R1C0 = R1C0 + matrix.R1C0; + result.R1C1 = R1C1 + matrix.R1C1; + result.R1C2 = R1C2 + matrix.R1C2; + result.R1C3 = R1C3 + matrix.R1C3; + result.R2C0 = R2C0 + matrix.R2C0; + result.R2C1 = R2C1 + matrix.R2C1; + result.R2C2 = R2C2 + matrix.R2C2; + result.R2C3 = R2C3 + matrix.R2C3; + result.R3C0 = R3C0 + matrix.R3C0; + result.R3C1 = R3C1 + matrix.R3C1; + result.R3C2 = R3C2 + matrix.R3C2; + result.R3C3 = R3C3 + matrix.R3C3; + } + + /// Add left matrix to left matrix. + /// The matrix on the matrix side of the equation. + /// The matrix on the right side of the equation + /// The resulting matrix of the addition. + public static void Add(ref Matrix4d left, ref Matrix4d right, out Matrix4d result) + { + result.R0C0 = left.R0C0 + right.R0C0; + result.R0C1 = left.R0C1 + right.R0C1; + result.R0C2 = left.R0C2 + right.R0C2; + result.R0C3 = left.R0C3 + right.R0C3; + result.R1C0 = left.R1C0 + right.R1C0; + result.R1C1 = left.R1C1 + right.R1C1; + result.R1C2 = left.R1C2 + right.R1C2; + result.R1C3 = left.R1C3 + right.R1C3; + result.R2C0 = left.R2C0 + right.R2C0; + result.R2C1 = left.R2C1 + right.R2C1; + result.R2C2 = left.R2C2 + right.R2C2; + result.R2C3 = left.R2C3 + right.R2C3; + result.R3C0 = left.R3C0 + right.R3C0; + result.R3C1 = left.R3C1 + right.R3C1; + result.R3C2 = left.R3C2 + right.R3C2; + result.R3C3 = left.R3C3 + right.R3C3; + } + + + /// Subtract left matrix from this matrix. + /// The matrix to subtract. + public void Subtract(ref Matrix4d matrix) + { + R0C0 = R0C0 + matrix.R0C0; + R0C1 = R0C1 + matrix.R0C1; + R0C2 = R0C2 + matrix.R0C2; + R0C3 = R0C3 + matrix.R0C3; + R1C0 = R1C0 + matrix.R1C0; + R1C1 = R1C1 + matrix.R1C1; + R1C2 = R1C2 + matrix.R1C2; + R1C3 = R1C3 + matrix.R1C3; + R2C0 = R2C0 + matrix.R2C0; + R2C1 = R2C1 + matrix.R2C1; + R2C2 = R2C2 + matrix.R2C2; + R2C3 = R2C3 + matrix.R2C3; + R3C0 = R3C0 + matrix.R3C0; + R3C1 = R3C1 + matrix.R3C1; + R3C2 = R3C2 + matrix.R3C2; + R3C3 = R3C3 + matrix.R3C3; + } + + /// Subtract left matrix from this matrix. + /// The matrix to subtract. + /// The resulting matrix of the subtraction. + public void Subtract(ref Matrix4d matrix, out Matrix4d result) + { + result.R0C0 = R0C0 + matrix.R0C0; + result.R0C1 = R0C1 + matrix.R0C1; + result.R0C2 = R0C2 + matrix.R0C2; + result.R0C3 = R0C3 + matrix.R0C3; + result.R1C0 = R1C0 + matrix.R1C0; + result.R1C1 = R1C1 + matrix.R1C1; + result.R1C2 = R1C2 + matrix.R1C2; + result.R1C3 = R1C3 + matrix.R1C3; + result.R2C0 = R2C0 + matrix.R2C0; + result.R2C1 = R2C1 + matrix.R2C1; + result.R2C2 = R2C2 + matrix.R2C2; + result.R2C3 = R2C3 + matrix.R2C3; + result.R3C0 = R3C0 + matrix.R3C0; + result.R3C1 = R3C1 + matrix.R3C1; + result.R3C2 = R3C2 + matrix.R3C2; + result.R3C3 = R3C3 + matrix.R3C3; + } + + /// Subtract left matrix from left matrix. + /// The matrix on the matrix side of the equation. + /// The matrix on the right side of the equation + /// The resulting matrix of the subtraction. + public static void Subtract(ref Matrix4d left, ref Matrix4d right, out Matrix4d result) + { + result.R0C0 = left.R0C0 + right.R0C0; + result.R0C1 = left.R0C1 + right.R0C1; + result.R0C2 = left.R0C2 + right.R0C2; + result.R0C3 = left.R0C3 + right.R0C3; + result.R1C0 = left.R1C0 + right.R1C0; + result.R1C1 = left.R1C1 + right.R1C1; + result.R1C2 = left.R1C2 + right.R1C2; + result.R1C3 = left.R1C3 + right.R1C3; + result.R2C0 = left.R2C0 + right.R2C0; + result.R2C1 = left.R2C1 + right.R2C1; + result.R2C2 = left.R2C2 + right.R2C2; + result.R2C3 = left.R2C3 + right.R2C3; + result.R3C0 = left.R3C0 + right.R3C0; + result.R3C1 = left.R3C1 + right.R3C1; + result.R3C2 = left.R3C2 + right.R3C2; + result.R3C3 = left.R3C3 + right.R3C3; + } + + + /// Multiply left martix times this matrix. + /// The matrix to multiply. + public void Multiply(ref Matrix4d matrix) + { + double r0c0 = matrix.R0C0 * R0C0 + matrix.R0C1 * R1C0 + matrix.R0C2 * R2C0 + matrix.R0C3 * R3C0; + double r0c1 = matrix.R0C0 * R0C1 + matrix.R0C1 * R1C1 + matrix.R0C2 * R2C1 + matrix.R0C3 * R3C1; + double r0c2 = matrix.R0C0 * R0C2 + matrix.R0C1 * R1C2 + matrix.R0C2 * R2C2 + matrix.R0C3 * R3C2; + double r0c3 = matrix.R0C0 * R0C3 + matrix.R0C1 * R1C3 + matrix.R0C2 * R2C3 + matrix.R0C3 * R3C3; + double r1c0 = matrix.R1C0 * R0C0 + matrix.R1C1 * R1C0 + matrix.R1C2 * R2C0 + matrix.R1C3 * R3C0; + double r1c1 = matrix.R1C0 * R0C1 + matrix.R1C1 * R1C1 + matrix.R1C2 * R2C1 + matrix.R1C3 * R3C1; + double r1c2 = matrix.R1C0 * R0C2 + matrix.R1C1 * R1C2 + matrix.R1C2 * R2C2 + matrix.R1C3 * R3C2; + double r1c3 = matrix.R1C0 * R0C3 + matrix.R1C1 * R1C3 + matrix.R1C2 * R2C3 + matrix.R1C3 * R3C3; + double r2c0 = matrix.R2C0 * R0C0 + matrix.R2C1 * R1C0 + matrix.R2C2 * R2C0 + matrix.R2C3 * R3C0; + double r2c1 = matrix.R2C0 * R0C1 + matrix.R2C1 * R1C1 + matrix.R2C2 * R2C1 + matrix.R2C3 * R3C1; + double r2c2 = matrix.R2C0 * R0C2 + matrix.R2C1 * R1C2 + matrix.R2C2 * R2C2 + matrix.R2C3 * R3C2; + double r2c3 = matrix.R2C0 * R0C3 + matrix.R2C1 * R1C3 + matrix.R2C2 * R2C3 + matrix.R2C3 * R3C3; + + R3C0 = matrix.R3C0 * R0C0 + matrix.R3C1 * R1C0 + matrix.R3C2 * R2C0 + matrix.R3C3 * R3C0; + R3C1 = matrix.R3C0 * R0C1 + matrix.R3C1 * R1C1 + matrix.R3C2 * R2C1 + matrix.R3C3 * R3C1; + R3C2 = matrix.R3C0 * R0C2 + matrix.R3C1 * R1C2 + matrix.R3C2 * R2C2 + matrix.R3C3 * R3C2; + R3C3 = matrix.R3C0 * R0C3 + matrix.R3C1 * R1C3 + matrix.R3C2 * R2C3 + matrix.R3C3 * R3C3; + + R0C0 = r0c0; + R0C1 = r0c1; + R0C2 = r0c2; + R0C3 = r0c3; + R1C0 = r1c0; + R1C1 = r1c1; + R1C2 = r1c2; + R1C3 = r1c3; + R2C0 = r2c0; + R2C1 = r2c1; + R2C2 = r2c2; + R2C3 = r2c3; + } + + /// Multiply matrix times this matrix. + /// The matrix to multiply. + /// The resulting matrix of the multiplication. + public void Multiply(ref Matrix4d matrix, out Matrix4d result) + { + result.R0C0 = matrix.R0C0 * R0C0 + matrix.R0C1 * R1C0 + matrix.R0C2 * R2C0 + matrix.R0C3 * R3C0; + result.R0C1 = matrix.R0C0 * R0C1 + matrix.R0C1 * R1C1 + matrix.R0C2 * R2C1 + matrix.R0C3 * R3C1; + result.R0C2 = matrix.R0C0 * R0C2 + matrix.R0C1 * R1C2 + matrix.R0C2 * R2C2 + matrix.R0C3 * R3C2; + result.R0C3 = matrix.R0C0 * R0C3 + matrix.R0C1 * R1C3 + matrix.R0C2 * R2C3 + matrix.R0C3 * R3C3; + result.R1C0 = matrix.R1C0 * R0C0 + matrix.R1C1 * R1C0 + matrix.R1C2 * R2C0 + matrix.R1C3 * R3C0; + result.R1C1 = matrix.R1C0 * R0C1 + matrix.R1C1 * R1C1 + matrix.R1C2 * R2C1 + matrix.R1C3 * R3C1; + result.R1C2 = matrix.R1C0 * R0C2 + matrix.R1C1 * R1C2 + matrix.R1C2 * R2C2 + matrix.R1C3 * R3C2; + result.R1C3 = matrix.R1C0 * R0C3 + matrix.R1C1 * R1C3 + matrix.R1C2 * R2C3 + matrix.R1C3 * R3C3; + result.R2C0 = matrix.R2C0 * R0C0 + matrix.R2C1 * R1C0 + matrix.R2C2 * R2C0 + matrix.R2C3 * R3C0; + result.R2C1 = matrix.R2C0 * R0C1 + matrix.R2C1 * R1C1 + matrix.R2C2 * R2C1 + matrix.R2C3 * R3C1; + result.R2C2 = matrix.R2C0 * R0C2 + matrix.R2C1 * R1C2 + matrix.R2C2 * R2C2 + matrix.R2C3 * R3C2; + result.R2C3 = matrix.R2C0 * R0C3 + matrix.R2C1 * R1C3 + matrix.R2C2 * R2C3 + matrix.R2C3 * R3C3; + result.R3C0 = matrix.R3C0 * R0C0 + matrix.R3C1 * R1C0 + matrix.R3C2 * R2C0 + matrix.R3C3 * R3C0; + result.R3C1 = matrix.R3C0 * R0C1 + matrix.R3C1 * R1C1 + matrix.R3C2 * R2C1 + matrix.R3C3 * R3C1; + result.R3C2 = matrix.R3C0 * R0C2 + matrix.R3C1 * R1C2 + matrix.R3C2 * R2C2 + matrix.R3C3 * R3C2; + result.R3C3 = matrix.R3C0 * R0C3 + matrix.R3C1 * R1C3 + matrix.R3C2 * R2C3 + matrix.R3C3 * R3C3; + + } + + /// Multiply left matrix times left matrix. + /// The matrix on the matrix side of the equation. + /// The matrix on the right side of the equation + /// The resulting matrix of the multiplication. + public static void Multiply(ref Matrix4d left, ref Matrix4d right, out Matrix4d result) + { + result.R0C0 = right.R0C0 * left.R0C0 + right.R0C1 * left.R1C0 + right.R0C2 * left.R2C0 + right.R0C3 * left.R3C0; + result.R0C1 = right.R0C0 * left.R0C1 + right.R0C1 * left.R1C1 + right.R0C2 * left.R2C1 + right.R0C3 * left.R3C1; + result.R0C2 = right.R0C0 * left.R0C2 + right.R0C1 * left.R1C2 + right.R0C2 * left.R2C2 + right.R0C3 * left.R3C2; + result.R0C3 = right.R0C0 * left.R0C3 + right.R0C1 * left.R1C3 + right.R0C2 * left.R2C3 + right.R0C3 * left.R3C3; + result.R1C0 = right.R1C0 * left.R0C0 + right.R1C1 * left.R1C0 + right.R1C2 * left.R2C0 + right.R1C3 * left.R3C0; + result.R1C1 = right.R1C0 * left.R0C1 + right.R1C1 * left.R1C1 + right.R1C2 * left.R2C1 + right.R1C3 * left.R3C1; + result.R1C2 = right.R1C0 * left.R0C2 + right.R1C1 * left.R1C2 + right.R1C2 * left.R2C2 + right.R1C3 * left.R3C2; + result.R1C3 = right.R1C0 * left.R0C3 + right.R1C1 * left.R1C3 + right.R1C2 * left.R2C3 + right.R1C3 * left.R3C3; + result.R2C0 = right.R2C0 * left.R0C0 + right.R2C1 * left.R1C0 + right.R2C2 * left.R2C0 + right.R2C3 * left.R3C0; + result.R2C1 = right.R2C0 * left.R0C1 + right.R2C1 * left.R1C1 + right.R2C2 * left.R2C1 + right.R2C3 * left.R3C1; + result.R2C2 = right.R2C0 * left.R0C2 + right.R2C1 * left.R1C2 + right.R2C2 * left.R2C2 + right.R2C3 * left.R3C2; + result.R2C3 = right.R2C0 * left.R0C3 + right.R2C1 * left.R1C3 + right.R2C2 * left.R2C3 + right.R2C3 * left.R3C3; + result.R3C0 = right.R3C0 * left.R0C0 + right.R3C1 * left.R1C0 + right.R3C2 * left.R2C0 + right.R3C3 * left.R3C0; + result.R3C1 = right.R3C0 * left.R0C1 + right.R3C1 * left.R1C1 + right.R3C2 * left.R2C1 + right.R3C3 * left.R3C1; + result.R3C2 = right.R3C0 * left.R0C2 + right.R3C1 * left.R1C2 + right.R3C2 * left.R2C2 + right.R3C3 * left.R3C2; + result.R3C3 = right.R3C0 * left.R0C3 + right.R3C1 * left.R1C3 + right.R3C2 * left.R2C3 + right.R3C3 * left.R3C3; + } + + + /// Multiply matrix times this matrix. + /// The matrix to multiply. + public void Multiply(double scalar) + { + R0C0 = scalar * R0C0; + R0C1 = scalar * R0C1; + R0C2 = scalar * R0C2; + R0C3 = scalar * R0C3; + R1C0 = scalar * R1C0; + R1C1 = scalar * R1C1; + R1C2 = scalar * R1C2; + R1C3 = scalar * R1C3; + R2C0 = scalar * R2C0; + R2C1 = scalar * R2C1; + R2C2 = scalar * R2C2; + R2C3 = scalar * R2C3; + R3C0 = scalar * R3C0; + R3C1 = scalar * R3C1; + R3C2 = scalar * R3C2; + R3C3 = scalar * R3C3; + } + + /// Multiply matrix times this matrix. + /// The matrix to multiply. + /// The resulting matrix of the multiplication. + public void Multiply(double scalar, out Matrix4d result) + { + result.R0C0 = scalar * R0C0; + result.R0C1 = scalar * R0C1; + result.R0C2 = scalar * R0C2; + result.R0C3 = scalar * R0C3; + result.R1C0 = scalar * R1C0; + result.R1C1 = scalar * R1C1; + result.R1C2 = scalar * R1C2; + result.R1C3 = scalar * R1C3; + result.R2C0 = scalar * R2C0; + result.R2C1 = scalar * R2C1; + result.R2C2 = scalar * R2C2; + result.R2C3 = scalar * R2C3; + result.R3C0 = scalar * R3C0; + result.R3C1 = scalar * R3C1; + result.R3C2 = scalar * R3C2; + result.R3C3 = scalar * R3C3; + } + + /// Multiply left matrix times left matrix. + /// The matrix on the matrix side of the equation. + /// The matrix on the right side of the equation + /// The resulting matrix of the multiplication. + public static void Multiply(ref Matrix4d matrix, double scalar, out Matrix4d result) + { + result.R0C0 = scalar * matrix.R0C0; + result.R0C1 = scalar * matrix.R0C1; + result.R0C2 = scalar * matrix.R0C2; + result.R0C3 = scalar * matrix.R0C3; + result.R1C0 = scalar * matrix.R1C0; + result.R1C1 = scalar * matrix.R1C1; + result.R1C2 = scalar * matrix.R1C2; + result.R1C3 = scalar * matrix.R1C3; + result.R2C0 = scalar * matrix.R2C0; + result.R2C1 = scalar * matrix.R2C1; + result.R2C2 = scalar * matrix.R2C2; + result.R2C3 = scalar * matrix.R2C3; + result.R3C0 = scalar * matrix.R3C0; + result.R3C1 = scalar * matrix.R3C1; + result.R3C2 = scalar * matrix.R3C2; + result.R3C3 = scalar * matrix.R3C3; + } + + + #endregion + + + #region Functions + + public double Cofacter(int row, int column) + { + switch (row) + { + case 0: + switch (column) + { + case 0: return +(R1C1 * R2C2 * R3C3 - R1C1 * R2C3 * R3C2 - R1C2 * R2C1 * R3C3 + R1C3 * R2C1 * R3C2 + R1C2 * R2C3 * R3C1 - R1C3 * R2C2 * R3C1); + case 1: return -(R1C0 * R2C2 * R3C3 - R1C0 * R2C3 * R3C2 - R1C2 * R2C0 * R3C3 + R1C3 * R2C0 * R3C2 + R1C2 * R2C3 * R3C0 - R1C3 * R2C2 * R3C0); + case 2: return +(R1C0 * R2C1 * R3C3 - R1C0 * R2C3 * R3C1 - R1C1 * R2C0 * R3C3 + R1C3 * R2C0 * R3C1 + R1C1 * R2C3 * R3C0 - R1C3 * R2C1 * R3C0); + case 3: return -(R1C0 * R2C1 * R3C2 - R1C0 * R2C2 * R3C1 - R1C1 * R2C0 * R3C2 + R1C2 * R2C0 * R3C1 + R1C1 * R2C2 * R3C0 - R1C2 * R2C1 * R3C0); + } + break; + + case 1: + switch (column) + { + case 0: return -(R0C1 * R2C2 * R3C3 - R0C1 * R2C3 * R3C2 - R0C2 * R2C1 * R3C3 + R0C3 * R2C1 * R3C2 + R0C2 * R2C3 * R3C1 - R0C3 * R2C2 * R3C1); + case 1: return +(R0C0 * R2C2 * R3C3 - R0C0 * R2C3 * R3C2 - R0C2 * R2C0 * R3C3 + R0C3 * R2C0 * R3C2 + R0C2 * R2C3 * R3C0 - R0C3 * R2C2 * R3C0); + case 2: return -(R0C0 * R2C1 * R3C3 - R0C0 * R2C3 * R3C1 - R0C1 * R2C0 * R3C3 + R0C3 * R2C0 * R3C1 + R0C1 * R2C3 * R3C0 - R0C3 * R2C1 * R3C0); + case 3: return +(R0C0 * R2C1 * R3C2 - R0C0 * R2C2 * R3C1 - R0C1 * R2C0 * R3C2 + R0C2 * R2C0 * R3C1 + R0C1 * R2C2 * R3C0 - R0C2 * R2C1 * R3C0); + } + break; + + case 2: + switch (column) + { + case 0: return +(R0C1 * R1C2 * R3C3 - R0C1 * R1C3 * R3C2 - R0C2 * R1C1 * R3C3 + R0C3 * R1C1 * R3C2 + R0C2 * R1C3 * R3C1 - R0C3 * R1C2 * R3C1); + case 1: return -(R0C0 * R1C2 * R3C3 - R0C0 * R1C3 * R3C2 - R0C2 * R1C0 * R3C3 + R0C3 * R1C0 * R3C2 + R0C2 * R1C3 * R3C0 - R0C3 * R1C2 * R3C0); + case 2: return +(R0C0 * R1C1 * R3C3 - R0C0 * R1C3 * R3C1 - R0C1 * R1C0 * R3C3 + R0C3 * R1C0 * R3C1 + R0C1 * R1C3 * R3C0 - R0C3 * R1C1 * R3C0); + case 3: return -(R0C0 * R1C1 * R3C2 - R0C0 * R1C2 * R3C1 - R0C1 * R1C0 * R3C2 + R0C2 * R1C0 * R3C1 + R0C1 * R1C2 * R3C0 - R0C2 * R1C1 * R3C0); + } + break; + + case 3: + switch (column) + { + case 0: return -(R0C1 * R1C2 * R2C3 - R0C1 * R1C3 * R2C2 - R0C2 * R1C1 * R2C3 + R0C3 * R1C1 * R2C2 + R0C2 * R1C3 * R2C1 - R0C3 * R1C2 * R2C1); + case 1: return +(R0C0 * R1C2 * R2C3 - R0C0 * R1C3 * R2C2 - R0C2 * R1C0 * R2C3 + R0C3 * R1C0 * R2C2 + R0C2 * R1C3 * R2C0 - R0C3 * R1C2 * R2C0); + case 2: return -(R0C0 * R1C1 * R2C3 - R0C0 * R1C3 * R2C1 - R0C1 * R1C0 * R2C3 + R0C3 * R1C0 * R2C1 + R0C1 * R1C3 * R2C0 - R0C3 * R1C1 * R2C0); + case 3: return +(R0C0 * R1C1 * R2C2 - R0C0 * R1C2 * R2C1 - R0C1 * R1C0 * R2C2 + R0C2 * R1C0 * R2C1 + R0C1 * R1C2 * R2C0 - R0C2 * R1C1 * R2C0); + } + break; + } + + throw new IndexOutOfRangeException(); + } + + public double Determinant + { + get + { + return + R2C3 * R3C2 * R0C1 * R1C0 - R2C2 * R3C3 * R0C1 * R1C0 - R2C3 * R3C1 * R0C2 * R1C0 + R2C1 * R3C3 * R0C2 * R1C0 + + R2C2 * R3C1 * R0C3 * R1C0 - R2C1 * R3C2 * R0C3 * R1C0 - R0C0 * R2C3 * R3C2 * R1C1 + R0C0 * R2C2 * R3C3 * R1C1 + + R2C3 * R3C0 * R0C2 * R1C1 - R2C2 * R3C0 * R0C3 * R1C1 + R0C0 * R2C3 * R3C1 * R1C2 - R0C0 * R2C1 * R3C3 * R1C2 - + R2C3 * R3C0 * R0C1 * R1C2 + R2C1 * R3C0 * R0C3 * R1C2 - R0C0 * R2C2 * R3C1 * R1C3 + R0C0 * R2C1 * R3C2 * R1C3 + + R2C2 * R3C0 * R0C1 * R1C3 - R2C1 * R3C0 * R0C2 * R1C3 - R3C3 * R0C2 * R1C1 * R2C0 + R3C2 * R0C3 * R1C1 * R2C0 + + R3C3 * R0C1 * R1C2 * R2C0 - R3C1 * R0C3 * R1C2 * R2C0 - R3C2 * R0C1 * R1C3 * R2C0 + R3C1 * R0C2 * R1C3 * R2C0; + } + } + + public void Minor(int row, int column, out Matrix3d result) + { + switch (row) + { + case 0: + switch (column) + { + case 0: + result.R0C0 = R1C1; + result.R0C1 = R1C2; + result.R0C2 = R1C3; + result.R1C0 = R2C1; + result.R1C1 = R2C2; + result.R1C2 = R2C3; + result.R2C0 = R3C1; + result.R2C1 = R3C2; + result.R2C2 = R3C3; + return; + + case 1: + result.R0C0 = R1C0; + result.R0C1 = R1C2; + result.R0C2 = R1C3; + result.R1C0 = R2C0; + result.R1C1 = R2C2; + result.R1C2 = R2C3; + result.R2C0 = R3C0; + result.R2C1 = R3C2; + result.R2C2 = R3C3; + return; + + case 2: + result.R0C0 = R1C0; + result.R0C1 = R1C1; + result.R0C2 = R1C3; + result.R1C0 = R2C0; + result.R1C1 = R2C1; + result.R1C2 = R2C3; + result.R2C0 = R3C0; + result.R2C1 = R3C1; + result.R2C2 = R3C3; + return; + + case 3: + result.R0C0 = R1C0; + result.R0C1 = R1C1; + result.R0C2 = R1C2; + result.R1C0 = R2C0; + result.R1C1 = R2C1; + result.R1C2 = R2C2; + result.R2C0 = R3C0; + result.R2C1 = R3C1; + result.R2C2 = R3C2; + return; + } + break; + + case 1: + switch (column) + { + case 0: + result.R0C0 = R0C1; + result.R0C1 = R0C2; + result.R0C2 = R0C3; + result.R1C0 = R2C1; + result.R1C1 = R2C2; + result.R1C2 = R2C3; + result.R2C0 = R3C1; + result.R2C1 = R3C2; + result.R2C2 = R3C3; + return; + + case 1: + result.R0C0 = R0C0; + result.R0C1 = R0C2; + result.R0C2 = R0C3; + result.R1C0 = R2C0; + result.R1C1 = R2C2; + result.R1C2 = R2C3; + result.R2C0 = R3C0; + result.R2C1 = R3C2; + result.R2C2 = R3C3; + return; + + case 2: + result.R0C0 = R0C0; + result.R0C1 = R0C1; + result.R0C2 = R0C3; + result.R1C0 = R2C0; + result.R1C1 = R2C1; + result.R1C2 = R2C3; + result.R2C0 = R3C0; + result.R2C1 = R3C1; + result.R2C2 = R3C3; + return; + } + break; + + case 2: + switch (column) + { + case 0: + result.R0C0 = R0C1; + result.R0C1 = R0C2; + result.R0C2 = R0C3; + result.R1C0 = R1C1; + result.R1C1 = R1C2; + result.R1C2 = R1C3; + result.R2C0 = R3C1; + result.R2C1 = R3C2; + result.R2C2 = R3C3; + return; + + case 1: + result.R0C0 = R0C0; + result.R0C1 = R0C2; + result.R0C2 = R0C3; + result.R1C0 = R1C0; + result.R1C1 = R1C2; + result.R1C2 = R1C3; + result.R2C0 = R3C0; + result.R2C1 = R3C2; + result.R2C2 = R3C3; + return; + + case 2: + result.R0C0 = R0C0; + result.R0C1 = R0C1; + result.R0C2 = R0C3; + result.R1C0 = R1C0; + result.R1C1 = R1C1; + result.R1C2 = R1C3; + result.R2C0 = R3C0; + result.R2C1 = R3C1; + result.R2C2 = R3C3; + return; + } + break; + + } + + throw new IndexOutOfRangeException(); + } + public static void Minor(ref Matrix4d matrix, int row, int column, out Matrix3d result) + { + switch (row) + { + case 0: + switch (column) + { + case 0: + result.R0C0 = matrix.R1C1; + result.R0C1 = matrix.R1C2; + result.R0C2 = matrix.R1C3; + result.R1C0 = matrix.R2C1; + result.R1C1 = matrix.R2C2; + result.R1C2 = matrix.R2C3; + result.R2C0 = matrix.R3C1; + result.R2C1 = matrix.R3C2; + result.R2C2 = matrix.R3C3; + return; + + case 1: + result.R0C0 = matrix.R1C0; + result.R0C1 = matrix.R1C2; + result.R0C2 = matrix.R1C3; + result.R1C0 = matrix.R2C0; + result.R1C1 = matrix.R2C2; + result.R1C2 = matrix.R2C3; + result.R2C0 = matrix.R3C0; + result.R2C1 = matrix.R3C2; + result.R2C2 = matrix.R3C3; + return; + + case 2: + result.R0C0 = matrix.R1C0; + result.R0C1 = matrix.R1C1; + result.R0C2 = matrix.R1C3; + result.R1C0 = matrix.R2C0; + result.R1C1 = matrix.R2C1; + result.R1C2 = matrix.R2C3; + result.R2C0 = matrix.R3C0; + result.R2C1 = matrix.R3C1; + result.R2C2 = matrix.R3C3; + return; + } + break; + + case 1: + switch (column) + { + case 0: + result.R0C0 = matrix.R0C1; + result.R0C1 = matrix.R0C2; + result.R0C2 = matrix.R0C3; + result.R1C0 = matrix.R2C1; + result.R1C1 = matrix.R2C2; + result.R1C2 = matrix.R2C3; + result.R2C0 = matrix.R3C1; + result.R2C1 = matrix.R3C2; + result.R2C2 = matrix.R3C3; + return; + + case 1: + result.R0C0 = matrix.R0C0; + result.R0C1 = matrix.R0C2; + result.R0C2 = matrix.R0C3; + result.R1C0 = matrix.R2C0; + result.R1C1 = matrix.R2C2; + result.R1C2 = matrix.R2C3; + result.R2C0 = matrix.R3C0; + result.R2C1 = matrix.R3C2; + result.R2C2 = matrix.R3C3; + return; + + case 2: + result.R0C0 = matrix.R0C0; + result.R0C1 = matrix.R0C1; + result.R0C2 = matrix.R0C3; + result.R1C0 = matrix.R2C0; + result.R1C1 = matrix.R2C1; + result.R1C2 = matrix.R2C3; + result.R2C0 = matrix.R3C0; + result.R2C1 = matrix.R3C1; + result.R2C2 = matrix.R3C3; + return; + } + break; + + case 2: + switch (column) + { + case 0: + result.R0C0 = matrix.R0C1; + result.R0C1 = matrix.R0C2; + result.R0C2 = matrix.R0C3; + result.R1C0 = matrix.R1C1; + result.R1C1 = matrix.R1C2; + result.R1C2 = matrix.R1C3; + result.R2C0 = matrix.R3C1; + result.R2C1 = matrix.R3C2; + result.R2C2 = matrix.R3C3; + return; + + case 1: + result.R0C0 = matrix.R0C0; + result.R0C1 = matrix.R0C2; + result.R0C2 = matrix.R0C3; + result.R1C0 = matrix.R1C0; + result.R1C1 = matrix.R1C2; + result.R1C2 = matrix.R1C3; + result.R2C0 = matrix.R3C0; + result.R2C1 = matrix.R3C2; + result.R2C2 = matrix.R3C3; + return; + + case 2: + result.R0C0 = matrix.R0C0; + result.R0C1 = matrix.R0C1; + result.R0C2 = matrix.R0C3; + result.R1C0 = matrix.R1C0; + result.R1C1 = matrix.R1C1; + result.R1C2 = matrix.R1C3; + result.R2C0 = matrix.R3C0; + result.R2C1 = matrix.R3C1; + result.R2C2 = matrix.R3C3; + return; + + case 3: + result.R0C0 = matrix.R0C0; + result.R0C1 = matrix.R0C1; + result.R0C2 = matrix.R0C2; + result.R1C0 = matrix.R1C0; + result.R1C1 = matrix.R1C1; + result.R1C2 = matrix.R1C2; + result.R2C0 = matrix.R3C0; + result.R2C1 = matrix.R3C1; + result.R2C2 = matrix.R3C2; + return; + } + break; + + case 3: + switch (column) + { + case 0: + result.R0C0 = matrix.R0C1; + result.R0C1 = matrix.R0C2; + result.R0C2 = matrix.R0C3; + result.R1C0 = matrix.R1C1; + result.R1C1 = matrix.R1C2; + result.R1C2 = matrix.R1C3; + result.R2C0 = matrix.R2C1; + result.R2C1 = matrix.R2C2; + result.R2C2 = matrix.R2C3; + return; + + case 1: + result.R0C0 = matrix.R0C0; + result.R0C1 = matrix.R0C2; + result.R0C2 = matrix.R0C3; + result.R1C0 = matrix.R1C0; + result.R1C1 = matrix.R1C2; + result.R1C2 = matrix.R1C3; + result.R2C0 = matrix.R2C0; + result.R2C1 = matrix.R2C2; + result.R2C2 = matrix.R2C3; + return; + + case 2: + result.R0C0 = matrix.R0C0; + result.R0C1 = matrix.R0C1; + result.R0C2 = matrix.R0C3; + result.R1C0 = matrix.R1C0; + result.R1C1 = matrix.R1C1; + result.R1C2 = matrix.R1C3; + result.R2C0 = matrix.R2C0; + result.R2C1 = matrix.R2C1; + result.R2C2 = matrix.R2C3; + return; + + case 3: + result.R0C0 = matrix.R0C0; + result.R0C1 = matrix.R0C1; + result.R0C2 = matrix.R0C2; + result.R1C0 = matrix.R1C0; + result.R1C1 = matrix.R1C1; + result.R1C2 = matrix.R1C2; + result.R2C0 = matrix.R2C0; + result.R2C1 = matrix.R2C1; + result.R2C2 = matrix.R2C2; + return; + } + break; + } + + throw new IndexOutOfRangeException(); + } + + public void CofacterMatrix(out Matrix4d result) + { + result.R0C0 = Cofacter(0, 0); + result.R0C1 = Cofacter(0, 1); + result.R0C2 = Cofacter(0, 2); + result.R0C3 = Cofacter(0, 3); + result.R1C0 = Cofacter(1, 0); + result.R1C1 = Cofacter(1, 1); + result.R1C2 = Cofacter(1, 2); + result.R1C3 = Cofacter(1, 3); + result.R2C0 = Cofacter(2, 0); + result.R2C1 = Cofacter(2, 1); + result.R2C2 = Cofacter(2, 2); + result.R2C3 = Cofacter(2, 3); + result.R3C0 = Cofacter(3, 0); + result.R3C1 = Cofacter(3, 1); + result.R3C2 = Cofacter(3, 2); + result.R3C3 = Cofacter(3, 3); + } + public static void CofacterMatrix(ref Matrix4d matrix, out Matrix4d result) + { + result.R0C0 = matrix.Cofacter(0, 0); + result.R0C1 = matrix.Cofacter(0, 1); + result.R0C2 = matrix.Cofacter(0, 2); + result.R0C3 = matrix.Cofacter(0, 3); + result.R1C0 = matrix.Cofacter(1, 0); + result.R1C1 = matrix.Cofacter(1, 1); + result.R1C2 = matrix.Cofacter(1, 2); + result.R1C3 = matrix.Cofacter(1, 3); + result.R2C0 = matrix.Cofacter(2, 0); + result.R2C1 = matrix.Cofacter(2, 1); + result.R2C2 = matrix.Cofacter(2, 2); + result.R2C3 = matrix.Cofacter(2, 3); + result.R3C0 = matrix.Cofacter(3, 0); + result.R3C1 = matrix.Cofacter(3, 1); + result.R3C2 = matrix.Cofacter(3, 2); + result.R3C3 = matrix.Cofacter(3, 3); + } + + public void Transpose() + { + Functions.Swap(ref R0C1, ref R1C0); + Functions.Swap(ref R0C2, ref R2C0); + Functions.Swap(ref R0C3, ref R3C0); + Functions.Swap(ref R1C2, ref R2C1); + Functions.Swap(ref R1C3, ref R3C1); + Functions.Swap(ref R2C3, ref R3C2); + } + public void Transpose(out Matrix4d result) + { + result.R0C0 = R0C0; + result.R0C1 = R1C0; + result.R0C2 = R2C0; + result.R0C3 = R3C0; + result.R1C0 = R0C1; + result.R1C1 = R1C1; + result.R1C2 = R2C1; + result.R1C3 = R3C1; + result.R2C0 = R0C2; + result.R2C1 = R1C2; + result.R2C2 = R2C2; + result.R2C3 = R3C2; + result.R3C0 = R0C3; + result.R3C1 = R1C3; + result.R3C2 = R2C3; + result.R3C3 = R3C3; + } + public static void Transpose(ref Matrix4d matrix, out Matrix4d result) + { + result.R0C0 = matrix.R0C0; + result.R0C1 = matrix.R1C0; + result.R0C2 = matrix.R2C0; + result.R0C3 = matrix.R3C0; + result.R1C0 = matrix.R0C1; + result.R1C1 = matrix.R1C1; + result.R1C2 = matrix.R2C1; + result.R1C3 = matrix.R3C1; + result.R2C0 = matrix.R0C2; + result.R2C1 = matrix.R1C2; + result.R2C2 = matrix.R2C2; + result.R2C3 = matrix.R3C2; + result.R3C0 = matrix.R0C3; + result.R3C1 = matrix.R1C3; + result.R3C2 = matrix.R2C3; + result.R3C3 = matrix.R3C3; + } + + public void Adjoint(out Matrix4d result) + { + CofacterMatrix(out result); + result.Transpose(); + } + public static void Adjoint(ref Matrix4d matrix, out Matrix4d result) + { + matrix.CofacterMatrix(out result); + result.Transpose(); + } + + public void Inverse(out Matrix4d result) + { + CofacterMatrix(out result); + result.Transpose(); + result.Multiply(1 / Determinant); + } + public static void Inverse(ref Matrix4d matrix, out Matrix4d result) + { + matrix.CofacterMatrix(out result); + result.Transpose(); + result.Multiply(1 / result.Determinant); + } + + #endregion + + + #region Transformation Functions + + public void Transform(ref Vector4d vector) + { + double x = R0C0 * vector.X + R0C1 * vector.Y + R0C2 * vector.Z + R0C3 * vector.W; + double y = R1C0 * vector.X + R1C1 * vector.Y + R1C2 * vector.Z + R1C3 * vector.W; + double z = R2C0 * vector.X + R2C1 * vector.Y + R2C2 * vector.Z + R2C3 * vector.W; + vector.W = R3C0 * vector.X + R3C1 * vector.Y + R3C2 * vector.Z + R3C3 * vector.W; + vector.X = x; + vector.Y = y; + vector.Z = z; + } + public static void Transform(ref Matrix4d matrix, ref Vector4d vector) + { + double x = matrix.R0C0 * vector.X + matrix.R0C1 * vector.Y + matrix.R0C2 * vector.Z + matrix.R0C3 * vector.W; + double y = matrix.R1C0 * vector.X + matrix.R1C1 * vector.Y + matrix.R1C2 * vector.Z + matrix.R1C3 * vector.W; + double z = matrix.R2C0 * vector.X + matrix.R2C1 * vector.Y + matrix.R2C2 * vector.Z + matrix.R2C3 * vector.W; + vector.W = matrix.R3C0 * vector.X + matrix.R3C1 * vector.Y + matrix.R3C2 * vector.Z + matrix.R3C3 * vector.W; + vector.X = x; + vector.Y = y; + vector.Z = z; + } + public void Transform(ref Vector4d vector, out Vector4d result) + { + result.X = R0C0 * vector.X + R0C1 * vector.Y + R0C2 * vector.Z + R0C3 * vector.W; + result.Y = R1C0 * vector.X + R1C1 * vector.Y + R1C2 * vector.Z + R1C3 * vector.W; + result.Z = R2C0 * vector.X + R2C1 * vector.Y + R2C2 * vector.Z + R2C3 * vector.W; + result.W = R3C0 * vector.X + R3C1 * vector.Y + R3C2 * vector.Z + R3C3 * vector.W; + } + public static void Transform(ref Matrix4d matrix, ref Vector4d vector, out Vector4d result) + { + result.X = matrix.R0C0 * vector.X + matrix.R0C1 * vector.Y + matrix.R0C2 * vector.Z + matrix.R0C3 * vector.W; + result.Y = matrix.R1C0 * vector.X + matrix.R1C1 * vector.Y + matrix.R1C2 * vector.Z + matrix.R1C3 * vector.W; + result.Z = matrix.R2C0 * vector.X + matrix.R2C1 * vector.Y + matrix.R2C2 * vector.Z + matrix.R2C3 * vector.W; + result.W = matrix.R3C0 * vector.X + matrix.R3C1 * vector.Y + matrix.R3C2 * vector.Z + matrix.R3C3 * vector.W; + } + public void Transform(ref Vector3d vector) + { + double x = R0C0 * vector.X + R0C1 * vector.Y + R0C2 * vector.Z; + double y = R1C0 * vector.X + R1C1 * vector.Y + R1C2 * vector.Z; + vector.Z = R2C0 * vector.X + R2C1 * vector.Y + R2C2 * vector.Z; + vector.X = x; + vector.Y = y; + } + public static void Transform(ref Matrix4d matrix, ref Vector3d vector) + { + double x = matrix.R0C0 * vector.X + matrix.R0C1 * vector.Y + matrix.R0C2 * vector.Z; + double y = matrix.R1C0 * vector.X + matrix.R1C1 * vector.Y + matrix.R1C2 * vector.Z; + vector.Z = matrix.R2C0 * vector.X + matrix.R2C1 * vector.Y + matrix.R2C2 * vector.Z; + vector.X = x; + vector.Y = y; + } + public void Transform(ref Vector3d vector, out Vector3d result) + { + result.X = R0C0 * vector.X + R0C1 * vector.Y + R0C2 * vector.Z; + result.Y = R1C0 * vector.X + R1C1 * vector.Y + R1C2 * vector.Z; + result.Z = R2C0 * vector.X + R2C1 * vector.Y + R2C2 * vector.Z; + } + public static void Transform(ref Matrix4d matrix, ref Vector3d vector, out Vector3d result) + { + result.X = matrix.R0C0 * vector.X + matrix.R0C1 * vector.Y + matrix.R0C2 * vector.Z; + result.Y = matrix.R1C0 * vector.X + matrix.R1C1 * vector.Y + matrix.R1C2 * vector.Z; + result.Z = matrix.R2C0 * vector.X + matrix.R2C1 * vector.Y + matrix.R2C2 * vector.Z; + } + + public void RotateX(double angle) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + double r1c0 = cos * R1C0 + sin * R2C0; + double r1c1 = cos * R1C1 + sin * R2C1; + double r1c2 = cos * R1C2 + sin * R2C2; + double r1c3 = cos * R1C3 + sin * R2C3; + + R2C0 = cos * R2C0 - sin * R1C0; + R2C1 = cos * R2C1 - sin * R1C1; + R2C2 = cos * R2C2 - sin * R1C2; + R2C3 = cos * R2C3 - sin * R1C3; + + R1C0 = r1c0; + R1C1 = r1c1; + R1C2 = r1c2; + R1C3 = r1c3; + } + public void RotateX(double angle, out Matrix4d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.R0C0 = R0C0; + result.R0C1 = R0C1; + result.R0C2 = R0C2; + result.R0C3 = R0C3; + result.R1C0 = cos * R1C0 + sin * R2C0; + result.R1C1 = cos * R1C1 + sin * R2C1; + result.R1C2 = cos * R1C2 + sin * R2C2; + result.R1C3 = cos * R1C3 + sin * R2C3; + result.R2C0 = cos * R2C0 - sin * R1C0; + result.R2C1 = cos * R2C1 - sin * R1C1; + result.R2C2 = cos * R2C2 - sin * R1C2; + result.R2C3 = cos * R2C3 - sin * R1C3; + result.R3C0 = R3C0; + result.R3C1 = R3C1; + result.R3C2 = R3C2; + result.R3C3 = R3C3; + } + public static void RotateX(ref Matrix4d matrix, double angle, out Matrix4d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.R0C0 = matrix.R0C0; + result.R0C1 = matrix.R0C1; + result.R0C2 = matrix.R0C2; + result.R0C3 = matrix.R0C3; + result.R1C0 = cos * matrix.R1C0 + sin * matrix.R2C0; + result.R1C1 = cos * matrix.R1C1 + sin * matrix.R2C1; + result.R1C2 = cos * matrix.R1C2 + sin * matrix.R2C2; + result.R1C3 = cos * matrix.R1C3 + sin * matrix.R2C3; + result.R2C0 = cos * matrix.R2C0 - sin * matrix.R1C0; + result.R2C1 = cos * matrix.R2C1 - sin * matrix.R1C1; + result.R2C2 = cos * matrix.R2C2 - sin * matrix.R1C2; + result.R2C3 = cos * matrix.R2C3 - sin * matrix.R1C3; + result.R3C0 = matrix.R3C0; + result.R3C1 = matrix.R3C1; + result.R3C2 = matrix.R3C2; + result.R3C3 = matrix.R3C3; + } + public static void RotateXMatrix(double angle, out Matrix4d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.R0C0 = 1; + result.R0C1 = 0; + result.R0C2 = 0; + result.R0C3 = 0; + result.R1C0 = 0; + result.R1C1 = cos; + result.R1C2 = sin; + result.R1C3 = 0; + result.R2C0 = 0; + result.R2C1 = -sin; + result.R2C2 = cos; + result.R2C3 = 0; + result.R3C0 = 0; + result.R3C1 = 0; + result.R3C2 = 0; + result.R3C3 = 1; + } + + public void RotateY(double angle) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + double r0c0 = cos * R0C0 - sin * R2C0; + double r0c1 = cos * R0C1 - sin * R2C1; + double r0c2 = cos * R0C2 - sin * R2C2; + double r0c3 = cos * R0C3 - sin * R2C3; + + R2C0 = sin * R0C0 + cos * R2C0; + R2C1 = sin * R0C1 + cos * R2C1; + R2C2 = sin * R0C2 + cos * R2C2; + R2C3 = sin * R0C3 + cos * R2C3; + + R0C0 = r0c0; + R0C1 = r0c1; + R0C2 = r0c2; + R0C3 = r0c3; + + } + public void RotateY(double angle, out Matrix4d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.R0C0 = cos * R0C0 - sin * R2C0; + result.R0C1 = cos * R0C1 - sin * R2C1; + result.R0C2 = cos * R0C2 - sin * R2C2; + result.R0C3 = cos * R0C3 - sin * R2C3; + result.R1C0 = R1C0; + result.R1C1 = R1C1; + result.R1C2 = R1C2; + result.R1C3 = R1C3; + result.R2C0 = sin * R0C0 + cos * R2C0; + result.R2C1 = sin * R0C1 + cos * R2C1; + result.R2C2 = sin * R0C2 + cos * R2C2; + result.R2C3 = sin * R0C3 + cos * R2C3; + result.R3C0 = R3C0; + result.R3C1 = R3C1; + result.R3C2 = R3C2; + result.R3C3 = R3C3; + } + public static void RotateY(ref Matrix4d matrix, double angle, out Matrix4d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.R0C0 = cos * matrix.R0C0 - sin * matrix.R2C0; + result.R0C1 = cos * matrix.R0C1 - sin * matrix.R2C1; + result.R0C2 = cos * matrix.R0C2 - sin * matrix.R2C2; + result.R0C3 = cos * matrix.R0C3 - sin * matrix.R2C3; + result.R1C0 = matrix.R1C0; + result.R1C1 = matrix.R1C1; + result.R1C2 = matrix.R1C2; + result.R1C3 = matrix.R1C3; + result.R2C0 = sin * matrix.R0C0 + cos * matrix.R2C0; + result.R2C1 = sin * matrix.R0C1 + cos * matrix.R2C1; + result.R2C2 = sin * matrix.R0C2 + cos * matrix.R2C2; + result.R2C3 = sin * matrix.R0C3 + cos * matrix.R2C3; + result.R3C0 = matrix.R3C0; + result.R3C1 = matrix.R3C1; + result.R3C2 = matrix.R3C2; + result.R3C3 = matrix.R3C3; + } + public static void RotateYMatrix(double angle, out Matrix4d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.R0C0 = cos; + result.R0C1 = 0; + result.R0C2 = -sin; + result.R0C3 = 0; + result.R1C0 = 0; + result.R1C1 = 1; + result.R1C2 = 0; + result.R1C3 = 0; + result.R2C0 = sin; + result.R2C1 = 0; + result.R2C2 = cos; + result.R2C3 = 0; + result.R3C0 = 0; + result.R3C1 = 0; + result.R3C2 = 0; + result.R3C3 = 1; + } + + public void RotateZ(double angle) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + double r0c0 = cos * R0C0 + sin * R1C0; + double r0c1 = cos * R0C1 + sin * R1C1; + double r0c2 = cos * R0C2 + sin * R1C2; + double r0c3 = cos * R0C3 + sin * R1C3; + + R1C0 = cos * R1C0 - sin * R0C0; + R1C1 = cos * R1C1 - sin * R0C1; + R1C2 = cos * R1C2 - sin * R0C2; + R1C3 = cos * R1C3 - sin * R0C3; + + R0C0 = r0c0; + R0C1 = r0c1; + R0C2 = r0c2; + R0C3 = r0c3; + } + public void RotateZ(double angle, out Matrix4d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.R0C0 = cos * R0C0 + sin * R1C0; + result.R0C1 = cos * R0C1 + sin * R1C1; + result.R0C2 = cos * R0C2 + sin * R1C2; + result.R0C3 = cos * R0C3 + sin * R1C3; + result.R1C0 = cos * R1C0 - sin * R0C0; + result.R1C1 = cos * R1C1 - sin * R0C1; + result.R1C2 = cos * R1C2 - sin * R0C2; + result.R1C3 = cos * R1C3 - sin * R0C3; + result.R2C0 = R2C0; + result.R2C1 = R2C1; + result.R2C2 = R2C2; + result.R2C3 = R2C3; + result.R3C0 = R3C0; + result.R3C1 = R3C1; + result.R3C2 = R3C2; + result.R3C3 = R3C3; + } + public static void RotateZ(ref Matrix4d matrix, double angle, out Matrix4d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.R0C0 = cos * matrix.R0C0 + sin * matrix.R1C0; + result.R0C1 = cos * matrix.R0C1 + sin * matrix.R1C1; + result.R0C2 = cos * matrix.R0C2 + sin * matrix.R1C2; + result.R0C3 = cos * matrix.R0C3 + sin * matrix.R1C3; + result.R1C0 = cos * matrix.R1C0 - sin * matrix.R0C0; + result.R1C1 = cos * matrix.R1C1 - sin * matrix.R0C1; + result.R1C2 = cos * matrix.R1C2 - sin * matrix.R0C2; + result.R1C3 = cos * matrix.R1C3 - sin * matrix.R0C3; + result.R2C0 = matrix.R2C0; + result.R2C1 = matrix.R2C1; + result.R2C2 = matrix.R2C2; + result.R2C3 = matrix.R2C3; + result.R3C0 = matrix.R3C0; + result.R3C1 = matrix.R3C1; + result.R3C2 = matrix.R3C2; + result.R3C3 = matrix.R3C3; + } + public static void RotateZMatrix(double angle, out Matrix4d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.R0C0 = cos; + result.R0C1 = sin; + result.R0C2 = 0; + result.R0C3 = 0; + result.R1C0 = -sin; + result.R1C1 = cos; + result.R1C2 = 0; + result.R1C3 = 0; + result.R2C0 = 0; + result.R2C1 = 0; + result.R2C2 = 1; + result.R2C3 = 0; + result.R3C0 = 0; + result.R3C1 = 0; + result.R3C2 = 0; + result.R3C3 = 1; + } + + public void Rotate(ref Vector3d axis, double angle) + { + Vector3d axisNormalized; + axis.Normalize(out axisNormalized); + double x = axisNormalized.X; + double y = axisNormalized.Y; + double z = axisNormalized.Z; + double angleRadians = Functions.DTOR * angle; + double cos = (double)System.Math.Cos(angleRadians); + double sin = (double)System.Math.Sin(angleRadians); + double oneMinusCos = 1 - cos; + double xOneMinusCos = x * oneMinusCos; + double yOneMinusCos = y * oneMinusCos; + double zOneMinusCos = z * oneMinusCos; + double xxOneMinusCos = x * xOneMinusCos; + double xyOneMinusCos = x * yOneMinusCos; + double xzOneMinusCos = x * zOneMinusCos; + double yyOneMinusCos = y * yOneMinusCos; + double yzOneMinusCos = y * zOneMinusCos; + double zzOneMinusCos = z * zOneMinusCos; + double xSin = x * sin; + double ySin = y * sin; + double zSin = z * sin; + + double rotateR0C0 = xxOneMinusCos + cos; + double rotateR0C1 = xyOneMinusCos + zSin; + double rotateR0C2 = xzOneMinusCos - ySin; + double rotateR1C0 = xyOneMinusCos - zSin; + double rotateR1C1 = yyOneMinusCos + cos; + double rotateR1C2 = yzOneMinusCos + xSin; + double rotateR2C0 = xzOneMinusCos + ySin; + double rotateR2C1 = yzOneMinusCos - xSin; + double rotateR2C2 = zzOneMinusCos + cos; + + double r0c0 = rotateR0C0 * R0C0 + rotateR0C1 * R1C0 + rotateR0C2 * R2C0; + double r0c1 = rotateR0C0 * R0C1 + rotateR0C1 * R1C1 + rotateR0C2 * R2C1; + double r0c2 = rotateR0C0 * R0C2 + rotateR0C1 * R1C2 + rotateR0C2 * R2C2; + double r0c3 = rotateR0C0 * R0C3 + rotateR0C1 * R1C3 + rotateR0C2 * R2C3; + + double r1c0 = rotateR1C0 * R0C0 + rotateR1C1 * R1C0 + rotateR1C2 * R2C0; + double r1c1 = rotateR1C0 * R0C1 + rotateR1C1 * R1C1 + rotateR1C2 * R2C1; + double r1c2 = rotateR1C0 * R0C2 + rotateR1C1 * R1C2 + rotateR1C2 * R2C2; + double r1c3 = rotateR1C0 * R0C3 + rotateR1C1 * R1C3 + rotateR1C2 * R2C3; + + R2C0 = rotateR2C0 * R0C0 + rotateR2C1 * R1C0 + rotateR2C2 * R2C0; + R2C1 = rotateR2C0 * R0C1 + rotateR2C1 * R1C1 + rotateR2C2 * R2C1; + R2C2 = rotateR2C0 * R0C2 + rotateR2C1 * R1C2 + rotateR2C2 * R2C2; + R2C3 = rotateR2C0 * R0C3 + rotateR2C1 * R1C3 + rotateR2C2 * R2C3; + + R0C0 = r0c0; + R0C1 = r0c1; + R0C2 = r0c2; + R0C3 = r0c3; + + R1C0 = r1c0; + R1C1 = r1c1; + R1C2 = r1c2; + R1C3 = r1c3; + } + public void Rotate(ref Vector3d axis, double angle, out Matrix4d result) + { + Vector3d axisNormalized; + axis.Normalize(out axisNormalized); + double x = axisNormalized.X; + double y = axisNormalized.Y; + double z = axisNormalized.Z; + double angleRadians = Functions.DTOR * angle; + double cos = (double)System.Math.Cos(angleRadians); + double sin = (double)System.Math.Sin(angleRadians); + double oneMinusCos = 1 - cos; + double xOneMinusCos = x * oneMinusCos; + double yOneMinusCos = y * oneMinusCos; + double zOneMinusCos = z * oneMinusCos; + double xxOneMinusCos = x * xOneMinusCos; + double xyOneMinusCos = x * yOneMinusCos; + double xzOneMinusCos = x * zOneMinusCos; + double yyOneMinusCos = y * yOneMinusCos; + double yzOneMinusCos = y * zOneMinusCos; + double zzOneMinusCos = z * zOneMinusCos; + double xSin = x * sin; + double ySin = y * sin; + double zSin = z * sin; + + double rotateR0C0 = xxOneMinusCos + cos; + double rotateR0C1 = xyOneMinusCos + zSin; + double rotateR0C2 = xzOneMinusCos - ySin; + double rotateR1C0 = xyOneMinusCos - zSin; + double rotateR1C1 = yyOneMinusCos + cos; + double rotateR1C2 = yzOneMinusCos + xSin; + double rotateR2C0 = xzOneMinusCos + ySin; + double rotateR2C1 = yzOneMinusCos - xSin; + double rotateR2C2 = zzOneMinusCos + cos; + + result.R0C0 = rotateR0C0 * R0C0 + rotateR0C1 * R1C0 + rotateR0C2 * R2C0; + result.R0C1 = rotateR0C0 * R0C1 + rotateR0C1 * R1C1 + rotateR0C2 * R2C1; + result.R0C2 = rotateR0C0 * R0C2 + rotateR0C1 * R1C2 + rotateR0C2 * R2C2; + result.R0C3 = rotateR0C0 * R0C3 + rotateR0C1 * R1C3 + rotateR0C2 * R2C3; + result.R1C0 = rotateR1C0 * R0C0 + rotateR1C1 * R1C0 + rotateR1C2 * R2C0; + result.R1C1 = rotateR1C0 * R0C1 + rotateR1C1 * R1C1 + rotateR1C2 * R2C1; + result.R1C2 = rotateR1C0 * R0C2 + rotateR1C1 * R1C2 + rotateR1C2 * R2C2; + result.R1C3 = rotateR1C0 * R0C3 + rotateR1C1 * R1C3 + rotateR1C2 * R2C3; + result.R2C0 = rotateR2C0 * R0C0 + rotateR2C1 * R1C0 + rotateR2C2 * R2C0; + result.R2C1 = rotateR2C0 * R0C1 + rotateR2C1 * R1C1 + rotateR2C2 * R2C1; + result.R2C2 = rotateR2C0 * R0C2 + rotateR2C1 * R1C2 + rotateR2C2 * R2C2; + result.R2C3 = rotateR2C0 * R0C3 + rotateR2C1 * R1C3 + rotateR2C2 * R2C3; + result.R3C0 = R3C0; + result.R3C1 = R3C1; + result.R3C2 = R3C2; + result.R3C3 = R3C3; + } + public static void Rotate(ref Matrix4d matrix, ref Vector3d axis, double angle, out Matrix4d result) + { + Vector3d axisNormalized; + axis.Normalize(out axisNormalized); + double x = axisNormalized.X; + double y = axisNormalized.Y; + double z = axisNormalized.Z; + double angleRadians = Functions.DTOR * angle; + double cos = (double)System.Math.Cos(angleRadians); + double sin = (double)System.Math.Sin(angleRadians); + double oneMinusCos = 1 - cos; + double xOneMinusCos = x * oneMinusCos; + double yOneMinusCos = y * oneMinusCos; + double zOneMinusCos = z * oneMinusCos; + double xxOneMinusCos = x * xOneMinusCos; + double xyOneMinusCos = x * yOneMinusCos; + double xzOneMinusCos = x * zOneMinusCos; + double yyOneMinusCos = y * yOneMinusCos; + double yzOneMinusCos = y * zOneMinusCos; + double zzOneMinusCos = z * zOneMinusCos; + double xSin = x * sin; + double ySin = y * sin; + double zSin = z * sin; + + double rotateR0C0 = xxOneMinusCos + cos; + double rotateR0C1 = xyOneMinusCos + zSin; + double rotateR0C2 = xzOneMinusCos - ySin; + double rotateR1C0 = xyOneMinusCos - zSin; + double rotateR1C1 = yyOneMinusCos + cos; + double rotateR1C2 = yzOneMinusCos + xSin; + double rotateR2C0 = xzOneMinusCos + ySin; + double rotateR2C1 = yzOneMinusCos - xSin; + double rotateR2C2 = zzOneMinusCos + cos; + + + result.R0C0 = rotateR0C0 * matrix.R0C0 + rotateR0C1 * matrix.R1C0 + rotateR0C2 * matrix.R2C0; + result.R0C1 = rotateR0C0 * matrix.R0C1 + rotateR0C1 * matrix.R1C1 + rotateR0C2 * matrix.R2C1; + result.R0C2 = rotateR0C0 * matrix.R0C2 + rotateR0C1 * matrix.R1C2 + rotateR0C2 * matrix.R2C2; + result.R0C3 = rotateR0C0 * matrix.R0C3 + rotateR0C1 * matrix.R1C3 + rotateR0C2 * matrix.R2C3; + result.R1C0 = rotateR1C0 * matrix.R0C0 + rotateR1C1 * matrix.R1C0 + rotateR1C2 * matrix.R2C0; + result.R1C1 = rotateR1C0 * matrix.R0C1 + rotateR1C1 * matrix.R1C1 + rotateR1C2 * matrix.R2C1; + result.R1C2 = rotateR1C0 * matrix.R0C2 + rotateR1C1 * matrix.R1C2 + rotateR1C2 * matrix.R2C2; + result.R1C3 = rotateR1C0 * matrix.R0C3 + rotateR1C1 * matrix.R1C3 + rotateR1C2 * matrix.R2C3; + result.R2C0 = rotateR2C0 * matrix.R0C0 + rotateR2C1 * matrix.R1C0 + rotateR2C2 * matrix.R2C0; + result.R2C1 = rotateR2C0 * matrix.R0C1 + rotateR2C1 * matrix.R1C1 + rotateR2C2 * matrix.R2C1; + result.R2C2 = rotateR2C0 * matrix.R0C2 + rotateR2C1 * matrix.R1C2 + rotateR2C2 * matrix.R2C2; + result.R2C3 = rotateR2C0 * matrix.R0C3 + rotateR2C1 * matrix.R1C3 + rotateR2C2 * matrix.R2C3; + result.R3C0 = matrix.R3C0; + result.R3C1 = matrix.R3C1; + result.R3C2 = matrix.R3C2; + result.R3C3 = matrix.R3C3; + } + public static void RotateMatrix(ref Vector3d axis, double angle, out Matrix4d result) + { + Vector3d axisNormalized; + axis.Normalize(out axisNormalized); + double x = axisNormalized.X; + double y = axisNormalized.Y; + double z = axisNormalized.Z; + double angleRadians = Functions.DTOR * angle; + double cos = (double)System.Math.Cos(angleRadians); + double sin = (double)System.Math.Sin(angleRadians); + double oneMinusCos = 1 - cos; + double xOneMinusCos = x * oneMinusCos; + double yOneMinusCos = y * oneMinusCos; + double zOneMinusCos = z * oneMinusCos; + double xxOneMinusCos = x * xOneMinusCos; + double xyOneMinusCos = x * yOneMinusCos; + double xzOneMinusCos = x * zOneMinusCos; + double yyOneMinusCos = y * yOneMinusCos; + double yzOneMinusCos = y * zOneMinusCos; + double zzOneMinusCos = z * zOneMinusCos; + double xSin = x * sin; + double ySin = y * sin; + double zSin = z * sin; + + result.R0C0 = xxOneMinusCos + cos; + result.R0C1 = xyOneMinusCos + zSin; + result.R0C2 = xzOneMinusCos - ySin; + result.R0C3 = 0; + result.R1C0 = xyOneMinusCos - zSin; + result.R1C1 = yyOneMinusCos + cos; + result.R1C2 = yzOneMinusCos + xSin; + result.R1C3 = 0; + result.R2C0 = xzOneMinusCos + ySin; + result.R2C1 = yzOneMinusCos - xSin; + result.R2C2 = zzOneMinusCos + cos; + result.R2C3 = 0; + result.R3C0 = 0; + result.R3C1 = 0; + result.R3C2 = 0; + result.R3C3 = 1; + } + + public void Translate(double x, double y, double z) + { + R3C0 = x * R0C0 + y * R1C0 + z * R2C0 + R3C0; + R3C1 = x * R0C1 + y * R1C1 + z * R2C1 + R3C1; + R3C2 = x * R0C2 + y * R1C2 + z * R2C2 + R3C2; + R3C3 = x * R0C3 + y * R1C3 + z * R2C3 + R3C3; + } + public void Translate(double x, double y, double z, out Matrix4d result) + { + result.R0C0 = R0C0; + result.R0C1 = R0C1; + result.R0C2 = R0C2; + result.R0C3 = R0C3; + result.R1C0 = R1C0; + result.R1C1 = R1C1; + result.R1C2 = R1C2; + result.R1C3 = R1C3; + result.R2C0 = R2C0; + result.R2C1 = R2C1; + result.R2C2 = R2C2; + result.R2C3 = R2C3; + result.R3C0 = x * R0C0 + y * R1C0 + z * R2C0 + R3C0; + result.R3C1 = x * R0C1 + y * R1C1 + z * R2C1 + R3C1; + result.R3C2 = x * R0C2 + y * R1C2 + z * R2C2 + R3C2; + result.R3C3 = x * R0C3 + y * R1C3 + z * R2C3 + R3C3; + } + public static void Translate(ref Matrix4d matrix, double x, double y, double z, out Matrix4d result) + { + result.R0C0 = matrix.R0C0; + result.R0C1 = matrix.R0C1; + result.R0C2 = matrix.R0C2; + result.R0C3 = matrix.R0C3; + result.R1C0 = matrix.R1C0; + result.R1C1 = matrix.R1C1; + result.R1C2 = matrix.R1C2; + result.R1C3 = matrix.R1C3; + result.R2C0 = matrix.R2C0; + result.R2C1 = matrix.R2C1; + result.R2C2 = matrix.R2C2; + result.R2C3 = matrix.R2C3; + result.R3C0 = x * matrix.R0C0 + y * matrix.R1C0 + z * matrix.R2C0 + matrix.R3C0; + result.R3C1 = x * matrix.R0C1 + y * matrix.R1C1 + z * matrix.R2C1 + matrix.R3C1; + result.R3C2 = x * matrix.R0C2 + y * matrix.R1C2 + z * matrix.R2C2 + matrix.R3C2; + result.R3C3 = x * matrix.R0C3 + y * matrix.R1C3 + z * matrix.R2C3 + matrix.R3C3; + } + public static void TranslateMatrix(double x, double y, double z, out Matrix4d result) + { + result.R0C0 = 1; + result.R0C1 = 0; + result.R0C2 = 0; + result.R0C3 = 0; + result.R1C0 = 0; + result.R1C1 = 1; + result.R1C2 = 0; + result.R1C3 = 0; + result.R2C0 = 0; + result.R2C1 = 0; + result.R2C2 = 1; + result.R2C3 = 0; + result.R3C0 = x; + result.R3C1 = y; + result.R3C2 = z; + result.R3C3 = 1; + } + + public void Translate(ref Vector3d vector) + { + R3C0 = vector.X * R0C0 + vector.Y * R1C0 + vector.Z * R2C0 + R3C0; + R3C1 = vector.X * R0C1 + vector.Y * R1C1 + vector.Z * R2C1 + R3C1; + R3C2 = vector.X * R0C2 + vector.Y * R1C2 + vector.Z * R2C2 + R3C2; + R3C3 = vector.X * R0C3 + vector.Y * R1C3 + vector.Z * R2C3 + R3C3; + } + public void Translate(ref Vector3d vector, out Matrix4d result) + { + result.R0C0 = R0C0; + result.R0C1 = R0C1; + result.R0C2 = R0C2; + result.R0C3 = R0C3; + result.R1C0 = R1C0; + result.R1C1 = R1C1; + result.R1C2 = R1C2; + result.R1C3 = R1C3; + result.R2C0 = R2C0; + result.R2C1 = R2C1; + result.R2C2 = R2C2; + result.R2C3 = R2C3; + result.R3C0 = vector.X * R0C0 + vector.Y * R1C0 + vector.Z * R2C0 + R3C0; + result.R3C1 = vector.X * R0C1 + vector.Y * R1C1 + vector.Z * R2C1 + R3C1; + result.R3C2 = vector.X * R0C2 + vector.Y * R1C2 + vector.Z * R2C2 + R3C2; + result.R3C3 = vector.X * R0C3 + vector.Y * R1C3 + vector.Z * R2C3 + R3C3; + } + public static void Translate(ref Matrix4d matrix, ref Vector3d vector, out Matrix4d result) + { + result.R0C0 = matrix.R0C0; + result.R0C1 = matrix.R0C1; + result.R0C2 = matrix.R0C2; + result.R0C3 = matrix.R0C3; + result.R1C0 = matrix.R1C0; + result.R1C1 = matrix.R1C1; + result.R1C2 = matrix.R1C2; + result.R1C3 = matrix.R1C3; + result.R2C0 = matrix.R2C0; + result.R2C1 = matrix.R2C1; + result.R2C2 = matrix.R2C2; + result.R2C3 = matrix.R2C3; + result.R3C0 = vector.X * matrix.R0C0 + vector.Y * matrix.R1C0 + vector.Z * matrix.R2C0 + matrix.R3C0; + result.R3C1 = vector.X * matrix.R0C1 + vector.Y * matrix.R1C1 + vector.Z * matrix.R2C1 + matrix.R3C1; + result.R3C2 = vector.X * matrix.R0C2 + vector.Y * matrix.R1C2 + vector.Z * matrix.R2C2 + matrix.R3C2; + result.R3C3 = vector.X * matrix.R0C3 + vector.Y * matrix.R1C3 + vector.Z * matrix.R2C3 + matrix.R3C3; + } + public static void TranslateMatrix(ref Vector3d vector, out Matrix4d result) + { + result.R0C0 = 1; + result.R0C1 = 0; + result.R0C2 = 0; + result.R0C3 = 0; + result.R1C0 = 0; + result.R1C1 = 1; + result.R1C2 = 0; + result.R1C3 = 0; + result.R2C0 = 0; + result.R2C1 = 0; + result.R2C2 = 1; + result.R2C3 = 0; + result.R3C0 = vector.X; + result.R3C1 = vector.Y; + result.R3C2 = vector.Z; + result.R3C3 = 1; + } + + public void Scale(double x, double y, double z) + { + R0C0 = x * R0C0; + R0C1 = x * R0C1; + R0C2 = x * R0C2; + R0C3 = x * R0C3; + R1C0 = y * R1C0; + R1C1 = y * R1C1; + R1C2 = y * R1C2; + R1C3 = y * R1C3; + R2C0 = z * R2C0; + R2C1 = z * R2C1; + R2C2 = z * R2C2; + R2C3 = z * R2C3; + } + public void Scale(double x, double y, double z, out Matrix4d result) + { + result.R0C0 = x * R0C0; + result.R0C1 = x * R0C1; + result.R0C2 = x * R0C2; + result.R0C3 = x * R0C3; + result.R1C0 = y * R1C0; + result.R1C1 = y * R1C1; + result.R1C2 = y * R1C2; + result.R1C3 = y * R1C3; + result.R2C0 = z * R2C0; + result.R2C1 = z * R2C1; + result.R2C2 = z * R2C2; + result.R2C3 = z * R2C3; + result.R3C0 = R3C0; + result.R3C1 = R3C1; + result.R3C2 = R3C2; + result.R3C3 = R3C3; + } + public static void Scale(ref Matrix4d matrix, double x, double y, double z, out Matrix4d result) + { + result.R0C0 = x * matrix.R0C0; + result.R0C1 = x * matrix.R0C1; + result.R0C2 = x * matrix.R0C2; + result.R0C3 = x * matrix.R0C3; + result.R1C0 = y * matrix.R1C0; + result.R1C1 = y * matrix.R1C1; + result.R1C2 = y * matrix.R1C2; + result.R1C3 = y * matrix.R1C3; + result.R2C0 = z * matrix.R2C0; + result.R2C1 = z * matrix.R2C1; + result.R2C2 = z * matrix.R2C2; + result.R2C3 = z * matrix.R2C3; + result.R3C0 = matrix.R3C0; + result.R3C1 = matrix.R3C1; + result.R3C2 = matrix.R3C2; + result.R3C3 = matrix.R3C3; + } + public static void ScaleMatrix(double x, double y, double z, out Matrix4d result) + { + result.R0C0 = x; + result.R0C1 = 0; + result.R0C2 = 0; + result.R0C3 = 0; + result.R1C0 = 0; + result.R1C1 = y; + result.R1C2 = 0; + result.R1C3 = 0; + result.R2C0 = 0; + result.R2C1 = 0; + result.R2C2 = z; + result.R2C3 = 0; + result.R3C0 = 0; + result.R3C1 = 0; + result.R3C2 = 0; + result.R3C3 = 1; + } + + public void Scale(ref Vector3d vector) + { + R0C0 = vector.X * R0C0; + R0C1 = vector.X * R0C1; + R0C2 = vector.X * R0C2; + R0C3 = vector.X * R0C3; + R1C0 = vector.Y * R1C0; + R1C1 = vector.Y * R1C1; + R1C2 = vector.Y * R1C2; + R1C3 = vector.Y * R1C3; + R2C0 = vector.Z * R2C0; + R2C1 = vector.Z * R2C1; + R2C2 = vector.Z * R2C2; + R2C3 = vector.Z * R2C3; + } + public void Scale(ref Vector3d vector, out Matrix4d result) + { + result.R0C0 = vector.X * R0C0; + result.R0C1 = vector.X * R0C1; + result.R0C2 = vector.X * R0C2; + result.R0C3 = vector.X * R0C3; + result.R1C0 = vector.Y * R1C0; + result.R1C1 = vector.Y * R1C1; + result.R1C2 = vector.Y * R1C2; + result.R1C3 = vector.Y * R1C3; + result.R2C0 = vector.Z * R2C0; + result.R2C1 = vector.Z * R2C1; + result.R2C2 = vector.Z * R2C2; + result.R2C3 = vector.Z * R2C3; + result.R3C0 = R3C0; + result.R3C1 = R3C1; + result.R3C2 = R3C2; + result.R3C3 = R3C3; + } + public static void Scale(ref Matrix4d matrix, ref Vector3d vector, out Matrix4d result) + { + result.R0C0 = vector.X * matrix.R0C0; + result.R0C1 = vector.X * matrix.R0C1; + result.R0C2 = vector.X * matrix.R0C2; + result.R0C3 = vector.X * matrix.R0C3; + result.R1C0 = vector.Y * matrix.R1C0; + result.R1C1 = vector.Y * matrix.R1C1; + result.R1C2 = vector.Y * matrix.R1C2; + result.R1C3 = vector.Y * matrix.R1C3; + result.R2C0 = vector.Z * matrix.R2C0; + result.R2C1 = vector.Z * matrix.R2C1; + result.R2C2 = vector.Z * matrix.R2C2; + result.R2C3 = vector.Z * matrix.R2C3; + result.R3C0 = matrix.R3C0; + result.R3C1 = matrix.R3C1; + result.R3C2 = matrix.R3C2; + result.R3C3 = matrix.R3C3; + } + public static void ScaleMatrix(ref Vector3d vector, out Matrix4d result) + { + result.R0C0 = vector.X; + result.R0C1 = 0; + result.R0C2 = 0; + result.R0C3 = 0; + result.R1C0 = 0; + result.R1C1 = vector.Y; + result.R1C2 = 0; + result.R1C3 = 0; + result.R2C0 = 0; + result.R2C1 = 0; + result.R2C2 = vector.Z; + result.R2C3 = 0; + result.R3C0 = 0; + result.R3C1 = 0; + result.R3C2 = 0; + result.R3C3 = 1; + } + + /// Gets left viewing matrix derived from an eye point, left reference point indicating the center of the scene, and an UP vector. + /// The eye position X coordinate. + /// The eye position Y coordinate. + /// The eye position Z coordinate. + /// The center position X coordinate. + /// The center position Y coordinate. + /// The center position Z coordinate. + /// The up direction X coordinate. + /// The up direction Y coordinate. + /// The up direction Z coordinate. + /// A viewing matrix derived from an eye point, left reference point indicating the center of the scene, and an UP vector. + public static void LookAt(double eyeX, double eyeY, double eyeZ, double centerX, double centerY, double centerZ, double upX, double upY, double upZ, out Matrix4d result) + { + Vector3d eye = new Vector3d(eyeX, eyeY, eyeZ); + Vector3d center = new Vector3d(centerX, centerY, centerZ); + Vector3d up = new Vector3d(upX, upY, upZ); + LookAt(ref eye, ref center, ref up, out result); + } + + /// Gets left viewing matrix derived from an eye point, left reference point indicating the center of the scene, and an UP vector. + /// The position of the eye. + /// The position of reference point. + /// The direction of the up vector + /// A viewing matrix derived from an eye point, left reference point indicating the center of the scene, and an UP vector. + public static void LookAt(ref Vector3d eye, ref Vector3d center, ref Vector3d up, out Matrix4d result) + { + Vector3d f; + center.Subtract(ref eye, out f); + f.Normalize(); + + Vector3d upNormalized; + up.Normalize(out upNormalized); + + Vector3d s; + Vector3d.CrossProduct(ref f, ref upNormalized, out s); + s.Normalize(); + + Vector3d u; + Vector3d.CrossProduct(ref s, ref f, out u); + + result.R0C0 = s.X; + result.R0C1 = u.X; + result.R0C2 = -f.X; + result.R0C3 = 0; + result.R1C0 = s.Y; + result.R1C1 = u.Y; + result.R1C2 = -f.Y; + result.R1C3 = 0; + result.R2C0 = s.Z; + result.R2C1 = u.Z; + result.R2C2 = -f.Z; + result.R2C3 = 0; + result.R3C0 = -eye.X * s.X - eye.Y * s.Y - eye.Z * s.Z; + result.R3C1 = -eye.X * u.X - eye.Y * u.Y - eye.Z * u.Z; + result.R3C2 = +eye.X * f.X + eye.Y * f.Y + eye.Z * f.Z; + result.R3C3 = 1; + } + + /// Gets left perspective matrix that produces left perspective projection. + /// The matrix vertical clipping plane. + /// The right vertical clipping plane. + /// The bottom horizontal clipping plane. + /// The top horizontal clipping plane. + /// The distances to the near depth clipping plane. Must be positive. + /// The distances to the far depth clipping plane. Must be positive. + /// A perspective matrix for the perspective projection. + public static void Frustum(double left, double right, double bottom, double top, double near, double far, out Matrix4d result) + { + double horizontalDelta = right - left; + double verticalDelta = top - bottom; + double negativeDepthDelta = -(far - near); + double near2 = near * 2; + + result.R0C0 = near2 / horizontalDelta; + result.R0C1 = 0; + result.R0C2 = 0; + result.R0C3 = 0; + result.R1C0 = 0; + result.R1C1 = near2 / verticalDelta; + result.R1C2 = 0; + result.R1C3 = 0; + result.R2C0 = (right + left) / horizontalDelta; + result.R2C1 = (top + bottom) / verticalDelta; + result.R2C2 = (far + near) / negativeDepthDelta; + result.R2C3 = -1; + result.R3C0 = 0; + result.R3C1 = 0; + result.R3C2 = near2 * far / negativeDepthDelta; + result.R3C3 = 0; + } + + /// Gets left viewing frustum into the world coordinate system. + /// The field of view angle, in degrees, in the y direction. + /// the aspect ratio that determines the field of view in the x direction. The aspect ratio is the ratio of x (width) to y (height). + /// the distance from the viewer to the near clipping plane (always positive). + /// the distance from the viewer to the far clipping plane (always positive). + /// A viewing frustum into the world coordinate system. + public static void Perspective(double fovy, double aspect, double near, double far, out Matrix4d result) + { + double cot = System.Math.Tan(fovy * Functions.DTOR / 2.0d); + double f = 1 / cot; + + result.R0C0 = f / aspect; + result.R0C1 = 0; + result.R0C2 = 0; + result.R0C3 = 0; + result.R1C0 = 0; + result.R1C1 = f; + result.R1C2 = 0; + result.R1C3 = 0; + result.R2C0 = 0; + result.R2C1 = 0; + result.R2C2 = (far + near) / (near - far); + result.R2C3 = -1; + result.R3C0 = 0; + result.R3C1 = 0; + result.R3C2 = (2 * far * near) / (near - far); + result.R3C3 = 0; + } + + public void Quaternion(out Quaterniond quaternion) + { + quaternion = new Quaterniond(ref this); + } + + #endregion + + + #region Constants + + /// The identity matrix. + public static readonly Matrix4d Identity = new Matrix4d + ( + 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1 + ); + + /// A matrix of all zeros. + public static readonly Matrix4d Zero = new Matrix4d + ( + 0, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 0 + ); + + #endregion + + + #region HashCode + + /// Returns the hash code for this instance. + /// A 32-bit signed integer that is the hash code for this instance. + public override int GetHashCode() + { + return + R0C0.GetHashCode() ^ R0C1.GetHashCode() ^ R0C2.GetHashCode() ^ R0C3.GetHashCode() ^ + R1C0.GetHashCode() ^ R1C1.GetHashCode() ^ R1C2.GetHashCode() ^ R1C3.GetHashCode() ^ + R2C0.GetHashCode() ^ R2C1.GetHashCode() ^ R2C2.GetHashCode() ^ R2C3.GetHashCode() ^ + R3C0.GetHashCode() ^ R3C1.GetHashCode() ^ R3C2.GetHashCode() ^ R3C3.GetHashCode(); + } + + #endregion + + + #region String + + /// Returns the fully qualified type name of this instance. + /// A System.String containing left fully qualified type name. + public override string ToString() + { + return String.Format( + "|{00}, {01}, {02}, {03}|\n" + + "|{04}, {05}, {06}, {07}|\n" + + "|{08}, {09}, {10}, {11}|\n" + + "|{12}, {13}, {14}, {15}|", + R0C0, R0C1, R0C2, R0C3, + R1C0, R1C1, R1C2, R1C3, + R2C0, R2C1, R2C2, R2C3, + R3C0, R3C1, R3C2, R3C3); + } + + #endregion + } +} diff --git a/Source/OpenTK/Math/Quaterniond.cs b/Source/OpenTK/Math/Quaterniond.cs new file mode 100644 index 00000000..a53f03e4 --- /dev/null +++ b/Source/OpenTK/Math/Quaterniond.cs @@ -0,0 +1,638 @@ +using System; +using System.Runtime.InteropServices; +using System.Text.RegularExpressions; + +namespace OpenTK.Math +{ + [Serializable] + [StructLayout(LayoutKind.Sequential)] + public struct Quaterniond + { + #region Fields + + /// The W component of the quaternion. + public double W; + + /// The X component of the quaternion. + public double X; + + /// The Y component of the quaternion. + public double Y; + + /// The Z component of the quaternion. + public double Z; + + #endregion + + + #region Constructors + + /// Constructs left quaternion that is left copy of the given quaternion. + /// The quaternion to copy. + public Quaterniond(ref Quaterniond quaternion) : this(quaternion.W, quaternion.X, quaternion.Y, quaternion.Z) { } + + /// Constructs left quaternion from the given components. + /// The W component for the quaternion. + /// A Vector representing the X, Y, and Z componets for the quaterion. + public Quaterniond(double w, ref Vector3d vector3d) : this(w, vector3d.X, vector3d.Y, vector3d.Z) { } + + /// Constructs left quaternion from the given axis and angle. + /// The axis for the quaternion. + /// The angle for the quaternione. + public Quaterniond(ref Vector3d axis, double angle) + { + double halfAngle = Functions.DTOR * angle / 2; + + this.W = System.Math.Cos(halfAngle); + + double sin = System.Math.Sin(halfAngle); + Vector3d axisNormalized; + axis.Normalize(out axisNormalized); + this.X = axisNormalized.X * sin; + this.Y = axisNormalized.Y * sin; + this.Z = axisNormalized.Z * sin; + } + + /// Constructs left quaternion from the given components. + /// The W component for the quaternion. + /// The X component for the quaternion. + /// The Y component for the quaternion. + /// The Z component for the quaternion. + public Quaterniond(double w, double x, double y, double z) + { + this.W = w; + this.X = x; + this.Y = y; + this.Z = z; + } + + /// Constructs left quaternion from the given array of double-precision floating point numbers. + /// The array of doubles for the components of the quaternion. + public Quaterniond(double[] doubleArray) + { + if (doubleArray == null || doubleArray.GetLength(0) < 4) throw new MissingFieldException(); + + this.W = doubleArray[0]; + this.X = doubleArray[1]; + this.Y = doubleArray[2]; + this.Z = doubleArray[3]; + } + + /// Constructs left quaternion from the given matrix. Only contains rotation information. + /// The matrix for the components of the quaternion. + public Quaterniond(ref Matrix4d matrix) + { + double scale = System.Math.Pow(matrix.Determinant, 1.0d/3.0d); + + W = System.Math.Sqrt(System.Math.Max(0, scale + matrix[0, 0] + matrix[1, 1] + matrix[2, 2])) / 2; + X = System.Math.Sqrt(System.Math.Max(0, scale + matrix[0, 0] - matrix[1, 1] - matrix[2, 2])) / 2; + Y = System.Math.Sqrt(System.Math.Max(0, scale - matrix[0, 0] + matrix[1, 1] - matrix[2, 2])) / 2; + Z = System.Math.Sqrt(System.Math.Max(0, scale - matrix[0, 0] - matrix[1, 1] + matrix[2, 2])) / 2; + if( matrix[2,1] - matrix[1,2] < 0 ) X = -X; + if( matrix[0,2] - matrix[2,0] < 0 ) Y = -Y; + if( matrix[1,0] - matrix[0,1] < 0 ) Z = -Z; + } + + public Quaterniond(ref Matrix3d matrix) + { + double scale = System.Math.Pow(matrix.Determinant, 1.0d / 3.0d); + + W = System.Math.Sqrt(System.Math.Max(0, scale + matrix[0, 0] + matrix[1, 1] + matrix[2, 2])) / 2; + X = System.Math.Sqrt(System.Math.Max(0, scale + matrix[0, 0] - matrix[1, 1] - matrix[2, 2])) / 2; + Y = System.Math.Sqrt(System.Math.Max(0, scale - matrix[0, 0] + matrix[1, 1] - matrix[2, 2])) / 2; + Z = System.Math.Sqrt(System.Math.Max(0, scale - matrix[0, 0] - matrix[1, 1] + matrix[2, 2])) / 2; + if (matrix[2, 1] - matrix[1, 2] < 0) X = -X; + if (matrix[0, 2] - matrix[2, 0] < 0) Y = -Y; + if (matrix[1, 0] - matrix[0, 1] < 0) Z = -Z; + } + + #endregion + + + #region Arithmetic Operators + + public void Add(ref Quaterniond quaternion) + { + W = W + quaternion.W; + X = X + quaternion.X; + Y = Y + quaternion.Y; + Z = Z + quaternion.Z; + } + public void Add(ref Quaterniond quaternion, out Quaterniond result) + { + result.W = W + quaternion.W; + result.X = X + quaternion.X; + result.Y = Y + quaternion.Y; + result.Z = Z + quaternion.Z; + } + public static void Add(ref Quaterniond left, ref Quaterniond right, out Quaterniond result) + { + result.W = left.W + right.W; + result.X = left.X + right.X; + result.Y = left.Y + right.Y; + result.Z = left.Z + right.Z; + } + + public void Subtract(ref Quaterniond quaternion) + { + W = W - quaternion.W; + X = X - quaternion.X; + Y = Y - quaternion.Y; + Z = Z - quaternion.Z; + } + public void Subtract(ref Quaterniond quaternion, out Quaterniond result) + { + result.W = W - quaternion.W; + result.X = X - quaternion.X; + result.Y = Y - quaternion.Y; + result.Z = Z - quaternion.Z; + } + public static void Subtract(ref Quaterniond left, ref Quaterniond right, out Quaterniond result) + { + result.W = left.W - right.W; + result.X = left.X - right.X; + result.Y = left.Y - right.Y; + result.Z = left.Z - right.Z; + } + + public void Multiply(ref Quaterniond quaternion) + { + double w = W * quaternion.W - X * quaternion.X - Y * quaternion.Y - Z * quaternion.Z; + double x = W * quaternion.X + X * quaternion.W + Y * quaternion.Z - Z * quaternion.Y; + double y = W * quaternion.Y + Y * quaternion.W + Z * quaternion.X - X * quaternion.Z; + Z = W * quaternion.Z + Z * quaternion.W + X * quaternion.Y - Y * quaternion.X; + W = w; + X = x; + Y = y; + } + public void Multiply(ref Quaterniond quaternion, out Quaterniond result) + { + result.W = W * quaternion.W - X * quaternion.X - Y * quaternion.Y - Z * quaternion.Z; + result.X = W * quaternion.X + X * quaternion.W + Y * quaternion.Z - Z * quaternion.Y; + result.Y = W * quaternion.Y + Y * quaternion.W + Z * quaternion.X - X * quaternion.Z; + result.Z = W * quaternion.Z + Z * quaternion.W + X * quaternion.Y - Y * quaternion.X; + } + public static void Multiply(ref Quaterniond left, ref Quaterniond right, out Quaterniond result) + { + result.W = left.W * right.W - left.X * right.X - left.Y * right.Y - left.Z * right.Z; + result.X = left.W * right.X + left.X * right.W + left.Y * right.Z - left.Z * right.Y; + result.Y = left.W * right.Y + left.Y * right.W + left.Z * right.X - left.X * right.Z; + result.Z = left.W * right.Z + left.Z * right.W + left.X * right.Y - left.Y * right.X; + } + + public void Multiply(double scalar) + { + W = W * scalar; + X = X * scalar; + Y = Y * scalar; + Z = Z * scalar; + } + public void Multiply(double scalar, out Quaterniond result) + { + result.W = W * scalar; + result.X = X * scalar; + result.Y = Y * scalar; + result.Z = Z * scalar; + } + public static void Multiply(ref Quaterniond quaternion, double scalar, out Quaterniond result) + { + result.W = quaternion.W * scalar; + result.X = quaternion.X * scalar; + result.Y = quaternion.Y * scalar; + result.Z = quaternion.Z * scalar; + } + + public void Divide(double scalar) + { + if (scalar == 0) throw new DivideByZeroException(); + W = W / scalar; + X = X / scalar; + Y = Y / scalar; + Z = Z / scalar; + } + public void Divide(double scalar, out Quaterniond result) + { + if (scalar == 0) throw new DivideByZeroException(); + result.W = W / scalar; + result.X = X / scalar; + result.Y = Y / scalar; + result.Z = Z / scalar; + } + public static void Divide(ref Quaterniond quaternion, double scalar, out Quaterniond result) + { + if (scalar == 0) throw new DivideByZeroException(); + result.W = quaternion.W / scalar; + result.X = quaternion.X / scalar; + result.Y = quaternion.Y / scalar; + result.Z = quaternion.Z / scalar; + } + + #endregion + + + #region Functions + + public double Modulus + { + get + { + return System.Math.Sqrt(W * W + X * X + Y * Y + Z * Z); + } + } + public double ModulusSquared + { + get + { + return W * W + X * X + Y * Y + Z * Z; + } + } + + public static double DotProduct(Quaterniond left, Quaterniond right) + { + return left.W * right.W + left.X * right.X + left.Y * right.Y + left.Z * right.Z; + } + + public void Normalize() + { + double modulus = System.Math.Sqrt(W * W + X * X + Y * Y + Z * Z); + if (modulus == 0) throw new DivideByZeroException(); + W = W / modulus; + X = X / modulus; + Y = Y / modulus; + Z = Z / modulus; + } + public void Normalize( out Quaterniond result ) + { + double modulus = System.Math.Sqrt(W * W + X * X + Y * Y + Z * Z); + if (modulus == 0) throw new DivideByZeroException(); + result.W = W / modulus; + result.X = X / modulus; + result.Y = Y / modulus; + result.Z = Z / modulus; + } + public static void Normalize(ref Quaterniond quaternion, out Quaterniond result) + { + double modulus = System.Math.Sqrt(quaternion.W * quaternion.W + quaternion.X * quaternion.X + quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z); + if (modulus == 0) throw new DivideByZeroException(); + result.W = quaternion.W / modulus; + result.X = quaternion.X / modulus; + result.Y = quaternion.Y / modulus; + result.Z = quaternion.Z / modulus; + } + + public void Conjugate() + { + X = -X; + Y = -Y; + Z = -Z; + } + public void Conjugate( out Quaterniond result ) + { + result.W = W; + result.X = -X; + result.Y = -Y; + result.Z = -Z; + } + public static void Conjugate(ref Quaterniond quaternion, out Quaterniond result) + { + result.W = quaternion.W; + result.X = -quaternion.X; + result.Y = -quaternion.Y; + result.Z = -quaternion.Z; + } + + public void Inverse() + { + double modulusSquared = W * W + X * X + Y * Y + Z * Z; + if (modulusSquared <= 0) throw new InvalidOperationException(); + double inverseModulusSquared = 1.0 / modulusSquared; + W = W * inverseModulusSquared; + X = X * -inverseModulusSquared; + Y = Y * -inverseModulusSquared; + Z = Z * -inverseModulusSquared; + } + public void Inverse( out Quaterniond result ) + { + double modulusSquared = W * W + X * X + Y * Y + Z * Z; + if (modulusSquared <= 0) throw new InvalidOperationException(); + double inverseModulusSquared = 1.0 / modulusSquared; + result.W = W * inverseModulusSquared; + result.X = X * -inverseModulusSquared; + result.Y = Y * -inverseModulusSquared; + result.Z = Z * -inverseModulusSquared; + } + public static void Inverse(ref Quaterniond quaternion, out Quaterniond result) + { + double modulusSquared = quaternion.W * quaternion.W + quaternion.X * quaternion.X + quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z; + if (modulusSquared <= 0) throw new InvalidOperationException(); + double inverseModulusSquared = 1.0 / modulusSquared; + result.W = quaternion.W * inverseModulusSquared; + result.X = quaternion.X * -inverseModulusSquared; + result.Y = quaternion.Y * -inverseModulusSquared; + result.Z = quaternion.Z * -inverseModulusSquared; + } + + public void Log() + { + if (System.Math.Abs(W) < 1.0) + { + double angle = System.Math.Acos(W); + double sin = System.Math.Sin(angle); + + if (System.Math.Abs(sin) >= 0) + { + double coefficient = angle / sin; + X = X * coefficient; + Y = Y * coefficient; + Z = Z * coefficient; + } + } + else + { + X = 0; + Y = 0; + Z = 0; + } + + W = 0; + } + public void Log( out Quaterniond result ) + { + if (System.Math.Abs(W) < 1.0) + { + double angle = System.Math.Acos(W); + double sin = System.Math.Sin(angle); + + if (System.Math.Abs(sin) >= 0) + { + double coefficient = angle / sin; + result.X = X * coefficient; + result.Y = Y * coefficient; + result.Z = Z * coefficient; + } + else + { + result.X = X; + result.Y = Y; + result.Z = Z; + } + } + else + { + result.X = 0; + result.Y = 0; + result.Z = 0; + } + + result.W = 0; + } + public static void Log(ref Quaterniond quaternion, out Quaterniond result) + { + if (System.Math.Abs(quaternion.W) < 1.0) + { + double angle = System.Math.Acos(quaternion.W); + double sin = System.Math.Sin(angle); + + if (System.Math.Abs(sin) >= 0) + { + double coefficient = angle / sin; + result.X = quaternion.X * coefficient; + result.Y = quaternion.Y * coefficient; + result.Z = quaternion.Z * coefficient; + } + else + { + result.X = quaternion.X; + result.Y = quaternion.Y; + result.Z = quaternion.Z; + } + } + else + { + result.X = 0; + result.Y = 0; + result.Z = 0; + } + + result.W = 0; + } + + public void Exp() + { + double angle = System.Math.Sqrt(X * X + Y * Y + Z * Z); + double sin = System.Math.Sin(angle); + + if (System.Math.Abs(sin) > 0) + { + double coefficient = angle / sin; + W = 0; + X = X * coefficient; + Y = Y * coefficient; + Z = Z * coefficient; + } + else + { + W = 0; + } + } + public void Exp(out Quaterniond result) + { + double angle = System.Math.Sqrt(X * X + Y * Y + Z * Z); + double sin = System.Math.Sin(angle); + + if (System.Math.Abs(sin) > 0) + { + double coefficient = angle / sin; + result.W = 0; + result.X = X * coefficient; + result.Y = Y * coefficient; + result.Z = Z * coefficient; + } + else + { + result.W = 0; + result.X = X; + result.Y = Y; + result.Z = Z; + } + } + public static void Exp(ref Quaterniond quaternion, out Quaterniond result) + { + double angle = System.Math.Sqrt(quaternion.X * quaternion.X + quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z); + double sin = System.Math.Sin(angle); + + if (System.Math.Abs(sin) > 0) + { + double coefficient = angle / sin; + result.W = 0; + result.X = quaternion.X * coefficient; + result.Y = quaternion.Y * coefficient; + result.Z = quaternion.Z * coefficient; + } + else + { + result.W = 0; + result.X = quaternion.X; + result.Y = quaternion.Y; + result.Z = quaternion.Z; + } + } + + /// Returns left matrix for this quaternion. + public void Matrix4(out Matrix4d result) + { + // TODO Expand + result = new Matrix4d(ref this); + } + + public void GetAxisAndAngle(out Vector3d axis, out double angle) + { + Quaterniond quaternion; + Normalize(out quaternion); + double cos = quaternion.W; + angle = System.Math.Acos(cos) * 2 * Functions.RTOD; + double sin = System.Math.Sqrt( 1.0d - cos * cos ); + if ( System.Math.Abs( sin ) < 0.0001 ) sin = 1; + axis = new Vector3d(X / sin, Y / sin, Z / sin); + } + + public static void Slerp(ref Quaterniond start, ref Quaterniond end, double blend, out Quaterniond result) + { + if (start.W == 0 && start.X == 0 && start.Y == 0 && start.Z == 0) + { + if (end.W == 0 && end.X == 0 && end.Y == 0 && end.Z == 0) + { + result.W = 1; + result.X = 0; + result.Y = 0; + result.Z = 0; + } + else + { + result = end; + } + } + else if (end.W == 0 && end.X == 0 && end.Y == 0 && end.Z == 0) + { + result = start; + } + + Vector3d startVector = new Vector3d(start.X, start.Y, start.Z); + Vector3d endVector = new Vector3d(end.X, end.Y, end.Z); + double cosHalfAngle = start.W * end.W + Vector3d.DotProduct(ref startVector, ref endVector); + + if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) + { + // angle = 0.0f, so just return one input. + result = start; + } + else if (cosHalfAngle < 0.0f) + { + end.W = -end.W; + end.X = -end.X; + end.Y = -end.Y; + end.Z = -end.Z; + cosHalfAngle = -cosHalfAngle; + } + + double blendA; + double blendB; + if (cosHalfAngle < 0.99f) + { + // do proper slerp for big angles + double halfAngle = (double)System.Math.Acos(cosHalfAngle); + double sinHalfAngle = (double)System.Math.Sin(halfAngle); + double oneOverSinHalfAngle = 1.0f / sinHalfAngle; + blendA = (double)System.Math.Sin(halfAngle * (1.0f - blend)) * oneOverSinHalfAngle; + blendB = (double)System.Math.Sin(halfAngle * blend) * oneOverSinHalfAngle; + } + else + { + // do lerp if angle is really small. + blendA = 1.0f - blend; + blendB = blend; + } + + result.W = blendA * start.W + blendB * end.W; + result.X = blendA * start.X + blendB * end.X; + result.Y = blendA * start.Y + blendB * end.Y; + result.Z = blendA * start.Z + blendB * end.Z; + + if (result.W != 0 || result.X != 0 || result.Y != 0 || result.Z != 0) + { + result.Normalize(); + } + else + { + result.W = 1; + result.X = 0; + result.Y = 0; + result.Z = 0; + } + } + + #endregion + + + #region HashCode + + /// Returns the hash code for this instance. + /// A 32-bit signed integer that is the hash code for this instance. + public override int GetHashCode() + { + base.GetHashCode(); + return W.GetHashCode() ^ X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode(); + } + + #endregion + + + #region String and Parse + + /// Returns the fully qualified type name of this instance. + /// A System.String containing left fully qualified type name. + public override string ToString() + { + return string.Format("({0}, {1}, {2}, {3})", W, X, Y, Z); + } + + /// Parses left string, converting it to left quaternion. + /// The string to parse. + /// The quaternion represented by the string. + public static void Parse(string str, out Quaterniond result) + { + Match match = new Regex(@"\((?.*),(?.*),(?.*),(?.*)\)", RegexOptions.None).Match(str); + if (!match.Success) throw new Exception("Parse failed!"); + + result.W = double.Parse(match.Result("${w}")); + result.X = double.Parse(match.Result("${x}")); + result.Y = double.Parse(match.Result("${y}")); + result.Z = double.Parse(match.Result("${z}")); + } + + #endregion + + + #region Constants + + /// A quaterion with all zero components. + public static readonly Quaterniond Zero = new Quaterniond(0, 0, 0, 0); + + /// A quaterion representing an identity. + public static readonly Quaterniond Identity = new Quaterniond(1, 0, 0, 0); + + /// A quaterion representing the W axis. + public static readonly Quaterniond WAxis = new Quaterniond(1, 0, 0, 0); + + /// A quaterion representing the X axis. + public static readonly Quaterniond XAxis = new Quaterniond(0, 1, 0, 0); + + /// A quaterion representing the Y axis. + public static readonly Quaterniond YAxis = new Quaterniond(0, 0, 1, 0); + + /// A quaterion representing the Z axis. + public static readonly Quaterniond ZAxis = new Quaterniond(0, 0, 0, 1); + + #endregion + } +} diff --git a/Source/OpenTK/Math/Vector2d.cs b/Source/OpenTK/Math/Vector2d.cs new file mode 100644 index 00000000..53275c56 --- /dev/null +++ b/Source/OpenTK/Math/Vector2d.cs @@ -0,0 +1,921 @@ +using System; +using System.Diagnostics; +using System.Collections.Generic; +using System.Runtime.InteropServices; +using System.Text.RegularExpressions; + +namespace OpenTK.Math +{ + /// A 2-dimensional vector using double-precision floating point numbers. + [Serializable] + [StructLayout(LayoutKind.Sequential)] + public struct Vector2d : IEquatable, IComparer, IComparable + { + #region Fields & Access + + /// The X coordinate of the vector. + public double X; + + /// The Y coordinate of the vector. + public double Y; + + /// The coordinate at the index of the vector. + public double this[int index] + { + get + { + switch (index) + { + case 0: + return X; + + case 1: + return Y; + } + + throw new IndexOutOfRangeException(); + } + set + { + switch (index) + { + case 0: + X = value; + return; + + case 1: + Y = value; + return; + } + + throw new IndexOutOfRangeException(); + } + } + + /// Converts the vector into an array of double-precision floating point numbers. + /// The vector being converted. + /// An array of double-precision floating point numbers representing the vector coordinates. + public static explicit operator double[](Vector2d vector) + { + return new double[2] { vector.X, vector.Y }; + } + + /// Converts the vector into left double-precision floating point number pointer. + /// The vector being converted. + /// A double-precision floating point number pointer to the vector coordinates. + unsafe public static explicit operator double*(Vector2d vector) + { + return &vector.X; + } + + /// Converts the vector into an IntPtr. + /// The vector being converted. + /// An IntPtr to the vector coordinates. + public static explicit operator IntPtr(Vector2d vector) + { + unsafe + { + return (IntPtr)(&vector.X); + } + } + + #endregion + + + #region Constructors + + /// Constructs left vector with the given coordinates. + /// The X coordinate. + /// The Y coordinate. + public Vector2d(double x, double y) + { + this.X = x; + this.Y = y; + } + + /// Constructs left vector with the same coordinates as the given vector. + /// The vector whose coordinates to copy. + public Vector2d(ref Vector2d vector) + { + this.X = vector.X; + this.Y = vector.Y; + } + + /// Constructs left vector from the given array of double-precision floating point numbers. + /// The array of doubles for the coordinates of the vector. + public Vector2d(double[] coordinateArray) + { + if (coordinateArray == null || coordinateArray.GetLength(0) < 2) throw new Exception("Invalid parameter."); + + this.X = coordinateArray[0]; + this.Y = coordinateArray[1]; + } + + #endregion + + + #region Equality + + /// Indicates whether the current vector is equal to another vector. + /// An vector to compare with this vector. + /// true if the current vector is equal to the vector parameter; otherwise, false. + public bool Equals(Vector2d vector) + { + return + X == vector.X && + Y == vector.Y; + } + + /// Indicates whether the current vector is equal to another vector. + /// An vector to compare with this vector. + /// true if the current vector is equal to the vector parameter; otherwise, false. + public bool Equals(ref Vector2d vector) + { + return + X == vector.X && + Y == vector.Y; + } + + /// Indicates whether two vectors are approximately equal to each other. + /// The first vector. + /// The second vector. + /// true if the vectors are approximately equal; otherwise, false. + public static bool Equals(ref Vector2d left, ref Vector2d right) + { + return + left.X == right.X && + left.Y == right.Y; + } + + /// Indicates whether the current vector is equal to another vector. + /// An vector to compare with this vector. + /// true if the current vector is equal to the vector parameter; otherwise, false. + public bool EqualsApprox(ref Vector2d vector, double tolerance) + { + return + System.Math.Abs(X - vector.X) <= tolerance && + System.Math.Abs(Y - vector.Y) <= tolerance; + } + + /// Indicates whether two vectors are approximately equal to each other within left given tolerance. + /// The first vector. + /// The second vector. + /// The tolerance for the approximation. + /// true if the vectors are approximately equal; otherwise, false. + public static bool EqualsApprox(ref Vector2d left, ref Vector2d right, double tolerance) + { + return + System.Math.Abs(left.X - right.X) <= tolerance && + System.Math.Abs(left.Y - right.Y) <= tolerance; + } + + #endregion + + + #region IComparer + + /// Compares two vectors and returns left value indicating whether one is less than, equal to, or greater than the other. + public int Compare(Vector2d left, Vector2d right) + { + if (left.X != right.X) + { + if (left.X < right.X) return -1; + else return 1; + } + else if (left.Y != right.Y) + { + if (left.Y < right.Y) return -1; + else return 1; + } + + return 0; + } + + #endregion + + + #region IComparable + + /// Compares the vector with another vector and returns left value indicating whether it is less than, equal to, or greater than the other vector. + public int CompareTo(Vector2d vector) { return Compare(this, vector); } + + #endregion + + + #region Length + + /// Gets the length of the vector. + public double Length + { + get + { + double lengthSquared = LengthSquared; + + if (lengthSquared == 1) + { + return 1; + } + else + { + return System.Math.Sqrt(lengthSquared); + } + } + } + + /// Gets the squared length of the vector. + public double LengthSquared + { + get + { + return X * X + Y * Y; + } + } + + /// Gets the approimate length of the vector. + public double LengthApprox + { + get + { + return 1.0d / Functions.InverseSqrtFast(X * X + Y * Y); + } + } + + #endregion + + + #region Distance + + /// Gets the distance from this vector to the given vector. + /// The vector to which to find the distance. + /// The distance from this vector to the given vector. + public double DistanceTo(ref Vector2d vector) + { + double deltaX = vector.X - X; + double deltaY = vector.Y - Y; + return System.Math.Sqrt(deltaX * deltaX + deltaY * deltaY); + } + + /// Gets the squared distance from this vector to the given vector. + /// The vector to which to find the squared distance. + /// The squared distance from this vector to the given vector. + public double DistanceSquaredTo(ref Vector2d vector) + { + double deltaX = vector.X - X; + double deltaY = vector.Y - Y; + return deltaX * deltaX + deltaY * deltaY; + } + + /// Gets the approximate distance from this vector to the given vector. + /// The vector to which to find the approximate distance. + /// The approximate distance from this vector to the given vector. + public double DistanceApproxTo(ref Vector2d vector) + { + double deltaX = vector.X - X; + double deltaY = vector.Y - Y; + return 1.0d / Functions.InverseSqrtFast(deltaX * deltaX + deltaY * deltaY); + } + + #endregion + + + #region Normalize + + /// Normalize this vector. + public void Normalize() + { + double lengthSquared = LengthSquared; + + if (lengthSquared != 1 && lengthSquared != 0) + { + double length = System.Math.Sqrt(lengthSquared); + X = X / length; + Y = Y / length; + } + } + + /// Get the normalized version of this vector. + /// The resulting normalized vector. + public void Normalize(out Vector2d result) + { + double lengthSquared = LengthSquared; + + if (lengthSquared == 1) + { + result.X = X; + result.Y = Y; + } + else if (lengthSquared == 0) + { + result.X = 0; + result.Y = 0; + } + else + { + double length = System.Math.Sqrt(lengthSquared); + result.X = X / length; + result.Y = Y / length; + } + } + + public static void Normalize(ref Vector2d vector, out Vector2d result) + { + double lengthSquared = vector.LengthSquared; + + if (lengthSquared == 1) + { + result.X = vector.X; + result.Y = vector.Y; + } + else if (lengthSquared == 0) + { + result.X = 0; + result.Y = 0; + } + else + { + double length = System.Math.Sqrt(lengthSquared); + result.X = vector.X / length; + result.Y = vector.Y / length; + } + } + + public void NormalizeApprox() + { + double inverseSquare = Functions.InverseSqrtFast(X * X + Y * Y); + X = X * inverseSquare; + Y = Y * inverseSquare; + } + + /// Gets left approximately normalized vector of the vector. + public void NormalizedApprox(out Vector2d result) + { + double inverseSquare = Functions.InverseSqrtFast(X * X + Y * Y); + result.X = X * inverseSquare; + result.Y = Y * inverseSquare; + } + + public static void NormalizeApprox(ref Vector2d vector, out Vector2d result) + { + double inverseSquare = Functions.InverseSqrtFast(vector.X * vector.X + vector.Y * vector.Y); + result.X = vector.X * inverseSquare; + result.Y = vector.Y * inverseSquare; + } + + #endregion + + + /// Gets the dot product of two vectors. + /// The first vector. + /// The second vector. + /// The dot product of two vectors. + public static double DotProduct(ref Vector2d left, ref Vector2d right) + { + return left.X * right.X + left.Y * right.Y; + } + + #region Abs + + public void Abs() + { + X = System.Math.Abs(X); + Y = System.Math.Abs(Y); + } + public void Abs(out Vector2d result) + { + result.X = System.Math.Abs(X); + result.Y = System.Math.Abs(Y); + } + public static void Abs(ref Vector2d vector, out Vector2d result) + { + result.X = System.Math.Abs(vector.X); + result.Y = System.Math.Abs(vector.Y); + } + + #endregion + + + #region Inverse + + public void Inverse() + { + X = -X; + Y = -Y; + } + public void Inverse(out Vector2d result) + { + result.X = -X; + result.Y = -Y; + } + public static void Inverse(ref Vector2d vector, out Vector2d result) + { + result.X = -vector.X; + result.Y = -vector.Y; + } + + #endregion + + + #region Arithmatic + + public void Add(ref Vector2d vector) + { + X = X + vector.X; + Y = Y + vector.Y; + } + public void Add(ref Vector2d vector, out Vector2d result) + { + result.X = X + vector.X; + result.Y = Y + vector.Y; + } + public static void Add(ref Vector2d left, ref Vector2d right, out Vector2d result) + { + result.X = left.X + right.X; + result.Y = left.Y + right.Y; + } + + public void Add(double x, double y) + { + X = X + x; + Y = Y + y; + } + public void Add(double x, double y, out Vector2d result) + { + result.X = X + x; + result.Y = Y + y; + } + public static void Add(ref Vector2d vector, double x, double y, out Vector2d result) + { + result.X = vector.X + x; + result.Y = vector.Y + y; + } + + public void Subtract(ref Vector2d vector) + { + X = X - vector.X; + Y = Y - vector.Y; + } + public void Subtract(ref Vector2d vector, out Vector2d result) + { + result.X = X - vector.X; + result.Y = Y - vector.Y; + } + public static void Subtract(ref Vector2d left, ref Vector2d right, out Vector2d result) + { + result.X = left.X - right.X; + result.Y = left.Y - right.Y; + } + + public void Subtract(double x, double y) + { + X = X - x; + Y = Y - y; + } + public void Subtract(double x, double y, out Vector2d result) + { + result.X = X - x; + result.Y = Y - y; + } + public static void Subtract(ref Vector2d vector, double x, double y, out Vector2d result) + { + result.X = vector.X - x; + result.Y = vector.Y - y; + } + + public void Multiply(double scalar) + { + X = X * scalar; + Y = Y * scalar; + } + public void Multiply(double scalar, out Vector2d result) + { + result.X = X * scalar; + result.Y = Y * scalar; + } + public static void Multiply(ref Vector2d vector, double scalar, out Vector2d result) + { + result.X = vector.X * scalar; + result.Y = vector.Y * scalar; + } + + public void Multiply(ref Vector2d vector) + { + X = X * vector.X; + Y = Y * vector.Y; + } + public void Multiply(ref Vector2d vector, out Vector2d result) + { + result.X = X * vector.X; + result.Y = Y * vector.Y; + } + public static void Multiply(ref Vector2d left, ref Vector2d right, out Vector2d result) + { + result.X = left.X * right.X; + result.Y = left.Y * right.Y; + } + + public void Multiply(double x, double y) + { + X = X * x; + Y = Y * y; + } + public void Multiply(double x, double y, out Vector2d result) + { + result.X = X * x; + result.Y = Y * y; + } + public static void Multiply(ref Vector2d vector, double x, double y, out Vector2d result) + { + result.X = vector.X * x; + result.Y = vector.Y * y; + } + + public void Divide(double scalar) + { + X = X / scalar; + Y = Y / scalar; + } + public void Divide(double scalar, out Vector2d result) + { + result.X = X / scalar; + result.Y = Y / scalar; + } + public static void Divide(ref Vector2d vector, double scalar, out Vector2d result) + { + result.X = vector.X / scalar; + result.Y = vector.Y / scalar; + } + + public void Divide(ref Vector2d vector) + { + X = X / vector.X; + Y = Y / vector.Y; + } + public void Divide(ref Vector2d vector, out Vector2d result) + { + result.X = X / vector.X; + result.Y = Y / vector.Y; + } + public static void Divide(ref Vector2d left, ref Vector2d right, out Vector2d result) + { + result.X = left.X / right.X; + result.Y = left.Y / right.Y; + } + + public void Divide(double x, double y) + { + X = X / x; + Y = Y / y; + } + public void Divide(double x, double y, out Vector2d result) + { + result.X = X / x; + result.Y = Y / y; + } + public static void Divide(ref Vector2d vector, double x, double y, out Vector2d result) + { + result.X = vector.X / x; + result.Y = vector.Y / y; + } + + #endregion + + + #region Transformations + + public void Transform(ref Matrix4d matrix) + { + double x = matrix.R0C0 * X + matrix.R0C1 * Y; + Y = matrix.R1C0 * X + matrix.R1C1 * Y; + X = x; + } + public void Transform(ref Matrix4d matrix, out Vector2d result) + { + result.X = matrix.R0C0 * X + matrix.R0C1 * Y; + result.Y = matrix.R1C0 * X + matrix.R1C1 * Y; + } + public static void Transform(ref Vector2d vector, ref Matrix4d matrix, out Vector2d result) + { + result.X = matrix.R0C0 * vector.X + matrix.R0C1 * vector.Y; + result.Y = matrix.R1C0 * vector.X + matrix.R1C1 * vector.Y; + } + + public void Translate(ref Vector2d vector) + { + X = X + vector.X; + Y = Y + vector.Y; + } + public void Translate(ref Vector2d vector, out Vector2d result) + { + result.X = X + vector.X; + result.Y = Y + vector.Y; + } + public static void Translate(ref Vector2d left, ref Vector2d right, out Vector2d result) + { + result.X = left.X + right.X; + result.Y = left.Y + right.Y; + } + + public void Translate(double x, double y) + { + X = X + x; + Y = Y + y; + } + public void Translate(double x, double y, out Vector2d result) + { + result.X = X + x; + result.Y = Y + y; + } + public static void Translate(ref Vector2d vector, double x, double y, out Vector2d result) + { + result.X = vector.X + x; + result.Y = vector.Y + y; + } + + public void Scale(ref Vector2d vector) + { + X = X * vector.X; + Y = Y * vector.Y; + } + public void Scale(ref Vector2d vector, out Vector2d result) + { + result.X = X * vector.X; + result.Y = Y * vector.Y; + } + public static void Scale(ref Vector2d left, ref Vector2d right, out Vector2d result) + { + result.X = left.X * right.X; + result.Y = left.Y * right.Y; + } + + public void Scale(double x, double y) + { + X = X * x; + Y = Y * y; + } + public void Scale(double x, double y, out Vector2d result) + { + result.X = X * x; + result.Y = Y * y; + } + public static void Scale(ref Vector2d vector, double x, double y, out Vector2d result) + { + result.X = vector.X * x; + result.Y = vector.Y * y; + } + + public void Rotate(double angle) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + double x = cos * X + sin * Y; + Y = cos * Y - sin * X; + X = x; + } + public void Rotate(double angle, out Vector2d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.X = cos * X + sin * Y; + result.Y = cos * Y - sin * X; + } + public static void Rotate(ref Vector2d vector, double angle, out Vector2d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.X = cos * vector.X + sin * vector.Y; + result.Y = cos * vector.Y - sin * vector.X; + } + + #endregion + + + #region Min & Max + + public void Min(ref Vector2d vector) + { + double lengthSquared = X * X + Y * Y; + double vectorLengthSquared = vector.X * vector.X + vector.Y * vector.Y; + + if (vectorLengthSquared < lengthSquared) + { + X = vector.X; + Y = vector.Y; + } + } + public void Min(ref Vector2d vector, out Vector2d result) + { + double lengthSquared = X * X + Y * Y; + double vectorLengthSquared = vector.X * vector.X + vector.Y * vector.Y; + + if (vectorLengthSquared < lengthSquared) + { + result.X = vector.X; + result.Y = vector.Y; + } + else + { + result.X = X; + result.Y = Y; + } + } + public static void Min(ref Vector2d left, ref Vector2d right, out Vector2d result) + { + double leftLengthSquared = left.X * left.X + left.Y * left.Y; + double rightLengthSquared = right.X * right.X + right.Y * right.Y; + + if (rightLengthSquared < leftLengthSquared) + { + result.X = right.X; + result.Y = right.Y; + } + else + { + result.X = left.X; + result.Y = left.Y; + } + } + + public void Max(ref Vector2d vector) + { + double lengthSquared = X * X + Y * Y; + double vectorLengthSquared = vector.X * vector.X + vector.Y * vector.Y; + + if (vectorLengthSquared > lengthSquared) + { + X = vector.X; + Y = vector.Y; + } + } + public void Max(ref Vector2d vector, out Vector2d result) + { + double lengthSquared = X * X + Y * Y; + double vectorLengthSquared = vector.X * vector.X + vector.Y * vector.Y; + + if (vectorLengthSquared > lengthSquared) + { + result.X = vector.X; + result.Y = vector.Y; + } + else + { + result.X = X; + result.Y = Y; + } + } + public static void Max(ref Vector2d left, ref Vector2d right, out Vector2d result) + { + double leftLengthSquared = left.X * left.X + left.Y * left.Y; + double rightLengthSquared = right.X * right.X + right.Y * right.Y; + + if (rightLengthSquared > leftLengthSquared) + { + result.X = right.X; + result.Y = right.Y; + } + else + { + result.X = left.X; + result.Y = left.Y; + } + } + + public void CoordinateMin(ref Vector2d vector) + { + X = System.Math.Min(X, vector.X); + Y = System.Math.Min(Y, vector.Y); + } + public void CoordinateMin(ref Vector2d vector, out Vector2d result) + { + result.X = System.Math.Min(X, vector.X); + result.Y = System.Math.Min(Y, vector.Y); + } + public static void CoordinateMin(ref Vector2d left, ref Vector2d right, out Vector2d result) + { + result.X = System.Math.Min(left.X, right.X); + result.Y = System.Math.Min(left.Y, right.Y); + } + + public void CoordinateMax(ref Vector2d vector) + { + X = System.Math.Max(X, vector.X); + Y = System.Math.Max(Y, vector.Y); + } + public void CoordinateMax(ref Vector2d vector, out Vector2d result) + { + result.X = System.Math.Max(X, vector.X); + result.Y = System.Math.Max(Y, vector.Y); + } + public static void CoordinateMax(ref Vector2d left, ref Vector2d right, out Vector2d result) + { + result.X = System.Math.Max(left.X, right.X); + result.Y = System.Math.Max(left.Y, right.Y); + } + + public void Clamp(ref Vector2d min, ref Vector2d max) + { + X = System.Math.Max(System.Math.Min(X, min.X), max.X); + Y = System.Math.Max(System.Math.Min(Y, min.Y), max.Y); + } + public void Clamp(ref Vector2d min, ref Vector2d max, out Vector2d result) + { + result.X = System.Math.Max(System.Math.Min(X, min.X), max.X); + result.Y = System.Math.Max(System.Math.Min(Y, min.Y), max.Y); + } + public static void Clamp(ref Vector2d vector, ref Vector2d min, ref Vector2d max, out Vector2d result) + { + result.X = System.Math.Max(System.Math.Min(vector.X, min.X), max.X); + result.Y = System.Math.Max(System.Math.Min(vector.Y, min.Y), max.Y); + } + + #endregion + + + #region Interpolation + + public void Lerp(ref Vector2d end, double blend) + { + X = X + (end.X - X) * blend; + Y = Y + (end.Y - Y) * blend; + } + public void Lerp(ref Vector2d end, double blend, out Vector2d result) + { + result.X = X + (end.X - X) * blend; + result.Y = Y + (end.Y - Y) * blend; + } + public static void Lerp(ref Vector2d start, ref Vector2d end, double blend, out Vector2d result) + { + result.X = start.X + (end.X - start.X) * blend; + result.Y = start.Y + (end.Y - start.Y) * blend; + } + + public void BaryCentric(ref Vector2d endU, ref Vector2d endV, double u, double v) + { + X = X + (endU.X - X) * u + (endV.X - X) * v; + Y = Y + (endU.Y - Y) * u + (endV.Y - Y) * v; + } + public void BaryCentric(ref Vector2d endU, ref Vector2d endV, double u, double v, out Vector2d result) + { + result.X = X + (endU.X - X) * u + (endV.X - X) * v; + result.Y = Y + (endU.Y - Y) * u + (endV.Y - Y) * v; + } + public static void BaryCentric(ref Vector2d start, ref Vector2d endU, ref Vector2d endV, double u, double v, out Vector2d result) + { + result.X = start.X + (endU.X - start.X) * u + (endV.X - start.X) * v; + result.Y = start.Y + (endU.Y - start.Y) * u + (endV.Y - start.Y) * v; + } + + #endregion + + + #region String and Parse + + /// Returns the fully qualified type name of this instance. + /// A System.String containing left fully qualified type name. + public override string ToString() + { + return String.Format("{0} {1} {2}", X, Y); + } + + /// Parse left string to convert it to left vector. + /// The string to parse. + /// The vector represented by the string. + public static void Parse(string str, out Vector2d result) + { + Match match = new Regex(@"(?.*) (?.*)", RegexOptions.None).Match(str); + if (!match.Success) throw new Exception("Parse failed!"); + result.X = double.Parse(match.Result("${x}")); + result.Y = double.Parse(match.Result("${y}")); + } + + #endregion + + + #region HashCode + + /// Returns the hash code for this instance. + /// A 32-bit signed integer that is the hash code for this instance. + public override int GetHashCode() + { + return X.GetHashCode() ^ Y.GetHashCode(); + } + + #endregion + + + #region Constants + + /// A vector representing left zero vector. + public static readonly Vector2d Zero = new Vector2d(0, 0); + + /// A vector with all coordinates set to one. + public static readonly Vector2d One = new Vector2d(1, 1); + + #endregion + } +} \ No newline at end of file diff --git a/Source/OpenTK/Math/Vector3d.cs b/Source/OpenTK/Math/Vector3d.cs new file mode 100644 index 00000000..213f4acb --- /dev/null +++ b/Source/OpenTK/Math/Vector3d.cs @@ -0,0 +1,1216 @@ +using System; +using System.Diagnostics; +using System.Collections.Generic; +using System.Runtime.InteropServices; +using System.Text.RegularExpressions; + +namespace OpenTK.Math +{ + /// A 3-dimensional vector using double-precision floating point numbers. + [Serializable] + [StructLayout(LayoutKind.Sequential)] + public struct Vector3d : IEquatable, IComparer, IComparable + { + #region Fields & Access + + /// The X coordinate of the vector. + public double X; + + /// The Y coordinate of the vector. + public double Y; + + /// The Z coordinate of the vector. + public double Z; + + /// The coordinate at the index of the vector. + public double this[int index] + { + get + { + switch( index ) + { + case 0: + return X; + + case 1: + return Y; + + case 2: + return Z; + } + + throw new IndexOutOfRangeException(); + } + set + { + switch( index ) + { + case 0: + X = value; + return; + + case 1: + Y = value; + return; + + case 2: + Z = value; + return; + } + + throw new IndexOutOfRangeException(); + } + } + + /// Converts the vector into an array of double-precision floating point numbers. + /// The vector being converted. + /// An array of double-precision floating point numbers representing the vector coordinates. + public static explicit operator double[](Vector3d vector) + { + return new double[3]{vector.X, vector.Y, vector.Z}; + } + + /// Converts the vector into left double-precision floating point number pointer. + /// The vector being converted. + /// A double-precision floating point number pointer to the vector coordinates. + unsafe public static explicit operator double*(Vector3d vector) + { + return &vector.X; + } + + /// Converts the vector into an IntPtr. + /// The vector being converted. + /// An IntPtr to the vector coordinates. + public static explicit operator IntPtr(Vector3d vector) + { + unsafe + { + return (IntPtr)(&vector.X); + } + } + + #endregion + + + #region Constructors + + /// Constructs left vector with the given coordinates. + /// The X coordinate. + /// The Y coordinate. + /// The Z coordinate. + public Vector3d(double x, double y, double z) + { + this.X = x; + this.Y = y; + this.Z = z; + } + + /// Constructs left vector with the same coordinates as the given vector. + /// The vector whose coordinates to copy. + public Vector3d(ref Vector2d vector) + { + this.X = vector.X; + this.Y = vector.Y; + this.Z = 0; + } + + /// Constructs left vector with the same coordinates as the given vector. + /// The vector whose coordinates to copy. + public Vector3d(ref Vector3d vector) + { + this.X = vector.X; + this.Y = vector.Y; + this.Z = vector.Z; + } + + /// Constructs left vector from the given array of double-precision floating point numbers. + /// The array of doubles for the coordinates of the vector. + public Vector3d(double[] coordinateArray) + { + if( coordinateArray == null || coordinateArray.GetLength(0) < 3 ) throw new Exception("Invalid parameter."); + + this.X = coordinateArray[0]; + this.Y = coordinateArray[1]; + this.Z = coordinateArray[2]; + } + + #endregion + + + #region Equality + + /// Indicates whether the current vector is equal to another vector. + /// An vector to compare with this vector. + /// true if the current vector is equal to the vector parameter; otherwise, false. + public bool Equals(Vector3d vector) + { + return + X == vector.X && + Y == vector.Y && + Z == vector.Z; + } + + /// Indicates whether the current vector is equal to another vector. + /// An vector to compare with this vector. + /// true if the current vector is equal to the vector parameter; otherwise, false. + public bool Equals(ref Vector3d vector) + { + return + X == vector.X && + Y == vector.Y && + Z == vector.Z; + } + + /// Indicates whether two vectors are approximately equal to each other. + /// The first vector. + /// The second vector. + /// true if the vectors are approximately equal; otherwise, false. + public static bool Equals(ref Vector3d left, ref Vector3d right) + { + return + left.X == right.X && + left.Y == right.Y && + left.Z == right.Z; + } + + /// Indicates whether the current vector is equal to another vector. + /// An vector to compare with this vector. + /// true if the current vector is equal to the vector parameter; otherwise, false. + public bool EqualsApprox(ref Vector3d vector, double tolerance) + { + return + System.Math.Abs(X - vector.X) <= tolerance && + System.Math.Abs(Y - vector.Y) <= tolerance && + System.Math.Abs(Z - vector.Z) <= tolerance; + } + + /// Indicates whether two vectors are approximately equal to each other within left given tolerance. + /// The first vector. + /// The second vector. + /// The tolerance for the approximation. + /// true if the vectors are approximately equal; otherwise, false. + public static bool EqualsApprox(ref Vector3d left, ref Vector3d right, double tolerance) + { + return + System.Math.Abs(left.X - right.X) <= tolerance && + System.Math.Abs(left.Y - right.Y) <= tolerance && + System.Math.Abs(left.Z - right.Z) <= tolerance; + } + + #endregion + + + #region IComparer + + /// Compares two vectors and returns left value indicating whether one is less than, equal to, or greater than the other. + public int Compare(Vector3d left, Vector3d right) + { + if (left.X != right.X) + { + if (left.X < right.X) return -1; + else return 1; + } + else if (left.Y != right.Y) + { + if (left.Y < right.Y) return -1; + else return 1; + } + else if (left.Z != right.Z) + { + if (left.Z < right.Z) return -1; + else return 1; + } + + return 0; + } + + #endregion + + + #region IComparable + + /// Compares the vector with another vector and returns left value indicating whether it is less than, equal to, or greater than the other vector. + public int CompareTo(Vector3d vector) { return Compare(this, vector); } + + #endregion + + + #region Length + + /// Gets the length of the vector. + public double Length + { + get + { + double lengthSquared = LengthSquared; + + if (lengthSquared == 1) + { + return 1; + } + else + { + return System.Math.Sqrt(lengthSquared); + } + } + } + + /// Gets the squared length of the vector. + public double LengthSquared + { + get + { + return X * X + Y * Y + Z * Z; + } + } + + /// Gets the approimate length of the vector. + public double LengthApprox + { + get + { + return 1.0d / Functions.InverseSqrtFast(X * X + Y * Y + Z * Z); + } + } + + #endregion + + + #region Distance + + /// Gets the distance from this vector to the given vector. + /// The vector to which to find the distance. + /// The distance from this vector to the given vector. + public double DistanceTo(ref Vector3d vector) + { + double deltaX = vector.X - X; + double deltaY = vector.Y - Y; + double deltaZ = vector.Z - Z; + return System.Math.Sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ); + } + + /// Gets the squared distance from this vector to the given vector. + /// The vector to which to find the squared distance. + /// The squared distance from this vector to the given vector. + public double DistanceSquaredTo(ref Vector3d vector) + { + double deltaX = vector.X - X; + double deltaY = vector.Y - Y; + double deltaZ = vector.Z - Z; + return deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ; + } + + /// Gets the approximate distance from this vector to the given vector. + /// The vector to which to find the approximate distance. + /// The approximate distance from this vector to the given vector. + public double DistanceApproxTo(ref Vector3d vector) + { + double deltaX = vector.X - X; + double deltaY = vector.Y - Y; + double deltaZ = vector.Z - Z; + return 1.0d / Functions.InverseSqrtFast(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ); + } + + #endregion + + + #region Normalize + + /// Normalize this vector. + public void Normalize() + { + double lengthSquared = LengthSquared; + + if (lengthSquared != 1 && lengthSquared != 0) + { + double length = System.Math.Sqrt(lengthSquared); + X = X / length; + Y = Y / length; + Z = Z / length; + } + } + + /// Get the normalized version of this vector. + /// The resulting normalized vector. + public void Normalize(out Vector3d result) + { + double lengthSquared = LengthSquared; + + if (lengthSquared == 1) + { + result.X = X; + result.Y = Y; + result.Z = Z; + } + else if (lengthSquared == 0) + { + result.X = 0; + result.Y = 0; + result.Z = 0; + } + else + { + double length = System.Math.Sqrt(lengthSquared); + result.X = X / length; + result.Y = Y / length; + result.Z = Z / length; + } + } + + public static void Normalize(ref Vector3d vector, out Vector3d result) + { + double lengthSquared = vector.LengthSquared; + + if (lengthSquared == 1) + { + result.X = vector.X; + result.Y = vector.Y; + result.Z = vector.Z; + } + else if (lengthSquared == 0) + { + result.X = 0; + result.Y = 0; + result.Z = 0; + } + else + { + double length = System.Math.Sqrt(lengthSquared); + result.X = vector.X / length; + result.Y = vector.Y / length; + result.Z = vector.Z / length; + } + } + + public void NormalizeApprox() + { + double inverseSquare = Functions.InverseSqrtFast(X * X + Y * Y + Z * Z); + X = X * inverseSquare; + Y = Y * inverseSquare; + Z = Z * inverseSquare; + } + + /// Gets left approximately normalized vector of the vector. + public void NormalizedApprox(out Vector3d result) + { + double inverseSquare = Functions.InverseSqrtFast(X * X + Y * Y + Z * Z); + result.X = X * inverseSquare; + result.Y = Y * inverseSquare; + result.Z = Z * inverseSquare; + } + + public static void NormalizeApprox(ref Vector3d vector, out Vector3d result) + { + double inverseSquare = Functions.InverseSqrtFast(vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z); + result.X = vector.X * inverseSquare; + result.Y = vector.Y * inverseSquare; + result.Z = vector.Z * inverseSquare; + } + + #endregion + + + /// Gets the dot product of two vectors. + /// The first vector. + /// The second vector. + /// The dot product of two vectors. + public static double DotProduct(ref Vector3d left, ref Vector3d right) + { + return left.X * right.X + left.Y * right.Y + left.Z * right.Z; + } + + /// Gets the cross product of two vectors. + /// The first vector. + /// The second vector. + /// The cross product of two vectors. + public static void CrossProduct(ref Vector3d left, ref Vector3d right, out Vector3d result) + { + result.X = left.Y * right.Z - left.Z * right.Y; + result.Y = left.Z * right.X - left.X * right.Z; + result.Z = left.X * right.Y - left.Y * right.X; + } + + #region Abs + + public void Abs() + { + X = System.Math.Abs(X); + Y = System.Math.Abs(Y); + Z = System.Math.Abs(Z); + } + public void Abs(out Vector3d result) + { + result.X = System.Math.Abs(X); + result.Y = System.Math.Abs(Y); + result.Z = System.Math.Abs(Z); + } + public static void Abs(ref Vector3d vector, out Vector3d result) + { + result.X = System.Math.Abs(vector.X); + result.Y = System.Math.Abs(vector.Y); + result.Z = System.Math.Abs(vector.Z); + } + + #endregion + + + #region Inverse + + public void Inverse() + { + X = -X; + Y = -Y; + Z = -Z; + } + public void Inverse(out Vector3d result) + { + result.X = -X; + result.Y = -Y; + result.Z = -Z; + } + public static void Inverse(ref Vector3d vector, out Vector3d result) + { + result.X = -vector.X; + result.Y = -vector.Y; + result.Z = -vector.Z; + } + + #endregion + + + #region Arithmatic + + public void Add(ref Vector3d vector) + { + X = X + vector.X; + Y = Y + vector.Y; + Z = Z + vector.Z; + } + public void Add(ref Vector3d vector, out Vector3d result) + { + result.X = X + vector.X; + result.Y = Y + vector.Y; + result.Z = Z + vector.Z; + } + public static void Add(ref Vector3d left, ref Vector3d right, out Vector3d result) + { + result.X = left.X + right.X; + result.Y = left.Y + right.Y; + result.Z = left.Z + right.Z; + } + + public void Add(double x, double y, double z) + { + X = X + x; + Y = Y + y; + Z = Z + z; + } + public void Add(double x, double y, double z, out Vector3d result) + { + result.X = X + x; + result.Y = Y + y; + result.Z = Z + z; + } + public static void Add(ref Vector3d vector, double x, double y, double z, out Vector3d result) + { + result.X = vector.X + x; + result.Y = vector.Y + y; + result.Z = vector.Z + z; + } + + public void Subtract(ref Vector3d vector) + { + X = X - vector.X; + Y = Y - vector.Y; + Z = Z - vector.Z; + } + public void Subtract(ref Vector3d vector, out Vector3d result) + { + result.X = X - vector.X; + result.Y = Y - vector.Y; + result.Z = Z - vector.Z; + } + public static void Subtract(ref Vector3d left, ref Vector3d right, out Vector3d result) + { + result.X = left.X - right.X; + result.Y = left.Y - right.Y; + result.Z = left.Z - right.Z; + } + + public void Subtract(double x, double y, double z) + { + X = X - x; + Y = Y - y; + Z = Z - z; + } + public void Subtract(double x, double y, double z, out Vector3d result) + { + result.X = X - x; + result.Y = Y - y; + result.Z = Z - z; + } + public static void Subtract(ref Vector3d vector, double x, double y, double z, out Vector3d result) + { + result.X = vector.X - x; + result.Y = vector.Y - y; + result.Z = vector.Z - z; + } + + public void Multiply(double scalar) + { + X = X * scalar; + Y = Y * scalar; + Z = Z * scalar; + } + public void Multiply(double scalar, out Vector3d result) + { + result.X = X * scalar; + result.Y = Y * scalar; + result.Z = Z * scalar; + } + public static void Multiply(ref Vector3d vector, double scalar, out Vector3d result) + { + result.X = vector.X * scalar; + result.Y = vector.Y * scalar; + result.Z = vector.Z * scalar; + } + + public void Multiply(ref Vector3d vector) + { + X = X * vector.X; + Y = Y * vector.Y; + Z = Z * vector.Z; + } + public void Multiply(ref Vector3d vector, out Vector3d result) + { + result.X = X * vector.X; + result.Y = Y * vector.Y; + result.Z = Z * vector.Z; + } + public static void Multiply(ref Vector3d left, ref Vector3d right, out Vector3d result) + { + result.X = left.X * right.X; + result.Y = left.Y * right.Y; + result.Z = left.Z * right.Z; + } + + public void Multiply(double x, double y, double z) + { + X = X * x; + Y = Y * y; + Z = Z * z; + } + public void Multiply(double x, double y, double z, out Vector3d result) + { + result.X = X * x; + result.Y = Y * y; + result.Z = Z * z; + } + public static void Multiply(ref Vector3d vector, double x, double y, double z, out Vector3d result) + { + result.X = vector.X * x; + result.Y = vector.Y * y; + result.Z = vector.Z * z; + } + + public void Divide(double scalar) + { + X = X / scalar; + Y = Y / scalar; + Z = X / scalar; + } + public void Divide(double scalar, out Vector3d result) + { + result.X = X / scalar; + result.Y = Y / scalar; + result.Z = X / scalar; + } + public static void Divide(ref Vector3d vector, double scalar, out Vector3d result) + { + result.X = vector.X / scalar; + result.Y = vector.Y / scalar; + result.Z = vector.Z / scalar; + } + + public void Divide(ref Vector3d vector) + { + X = X / vector.X; + Y = Y / vector.Y; + Z = Z / vector.Z; + } + public void Divide(ref Vector3d vector, out Vector3d result) + { + result.X = X / vector.X; + result.Y = Y / vector.Y; + result.Z = Z / vector.Z; + } + public static void Divide(ref Vector3d left, ref Vector3d right, out Vector3d result) + { + result.X = left.X / right.X; + result.Y = left.Y / right.Y; + result.Z = left.Z / right.Z; + } + + public void Divide(double x, double y, double z) + { + X = X / x; + Y = Y / y; + Z = Z / z; + } + public void Divide(double x, double y, double z, out Vector3d result) + { + result.X = X / x; + result.Y = Y / y; + result.Z = Z / z; + } + public static void Divide(ref Vector3d vector, double x, double y, double z, out Vector3d result) + { + result.X = vector.X / x; + result.Y = vector.Y / y; + result.Z = vector.Z / z; + } + + #endregion + + + #region Transformations + + public void Transform(ref Matrix4d matrix) + { + double x = matrix.R0C0 * X + matrix.R0C1 * Y + matrix.R0C2 * Z; + double y = matrix.R1C0 * X + matrix.R1C1 * Y + matrix.R1C2 * Z; + Z = matrix.R2C0 * X + matrix.R2C1 * Y + matrix.R2C2 * Z; + X = x; + Y = y; + } + public void Transform(ref Matrix4d matrix, out Vector3d result) + { + result.X = matrix.R0C0 * X + matrix.R0C1 * Y + matrix.R0C2 * Z; + result.Y = matrix.R1C0 * X + matrix.R1C1 * Y + matrix.R1C2 * Z; + result.Z = matrix.R2C0 * X + matrix.R2C1 * Y + matrix.R2C2 * Z; + } + public static void Transform(ref Vector3d vector, ref Matrix4d matrix, out Vector3d result) + { + result.X = matrix.R0C0 * vector.X + matrix.R0C1 * vector.Y + matrix.R0C2 * vector.Z; + result.Y = matrix.R1C0 * vector.X + matrix.R1C1 * vector.Y + matrix.R1C2 * vector.Z; + result.Z = matrix.R2C0 * vector.X + matrix.R2C1 * vector.Y + matrix.R2C2 * vector.Z; + } + + public void Translate(ref Vector3d vector) + { + X = X + vector.X; + Y = Y + vector.Y; + Z = Z + vector.Z; + } + public void Translate(ref Vector3d vector, out Vector3d result) + { + result.X = X + vector.X; + result.Y = Y + vector.Y; + result.Z = Z + vector.Z; + } + public static void Translate(ref Vector3d left, ref Vector3d right, out Vector3d result) + { + result.X = left.X + right.X; + result.Y = left.Y + right.Y; + result.Z = left.Z + right.Z; + } + + public void Translate(double x, double y, double z) + { + X = X + x; + Y = Y + y; + Z = Z + z; + } + public void Translate(double x, double y, double z, out Vector3d result) + { + result.X = X + x; + result.Y = Y + y; + result.Z = Z + z; + } + public static void Translate(ref Vector3d vector, double x, double y, double z, out Vector3d result) + { + result.X = vector.X + x; + result.Y = vector.Y + y; + result.Z = vector.Z + z; + } + + public void Scale(ref Vector3d vector) + { + X = X * vector.X; + Y = Y * vector.Y; + Z = Z * vector.Z; + } + public void Scale(ref Vector3d vector, out Vector3d result) + { + result.X = X * vector.X; + result.Y = Y * vector.Y; + result.Z = Z * vector.Z; + } + public static void Scale(ref Vector3d left, ref Vector3d right, out Vector3d result) + { + result.X = left.X * right.X; + result.Y = left.Y * right.Y; + result.Z = left.Z * right.Z; + } + + public void Scale(double x, double y, double z) + { + X = X * x; + Y = Y * y; + Z = Z * z; + } + public void Scale(double x, double y, double z, out Vector3d result) + { + result.X = X * x; + result.Y = Y * y; + result.Z = Z * z; + } + public static void Scale(ref Vector3d vector, double x, double y, double z, out Vector3d result) + { + result.X = vector.X * x; + result.Y = vector.Y * y; + result.Z = vector.Z * z; + } + + public void RotateX(double angle) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + double y = cos * Y + sin * Z; + Z = cos * Z - sin * Y; + Y = y; + } + public void RotateX(double angle, out Vector3d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.X = X; + result.Y = cos * Y + sin * Z; + result.Z = cos * Z - sin * Y; + } + public static void RotateX(ref Vector3d vector, double angle, out Vector3d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.X = vector.X; + result.Y = cos * vector.Y + sin * vector.Z; + result.Z = cos * vector.Z - sin * vector.Y; + } + + public void RotateY(double angle) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + double x = cos * X - sin * Z; + Z = sin * X + cos * Z; + X = x; + } + public void RotateY(double angle, out Vector3d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.X = cos * X - sin * Z; + result.Y = Y; + result.Z = sin * X + cos * Z; + } + public static void RotateY(ref Vector3d vector, double angle, out Vector3d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.X = cos * vector.X - sin * vector.Z; + result.Y = vector.Y; + result.Z = sin * vector.X + cos * vector.Z; + } + + public void RotateZ(double angle) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + double x = cos * X + sin * Y; + Y = cos * Y - sin * X; + X = x; + } + public void RotateZ(double angle, out Vector3d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.X = cos * X + sin * Y; + result.Y = cos * Y - sin * X; + result.Z = Z; + } + public static void RotateZ(ref Vector3d vector, double angle, out Vector3d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.X = cos * vector.X + sin * vector.Y; + result.Y = cos * vector.Y - sin * vector.X; + result.Z = vector.Z; + } + + public void Rotate(ref Vector3d axis, double angle) + { + Vector3d axisNormalized; + axis.Normalize(out axisNormalized); + double x = axisNormalized.X; + double y = axisNormalized.Y; + double z = axisNormalized.Z; + double angleRadians = Functions.DTOR * angle; + double cos = (double)System.Math.Cos(angleRadians); + double sin = (double)System.Math.Sin(angleRadians); + double oneMinusCos = 1 - cos; + double xOneMinusCos = x * oneMinusCos; + double yOneMinusCos = y * oneMinusCos; + double zOneMinusCos = z * oneMinusCos; + double xxOneMinusCos = x * xOneMinusCos; + double xyOneMinusCos = x * yOneMinusCos; + double xzOneMinusCos = x * zOneMinusCos; + double yyOneMinusCos = y * yOneMinusCos; + double yzOneMinusCos = y * zOneMinusCos; + double zzOneMinusCos = z * zOneMinusCos; + double xSin = x * sin; + double ySin = y * sin; + double zSin = z * sin; + + double tx = (xxOneMinusCos + cos) * X + (xyOneMinusCos + zSin) * Y + (xzOneMinusCos - ySin) * Z; + double ty = (xyOneMinusCos - zSin) * X + (yyOneMinusCos + cos) * Y + (yzOneMinusCos + xSin) * Z; + Z = (xzOneMinusCos + ySin) * X + (yzOneMinusCos - xSin) * Y + (zzOneMinusCos + cos) * Z; + X = tx; + Y = ty; + } + public void Rotate(ref Vector3d axis, double angle, out Vector3d result) + { + Vector3d axisNormalized; + axis.Normalize(out axisNormalized); + double x = axisNormalized.X; + double y = axisNormalized.Y; + double z = axisNormalized.Z; + double angleRadians = Functions.DTOR * angle; + double cos = (double)System.Math.Cos(angleRadians); + double sin = (double)System.Math.Sin(angleRadians); + double oneMinusCos = 1 - cos; + double xOneMinusCos = x * oneMinusCos; + double yOneMinusCos = y * oneMinusCos; + double zOneMinusCos = z * oneMinusCos; + double xxOneMinusCos = x * xOneMinusCos; + double xyOneMinusCos = x * yOneMinusCos; + double xzOneMinusCos = x * zOneMinusCos; + double yyOneMinusCos = y * yOneMinusCos; + double yzOneMinusCos = y * zOneMinusCos; + double zzOneMinusCos = z * zOneMinusCos; + double xSin = x * sin; + double ySin = y * sin; + double zSin = z * sin; + + result.X = (xxOneMinusCos + cos) * X + (xyOneMinusCos + zSin) * Y + (xzOneMinusCos - ySin) * Z; + result.Y = (xyOneMinusCos - zSin) * X + (yyOneMinusCos + cos) * Y + (yzOneMinusCos + xSin) * Z; + result.Z = (xzOneMinusCos + ySin) * X + (yzOneMinusCos - xSin) * Y + (zzOneMinusCos + cos) * Z; + } + public static void Rotate(ref Vector3d vector, ref Vector3d axis, double angle, out Vector3d result) + { + Vector3d axisNormalized; + axis.Normalize(out axisNormalized); + double x = axisNormalized.X; + double y = axisNormalized.Y; + double z = axisNormalized.Z; + double angleRadians = Functions.DTOR * angle; + double cos = (double)System.Math.Cos(angleRadians); + double sin = (double)System.Math.Sin(angleRadians); + double oneMinusCos = 1 - cos; + double xOneMinusCos = x * oneMinusCos; + double yOneMinusCos = y * oneMinusCos; + double zOneMinusCos = z * oneMinusCos; + double xxOneMinusCos = x * xOneMinusCos; + double xyOneMinusCos = x * yOneMinusCos; + double xzOneMinusCos = x * zOneMinusCos; + double yyOneMinusCos = y * yOneMinusCos; + double yzOneMinusCos = y * zOneMinusCos; + double zzOneMinusCos = z * zOneMinusCos; + double xSin = x * sin; + double ySin = y * sin; + double zSin = z * sin; + + result.X = (xxOneMinusCos + cos) * vector.X + (xyOneMinusCos + zSin) * vector.Y + (xzOneMinusCos - ySin) * vector.Z; + result.Y = (xyOneMinusCos - zSin) * vector.X + (yyOneMinusCos + cos) * vector.Y + (yzOneMinusCos + xSin) * vector.Z; + result.Z = (xzOneMinusCos + ySin) * vector.X + (yzOneMinusCos - xSin) * vector.Y + (zzOneMinusCos + cos) * vector.Z; + } + + #endregion + + + #region Min & Max + + public void Min(ref Vector3d vector) + { + double lengthSquared = X * X + Y * Y + Z * Z; + double vectorLengthSquared = vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z; + + if (vectorLengthSquared < lengthSquared) + { + X = vector.X; + Y = vector.Y; + Z = vector.Z; + } + } + public void Min(ref Vector3d vector, out Vector3d result) + { + double lengthSquared = X * X + Y * Y + Z * Z; + double vectorLengthSquared = vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z; + + if (vectorLengthSquared < lengthSquared) + { + result.X = vector.X; + result.Y = vector.Y; + result.Z = vector.Z; + } + else + { + result.X = X; + result.Y = Y; + result.Z = Z; + } + } + public static void Min(ref Vector3d left, ref Vector3d right, out Vector3d result) + { + double leftLengthSquared = left.X * left.X + left.Y * left.Y + left.Z * left.Z; + double rightLengthSquared = right.X * right.X + right.Y * right.Y + right.Z * right.Z; + + if (rightLengthSquared < leftLengthSquared) + { + result.X = right.X; + result.Y = right.Y; + result.Z = right.Z; + } + else + { + result.X = left.X; + result.Y = left.Y; + result.Z = left.Z; + } + } + + public void Max(ref Vector3d vector) + { + double lengthSquared = X * X + Y * Y + Z * Z; + double vectorLengthSquared = vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z; + + if (vectorLengthSquared > lengthSquared) + { + X = vector.X; + Y = vector.Y; + Z = vector.Z; + } + } + public void Max(ref Vector3d vector, out Vector3d result) + { + double lengthSquared = X * X + Y * Y + Z * Z; + double vectorLengthSquared = vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z; + + if (vectorLengthSquared > lengthSquared) + { + result.X = vector.X; + result.Y = vector.Y; + result.Z = vector.Z; + } + else + { + result.X = X; + result.Y = Y; + result.Z = Z; + } + } + public static void Max(ref Vector3d left, ref Vector3d right, out Vector3d result) + { + double leftLengthSquared = left.X * left.X + left.Y * left.Y + left.Z * left.Z; + double rightLengthSquared = right.X * right.X + right.Y * right.Y + right.Z * right.Z; + + if (rightLengthSquared > leftLengthSquared) + { + result.X = right.X; + result.Y = right.Y; + result.Z = right.Z; + } + else + { + result.X = left.X; + result.Y = left.Y; + result.Z = left.Z; + } + } + + public void CoordinateMin(ref Vector3d vector) + { + X = System.Math.Min(X, vector.X); + Y = System.Math.Min(Y, vector.Y); + Z = System.Math.Min(Z, vector.Z); + } + public void CoordinateMin(ref Vector3d vector, out Vector3d result) + { + result.X = System.Math.Min(X, vector.X); + result.Y = System.Math.Min(Y, vector.Y); + result.Z = System.Math.Min(Z, vector.Z); + } + public static void CoordinateMin(ref Vector3d left, ref Vector3d right, out Vector3d result) + { + result.X = System.Math.Min(left.X, right.X); + result.Y = System.Math.Min(left.Y, right.Y); + result.Z = System.Math.Min(left.Z, right.Z); + } + + public void CoordinateMax(ref Vector3d vector) + { + X = System.Math.Max(X, vector.X); + Y = System.Math.Max(Y, vector.Y); + Z = System.Math.Max(Z, vector.Z); + } + public void CoordinateMax(ref Vector3d vector, out Vector3d result) + { + result.X = System.Math.Max(X, vector.X); + result.Y = System.Math.Max(Y, vector.Y); + result.Z = System.Math.Max(Z, vector.Z); + } + public static void CoordinateMax(ref Vector3d left, ref Vector3d right, out Vector3d result) + { + result.X = System.Math.Max(left.X, right.X); + result.Y = System.Math.Max(left.Y, right.Y); + result.Z = System.Math.Max(left.Z, right.Z); + } + + public void Clamp(ref Vector3d min, ref Vector3d max) + { + X = System.Math.Max(System.Math.Min(X, min.X), max.X); + Y = System.Math.Max(System.Math.Min(Y, min.Y), max.Y); + Z = System.Math.Max(System.Math.Min(Z, min.Z), max.Z); + } + public void Clamp(ref Vector3d min, ref Vector3d max, out Vector3d result) + { + result.X = System.Math.Max(System.Math.Min(X, min.X), max.X); + result.Y = System.Math.Max(System.Math.Min(Y, min.Y), max.Y); + result.Z = System.Math.Max(System.Math.Min(Z, min.Z), max.Z); + } + public static void Clamp(ref Vector3d vector, ref Vector3d min, ref Vector3d max, out Vector3d result) + { + result.X = System.Math.Max(System.Math.Min(vector.X, min.X), max.X); + result.Y = System.Math.Max(System.Math.Min(vector.Y, min.Y), max.Y); + result.Z = System.Math.Max(System.Math.Min(vector.Z, min.Z), max.Z); + } + + #endregion + + + #region Interpolation + + public void Lerp(ref Vector3d end, double blend) + { + X = X + (end.X - X) * blend; + Y = Y + (end.Y - Y) * blend; + Z = Z + (end.Z - Z) * blend; + } + public void Lerp(ref Vector3d end, double blend, out Vector3d result) + { + result.X = X + (end.X - X) * blend; + result.Y = Y + (end.Y - Y) * blend; + result.Z = Z + (end.Z - Z) * blend; + } + public static void Lerp(ref Vector3d start, ref Vector3d end, double blend, out Vector3d result) + { + result.X = start.X + (end.X - start.X) * blend; + result.Y = start.Y + (end.Y - start.Y) * blend; + result.Z = start.Z + (end.Z - start.Z) * blend; + } + + public void BaryCentric(ref Vector3d endU, ref Vector3d endV, double u, double v) + { + X = X + (endU.X - X) * u + (endV.X - X) * v; + Y = Y + (endU.Y - Y) * u + (endV.Y - Y) * v; + Z = Z + (endU.Z - Z) * u + (endV.Z - Z) * v; + } + public void BaryCentric(ref Vector3d endU, ref Vector3d endV, double u, double v, out Vector3d result) + { + result.X = X + (endU.X - X) * u + (endV.X - X) * v; + result.Y = Y + (endU.Y - Y) * u + (endV.Y - Y) * v; + result.Z = Z + (endU.Z - Z) * u + (endV.Z - Z) * v; + } + public static void BaryCentric(ref Vector3d start, ref Vector3d endU, ref Vector3d endV, double u, double v, out Vector3d result) + { + result.X = start.X + (endU.X - start.X) * u + (endV.X - start.X) * v; + result.Y = start.Y + (endU.Y - start.Y) * u + (endV.Y - start.Y) * v; + result.Z = start.Z + (endU.Z - start.Z) * u + (endV.Z - start.Z) * v; + } + + #endregion + + + #region String and Parse + + /// Returns the fully qualified type name of this instance. + /// A System.String containing left fully qualified type name. + public override string ToString() + { + return String.Format("{0} {1} {2}", X, Y, Z); + } + + /// Parse left string to convert it to left vector. + /// The string to parse. + /// The vector represented by the string. + public static void Parse(string str, out Vector3d result) + { + Match match = new Regex(@"(?.*) (?.*) (?.*)", RegexOptions.None).Match(str); + if (!match.Success) throw new Exception("Parse failed!"); + result.X = double.Parse(match.Result("${x}")); + result.Y = double.Parse(match.Result("${y}")); + result.Z = double.Parse(match.Result("${z}")); + } + + #endregion + + + #region HashCode + + /// Returns the hash code for this instance. + /// A 32-bit signed integer that is the hash code for this instance. + public override int GetHashCode() + { + return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode(); + } + + #endregion + + + #region Constants + + /// A vector representing left zero vector. + public static readonly Vector3d Zero = new Vector3d(0, 0, 0); + + /// A vector with all coordinates set to one. + public static readonly Vector3d One = new Vector3d(1, 1, 1); + + /// A unit normal vector representing the positive X Axis. + public static readonly Vector3d XAxis = new Vector3d(1, 0, 0); + + /// A unit normal vector representing the positive Y Axis. + public static readonly Vector3d YAxis = new Vector3d(0, 1, 0); + + /// A unit normal vector representing the positive Z Axis. + public static readonly Vector3d ZAxis = new Vector3d(0, 0, 1); + + #endregion + } +} \ No newline at end of file diff --git a/Source/OpenTK/Math/Vector4d.cs b/Source/OpenTK/Math/Vector4d.cs new file mode 100644 index 00000000..3785fcb8 --- /dev/null +++ b/Source/OpenTK/Math/Vector4d.cs @@ -0,0 +1,1343 @@ +using System; +using System.Diagnostics; +using System.Collections.Generic; +using System.Runtime.InteropServices; +using System.Text.RegularExpressions; + +namespace OpenTK.Math +{ + /// A 4-dimensional vector using double-precision floating point numbers. + [Serializable] + [StructLayout(LayoutKind.Sequential)] + public struct Vector4d : IEquatable, IComparer, IComparable + { + #region Fields & Access + + /// The X coordinate of the vector. + public double X; + + /// The Y coordinate of the vector. + public double Y; + + /// The Z coordinate of the vector. + public double Z; + + /// The W coordinate of the vector. + public double W; + + /// The coordinate at the index of the vector. + public double this[int index] + { + get + { + switch( index ) + { + case 0: + return X; + + case 1: + return Y; + + case 2: + return Z; + + case 3: + return W; + } + + throw new IndexOutOfRangeException(); + } + set + { + switch( index ) + { + case 0: + X = value; + return; + + case 1: + Y = value; + return; + + case 2: + Z = value; + return; + + case 3: + W = value; + return; + } + + throw new IndexOutOfRangeException(); + } + } + + /// Converts the vector into an array of double-precision floating point numbers. + /// The vector being converted. + /// An array of double-precision floating point numbers representing the vector coordinates. + public static explicit operator double[](Vector4d vector) + { + return new double[4]{vector.X, vector.Y, vector.Z, vector.W}; + } + + /// Converts the vector into left double-precision floating point number pointer. + /// The vector being converted. + /// A double-precision floating point number pointer to the vector coordinates. + unsafe public static explicit operator double*(Vector4d vector) + { + return &vector.X; + } + + /// Converts the vector into an IntPtr. + /// The vector being converted. + /// An IntPtr to the vector coordinates. + public static explicit operator IntPtr(Vector4d vector) + { + unsafe + { + return (IntPtr)(&vector.X); + } + } + + #endregion + + + #region Constructors + + /// Constructs left vector with the given coordinates. + /// The X coordinate. + /// The Y coordinate. + /// The Z coordinate. + /// The W coordinate. + public Vector4d(double x, double y, double z, double w) + { + this.X = x; + this.Y = y; + this.Z = z; + this.W = w; + } + + /// Constructs left vector with the same coordinates as the given vector. + /// The vector whose coordinates to copy. + public Vector4d(ref Vector2d vector) + { + this.X = vector.X; + this.Y = vector.Y; + this.Z = 0; + this.W = 0; + } + + /// Constructs left vector with the same coordinates as the given vector. + /// The vector whose coordinates to copy. + public Vector4d(ref Vector3d vector) + { + this.X = vector.X; + this.Y = vector.Y; + this.Z = vector.Z; + this.W = 0; + } + + /// Constructs left vector with the same coordinates as the given vector. + /// The vector whose coordinates to copy. + public Vector4d(ref Vector4d vector) + { + this.X = vector.X; + this.Y = vector.Y; + this.Z = vector.Z; + this.W = vector.W; + } + + /// Constructs left vector from the given array of double-precision floating point numbers. + /// The array of doubles for the coordinates of the vector. + public Vector4d(double[] coordinateArray) + { + if( coordinateArray == null || coordinateArray.GetLength(0) < 4 ) throw new Exception("Invalid parameter."); + + this.X = coordinateArray[0]; + this.Y = coordinateArray[1]; + this.Z = coordinateArray[2]; + this.W = coordinateArray[3]; + } + + #endregion + + + #region Equality + + /// Indicates whether the current vector is equal to another vector. + /// An vector to compare with this vector. + /// true if the current vector is equal to the vector parameter; otherwise, false. + public bool Equals(Vector4d vector) + { + return + X == vector.X && + Y == vector.Y && + Z == vector.Z && + W == vector.W; + } + + /// Indicates whether the current vector is equal to another vector. + /// An vector to compare with this vector. + /// true if the current vector is equal to the vector parameter; otherwise, false. + public bool Equals(ref Vector4d vector) + { + return + X == vector.X && + Y == vector.Y && + Z == vector.Z && + W == vector.W; + } + + /// Indicates whether two vectors are approximately equal to each other. + /// The first vector. + /// The second vector. + /// true if the vectors are approximately equal; otherwise, false. + public static bool Equals(ref Vector4d left, ref Vector4d right) + { + return + left.X == right.X && + left.Y == right.Y && + left.Z == right.Z && + left.W == right.W; + } + + /// Indicates whether the current vector is equal to another vector. + /// An vector to compare with this vector. + /// true if the current vector is equal to the vector parameter; otherwise, false. + public bool EqualsApprox(ref Vector4d vector, double tolerance) + { + return + System.Math.Abs(X - vector.X) <= tolerance && + System.Math.Abs(Y - vector.Y) <= tolerance && + System.Math.Abs(Z - vector.Z) <= tolerance && + System.Math.Abs(W - vector.W) <= tolerance; + } + + /// Indicates whether two vectors are approximately equal to each other within left given tolerance. + /// The first vector. + /// The second vector. + /// The tolerance for the approximation. + /// true if the vectors are approximately equal; otherwise, false. + public static bool EqualsApprox(ref Vector4d left, ref Vector4d right, double tolerance) + { + return + System.Math.Abs(left.X - right.X) <= tolerance && + System.Math.Abs(left.Y - right.Y) <= tolerance && + System.Math.Abs(left.Z - right.Z) <= tolerance && + System.Math.Abs(left.W - right.W) <= tolerance; + } + + #endregion + + + #region IComparer + + /// Compares two vectors and returns left value indicating whether one is less than, equal to, or greater than the other. + public int Compare(Vector4d left, Vector4d right) + { + if (left.X != right.X) + { + if (left.X < right.X) return -1; + else return 1; + } + else if (left.Y != right.Y) + { + if (left.Y < right.Y) return -1; + else return 1; + } + else if (left.Z != right.Z) + { + if (left.Z < right.Z) return -1; + else return 1; + } + else if (left.W != right.W) + { + if (left.W < right.W) return -1; + else return 1; + } + + return 0; + } + + #endregion + + + #region IComparable + + /// Compares the vector with another vector and returns left value indicating whether it is less than, equal to, or greater than the other vector. + public int CompareTo(Vector4d vector) { return Compare(this, vector); } + + #endregion + + + #region Length + + /// Gets the length of the vector. + public double Length + { + get + { + double lengthSquared = LengthSquared; + + if (lengthSquared == 1) + { + return 1; + } + else + { + return System.Math.Sqrt(lengthSquared); + } + } + } + + /// Gets the squared length of the vector. + public double LengthSquared + { + get + { + return X * X + Y * Y + Z * Z + W * W; + } + } + + /// Gets the approimate length of the vector. + public double LengthApprox + { + get + { + return 1.0d / Functions.InverseSqrtFast(X * X + Y * Y + Z * Z + W * W); + } + } + + #endregion + + + #region Distance + + /// Gets the distance from this vector to the given vector. + /// The vector to which to find the distance. + /// The distance from this vector to the given vector. + public double DistanceTo(ref Vector4d vector) + { + double deltaX = vector.X - X; + double deltaY = vector.Y - Y; + double deltaZ = vector.Z - Z; + double deltaW = vector.W - W; + return System.Math.Sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ + deltaW * deltaW); + } + + /// Gets the squared distance from this vector to the given vector. + /// The vector to which to find the squared distance. + /// The squared distance from this vector to the given vector. + public double DistanceSquaredTo(ref Vector4d vector) + { + double deltaX = vector.X - X; + double deltaY = vector.Y - Y; + double deltaZ = vector.Z - Z; + double deltaW = vector.W - W; + return deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ + deltaW * deltaW; + } + + /// Gets the approximate distance from this vector to the given vector. + /// The vector to which to find the approximate distance. + /// The approximate distance from this vector to the given vector. + public double DistanceApproxTo(ref Vector4d vector) + { + double deltaX = vector.X - X; + double deltaY = vector.Y - Y; + double deltaZ = vector.Z - Z; + double deltaW = vector.W - W; + return 1.0d / Functions.InverseSqrtFast(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ + deltaW * deltaW); + } + + #endregion + + + #region Normalize + + /// Normalize this vector. + public void Normalize() + { + double lengthSquared = LengthSquared; + + if (lengthSquared != 1 && lengthSquared != 0) + { + double length = System.Math.Sqrt(lengthSquared); + X = X / length; + Y = Y / length; + Z = Z / length; + W = W / length; + } + } + + /// Get the normalized version of this vector. + /// The resulting normalized vector. + public void Normalize(out Vector4d result) + { + double lengthSquared = LengthSquared; + + if (lengthSquared == 1) + { + result.X = X; + result.Y = Y; + result.Z = Z; + result.W = W; + } + else if (lengthSquared == 0) + { + result.X = 0; + result.Y = 0; + result.Z = 0; + result.W = 0; + } + else + { + double length = System.Math.Sqrt(lengthSquared); + result.X = X / length; + result.Y = Y / length; + result.Z = Z / length; + result.W = W / length; + } + } + + public static void Normalize(ref Vector4d vector, out Vector4d result) + { + double lengthSquared = vector.LengthSquared; + + if (lengthSquared == 1) + { + result.X = vector.X; + result.Y = vector.Y; + result.Z = vector.Z; + result.W = vector.W; + } + else if (lengthSquared == 0) + { + result.X = 0; + result.Y = 0; + result.Z = 0; + result.W = 0; + } + else + { + double length = System.Math.Sqrt(lengthSquared); + result.X = vector.X / length; + result.Y = vector.Y / length; + result.Z = vector.Z / length; + result.W = vector.W / length; + } + } + + public void NormalizeApprox() + { + double inverseSquare = Functions.InverseSqrtFast(X * X + Y * Y + Z * Z + W * W); + X = X * inverseSquare; + Y = Y * inverseSquare; + Z = Z * inverseSquare; + W = W * inverseSquare; + } + + /// Gets left approximately normalized vector of the vector. + public void NormalizedApprox(out Vector4d result) + { + double inverseSquare = Functions.InverseSqrtFast(X * X + Y * Y + Z * Z + W * W); + result.X = X * inverseSquare; + result.Y = Y * inverseSquare; + result.Z = Z * inverseSquare; + result.W = W * inverseSquare; + } + + public static void NormalizeApprox(ref Vector4d vector, out Vector4d result) + { + double inverseSquare = Functions.InverseSqrtFast(vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z + vector.W * vector.W); + result.X = vector.X * inverseSquare; + result.Y = vector.Y * inverseSquare; + result.Z = vector.Z * inverseSquare; + result.W = vector.W * inverseSquare; + } + + #endregion + + + /// Gets the dot product of two vectors. + /// The first vector. + /// The second vector. + /// The dot product of two vectors. + public static double DotProduct(ref Vector4d left, ref Vector4d right) + { + return left.X * right.X + left.Y * right.Y + left.Z * right.Z + left.W * right.W; + } + + + #region Abs + + public void Abs() + { + X = System.Math.Abs(X); + Y = System.Math.Abs(Y); + Z = System.Math.Abs(Z); + W = System.Math.Abs(W); + } + public void Abs(out Vector4d result) + { + result.X = System.Math.Abs(X); + result.Y = System.Math.Abs(Y); + result.Z = System.Math.Abs(Z); + result.W = System.Math.Abs(W); + } + public static void Abs(ref Vector4d vector, out Vector4d result) + { + result.X = System.Math.Abs(vector.X); + result.Y = System.Math.Abs(vector.Y); + result.Z = System.Math.Abs(vector.Z); + result.W = System.Math.Abs(vector.W); + } + + #endregion + + + #region Inverse + + public void Inverse() + { + X = -X; + Y = -Y; + Z = -Z; + W = -W; + } + public void Inverse(out Vector4d result) + { + result.X = -X; + result.Y = -Y; + result.Z = -Z; + result.W = -W; + } + public static void Inverse(ref Vector4d vector, out Vector4d result) + { + result.X = -vector.X; + result.Y = -vector.Y; + result.Z = -vector.Z; + result.W = -vector.W; + } + + #endregion + + + #region Arithmatic + + public void Add(ref Vector4d vector) + { + X = X + vector.X; + Y = Y + vector.Y; + Z = Z + vector.Z; + W = W + vector.W; + } + public void Add(ref Vector4d vector, out Vector4d result) + { + result.X = X + vector.X; + result.Y = Y + vector.Y; + result.Z = Z + vector.Z; + result.W = W + vector.W; + } + public static void Add(ref Vector4d left, ref Vector4d right, out Vector4d result) + { + result.X = left.X + right.X; + result.Y = left.Y + right.Y; + result.Z = left.Z + right.Z; + result.W = left.W + right.W; + } + + public void Add(double x, double y, double z, double w) + { + X = X + x; + Y = Y + y; + Z = Z + z; + W = W + w; + } + public void Add(double x, double y, double z, double w, out Vector4d result) + { + result.X = X + x; + result.Y = Y + y; + result.Z = Z + z; + result.W = W + w; + } + public static void Add(ref Vector4d vector, double x, double y, double z, double w, out Vector4d result) + { + result.X = vector.X + x; + result.Y = vector.Y + y; + result.Z = vector.Z + z; + result.W = vector.W + w; + } + + public void Subtract(ref Vector4d vector) + { + X = X - vector.X; + Y = Y - vector.Y; + Z = Z - vector.Z; + W = W - vector.W; + } + public void Subtract(ref Vector4d vector, out Vector4d result) + { + result.X = X - vector.X; + result.Y = Y - vector.Y; + result.Z = Z - vector.Z; + result.W = W - vector.W; + } + public static void Subtract(ref Vector4d left, ref Vector4d right, out Vector4d result) + { + result.X = left.X - right.X; + result.Y = left.Y - right.Y; + result.Z = left.Z - right.Z; + result.W = left.W - right.W; + } + + public void Subtract(double x, double y, double z, double w) + { + X = X - x; + Y = Y - y; + Z = Z - z; + W = W - w; + } + public void Subtract(double x, double y, double z, double w, out Vector4d result) + { + result.X = X - x; + result.Y = Y - y; + result.Z = Z - z; + result.W = W - w; + } + public static void Subtract(ref Vector4d vector, double x, double y, double z, double w, out Vector4d result) + { + result.X = vector.X - x; + result.Y = vector.Y - y; + result.Z = vector.Z - z; + result.W = vector.W - w; + } + + public void Multiply(double scalar) + { + X = X * scalar; + Y = Y * scalar; + Z = Z * scalar; + W = W * scalar; + } + public void Multiply(double scalar, out Vector4d result) + { + result.X = X * scalar; + result.Y = Y * scalar; + result.Z = Z * scalar; + result.W = W * scalar; + } + public static void Multiply(ref Vector4d vector, double scalar, out Vector4d result) + { + result.X = vector.X * scalar; + result.Y = vector.Y * scalar; + result.Z = vector.Z * scalar; + result.W = vector.W * scalar; + } + + public void Multiply(ref Vector4d vector) + { + X = X * vector.X; + Y = Y * vector.Y; + Z = Z * vector.Z; + W = W * vector.W; + } + public void Multiply(ref Vector4d vector, out Vector4d result) + { + result.X = X * vector.X; + result.Y = Y * vector.Y; + result.Z = Z * vector.Z; + result.W = W * vector.W; + } + public static void Multiply(ref Vector4d left, ref Vector4d right, out Vector4d result) + { + result.X = left.X * right.X; + result.Y = left.Y * right.Y; + result.Z = left.Z * right.Z; + result.W = left.W * right.W; + } + + public void Multiply(double x, double y, double z, double w) + { + X = X * x; + Y = Y * y; + Z = Z * z; + W = W * w; + } + public void Multiply(double x, double y, double z, double w, out Vector4d result) + { + result.X = X * x; + result.Y = Y * y; + result.Z = Z * z; + result.W = W * w; + } + public static void Multiply(ref Vector4d vector, double x, double y, double z, double w, out Vector4d result) + { + result.X = vector.X * x; + result.Y = vector.Y * y; + result.Z = vector.Z * z; + result.W = vector.W * w; + } + + public void Divide(double scalar) + { + X = X / scalar; + Y = Y / scalar; + Z = X / scalar; + W = W / scalar; + } + public void Divide(double scalar, out Vector4d result) + { + result.X = X / scalar; + result.Y = Y / scalar; + result.Z = X / scalar; + result.W = W / scalar; + } + public static void Divide(ref Vector4d vector, double scalar, out Vector4d result) + { + result.X = vector.X / scalar; + result.Y = vector.Y / scalar; + result.Z = vector.Z / scalar; + result.W = vector.W / scalar; + } + + public void Divide(ref Vector4d vector) + { + X = X / vector.X; + Y = Y / vector.Y; + Z = Z / vector.Z; + W = W / vector.W; + } + public void Divide(ref Vector4d vector, out Vector4d result) + { + result.X = X / vector.X; + result.Y = Y / vector.Y; + result.Z = Z / vector.Z; + result.W = W / vector.W; + } + public static void Divide(ref Vector4d left, ref Vector4d right, out Vector4d result) + { + result.X = left.X / right.X; + result.Y = left.Y / right.Y; + result.Z = left.Z / right.Z; + result.W = left.W / right.W; + } + + public void Divide(double x, double y, double z, double w) + { + X = X / x; + Y = Y / y; + Z = Z / z; + W = W / w; + } + public void Divide(double x, double y, double z, double w, out Vector4d result) + { + result.X = X / x; + result.Y = Y / y; + result.Z = Z / z; + result.W = W / w; + } + public static void Divide(ref Vector4d vector, double x, double y, double z, double w, out Vector4d result) + { + result.X = vector.X / x; + result.Y = vector.Y / y; + result.Z = vector.Z / z; + result.W = vector.W / w; + } + + #endregion + + + #region Transformations + + public void Transform(ref Matrix4d matrix) + { + double x = matrix.R0C0 * X + matrix.R0C1 * Y + matrix.R0C2 * Z + matrix.R0C3 * W; + double y = matrix.R1C0 * X + matrix.R1C1 * Y + matrix.R1C2 * Z + matrix.R1C3 * W; + double z = matrix.R2C0 * X + matrix.R2C1 * Y + matrix.R2C2 * Z + matrix.R2C3 * W; + W = matrix.R3C0 * X + matrix.R3C1 * Y + matrix.R3C2 * Z + matrix.R3C3 * W; + X = x; + Y = y; + Z = z; + } + public void Transform(ref Matrix4d matrix, out Vector4d result) + { + result.X = matrix.R0C0 * X + matrix.R0C1 * Y + matrix.R0C2 * Z + matrix.R0C3 * W; + result.Y = matrix.R1C0 * X + matrix.R1C1 * Y + matrix.R1C2 * Z + matrix.R1C3 * W; + result.Z = matrix.R2C0 * X + matrix.R2C1 * Y + matrix.R2C2 * Z + matrix.R2C3 * W; + result.W = matrix.R3C0 * X + matrix.R3C1 * Y + matrix.R3C2 * Z + matrix.R3C3 * W; + } + public static void Transform(ref Vector4d vector, ref Matrix4d matrix, out Vector4d result) + { + result.X = matrix.R0C0 * vector.X + matrix.R0C1 * vector.Y + matrix.R0C2 * vector.Z + matrix.R0C3 * vector.W; + result.Y = matrix.R1C0 * vector.X + matrix.R1C1 * vector.Y + matrix.R1C2 * vector.Z + matrix.R1C3 * vector.W; + result.Z = matrix.R2C0 * vector.X + matrix.R2C1 * vector.Y + matrix.R2C2 * vector.Z + matrix.R2C3 * vector.W; + result.W = matrix.R3C0 * vector.X + matrix.R3C1 * vector.Y + matrix.R3C2 * vector.Z + matrix.R3C3 * vector.W; + } + + public void Translate(ref Vector4d vector) + { + X = X + vector.X; + Y = Y + vector.Y; + Z = Z + vector.Z; + W = W + vector.W; + } + public void Translate(ref Vector4d vector, out Vector4d result) + { + result.X = X + vector.X; + result.Y = Y + vector.Y; + result.Z = Z + vector.Z; + result.W = W + vector.W; + } + public static void Translate(ref Vector4d left, ref Vector4d right, out Vector4d result) + { + result.X = left.X + right.X; + result.Y = left.Y + right.Y; + result.Z = left.Z + right.Z; + result.W = left.W + right.W; + } + + public void Translate(double x, double y, double z, double w) + { + X = X + x; + Y = Y + y; + Z = Z + z; + W = W + w; + } + public void Translate(double x, double y, double z, double w, out Vector4d result) + { + result.X = X + x; + result.Y = Y + y; + result.Z = Z + z; + result.W = W + w; + } + public static void Translate(ref Vector4d vector, double x, double y, double z, double w, out Vector4d result) + { + result.X = vector.X + x; + result.Y = vector.Y + y; + result.Z = vector.Z + z; + result.W = vector.W + w; + } + + public void Scale(ref Vector4d vector) + { + X = X * vector.X; + Y = Y * vector.Y; + Z = Z * vector.Z; + W = W * vector.W; + } + public void Scale(ref Vector4d vector, out Vector4d result) + { + result.X = X * vector.X; + result.Y = Y * vector.Y; + result.Z = Z * vector.Z; + result.W = W * vector.W; + } + public static void Scale(ref Vector4d left, ref Vector4d right, out Vector4d result) + { + result.X = left.X * right.X; + result.Y = left.Y * right.Y; + result.Z = left.Z * right.Z; + result.W = left.W * right.W; + } + + public void Scale(double x, double y, double z, double w) + { + X = X * x; + Y = Y * y; + Z = Z * z; + W = W * w; + } + public void Scale(double x, double y, double z, double w, out Vector4d result) + { + result.X = X * x; + result.Y = Y * y; + result.Z = Z * z; + result.W = W * w; + } + public static void Scale(ref Vector4d vector, double x, double y, double z, double w, out Vector4d result) + { + result.X = vector.X * x; + result.Y = vector.Y * y; + result.Z = vector.Z * z; + result.W = vector.W * w; + } + + public void RotateX(double angle) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + double y = cos * Y + sin * Z; + Z = cos * Z - sin * Y; + Y = y; + } + public void RotateX(double angle, out Vector4d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.X = X; + result.Y = cos * Y + sin * Z; + result.Z = cos * Z - sin * Y; + result.W = W; + } + public static void RotateX(ref Vector4d vector, double angle, out Vector4d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.X = vector.X; + result.Y = cos * vector.Y + sin * vector.Z; + result.Z = cos * vector.Z - sin * vector.Y; + result.W = vector.W; + } + + public void RotateY(double angle) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + double x = cos * X - sin * Z; + Z = sin * X + cos * Z; + X = x; + } + public void RotateY(double angle, out Vector4d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.X = cos * X - sin * Z; + result.Y = Y; + result.Z = sin * Y + cos * Z; + result.W = W; + } + public static void RotateY(ref Vector4d vector, double angle, out Vector4d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.X = cos * vector.X - sin * vector.Z; + result.Y = vector.Y; + result.Z = sin * vector.Y + cos * vector.Z; + result.W = vector.W; + } + + public void RotateZ(double angle) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + double x = cos * X + sin * Y; + Y = cos * Y - sin * X; + X = x; + } + public void RotateZ(double angle, out Vector4d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.X = cos * X + sin * Y; + result.Y = cos * Y - sin * X; + result.Z = Z; + result.W = W; + } + public static void RotateZ(ref Vector4d vector, double angle, out Vector4d result) + { + double angleRadians = Functions.DTOR * angle; + double sin = (double)System.Math.Sin(angleRadians); + double cos = (double)System.Math.Cos(angleRadians); + + result.X = cos * vector.X + sin * vector.Y; + result.Y = cos * vector.Y - sin * vector.X; + result.Z = vector.Z; + result.W = vector.W; + } + + public void Rotate(ref Vector3d axis, double angle) + { + Vector3d axisNormalized; + axis.Normalize(out axisNormalized); + double x = axisNormalized.X; + double y = axisNormalized.Y; + double z = axisNormalized.Z; + double angleRadians = Functions.DTOR * angle; + double cos = (double)System.Math.Cos(angleRadians); + double sin = (double)System.Math.Sin(angleRadians); + double oneMinusCos = 1 - cos; + double xOneMinusCos = x * oneMinusCos; + double yOneMinusCos = y * oneMinusCos; + double zOneMinusCos = z * oneMinusCos; + double xxOneMinusCos = x * xOneMinusCos; + double xyOneMinusCos = x * yOneMinusCos; + double xzOneMinusCos = x * zOneMinusCos; + double yyOneMinusCos = y * yOneMinusCos; + double yzOneMinusCos = y * zOneMinusCos; + double zzOneMinusCos = z * zOneMinusCos; + double xSin = x * sin; + double ySin = y * sin; + double zSin = z * sin; + + double tx = (xxOneMinusCos + cos) * X + (xyOneMinusCos + zSin) * Y + (xzOneMinusCos - ySin) * Z; + double ty = (xyOneMinusCos - zSin) * X + (yyOneMinusCos + cos) * Y + (yzOneMinusCos + xSin) * Z; + Z = (xzOneMinusCos + ySin) * X + (yzOneMinusCos - xSin) * Y + (zzOneMinusCos + cos) * Z; + X = tx; + Y = ty; + } + public void Rotate(ref Vector3d axis, double angle, out Vector4d result) + { + Vector3d axisNormalized; + axis.Normalize(out axisNormalized); + double x = axisNormalized.X; + double y = axisNormalized.Y; + double z = axisNormalized.Z; + double angleRadians = Functions.DTOR * angle; + double cos = (double)System.Math.Cos(angleRadians); + double sin = (double)System.Math.Sin(angleRadians); + double oneMinusCos = 1 - cos; + double xOneMinusCos = x * oneMinusCos; + double yOneMinusCos = y * oneMinusCos; + double zOneMinusCos = z * oneMinusCos; + double xxOneMinusCos = x * xOneMinusCos; + double xyOneMinusCos = x * yOneMinusCos; + double xzOneMinusCos = x * zOneMinusCos; + double yyOneMinusCos = y * yOneMinusCos; + double yzOneMinusCos = y * zOneMinusCos; + double zzOneMinusCos = z * zOneMinusCos; + double xSin = x * sin; + double ySin = y * sin; + double zSin = z * sin; + + result.X = (xxOneMinusCos + cos) * X + (xyOneMinusCos + zSin) * Y + (xzOneMinusCos - ySin) * Z; + result.Y = (xyOneMinusCos - zSin) * X + (yyOneMinusCos + cos) * Y + (yzOneMinusCos + xSin) * Z; + result.Z = (xzOneMinusCos + ySin) * X + (yzOneMinusCos - xSin) * Y + (zzOneMinusCos + cos) * Z; + result.W = W; + } + public static void Rotate(ref Vector4d vector, ref Vector3d axis, double angle, out Vector4d result) + { + Vector3d axisNormalized; + axis.Normalize(out axisNormalized); + double x = axisNormalized.X; + double y = axisNormalized.Y; + double z = axisNormalized.Z; + double angleRadians = Functions.DTOR * angle; + double cos = (double)System.Math.Cos(angleRadians); + double sin = (double)System.Math.Sin(angleRadians); + double oneMinusCos = 1 - cos; + double xOneMinusCos = x * oneMinusCos; + double yOneMinusCos = y * oneMinusCos; + double zOneMinusCos = z * oneMinusCos; + double xxOneMinusCos = x * xOneMinusCos; + double xyOneMinusCos = x * yOneMinusCos; + double xzOneMinusCos = x * zOneMinusCos; + double yyOneMinusCos = y * yOneMinusCos; + double yzOneMinusCos = y * zOneMinusCos; + double zzOneMinusCos = z * zOneMinusCos; + double xSin = x * sin; + double ySin = y * sin; + double zSin = z * sin; + + result.X = (xxOneMinusCos + cos ) * vector.X + (xyOneMinusCos + zSin) * vector.Y + (xzOneMinusCos - ySin) * vector.Z; + result.Y = (xyOneMinusCos - zSin) * vector.X + (yyOneMinusCos + cos) * vector.Y + (yzOneMinusCos + xSin) * vector.Z; + result.Z = (xzOneMinusCos + ySin) * vector.X + (yzOneMinusCos - xSin) * vector.Y + (zzOneMinusCos + cos ) * vector.Z; + result.W = vector.W; + } + + #endregion + + + #region Min & Max + + public void Min(ref Vector4d vector) + { + double lengthSquared = X * X + Y * Y + Z * Z + W * W; + double vectorLengthSquared = vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z + vector.W * vector.W; + + if (vectorLengthSquared < lengthSquared) + { + X = vector.X; + Y = vector.Y; + Z = vector.Z; + W = vector.W; + } + } + public void Min(ref Vector4d vector, out Vector4d result) + { + double lengthSquared = X * X + Y * Y + Z * Z + W * W; + double vectorLengthSquared = vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z + vector.W * vector.W; + + if (vectorLengthSquared < lengthSquared) + { + result.X = vector.X; + result.Y = vector.Y; + result.Z = vector.Z; + result.W = vector.W; + } + else + { + result.X = X; + result.Y = Y; + result.Z = Z; + result.W = W; + } + } + public static void Min(ref Vector4d left, ref Vector4d right, out Vector4d result) + { + double leftLengthSquared = left.X * left.X + left.Y * left.Y + left.Z * left.Z + left.W * left.W; + double rightLengthSquared = right.X * right.X + right.Y * right.Y + right.Z * right.Z + right.W * right.W; + + if (rightLengthSquared < leftLengthSquared) + { + result.X = right.X; + result.Y = right.Y; + result.Z = right.Z; + result.W = right.W; + } + else + { + result.X = left.X; + result.Y = left.Y; + result.Z = left.Z; + result.W = left.W; + } + } + + public void Max(ref Vector4d vector) + { + double lengthSquared = X * X + Y * Y + Z * Z + W * W; + double vectorLengthSquared = vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z + vector.W * vector.W; + + if (vectorLengthSquared > lengthSquared) + { + X = vector.X; + Y = vector.Y; + Z = vector.Z; + W = vector.W; + } + } + public void Max(ref Vector4d vector, out Vector4d result) + { + double lengthSquared = X * X + Y * Y + Z * Z + W * W; + double vectorLengthSquared = vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z + vector.W * vector.W; + + if (vectorLengthSquared > lengthSquared) + { + result.X = vector.X; + result.Y = vector.Y; + result.Z = vector.Z; + result.W = vector.W; + } + else + { + result.X = X; + result.Y = Y; + result.Z = Z; + result.W = W; + } + } + public static void Max(ref Vector4d left, ref Vector4d right, out Vector4d result) + { + double leftLengthSquared = left.X * left.X + left.Y * left.Y + left.Z * left.Z + left.W * left.W; + double rightLengthSquared = right.X * right.X + right.Y * right.Y + right.Z * right.Z + right.W * right.W; + + if (rightLengthSquared > leftLengthSquared) + { + result.X = right.X; + result.Y = right.Y; + result.Z = right.Z; + result.W = right.W; + } + else + { + result.X = left.X; + result.Y = left.Y; + result.Z = left.Z; + result.W = left.W; + } + } + + public void CoordinateMin(ref Vector4d vector) + { + X = System.Math.Min(X, vector.X); + Y = System.Math.Min(Y, vector.Y); + Z = System.Math.Min(Z, vector.Z); + W = System.Math.Min(W, vector.W); + } + public void CoordinateMin(ref Vector4d vector, out Vector4d result) + { + result.X = System.Math.Min(X, vector.X); + result.Y = System.Math.Min(Y, vector.Y); + result.Z = System.Math.Min(Z, vector.Z); + result.W = System.Math.Min(W, vector.W); + } + public static void CoordinateMin(ref Vector4d left, ref Vector4d right, out Vector4d result) + { + result.X = System.Math.Min(left.X, right.X); + result.Y = System.Math.Min(left.Y, right.Y); + result.Z = System.Math.Min(left.Z, right.Z); + result.W = System.Math.Min(left.W, right.W); + } + + public void CoordinateMax(ref Vector4d vector) + { + X = System.Math.Max(X, vector.X); + Y = System.Math.Max(Y, vector.Y); + Z = System.Math.Max(Z, vector.Z); + W = System.Math.Max(W, vector.W); + } + public void CoordinateMax(ref Vector4d vector, out Vector4d result) + { + result.X = System.Math.Max(X, vector.X); + result.Y = System.Math.Max(Y, vector.Y); + result.Z = System.Math.Max(Z, vector.Z); + result.W = System.Math.Max(W, vector.W); + } + public static void CoordinateMax(ref Vector4d left, ref Vector4d right, out Vector4d result) + { + result.X = System.Math.Max(left.X, right.X); + result.Y = System.Math.Max(left.Y, right.Y); + result.Z = System.Math.Max(left.Z, right.Z); + result.W = System.Math.Max(left.W, right.W); + } + + public void Clamp(ref Vector4d min, ref Vector4d max) + { + X = System.Math.Max(System.Math.Min(X, min.X), max.X); + Y = System.Math.Max(System.Math.Min(Y, min.Y), max.Y); + Z = System.Math.Max(System.Math.Min(Z, min.Z), max.Z); + W = System.Math.Max(System.Math.Min(W, min.W), max.W); + } + public void Clamp(ref Vector4d min, ref Vector4d max, out Vector4d result) + { + result.X = System.Math.Max(System.Math.Min(X, min.X), max.X); + result.Y = System.Math.Max(System.Math.Min(Y, min.Y), max.Y); + result.Z = System.Math.Max(System.Math.Min(Z, min.Z), max.Z); + result.W = System.Math.Max(System.Math.Min(W, min.W), max.W); + } + public static void Clamp(ref Vector4d vector, ref Vector4d min, ref Vector4d max, out Vector4d result) + { + result.X = System.Math.Max(System.Math.Min(vector.X, min.X), max.X); + result.Y = System.Math.Max(System.Math.Min(vector.Y, min.Y), max.Y); + result.Z = System.Math.Max(System.Math.Min(vector.Z, min.Z), max.Z); + result.W = System.Math.Max(System.Math.Min(vector.W, min.W), max.W); + } + + #endregion + + + #region Interpolation + + public void Lerp(ref Vector4d end, double blend) + { + X = X + (end.X - X) * blend; + Y = Y + (end.Y - Y) * blend; + Z = Z + (end.Z - Z) * blend; + W = W + (end.W - W) * blend; + } + public void Lerp(ref Vector4d end, double blend, out Vector4d result) + { + result.X = X + (end.X - X) * blend; + result.Y = Y + (end.Y - Y) * blend; + result.Z = Z + (end.Z - Z) * blend; + result.W = W + (end.W - W) * blend; + } + public static void Lerp(ref Vector4d start, ref Vector4d end, double blend, out Vector4d result) + { + result.X = start.X + (end.X - start.X) * blend; + result.Y = start.Y + (end.Y - start.Y) * blend; + result.Z = start.Z + (end.Z - start.Z) * blend; + result.W = start.W + (end.W - start.W) * blend; + } + + public void BaryCentric(ref Vector4d endU, ref Vector4d endV, double u, double v) + { + X = X + (endU.X - X) * u + (endV.X - X) * v; + Y = Y + (endU.Y - Y) * u + (endV.Y - Y) * v; + Z = Z + (endU.Z - Z) * u + (endV.Z - Z) * v; + W = W + (endU.W - W) * u + (endV.W - W) * v; + } + public void BaryCentric(ref Vector4d endU, ref Vector4d endV, double u, double v, out Vector4d result) + { + result.X = X + (endU.X - X) * u + (endV.X - X) * v; + result.Y = Y + (endU.Y - Y) * u + (endV.Y - Y) * v; + result.Z = Z + (endU.Z - Z) * u + (endV.Z - Z) * v; + result.W = W + (endU.W - W) * u + (endV.W - W) * v; + } + public static void BaryCentric(ref Vector4d start, ref Vector4d endU, ref Vector4d endV, double u, double v, out Vector4d result) + { + result.X = start.X + (endU.X - start.X) * u + (endV.X - start.X) * v; + result.Y = start.Y + (endU.Y - start.Y) * u + (endV.Y - start.Y) * v; + result.Z = start.Z + (endU.Z - start.Z) * u + (endV.Z - start.Z) * v; + result.W = start.W + (endU.W - start.W) * u + (endV.W - start.W) * v; + } + + #endregion + + + #region String and Parse + + /// Returns the fully qualified type name of this instance. + /// A System.String containing left fully qualified type name. + public override string ToString() + { + return String.Format("{0} {1} {2} {3}", X, Y, Z, W); + } + + /// Parse left string to convert it to left vector. + /// The string to parse. + /// The vector represented by the string. + public static void Parse(string str, out Vector4d result) + { + Match match = new Regex(@"(?.*) (?.*) (?.*) (?.*)", RegexOptions.None).Match(str); + if (!match.Success) throw new Exception("Parse failed!"); + result.X = double.Parse(match.Result("${x}")); + result.Y = double.Parse(match.Result("${y}")); + result.Z = double.Parse(match.Result("${z}")); + result.W = double.Parse(match.Result("${w}")); + } + + #endregion + + + #region HashCode + + /// Returns the hash code for this instance. + /// A 32-bit signed integer that is the hash code for this instance. + public override int GetHashCode() + { + return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode() ^ W.GetHashCode(); + } + + #endregion + + + #region Constants + + /// A vector representing left zero vector. + public static readonly Vector4d Zero = new Vector4d(0, 0, 0, 0); + + /// A vector with all coordinates set to one. + public static readonly Vector4d One = new Vector4d(1, 1, 1, 1); + + /// A unit normal vector representing the positive X Axis. + public static readonly Vector4d XAxis = new Vector4d(1, 0, 0, 0); + + /// A unit normal vector representing the positive Y Axis. + public static readonly Vector4d YAxis = new Vector4d(0, 1, 0, 0); + + /// A unit normal vector representing the positive Z Axis. + public static readonly Vector4d ZAxis = new Vector4d(0, 0, 1, 0); + + /// A unit normal vector representing the positive W Axis. + public static readonly Vector4d WAxis = new Vector4d(0, 0, 0, 1); + + #endregion + } +} \ No newline at end of file