1 /******************************************************************************
2
3 @File PVRTMatrixX.cpp
4
5 @Title PVRTMatrixX
6
7 @Version
8
9 @Copyright Copyright (c) Imagination Technologies Limited.
10
11 @Platform ANSI compatible
12
13 @Description Set of mathematical functions involving matrices, vectors and
14 quaternions.
15 The general matrix format used is directly compatible with, for example,
16 both DirectX and OpenGL. For the reasons why, read this:
17 http://research.microsoft.com/~hollasch/cgindex/math/matrix/column-vec.html
18
19 ******************************************************************************/
20
21 #include "PVRTContext.h"
22 #include <math.h>
23 #include <string.h>
24
25 #include "PVRTFixedPoint.h"
26 #include "PVRTMatrix.h"
27
28
29
30 /****************************************************************************
31 ** Constants
32 ****************************************************************************/
33 static const PVRTMATRIXx c_mIdentity = {
34 {
35 PVRTF2X(1.0f), PVRTF2X(0.0f), PVRTF2X(0.0f), PVRTF2X(0.0f),
36 PVRTF2X(0.0f), PVRTF2X(1.0f), PVRTF2X(0.0f), PVRTF2X(0.0f),
37 PVRTF2X(0.0f), PVRTF2X(0.0f), PVRTF2X(1.0f), PVRTF2X(0.0f),
38 PVRTF2X(0.0f), PVRTF2X(0.0f), PVRTF2X(0.0f), PVRTF2X(1.0f)
39 }
40 };
41
42
43 /****************************************************************************
44 ** Functions
45 ****************************************************************************/
46
47 /*!***************************************************************************
48 @Function PVRTMatrixIdentityX
49 @Output mOut Set to identity
50 @Description Reset matrix to identity matrix.
51 *****************************************************************************/
PVRTMatrixIdentityX(PVRTMATRIXx & mOut)52 void PVRTMatrixIdentityX(PVRTMATRIXx &mOut)
53 {
54 mOut.f[ 0]=PVRTF2X(1.0f); mOut.f[ 4]=PVRTF2X(0.0f); mOut.f[ 8]=PVRTF2X(0.0f); mOut.f[12]=PVRTF2X(0.0f);
55 mOut.f[ 1]=PVRTF2X(0.0f); mOut.f[ 5]=PVRTF2X(1.0f); mOut.f[ 9]=PVRTF2X(0.0f); mOut.f[13]=PVRTF2X(0.0f);
56 mOut.f[ 2]=PVRTF2X(0.0f); mOut.f[ 6]=PVRTF2X(0.0f); mOut.f[10]=PVRTF2X(1.0f); mOut.f[14]=PVRTF2X(0.0f);
57 mOut.f[ 3]=PVRTF2X(0.0f); mOut.f[ 7]=PVRTF2X(0.0f); mOut.f[11]=PVRTF2X(0.0f); mOut.f[15]=PVRTF2X(1.0f);
58 }
59
60 /*!***************************************************************************
61 @Function PVRTMatrixMultiplyX
62 @Output mOut Result of mA x mB
63 @Input mA First operand
64 @Input mB Second operand
65 @Description Multiply mA by mB and assign the result to mOut
66 (mOut = p1 * p2). A copy of the result matrix is done in
67 the function because mOut can be a parameter mA or mB.
68 The fixed-point shift could be performed after adding
69 all four intermediate results together however this might
70 cause some overflow issues.
71 ****************************************************************************/
PVRTMatrixMultiplyX(PVRTMATRIXx & mOut,const PVRTMATRIXx & mA,const PVRTMATRIXx & mB)72 void PVRTMatrixMultiplyX(
73 PVRTMATRIXx &mOut,
74 const PVRTMATRIXx &mA,
75 const PVRTMATRIXx &mB)
76 {
77 PVRTMATRIXx mRet;
78
79 /* Perform calculation on a dummy matrix (mRet) */
80 mRet.f[ 0] = PVRTXMUL(mA.f[ 0], mB.f[ 0]) + PVRTXMUL(mA.f[ 1], mB.f[ 4]) + PVRTXMUL(mA.f[ 2], mB.f[ 8]) + PVRTXMUL(mA.f[ 3], mB.f[12]);
81 mRet.f[ 1] = PVRTXMUL(mA.f[ 0], mB.f[ 1]) + PVRTXMUL(mA.f[ 1], mB.f[ 5]) + PVRTXMUL(mA.f[ 2], mB.f[ 9]) + PVRTXMUL(mA.f[ 3], mB.f[13]);
82 mRet.f[ 2] = PVRTXMUL(mA.f[ 0], mB.f[ 2]) + PVRTXMUL(mA.f[ 1], mB.f[ 6]) + PVRTXMUL(mA.f[ 2], mB.f[10]) + PVRTXMUL(mA.f[ 3], mB.f[14]);
83 mRet.f[ 3] = PVRTXMUL(mA.f[ 0], mB.f[ 3]) + PVRTXMUL(mA.f[ 1], mB.f[ 7]) + PVRTXMUL(mA.f[ 2], mB.f[11]) + PVRTXMUL(mA.f[ 3], mB.f[15]);
84
85 mRet.f[ 4] = PVRTXMUL(mA.f[ 4], mB.f[ 0]) + PVRTXMUL(mA.f[ 5], mB.f[ 4]) + PVRTXMUL(mA.f[ 6], mB.f[ 8]) + PVRTXMUL(mA.f[ 7], mB.f[12]);
86 mRet.f[ 5] = PVRTXMUL(mA.f[ 4], mB.f[ 1]) + PVRTXMUL(mA.f[ 5], mB.f[ 5]) + PVRTXMUL(mA.f[ 6], mB.f[ 9]) + PVRTXMUL(mA.f[ 7], mB.f[13]);
87 mRet.f[ 6] = PVRTXMUL(mA.f[ 4], mB.f[ 2]) + PVRTXMUL(mA.f[ 5], mB.f[ 6]) + PVRTXMUL(mA.f[ 6], mB.f[10]) + PVRTXMUL(mA.f[ 7], mB.f[14]);
88 mRet.f[ 7] = PVRTXMUL(mA.f[ 4], mB.f[ 3]) + PVRTXMUL(mA.f[ 5], mB.f[ 7]) + PVRTXMUL(mA.f[ 6], mB.f[11]) + PVRTXMUL(mA.f[ 7], mB.f[15]);
89
90 mRet.f[ 8] = PVRTXMUL(mA.f[ 8], mB.f[ 0]) + PVRTXMUL(mA.f[ 9], mB.f[ 4]) + PVRTXMUL(mA.f[10], mB.f[ 8]) + PVRTXMUL(mA.f[11], mB.f[12]);
91 mRet.f[ 9] = PVRTXMUL(mA.f[ 8], mB.f[ 1]) + PVRTXMUL(mA.f[ 9], mB.f[ 5]) + PVRTXMUL(mA.f[10], mB.f[ 9]) + PVRTXMUL(mA.f[11], mB.f[13]);
92 mRet.f[10] = PVRTXMUL(mA.f[ 8], mB.f[ 2]) + PVRTXMUL(mA.f[ 9], mB.f[ 6]) + PVRTXMUL(mA.f[10], mB.f[10]) + PVRTXMUL(mA.f[11], mB.f[14]);
93 mRet.f[11] = PVRTXMUL(mA.f[ 8], mB.f[ 3]) + PVRTXMUL(mA.f[ 9], mB.f[ 7]) + PVRTXMUL(mA.f[10], mB.f[11]) + PVRTXMUL(mA.f[11], mB.f[15]);
94
95 mRet.f[12] = PVRTXMUL(mA.f[12], mB.f[ 0]) + PVRTXMUL(mA.f[13], mB.f[ 4]) + PVRTXMUL(mA.f[14], mB.f[ 8]) + PVRTXMUL(mA.f[15], mB.f[12]);
96 mRet.f[13] = PVRTXMUL(mA.f[12], mB.f[ 1]) + PVRTXMUL(mA.f[13], mB.f[ 5]) + PVRTXMUL(mA.f[14], mB.f[ 9]) + PVRTXMUL(mA.f[15], mB.f[13]);
97 mRet.f[14] = PVRTXMUL(mA.f[12], mB.f[ 2]) + PVRTXMUL(mA.f[13], mB.f[ 6]) + PVRTXMUL(mA.f[14], mB.f[10]) + PVRTXMUL(mA.f[15], mB.f[14]);
98 mRet.f[15] = PVRTXMUL(mA.f[12], mB.f[ 3]) + PVRTXMUL(mA.f[13], mB.f[ 7]) + PVRTXMUL(mA.f[14], mB.f[11]) + PVRTXMUL(mA.f[15], mB.f[15]);
99
100 /* Copy result in pResultMatrix */
101 mOut = mRet;
102 }
103
104 /*!***************************************************************************
105 @Function Name PVRTMatrixTranslationX
106 @Output mOut Translation matrix
107 @Input fX X component of the translation
108 @Input fY Y component of the translation
109 @Input fZ Z component of the translation
110 @Description Build a transaltion matrix mOut using fX, fY and fZ.
111 *****************************************************************************/
PVRTMatrixTranslationX(PVRTMATRIXx & mOut,const int fX,const int fY,const int fZ)112 void PVRTMatrixTranslationX(
113 PVRTMATRIXx &mOut,
114 const int fX,
115 const int fY,
116 const int fZ)
117 {
118 mOut.f[ 0]=PVRTF2X(1.0f); mOut.f[ 4]=PVRTF2X(0.0f); mOut.f[ 8]=PVRTF2X(0.0f); mOut.f[12]=fX;
119 mOut.f[ 1]=PVRTF2X(0.0f); mOut.f[ 5]=PVRTF2X(1.0f); mOut.f[ 9]=PVRTF2X(0.0f); mOut.f[13]=fY;
120 mOut.f[ 2]=PVRTF2X(0.0f); mOut.f[ 6]=PVRTF2X(0.0f); mOut.f[10]=PVRTF2X(1.0f); mOut.f[14]=fZ;
121 mOut.f[ 3]=PVRTF2X(0.0f); mOut.f[ 7]=PVRTF2X(0.0f); mOut.f[11]=PVRTF2X(0.0f); mOut.f[15]=PVRTF2X(1.0f);
122 }
123
124 /*!***************************************************************************
125 @Function Name PVRTMatrixScalingX
126 @Output mOut Scale matrix
127 @Input fX X component of the scaling
128 @Input fY Y component of the scaling
129 @Input fZ Z component of the scaling
130 @Description Build a scale matrix mOut using fX, fY and fZ.
131 *****************************************************************************/
PVRTMatrixScalingX(PVRTMATRIXx & mOut,const int fX,const int fY,const int fZ)132 void PVRTMatrixScalingX(
133 PVRTMATRIXx &mOut,
134 const int fX,
135 const int fY,
136 const int fZ)
137 {
138 mOut.f[ 0]=fX; mOut.f[ 4]=PVRTF2X(0.0f); mOut.f[ 8]=PVRTF2X(0.0f); mOut.f[12]=PVRTF2X(0.0f);
139 mOut.f[ 1]=PVRTF2X(0.0f); mOut.f[ 5]=fY; mOut.f[ 9]=PVRTF2X(0.0f); mOut.f[13]=PVRTF2X(0.0f);
140 mOut.f[ 2]=PVRTF2X(0.0f); mOut.f[ 6]=PVRTF2X(0.0f); mOut.f[10]=fZ; mOut.f[14]=PVRTF2X(0.0f);
141 mOut.f[ 3]=PVRTF2X(0.0f); mOut.f[ 7]=PVRTF2X(0.0f); mOut.f[11]=PVRTF2X(0.0f); mOut.f[15]=PVRTF2X(1.0f);
142 }
143
144 /*!***************************************************************************
145 @Function Name PVRTMatrixRotationXX
146 @Output mOut Rotation matrix
147 @Input fAngle Angle of the rotation
148 @Description Create an X rotation matrix mOut.
149 *****************************************************************************/
PVRTMatrixRotationXX(PVRTMATRIXx & mOut,const int fAngle)150 void PVRTMatrixRotationXX(
151 PVRTMATRIXx &mOut,
152 const int fAngle)
153 {
154 int fCosine, fSine;
155
156 /* Precompute cos and sin */
157 #if defined(BUILD_DX11)
158 fCosine = PVRTXCOS(-fAngle);
159 fSine = PVRTXSIN(-fAngle);
160 #else
161 fCosine = PVRTXCOS(fAngle);
162 fSine = PVRTXSIN(fAngle);
163 #endif
164
165 /* Create the trigonometric matrix corresponding to X Rotation */
166 mOut.f[ 0]=PVRTF2X(1.0f); mOut.f[ 4]=PVRTF2X(0.0f); mOut.f[ 8]=PVRTF2X(0.0f); mOut.f[12]=PVRTF2X(0.0f);
167 mOut.f[ 1]=PVRTF2X(0.0f); mOut.f[ 5]=fCosine; mOut.f[ 9]=fSine; mOut.f[13]=PVRTF2X(0.0f);
168 mOut.f[ 2]=PVRTF2X(0.0f); mOut.f[ 6]=-fSine; mOut.f[10]=fCosine; mOut.f[14]=PVRTF2X(0.0f);
169 mOut.f[ 3]=PVRTF2X(0.0f); mOut.f[ 7]=PVRTF2X(0.0f); mOut.f[11]=PVRTF2X(0.0f); mOut.f[15]=PVRTF2X(1.0f);
170 }
171
172 /*!***************************************************************************
173 @Function Name PVRTMatrixRotationYX
174 @Output mOut Rotation matrix
175 @Input fAngle Angle of the rotation
176 @Description Create an Y rotation matrix mOut.
177 *****************************************************************************/
PVRTMatrixRotationYX(PVRTMATRIXx & mOut,const int fAngle)178 void PVRTMatrixRotationYX(
179 PVRTMATRIXx &mOut,
180 const int fAngle)
181 {
182 int fCosine, fSine;
183
184 /* Precompute cos and sin */
185 #if defined(BUILD_DX11)
186 fCosine = PVRTXCOS(-fAngle);
187 fSine = PVRTXSIN(-fAngle);
188 #else
189 fCosine = PVRTXCOS(fAngle);
190 fSine = PVRTXSIN(fAngle);
191 #endif
192
193 /* Create the trigonometric matrix corresponding to Y Rotation */
194 mOut.f[ 0]=fCosine; mOut.f[ 4]=PVRTF2X(0.0f); mOut.f[ 8]=-fSine; mOut.f[12]=PVRTF2X(0.0f);
195 mOut.f[ 1]=PVRTF2X(0.0f); mOut.f[ 5]=PVRTF2X(1.0f); mOut.f[ 9]=PVRTF2X(0.0f); mOut.f[13]=PVRTF2X(0.0f);
196 mOut.f[ 2]=fSine; mOut.f[ 6]=PVRTF2X(0.0f); mOut.f[10]=fCosine; mOut.f[14]=PVRTF2X(0.0f);
197 mOut.f[ 3]=PVRTF2X(0.0f); mOut.f[ 7]=PVRTF2X(0.0f); mOut.f[11]=PVRTF2X(0.0f); mOut.f[15]=PVRTF2X(1.0f);
198 }
199
200 /*!***************************************************************************
201 @Function Name PVRTMatrixRotationZX
202 @Output mOut Rotation matrix
203 @Input fAngle Angle of the rotation
204 @Description Create an Z rotation matrix mOut.
205 *****************************************************************************/
PVRTMatrixRotationZX(PVRTMATRIXx & mOut,const int fAngle)206 void PVRTMatrixRotationZX(
207 PVRTMATRIXx &mOut,
208 const int fAngle)
209 {
210 int fCosine, fSine;
211
212 /* Precompute cos and sin */
213 #if defined(BUILD_DX11)
214 fCosine = PVRTXCOS(-fAngle);
215 fSine = PVRTXSIN(-fAngle);
216 #else
217 fCosine = PVRTXCOS(fAngle);
218 fSine = PVRTXSIN(fAngle);
219 #endif
220
221 /* Create the trigonometric matrix corresponding to Z Rotation */
222 mOut.f[ 0]=fCosine; mOut.f[ 4]=fSine; mOut.f[ 8]=PVRTF2X(0.0f); mOut.f[12]=PVRTF2X(0.0f);
223 mOut.f[ 1]=-fSine; mOut.f[ 5]=fCosine; mOut.f[ 9]=PVRTF2X(0.0f); mOut.f[13]=PVRTF2X(0.0f);
224 mOut.f[ 2]=PVRTF2X(0.0f); mOut.f[ 6]=PVRTF2X(0.0f); mOut.f[10]=PVRTF2X(1.0f); mOut.f[14]=PVRTF2X(0.0f);
225 mOut.f[ 3]=PVRTF2X(0.0f); mOut.f[ 7]=PVRTF2X(0.0f); mOut.f[11]=PVRTF2X(0.0f); mOut.f[15]=PVRTF2X(1.0f);
226 }
227
228 /*!***************************************************************************
229 @Function Name PVRTMatrixTransposeX
230 @Output mOut Transposed matrix
231 @Input mIn Original matrix
232 @Description Compute the transpose matrix of mIn.
233 *****************************************************************************/
PVRTMatrixTransposeX(PVRTMATRIXx & mOut,const PVRTMATRIXx & mIn)234 void PVRTMatrixTransposeX(
235 PVRTMATRIXx &mOut,
236 const PVRTMATRIXx &mIn)
237 {
238 PVRTMATRIXx mTmp;
239
240 mTmp.f[ 0]=mIn.f[ 0]; mTmp.f[ 4]=mIn.f[ 1]; mTmp.f[ 8]=mIn.f[ 2]; mTmp.f[12]=mIn.f[ 3];
241 mTmp.f[ 1]=mIn.f[ 4]; mTmp.f[ 5]=mIn.f[ 5]; mTmp.f[ 9]=mIn.f[ 6]; mTmp.f[13]=mIn.f[ 7];
242 mTmp.f[ 2]=mIn.f[ 8]; mTmp.f[ 6]=mIn.f[ 9]; mTmp.f[10]=mIn.f[10]; mTmp.f[14]=mIn.f[11];
243 mTmp.f[ 3]=mIn.f[12]; mTmp.f[ 7]=mIn.f[13]; mTmp.f[11]=mIn.f[14]; mTmp.f[15]=mIn.f[15];
244
245 mOut = mTmp;
246 }
247
248 /*!***************************************************************************
249 @Function PVRTMatrixInverseX
250 @Output mOut Inversed matrix
251 @Input mIn Original matrix
252 @Description Compute the inverse matrix of mIn.
253 The matrix must be of the form :
254 A 0
255 C 1
256 Where A is a 3x3 matrix and C is a 1x3 matrix.
257 *****************************************************************************/
PVRTMatrixInverseX(PVRTMATRIXx & mOut,const PVRTMATRIXx & mIn)258 void PVRTMatrixInverseX(
259 PVRTMATRIXx &mOut,
260 const PVRTMATRIXx &mIn)
261 {
262 PVRTMATRIXx mDummyMatrix;
263 int det_1;
264 int pos, neg, temp;
265
266 /* Calculate the determinant of submatrix A and determine if the
267 the matrix is singular as limited by the double precision
268 floating-point data representation. */
269 pos = neg = 0;
270 temp = PVRTXMUL(PVRTXMUL(mIn.f[ 0], mIn.f[ 5]), mIn.f[10]);
271 if (temp >= 0) pos += temp; else neg += temp;
272 temp = PVRTXMUL(PVRTXMUL(mIn.f[ 4], mIn.f[ 9]), mIn.f[ 2]);
273 if (temp >= 0) pos += temp; else neg += temp;
274 temp = PVRTXMUL(PVRTXMUL(mIn.f[ 8], mIn.f[ 1]), mIn.f[ 6]);
275 if (temp >= 0) pos += temp; else neg += temp;
276 temp = PVRTXMUL(PVRTXMUL(-mIn.f[ 8], mIn.f[ 5]), mIn.f[ 2]);
277 if (temp >= 0) pos += temp; else neg += temp;
278 temp = PVRTXMUL(PVRTXMUL(-mIn.f[ 4], mIn.f[ 1]), mIn.f[10]);
279 if (temp >= 0) pos += temp; else neg += temp;
280 temp = PVRTXMUL(PVRTXMUL(-mIn.f[ 0], mIn.f[ 9]), mIn.f[ 6]);
281 if (temp >= 0) pos += temp; else neg += temp;
282 det_1 = pos + neg;
283
284 /* Is the submatrix A singular? */
285 if (det_1 == 0)
286 {
287 /* Matrix M has no inverse */
288 _RPT0(_CRT_WARN, "Matrix has no inverse : singular matrix\n");
289 return;
290 }
291 else
292 {
293 /* Calculate inverse(A) = adj(A) / det(A) */
294 //det_1 = 1.0 / det_1;
295 det_1 = PVRTXDIV(PVRTF2X(1.0f), det_1);
296 mDummyMatrix.f[ 0] = PVRTXMUL(( PVRTXMUL(mIn.f[ 5], mIn.f[10]) - PVRTXMUL(mIn.f[ 9], mIn.f[ 6]) ), det_1);
297 mDummyMatrix.f[ 1] = - PVRTXMUL(( PVRTXMUL(mIn.f[ 1], mIn.f[10]) - PVRTXMUL(mIn.f[ 9], mIn.f[ 2]) ), det_1);
298 mDummyMatrix.f[ 2] = PVRTXMUL(( PVRTXMUL(mIn.f[ 1], mIn.f[ 6]) - PVRTXMUL(mIn.f[ 5], mIn.f[ 2]) ), det_1);
299 mDummyMatrix.f[ 4] = - PVRTXMUL(( PVRTXMUL(mIn.f[ 4], mIn.f[10]) - PVRTXMUL(mIn.f[ 8], mIn.f[ 6]) ), det_1);
300 mDummyMatrix.f[ 5] = PVRTXMUL(( PVRTXMUL(mIn.f[ 0], mIn.f[10]) - PVRTXMUL(mIn.f[ 8], mIn.f[ 2]) ), det_1);
301 mDummyMatrix.f[ 6] = - PVRTXMUL(( PVRTXMUL(mIn.f[ 0], mIn.f[ 6]) - PVRTXMUL(mIn.f[ 4], mIn.f[ 2]) ), det_1);
302 mDummyMatrix.f[ 8] = PVRTXMUL(( PVRTXMUL(mIn.f[ 4], mIn.f[ 9]) - PVRTXMUL(mIn.f[ 8], mIn.f[ 5]) ), det_1);
303 mDummyMatrix.f[ 9] = - PVRTXMUL(( PVRTXMUL(mIn.f[ 0], mIn.f[ 9]) - PVRTXMUL(mIn.f[ 8], mIn.f[ 1]) ), det_1);
304 mDummyMatrix.f[10] = PVRTXMUL(( PVRTXMUL(mIn.f[ 0], mIn.f[ 5]) - PVRTXMUL(mIn.f[ 4], mIn.f[ 1]) ), det_1);
305
306 /* Calculate -C * inverse(A) */
307 mDummyMatrix.f[12] = - ( PVRTXMUL(mIn.f[12], mDummyMatrix.f[ 0]) + PVRTXMUL(mIn.f[13], mDummyMatrix.f[ 4]) + PVRTXMUL(mIn.f[14], mDummyMatrix.f[ 8]) );
308 mDummyMatrix.f[13] = - ( PVRTXMUL(mIn.f[12], mDummyMatrix.f[ 1]) + PVRTXMUL(mIn.f[13], mDummyMatrix.f[ 5]) + PVRTXMUL(mIn.f[14], mDummyMatrix.f[ 9]) );
309 mDummyMatrix.f[14] = - ( PVRTXMUL(mIn.f[12], mDummyMatrix.f[ 2]) + PVRTXMUL(mIn.f[13], mDummyMatrix.f[ 6]) + PVRTXMUL(mIn.f[14], mDummyMatrix.f[10]) );
310
311 /* Fill in last row */
312 mDummyMatrix.f[ 3] = PVRTF2X(0.0f);
313 mDummyMatrix.f[ 7] = PVRTF2X(0.0f);
314 mDummyMatrix.f[11] = PVRTF2X(0.0f);
315 mDummyMatrix.f[15] = PVRTF2X(1.0f);
316 }
317
318 /* Copy contents of dummy matrix in pfMatrix */
319 mOut = mDummyMatrix;
320 }
321
322 /*!***************************************************************************
323 @Function PVRTMatrixInverseExX
324 @Output mOut Inversed matrix
325 @Input mIn Original matrix
326 @Description Compute the inverse matrix of mIn.
327 Uses a linear equation solver and the knowledge that M.M^-1=I.
328 Use this fn to calculate the inverse of matrices that
329 PVRTMatrixInverse() cannot.
330 *****************************************************************************/
PVRTMatrixInverseExX(PVRTMATRIXx & mOut,const PVRTMATRIXx & mIn)331 void PVRTMatrixInverseExX(
332 PVRTMATRIXx &mOut,
333 const PVRTMATRIXx &mIn)
334 {
335 PVRTMATRIXx mTmp;
336 int *ppfRows[4], pfRes[4], pfIn[20];
337 int i, j;
338
339 for (i = 0; i < 4; ++i)
340 {
341 ppfRows[i] = &pfIn[i * 5];
342 }
343
344 /* Solve 4 sets of 4 linear equations */
345 for (i = 0; i < 4; ++i)
346 {
347 for (j = 0; j < 4; ++j)
348 {
349 ppfRows[j][0] = c_mIdentity.f[i + 4 * j];
350 memcpy(&ppfRows[j][1], &mIn.f[j * 4], 4 * sizeof(float));
351 }
352
353 PVRTMatrixLinearEqSolveX(pfRes, (int**)ppfRows, 4);
354
355 for(j = 0; j < 4; ++j)
356 {
357 mTmp.f[i + 4 * j] = pfRes[j];
358 }
359 }
360
361 mOut = mTmp;
362 }
363
364 /*!***************************************************************************
365 @Function PVRTMatrixLookAtLHX
366 @Output mOut Look-at view matrix
367 @Input vEye Position of the camera
368 @Input vAt Point the camera is looking at
369 @Input vUp Up direction for the camera
370 @Description Create a look-at view matrix.
371 *****************************************************************************/
PVRTMatrixLookAtLHX(PVRTMATRIXx & mOut,const PVRTVECTOR3x & vEye,const PVRTVECTOR3x & vAt,const PVRTVECTOR3x & vUp)372 void PVRTMatrixLookAtLHX(
373 PVRTMATRIXx &mOut,
374 const PVRTVECTOR3x &vEye,
375 const PVRTVECTOR3x &vAt,
376 const PVRTVECTOR3x &vUp)
377 {
378 PVRTVECTOR3x f, vUpActual, s, u;
379 PVRTMATRIXx t;
380
381 f.x = vEye.x - vAt.x;
382 f.y = vEye.y - vAt.y;
383 f.z = vEye.z - vAt.z;
384
385 PVRTMatrixVec3NormalizeX(f, f);
386 PVRTMatrixVec3NormalizeX(vUpActual, vUp);
387 PVRTMatrixVec3CrossProductX(s, f, vUpActual);
388 PVRTMatrixVec3CrossProductX(u, s, f);
389
390 mOut.f[ 0] = s.x;
391 mOut.f[ 1] = u.x;
392 mOut.f[ 2] = -f.x;
393 mOut.f[ 3] = PVRTF2X(0.0f);
394
395 mOut.f[ 4] = s.y;
396 mOut.f[ 5] = u.y;
397 mOut.f[ 6] = -f.y;
398 mOut.f[ 7] = PVRTF2X(0.0f);
399
400 mOut.f[ 8] = s.z;
401 mOut.f[ 9] = u.z;
402 mOut.f[10] = -f.z;
403 mOut.f[11] = PVRTF2X(0.0f);
404
405 mOut.f[12] = PVRTF2X(0.0f);
406 mOut.f[13] = PVRTF2X(0.0f);
407 mOut.f[14] = PVRTF2X(0.0f);
408 mOut.f[15] = PVRTF2X(1.0f);
409
410 PVRTMatrixTranslationX(t, -vEye.x, -vEye.y, -vEye.z);
411 PVRTMatrixMultiplyX(mOut, t, mOut);
412 }
413
414 /*!***************************************************************************
415 @Function PVRTMatrixLookAtRHX
416 @Output mOut Look-at view matrix
417 @Input vEye Position of the camera
418 @Input vAt Point the camera is looking at
419 @Input vUp Up direction for the camera
420 @Description Create a look-at view matrix.
421 *****************************************************************************/
PVRTMatrixLookAtRHX(PVRTMATRIXx & mOut,const PVRTVECTOR3x & vEye,const PVRTVECTOR3x & vAt,const PVRTVECTOR3x & vUp)422 void PVRTMatrixLookAtRHX(
423 PVRTMATRIXx &mOut,
424 const PVRTVECTOR3x &vEye,
425 const PVRTVECTOR3x &vAt,
426 const PVRTVECTOR3x &vUp)
427 {
428 PVRTVECTOR3x f, vUpActual, s, u;
429 PVRTMATRIXx t;
430
431 f.x = vAt.x - vEye.x;
432 f.y = vAt.y - vEye.y;
433 f.z = vAt.z - vEye.z;
434
435 PVRTMatrixVec3NormalizeX(f, f);
436 PVRTMatrixVec3NormalizeX(vUpActual, vUp);
437 PVRTMatrixVec3CrossProductX(s, f, vUpActual);
438 PVRTMatrixVec3CrossProductX(u, s, f);
439
440 mOut.f[ 0] = s.x;
441 mOut.f[ 1] = u.x;
442 mOut.f[ 2] = -f.x;
443 mOut.f[ 3] = PVRTF2X(0.0f);
444
445 mOut.f[ 4] = s.y;
446 mOut.f[ 5] = u.y;
447 mOut.f[ 6] = -f.y;
448 mOut.f[ 7] = PVRTF2X(0.0f);
449
450 mOut.f[ 8] = s.z;
451 mOut.f[ 9] = u.z;
452 mOut.f[10] = -f.z;
453 mOut.f[11] = PVRTF2X(0.0f);
454
455 mOut.f[12] = PVRTF2X(0.0f);
456 mOut.f[13] = PVRTF2X(0.0f);
457 mOut.f[14] = PVRTF2X(0.0f);
458 mOut.f[15] = PVRTF2X(1.0f);
459
460 PVRTMatrixTranslationX(t, -vEye.x, -vEye.y, -vEye.z);
461 PVRTMatrixMultiplyX(mOut, t, mOut);
462 }
463
464 /*!***************************************************************************
465 @Function PVRTMatrixPerspectiveFovLHX
466 @Output mOut Perspective matrix
467 @Input fFOVy Field of view
468 @Input fAspect Aspect ratio
469 @Input fNear Near clipping distance
470 @Input fFar Far clipping distance
471 @Input bRotate Should we rotate it ? (for upright screens)
472 @Description Create a perspective matrix.
473 *****************************************************************************/
PVRTMatrixPerspectiveFovLHX(PVRTMATRIXx & mOut,const int fFOVy,const int fAspect,const int fNear,const int fFar,const bool bRotate)474 void PVRTMatrixPerspectiveFovLHX(
475 PVRTMATRIXx &mOut,
476 const int fFOVy,
477 const int fAspect,
478 const int fNear,
479 const int fFar,
480 const bool bRotate)
481 {
482 int f, fRealAspect;
483
484 if (bRotate)
485 fRealAspect = PVRTXDIV(PVRTF2X(1.0f), fAspect);
486 else
487 fRealAspect = fAspect;
488
489 f = PVRTXDIV(PVRTF2X(1.0f), PVRTXTAN(PVRTXMUL(fFOVy, PVRTF2X(0.5f))));
490
491 mOut.f[ 0] = PVRTXDIV(f, fRealAspect);
492 mOut.f[ 1] = PVRTF2X(0.0f);
493 mOut.f[ 2] = PVRTF2X(0.0f);
494 mOut.f[ 3] = PVRTF2X(0.0f);
495
496 mOut.f[ 4] = PVRTF2X(0.0f);
497 mOut.f[ 5] = f;
498 mOut.f[ 6] = PVRTF2X(0.0f);
499 mOut.f[ 7] = PVRTF2X(0.0f);
500
501 mOut.f[ 8] = PVRTF2X(0.0f);
502 mOut.f[ 9] = PVRTF2X(0.0f);
503 mOut.f[10] = PVRTXDIV(fFar, fFar - fNear);
504 mOut.f[11] = PVRTF2X(1.0f);
505
506 mOut.f[12] = PVRTF2X(0.0f);
507 mOut.f[13] = PVRTF2X(0.0f);
508 mOut.f[14] = -PVRTXMUL(PVRTXDIV(fFar, fFar - fNear), fNear);
509 mOut.f[15] = PVRTF2X(0.0f);
510
511 if (bRotate)
512 {
513 PVRTMATRIXx mRotation, mTemp = mOut;
514 PVRTMatrixRotationZX(mRotation, PVRTF2X(90.0f*PVRT_PIf/180.0f));
515 PVRTMatrixMultiplyX(mOut, mTemp, mRotation);
516 }
517 }
518
519 /*!***************************************************************************
520 @Function PVRTMatrixPerspectiveFovRHX
521 @Output mOut Perspective matrix
522 @Input fFOVy Field of view
523 @Input fAspect Aspect ratio
524 @Input fNear Near clipping distance
525 @Input fFar Far clipping distance
526 @Input bRotate Should we rotate it ? (for upright screens)
527 @Description Create a perspective matrix.
528 *****************************************************************************/
PVRTMatrixPerspectiveFovRHX(PVRTMATRIXx & mOut,const int fFOVy,const int fAspect,const int fNear,const int fFar,const bool bRotate)529 void PVRTMatrixPerspectiveFovRHX(
530 PVRTMATRIXx &mOut,
531 const int fFOVy,
532 const int fAspect,
533 const int fNear,
534 const int fFar,
535 const bool bRotate)
536 {
537 int f;
538
539 int fCorrectAspect = fAspect;
540 if (bRotate)
541 {
542 fCorrectAspect = PVRTXDIV(PVRTF2X(1.0f), fAspect);
543 }
544 f = PVRTXDIV(PVRTF2X(1.0f), PVRTXTAN(PVRTXMUL(fFOVy, PVRTF2X(0.5f))));
545
546 mOut.f[ 0] = PVRTXDIV(f, fCorrectAspect);
547 mOut.f[ 1] = PVRTF2X(0.0f);
548 mOut.f[ 2] = PVRTF2X(0.0f);
549 mOut.f[ 3] = PVRTF2X(0.0f);
550
551 mOut.f[ 4] = PVRTF2X(0.0f);
552 mOut.f[ 5] = f;
553 mOut.f[ 6] = PVRTF2X(0.0f);
554 mOut.f[ 7] = PVRTF2X(0.0f);
555
556 mOut.f[ 8] = PVRTF2X(0.0f);
557 mOut.f[ 9] = PVRTF2X(0.0f);
558 mOut.f[10] = PVRTXDIV(fFar + fNear, fNear - fFar);
559 mOut.f[11] = PVRTF2X(-1.0f);
560
561 mOut.f[12] = PVRTF2X(0.0f);
562 mOut.f[13] = PVRTF2X(0.0f);
563 mOut.f[14] = PVRTXMUL(PVRTXDIV(fFar, fNear - fFar), fNear) << 1; // Cheap 2x
564 mOut.f[15] = PVRTF2X(0.0f);
565
566 if (bRotate)
567 {
568 PVRTMATRIXx mRotation, mTemp = mOut;
569 PVRTMatrixRotationZX(mRotation, PVRTF2X(-90.0f*PVRT_PIf/180.0f));
570 PVRTMatrixMultiplyX(mOut, mTemp, mRotation);
571 }
572 }
573
574 /*!***************************************************************************
575 @Function PVRTMatrixOrthoLHX
576 @Output mOut Orthographic matrix
577 @Input w Width of the screen
578 @Input h Height of the screen
579 @Input zn Near clipping distance
580 @Input zf Far clipping distance
581 @Input bRotate Should we rotate it ? (for upright screens)
582 @Description Create an orthographic matrix.
583 *****************************************************************************/
PVRTMatrixOrthoLHX(PVRTMATRIXx & mOut,const int w,const int h,const int zn,const int zf,const bool bRotate)584 void PVRTMatrixOrthoLHX(
585 PVRTMATRIXx &mOut,
586 const int w,
587 const int h,
588 const int zn,
589 const int zf,
590 const bool bRotate)
591 {
592 int fCorrectW = w;
593 int fCorrectH = h;
594 if (bRotate)
595 {
596 fCorrectW = h;
597 fCorrectH = w;
598 }
599 mOut.f[ 0] = PVRTXDIV(PVRTF2X(2.0f), fCorrectW);
600 mOut.f[ 1] = PVRTF2X(0.0f);
601 mOut.f[ 2] = PVRTF2X(0.0f);
602 mOut.f[ 3] = PVRTF2X(0.0f);
603
604 mOut.f[ 4] = PVRTF2X(0.0f);
605 mOut.f[ 5] = PVRTXDIV(PVRTF2X(2.0f), fCorrectH);
606 mOut.f[ 6] = PVRTF2X(0.0f);
607 mOut.f[ 7] = PVRTF2X(0.0f);
608
609 mOut.f[ 8] = PVRTF2X(0.0f);
610 mOut.f[ 9] = PVRTF2X(0.0f);
611 mOut.f[10] = PVRTXDIV(PVRTF2X(1.0f), zf - zn);
612 mOut.f[11] = PVRTXDIV(zn, zn - zf);
613
614 mOut.f[12] = PVRTF2X(0.0f);
615 mOut.f[13] = PVRTF2X(0.0f);
616 mOut.f[14] = PVRTF2X(0.0f);
617 mOut.f[15] = PVRTF2X(1.0f);
618
619 if (bRotate)
620 {
621 PVRTMATRIXx mRotation, mTemp = mOut;
622 PVRTMatrixRotationZX(mRotation, PVRTF2X(-90.0f*PVRT_PIf/180.0f));
623 PVRTMatrixMultiplyX(mOut, mRotation, mTemp);
624 }
625 }
626
627 /*!***************************************************************************
628 @Function PVRTMatrixOrthoRHX
629 @Output mOut Orthographic matrix
630 @Input w Width of the screen
631 @Input h Height of the screen
632 @Input zn Near clipping distance
633 @Input zf Far clipping distance
634 @Input bRotate Should we rotate it ? (for upright screens)
635 @Description Create an orthographic matrix.
636 *****************************************************************************/
PVRTMatrixOrthoRHX(PVRTMATRIXx & mOut,const int w,const int h,const int zn,const int zf,const bool bRotate)637 void PVRTMatrixOrthoRHX(
638 PVRTMATRIXx &mOut,
639 const int w,
640 const int h,
641 const int zn,
642 const int zf,
643 const bool bRotate)
644 {
645 int fCorrectW = w;
646 int fCorrectH = h;
647 if (bRotate)
648 {
649 fCorrectW = h;
650 fCorrectH = w;
651 }
652 mOut.f[ 0] = PVRTXDIV(PVRTF2X(2.0f), fCorrectW);
653 mOut.f[ 1] = PVRTF2X(0.0f);
654 mOut.f[ 2] = PVRTF2X(0.0f);
655 mOut.f[ 3] = PVRTF2X(0.0f);
656
657 mOut.f[ 4] = PVRTF2X(0.0f);
658 mOut.f[ 5] = PVRTXDIV(PVRTF2X(2.0f), fCorrectH);
659 mOut.f[ 6] = PVRTF2X(0.0f);
660 mOut.f[ 7] = PVRTF2X(0.0f);
661
662 mOut.f[ 8] = PVRTF2X(0.0f);
663 mOut.f[ 9] = PVRTF2X(0.0f);
664 mOut.f[10] = PVRTXDIV(PVRTF2X(1.0f), zn - zf);
665 mOut.f[11] = PVRTXDIV(zn, zn - zf);
666
667 mOut.f[12] = PVRTF2X(0.0f);
668 mOut.f[13] = PVRTF2X(0.0f);
669 mOut.f[14] = PVRTF2X(0.0f);
670 mOut.f[15] = PVRTF2X(1.0f);
671
672 if (bRotate)
673 {
674 PVRTMATRIXx mRotation, mTemp = mOut;
675 PVRTMatrixRotationZX(mRotation, PVRTF2X(-90.0f*PVRT_PIf/180.0f));
676 PVRTMatrixMultiplyX(mOut, mRotation, mTemp);
677 }
678 }
679
680 /*!***************************************************************************
681 @Function PVRTMatrixVec3LerpX
682 @Output vOut Result of the interpolation
683 @Input v1 First vector to interpolate from
684 @Input v2 Second vector to interpolate form
685 @Input s Coefficient of interpolation
686 @Description This function performs the linear interpolation based on
687 the following formula: V1 + s(V2-V1).
688 *****************************************************************************/
PVRTMatrixVec3LerpX(PVRTVECTOR3x & vOut,const PVRTVECTOR3x & v1,const PVRTVECTOR3x & v2,const int s)689 void PVRTMatrixVec3LerpX(
690 PVRTVECTOR3x &vOut,
691 const PVRTVECTOR3x &v1,
692 const PVRTVECTOR3x &v2,
693 const int s)
694 {
695 vOut.x = v1.x + PVRTXMUL(s, v2.x - v1.x);
696 vOut.y = v1.y + PVRTXMUL(s, v2.y - v1.y);
697 vOut.z = v1.z + PVRTXMUL(s, v2.z - v1.z);
698 }
699
700 /*!***************************************************************************
701 @Function PVRTMatrixVec3DotProductX
702 @Input v1 First vector
703 @Input v2 Second vector
704 @Return Dot product of the two vectors.
705 @Description This function performs the dot product of the two
706 supplied vectors.
707 A single >> 16 shift could be applied to the final accumulated
708 result however this runs the risk of overflow between the
709 results of the intermediate additions.
710 *****************************************************************************/
PVRTMatrixVec3DotProductX(const PVRTVECTOR3x & v1,const PVRTVECTOR3x & v2)711 int PVRTMatrixVec3DotProductX(
712 const PVRTVECTOR3x &v1,
713 const PVRTVECTOR3x &v2)
714 {
715 return (PVRTXMUL(v1.x, v2.x) + PVRTXMUL(v1.y, v2.y) + PVRTXMUL(v1.z, v2.z));
716 }
717
718 /*!***************************************************************************
719 @Function PVRTMatrixVec3CrossProductX
720 @Output vOut Cross product of the two vectors
721 @Input v1 First vector
722 @Input v2 Second vector
723 @Description This function performs the cross product of the two
724 supplied vectors.
725 *****************************************************************************/
PVRTMatrixVec3CrossProductX(PVRTVECTOR3x & vOut,const PVRTVECTOR3x & v1,const PVRTVECTOR3x & v2)726 void PVRTMatrixVec3CrossProductX(
727 PVRTVECTOR3x &vOut,
728 const PVRTVECTOR3x &v1,
729 const PVRTVECTOR3x &v2)
730 {
731 PVRTVECTOR3x result;
732
733 /* Perform calculation on a dummy VECTOR (result) */
734 result.x = PVRTXMUL(v1.y, v2.z) - PVRTXMUL(v1.z, v2.y);
735 result.y = PVRTXMUL(v1.z, v2.x) - PVRTXMUL(v1.x, v2.z);
736 result.z = PVRTXMUL(v1.x, v2.y) - PVRTXMUL(v1.y, v2.x);
737
738 /* Copy result in pOut */
739 vOut = result;
740 }
741
742 /*!***************************************************************************
743 @Function PVRTMatrixVec3NormalizeX
744 @Output vOut Normalized vector
745 @Input vIn Vector to normalize
746 @Description Normalizes the supplied vector.
747 The square root function is currently still performed
748 in floating-point.
749 Original vector is scaled down prior to be normalized in
750 order to avoid overflow issues.
751 ****************************************************************************/
PVRTMatrixVec3NormalizeX(PVRTVECTOR3x & vOut,const PVRTVECTOR3x & vIn)752 void PVRTMatrixVec3NormalizeX(
753 PVRTVECTOR3x &vOut,
754 const PVRTVECTOR3x &vIn)
755 {
756 int f, n;
757 PVRTVECTOR3x vTemp;
758
759 /* Scale vector by uniform value */
760 n = PVRTABS(vIn.x) + PVRTABS(vIn.y) + PVRTABS(vIn.z);
761 vTemp.x = PVRTXDIV(vIn.x, n);
762 vTemp.y = PVRTXDIV(vIn.y, n);
763 vTemp.z = PVRTXDIV(vIn.z, n);
764
765 /* Calculate x2+y2+z2/sqrt(x2+y2+z2) */
766 f = PVRTMatrixVec3DotProductX(vTemp, vTemp);
767 f = PVRTXDIV(PVRTF2X(1.0f), PVRTF2X(sqrt(PVRTX2F(f))));
768
769 /* Multiply vector components by f */
770 vOut.x = PVRTXMUL(vTemp.x, f);
771 vOut.y = PVRTXMUL(vTemp.y, f);
772 vOut.z = PVRTXMUL(vTemp.z, f);
773 }
774
775 /*!***************************************************************************
776 @Function PVRTMatrixVec3LengthX
777 @Input vIn Vector to get the length of
778 @Return The length of the vector
779 @Description Gets the length of the supplied vector
780 *****************************************************************************/
PVRTMatrixVec3LengthX(const PVRTVECTOR3x & vIn)781 int PVRTMatrixVec3LengthX(
782 const PVRTVECTOR3x &vIn)
783 {
784 int temp;
785
786 temp = PVRTXMUL(vIn.x,vIn.x) + PVRTXMUL(vIn.y,vIn.y) + PVRTXMUL(vIn.z,vIn.z);
787 return PVRTF2X(sqrt(PVRTX2F(temp)));
788 }
789
790 /*!***************************************************************************
791 @Function PVRTMatrixLinearEqSolveX
792 @Input pSrc 2D array of floats. 4 Eq linear problem is 5x4
793 matrix, constants in first column
794 @Input nCnt Number of equations to solve
795 @Output pRes Result
796 @Description Solves 'nCnt' simultaneous equations of 'nCnt' variables.
797 pRes should be an array large enough to contain the
798 results: the values of the 'nCnt' variables.
799 This fn recursively uses Gaussian Elimination.
800 *****************************************************************************/
PVRTMatrixLinearEqSolveX(int * const pRes,int ** const pSrc,const int nCnt)801 void PVRTMatrixLinearEqSolveX(
802 int * const pRes,
803 int ** const pSrc,
804 const int nCnt)
805 {
806 int i, j, k;
807 int f;
808
809 if (nCnt == 1)
810 {
811 _ASSERT(pSrc[0][1] != 0);
812 pRes[0] = PVRTXDIV(pSrc[0][0], pSrc[0][1]);
813 return;
814 }
815
816 // Loop backwards in an attempt avoid the need to swap rows
817 i = nCnt;
818 while(i)
819 {
820 --i;
821
822 if(pSrc[i][nCnt] != PVRTF2X(0.0f))
823 {
824 // Row i can be used to zero the other rows; let's move it to the bottom
825 if(i != (nCnt-1))
826 {
827 for(j = 0; j <= nCnt; ++j)
828 {
829 // Swap the two values
830 f = pSrc[nCnt-1][j];
831 pSrc[nCnt-1][j] = pSrc[i][j];
832 pSrc[i][j] = f;
833 }
834 }
835
836 // Now zero the last columns of the top rows
837 for(j = 0; j < (nCnt-1); ++j)
838 {
839 _ASSERT(pSrc[nCnt-1][nCnt] != PVRTF2X(0.0f));
840 f = PVRTXDIV(pSrc[j][nCnt], pSrc[nCnt-1][nCnt]);
841
842 // No need to actually calculate a zero for the final column
843 for(k = 0; k < nCnt; ++k)
844 {
845 pSrc[j][k] -= PVRTXMUL(f, pSrc[nCnt-1][k]);
846 }
847 }
848
849 break;
850 }
851 }
852
853 // Solve the top-left sub matrix
854 PVRTMatrixLinearEqSolveX(pRes, pSrc, nCnt - 1);
855
856 // Now calc the solution for the bottom row
857 f = pSrc[nCnt-1][0];
858 for(k = 1; k < nCnt; ++k)
859 {
860 f -= PVRTXMUL(pSrc[nCnt-1][k], pRes[k-1]);
861 }
862 _ASSERT(pSrc[nCnt-1][nCnt] != PVRTF2X(0));
863 f = PVRTXDIV(f, pSrc[nCnt-1][nCnt]);
864 pRes[nCnt-1] = f;
865 }
866
867 /*****************************************************************************
868 End of file (PVRTMatrixX.cpp)
869 *****************************************************************************/
870
871