Permalink
Browse files

Copied Matrix4 changes to other classes

* Removed  pointless LengthSquared check from ExtractRotation()
* Improved inline documentation
  • Loading branch information...
Artfunkel committed Mar 20, 2013
1 parent 498b659 commit ca7e2c9c4b754055aeca4afa70435bce09924c59
@@ -28,7 +28,7 @@
namespace OpenTK
{
/// <summary>
- /// Represents a 3x3 Matrix
+ /// Represents a 3x3 matrix containing 3D rotation and scale.
/// </summary>
[Serializable]
[StructLayout(LayoutKind.Sequential)]
@@ -254,7 +254,111 @@ public void Transpose()
}
#endregion
-
+
+ /// <summary>
+ /// Returns a normalised copy of this instance.
+ /// </summary>
+ public Matrix3 Normalized()
+ {
+ Matrix3 m = this;
+ m.Normalize();
+ return m;
+ }
+
+ /// <summary>
+ /// Divides each element in the Matrix by the <see cref="Determinant"/>.
+ /// </summary>
+ public void Normalize()
+ {
+ var determinant = this.Determinant;
+ Row0 /= determinant;
+ Row1 /= determinant;
+ Row2 /= determinant;
+ }
+
+ /// <summary>
+ /// Returns an inverted copy of this instance.
+ /// </summary>
+ public Matrix3 Inverted()
+ {
+ Matrix3 m = this;
+ if (m.Determinant != 0)
+ m.Invert();
+ return m;
+ }
+
+ /// <summary>
+ /// Returns the scale component of this instance.
+ /// </summary>
+ public Vector3 ExtractScale() { return new Vector3(Row0.Length, Row1.Length, Row2.Length); }
+
+ /// <summary>
+ /// Returns the rotation component of this instance. Quite slow.
+ /// </summary>
+ /// <param name="row_normalise">Whether the method should row-normalise (i.e. remove scale from) the Matrix. Pass false if you know it's already normalised.</param>
+ 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
@@ -28,7 +28,7 @@
namespace OpenTK
{
/// <summary>
- /// Represents a 3x3 Matrix
+ /// Represents a 3x3 matrix containing 3D rotation and scale with double-precision components.
/// </summary>
[Serializable]
[StructLayout(LayoutKind.Sequential)]
@@ -250,7 +250,111 @@ public void Transpose()
}
#endregion
-
+
+ /// <summary>
+ /// Returns a normalised copy of this instance.
+ /// </summary>
+ public Matrix3d Normalized()
+ {
+ Matrix3d m = this;
+ m.Normalize();
+ return m;
+ }
+
+ /// <summary>
+ /// Divides each element in the Matrix by the <see cref="Determinant"/>.
+ /// </summary>
+ public void Normalize()
+ {
+ var determinant = this.Determinant;
+ Row0 /= determinant;
+ Row1 /= determinant;
+ Row2 /= determinant;
+ }
+
+ /// <summary>
+ /// Returns an inverted copy of this instance.
+ /// </summary>
+ public Matrix3d Inverted()
+ {
+ Matrix3d m = this;
+ if (m.Determinant != 0)
+ m.Invert();
+ return m;
+ }
+
+ /// <summary>
+ /// Returns the scale component of this instance.
+ /// </summary>
+ public Vector3d ExtractScale() { return new Vector3d(Row0.Length, Row1.Length, Row2.Length); }
+
+ /// <summary>
+ /// Returns the rotation component of this instance. Quite slow.
+ /// </summary>
+ /// <param name="row_normalise">Whether the method should row-normalise (i.e. remove scale from) the Matrix. Pass false if you know it's already normalised.</param>
+ 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
Oops, something went wrong.

0 comments on commit ca7e2c9

Please sign in to comment.