external/src/OpenTK/OpenTK/Math/Matrix3.cs

   1 /*
   2 Copyright (c) 2006 - 2008 The Open Toolkit library.
   3
   4 Permission is hereby granted, free of charge, to any person obtaining a copy of
   5 this software and associated documentation files (the "Software"), to deal in
   6 the Software without restriction, including without limitation the rights to
   7 use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
   8 of the Software, and to permit persons to whom the Software is furnished to do
   9 so, subject to the following conditions:
  10
  11 The above copyright notice and this permission notice shall be included in all
  12 copies or substantial portions of the Software.
  13
  14 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  15 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  16 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  17 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  18 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  19 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  20 SOFTWARE.
  21  */
  22
  23 using System;
  24 using System.Runtime.InteropServices;
  25
  26 namespace OpenTK
  27 {
  28     /// <summary>
  29     /// Represents a 3x3 matrix containing 3D rotation and scale.
  30     /// </summary>
  31     [Serializable]
  32     [StructLayout(LayoutKind.Sequential)]
  33     public struct Matrix3 : IEquatable<Matrix3>
  34     {
  35         /// <summary>
  36         /// First row of the matrix.
  37         /// </summary>
  38         public Vector3 Row0;
  39
  40         /// <summary>
  41         /// Second row of the matrix.
  42         /// </summary>
  43         public Vector3 Row1;
  44
  45         /// <summary>
  46         /// Third row of the matrix.
  47         /// </summary>
  48         public Vector3 Row2;
  49
  50         /// <summary>
  51         /// The identity matrix.
  52         /// </summary>
  53         public static readonly Matrix3 Identity = new Matrix3(Vector3.UnitX, Vector3.UnitY, Vector3.UnitZ);
  54
  55         /// <summary>
  56         /// The zero matrix.
  57         /// </summary>
  58         public static readonly Matrix3 Zero = new Matrix3(Vector3.Zero, Vector3.Zero, Vector3.Zero);
  59
  60
  61
  62         /// <summary>
  63         /// Constructs a new instance.
  64         /// </summary>
  65         /// <param name="row0">Top row of the matrix</param>
  66         /// <param name="row1">Second row of the matrix</param>
  67         /// <param name="row2">Bottom row of the matrix</param>
  68         public Matrix3(Vector3 row0, Vector3 row1, Vector3 row2)
  69         {
  70             Row0 = row0;
  71             Row1 = row1;
  72             Row2 = row2;
  73         }
  74
  75         /// <summary>
  76         /// Constructs a new instance.
  77         /// </summary>
  78         /// <param name="m00">First item of the first row of the matrix.</param>
  79         /// <param name="m01">Second item of the first row of the matrix.</param>
  80         /// <param name="m02">Third item of the first row of the matrix.</param>
  81         /// <param name="m10">First item of the second row of the matrix.</param>
  82         /// <param name="m11">Second item of the second row of the matrix.</param>
  83         /// <param name="m12">Third item of the second row of the matrix.</param>
  84         /// <param name="m20">First item of the third row of the matrix.</param>
  85         /// <param name="m21">Second item of the third row of the matrix.</param>
  86         /// <param name="m22">Third item of the third row of the matrix.</param>
  87         public Matrix3(
  88             float m00, float m01, float m02,
  89             float m10, float m11, float m12,
  90             float m20, float m21, float m22)
  91         {
  92             Row0 = new Vector3(m00, m01, m02);
  93             Row1 = new Vector3(m10, m11, m12);
  94             Row2 = new Vector3(m20, m21, m22);
  95         }
  96
  97         /// <summary>
  98         /// Constructs a new instance.
  99         /// </summary>
 100         /// <param name="matrix">A Matrix4 to take the upper-left 3x3 from.</param>
 101         public Matrix3(Matrix4 matrix)
 102         {
 103             Row0 = matrix.Row0.Xyz;
 104             Row1 = matrix.Row1.Xyz;
 105             Row2 = matrix.Row2.Xyz;
 106         }
 107
 108
 109
 110
 111         /// <summary>
 112         /// Gets the determinant of this matrix.
 113         /// </summary>
 114         public float Determinant
 115         {
 116             get
 117             {
 118                 float m11 = Row0.X, m12 = Row0.Y, m13 = Row0.Z,
 119                 m21 = Row1.X, m22 = Row1.Y, m23 = Row1.Z,
 120                 m31 = Row2.X, m32 = Row2.Y, m33 = Row2.Z;
 121
 122                 return m11 * m22 * m33 + m12 * m23 * m31 + m13 * m21 * m32
 123                      - m13 * m22 * m31 - m11 * m23 * m32 - m12 * m21 * m33;
 124             }
 125         }
 126
 127         /// <summary>
 128         /// Gets the first column of this matrix.
 129         /// </summary>
 130         public Vector3 Column0
 131         {
 132             get { return new Vector3(Row0.X, Row1.X, Row2.X); }
 133         }
 134
 135         /// <summary>
 136         /// Gets the second column of this matrix.
 137         /// </summary>
 138         public Vector3 Column1
 139         {
 140             get { return new Vector3(Row0.Y, Row1.Y, Row2.Y); }
 141         }
 142
 143         /// <summary>
 144         /// Gets the third column of this matrix.
 145         /// </summary>
 146         public Vector3 Column2
 147         {
 148             get { return new Vector3(Row0.Z, Row1.Z, Row2.Z); }
 149         }
 150
 151         /// <summary>
 152         /// Gets or sets the value at row 1, column 1 of this instance.
 153         /// </summary>
 154         public float M11 { get { return Row0.X; } set { Row0.X = value; } }
 155
 156         /// <summary>
 157         /// Gets or sets the value at row 1, column 2 of this instance.
 158         /// </summary>
 159         public float M12 { get { return Row0.Y; } set { Row0.Y = value; } }
 160
 161         /// <summary>
 162         /// Gets or sets the value at row 1, column 3 of this instance.
 163         /// </summary>
 164         public float M13 { get { return Row0.Z; } set { Row0.Z = value; } }
 165
 166         /// <summary>
 167         /// Gets or sets the value at row 2, column 1 of this instance.
 168         /// </summary>
 169         public float M21 { get { return Row1.X; } set { Row1.X = value; } }
 170
 171         /// <summary>
 172         /// Gets or sets the value at row 2, column 2 of this instance.
 173         /// </summary>
 174         public float M22 { get { return Row1.Y; } set { Row1.Y = value; } }
 175
 176         /// <summary>
 177         /// Gets or sets the value at row 2, column 3 of this instance.
 178         /// </summary>
 179         public float M23 { get { return Row1.Z; } set { Row1.Z = value; } }
 180
 181         /// <summary>
 182         /// Gets or sets the value at row 3, column 1 of this instance.
 183         /// </summary>
 184         public float M31 { get { return Row2.X; } set { Row2.X = value; } }
 185
 186         /// <summary>
 187         /// Gets or sets the value at row 3, column 2 of this instance.
 188         /// </summary>
 189         public float M32 { get { return Row2.Y; } set { Row2.Y = value; } }
 190
 191         /// <summary>
 192         /// Gets or sets the value at row 3, column 3 of this instance.
 193         /// </summary>
 194         public float M33 { get { return Row2.Z; } set { Row2.Z = value; } }
 195
 196         /// <summary>
 197         /// Gets or sets the values along the main diagonal of the matrix.
 198         /// </summary>
 199         public Vector3 Diagonal
 200         {
 201             get
 202             {
 203                 return new Vector3(Row0.X, Row1.Y, Row2.Z);
 204             }
 205             set
 206             {
 207                 Row0.X = value.X;
 208                 Row1.Y = value.Y;
 209                 Row2.Z = value.Z;
 210             }
 211         }
 212
 213         /// <summary>
 214         /// Gets the trace of the matrix, the sum of the values along the diagonal.
 215         /// </summary>
 216         public float Trace { get { return Row0.X + Row1.Y + Row2.Z; } }
 217
 218
 219         /// <summary>
 220         /// Gets or sets the value at a specified row and column.
 221         /// </summary>
 222         public float this[int rowIndex, int columnIndex]
 223         {
 224             get
 225             {
 226                 if (rowIndex == 0)
 227                 {
 228                     return Row0[columnIndex];
 229                 }
 230                 else if (rowIndex == 1)
 231                 {
 232                     return Row1[columnIndex];
 233                 }
 234                 else if (rowIndex == 2)
 235                 {
 236                     return Row2[columnIndex];
 237                 }
 238                 throw new IndexOutOfRangeException("You tried to access this matrix at: (" + rowIndex + ", " + columnIndex + ")");
 239             }
 240             set
 241             {
 242                 if (rowIndex == 0)
 243                 {
 244                     Row0[columnIndex] = value;
 245                 }
 246                 else if (rowIndex == 1)
 247                 {
 248                     Row1[columnIndex] = value;
 249                 }
 250                 else if (rowIndex == 2)
 251                 {
 252                     Row2[columnIndex] = value;
 253                 }
 254                 else
 255                 {
 256                     throw new IndexOutOfRangeException("You tried to set this matrix at: (" + rowIndex + ", " + columnIndex + ")");
 257                 }
 258             }
 259         }
 260
 261         /// <summary>
 262         /// Converts this instance into its inverse.
 263         /// </summary>
 264         public void Invert()
 265         {
 266             this = Matrix3.Invert(this);
 267         }
 268
 269
 270
 271         /// <summary>
 272         /// Converts this instance into its transpose.
 273         /// </summary>
 274         public void Transpose()
 275         {
 276             this = Matrix3.Transpose(this);
 277         }
 278
 279
 280         /// <summary>
 281         /// Returns a normalised copy of this instance.
 282         /// </summary>
 283         public Matrix3 Normalized()
 284         {
 285             Matrix3 m = this;
 286             m.Normalize();
 287             return m;
 288         }
 289
 290         /// <summary>
 291         /// Divides each element in the Matrix by the <see cref="Determinant"/>.
 292         /// </summary>
 293         public void Normalize()
 294         {
 295             var determinant = this.Determinant;
 296             Row0 /= determinant;
 297             Row1 /= determinant;
 298             Row2 /= determinant;
 299         }
 300
 301         /// <summary>
 302         /// Returns an inverted copy of this instance.
 303         /// </summary>
 304         public Matrix3 Inverted()
 305         {
 306             Matrix3 m = this;
 307             if (m.Determinant != 0)
 308             {
 309                 m.Invert();
 310             }
 311             return m;
 312         }
 313
 314         /// <summary>
 315         /// Returns a copy of this Matrix3 without scale.
 316         /// </summary>
 317         public Matrix3 ClearScale()
 318         {
 319             Matrix3 m = this;
 320             m.Row0 = m.Row0.Normalized();
 321             m.Row1 = m.Row1.Normalized();
 322             m.Row2 = m.Row2.Normalized();
 323             return m;
 324         }
 325         /// <summary>
 326         /// Returns a copy of this Matrix3 without rotation.
 327         /// </summary>
 328         public Matrix3 ClearRotation()
 329         {
 330             Matrix3 m = this;
 331             m.Row0 = new Vector3(m.Row0.Length, 0, 0);
 332             m.Row1 = new Vector3(0, m.Row1.Length, 0);
 333             m.Row2 = new Vector3(0, 0, m.Row2.Length);
 334             return m;
 335         }
 336
 337         /// <summary>
 338         /// Returns the scale component of this instance.
 339         /// </summary>
 340         public Vector3 ExtractScale() { return new Vector3(Row0.Length, Row1.Length, Row2.Length); }
 341
 342         /// <summary>
 343         /// Returns the rotation component of this instance. Quite slow.
 344         /// </summary>
 345         /// <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>
 346         public Quaternion ExtractRotation(bool row_normalise = true)
 347         {
 348             var row0 = Row0;
 349             var row1 = Row1;
 350             var row2 = Row2;
 351
 352             if (row_normalise)
 353             {
 354                 row0 = row0.Normalized();
 355                 row1 = row1.Normalized();
 356                 row2 = row2.Normalized();
 357             }
 358
 359             // code below adapted from Blender
 360
 361             Quaternion q = new Quaternion();
 362             double trace = 0.25 * (row0[0] + row1[1] + row2[2] + 1.0);
 363
 364             if (trace > 0)
 365             {
 366                 double sq = Math.Sqrt(trace);
 367
 368                 q.W = (float)sq;
 369                 sq = 1.0 / (4.0 * sq);
 370                 q.X = (float)((row1[2] - row2[1]) * sq);
 371                 q.Y = (float)((row2[0] - row0[2]) * sq);
 372                 q.Z = (float)((row0[1] - row1[0]) * sq);
 373             }
 374             else if (row0[0] > row1[1] && row0[0] > row2[2])
 375             {
 376                 double sq = 2.0 * Math.Sqrt(1.0 + row0[0] - row1[1] - row2[2]);
 377
 378                 q.X = (float)(0.25 * sq);
 379                 sq = 1.0 / sq;
 380                 q.W = (float)((row2[1] - row1[2]) * sq);
 381                 q.Y = (float)((row1[0] + row0[1]) * sq);
 382                 q.Z = (float)((row2[0] + row0[2]) * sq);
 383             }
 384             else if (row1[1] > row2[2])
 385             {
 386                 double sq = 2.0 * Math.Sqrt(1.0 + row1[1] - row0[0] - row2[2]);
 387
 388                 q.Y = (float)(0.25 * sq);
 389                 sq = 1.0 / sq;
 390                 q.W = (float)((row2[0] - row0[2]) * sq);
 391                 q.X = (float)((row1[0] + row0[1]) * sq);
 392                 q.Z = (float)((row2[1] + row1[2]) * sq);
 393             }
 394             else
 395             {
 396                 double sq = 2.0 * Math.Sqrt(1.0 + row2[2] - row0[0] - row1[1]);
 397
 398                 q.Z = (float)(0.25 * sq);
 399                 sq = 1.0 / sq;
 400                 q.W = (float)((row1[0] - row0[1]) * sq);
 401                 q.X = (float)((row2[0] + row0[2]) * sq);
 402                 q.Y = (float)((row2[1] + row1[2]) * sq);
 403             }
 404
 405             q.Normalize();
 406             return q;
 407         }
 408
 409
 410
 411         /// <summary>
 412         /// Build a rotation matrix from the specified axis/angle rotation.
 413         /// </summary>
 414         /// <param name="axis">The axis to rotate about.</param>
 415         /// <param name="angle">Angle in radians to rotate counter-clockwise (looking in the direction of the given axis).</param>
 416         /// <param name="result">A matrix instance.</param>
 417         public static void CreateFromAxisAngle(Vector3 axis, float angle, out Matrix3 result)
 418         {
 419             //normalize and create a local copy of the vector.
 420             axis.Normalize();
 421             float axisX = axis.X, axisY = axis.Y, axisZ = axis.Z;
 422
 423             //calculate angles
 424             float cos = (float)System.Math.Cos(-angle);
 425             float sin = (float)System.Math.Sin(-angle);
 426             float t = 1.0f - cos;
 427
 428             //do the conversion math once
 429             float tXX = t * axisX * axisX,
 430             tXY = t * axisX * axisY,
 431             tXZ = t * axisX * axisZ,
 432             tYY = t * axisY * axisY,
 433             tYZ = t * axisY * axisZ,
 434             tZZ = t * axisZ * axisZ;
 435
 436             float sinX = sin * axisX,
 437             sinY = sin * axisY,
 438             sinZ = sin * axisZ;
 439
 440             result.Row0.X = tXX + cos;
 441             result.Row0.Y = tXY - sinZ;
 442             result.Row0.Z = tXZ + sinY;
 443             result.Row1.X = tXY + sinZ;
 444             result.Row1.Y = tYY + cos;
 445             result.Row1.Z = tYZ - sinX;
 446             result.Row2.X = tXZ - sinY;
 447             result.Row2.Y = tYZ + sinX;
 448             result.Row2.Z = tZZ + cos;
 449         }
 450
 451         /// <summary>
 452         /// Build a rotation matrix from the specified axis/angle rotation.
 453         /// </summary>
 454         /// <param name="axis">The axis to rotate about.</param>
 455         /// <param name="angle">Angle in radians to rotate counter-clockwise (looking in the direction of the given axis).</param>
 456         /// <returns>A matrix instance.</returns>
 457         public static Matrix3 CreateFromAxisAngle(Vector3 axis, float angle)
 458         {
 459             Matrix3 result;
 460             CreateFromAxisAngle(axis, angle, out result);
 461             return result;
 462         }
 463
 464
 465
 466         /// <summary>
 467         /// Build a rotation matrix from the specified quaternion.
 468         /// </summary>
 469         /// <param name="q">Quaternion to translate.</param>
 470         /// <param name="result">Matrix result.</param>
 471         public static void CreateFromQuaternion(ref Quaternion q, out Matrix3 result)
 472         {
 473             Vector3 axis;
 474             float angle;
 475             q.ToAxisAngle(out axis, out angle);
 476             CreateFromAxisAngle(axis, angle, out result);
 477         }
 478
 479         /// <summary>
 480         /// Build a rotation matrix from the specified quaternion.
 481         /// </summary>
 482         /// <param name="q">Quaternion to translate.</param>
 483         /// <returns>A matrix instance.</returns>
 484         public static Matrix3 CreateFromQuaternion(Quaternion q)
 485         {
 486             Matrix3 result;
 487             CreateFromQuaternion(ref q, out result);
 488             return result;
 489         }
 490
 491
 492
 493         /// <summary>
 494         /// Builds a rotation matrix for a rotation around the x-axis.
 495         /// </summary>
 496         /// <param name="angle">The counter-clockwise angle in radians.</param>
 497         /// <param name="result">The resulting Matrix3 instance.</param>
 498         public static void CreateRotationX(float angle, out Matrix3 result)
 499         {
 500             float cos = (float)System.Math.Cos(angle);
 501             float sin = (float)System.Math.Sin(angle);
 502
 503             result = Identity;
 504             result.Row1.Y = cos;
 505             result.Row1.Z = sin;
 506             result.Row2.Y = -sin;
 507             result.Row2.Z = cos;
 508         }
 509
 510         /// <summary>
 511         /// Builds a rotation matrix for a rotation around the x-axis.
 512         /// </summary>
 513         /// <param name="angle">The counter-clockwise angle in radians.</param>
 514         /// <returns>The resulting Matrix3 instance.</returns>
 515         public static Matrix3 CreateRotationX(float angle)
 516         {
 517             Matrix3 result;
 518             CreateRotationX(angle, out result);
 519             return result;
 520         }
 521
 522         /// <summary>
 523         /// Builds a rotation matrix for a rotation around the y-axis.
 524         /// </summary>
 525         /// <param name="angle">The counter-clockwise angle in radians.</param>
 526         /// <param name="result">The resulting Matrix3 instance.</param>
 527         public static void CreateRotationY(float angle, out Matrix3 result)
 528         {
 529             float cos = (float)System.Math.Cos(angle);
 530             float sin = (float)System.Math.Sin(angle);
 531
 532             result = Identity;
 533             result.Row0.X = cos;
 534             result.Row0.Z = -sin;
 535             result.Row2.X = sin;
 536             result.Row2.Z = cos;
 537         }
 538
 539         /// <summary>
 540         /// Builds a rotation matrix for a rotation around the y-axis.
 541         /// </summary>
 542         /// <param name="angle">The counter-clockwise angle in radians.</param>
 543         /// <returns>The resulting Matrix3 instance.</returns>
 544         public static Matrix3 CreateRotationY(float angle)
 545         {
 546             Matrix3 result;
 547             CreateRotationY(angle, out result);
 548             return result;
 549         }
 550
 551         /// <summary>
 552         /// Builds a rotation matrix for a rotation around the z-axis.
 553         /// </summary>
 554         /// <param name="angle">The counter-clockwise angle in radians.</param>
 555         /// <param name="result">The resulting Matrix3 instance.</param>
 556         public static void CreateRotationZ(float angle, out Matrix3 result)
 557         {
 558             float cos = (float)System.Math.Cos(angle);
 559             float sin = (float)System.Math.Sin(angle);
 560
 561             result = Identity;
 562             result.Row0.X = cos;
 563             result.Row0.Y = sin;
 564             result.Row1.X = -sin;
 565             result.Row1.Y = cos;
 566         }
 567
 568         /// <summary>
 569         /// Builds a rotation matrix for a rotation around the z-axis.
 570         /// </summary>
 571         /// <param name="angle">The counter-clockwise angle in radians.</param>
 572         /// <returns>The resulting Matrix3 instance.</returns>
 573         public static Matrix3 CreateRotationZ(float angle)
 574         {
 575             Matrix3 result;
 576             CreateRotationZ(angle, out result);
 577             return result;
 578         }
 579
 580
 581
 582         /// <summary>
 583         /// Creates a scale matrix.
 584         /// </summary>
 585         /// <param name="scale">Single scale factor for the x, y, and z axes.</param>
 586         /// <returns>A scale matrix.</returns>
 587         public static Matrix3 CreateScale(float scale)
 588         {
 589             Matrix3 result;
 590             CreateScale(scale, out result);
 591             return result;
 592         }
 593
 594         /// <summary>
 595         /// Creates a scale matrix.
 596         /// </summary>
 597         /// <param name="scale">Scale factors for the x, y, and z axes.</param>
 598         /// <returns>A scale matrix.</returns>
 599         public static Matrix3 CreateScale(Vector3 scale)
 600         {
 601             Matrix3 result;
 602             CreateScale(ref scale, out result);
 603             return result;
 604         }
 605
 606         /// <summary>
 607         /// Creates a scale matrix.
 608         /// </summary>
 609         /// <param name="x">Scale factor for the x axis.</param>
 610         /// <param name="y">Scale factor for the y axis.</param>
 611         /// <param name="z">Scale factor for the z axis.</param>
 612         /// <returns>A scale matrix.</returns>
 613         public static Matrix3 CreateScale(float x, float y, float z)
 614         {
 615             Matrix3 result;
 616             CreateScale(x, y, z, out result);
 617             return result;
 618         }
 619
 620         /// <summary>
 621         /// Creates a scale matrix.
 622         /// </summary>
 623         /// <param name="scale">Single scale factor for the x, y, and z axes.</param>
 624         /// <param name="result">A scale matrix.</param>
 625         public static void CreateScale(float scale, out Matrix3 result)
 626         {
 627             result = Identity;
 628             result.Row0.X = scale;
 629             result.Row1.Y = scale;
 630             result.Row2.Z = scale;
 631         }
 632
 633         /// <summary>
 634         /// Creates a scale matrix.
 635         /// </summary>
 636         /// <param name="scale">Scale factors for the x, y, and z axes.</param>
 637         /// <param name="result">A scale matrix.</param>
 638         public static void CreateScale(ref Vector3 scale, out Matrix3 result)
 639         {
 640             result = Identity;
 641             result.Row0.X = scale.X;
 642             result.Row1.Y = scale.Y;
 643             result.Row2.Z = scale.Z;
 644         }
 645
 646         /// <summary>
 647         /// Creates a scale matrix.
 648         /// </summary>
 649         /// <param name="x">Scale factor for the x axis.</param>
 650         /// <param name="y">Scale factor for the y axis.</param>
 651         /// <param name="z">Scale factor for the z axis.</param>
 652         /// <param name="result">A scale matrix.</param>
 653         public static void CreateScale(float x, float y, float z, out Matrix3 result)
 654         {
 655             result = Identity;
 656             result.Row0.X = x;
 657             result.Row1.Y = y;
 658             result.Row2.Z = z;
 659         }
 660
 661
 662         /// <summary>
 663         /// Adds two instances.
 664         /// </summary>
 665         /// <param name="left">The left operand of the addition.</param>
 666         /// <param name="right">The right operand of the addition.</param>
 667         /// <returns>A new instance that is the result of the addition.</returns>
 668         public static Matrix3 Add(Matrix3 left, Matrix3 right)
 669         {
 670             Matrix3 result;
 671             Add(ref left, ref right, out result);
 672             return result;
 673         }
 674
 675         /// <summary>
 676         /// Adds two instances.
 677         /// </summary>
 678         /// <param name="left">The left operand of the addition.</param>
 679         /// <param name="right">The right operand of the addition.</param>
 680         /// <param name="result">A new instance that is the result of the addition.</param>
 681         public static void Add(ref Matrix3 left, ref Matrix3 right, out Matrix3 result)
 682         {
 683             Vector3.Add(ref left.Row0, ref right.Row0, out result.Row0);
 684             Vector3.Add(ref left.Row1, ref right.Row1, out result.Row1);
 685             Vector3.Add(ref left.Row2, ref right.Row2, out result.Row2);
 686         }
 687
 688         /// <summary>
 689         /// Multiplies two instances.
 690         /// </summary>
 691         /// <param name="left">The left operand of the multiplication.</param>
 692         /// <param name="right">The right operand of the multiplication.</param>
 693         /// <returns>A new instance that is the result of the multiplication</returns>
 694         public static Matrix3 Mult(Matrix3 left, Matrix3 right)
 695         {
 696             Matrix3 result;
 697             Mult(ref left, ref right, out result);
 698             return result;
 699         }
 700
 701         /// <summary>
 702         /// Multiplies two instances.
 703         /// </summary>
 704         /// <param name="left">The left operand of the multiplication.</param>
 705         /// <param name="right">The right operand of the multiplication.</param>
 706         /// <param name="result">A new instance that is the result of the multiplication</param>
 707         public static void Mult(ref Matrix3 left, ref Matrix3 right, out Matrix3 result)
 708         {
 709             float   lM11 = left.Row0.X, lM12 = left.Row0.Y, lM13 = left.Row0.Z,
 710             lM21 = left.Row1.X, lM22 = left.Row1.Y, lM23 = left.Row1.Z,
 711             lM31 = left.Row2.X, lM32 = left.Row2.Y, lM33 = left.Row2.Z,
 712             rM11 = right.Row0.X, rM12 = right.Row0.Y, rM13 = right.Row0.Z,
 713             rM21 = right.Row1.X, rM22 = right.Row1.Y, rM23 = right.Row1.Z,
 714             rM31 = right.Row2.X, rM32 = right.Row2.Y, rM33 = right.Row2.Z;
 715
 716             result.Row0.X = ((lM11 * rM11) + (lM12 * rM21)) + (lM13 * rM31);
 717             result.Row0.Y = ((lM11 * rM12) + (lM12 * rM22)) + (lM13 * rM32);
 718             result.Row0.Z = ((lM11 * rM13) + (lM12 * rM23)) + (lM13 * rM33);
 719             result.Row1.X = ((lM21 * rM11) + (lM22 * rM21)) + (lM23 * rM31);
 720             result.Row1.Y = ((lM21 * rM12) + (lM22 * rM22)) + (lM23 * rM32);
 721             result.Row1.Z = ((lM21 * rM13) + (lM22 * rM23)) + (lM23 * rM33);
 722             result.Row2.X = ((lM31 * rM11) + (lM32 * rM21)) + (lM33 * rM31);
 723             result.Row2.Y = ((lM31 * rM12) + (lM32 * rM22)) + (lM33 * rM32);
 724             result.Row2.Z = ((lM31 * rM13) + (lM32 * rM23)) + (lM33 * rM33);
 725         }
 726
 727
 728
 729         /// <summary>
 730         /// Calculate the inverse of the given matrix
 731         /// </summary>
 732         /// <param name="mat">The matrix to invert</param>
 733         /// <param name="result">The inverse of the given matrix if it has one, or the input if it is singular</param>
 734         /// <exception cref="InvalidOperationException">Thrown if the Matrix3 is singular.</exception>
 735         public static void Invert(ref Matrix3 mat, out Matrix3 result)
 736         {
 737             int[] colIdx = { 0, 0, 0 };
 738             int[] rowIdx = { 0, 0, 0 };
 739             int[] pivotIdx = { -1, -1, -1 };
 740
 741             float[,] inverse = {{mat.Row0.X, mat.Row0.Y, mat.Row0.Z},
 742                 {mat.Row1.X, mat.Row1.Y, mat.Row1.Z},
 743                 {mat.Row2.X, mat.Row2.Y, mat.Row2.Z}};
 744
 745             int icol = 0;
 746             int irow = 0;
 747             for (int i = 0; i < 3; i++)
 748             {
 749                 float maxPivot = 0.0f;
 750                 for (int j = 0; j < 3; j++)
 751                 {
 752                     if (pivotIdx[j] != 0)
 753                     {
 754                         for (int k = 0; k < 3; ++k)
 755                         {
 756                             if (pivotIdx[k] == -1)
 757                             {
 758                                 float absVal = System.Math.Abs(inverse[j, k]);
 759                                 if (absVal > maxPivot)
 760                                 {
 761                                     maxPivot = absVal;
 762                                     irow = j;
 763                                     icol = k;
 764                                 }
 765                             }
 766                             else if (pivotIdx[k] > 0)
 767                             {
 768                                 result = mat;
 769                                 return;
 770                             }
 771                         }
 772                     }
 773                 }
 774
 775                 ++(pivotIdx[icol]);
 776
 777                 if (irow != icol)
 778                 {
 779                     for (int k = 0; k < 3; ++k)
 780                     {
 781                         float f = inverse[irow, k];
 782                         inverse[irow, k] = inverse[icol, k];
 783                         inverse[icol, k] = f;
 784                     }
 785                 }
 786
 787                 rowIdx[i] = irow;
 788                 colIdx[i] = icol;
 789
 790                 float pivot = inverse[icol, icol];
 791
 792                 if (pivot == 0.0f)
 793                 {
 794                     throw new InvalidOperationException("Matrix is singular and cannot be inverted.");
 795                 }
 796
 797                 float oneOverPivot = 1.0f / pivot;
 798                 inverse[icol, icol] = 1.0f;
 799                 for (int k = 0; k < 3; ++k)
 800                 {
 801                     inverse[icol, k] *= oneOverPivot;
 802                 }
 803
 804                 for (int j = 0; j < 3; ++j)
 805                 {
 806                     if (icol != j)
 807                     {
 808                         float f = inverse[j, icol];
 809                         inverse[j, icol] = 0.0f;
 810                         for (int k = 0; k < 3; ++k)
 811                         {
 812                             inverse[j, k] -= inverse[icol, k] * f;
 813                         }
 814                     }
 815                 }
 816             }
 817
 818             for (int j = 2; j >= 0; --j)
 819             {
 820                 int ir = rowIdx[j];
 821                 int ic = colIdx[j];
 822                 for (int k = 0; k < 3; ++k)
 823                 {
 824                     float f = inverse[k, ir];
 825                     inverse[k, ir] = inverse[k, ic];
 826                     inverse[k, ic] = f;
 827                 }
 828             }
 829
 830             result.Row0.X = inverse[0, 0];
 831             result.Row0.Y = inverse[0, 1];
 832             result.Row0.Z = inverse[0, 2];
 833             result.Row1.X = inverse[1, 0];
 834             result.Row1.Y = inverse[1, 1];
 835             result.Row1.Z = inverse[1, 2];
 836             result.Row2.X = inverse[2, 0];
 837             result.Row2.Y = inverse[2, 1];
 838             result.Row2.Z = inverse[2, 2];
 839         }
 840
 841         /// <summary>
 842         /// Calculate the inverse of the given matrix
 843         /// </summary>
 844         /// <param name="mat">The matrix to invert</param>
 845         /// <returns>The inverse of the given matrix if it has one, or the input if it is singular</returns>
 846         /// <exception cref="InvalidOperationException">Thrown if the Matrix4 is singular.</exception>
 847         public static Matrix3 Invert(Matrix3 mat)
 848         {
 849             Matrix3 result;
 850             Invert(ref mat, out result);
 851             return result;
 852         }
 853
 854
 855
 856         /// <summary>
 857         /// Calculate the transpose of the given matrix
 858         /// </summary>
 859         /// <param name="mat">The matrix to transpose</param>
 860         /// <returns>The transpose of the given matrix</returns>
 861         public static Matrix3 Transpose(Matrix3 mat)
 862         {
 863             return new Matrix3(mat.Column0, mat.Column1, mat.Column2);
 864         }
 865
 866         /// <summary>
 867         /// Calculate the transpose of the given matrix
 868         /// </summary>
 869         /// <param name="mat">The matrix to transpose</param>
 870         /// <param name="result">The result of the calculation</param>
 871         public static void Transpose(ref Matrix3 mat, out Matrix3 result)
 872         {
 873             result.Row0.X = mat.Row0.X;
 874             result.Row0.Y = mat.Row1.X;
 875             result.Row0.Z = mat.Row2.X;
 876             result.Row1.X = mat.Row0.Y;
 877             result.Row1.Y = mat.Row1.Y;
 878             result.Row1.Z = mat.Row2.Y;
 879             result.Row2.X = mat.Row0.Z;
 880             result.Row2.Y = mat.Row1.Z;
 881             result.Row2.Z = mat.Row2.Z;
 882         }
 883
 884
 885
 886
 887         /// <summary>
 888         /// Matrix multiplication
 889         /// </summary>
 890         /// <param name="left">left-hand operand</param>
 891         /// <param name="right">right-hand operand</param>
 892         /// <returns>A new Matrix3d which holds the result of the multiplication</returns>
 893         public static Matrix3 operator *(Matrix3 left, Matrix3 right)
 894         {
 895             return Matrix3.Mult(left, right);
 896         }
 897
 898         /// <summary>
 899         /// Compares two instances for equality.
 900         /// </summary>
 901         /// <param name="left">The first instance.</param>
 902         /// <param name="right">The second instance.</param>
 903         /// <returns>True, if left equals right; false otherwise.</returns>
 904         public static bool operator ==(Matrix3 left, Matrix3 right)
 905         {
 906             return left.Equals(right);
 907         }
 908
 909         /// <summary>
 910         /// Compares two instances for inequality.
 911         /// </summary>
 912         /// <param name="left">The first instance.</param>
 913         /// <param name="right">The second instance.</param>
 914         /// <returns>True, if left does not equal right; false otherwise.</returns>
 915         public static bool operator !=(Matrix3 left, Matrix3 right)
 916         {
 917             return !left.Equals(right);
 918         }
 919
 920
 921
 922
 923         /// <summary>
 924         /// Returns a System.String that represents the current Matrix3d.
 925         /// </summary>
 926         /// <returns>The string representation of the matrix.</returns>
 927         public override string ToString()
 928         {
 929             return String.Format("{0}\n{1}\n{2}", Row0, Row1, Row2);
 930         }
 931
 932
 933
 934         /// <summary>
 935         /// Returns the hashcode for this instance.
 936         /// </summary>
 937         /// <returns>A System.Int32 containing the unique hashcode for this instance.</returns>
 938         public override int GetHashCode()
 939         {
 940             unchecked
 941             {
 942                 var hashCode = this.Row0.GetHashCode();
 943                 hashCode = (hashCode * 397) ^ this.Row1.GetHashCode();
 944                 hashCode = (hashCode * 397) ^ this.Row2.GetHashCode();
 945                 return hashCode;
 946             }
 947         }
 948
 949
 950
 951         /// <summary>
 952         /// Indicates whether this instance and a specified object are equal.
 953         /// </summary>
 954         /// <param name="obj">The object to compare to.</param>
 955         /// <returns>True if the instances are equal; false otherwise.</returns>
 956         public override bool Equals(object obj)
 957         {
 958             if (!(obj is Matrix3))
 959             {
 960                 return false;
 961             }
 962
 963             return this.Equals((Matrix3)obj);
 964         }
 965
 966
 967
 968
 969
 970         /// <summary>Indicates whether the current matrix is equal to another matrix.</summary>
 971         /// <param name="other">A matrix to compare with this matrix.</param>
 972         /// <returns>true if the current matrix is equal to the matrix parameter; otherwise, false.</returns>
 973         public bool Equals(Matrix3 other)
 974         {
 975             return
 976                 Row0 == other.Row0 &&
 977                     Row1 == other.Row1 &&
 978                     Row2 == other.Row2;
 979         }
 980
 981     }
 982 }