diff --git a/Source/OpenTK/Math/Matrix3.cs b/Source/OpenTK/Math/Matrix3.cs index 11816206..3fb8e2e5 100644 --- a/Source/OpenTK/Math/Matrix3.cs +++ b/Source/OpenTK/Math/Matrix3.cs @@ -28,7 +28,7 @@ using System.Runtime.InteropServices; namespace OpenTK { /// - /// Represents a 3x3 Matrix + /// Represents a 3x3 matrix containing 3D rotation and scale. /// [Serializable] [StructLayout(LayoutKind.Sequential)] @@ -254,7 +254,111 @@ namespace OpenTK } #endregion - + + /// + /// Returns a normalised copy of this instance. + /// + public Matrix3 Normalized() + { + Matrix3 m = this; + m.Normalize(); + return m; + } + + /// + /// Divides each element in the Matrix by the . + /// + public void Normalize() + { + var determinant = this.Determinant; + Row0 /= determinant; + Row1 /= determinant; + Row2 /= determinant; + } + + /// + /// Returns an inverted copy of this instance. + /// + public Matrix3 Inverted() + { + Matrix3 m = this; + if (m.Determinant != 0) + m.Invert(); + return m; + } + + /// + /// Returns the scale component of this instance. + /// + public Vector3 ExtractScale() { return new Vector3(Row0.Length, Row1.Length, Row2.Length); } + + /// + /// Returns the rotation component of this instance. Quite slow. + /// + /// Whether the method should row-normalise (i.e. remove scale from) the Matrix. Pass false if you know it's already normalised. + public Quaternion ExtractRotation(bool row_normalise = true) + { + var row0 = Row0; + var row1 = Row1; + var row2 = Row2; + + if (row_normalise) + { + row0 = row0.Normalized(); + row1 = row1.Normalized(); + row2 = row2.Normalized(); + } + + // code below adapted from Blender + + Quaternion q = new Quaternion(); + double trace = 0.25 * (row0[0] + row1[1] + row2[2] + 1.0); + + if (trace > 0) + { + double sq = Math.Sqrt(trace); + + q.W = (float)sq; + sq = 1.0 / (4.0 * sq); + q.X = (float)((row1[2] - row2[1]) * sq); + q.Y = (float)((row2[0] - row0[2]) * sq); + q.Z = (float)((row0[1] - row1[0]) * sq); + } + else if (row0[0] > row1[1] && row0[0] > row2[2]) + { + double sq = 2.0 * Math.Sqrt(1.0 + row0[0] - row1[1] - row2[2]); + + q.X = (float)(0.25 * sq); + sq = 1.0 / sq; + q.W = (float)((row2[1] - row1[2]) * sq); + q.Y = (float)((row1[0] + row0[1]) * sq); + q.Z = (float)((row2[0] + row0[2]) * sq); + } + else if (row1[1] > row2[2]) + { + double sq = 2.0 * Math.Sqrt(1.0 + row1[1] - row0[0] - row2[2]); + + q.Y = (float)(0.25 * sq); + sq = 1.0 / sq; + q.W = (float)((row2[0] - row0[2]) * sq); + q.X = (float)((row1[0] + row0[1]) * sq); + q.Z = (float)((row2[1] + row1[2]) * sq); + } + else + { + double sq = 2.0 * Math.Sqrt(1.0 + row2[2] - row0[0] - row1[1]); + + q.Z = (float)(0.25 * sq); + sq = 1.0 / sq; + q.W = (float)((row1[0] - row0[1]) * sq); + q.X = (float)((row2[0] + row0[2]) * sq); + q.Y = (float)((row2[1] + row1[2]) * sq); + } + + q.Normalize(); + return q; + } + #endregion #region Static diff --git a/Source/OpenTK/Math/Matrix3d.cs b/Source/OpenTK/Math/Matrix3d.cs index 7a717666..d98da9c9 100644 --- a/Source/OpenTK/Math/Matrix3d.cs +++ b/Source/OpenTK/Math/Matrix3d.cs @@ -28,7 +28,7 @@ using System.Runtime.InteropServices; namespace OpenTK { /// - /// Represents a 3x3 Matrix + /// Represents a 3x3 matrix containing 3D rotation and scale with double-precision components. /// [Serializable] [StructLayout(LayoutKind.Sequential)] @@ -250,7 +250,111 @@ namespace OpenTK } #endregion - + + /// + /// Returns a normalised copy of this instance. + /// + public Matrix3d Normalized() + { + Matrix3d m = this; + m.Normalize(); + return m; + } + + /// + /// Divides each element in the Matrix by the . + /// + public void Normalize() + { + var determinant = this.Determinant; + Row0 /= determinant; + Row1 /= determinant; + Row2 /= determinant; + } + + /// + /// Returns an inverted copy of this instance. + /// + public Matrix3d Inverted() + { + Matrix3d m = this; + if (m.Determinant != 0) + m.Invert(); + return m; + } + + /// + /// Returns the scale component of this instance. + /// + public Vector3d ExtractScale() { return new Vector3d(Row0.Length, Row1.Length, Row2.Length); } + + /// + /// Returns the rotation component of this instance. Quite slow. + /// + /// Whether the method should row-normalise (i.e. remove scale from) the Matrix. Pass false if you know it's already normalised. + public Quaterniond ExtractRotation(bool row_normalise = true) + { + var row0 = Row0; + var row1 = Row1; + var row2 = Row2; + + if (row_normalise) + { + row0 = row0.Normalized(); + row1 = row1.Normalized(); + row2 = row2.Normalized(); + } + + // code below adapted from Blender + + Quaterniond q = new Quaterniond(); + double trace = 0.25 * (row0[0] + row1[1] + row2[2] + 1.0); + + if (trace > 0) + { + double sq = Math.Sqrt(trace); + + q.W = sq; + sq = 1.0 / (4.0 * sq); + q.X = (row1[2] - row2[1]) * sq; + q.Y = (row2[0] - row0[2]) * sq; + q.Z = (row0[1] - row1[0]) * sq; + } + else if (row0[0] > row1[1] && row0[0] > row2[2]) + { + double sq = 2.0 * Math.Sqrt(1.0 + row0[0] - row1[1] - row2[2]); + + q.X = 0.25 * sq; + sq = 1.0 / sq; + q.W = (row2[1] - row1[2]) * sq; + q.Y = (row1[0] + row0[1]) * sq; + q.Z = (row2[0] + row0[2]) * sq; + } + else if (row1[1] > row2[2]) + { + double sq = 2.0 * Math.Sqrt(1.0 + row1[1] - row0[0] - row2[2]); + + q.Y = 0.25 * sq; + sq = 1.0 / sq; + q.W = (row2[0] - row0[2]) * sq; + q.X = (row1[0] + row0[1]) * sq; + q.Z = (row2[1] + row1[2]) * sq; + } + else + { + double sq = 2.0 * Math.Sqrt(1.0 + row2[2] - row0[0] - row1[1]); + + q.Z = 0.25 * sq; + sq = 1.0 / sq; + q.W = (row1[0] - row0[1]) * sq; + q.X = (row2[0] + row0[2]) * sq; + q.Y = (row2[1] + row1[2]) * sq; + } + + q.Normalize(); + return q; + } + #endregion #region Static diff --git a/Source/OpenTK/Math/Matrix4.cs b/Source/OpenTK/Math/Matrix4.cs index 22d2b000..756c1e22 100644 --- a/Source/OpenTK/Math/Matrix4.cs +++ b/Source/OpenTK/Math/Matrix4.cs @@ -28,8 +28,9 @@ using System.Runtime.InteropServices; namespace OpenTK { /// - /// Represents a 4x4 Matrix + /// Represents a 4x4 matrix containing 3D rotation, scale, transform, and projection. /// + /// [Serializable] [StructLayout(LayoutKind.Sequential)] public struct Matrix4 : IEquatable @@ -260,116 +261,6 @@ namespace OpenTK /// public float M44 { get { return Row3.W; } set { Row3.W = value; } } - /// - /// Returns a copy of this instance with scale transform removed. - /// - public Matrix4 Normalized() - { - Matrix4 m = this; - m.Normalize(); - return m; - } - - /// - /// Removes scale transform from this instance. - /// - public void Normalize() - { - var determinant = this.Determinant; - Row0 /= determinant; - Row1 /= determinant; - Row2 /= determinant; - Row3 /= determinant; - } - - /// - /// Returns an inverted copy of this instance. - /// - public Matrix4 Inverted() - { - Matrix4 m = this; - if (m.Determinant != 0) - m.Invert(); - return m; - } - - /// - /// Returns the translation component of this instance. - /// - public Vector3 ExtractTranslation() { return Row3.Xyz; } - - /// - /// Returns the scale component of this instance. - /// - public Vector3 ExtractScale() { return new Vector3 (Row0.Length, Row1.Length, Row2.Length); } - - /// - /// Returns the rotation component of this instance. Quite slow. - /// - /// Whether the method should operate on a row-normalised (i.e. scale == 1) version of the Matrix. Pass false if you know it's already normalised. - public Quaternion ExtractRotation(bool row_normalise = true) - { - var row0 = Row0.Xyz; - var row1 = Row1.Xyz; - var row2 = Row2.Xyz; - - if (row_normalise) - { - if (row0.LengthSquared != 1) row0 = row0.Normalized(); - if (row1.LengthSquared != 1) row1 = row1.Normalized(); - if (row2.LengthSquared != 1) row2 = row2.Normalized(); - } - - // code below adapted from Blender - - Quaternion q = new Quaternion(); - double trace = 0.25 * (row0[0] + row1[1] + row2[2] + 1.0); - - if (trace > 0) - { - double sq = Math.Sqrt(trace); - - q.W = (float)sq; - sq = 1.0 / (4.0 * sq); - q.X = (float)((row1[2] - row2[1]) * sq); - q.Y = (float)((row2[0] - row0[2]) * sq); - q.Z = (float)((row0[1] - row1[0]) * sq); - } - else if (row0[0] > row1[1] && row0[0] > row2[2]) - { - double sq = 2.0 * Math.Sqrt(1.0 + row0[0] - row1[1] - row2[2]); - - q.X = (float)(0.25 * sq); - sq = 1.0 / sq; - q.W = (float)((row2[1] - row1[2]) * sq); - q.Y = (float)((row1[0] + row0[1]) * sq); - q.Z = (float)((row2[0] + row0[2]) * sq); - } - else if (row1[1] > row2[2]) - { - double sq = 2.0 * Math.Sqrt(1.0 + row1[1] - row0[0] - row2[2]); - - q.Y = (float)(0.25 * sq); - sq = 1.0 / sq; - q.W = (float)((row2[0] - row0[2]) * sq); - q.X = (float)((row1[0] + row0[1]) * sq); - q.Z = (float)((row2[1] + row1[2]) * sq); - } - else - { - double sq = 2.0 * Math.Sqrt(1.0 + row2[2] - row0[0] - row1[1]); - - q.Z = (float)(0.25 * sq); - sq = 1.0 / sq; - q.W = (float)((row1[0] - row0[1]) * sq); - q.X = (float)((row2[0] + row0[2]) * sq); - q.Y = (float)((row2[1] + row1[2]) * sq); - } - - q.Normalize(); - return q; - } - #endregion #region Indexers @@ -425,6 +316,116 @@ namespace OpenTK #endregion + /// + /// Returns a normalised copy of this instance. + /// + public Matrix4 Normalized() + { + Matrix4 m = this; + m.Normalize(); + return m; + } + + /// + /// Divides each element in the Matrix by the . + /// + public void Normalize() + { + var determinant = this.Determinant; + Row0 /= determinant; + Row1 /= determinant; + Row2 /= determinant; + Row3 /= determinant; + } + + /// + /// Returns an inverted copy of this instance. + /// + public Matrix4 Inverted() + { + Matrix4 m = this; + if (m.Determinant != 0) + m.Invert(); + return m; + } + + /// + /// Returns the translation component of this instance. + /// + public Vector3 ExtractTranslation() { return Row3.Xyz; } + + /// + /// Returns the scale component of this instance. + /// + public Vector3 ExtractScale() { return new Vector3(Row0.Length, Row1.Length, Row2.Length); } + + /// + /// Returns the rotation component of this instance. Quite slow. + /// + /// Whether the method should row-normalise (i.e. remove scale from) the Matrix. Pass false if you know it's already normalised. + public Quaternion ExtractRotation(bool row_normalise = true) + { + var row0 = Row0.Xyz; + var row1 = Row1.Xyz; + var row2 = Row2.Xyz; + + if (row_normalise) + { + row0 = row0.Normalized(); + row1 = row1.Normalized(); + row2 = row2.Normalized(); + } + + // code below adapted from Blender + + Quaternion q = new Quaternion(); + double trace = 0.25 * (row0[0] + row1[1] + row2[2] + 1.0); + + if (trace > 0) + { + double sq = Math.Sqrt(trace); + + q.W = (float)sq; + sq = 1.0 / (4.0 * sq); + q.X = (float)((row1[2] - row2[1]) * sq); + q.Y = (float)((row2[0] - row0[2]) * sq); + q.Z = (float)((row0[1] - row1[0]) * sq); + } + else if (row0[0] > row1[1] && row0[0] > row2[2]) + { + double sq = 2.0 * Math.Sqrt(1.0 + row0[0] - row1[1] - row2[2]); + + q.X = (float)(0.25 * sq); + sq = 1.0 / sq; + q.W = (float)((row2[1] - row1[2]) * sq); + q.Y = (float)((row1[0] + row0[1]) * sq); + q.Z = (float)((row2[0] + row0[2]) * sq); + } + else if (row1[1] > row2[2]) + { + double sq = 2.0 * Math.Sqrt(1.0 + row1[1] - row0[0] - row2[2]); + + q.Y = (float)(0.25 * sq); + sq = 1.0 / sq; + q.W = (float)((row2[0] - row0[2]) * sq); + q.X = (float)((row1[0] + row0[1]) * sq); + q.Z = (float)((row2[1] + row1[2]) * sq); + } + else + { + double sq = 2.0 * Math.Sqrt(1.0 + row2[2] - row0[0] - row1[1]); + + q.Z = (float)(0.25 * sq); + sq = 1.0 / sq; + q.W = (float)((row1[0] - row0[1]) * sq); + q.X = (float)((row2[0] + row0[2]) * sq); + q.Y = (float)((row2[1] + row1[2]) * sq); + } + + q.Normalize(); + return q; + } + #endregion #region Static diff --git a/Source/OpenTK/Math/Matrix4d.cs b/Source/OpenTK/Math/Matrix4d.cs index 39059169..108c9ff5 100644 --- a/Source/OpenTK/Math/Matrix4d.cs +++ b/Source/OpenTK/Math/Matrix4d.cs @@ -28,8 +28,9 @@ using System.Runtime.InteropServices; namespace OpenTK { /// - /// Represents a 4x4 Matrix with double-precision components. + /// Represents a 4x4 matrix containing 3D rotation, scale, transform, and projection with double-precision components. /// + /// [Serializable] [StructLayout(LayoutKind.Sequential)] public struct Matrix4d : IEquatable @@ -298,6 +299,116 @@ namespace OpenTK #endregion + /// + /// Returns a normalised copy of this instance. + /// + public Matrix4d Normalized() + { + Matrix4d m = this; + m.Normalize(); + return m; + } + + /// + /// Divides each element in the Matrix by the . + /// + public void Normalize() + { + var determinant = this.Determinant; + Row0 /= determinant; + Row1 /= determinant; + Row2 /= determinant; + Row3 /= determinant; + } + + /// + /// Returns an inverted copy of this instance. + /// + public Matrix4d Inverted() + { + Matrix4d m = this; + if (m.Determinant != 0) + m.Invert(); + return m; + } + + /// + /// Returns the translation component of this instance. + /// + public Vector3d ExtractTranslation() { return Row3.Xyz; } + + /// + /// Returns the scale component of this instance. + /// + public Vector3d ExtractScale() { return new Vector3d(Row0.Length, Row1.Length, Row2.Length); } + + /// + /// Returns the rotation component of this instance. Quite slow. + /// + /// Whether the method should row-normalise (i.e. remove scale from) the Matrix. Pass false if you know it's already normalised. + public Quaterniond ExtractRotation(bool row_normalise = true) + { + var row0 = Row0.Xyz; + var row1 = Row1.Xyz; + var row2 = Row2.Xyz; + + if (row_normalise) + { + row0 = row0.Normalized(); + row1 = row1.Normalized(); + row2 = row2.Normalized(); + } + + // code below adapted from Blender + + Quaterniond q = new Quaterniond(); + double trace = 0.25 * (row0[0] + row1[1] + row2[2] + 1.0); + + if (trace > 0) + { + double sq = Math.Sqrt(trace); + + q.W = sq; + sq = 1.0 / (4.0 * sq); + q.X = (row1[2] - row2[1]) * sq; + q.Y = (row2[0] - row0[2]) * sq; + q.Z = (row0[1] - row1[0]) * sq; + } + else if (row0[0] > row1[1] && row0[0] > row2[2]) + { + double sq = 2.0 * Math.Sqrt(1.0 + row0[0] - row1[1] - row2[2]); + + q.X = 0.25 * sq; + sq = 1.0 / sq; + q.W = (row2[1] - row1[2]) * sq; + q.Y = (row1[0] + row0[1]) * sq; + q.Z = (row2[0] + row0[2]) * sq; + } + else if (row1[1] > row2[2]) + { + double sq = 2.0 * Math.Sqrt(1.0 + row1[1] - row0[0] - row2[2]); + + q.Y = 0.25 * sq; + sq = 1.0 / sq; + q.W = (row2[0] - row0[2]) * sq; + q.X = (row1[0] + row0[1]) * sq; + q.Z = (row2[1] + row1[2]) * sq; + } + else + { + double sq = 2.0 * Math.Sqrt(1.0 + row2[2] - row0[0] - row1[1]); + + q.Z = 0.25 * sq; + sq = 1.0 / sq; + q.W = (row1[0] - row0[1]) * sq; + q.X = (row2[0] + row0[2]) * sq; + q.Y = (row2[1] + row1[2]) * sq; + } + + q.Normalize(); + return q; + } + #endregion #region Static diff --git a/Source/OpenTK/Math/Quaternion.cs b/Source/OpenTK/Math/Quaternion.cs index fa08a01d..57f93bb9 100644 --- a/Source/OpenTK/Math/Quaternion.cs +++ b/Source/OpenTK/Math/Quaternion.cs @@ -199,11 +199,17 @@ namespace OpenTK return q; } + /// + /// Reverses the rotation angle of this Quaterniond. + /// public void Invert() { W = -W; } + /// + /// Returns a copy of this Quaterniond with its rotation angle reversed. + /// public Quaternion Inverted() { var q = this; @@ -228,7 +234,7 @@ namespace OpenTK #region public void Conjugate() /// - /// Convert this quaternion to its conjugate + /// Inverts the Vector3 component of this Quaternion. /// public void Conjugate() { diff --git a/Source/OpenTK/Math/Quaterniond.cs b/Source/OpenTK/Math/Quaterniond.cs index 46aec949..8d384312 100644 --- a/Source/OpenTK/Math/Quaterniond.cs +++ b/Source/OpenTK/Math/Quaterniond.cs @@ -189,6 +189,34 @@ namespace OpenTK #endregion + /// + /// Returns a copy of the Quaterniond scaled to unit length. + /// + public Quaterniond Normalized() + { + Quaterniond q = this; + q.Normalize(); + return q; + } + + /// + /// Reverses the rotation angle of this Quaterniond. + /// + public void Invert() + { + W = -W; + } + + /// + /// Returns a copy of this Quaterniond with its rotation angle reversed. + /// + public Quaterniond Inverted() + { + var q = this; + q.Invert(); + return q; + } + #region public void Normalize() /// @@ -206,7 +234,7 @@ namespace OpenTK #region public void Conjugate() /// - /// Convert this Quaterniond to its conjugate + /// Inverts the Vector3d component of this Quaterniond. /// public void Conjugate() { diff --git a/Source/OpenTK/Math/Vector4d.cs b/Source/OpenTK/Math/Vector4d.cs index 9370354c..236d6db0 100644 --- a/Source/OpenTK/Math/Vector4d.cs +++ b/Source/OpenTK/Math/Vector4d.cs @@ -341,7 +341,7 @@ namespace OpenTK #endregion /// - /// Returns a copy of the Vector4d scaled to unid length. + /// Returns a copy of the Vector4d scaled to unit length. /// public Vector4d Normalized() {