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 *****************************************************************************/
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*****************************************************************************/
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****************************************************************************/
55	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****************************************************************************/
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****************************************************************************/
93	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****************************************************************************/
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****************************************************************************/
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****************************************************************************/
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****************************************************************************/
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****************************************************************************/
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****************************************************************************/
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****************************************************************************/
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****************************************************************************/
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*****************************************************************************/
226	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*****************************************************************************/
265	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*****************************************************************************/
336void 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/*****************************************************************************
400End of file (PVRTVector.cpp)
401*****************************************************************************/
402
403