source: GTP/trunk/Lib/Illum/GPUObscurancesGT/src/MATRIX4X4.cpp @ 930

Revision 930, 24.4 KB checked in by igarcia, 19 years ago (diff)
RevLine 
[930]1//////////////////////////////////////////////////////////////////////////////////////////
2//      MATRIX4X4.cpp
3//      function definitions for 4x4 matrix class
4//      You may use this code however you wish, but if you do, please credit me and
5//      provide a link to my website in a readme file or similar
6//      Downloaded from: www.paulsprojects.net
7//      Created:        20th July 2002
8//      Updated:        19th August 2002        -       Corrected 2nd SetPerspective for n!=1.0f
9//                              26th September 2002     -       Added nudge to prevent artifacts with infinite far plane
10//                                                                      -       Improved speed
11//                              7th November 2002       -       Added Affine Inverse functions
12//                                                                      -       Changed constructors
13//                                                                      -       Added special cases for row3 = (0, 0, 0, 1)
14//                              17th December 2002      -       Converted from radians to degrees for consistency
15//                                                                              with OpenGL. Should have been done a long time ago...
16//////////////////////////////////////////////////////////////////////////////////////////     
17#include <memory.h>
18#include "Maths.h"
19
20MATRIX4X4::MATRIX4X4(float e0, float e1, float e2, float e3,
21                                        float e4, float e5, float e6, float e7,
22                                        float e8, float e9, float e10, float e11,
23                                        float e12, float e13, float e14, float e15)
24{
25        entries[0]=e0;
26        entries[1]=e1;
27        entries[2]=e2;
28        entries[3]=e3;
29        entries[4]=e4;
30        entries[5]=e5;
31        entries[6]=e6;
32        entries[7]=e7;
33        entries[8]=e8;
34        entries[9]=e9;
35        entries[10]=e10;
36        entries[11]=e11;
37        entries[12]=e12;
38        entries[13]=e13;
39        entries[14]=e14;
40        entries[15]=e15;
41}
42
43MATRIX4X4::MATRIX4X4(const MATRIX4X4 & rhs)
44{
45        memcpy(entries, rhs.entries, 16*sizeof(float));
46}
47
48MATRIX4X4::MATRIX4X4(const float * rhs)
49{
50        memcpy(entries, rhs, 16*sizeof(float));
51}
52
53void MATRIX4X4::SetEntry(int position, float value)
54{
55        if(position>=0 && position<=15)
56                entries[position]=value;
57}
58       
59float MATRIX4X4::GetEntry(int position) const
60{
61        if(position>=0 && position<=15)
62                return entries[position];
63
64        return 0.0f;
65}
66
67VECTOR4D MATRIX4X4::GetRow(int position) const
68{
69        if(position==0)
70                return VECTOR4D(entries[0], entries[4], entries[8], entries[12]);
71       
72        if(position==1)
73                return VECTOR4D(entries[1], entries[5], entries[9], entries[13]);
74       
75        if(position==2)
76                return VECTOR4D(entries[2], entries[6], entries[10], entries[14]);
77       
78        if(position==3)
79                return VECTOR4D(entries[3], entries[7], entries[11], entries[15]);
80
81        return VECTOR4D(0.0f, 0.0f, 0.0f, 0.0f);
82}
83
84VECTOR4D MATRIX4X4::GetColumn(int position) const
85{
86        if(position==0)
87                return VECTOR4D(entries[0], entries[1], entries[2], entries[3]);
88       
89        if(position==1)
90                return VECTOR4D(entries[4], entries[5], entries[6], entries[7]);
91       
92        if(position==2)
93                return VECTOR4D(entries[8], entries[9], entries[10], entries[11]);
94       
95        if(position==3)
96                return VECTOR4D(entries[12], entries[13], entries[14], entries[15]);
97
98        return VECTOR4D(0.0f, 0.0f, 0.0f, 0.0f);
99}
100
101void MATRIX4X4::LoadIdentity(void)
102{
103        memset(entries, 0, 16*sizeof(float));
104        entries[0]=1.0f;
105        entries[5]=1.0f;
106        entries[10]=1.0f;
107        entries[15]=1.0f;
108}
109
110void MATRIX4X4::LoadZero(void)
111{
112        memset(entries, 0, 16*sizeof(float));
113}
114
115MATRIX4X4 MATRIX4X4::operator+(const MATRIX4X4 & rhs) const             //overloaded operators
116{
117        return MATRIX4X4(       entries[0]+rhs.entries[0],
118                                                entries[1]+rhs.entries[1],
119                                                entries[2]+rhs.entries[2],
120                                                entries[3]+rhs.entries[3],
121                                                entries[4]+rhs.entries[4],
122                                                entries[5]+rhs.entries[5],
123                                                entries[6]+rhs.entries[6],
124                                                entries[7]+rhs.entries[7],
125                                                entries[8]+rhs.entries[8],
126                                                entries[9]+rhs.entries[9],
127                                                entries[10]+rhs.entries[10],
128                                                entries[11]+rhs.entries[11],
129                                                entries[12]+rhs.entries[12],
130                                                entries[13]+rhs.entries[13],
131                                                entries[14]+rhs.entries[14],
132                                                entries[15]+rhs.entries[15]);
133}
134
135MATRIX4X4 MATRIX4X4::operator-(const MATRIX4X4 & rhs) const             //overloaded operators
136{
137        return MATRIX4X4(       entries[0]-rhs.entries[0],
138                                                entries[1]-rhs.entries[1],
139                                                entries[2]-rhs.entries[2],
140                                                entries[3]-rhs.entries[3],
141                                                entries[4]-rhs.entries[4],
142                                                entries[5]-rhs.entries[5],
143                                                entries[6]-rhs.entries[6],
144                                                entries[7]-rhs.entries[7],
145                                                entries[8]-rhs.entries[8],
146                                                entries[9]-rhs.entries[9],
147                                                entries[10]-rhs.entries[10],
148                                                entries[11]-rhs.entries[11],
149                                                entries[12]-rhs.entries[12],
150                                                entries[13]-rhs.entries[13],
151                                                entries[14]-rhs.entries[14],
152                                                entries[15]-rhs.entries[15]);
153}
154
155MATRIX4X4 MATRIX4X4::operator*(const MATRIX4X4 & rhs) const
156{
157        //Optimise for matrices in which bottom row is (0, 0, 0, 1) in both matrices
158        if(     entries[3]==0.0f && entries[7]==0.0f && entries[11]==0.0f && entries[15]==1.0f  &&
159                rhs.entries[3]==0.0f && rhs.entries[7]==0.0f &&
160                rhs.entries[11]==0.0f && rhs.entries[15]==1.0f)
161        {
162                return MATRIX4X4(       entries[0]*rhs.entries[0]+entries[4]*rhs.entries[1]+entries[8]*rhs.entries[2],
163                                                        entries[1]*rhs.entries[0]+entries[5]*rhs.entries[1]+entries[9]*rhs.entries[2],
164                                                        entries[2]*rhs.entries[0]+entries[6]*rhs.entries[1]+entries[10]*rhs.entries[2],
165                                                        0.0f,
166                                                        entries[0]*rhs.entries[4]+entries[4]*rhs.entries[5]+entries[8]*rhs.entries[6],
167                                                        entries[1]*rhs.entries[4]+entries[5]*rhs.entries[5]+entries[9]*rhs.entries[6],
168                                                        entries[2]*rhs.entries[4]+entries[6]*rhs.entries[5]+entries[10]*rhs.entries[6],
169                                                        0.0f,
170                                                        entries[0]*rhs.entries[8]+entries[4]*rhs.entries[9]+entries[8]*rhs.entries[10],
171                                                        entries[1]*rhs.entries[8]+entries[5]*rhs.entries[9]+entries[9]*rhs.entries[10],
172                                                        entries[2]*rhs.entries[8]+entries[6]*rhs.entries[9]+entries[10]*rhs.entries[10],
173                                                        0.0f,
174                                                        entries[0]*rhs.entries[12]+entries[4]*rhs.entries[13]+entries[8]*rhs.entries[14]+entries[12],
175                                                        entries[1]*rhs.entries[12]+entries[5]*rhs.entries[13]+entries[9]*rhs.entries[14]+entries[13],
176                                                        entries[2]*rhs.entries[12]+entries[6]*rhs.entries[13]+entries[10]*rhs.entries[14]+entries[14],
177                                                        1.0f);
178        }
179
180        //Optimise for when bottom row of 1st matrix is (0, 0, 0, 1)
181        if(     entries[3]==0.0f && entries[7]==0.0f && entries[11]==0.0f && entries[15]==1.0f)
182        {
183                return MATRIX4X4(       entries[0]*rhs.entries[0]+entries[4]*rhs.entries[1]+entries[8]*rhs.entries[2]+entries[12]*rhs.entries[3],
184                                                        entries[1]*rhs.entries[0]+entries[5]*rhs.entries[1]+entries[9]*rhs.entries[2]+entries[13]*rhs.entries[3],
185                                                        entries[2]*rhs.entries[0]+entries[6]*rhs.entries[1]+entries[10]*rhs.entries[2]+entries[14]*rhs.entries[3],
186                                                        rhs.entries[3],
187                                                        entries[0]*rhs.entries[4]+entries[4]*rhs.entries[5]+entries[8]*rhs.entries[6]+entries[12]*rhs.entries[7],
188                                                        entries[1]*rhs.entries[4]+entries[5]*rhs.entries[5]+entries[9]*rhs.entries[6]+entries[13]*rhs.entries[7],
189                                                        entries[2]*rhs.entries[4]+entries[6]*rhs.entries[5]+entries[10]*rhs.entries[6]+entries[14]*rhs.entries[7],
190                                                        rhs.entries[7],
191                                                        entries[0]*rhs.entries[8]+entries[4]*rhs.entries[9]+entries[8]*rhs.entries[10]+entries[12]*rhs.entries[11],
192                                                        entries[1]*rhs.entries[8]+entries[5]*rhs.entries[9]+entries[9]*rhs.entries[10]+entries[13]*rhs.entries[11],
193                                                        entries[2]*rhs.entries[8]+entries[6]*rhs.entries[9]+entries[10]*rhs.entries[10]+entries[14]*rhs.entries[11],
194                                                        rhs.entries[11],
195                                                        entries[0]*rhs.entries[12]+entries[4]*rhs.entries[13]+entries[8]*rhs.entries[14]+entries[12]*rhs.entries[15],
196                                                        entries[1]*rhs.entries[12]+entries[5]*rhs.entries[13]+entries[9]*rhs.entries[14]+entries[13]*rhs.entries[15],
197                                                        entries[2]*rhs.entries[12]+entries[6]*rhs.entries[13]+entries[10]*rhs.entries[14]+entries[14]*rhs.entries[15],
198                                                        rhs.entries[15]);
199        }
200
201        //Optimise for when bottom row of 2nd matrix is (0, 0, 0, 1)
202        if(     rhs.entries[3]==0.0f && rhs.entries[7]==0.0f &&
203                rhs.entries[11]==0.0f && rhs.entries[15]==1.0f)
204        {
205                return MATRIX4X4(       entries[0]*rhs.entries[0]+entries[4]*rhs.entries[1]+entries[8]*rhs.entries[2],
206                                                        entries[1]*rhs.entries[0]+entries[5]*rhs.entries[1]+entries[9]*rhs.entries[2],
207                                                        entries[2]*rhs.entries[0]+entries[6]*rhs.entries[1]+entries[10]*rhs.entries[2],
208                                                        entries[3]*rhs.entries[0]+entries[7]*rhs.entries[1]+entries[11]*rhs.entries[2],
209                                                        entries[0]*rhs.entries[4]+entries[4]*rhs.entries[5]+entries[8]*rhs.entries[6],
210                                                        entries[1]*rhs.entries[4]+entries[5]*rhs.entries[5]+entries[9]*rhs.entries[6],
211                                                        entries[2]*rhs.entries[4]+entries[6]*rhs.entries[5]+entries[10]*rhs.entries[6],
212                                                        entries[3]*rhs.entries[4]+entries[7]*rhs.entries[5]+entries[11]*rhs.entries[6],
213                                                        entries[0]*rhs.entries[8]+entries[4]*rhs.entries[9]+entries[8]*rhs.entries[10],
214                                                        entries[1]*rhs.entries[8]+entries[5]*rhs.entries[9]+entries[9]*rhs.entries[10],
215                                                        entries[2]*rhs.entries[8]+entries[6]*rhs.entries[9]+entries[10]*rhs.entries[10],
216                                                        entries[3]*rhs.entries[8]+entries[7]*rhs.entries[9]+entries[11]*rhs.entries[10],
217                                                        entries[0]*rhs.entries[12]+entries[4]*rhs.entries[13]+entries[8]*rhs.entries[14]+entries[12],
218                                                        entries[1]*rhs.entries[12]+entries[5]*rhs.entries[13]+entries[9]*rhs.entries[14]+entries[13],
219                                                        entries[2]*rhs.entries[12]+entries[6]*rhs.entries[13]+entries[10]*rhs.entries[14]+entries[14],
220                                                        entries[3]*rhs.entries[12]+entries[7]*rhs.entries[13]+entries[11]*rhs.entries[14]+entries[15]);
221        }       
222       
223        return MATRIX4X4(       entries[0]*rhs.entries[0]+entries[4]*rhs.entries[1]+entries[8]*rhs.entries[2]+entries[12]*rhs.entries[3],
224                                                entries[1]*rhs.entries[0]+entries[5]*rhs.entries[1]+entries[9]*rhs.entries[2]+entries[13]*rhs.entries[3],
225                                                entries[2]*rhs.entries[0]+entries[6]*rhs.entries[1]+entries[10]*rhs.entries[2]+entries[14]*rhs.entries[3],
226                                                entries[3]*rhs.entries[0]+entries[7]*rhs.entries[1]+entries[11]*rhs.entries[2]+entries[15]*rhs.entries[3],
227                                                entries[0]*rhs.entries[4]+entries[4]*rhs.entries[5]+entries[8]*rhs.entries[6]+entries[12]*rhs.entries[7],
228                                                entries[1]*rhs.entries[4]+entries[5]*rhs.entries[5]+entries[9]*rhs.entries[6]+entries[13]*rhs.entries[7],
229                                                entries[2]*rhs.entries[4]+entries[6]*rhs.entries[5]+entries[10]*rhs.entries[6]+entries[14]*rhs.entries[7],
230                                                entries[3]*rhs.entries[4]+entries[7]*rhs.entries[5]+entries[11]*rhs.entries[6]+entries[15]*rhs.entries[7],
231                                                entries[0]*rhs.entries[8]+entries[4]*rhs.entries[9]+entries[8]*rhs.entries[10]+entries[12]*rhs.entries[11],
232                                                entries[1]*rhs.entries[8]+entries[5]*rhs.entries[9]+entries[9]*rhs.entries[10]+entries[13]*rhs.entries[11],
233                                                entries[2]*rhs.entries[8]+entries[6]*rhs.entries[9]+entries[10]*rhs.entries[10]+entries[14]*rhs.entries[11],
234                                                entries[3]*rhs.entries[8]+entries[7]*rhs.entries[9]+entries[11]*rhs.entries[10]+entries[15]*rhs.entries[11],
235                                                entries[0]*rhs.entries[12]+entries[4]*rhs.entries[13]+entries[8]*rhs.entries[14]+entries[12]*rhs.entries[15],
236                                                entries[1]*rhs.entries[12]+entries[5]*rhs.entries[13]+entries[9]*rhs.entries[14]+entries[13]*rhs.entries[15],
237                                                entries[2]*rhs.entries[12]+entries[6]*rhs.entries[13]+entries[10]*rhs.entries[14]+entries[14]*rhs.entries[15],
238                                                entries[3]*rhs.entries[12]+entries[7]*rhs.entries[13]+entries[11]*rhs.entries[14]+entries[15]*rhs.entries[15]);
239}
240
241MATRIX4X4 MATRIX4X4::operator*(const float rhs) const
242{
243        return MATRIX4X4(       entries[0]*rhs,
244                                                entries[1]*rhs,
245                                                entries[2]*rhs,
246                                                entries[3]*rhs,
247                                                entries[4]*rhs,
248                                                entries[5]*rhs,
249                                                entries[6]*rhs,
250                                                entries[7]*rhs,
251                                                entries[8]*rhs,
252                                                entries[9]*rhs,
253                                                entries[10]*rhs,
254                                                entries[11]*rhs,
255                                                entries[12]*rhs,
256                                                entries[13]*rhs,
257                                                entries[14]*rhs,
258                                                entries[15]*rhs);
259}
260
261MATRIX4X4 MATRIX4X4::operator/(const float rhs) const
262{
263        if (rhs==0.0f || rhs==1.0f)
264                return (*this);
265               
266        float temp=1/rhs;
267
268        return (*this)*temp;
269}
270
271MATRIX4X4 operator*(float scaleFactor, const MATRIX4X4 & rhs)
272{
273        return rhs*scaleFactor;
274}
275
276bool MATRIX4X4::operator==(const MATRIX4X4 & rhs) const
277{
278        for(int i=0; i<16; i++)
279        {
280                if(entries[i]!=rhs.entries[i])
281                        return false;
282        }
283        return true;
284}
285
286bool MATRIX4X4::operator!=(const MATRIX4X4 & rhs) const
287{
288        return !((*this)==rhs);
289}
290
291void MATRIX4X4::operator+=(const MATRIX4X4 & rhs)
292{
293        (*this)=(*this)+rhs;
294}
295
296void MATRIX4X4::operator-=(const MATRIX4X4 & rhs)
297{
298        (*this)=(*this)-rhs;
299}
300
301void MATRIX4X4::operator*=(const MATRIX4X4 & rhs)
302{
303        (*this)=(*this)*rhs;
304}
305
306void MATRIX4X4::operator*=(const float rhs)
307{
308        (*this)=(*this)*rhs;
309}
310
311void MATRIX4X4::operator/=(const float rhs)
312{
313        (*this)=(*this)/rhs;
314}
315
316MATRIX4X4 MATRIX4X4::operator-(void) const
317{
318        MATRIX4X4 result(*this);
319
320        for(int i=0; i<16; i++)
321                result.entries[i]=-result.entries[i];
322
323        return result;
324}
325
326VECTOR4D MATRIX4X4::operator*(const VECTOR4D rhs) const
327{
328        //Optimise for matrices in which bottom row is (0, 0, 0, 1)
329        if(entries[3]==0.0f && entries[7]==0.0f && entries[11]==0.0f && entries[15]==1.0f)
330        {
331                return VECTOR4D(entries[0]*rhs.x
332                                        +       entries[4]*rhs.y
333                                        +       entries[8]*rhs.z
334                                        +       entries[12]*rhs.w,
335
336                                                entries[1]*rhs.x
337                                        +       entries[5]*rhs.y
338                                        +       entries[9]*rhs.z
339                                        +       entries[13]*rhs.w,
340
341                                                entries[2]*rhs.x
342                                        +       entries[6]*rhs.y
343                                        +       entries[10]*rhs.z
344                                        +       entries[14]*rhs.w,
345
346                                                rhs.w);
347        }
348       
349        return VECTOR4D(        entries[0]*rhs.x
350                                        +       entries[4]*rhs.y
351                                        +       entries[8]*rhs.z
352                                        +       entries[12]*rhs.w,
353
354                                                entries[1]*rhs.x
355                                        +       entries[5]*rhs.y
356                                        +       entries[9]*rhs.z
357                                        +       entries[13]*rhs.w,
358
359                                                entries[2]*rhs.x
360                                        +       entries[6]*rhs.y
361                                        +       entries[10]*rhs.z
362                                        +       entries[14]*rhs.w,
363
364                                                entries[3]*rhs.x
365                                        +       entries[7]*rhs.y
366                                        +       entries[11]*rhs.z
367                                        +       entries[15]*rhs.w);
368}
369
370VECTOR3D MATRIX4X4::GetRotatedVector3D(const VECTOR3D & rhs) const
371{
372        return VECTOR3D(entries[0]*rhs.x + entries[4]*rhs.y + entries[8]*rhs.z,
373                                        entries[1]*rhs.x + entries[5]*rhs.y + entries[9]*rhs.z,
374                                        entries[2]*rhs.x + entries[6]*rhs.y + entries[10]*rhs.z);
375}
376
377VECTOR3D MATRIX4X4::GetInverseRotatedVector3D(const VECTOR3D & rhs) const
378{
379        //rotate by transpose:
380        return VECTOR3D(entries[0]*rhs.x + entries[1]*rhs.y + entries[2]*rhs.z,
381                                        entries[4]*rhs.x + entries[5]*rhs.y + entries[6]*rhs.z,
382                                        entries[8]*rhs.x + entries[9]*rhs.y + entries[10]*rhs.z);
383}
384
385VECTOR3D MATRIX4X4::GetTranslatedVector3D(const VECTOR3D & rhs) const
386{
387        return VECTOR3D(rhs.x+entries[12], rhs.y+entries[13], rhs.z+entries[14]);
388}
389
390VECTOR3D MATRIX4X4::GetInverseTranslatedVector3D(const VECTOR3D & rhs) const
391{
392        return VECTOR3D(rhs.x-entries[12], rhs.y-entries[13], rhs.z-entries[14]);
393}
394
395void MATRIX4X4::Invert(void)
396{
397        *this=GetInverse();
398}
399
400MATRIX4X4 MATRIX4X4::GetInverse(void) const
401{
402        MATRIX4X4 result=GetInverseTranspose();
403
404        result.Transpose();
405
406        return result;
407}
408
409
410void MATRIX4X4::Transpose(void)
411{
412        *this=GetTranspose();
413}
414
415MATRIX4X4 MATRIX4X4::GetTranspose(void) const
416{
417        return MATRIX4X4(       entries[ 0], entries[ 4], entries[ 8], entries[12],
418                                                entries[ 1], entries[ 5], entries[ 9], entries[13],
419                                                entries[ 2], entries[ 6], entries[10], entries[14],
420                                                entries[ 3], entries[ 7], entries[11], entries[15]);
421}
422
423void MATRIX4X4::InvertTranspose(void)
424{
425        *this=GetInverseTranspose();
426}
427
428MATRIX4X4 MATRIX4X4::GetInverseTranspose(void) const
429{
430        MATRIX4X4 result;
431
432        float tmp[12];                                                                                          //temporary pair storage
433        float det;                                                                                                      //determinant
434
435        //calculate pairs for first 8 elements (cofactors)
436        tmp[0] = entries[10] * entries[15];
437        tmp[1] = entries[11] * entries[14];
438        tmp[2] = entries[9] * entries[15];
439        tmp[3] = entries[11] * entries[13];
440        tmp[4] = entries[9] * entries[14];
441        tmp[5] = entries[10] * entries[13];
442        tmp[6] = entries[8] * entries[15];
443        tmp[7] = entries[11] * entries[12];
444        tmp[8] = entries[8] * entries[14];
445        tmp[9] = entries[10] * entries[12];
446        tmp[10] = entries[8] * entries[13];
447        tmp[11] = entries[9] * entries[12];
448
449        //calculate first 8 elements (cofactors)
450        result.SetEntry(0,              tmp[0]*entries[5] + tmp[3]*entries[6] + tmp[4]*entries[7]
451                                        -       tmp[1]*entries[5] - tmp[2]*entries[6] - tmp[5]*entries[7]);
452
453        result.SetEntry(1,              tmp[1]*entries[4] + tmp[6]*entries[6] + tmp[9]*entries[7]
454                                        -       tmp[0]*entries[4] - tmp[7]*entries[6] - tmp[8]*entries[7]);
455
456        result.SetEntry(2,              tmp[2]*entries[4] + tmp[7]*entries[5] + tmp[10]*entries[7]
457                                        -       tmp[3]*entries[4] - tmp[6]*entries[5] - tmp[11]*entries[7]);
458
459        result.SetEntry(3,              tmp[5]*entries[4] + tmp[8]*entries[5] + tmp[11]*entries[6]
460                                        -       tmp[4]*entries[4] - tmp[9]*entries[5] - tmp[10]*entries[6]);
461
462        result.SetEntry(4,              tmp[1]*entries[1] + tmp[2]*entries[2] + tmp[5]*entries[3]
463                                        -       tmp[0]*entries[1] - tmp[3]*entries[2] - tmp[4]*entries[3]);
464
465        result.SetEntry(5,              tmp[0]*entries[0] + tmp[7]*entries[2] + tmp[8]*entries[3]
466                                        -       tmp[1]*entries[0] - tmp[6]*entries[2] - tmp[9]*entries[3]);
467
468        result.SetEntry(6,              tmp[3]*entries[0] + tmp[6]*entries[1] + tmp[11]*entries[3]
469                                        -       tmp[2]*entries[0] - tmp[7]*entries[1] - tmp[10]*entries[3]);
470
471        result.SetEntry(7,              tmp[4]*entries[0] + tmp[9]*entries[1] + tmp[10]*entries[2]
472                                        -       tmp[5]*entries[0] - tmp[8]*entries[1] - tmp[11]*entries[2]);
473
474        //calculate pairs for second 8 elements (cofactors)
475        tmp[0] = entries[2]*entries[7];
476        tmp[1] = entries[3]*entries[6];
477        tmp[2] = entries[1]*entries[7];
478        tmp[3] = entries[3]*entries[5];
479        tmp[4] = entries[1]*entries[6];
480        tmp[5] = entries[2]*entries[5];
481        tmp[6] = entries[0]*entries[7];
482        tmp[7] = entries[3]*entries[4];
483        tmp[8] = entries[0]*entries[6];
484        tmp[9] = entries[2]*entries[4];
485        tmp[10] = entries[0]*entries[5];
486        tmp[11] = entries[1]*entries[4];
487
488        //calculate second 8 elements (cofactors)
489        result.SetEntry(8,              tmp[0]*entries[13] + tmp[3]*entries[14] + tmp[4]*entries[15]
490                                        -       tmp[1]*entries[13] - tmp[2]*entries[14] - tmp[5]*entries[15]);
491
492        result.SetEntry(9,              tmp[1]*entries[12] + tmp[6]*entries[14] + tmp[9]*entries[15]
493                                        -       tmp[0]*entries[12] - tmp[7]*entries[14] - tmp[8]*entries[15]);
494
495        result.SetEntry(10,             tmp[2]*entries[12] + tmp[7]*entries[13] + tmp[10]*entries[15]
496                                        -       tmp[3]*entries[12] - tmp[6]*entries[13] - tmp[11]*entries[15]);
497
498        result.SetEntry(11,             tmp[5]*entries[12] + tmp[8]*entries[13] + tmp[11]*entries[14]
499                                        -       tmp[4]*entries[12] - tmp[9]*entries[13] - tmp[10]*entries[14]);
500
501        result.SetEntry(12,             tmp[2]*entries[10] + tmp[5]*entries[11] + tmp[1]*entries[9]
502                                        -       tmp[4]*entries[11] - tmp[0]*entries[9] - tmp[3]*entries[10]);
503
504        result.SetEntry(13,             tmp[8]*entries[11] + tmp[0]*entries[8] + tmp[7]*entries[10]
505                                        -       tmp[6]*entries[10] - tmp[9]*entries[11] - tmp[1]*entries[8]);
506
507        result.SetEntry(14,             tmp[6]*entries[9] + tmp[11]*entries[11] + tmp[3]*entries[8]
508                                        -       tmp[10]*entries[11] - tmp[2]*entries[8] - tmp[7]*entries[9]);
509
510        result.SetEntry(15,             tmp[10]*entries[10] + tmp[4]*entries[8] + tmp[9]*entries[9]
511                                        -       tmp[8]*entries[9] - tmp[11]*entries[10] - tmp[5]*entries[8]);
512
513        // calculate determinant
514        det     =        entries[0]*result.GetEntry(0)
515                        +entries[1]*result.GetEntry(1)
516                        +entries[2]*result.GetEntry(2)
517                        +entries[3]*result.GetEntry(3);
518
519        if(det==0.0f)
520        {
521                MATRIX4X4 id;
522                return id;
523        }
524       
525        result=result/det;
526
527        return result;
528}
529
530//Invert if only composed of rotations & translations
531void MATRIX4X4::AffineInvert(void)
532{
533        (*this)=GetAffineInverse();
534}
535
536MATRIX4X4 MATRIX4X4::GetAffineInverse(void) const
537{
538        //return the transpose of the rotation part
539        //and the negative of the inverse rotated translation part
540        return MATRIX4X4(       entries[0],
541                                                entries[4],
542                                                entries[8],
543                                                0.0f,
544                                                entries[1],
545                                                entries[5],
546                                                entries[9],
547                                                0.0f,
548                                                entries[2],
549                                                entries[6],
550                                                entries[10],
551                                                0.0f,
552                                                -(entries[0]*entries[12]+entries[1]*entries[13]+entries[2]*entries[14]),
553                                                -(entries[4]*entries[12]+entries[5]*entries[13]+entries[6]*entries[14]),
554                                                -(entries[8]*entries[12]+entries[9]*entries[13]+entries[10]*entries[14]),
555                                                1.0f);
556}
557
558void MATRIX4X4::AffineInvertTranspose(void)
559{
560        (*this)=GetAffineInverseTranspose();
561}
562
563MATRIX4X4 MATRIX4X4::GetAffineInverseTranspose(void) const
564{
565        //return the transpose of the rotation part
566        //and the negative of the inverse rotated translation part
567        //transposed
568        return MATRIX4X4(       entries[0],
569                                                entries[1],
570                                                entries[2],
571                                                -(entries[0]*entries[12]+entries[1]*entries[13]+entries[2]*entries[14]),
572                                                entries[4],
573                                                entries[5],
574                                                entries[6],
575                                                -(entries[4]*entries[12]+entries[5]*entries[13]+entries[6]*entries[14]),
576                                                entries[8],
577                                                entries[9],
578                                                entries[10],
579                                                -(entries[8]*entries[12]+entries[9]*entries[13]+entries[10]*entries[14]),
580                                                0.0f, 0.0f, 0.0f, 1.0f);
581}
582
583void MATRIX4X4::SetTranslation(const VECTOR3D & translation)
584{
585        LoadIdentity();
586
587        SetTranslationPart(translation);
588}
589
590void MATRIX4X4::SetScale(const VECTOR3D & scaleFactor)
591{
592        LoadIdentity();
593
594        entries[0]=scaleFactor.x;
595        entries[5]=scaleFactor.y;
596        entries[10]=scaleFactor.z;
597}
598
599void MATRIX4X4::SetUniformScale(const float scaleFactor)
600{
601        LoadIdentity();
602
603        entries[0]=entries[5]=entries[10]=scaleFactor;
604}
605
606void MATRIX4X4::SetRotationAxis(const double angle, const VECTOR3D & axis)
607{
608        VECTOR3D u=axis.GetNormalized();
609
610        float sinAngle=(float)sin(M_PI*angle/180);
611        float cosAngle=(float)cos(M_PI*angle/180);
612        float oneMinusCosAngle=1.0f-cosAngle;
613
614        LoadIdentity();
615
616        entries[0]=(u.x)*(u.x) + cosAngle*(1-(u.x)*(u.x));
617        entries[4]=(u.x)*(u.y)*(oneMinusCosAngle) - sinAngle*u.z;
618        entries[8]=(u.x)*(u.z)*(oneMinusCosAngle) + sinAngle*u.y;
619
620        entries[1]=(u.x)*(u.y)*(oneMinusCosAngle) + sinAngle*u.z;
621        entries[5]=(u.y)*(u.y) + cosAngle*(1-(u.y)*(u.y));
622        entries[9]=(u.y)*(u.z)*(oneMinusCosAngle) - sinAngle*u.x;
623       
624        entries[2]=(u.x)*(u.z)*(oneMinusCosAngle) - sinAngle*u.y;
625        entries[6]=(u.y)*(u.z)*(oneMinusCosAngle) + sinAngle*u.x;
626        entries[10]=(u.z)*(u.z) + cosAngle*(1-(u.z)*(u.z));
627}
628
629void MATRIX4X4::SetRotationX(const double angle)
630{
631        LoadIdentity();
632
633        entries[5]=(float)cos(M_PI*angle/180);
634        entries[6]=(float)sin(M_PI*angle/180);
635
636        entries[9]=-entries[6];
637        entries[10]=entries[5];
638}
639
640void MATRIX4X4::SetRotationY(const double angle)
641{
642        LoadIdentity();
643
644        entries[0]=(float)cos(M_PI*angle/180);
645        entries[2]=-(float)sin(M_PI*angle/180);
646
647        entries[8]=-entries[2];
648        entries[10]=entries[0];
649}
650
651void MATRIX4X4::SetRotationZ(const double angle)
652{
653        LoadIdentity();
654
655        entries[0]=(float)cos(M_PI*angle/180);
656        entries[1]=(float)sin(M_PI*angle/180);
657
658        entries[4]=-entries[1];
659        entries[5]=entries[0];
660}
661
662void MATRIX4X4::SetRotationEuler(const double angleX, const double angleY, const double angleZ)
663{
664        LoadIdentity();
665
666        SetRotationPartEuler(angleX, angleY, angleZ);
667}
668
669void MATRIX4X4::SetPerspective( float left, float right, float bottom,
670                                                                float top, float n, float f)
671{
672        float nudge=0.999f;             //prevent artifacts with infinite far plane
673
674        LoadZero();
675
676        //check for division by 0
677        if(left==right || top==bottom || n==f)
678                return;
679
680        entries[0]=(2*n)/(right-left);
681
682        entries[5]=(2*n)/(top-bottom);
683
684        entries[8]=(right+left)/(right-left);
685        entries[9]=(top+bottom)/(top-bottom);
686
687        if(f!=-1)
688        {
689                entries[10]=-(f+n)/(f-n);
690        }
691        else            //if f==-1, use an infinite far plane
692        {
693                entries[10]=-nudge;
694        }
695
696        entries[11]=-1;
697
698        if(f!=-1)
699        {
700                entries[14]=-(2*f*n)/(f-n);
701        }
702        else            //if f==-1, use an infinite far plane
703        {
704                entries[14]=-2*n*nudge;
705        }
706}
707
708void MATRIX4X4::SetPerspective(float fovy, float aspect, float n, float f)
709{
710        float left, right, top, bottom;
711
712        //convert fov from degrees to radians
713        fovy*=(float)M_PI/180;
714
715        top=n*tanf(fovy/2.0f);
716        bottom=-top;
717
718        left=aspect*bottom;
719        right=aspect*top;
720
721        SetPerspective(left, right, bottom, top, n, f);
722}
723
724void MATRIX4X4::SetOrtho(       float left, float right, float bottom,
725                                                        float top, float n, float f)
726{
727        LoadIdentity();
728
729        entries[0]=2.0f/(right-left);
730
731        entries[5]=2.0f/(top-bottom);
732
733        entries[10]=-2.0f/(f-n);
734
735        entries[12]=-(right+left)/(right-left);
736        entries[13]=-(top+bottom)/(top-bottom);
737        entries[14]=-(f+n)/(f-n);
738}
739
740void MATRIX4X4::SetTranslationPart(const VECTOR3D & translation)
741{
742        entries[12]=translation.x;
743        entries[13]=translation.y;
744        entries[14]=translation.z;
745}
746
747void MATRIX4X4::SetRotationPartEuler(const double angleX, const double angleY, const double angleZ)
748{
749        double cr = cos( M_PI*angleX/180 );
750        double sr = sin( M_PI*angleX/180 );
751        double cp = cos( M_PI*angleY/180 );
752        double sp = sin( M_PI*angleY/180 );
753        double cy = cos( M_PI*angleZ/180 );
754        double sy = sin( M_PI*angleZ/180 );
755
756        entries[0] = ( float )( cp*cy );
757        entries[1] = ( float )( cp*sy );
758        entries[2] = ( float )( -sp );
759
760        double srsp = sr*sp;
761        double crsp = cr*sp;
762
763        entries[4] = ( float )( srsp*cy-cr*sy );
764        entries[5] = ( float )( srsp*sy+cr*cy );
765        entries[6] = ( float )( sr*cp );
766
767        entries[8] = ( float )( crsp*cy+sr*sy );
768        entries[9] = ( float )( crsp*sy-sr*cy );
769        entries[10] = ( float )( cr*cp );
770}
Note: See TracBrowser for help on using the repository browser.