From 498b659d587555bec3501f17e1ecadf832bf353b Mon Sep 17 00:00:00 2001 From: Tom Edwards Date: Tue, 19 Mar 2013 15:28:31 +0000 Subject: [PATCH] Changes suggested by Rob Rouhani * Turned the new Matrix4 properties into methods * Matrix4.Normalize() now behaves correctly. ExtractRotation() does its own row normalisation. --- Source/OpenTK/Math/Matrix4.cs | 127 ++++++++++++++++++---------------- 1 file changed, 69 insertions(+), 58 deletions(-) diff --git a/Source/OpenTK/Math/Matrix4.cs b/Source/OpenTK/Math/Matrix4.cs index 39033a20..22d2b000 100644 --- a/Source/OpenTK/Math/Matrix4.cs +++ b/Source/OpenTK/Math/Matrix4.cs @@ -275,9 +275,11 @@ namespace OpenTK /// public void Normalize() { - Row0.Xyz = Row0.Xyz.Normalized(); - Row1.Xyz = Row1.Xyz.Normalized(); - Row2.Xyz = Row2.Xyz.Normalized(); + var determinant = this.Determinant; + Row0 /= determinant; + Row1 /= determinant; + Row2 /= determinant; + Row3 /= determinant; } /// @@ -292,71 +294,80 @@ namespace OpenTK } /// - /// Gets the translation component of this instance. + /// Returns the translation component of this instance. /// - public Vector3 TranslationPart { get { return Row3.Xyz; } } + public Vector3 ExtractTranslation() { return Row3.Xyz; } /// - /// Gets the scale component of this instance. + /// Returns the scale component of this instance. /// - public Vector3 ScalePart { get { return new Vector3 (Row0.Length, Row1.Length, Row2.Length); } } + public Vector3 ExtractScale() { return new Vector3 (Row0.Length, Row1.Length, Row2.Length); } /// - /// Gets the rotation component of this instance. The Matrix MUST be normalized first. + /// Returns the rotation component of this instance. Quite slow. /// - public Quaternion RotationPart + /// 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) { - get + var row0 = Row0.Xyz; + var row1 = Row1.Xyz; + var row2 = Row2.Xyz; + + if (row_normalise) { - Quaternion q = new Quaternion(); - - // Adapted from Blender - double trace = 0.25 * (Row0[0] + Row1[1] + Row2[2] + 1.0); - - if (trace > 0.0f) - { - 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; + 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