1 //------------------------------------------------------------------------------
2 // File : mat33impl.hpp
3 //------------------------------------------------------------------------------
4 // GLVU : Copyright 1997 - 2002
5 // The University of North Carolina at Chapel Hill
6 //------------------------------------------------------------------------------
7 // Permission to use, copy, modify, distribute and sell this software and its
8 // documentation for any purpose is hereby granted without fee, provided that
9 // the above copyright notice appear in all copies and that both that copyright
10 // notice and this permission notice appear in supporting documentation.
11 // Binaries may be compiled with this software without any royalties or
14 // The University of North Carolina at Chapel Hill makes no representations
15 // about the suitability of this software for any purpose. It is provided
16 // "as is" without express or implied warranty.
18 //============================================================================
19 // mat33impl.hpp : 3x3 matrix template.
20 // This is the template implementation file. It is included by mat33.hpp.
21 //============================================================================
23 //---------------------------------------------------------------------------
25 //---------------------------------------------------------------------------
33 Mat33<Type>::Mat33(const Type *N)
35 M[0]=N[0]; M[3]=N[3]; M[6]=N[6];
36 M[1]=N[1]; M[4]=N[4]; M[7]=N[7];
37 M[2]=N[2]; M[5]=N[5]; M[8]=N[8];
41 Mat33<Type>::Mat33(Type M0, Type M3, Type M6,
42 Type M1, Type M4, Type M7,
43 Type M2, Type M5, Type M8)
46 M[0]=M0; M[3]=M3; M[6]=M6;
47 M[1]=M1; M[4]=M4; M[7]=M7;
48 M[2]=M2; M[5]=M5; M[8]=M8;
51 //---------------------------------------------------------------------------
52 // MATRIX/MATRIX AND MATRIX/VECTOR OPERATORS
53 //---------------------------------------------------------------------------
55 Mat33<Type>& Mat33<Type>::operator = (const Mat33& A) // ASSIGNMENT (=)
57 M[0]=A.M[0]; M[3]=A.M[3]; M[6]=A.M[6];
58 M[1]=A.M[1]; M[4]=A.M[4]; M[7]=A.M[7];
59 M[2]=A.M[2]; M[5]=A.M[5]; M[8]=A.M[8];
64 Mat33<Type>& Mat33<Type>::operator = (const Type* a) {
65 for (int i=0;i<9;i++) {
72 Mat33<Type> Mat33<Type>::operator * (const Mat33& A) const // MULTIPLICATION (*)
75 M[0]*A.M[0] + M[3]*A.M[1] + M[6]*A.M[2], // ROW 1
76 M[0]*A.M[3] + M[3]*A.M[4] + M[6]*A.M[5],
77 M[0]*A.M[6] + M[3]*A.M[7] + M[6]*A.M[8],
79 M[1]*A.M[0] + M[4]*A.M[1] + M[7]*A.M[2], // ROW 2
80 M[1]*A.M[3] + M[4]*A.M[4] + M[7]*A.M[5],
81 M[1]*A.M[6] + M[4]*A.M[7] + M[7]*A.M[8],
83 M[2]*A.M[0] + M[5]*A.M[1] + M[8]*A.M[2], // ROW 3
84 M[2]*A.M[3] + M[5]*A.M[4] + M[8]*A.M[5],
85 M[2]*A.M[6] + M[5]*A.M[7] + M[8]*A.M[8]);
89 // MAT-VECTOR MULTIPLICATION (*)
91 Vec3<Type> Mat33<Type>::operator * (const Vec3<Type>& V) const
94 NewV.x = M[0]*V.x + M[3]*V.y + M[6]*V.z;
95 NewV.y = M[1]*V.x + M[4]*V.y + M[7]*V.z;
96 NewV.z = M[2]*V.x + M[5]*V.y + M[8]*V.z;
100 // MAT-VECTOR PRE-MULTIPLICATON (*) (non-member)
101 // interpreted as V^t M
103 Vec3<Type> operator *(const Vec3<Type>& V, const Mat33<Type>& A)
106 NewV.x = A[0]*V.x + A[1]*V.y + A[2]*V.z;
107 NewV.y = A[3]*V.x + A[4]*V.y + A[5]*V.z;
108 NewV.z = A[6]*V.x + A[7]*V.y + A[8]*V.z;
112 // SCALAR POST-MULTIPLICATION
114 Mat33<Type> Mat33<Type>::operator * (Type a) const
117 for (int i = 0; i < 9; i++)
122 // SCALAR PRE-MULTIPLICATION (non-member)
123 template <class Type>
124 Mat33<Type> operator * (Type a, const Mat44<Type>& M)
127 for (int i = 0; i < 9; i++)
132 template <class Type>
133 Mat33<Type> Mat33<Type>::operator / (Type a) const // SCALAR DIVISION
136 Type ainv = Type(1.0)/a;
137 for (int i = 0; i < 9; i++)
142 template <class Type>
143 Mat33<Type> Mat33<Type>::operator + (Mat33& N) const // ADDITION (+)
146 for (int i = 0; i < 9; i++)
147 NewM[i] = M[i]+N.M[i];
150 template <class Type>
151 Mat33<Type>& Mat33<Type>::operator += (Mat33& N) // ACCUMULATE ADD (+=)
153 for (int i = 0; i < 9; i++)
158 template <class Type>
159 Mat33<Type>& Mat33<Type>::operator -= (Mat33& N) // ACCUMULATE SUB (-=)
161 for (int i = 0; i < 9; i++)
166 template <class Type>
167 Mat33<Type>& Mat33<Type>::operator *= (Type a) // ACCUMULATE MULTIPLY (*=)
169 for (int i = 0; i < 9; i++)
173 template <class Type>
174 Mat33<Type>& Mat33<Type>::operator /= (Type a) // ACCUMULATE DIVIDE (/=)
176 Type ainv = Type(1.0)/a;
177 for (int i = 0; i < 9; i++)
183 bool Mat33<Type>::Inverse(Mat33<Type> &inv, Type tolerance) const // MATRIX INVERSE
185 // Invert using cofactors.
187 inv[0] = M[4]*M[8] - M[7]*M[5];
188 inv[3] = M[6]*M[5] - M[3]*M[8];
189 inv[6] = M[3]*M[7] - M[6]*M[4];
190 inv[1] = M[7]*M[2] - M[1]*M[8];
191 inv[4] = M[0]*M[8] - M[6]*M[2];
192 inv[7] = M[6]*M[1] - M[0]*M[7];
193 inv[2] = M[1]*M[5] - M[4]*M[2];
194 inv[5] = M[3]*M[2] - M[0]*M[5];
195 inv[8] = M[0]*M[4] - M[3]*M[1];
197 Type det = M[0]*inv[0] + M[3]*inv[1] + M[6]*inv[2];
199 if (fabs(det) <= tolerance) // singular
202 Type invDet = 1.0f / det;
203 for (int i = 0; i < 9; i++) {
211 Mat33<Type>::operator const Type*() const
217 Mat33<Type>::operator Type*()
223 void Mat33<Type>::RowMajor(Type m[9])
225 m[0] = M[0]; m[1] = M[3]; m[2] = M[6];
226 m[3] = M[1]; m[4] = M[4]; m[5] = M[7];
227 m[6] = M[2]; m[7] = M[5]; m[8] = M[8];
231 Type& Mat33<Type>::operator()(int col, int row)
237 const Type& Mat33<Type>::operator()(int col, int row) const
243 void Mat33<Type>::Set(const Type* a)
245 for (int i=0;i<9;i++) {
251 void Mat33<Type>::Set(Type M0, Type M3, Type M6,
252 Type M1, Type M4, Type M7,
253 Type M2, Type M5, Type M8)
255 M[0]=M0; M[3]=M3; M[6]=M6;
256 M[1]=M1; M[4]=M4; M[7]=M7;
257 M[2]=M2; M[5]=M5; M[8]=M8;
261 //---------------------------------------------------------------------------
262 // Standard Matrix Operations
263 //---------------------------------------------------------------------------
265 void Mat33<Type>::Identity()
268 M[1]=M[2]=M[3]=M[5]=M[6]=M[7]=0;
271 void Mat33<Type>::Zero()
273 M[0]=M[1]=M[2]=M[3]=M[4]=M[5]=M[6]=M[7]=M[8]=0;
277 void Mat33<Type>::Transpose()
284 //---------------------------------------------------------------------------
285 // Standard Matrix Affine Transformations
286 //---------------------------------------------------------------------------
288 void Mat33<Type>::Scale(Type Sx, Type Sy, Type Sz)
290 M[0]=Sx; M[3]=0; M[6]=0;
291 M[1]=0; M[4]=Sy; M[7]=0;
292 M[2]=0; M[5]=0; M[8]=Sz;
296 void Mat33<Type>::Scale(const Vec3<Type>& S)
298 M[0]=S.x; M[3]=0; M[6]=0;
299 M[1]=0; M[4]=S.y; M[7]=0;
300 M[2]=0; M[5]=0; M[8]=S.z;
304 void Mat33<Type>::invScale(Type Sx, Type Sy, Type Sz)
306 M[0]=1/Sx; M[3]=0; M[6]=0;
307 M[1]=0; M[4]=1/Sy; M[7]=0;
308 M[2]=0; M[5]=0; M[8]=1/Sz;
312 void Mat33<Type>::invScale(const Vec3<Type>& S)
314 M[0]=1/S.x; M[3]=0; M[6]=0;
315 M[1]=0; M[4]=1/S.y; M[7]=0;
316 M[2]=0; M[5]=0; M[8]=1/S.z;
320 void Mat33<Type>::Rotate(Type DegAng, const Vec3<Type>& Axis)
322 Type RadAng = DegAng*Mat33TORADS;
323 Type ca=(Type)cos(RadAng),
324 sa=(Type)sin(RadAng);
325 if (Axis.x==1 && Axis.y==0 && Axis.z==0) // ABOUT X-AXIS
327 M[0]=1; M[3]=0; M[6]=0;
328 M[1]=0; M[4]=ca; M[7]=-sa;
329 M[2]=0; M[5]=sa; M[8]=ca;
332 else if (Axis.x==0 && Axis.y==1 && Axis.z==0) // ABOUT Y-AXIS
334 M[0]=ca; M[3]=0; M[6]=sa;
335 M[1]=0; M[4]=1; M[7]=0;
336 M[2]=-sa; M[5]=0; M[8]=ca;
338 else if (Axis.x==0 && Axis.y==0 && Axis.z==1) // ABOUT Z-AXIS
340 M[0]=ca; M[3]=-sa; M[6]=0;
341 M[1]=sa; M[4]=ca; M[7]=0;
342 M[2]=0; M[5]=0; M[8]=1;
344 else // ARBITRARY AXIS
346 Type l = Axis.LengthSqr();
348 x=Axis.x, y=Axis.y, z=Axis.z;
349 if (l > Type(1.0001) || l < Type(0.9999) && l!=0)
351 // needs normalization
355 Type x2=x*x, y2=y*y, z2=z*z;
356 M[0]=x2+ca*(1-x2); M[3]=(x*y)+ca*(-x*y)+sa*(-z); M[6]=(x*z)+ca*(-x*z)+sa*y;
357 M[1]=(x*y)+ca*(-x*y)+sa*z; M[4]=y2+ca*(1-y2); M[7]=(y*z)+ca*(-y*z)+sa*(-x);
358 M[2]=(x*z)+ca*(-x*z)+sa*(-y); M[5]=(y*z)+ca*(-y*z)+sa*x; M[8]=z2+ca*(1-z2);
363 void Mat33<Type>::invRotate(Type DegAng, const Vec3<Type>& Axis)
369 template <class Type>
370 inline void Mat33<Type>::Star(const Vec3<Type>& v)
372 M[0]= 0; M[3]=-v.z; M[6]= v.y;
373 M[1]= v.z; M[4]= 0; M[7]=-v.x;
374 M[2]=-v.y; M[5]= v.x; M[8]= 0;
377 template <class Type>
378 inline void Mat33<Type>::OuterProduct(const Vec3<Type>& u, const Vec3<Type>& v)
380 M[0]=u.x*v.x; M[3]=u.x*v.y; M[6]=u.x*v.z;
381 M[1]=u.y*v.x; M[4]=u.y*v.y; M[7]=u.y*v.z;
382 M[2]=u.z*v.x; M[5]=u.z*v.y; M[8]=u.z*v.z;
386 inline Type Mat33<Type>::Trace() const
388 return M[0] + M[4] + M[8];
391 //---------------------------------------------------------------------------
392 // Handy matrix printing routine.
393 //---------------------------------------------------------------------------
395 void Mat33<Type>::Print() const
397 printf("\n%f %f %f\n",M[0],M[3],M[6]);
398 printf("%f %f %f\n",M[1],M[4],M[7]);
399 printf("%f %f %f\n",M[2],M[5],M[8]);
402 //---------------------------------------------------------------------------
403 // Copy contents of matrix into matrix array.
404 //---------------------------------------------------------------------------
406 void Mat33<Type>::CopyInto(Type *Mat) const
408 Mat[0]=M[0]; Mat[3]=M[3]; Mat[6]=M[6];
409 Mat[1]=M[1]; Mat[4]=M[4]; Mat[7]=M[7];
410 Mat[2]=M[2]; Mat[5]=M[5]; Mat[8]=M[8];