MezzanineEngine 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
matrix3x3.cpp
1 // © Copyright 2010 - 2014 BlackTopp Studios Inc.
2 /* This file is part of The Mezzanine Engine.
3 
4  The Mezzanine Engine is free software: you can redistribute it and/or modify
5  it under the terms of the GNU General Public License as published by
6  the Free Software Foundation, either version 3 of the License, or
7  (at your option) any later version.
8 
9  The Mezzanine Engine is distributed in the hope that it will be useful,
10  but WITHOUT ANY WARRANTY; without even the implied warranty of
11  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  GNU General Public License for more details.
13 
14  You should have received a copy of the GNU General Public License
15  along with The Mezzanine Engine. If not, see <http://www.gnu.org/licenses/>.
16 */
17 /* The original authors have included a copy of the license specified above in the
18  'Docs' folder. See 'gpl.txt'
19 */
20 /* We welcome the use of the Mezzanine engine to anyone, including companies who wish to
21  Build professional software and charge for their product.
22 
23  However there are some practical restrictions, so if your project involves
24  any of the following you should contact us and we will try to work something
25  out:
26  - DRM or Copy Protection of any kind(except Copyrights)
27  - Software Patents You Do Not Wish to Freely License
28  - Any Kind of Linking to Non-GPL licensed Works
29  - Are Currently In Violation of Another Copyright Holder's GPL License
30  - If You want to change our code and not add a few hundred MB of stuff to
31  your distribution
32 
33  These and other limitations could cause serious legal problems if you ignore
34  them, so it is best to simply contact us or the Free Software Foundation, if
35  you have any questions.
36 
37  Joseph Toppi - toppij@gmail.com
38  John Blackwood - makoenergy02@gmail.com
39 */
40 #ifndef _matrix3x3_cpp
41 #define _matrix3x3_cpp
42 
43 #include "matrix3x3.h"
44 #include "mathtool.h"
45 #include "entresol.h"
46 
47 #include "btBulletDynamicsCommon.h"
48 #include <OgreMatrix3.h>
49 
50 namespace Mezzanine
51 {
52  Matrix3x3::Matrix3x3()
53  {
54  SetIdentity();
55  }
56 
57  Matrix3x3::~Matrix3x3()
58  {
59  }
60 
61  ///////////////////////////////////////////////////////////////////////////////
62  // Additional Constructors
63 
64  Matrix3x3::Matrix3x3(const Real& XX, const Real& XY, const Real& XZ, const Real& YX, const Real& YY, const Real& YZ, const Real& ZX, const Real& ZY, const Real& ZZ)
65  {
66  SetValues(XX,XY,XZ,YX,YY,YZ,ZX,ZY,ZZ);
67  }
68 
69  Matrix3x3::Matrix3x3(const Real& Yaw, const Real& Pitch, const Real& Roll)
70  {
71  SetFromEulerZYX(Yaw,Pitch,Roll);
72  }
73 
74  Matrix3x3::Matrix3x3(const Quaternion& Rot)
75  {
76  SetFromQuaternion(Rot);
77  }
78 
79  Matrix3x3::Matrix3x3(const Vector3& Axis, const Real& Angle)
80  {
81  SetFromAxisAngle(Axis,Angle);
82  }
83 
84  Matrix3x3::Matrix3x3(const btMatrix3x3& Mat)
85  {
86  ExtractBulletMatrix3x3(Mat);
87  }
88 
89  Matrix3x3::Matrix3x3(const Ogre::Matrix3& Mat)
90  {
91  ExtractOgreMatrix3x3(Mat);
92  }
93 
94  ///////////////////////////////////////////////////////////////////////////////
95  // Set From Other Data Functions
96 
97  void Matrix3x3::SetValues(const Real& XX, const Real& XY, const Real& XZ, const Real& YX, const Real& YY, const Real& YZ, const Real& ZX, const Real& ZY, const Real& ZZ)
98  {
99  Matrix[0][0] = XX;
100  Matrix[0][1] = XY;
101  Matrix[0][2] = XZ;
102  Matrix[1][0] = YX;
103  Matrix[1][1] = YY;
104  Matrix[1][2] = YZ;
105  Matrix[2][0] = ZX;
106  Matrix[2][1] = ZY;
107  Matrix[2][2] = ZZ;
108  }
109 
110  void Matrix3x3::SetFromEulerZYX(const Real& Yaw, const Real& Pitch, const Real& Roll)
111  {
112  Real Cos, Sin;
113 
114  Cos = MathTools::Cos(Yaw);
115  Sin = MathTools::Sin(Yaw);
116  Matrix3x3 ZMat(Cos,-Sin,0.0,Sin,Cos,0.0,0.0,0.0,1.0);
117 
118  Cos = MathTools::Cos(Pitch);
119  Sin = MathTools::Sin(Pitch);
120  Matrix3x3 YMat(Cos,0.0,Sin,0.0,1.0,0.0,-Sin,0.0,Cos);
121 
122  Cos = MathTools::Cos(Roll);
123  Sin = MathTools::Sin(Roll);
124  Matrix3x3 XMat(1.0,0.0,0.0,0.0,Cos,-Sin,0.0,Sin,Cos);
125 
126  *this = ZMat * (YMat * XMat);
127  }
128 
129  void Matrix3x3::SetFromQuaternion(const Quaternion& Rot)
130  {
131  Real Dist = Rot.LengthSqrd();
132  if(Dist == Real(0.0))
133  { MEZZ_EXCEPTION(Exception::ARITHMETIC_EXCEPTION,"Attempting to set Matrix3x3 with zero length Quaternion, this is bad."); }
134  Real Sc = Real(2.0) / Dist;
135  Real XS = Rot.X * Sc, YS = Rot.Y * Sc, ZS = Rot.Z * Sc;
136  Real WX = Rot.W * XS, WY = Rot.W * YS, WZ = Rot.W * ZS;
137  Real XX = Rot.X * XS, XY = Rot.X * YS, XZ = Rot.X * ZS;
138  Real YY = Rot.Y * YS, YZ = Rot.Y * ZS, ZZ = Rot.Z * ZS;
139  SetValues(Real(1.0) - (YY + ZZ), XY - WZ, XZ + WY,
140  XY + WZ, Real(1.0) - (XX + ZZ), YZ - WX,
141  XZ - WY, YZ + WX, Real(1.0) - (XX + YY));
142  }
143 
144  void Matrix3x3::SetFromAxisAngle(const Vector3& Axis, const Real& Angle)
145  {
146  Real Cos = MathTools::Cos(Angle);
147  Real Sin = MathTools::Sin(Angle);
148  Real OneMinusCos = 1.0 - Cos;
149  Real X2 = Axis.X * Axis.X;
150  Real Y2 = Axis.Y * Axis.Y;
151  Real Z2 = Axis.Z * Axis.Z;
152  Real XYM = Axis.X * Axis.Y * OneMinusCos;
153  Real XZM = Axis.X * Axis.Z * OneMinusCos;
154  Real YZM = Axis.Y * Axis.Z * OneMinusCos;
155  Real XSin = Axis.X * Sin;
156  Real YSin = Axis.Y * Sin;
157  Real ZSin = Axis.Z * Sin;
158 
159  Matrix[0][0] = X2 * OneMinusCos + Cos;
160  Matrix[0][1] = XYM - ZSin;
161  Matrix[0][2] = XZM + YSin;
162  Matrix[1][0] = XYM + ZSin;
163  Matrix[1][1] = Y2 * OneMinusCos + Cos;
164  Matrix[1][2] = YZM - XSin;
165  Matrix[2][0] = XZM - YSin;
166  Matrix[2][1] = YZM + XSin;
167  Matrix[2][2] = Z2 * OneMinusCos + Cos;
168  }
169 
170  void Matrix3x3::SetIdentity()
171  {
172  SetValues(1,0,0,
173  0,1,0,
174  0,0,1);
175  }
176 
177  void Matrix3x3::SetZero()
178  {
179  SetValues(0,0,0,
180  0,0,0,
181  0,0,0);
182  }
183 
184  ///////////////////////////////////////////////////////////////////////////////
185  // Utility and Conversion Functions
186 
187  Real Matrix3x3::GetDeterminant() const
188  {
189  Real Cofactor00 = Matrix[1][1] * Matrix[2][2] - Matrix[1][2] * Matrix[2][1];
190  Real Cofactor10 = Matrix[1][2] * Matrix[2][0] - Matrix[1][0] * Matrix[2][2];
191  Real Cofactor20 = Matrix[1][0] * Matrix[2][1] - Matrix[1][1] * Matrix[2][0];
192 
193  Real Det = Matrix[0][0] * Cofactor00 + Matrix[0][1] * Cofactor10 + Matrix[0][2] * Cofactor20;
194 
195  return Det;
196  }
197 
198  Quaternion Matrix3x3::GetAsQuaternion() const
199  {
200  Real Trace = Matrix[0][0] + Matrix[1][1] + Matrix[2][2];
201  Quaternion Ret;
202 
203  if (Trace > Real(0.0))
204  {
205  Real Sc = MathTools::Sqrt(Trace + Real(1.0));
206  Ret.W = (Sc * Real(0.5));
207  Sc = Real(0.5) / Sc;
208 
209  Ret.X = ( (Matrix[2][1] - Matrix[1][2]) * Sc );
210  Ret.Y = ( (Matrix[0][2] - Matrix[2][0]) * Sc );
211  Ret.Z = ( (Matrix[1][0] - Matrix[0][1]) * Sc );
212  }
213  else
214  {
215  int I = Matrix[0][0] < Matrix[1][1] ?
216  ( Matrix[1][1] < Matrix[2][2] ? 2 : 1 ) :
217  ( Matrix[0][0] < Matrix[2][2] ? 2 : 0 );
218  int J = (I + 1) % 3;
219  int K = (I + 2) % 3;
220 
221  Real Sc = MathTools::Sqrt(Matrix[I][I] - Matrix[J][J] - Matrix[K][K] + Real(1.0));
222  Ret[I] = Sc * Real(0.5);
223  Sc = Real(0.5) / Sc;
224 
225  Ret[J] = ( Matrix[J][I] + Matrix[I][J] ) * Sc;
226  Ret[K] = ( Matrix[K][I] + Matrix[I][K] ) * Sc;
227  Ret[3] = ( Matrix[K][J] - Matrix[J][K] ) * Sc;
228  }
229  return Ret;
230  }
231 
232  void Matrix3x3::ExtractBulletMatrix3x3(const btMatrix3x3& temp)
233  {
234  btVector3 Col1 = temp.getColumn(0);
235  btVector3 Col2 = temp.getColumn(1);
236  btVector3 Col3 = temp.getColumn(2);
237 
238  Matrix[0][0] = Col1.x();
239  Matrix[0][1] = Col2.x();
240  Matrix[0][2] = Col3.x();
241  Matrix[1][0] = Col1.y();
242  Matrix[1][1] = Col2.y();
243  Matrix[1][2] = Col3.y();
244  Matrix[2][0] = Col1.z();
245  Matrix[2][1] = Col2.z();
246  Matrix[2][2] = Col3.z();
247  }
248 
249  btMatrix3x3 Matrix3x3::GetBulletMatrix3x3() const
250  {
251  return btMatrix3x3(Matrix[0][0],Matrix[0][1],Matrix[0][2],Matrix[1][0],Matrix[1][1],Matrix[1][2],Matrix[2][0],Matrix[2][1],Matrix[2][2]);
252  }
253 
254  void Matrix3x3::ExtractOgreMatrix3x3(const Ogre::Matrix3& temp)
255  {
256  Ogre::Vector3 Col1 = temp.GetColumn(0);
257  Ogre::Vector3 Col2 = temp.GetColumn(1);
258  Ogre::Vector3 Col3 = temp.GetColumn(2);
259 
260  Matrix[0][0] = Col1.x;
261  Matrix[0][1] = Col2.x;
262  Matrix[0][2] = Col3.x;
263  Matrix[1][0] = Col1.y;
264  Matrix[1][1] = Col2.y;
265  Matrix[1][2] = Col3.y;
266  Matrix[2][0] = Col1.z;
267  Matrix[2][1] = Col2.z;
268  Matrix[2][2] = Col3.z;
269  }
270 
271  Ogre::Matrix3 Matrix3x3::GetOgreMatrix3x3() const
272  {
273  return Ogre::Matrix3(Matrix[0][0],Matrix[0][1],Matrix[0][2],Matrix[1][0],Matrix[1][1],Matrix[1][2],Matrix[2][0],Matrix[2][1],Matrix[2][2]);
274  }
275 
276  ///////////////////////////////////////////////////////////////////////////////
277  // Comparison Operators
278 
279  bool Matrix3x3::operator==(const Matrix3x3& Other) const
280  {
281  for( Whole Row = 0 ; Row < 3 ; ++Row )
282  {
283  for( Whole Col = 0 ; Col < 3 ; ++Col )
284  {
285  if( Matrix[Row][Col] != Other.Matrix[Row][Col] )
286  return false;
287  }
288  }
289  return true;
290  }
291 
292  bool Matrix3x3::operator!=(const Matrix3x3& Other) const
293  {
294  for( Whole Row = 0 ; Row < 3 ; ++Row )
295  {
296  for( Whole Col = 0 ; Col < 3 ; ++Col )
297  {
298  if( Matrix[Row][Col] == Other.Matrix[Row][Col] )
299  return false;
300  }
301  }
302  return true;
303  }
304 
305  ///////////////////////////////////////////////////////////////////////////////
306  // Arithmetic Operators With Matrix3x3
307 
308  Matrix3x3 Matrix3x3::operator+(const Matrix3x3& Other) const
309  {
310  Matrix3x3 Ret;
311  for( Whole Row = 0 ; Row < 3 ; ++Row )
312  {
313  for( Whole Col = 0 ; Col < 3 ; ++Col )
314  {
315  Ret.Matrix[Row][Col] = Matrix[Row][Col] + Other.Matrix[Row][Col];
316  }
317  }
318  return Ret;
319  }
320 
321  Matrix3x3 Matrix3x3::operator-(const Matrix3x3& Other) const
322  {
323  Matrix3x3 Ret;
324  for( Whole Row = 0 ; Row < 3 ; ++Row )
325  {
326  for( Whole Col = 0 ; Col < 3 ; ++Col )
327  {
328  Ret.Matrix[Row][Col] = Matrix[Row][Col] - Other.Matrix[Row][Col];
329  }
330  }
331  return Ret;
332  }
333 
334  Matrix3x3 Matrix3x3::operator*(const Matrix3x3& Other) const
335  {
336  Matrix3x3 Ret;
337  for( Whole Row = 0 ; Row < 3 ; ++Row )
338  {
339  for( Whole Col = 0 ; Col < 3 ; ++Col )
340  {
341  Ret.Matrix[Row][Col] =
342  Matrix[Row][0] * Other.Matrix[0][Col] +
343  Matrix[Row][1] * Other.Matrix[1][Col] +
344  Matrix[Row][2] * Other.Matrix[2][Col];
345  }
346  }
347  return Ret;
348  }
349 
350  Matrix3x3& Matrix3x3::operator+=(const Matrix3x3& Other)
351  {
352  for( Whole Row = 0 ; Row < 3 ; ++Row )
353  {
354  for( Whole Col = 0 ; Col < 3 ; ++Col )
355  {
356  Matrix[Row][Col] += Other.Matrix[Row][Col];
357  }
358  }
359  return *this;
360  }
361 
362  Matrix3x3& Matrix3x3::operator-=(const Matrix3x3& Other)
363  {
364  for( Whole Row = 0 ; Row < 3 ; ++Row )
365  {
366  for( Whole Col = 0 ; Col < 3 ; ++Col )
367  {
368  Matrix[Row][Col] -= Other.Matrix[Row][Col];
369  }
370  }
371  return *this;
372  }
373 
374  Matrix3x3& Matrix3x3::operator*=(const Matrix3x3& Other)
375  {
376  (*this) = (*this) * Other;
377  return *this;
378  }
379 
380  ///////////////////////////////////////////////////////////////////////////////
381  // Arithmetic Operators With Other Datatypes
382 
383  Vector3 Matrix3x3::operator*(const Vector3& Vec) const
384  {
385  Vector3 Ret;
386 
387  Ret.X = Matrix[0][0] * Vec.X + Matrix[0][1] * Vec.Y + Matrix[0][2] * Vec.Z;
388  Ret.Y = Matrix[1][0] * Vec.X + Matrix[1][1] * Vec.Y + Matrix[1][2] * Vec.Z;
389  Ret.Z = Matrix[2][0] * Vec.X + Matrix[2][1] * Vec.Y + Matrix[2][2] * Vec.Z;
390 
391  return Ret;
392  }
393 
394  Matrix3x3 Matrix3x3::operator*(const Real& Scaler) const
395  {
396  Matrix3x3 Ret;
397  for( Whole Row = 0 ; Row < 3 ; ++Row )
398  {
399  for( Whole Col = 0 ; Col < 3 ; ++Col )
400  {
401  Ret.Matrix[Row][Col] = Matrix[Row][Col] * Scaler;
402  }
403  }
404  return Ret;
405  }
406 
407  Matrix3x3& Matrix3x3::operator*=(const Real& Scaler)
408  {
409  for( Whole Row = 0 ; Row < 3 ; ++Row )
410  {
411  for( Whole Col = 0 ; Col < 3 ; ++Col )
412  {
413  Matrix[Row][Col] *= Scaler;
414  }
415  }
416  return *this;
417  }
418 
419  ///////////////////////////////////////////////////////////////////////////////
420  // Other Operators
421 
422  void Matrix3x3::operator=(const Matrix3x3& Other)
423  {
424  for( Whole Row = 0 ; Row < 3 ; ++Row )
425  {
426  for( Whole Col = 0 ; Col < 3 ; ++Col )
427  {
428  Matrix[Row][Col] = Other.Matrix[Row][Col];
429  }
430  }
431  }
432 
433  Matrix3x3 Matrix3x3::operator-() const
434  {
435  Matrix3x3 Ret;
436  for( Whole Row = 0 ; Row < 3 ; ++Row )
437  {
438  for( Whole Col = 0 ; Col < 3 ; ++Col )
439  {
440  Ret.Matrix[Row][Col] = -Matrix[Row][Col];
441  }
442  }
443  return Ret;
444  }
445 
446  ///////////////////////////////////////////////////////////////////////////////
447  // Fancy Math
448 
449  Matrix3x3 Matrix3x3::Transpose() const
450  {
451  Matrix3x3 Ret;
452  for( Whole Row = 0 ; Row < 3 ; Row++ )
453  {
454  for( Whole Col = 0 ; Col < 3 ; Col++ )
455  Ret.Matrix[Row][Col] = Matrix[Col][Row];
456  }
457  return Ret;
458  }
459 
460  Matrix3x3 Matrix3x3::Adjoint() const
461  {
462  return Matrix3x3(CoFactor(1,1,2,2), CoFactor(0,2,2,1), CoFactor(0,1,1,2),
463  CoFactor(1,2,2,0), CoFactor(0,0,2,2), CoFactor(0,2,1,0),
464  CoFactor(1,0,2,1), CoFactor(0,1,2,0), CoFactor(0,0,1,1));
465  }
466 
467  Matrix3x3 Matrix3x3::Inverse() const
468  {
469  Matrix3x3 Ret;
470  Ret.Matrix[0][0] = Matrix[1][1] * Matrix[2][2] - Matrix[1][2] * Matrix[2][1];
471  Ret.Matrix[0][1] = Matrix[0][2] * Matrix[2][1] - Matrix[0][1] * Matrix[2][2];
472  Ret.Matrix[0][2] = Matrix[0][1] * Matrix[1][2] - Matrix[0][2] * Matrix[1][1];
473  Ret.Matrix[1][0] = Matrix[1][2] * Matrix[2][0] - Matrix[1][0] * Matrix[2][2];
474  Ret.Matrix[1][1] = Matrix[0][0] * Matrix[2][2] - Matrix[0][2] * Matrix[2][0];
475  Ret.Matrix[1][2] = Matrix[0][2] * Matrix[1][0] - Matrix[0][0] * Matrix[1][2];
476  Ret.Matrix[2][0] = Matrix[1][0] * Matrix[2][1] - Matrix[1][1] * Matrix[2][0];
477  Ret.Matrix[2][1] = Matrix[0][1] * Matrix[2][0] - Matrix[0][0] * Matrix[2][1];
478  Ret.Matrix[2][2] = Matrix[0][0] * Matrix[1][1] - Matrix[0][1] * Matrix[1][0];
479 
480  Real Det = Matrix[0][0] * Ret.Matrix[0][0] + Matrix[0][1] * Ret.Matrix[1][0] + Matrix[0][2] * Ret.Matrix[2][0];
481 
482  if( 0 == Det )
483  { MEZZ_EXCEPTION(Exception::ARITHMETIC_EXCEPTION,"Determinant calculated in Matrix3x3::Inverse is zero. Avoiding divide by zero."); }
484 
485  Real InvDet = 1.0 / Det;
486  for( Whole Row = 0 ; Row < 3 ; Row++)
487  {
488  for( Whole Col = 0 ; Col < 3 ; Col++)
489  Ret.Matrix[Row][Col] *= InvDet;
490  }
491  return Ret;
492  }
493 
494  Real Matrix3x3::CoFactor(const Whole& Row1, const Whole& Col1, const Whole& Row2, const Whole& Col2) const
495  {
496  return Matrix[Row1][Col1] * Matrix[Row2][Col2] - Matrix[Row1][Col2] * Matrix[Row2][Col1];
497  }
498 
499  void Matrix3x3::SetScale(const Vector3& Scaling)
500  {
501  Matrix[0][0] *= Scaling.X;
502  Matrix[0][1] *= Scaling.Y;
503  Matrix[0][2] *= Scaling.Z;
504  Matrix[1][0] *= Scaling.X;
505  Matrix[1][1] *= Scaling.Y;
506  Matrix[1][2] *= Scaling.Z;
507  Matrix[2][0] *= Scaling.X;
508  Matrix[2][1] *= Scaling.Y;
509  Matrix[2][2] *= Scaling.Z;
510  }
511 
512  bool Matrix3x3::HasScaling() const
513  {
514  Real Test = Matrix[0][0] * Matrix[0][0] + Matrix[1][0] * Matrix[1][0] + Matrix[2][0] * Matrix[2][0];
515  if (!MathTools::WithinTolerance(Test,1.0,(Real)1e-04))
516  return true;
517  Test = Matrix[0][1] * Matrix[0][1] + Matrix[1][1] * Matrix[1][1] + Matrix[2][1] * Matrix[2][1];
518  if (!MathTools::WithinTolerance(Test,1.0,(Real)1e-04))
519  return true;
520  Test = Matrix[0][2] * Matrix[0][2] + Matrix[1][2] * Matrix[1][2] + Matrix[2][2] * Matrix[2][2];
521  if (!MathTools::WithinTolerance(Test,1.0,(Real)1e-04))
522  return true;
523 
524  return false;
525  }
526 }//Mezzanine
527 
528 #endif
Generated on Mon Jan 6 2014 20:58:05 for MezzanineEngine by doxygen 1.8.4