Skip to content

Commit

Permalink
Copied Matrix4 changes to other classes
Browse files Browse the repository at this point in the history 
* Removed  pointless LengthSquared check from ExtractRotation()
* Improved inline documentation
  • Loading branch information
@Artfunkel
Artfunkel committed Mar 20, 2013
1 parent 498b659 commit ca7e2c9
Show file tree
Hide file tree
Showing 7 changed files with 425 additions and 71 deletions.
108 changes: 106 additions & 2 deletions Source/OpenTK/Math/Matrix3.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
namespace OpenTK
{
/// <summary>
/// Represents a 3x3 Matrix
/// Represents a 3x3 matrix containing 3D rotation and scale.
/// </summary>
[Serializable]
[StructLayout(LayoutKind.Sequential)]
Expand Down Expand Up @@ -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
Expand Down
108 changes: 106 additions & 2 deletions Source/OpenTK/Math/Matrix3d.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
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)]
Expand Down Expand Up @@ -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
Expand Down
Loading

0 comments on commit ca7e2c9

Please sign in to comment.