1 /******************************************************************************
2
3 @File PVRTVector.cpp
4
5 @Title PVRTVector
6
7 @Version
8
9 @Copyright Copyright (c) Imagination Technologies Limited.
10
11 @Platform ANSI compatible
12
13 @Description Vector and matrix mathematics library
14
15 ******************************************************************************/
16
17 #include "PVRTVector.h"
18
19 #include <math.h>
20
21 /*!***************************************************************************
22 ** PVRTVec2 2 component vector
23 ****************************************************************************/
24
25 /*!***************************************************************************
26 @Function PVRTVec2
27 @Input v3Vec a Vec3
28 @Description Constructor from a Vec3
29 *****************************************************************************/
PVRTVec2(const PVRTVec3 & vec3)30 PVRTVec2::PVRTVec2(const PVRTVec3& vec3)
31 {
32 x = vec3.x; y = vec3.y;
33 }
34
35 /*!***************************************************************************
36 ** PVRTVec3 3 component vector
37 ****************************************************************************/
38
39 /*!***************************************************************************
40 @Function PVRTVec3
41 @Input v4Vec a PVRTVec4
42 @Description Constructor from a PVRTVec4
43 *****************************************************************************/
PVRTVec3(const PVRTVec4 & vec4)44 PVRTVec3::PVRTVec3(const PVRTVec4& vec4)
45 {
46 x = vec4.x; y = vec4.y; z = vec4.z;
47 }
48
49 /*!***************************************************************************
50 @Function *
51 @Input rhs a PVRTMat3
52 @Returns result of multiplication
53 @Description matrix multiplication operator PVRTVec3 and PVRTMat3
54 ****************************************************************************/
operator *(const PVRTMat3 & rhs) const55 PVRTVec3 PVRTVec3::operator*(const PVRTMat3& rhs) const
56 {
57 PVRTVec3 out;
58
59 out.x = VERTTYPEMUL(x,rhs.f[0])+VERTTYPEMUL(y,rhs.f[1])+VERTTYPEMUL(z,rhs.f[2]);
60 out.y = VERTTYPEMUL(x,rhs.f[3])+VERTTYPEMUL(y,rhs.f[4])+VERTTYPEMUL(z,rhs.f[5]);
61 out.z = VERTTYPEMUL(x,rhs.f[6])+VERTTYPEMUL(y,rhs.f[7])+VERTTYPEMUL(z,rhs.f[8]);
62
63 return out;
64 }
65
66 /*!***************************************************************************
67 @Function *=
68 @Input rhs a PVRTMat3
69 @Returns result of multiplication and assignment
70 @Description matrix multiplication and assignment operator for PVRTVec3 and PVRTMat3
71 ****************************************************************************/
operator *=(const PVRTMat3 & rhs)72 PVRTVec3& PVRTVec3::operator*=(const PVRTMat3& rhs)
73 {
74 VERTTYPE tx = VERTTYPEMUL(x,rhs.f[0])+VERTTYPEMUL(y,rhs.f[1])+VERTTYPEMUL(z,rhs.f[2]);
75 VERTTYPE ty = VERTTYPEMUL(x,rhs.f[3])+VERTTYPEMUL(y,rhs.f[4])+VERTTYPEMUL(z,rhs.f[5]);
76 z = VERTTYPEMUL(x,rhs.f[6])+VERTTYPEMUL(y,rhs.f[7])+VERTTYPEMUL(z,rhs.f[8]);
77 x = tx;
78 y = ty;
79
80 return *this;
81 }
82
83 /*!***************************************************************************
84 ** PVRTVec4 4 component vector
85 ****************************************************************************/
86
87 /*!***************************************************************************
88 @Function *
89 @Input rhs a PVRTMat4
90 @Returns result of multiplication
91 @Description matrix multiplication operator PVRTVec4 and PVRTMat4
92 ****************************************************************************/
operator *(const PVRTMat4 & rhs) const93 PVRTVec4 PVRTVec4::operator*(const PVRTMat4& rhs) const
94 {
95 PVRTVec4 out;
96 out.x = VERTTYPEMUL(x,rhs.f[0])+VERTTYPEMUL(y,rhs.f[1])+VERTTYPEMUL(z,rhs.f[2])+VERTTYPEMUL(w,rhs.f[3]);
97 out.y = VERTTYPEMUL(x,rhs.f[4])+VERTTYPEMUL(y,rhs.f[5])+VERTTYPEMUL(z,rhs.f[6])+VERTTYPEMUL(w,rhs.f[7]);
98 out.z = VERTTYPEMUL(x,rhs.f[8])+VERTTYPEMUL(y,rhs.f[9])+VERTTYPEMUL(z,rhs.f[10])+VERTTYPEMUL(w,rhs.f[11]);
99 out.w = VERTTYPEMUL(x,rhs.f[12])+VERTTYPEMUL(y,rhs.f[13])+VERTTYPEMUL(z,rhs.f[14])+VERTTYPEMUL(w,rhs.f[15]);
100 return out;
101 }
102
103 /*!***************************************************************************
104 @Function *=
105 @Input rhs a PVRTMat4
106 @Returns result of multiplication and assignment
107 @Description matrix multiplication and assignment operator for PVRTVec4 and PVRTMat4
108 ****************************************************************************/
operator *=(const PVRTMat4 & rhs)109 PVRTVec4& PVRTVec4::operator*=(const PVRTMat4& rhs)
110 {
111 VERTTYPE tx = VERTTYPEMUL(x,rhs.f[0])+VERTTYPEMUL(y,rhs.f[1])+VERTTYPEMUL(z,rhs.f[2])+VERTTYPEMUL(w,rhs.f[3]);
112 VERTTYPE ty = VERTTYPEMUL(x,rhs.f[4])+VERTTYPEMUL(y,rhs.f[5])+VERTTYPEMUL(z,rhs.f[6])+VERTTYPEMUL(w,rhs.f[7]);
113 VERTTYPE tz = VERTTYPEMUL(x,rhs.f[8])+VERTTYPEMUL(y,rhs.f[9])+VERTTYPEMUL(z,rhs.f[10])+VERTTYPEMUL(w,rhs.f[11]);
114 w = VERTTYPEMUL(x,rhs.f[12])+VERTTYPEMUL(y,rhs.f[13])+VERTTYPEMUL(z,rhs.f[14])+VERTTYPEMUL(w,rhs.f[15]);
115 x = tx;
116 y = ty;
117 z = tz;
118 return *this;
119 }
120
121 /*!***************************************************************************
122 ** PVRTMat3 3x3 matrix
123 ****************************************************************************/
124 /*!***************************************************************************
125 @Function PVRTMat3
126 @Input mat a PVRTMat4
127 @Description constructor to form a PVRTMat3 from a PVRTMat4
128 ****************************************************************************/
PVRTMat3(const PVRTMat4 & mat)129 PVRTMat3::PVRTMat3(const PVRTMat4& mat)
130 {
131 VERTTYPE *dest = (VERTTYPE*)f, *src = (VERTTYPE*)mat.f;
132 for(int i=0;i<3;i++)
133 {
134 for(int j=0;j<3;j++)
135 {
136 (*dest++) = (*src++);
137 }
138 src++;
139 }
140 }
141
142 /*!***************************************************************************
143 @Function RotationX
144 @Input angle the angle of rotation
145 @Returns rotation matrix
146 @Description generates a 3x3 rotation matrix about the X axis
147 ****************************************************************************/
RotationX(VERTTYPE angle)148 PVRTMat3 PVRTMat3::RotationX(VERTTYPE angle)
149 {
150 PVRTMat4 out;
151 PVRTMatrixRotationX(out,angle);
152 return PVRTMat3(out);
153 }
154 /*!***************************************************************************
155 @Function RotationY
156 @Input angle the angle of rotation
157 @Returns rotation matrix
158 @Description generates a 3x3 rotation matrix about the Y axis
159 ****************************************************************************/
RotationY(VERTTYPE angle)160 PVRTMat3 PVRTMat3::RotationY(VERTTYPE angle)
161 {
162 PVRTMat4 out;
163 PVRTMatrixRotationY(out,angle);
164 return PVRTMat3(out);
165 }
166 /*!***************************************************************************
167 @Function RotationZ
168 @Input angle the angle of rotation
169 @Returns rotation matrix
170 @Description generates a 3x3 rotation matrix about the Z axis
171 ****************************************************************************/
RotationZ(VERTTYPE angle)172 PVRTMat3 PVRTMat3::RotationZ(VERTTYPE angle)
173 {
174 PVRTMat4 out;
175 PVRTMatrixRotationZ(out,angle);
176 return PVRTMat3(out);
177 }
178
179
180 /*!***************************************************************************
181 ** PVRTMat4 4x4 matrix
182 ****************************************************************************/
183 /*!***************************************************************************
184 @Function RotationX
185 @Input angle the angle of rotation
186 @Returns rotation matrix
187 @Description generates a 4x4 rotation matrix about the X axis
188 ****************************************************************************/
RotationX(VERTTYPE angle)189 PVRTMat4 PVRTMat4::RotationX(VERTTYPE angle)
190 {
191 PVRTMat4 out;
192 PVRTMatrixRotationX(out,angle);
193 return out;
194 }
195 /*!***************************************************************************
196 @Function RotationY
197 @Input angle the angle of rotation
198 @Returns rotation matrix
199 @Description generates a 4x4 rotation matrix about the Y axis
200 ****************************************************************************/
RotationY(VERTTYPE angle)201 PVRTMat4 PVRTMat4::RotationY(VERTTYPE angle)
202 {
203 PVRTMat4 out;
204 PVRTMatrixRotationY(out,angle);
205 return out;
206 }
207 /*!***************************************************************************
208 @Function RotationZ
209 @Input angle the angle of rotation
210 @Returns rotation matrix
211 @Description generates a 4x4 rotation matrix about the Z axis
212 ****************************************************************************/
RotationZ(VERTTYPE angle)213 PVRTMat4 PVRTMat4::RotationZ(VERTTYPE angle)
214 {
215 PVRTMat4 out;
216 PVRTMatrixRotationZ(out,angle);
217 return out;
218 }
219
220 /*!***************************************************************************
221 @Function *
222 @Input rhs another PVRTMat4
223 @Returns result of multiplication
224 @Description Matrix multiplication of two 4x4 matrices.
225 *****************************************************************************/
operator *(const PVRTMat4 & rhs) const226 PVRTMat4 PVRTMat4::operator*(const PVRTMat4& rhs) const
227 {
228 PVRTMat4 out;
229 // col 1
230 out.f[0] = VERTTYPEMUL(f[0],rhs.f[0])+VERTTYPEMUL(f[4],rhs.f[1])+VERTTYPEMUL(f[8],rhs.f[2])+VERTTYPEMUL(f[12],rhs.f[3]);
231 out.f[1] = VERTTYPEMUL(f[1],rhs.f[0])+VERTTYPEMUL(f[5],rhs.f[1])+VERTTYPEMUL(f[9],rhs.f[2])+VERTTYPEMUL(f[13],rhs.f[3]);
232 out.f[2] = VERTTYPEMUL(f[2],rhs.f[0])+VERTTYPEMUL(f[6],rhs.f[1])+VERTTYPEMUL(f[10],rhs.f[2])+VERTTYPEMUL(f[14],rhs.f[3]);
233 out.f[3] = VERTTYPEMUL(f[3],rhs.f[0])+VERTTYPEMUL(f[7],rhs.f[1])+VERTTYPEMUL(f[11],rhs.f[2])+VERTTYPEMUL(f[15],rhs.f[3]);
234
235 // col 2
236 out.f[4] = VERTTYPEMUL(f[0],rhs.f[4])+VERTTYPEMUL(f[4],rhs.f[5])+VERTTYPEMUL(f[8],rhs.f[6])+VERTTYPEMUL(f[12],rhs.f[7]);
237 out.f[5] = VERTTYPEMUL(f[1],rhs.f[4])+VERTTYPEMUL(f[5],rhs.f[5])+VERTTYPEMUL(f[9],rhs.f[6])+VERTTYPEMUL(f[13],rhs.f[7]);
238 out.f[6] = VERTTYPEMUL(f[2],rhs.f[4])+VERTTYPEMUL(f[6],rhs.f[5])+VERTTYPEMUL(f[10],rhs.f[6])+VERTTYPEMUL(f[14],rhs.f[7]);
239 out.f[7] = VERTTYPEMUL(f[3],rhs.f[4])+VERTTYPEMUL(f[7],rhs.f[5])+VERTTYPEMUL(f[11],rhs.f[6])+VERTTYPEMUL(f[15],rhs.f[7]);
240
241 // col3
242 out.f[8] = VERTTYPEMUL(f[0],rhs.f[8])+VERTTYPEMUL(f[4],rhs.f[9])+VERTTYPEMUL(f[8],rhs.f[10])+VERTTYPEMUL(f[12],rhs.f[11]);
243 out.f[9] = VERTTYPEMUL(f[1],rhs.f[8])+VERTTYPEMUL(f[5],rhs.f[9])+VERTTYPEMUL(f[9],rhs.f[10])+VERTTYPEMUL(f[13],rhs.f[11]);
244 out.f[10] = VERTTYPEMUL(f[2],rhs.f[8])+VERTTYPEMUL(f[6],rhs.f[9])+VERTTYPEMUL(f[10],rhs.f[10])+VERTTYPEMUL(f[14],rhs.f[11]);
245 out.f[11] = VERTTYPEMUL(f[3],rhs.f[8])+VERTTYPEMUL(f[7],rhs.f[9])+VERTTYPEMUL(f[11],rhs.f[10])+VERTTYPEMUL(f[15],rhs.f[11]);
246
247 // col3
248 out.f[12] = VERTTYPEMUL(f[0],rhs.f[12])+VERTTYPEMUL(f[4],rhs.f[13])+VERTTYPEMUL(f[8],rhs.f[14])+VERTTYPEMUL(f[12],rhs.f[15]);
249 out.f[13] = VERTTYPEMUL(f[1],rhs.f[12])+VERTTYPEMUL(f[5],rhs.f[13])+VERTTYPEMUL(f[9],rhs.f[14])+VERTTYPEMUL(f[13],rhs.f[15]);
250 out.f[14] = VERTTYPEMUL(f[2],rhs.f[12])+VERTTYPEMUL(f[6],rhs.f[13])+VERTTYPEMUL(f[10],rhs.f[14])+VERTTYPEMUL(f[14],rhs.f[15]);
251 out.f[15] = VERTTYPEMUL(f[3],rhs.f[12])+VERTTYPEMUL(f[7],rhs.f[13])+VERTTYPEMUL(f[11],rhs.f[14])+VERTTYPEMUL(f[15],rhs.f[15]);
252 return out;
253 }
254
255
256 /*!***************************************************************************
257 @Function inverse
258 @Returns inverse mat4
259 @Description Calculates multiplicative inverse of this matrix
260 The matrix must be of the form :
261 A 0
262 C 1
263 Where A is a 3x3 matrix and C is a 1x3 matrix.
264 *****************************************************************************/
inverse() const265 PVRTMat4 PVRTMat4::inverse() const
266 {
267 PVRTMat4 out;
268 VERTTYPE det_1;
269 VERTTYPE pos, neg, temp;
270
271 /* Calculate the determinant of submatrix A and determine if the
272 the matrix is singular as limited by the double precision
273 floating-point data representation. */
274 pos = neg = f2vt(0.0);
275 temp = VERTTYPEMUL(VERTTYPEMUL(f[ 0], f[ 5]), f[10]);
276 if (temp >= 0) pos += temp; else neg += temp;
277 temp = VERTTYPEMUL(VERTTYPEMUL(f[ 4], f[ 9]), f[ 2]);
278 if (temp >= 0) pos += temp; else neg += temp;
279 temp = VERTTYPEMUL(VERTTYPEMUL(f[ 8], f[ 1]), f[ 6]);
280 if (temp >= 0) pos += temp; else neg += temp;
281 temp = VERTTYPEMUL(VERTTYPEMUL(-f[ 8], f[ 5]), f[ 2]);
282 if (temp >= 0) pos += temp; else neg += temp;
283 temp = VERTTYPEMUL(VERTTYPEMUL(-f[ 4], f[ 1]), f[10]);
284 if (temp >= 0) pos += temp; else neg += temp;
285 temp = VERTTYPEMUL(VERTTYPEMUL(-f[ 0], f[ 9]), f[ 6]);
286 if (temp >= 0) pos += temp; else neg += temp;
287 det_1 = pos + neg;
288
289 /* Is the submatrix A singular? */
290 if (det_1 == f2vt(0.0)) //|| (VERTTYPEABS(det_1 / (pos - neg)) < 1.0e-15)
291 {
292 /* Matrix M has no inverse */
293 _RPT0(_CRT_WARN, "Matrix has no inverse : singular matrix\n");
294 }
295 else
296 {
297 /* Calculate inverse(A) = adj(A) / det(A) */
298 //det_1 = 1.0 / det_1;
299 det_1 = VERTTYPEDIV(f2vt(1.0f), det_1);
300 out.f[ 0] = VERTTYPEMUL(( VERTTYPEMUL(f[ 5], f[10]) - VERTTYPEMUL(f[ 9], f[ 6]) ), det_1);
301 out.f[ 1] = - VERTTYPEMUL(( VERTTYPEMUL(f[ 1], f[10]) - VERTTYPEMUL(f[ 9], f[ 2]) ), det_1);
302 out.f[ 2] = VERTTYPEMUL(( VERTTYPEMUL(f[ 1], f[ 6]) - VERTTYPEMUL(f[ 5], f[ 2]) ), det_1);
303 out.f[ 4] = - VERTTYPEMUL(( VERTTYPEMUL(f[ 4], f[10]) - VERTTYPEMUL(f[ 8], f[ 6]) ), det_1);
304 out.f[ 5] = VERTTYPEMUL(( VERTTYPEMUL(f[ 0], f[10]) - VERTTYPEMUL(f[ 8], f[ 2]) ), det_1);
305 out.f[ 6] = - VERTTYPEMUL(( VERTTYPEMUL(f[ 0], f[ 6]) - VERTTYPEMUL(f[ 4], f[ 2]) ), det_1);
306 out.f[ 8] = VERTTYPEMUL(( VERTTYPEMUL(f[ 4], f[ 9]) - VERTTYPEMUL(f[ 8], f[ 5]) ), det_1);
307 out.f[ 9] = - VERTTYPEMUL(( VERTTYPEMUL(f[ 0], f[ 9]) - VERTTYPEMUL(f[ 8], f[ 1]) ), det_1);
308 out.f[10] = VERTTYPEMUL(( VERTTYPEMUL(f[ 0], f[ 5]) - VERTTYPEMUL(f[ 4], f[ 1]) ), det_1);
309
310 /* Calculate -C * inverse(A) */
311 out.f[12] = - ( VERTTYPEMUL(f[12], out.f[ 0]) + VERTTYPEMUL(f[13], out.f[ 4]) + VERTTYPEMUL(f[14], out.f[ 8]) );
312 out.f[13] = - ( VERTTYPEMUL(f[12], out.f[ 1]) + VERTTYPEMUL(f[13], out.f[ 5]) + VERTTYPEMUL(f[14], out.f[ 9]) );
313 out.f[14] = - ( VERTTYPEMUL(f[12], out.f[ 2]) + VERTTYPEMUL(f[13], out.f[ 6]) + VERTTYPEMUL(f[14], out.f[10]) );
314
315 /* Fill in last row */
316 out.f[ 3] = f2vt(0.0f);
317 out.f[ 7] = f2vt(0.0f);
318 out.f[11] = f2vt(0.0f);
319 out.f[15] = f2vt(1.0f);
320 }
321
322 return out;
323 }
324
325 /*!***************************************************************************
326 @Function PVRTLinearEqSolve
327 @Input pSrc 2D array of floats. 4 Eq linear problem is 5x4
328 matrix, constants in first column
329 @Input nCnt Number of equations to solve
330 @Output pRes Result
331 @Description Solves 'nCnt' simultaneous equations of 'nCnt' variables.
332 pRes should be an array large enough to contain the
333 results: the values of the 'nCnt' variables.
334 This fn recursively uses Gaussian Elimination.
335 *****************************************************************************/
PVRTLinearEqSolve(VERTTYPE * const pRes,VERTTYPE ** const pSrc,const int nCnt)336 void PVRTLinearEqSolve(VERTTYPE * const pRes, VERTTYPE ** const pSrc, const int nCnt)
337 {
338 int i, j, k;
339 VERTTYPE f;
340
341 if (nCnt == 1)
342 {
343 _ASSERT(pSrc[0][1] != 0);
344 pRes[0] = VERTTYPEDIV(pSrc[0][0], pSrc[0][1]);
345 return;
346 }
347
348 // Loop backwards in an attempt avoid the need to swap rows
349 i = nCnt;
350 while(i)
351 {
352 --i;
353
354 if(pSrc[i][nCnt] != f2vt(0.0f))
355 {
356 // Row i can be used to zero the other rows; let's move it to the bottom
357 if(i != (nCnt-1))
358 {
359 for(j = 0; j <= nCnt; ++j)
360 {
361 // Swap the two values
362 f = pSrc[nCnt-1][j];
363 pSrc[nCnt-1][j] = pSrc[i][j];
364 pSrc[i][j] = f;
365 }
366 }
367
368 // Now zero the last columns of the top rows
369 for(j = 0; j < (nCnt-1); ++j)
370 {
371 _ASSERT(pSrc[nCnt-1][nCnt] != f2vt(0.0f));
372 f = VERTTYPEDIV(pSrc[j][nCnt], pSrc[nCnt-1][nCnt]);
373
374 // No need to actually calculate a zero for the final column
375 for(k = 0; k < nCnt; ++k)
376 {
377 pSrc[j][k] -= VERTTYPEMUL(f, pSrc[nCnt-1][k]);
378 }
379 }
380
381 break;
382 }
383 }
384
385 // Solve the top-left sub matrix
386 PVRTLinearEqSolve(pRes, pSrc, nCnt - 1);
387
388 // Now calc the solution for the bottom row
389 f = pSrc[nCnt-1][0];
390 for(k = 1; k < nCnt; ++k)
391 {
392 f -= VERTTYPEMUL(pSrc[nCnt-1][k], pRes[k-1]);
393 }
394 _ASSERT(pSrc[nCnt-1][nCnt] != f2vt(0));
395 f = VERTTYPEDIV(f, pSrc[nCnt-1][nCnt]);
396 pRes[nCnt-1] = f;
397 }
398
399 /*****************************************************************************
400 End of file (PVRTVector.cpp)
401 *****************************************************************************/
402
403