1 /*---------------------------------------------------------------------------*
2 Project: NitroSDK - FX - demos - test
3 File: fx_append_mtx.c
4
5 Copyright 2003-2006 Nintendo. All rights reserved.
6
7 These coded instructions, statements, and computer programs contain
8 proprietary information of Nintendo of America Inc. and/or Nintendo
9 Company Ltd., and are protected by Federal copyright law. They may
10 not be disclosed to third parties or copied or duplicated in any form,
11 in whole or in part, without the prior written consent of Nintendo.
12
13 $Log: fx_append_mtx.c,v $
14 Revision 1.8 2006/01/18 02:11:21 kitase_hirotake
15 do-indent.
16
17 Revision 1.7 2005/02/28 05:26:13 yosizaki
18 do-indent.
19
20 Revision 1.6 2004/04/07 01:27:57 yada
21 Fixed header comment.
22
23 Revision 1.5 2004/03/17 04:39:49 takano_makoto
24 Bug fix in MTX_Concat44D_.
25
26 Revision 1.4 2004/02/06 04:42:11 nishida_kenji
27 Removed MTX_Trans43.
28
29 Revision 1.3 2004/02/05 07:09:02 yasu
30 Changed SDK prefix from iris to nitro.
31
32 Revision 1.2 2004/01/29 01:35:09 kitani_toshikazu
33 Fixed some bugs for debug build, restored appended test cases.
34
35 $NoKeywords: $
36
37 *---------------------------------------------------------------------------*/
38
39
40 #ifndef FX_APPEND_MTX_H_
41 #include "fx_append_mtx.h"
42 #endif // FX_APPEND_MTX_H_
43
44
45 //----------------------------------------------------------------------------
46 // Double Mtx
47 //----------------------------------------------------------------------------
48
49
50 typedef union
51 {
52 struct
53 {
54 double _00, _01, _02, _03;
55 double _10, _11, _12, _13;
56 double _20, _21, _22, _23;
57 double _30, _31, _32, _33;
58 };
59 double m[4][4];
60 double a[16];
61 }
62 MtxD44;
63
64
65 //----------------------------------------------------------------------------
66 // MtxFx43: 4x3 Matrix of fx32
67 //----------------------------------------------------------------------------
68
69
70
71 typedef union
72 {
73 struct
74 {
75 double _00, _01, _02;
76 double _10, _11, _12;
77 double _20, _21, _22;
78 double _30, _31, _32;
79 };
80 double m[4][3];
81 double a[12];
82 }
83 MtxD43;
84
85 const static MtxD43 s_mtxIdentity43D = {
86 1.0, 0, 0,
87 0, 1.0, 0,
88 0, 0, 1.0,
89 0, 0, 0
90 };
91
92 #define NUM_MTX43_ELEMENT 12
93 #define NUM_MTX44_ELEMENT 16
94
95
96 //----------------------------------------------------------------------------
97
98 static void MtxFxToD_(const fx32 *mf, double *md, int numElement);
99 static void MTX_Concat43D_(const MtxFx43 *afx, const MtxFx43 *bfx, MtxD43 * ab);
100 static void MTX_RotY43D_(MtxD43 * pDst, fx32 sinVal, fx32 cosVal);
101 static void MTX_RotZ43D_(MtxD43 * pDst, fx32 sinVal, fx32 cosVal);
102 static void MTX_Translate43D_(MtxD43 * pDst, fx32 x, fx32 y, fx32 z);
103 static void MTX_Concat44D_(const MtxFx44 *afx, const MtxFx44 *bfx, MtxD44 * ab);
104
105 static BOOL IsErrorMtxDValid_(const double *pM, int numElem);
106 static void GetDifferenceMtx_(const double *a, const double *b, double *diff, int numElement);
107 static BOOL IsNearlyIdentityMtx_(const MtxFx43 *mtxIn);
108 static BOOL CheckInvMtxValid_(const MtxFx43 *mtx);
109
110
111 //----------------------------------------------------------------------------
112 // Test methods
113 static void MTX_concat_43_test_();
114 static void MTX_concat_44_test_();
115 static void MTX_inv_43_test_();
116
117 static void PrintMtxD_(const double *mtxArray, int numElement);
118
119 //----------------------------------------------------------------------------
120 // Concatenation test: entry
MTXTest_appended()121 void MTXTest_appended()
122 {
123 OS_Printf("**================== MTXTest_appended start ====================\n");
124 MTX_concat_43_test_();
125 MTX_concat_44_test_();
126 MTX_inv_43_test_();
127 OS_Printf("**================== MTXTest_appended done ====================\n");
128 }
129
130 //----------------------------------------------------------------------------
131 // Output the contents of MtxD to debug console
PrintMtxD_(const double * mtxArray,int numElement)132 static void PrintMtxD_(const double *mtxArray, int numElement)
133 {
134 int i = 0;
135 SDK_ASSERTMSG((numElement == NUM_MTX43_ELEMENT)
136 || (numElement == NUM_MTX44_ELEMENT), "Wrong Mtx Format");
137
138 for (i = 0; i < numElement; i++)
139 {
140 if (i % 4 == 0)
141 OutDetail("\n");
142
143 OutDetail(" %f", mtxArray[i]);
144 }
145 }
146
147 //----------------------------------------------------------------------------
MtxFxToD_(const fx32 * mf,double * md,int numElement)148 static void MtxFxToD_(const fx32 *mf, double *md, int numElement)
149 {
150 int i = 0;
151 SDK_NULL_ASSERT(mf);
152 SDK_NULL_ASSERT(md);
153 SDK_ASSERTMSG((numElement == NUM_MTX43_ELEMENT)
154 || (numElement == NUM_MTX44_ELEMENT), "Wrong Mtx Format");
155
156 for (i = 0; i < numElement; i++)
157 {
158 md[i] = (double)(FX_FX32_TO_F32(mf[i]));
159 }
160 }
161
162 //----------------------------------------------------------------------------
MTX_Concat43D_(const MtxFx43 * afx,const MtxFx43 * bfx,MtxD43 * ab)163 static void MTX_Concat43D_(const MtxFx43 *afx, const MtxFx43 *bfx, MtxD43 * ab)
164 {
165 MtxD43 a;
166 MtxD43 b;
167
168 register double x, y, z;
169 register double xx, yy, zz;
170
171 SDK_NULL_ASSERT(afx);
172 SDK_NULL_ASSERT(bfx);
173 SDK_NULL_ASSERT(ab);
174
175 MtxFxToD_(afx->a, a.a, NUM_MTX43_ELEMENT);
176 MtxFxToD_(bfx->a, b.a, NUM_MTX43_ELEMENT);
177 // Compute (a x b) -> p
178
179 // Row 0
180 x = a._00;
181 y = a._01;
182 z = a._02;
183
184 ab->_00 = x * b._00 + y * b._10 + z * b._20;
185 ab->_01 = x * b._01 + y * b._11 + z * b._21;
186
187 xx = b._02;
188 yy = b._12;
189 zz = b._22;
190
191 ab->_02 = x * xx + y * yy + z * zz;
192
193 // Row 1
194 x = a._10;
195 y = a._11;
196 z = a._12;
197
198 ab->_12 = x * xx + y * yy + z * zz;
199 ab->_11 = x * b._01 + y * b._11 + z * b._21;
200
201 xx = b._00;
202 yy = b._10;
203 zz = b._20;
204
205 ab->_10 = x * xx + y * yy + z * zz;
206
207 // Row 2
208 x = a._20;
209 y = a._21;
210 z = a._22;
211
212 ab->_20 = x * xx + y * yy + z * zz;
213 ab->_21 = x * b._01 + y * b._11 + z * b._21;
214 xx = b._02;
215 yy = b._12;
216 zz = b._22;
217
218 ab->_22 = x * xx + y * yy + z * zz;
219
220 // Row 3
221 x = a._30;
222 y = a._31;
223 z = a._32;
224
225 ab->_32 = x * xx + y * yy + z * zz + b._32;
226 ab->_31 = x * b._01 + y * b._11 + z * b._21 + b._31;
227 ab->_30 = x * b._00 + y * b._10 + z * b._20 + b._30;
228
229 }
230
231 //----------------------------------------------------------------------------
MTX_RotY43D_(MtxD43 * pDst,fx32 sinVal,fx32 cosVal)232 static void MTX_RotY43D_(MtxD43 * pDst, fx32 sinVal, fx32 cosVal)
233 {
234 SDK_NULL_ASSERT(pDst);
235
236 pDst->_00 = (double)FX_FX32_TO_F32(cosVal);
237 pDst->_01 = 0;
238 pDst->_02 = (double)FX_FX32_TO_F32(-sinVal);
239 pDst->_10 = 0;
240 pDst->_11 = 1.0;
241 pDst->_12 = 0;
242 pDst->_20 = (double)FX_FX32_TO_F32(sinVal);
243 pDst->_21 = 0;
244 pDst->_22 = (double)FX_FX32_TO_F32(cosVal);
245 pDst->_30 = 0;
246 pDst->_31 = 0;
247 pDst->_32 = 0;
248 }
249
250 //----------------------------------------------------------------------------
MTX_RotZ43D_(MtxD43 * pDst,fx32 sinVal,fx32 cosVal)251 static void MTX_RotZ43D_(MtxD43 * pDst, fx32 sinVal, fx32 cosVal)
252 {
253 SDK_NULL_ASSERT(pDst);
254
255 pDst->_00 = (double)FX_FX32_TO_F32(cosVal);
256 pDst->_01 = (double)FX_FX32_TO_F32(sinVal);
257 pDst->_02 = 0;
258 pDst->_10 = (double)FX_FX32_TO_F32(-sinVal);
259 pDst->_11 = (double)FX_FX32_TO_F32(cosVal);
260 pDst->_12 = 0;
261 pDst->_20 = 0;
262 pDst->_21 = 0;
263 pDst->_22 = 1.0;
264 pDst->_30 = 0;
265 pDst->_31 = 0;
266 pDst->_32 = 0;
267 }
268
269 //----------------------------------------------------------------------------
MTX_Translate43D_(MtxD43 * pDst,fx32 x,fx32 y,fx32 z)270 static void MTX_Translate43D_(MtxD43 * pDst, fx32 x, fx32 y, fx32 z)
271 {
272 SDK_NULL_ASSERT(pDst);
273
274 pDst->_30 = (double)FX_FX32_TO_F32(x);
275 pDst->_31 = (double)FX_FX32_TO_F32(y);
276 pDst->_32 = (double)FX_FX32_TO_F32(z);
277 }
278
279 //----------------------------------------------------------------------------
280 // 44
MTX_Concat44D_(const MtxFx44 * afx,const MtxFx44 * bfx,MtxD44 * ab)281 static void MTX_Concat44D_(const MtxFx44 *afx, const MtxFx44 *bfx, MtxD44 * ab)
282 {
283 MtxD44 a;
284 MtxD44 b;
285 MtxD44 *m = ab;
286
287 register double x, y, z, w;
288 register double xx, yy, zz, ww;
289
290
291 SDK_NULL_ASSERT(afx);
292 SDK_NULL_ASSERT(bfx);
293 SDK_NULL_ASSERT(ab);
294
295 MtxFxToD_(afx->a, a.a, NUM_MTX44_ELEMENT);
296 MtxFxToD_(bfx->a, b.a, NUM_MTX44_ELEMENT);
297
298
299
300 // Compute (a x b) -> p
301 // Row 0
302 x = a._00;
303 y = a._01;
304 z = a._02;
305 w = a._03;
306
307 ab->_00 = x * b._00 + y * b._10 + z * b._20 + w * b._30;
308 ab->_01 = x * b._01 + y * b._11 + z * b._21 + w * b._31;
309 ab->_03 = x * b._03 + y * b._13 + z * b._23 + w * b._33;
310
311 xx = b._02;
312 yy = b._12;
313 zz = b._22;
314 ww = b._32;
315
316 ab->_02 = x * xx + y * yy + z * zz + w * ww;
317
318 // Row 1
319 x = a._10;
320 y = a._11;
321 z = a._12;
322 w = a._13;
323
324 ab->_12 = x * xx + y * yy + z * zz + w * ww;
325 ab->_11 = x * b._01 + y * b._11 + z * b._21 + w * b._31;
326 ab->_13 = x * b._03 + y * b._13 + z * b._23 + w * b._33;
327
328 xx = b._00;
329 yy = b._10;
330 zz = b._20;
331 ww = b._30;
332
333 ab->_10 = x * xx + y * yy + z * zz + w * ww;
334
335 // Row 2
336 x = a._20;
337 y = a._21;
338 z = a._22;
339 w = a._23;
340
341 ab->_20 = x * xx + y * yy + z * zz + w * ww;
342 ab->_21 = x * b._01 + y * b._11 + z * b._21 + w * b._31;
343 ab->_23 = x * b._03 + y * b._13 + z * b._23 + w * b._33;
344
345 xx = b._02;
346 yy = b._12;
347 zz = b._22;
348 ww = b._32;
349
350 ab->_22 = x * xx + y * yy + z * zz + w * ww;
351
352 // Row 3
353 x = a._30;
354 y = a._31;
355 z = a._32;
356 w = a._33;
357
358 ab->_32 = x * xx + y * yy + z * zz + w * ww;
359 ab->_31 = x * b._01 + y * b._11 + z * b._21 + w * b._31;
360 ab->_30 = x * b._00 + y * b._10 + z * b._20 + w * b._30;
361 ab->_33 = x * b._03 + y * b._13 + z * b._23 + w * b._33;
362
363 }
364
365 //----------------------------------------------------------------------------
IsErrorMtxDValid_(const double * pM,int numElem)366 static BOOL IsErrorMtxDValid_(const double *pM, int numElem)
367 {
368 int i = 0;
369
370 SDK_NULL_ASSERT(pM);
371 SDK_ASSERTMSG((numElem == 12) || (numElem == 16), "Wrong Mtx Format");
372
373 for (i = 0; i < numElem; i++)
374 {
375
376 if (fabs(pM[i]) > CRITICAL_ERROR_THRESHOLD)
377 {
378 OutDetail("Error Mtx \n");
379 for (i = 0; i < numElem; i++)
380 {
381 OutDetail(" %f \n", pM[i]);
382 }
383 return FALSE;
384 }
385 }
386
387 return TRUE;
388 }
389
390 //----------------------------------------------------------------------------
GetDifferenceMtx_(const double * a,const double * b,double * diff,int numElement)391 static void GetDifferenceMtx_(const double *a, const double *b, double *diff, int numElement)
392 {
393 int i = 0;
394 SDK_NULL_ASSERT(a);
395 SDK_NULL_ASSERT(b);
396 SDK_NULL_ASSERT(diff);
397
398 SDK_ASSERTMSG((numElement == 12) || (numElement == 16), "Wrong Mtx Format");
399
400 for (i = 0; i < numElement; i++)
401 {
402 diff[i] = fabs(a[i] - b[i]);
403 }
404 }
405
406 //----------------------------------------------------------------------------
407 // Close to the normal matrix or not (within tolerance or not)
IsNearlyIdentityMtx_(const MtxFx43 * mtxIn)408 static BOOL IsNearlyIdentityMtx_(const MtxFx43 *mtxIn)
409 {
410 int i = 0;
411 MtxD43 mtxInD;
412 MtxD43 errorMtxD;
413
414 MtxFxToD_(mtxIn->a, mtxInD.a, NUM_MTX43_ELEMENT);
415
416 for (i = 0; i < NUM_MTX43_ELEMENT; i++)
417 {
418 errorMtxD.a[i] = fabs(s_mtxIdentity43D.a[i] - mtxInD.a[i]);
419 }
420
421 return IsErrorMtxDValid_(errorMtxD.a, NUM_MTX43_ELEMENT);
422 }
423
424 //----------------------------------------------------------------------------
CheckInvMtxValid_(const MtxFx43 * mtx)425 static BOOL CheckInvMtxValid_(const MtxFx43 *mtx)
426 {
427 MtxFx43 mtxInv, result;
428
429 int invResult = MTX_Inverse43(mtx, &mtxInv);
430
431
432 OutDetail("---------------- mtx\n");
433 print_mtx43(mtx);
434 OutDetail("---------------- Invmtx\n");
435 print_mtx43(&mtxInv);
436
437 SDK_ASSERTMSG(invResult == 0, "Failure in MTX_Inverse43() that must be successful");
438 MTX_Concat43(mtx, &mtxInv, &result);
439
440 // Close to identity matrix or not?
441 // Calculate the difference and check the absolute value
442 return IsNearlyIdentityMtx_(&result);
443 }
444
445 //----------------------------------------------------------------------------
MTX_concat_43_test_()446 static void MTX_concat_43_test_()
447 {
448 const MtxFx43 src43 = {
449 FX32_ONE * 1, FX32_ONE * 2, FX32_ONE * 3,
450 FX32_ONE * 4, FX32_ONE * 5, FX32_ONE * 6,
451 FX32_ONE * 7, FX32_ONE * 8, FX32_ONE * 9,
452 FX32_ONE * 10, FX32_ONE * 11, FX32_ONE * 12
453 };
454
455 MtxD43 resD;
456 MtxFx43 resFx;
457 MtxD43 resFxD;
458 MtxD43 resDiff;
459
460 OS_Printf("==================== MTX_concat_43_test_ start ====================\n");
461
462 MTX_Concat43D_(&src43, &src43, &resD);
463 MTX_Concat43(&src43, &src43, &resFx);
464
465 MtxFxToD_(resFx.a, resFxD.a, NUM_MTX43_ELEMENT);
466
467
468 PrintMtxD_(resFxD.a, NUM_MTX43_ELEMENT);
469 PrintMtxD_(resD.a, NUM_MTX43_ELEMENT);
470
471
472 GetDifferenceMtx_(resD.a, resFxD.a, resDiff.a, NUM_MTX43_ELEMENT);
473
474 MUST_SUCCEED_ASSERT(IsErrorMtxDValid_(resDiff.a, NUM_MTX43_ELEMENT), 1);
475 OS_Printf("==================== MTX_concat_43_test_ done ====================\n");
476 }
477
478 //----------------------------------------------------------------------------
MTX_concat_44_test_()479 static void MTX_concat_44_test_()
480 {
481 const MtxFx44 src44 = {
482 FX32_ONE * 1, FX32_ONE * 2, FX32_ONE * 3, FX32_ONE * 4,
483 FX32_ONE * 5, FX32_ONE * 6, FX32_ONE * 7, FX32_ONE * 8,
484 FX32_ONE * 9, FX32_ONE * 10, FX32_ONE * 11, FX32_ONE * 12,
485 FX32_ONE * 13, FX32_ONE * 14, FX32_ONE * 15, FX32_ONE * 16
486 };
487
488 MtxD44 resD;
489 MtxFx44 resFx;
490 MtxD44 resFxD;
491 MtxD44 resDiff;
492
493 OS_Printf("==================== MTX_concat_44_test_ start ====================\n");
494
495 MTX_Concat44D_(&src44, &src44, &resD);
496 MTX_Concat44(&src44, &src44, &resFx);
497
498 MtxFxToD_(resFx.a, resFxD.a, NUM_MTX44_ELEMENT);
499
500 GetDifferenceMtx_(resD.a, resFxD.a, resDiff.a, NUM_MTX44_ELEMENT);
501
502
503 MUST_SUCCEED_ASSERT(IsErrorMtxDValid_(resDiff.a, NUM_MTX44_ELEMENT), 1);
504 OS_Printf("==================== MTX_concat_44_test_ done ====================\n");
505 }
506
507 //----------------------------------------------------------------------------
MTX_inv_43_test_()508 static void MTX_inv_43_test_()
509 {
510 int radY;
511 int radZ;
512
513 const int STEP_RAD = 256;
514 const int MAX_RAD = 65535;
515
516 MtxFx43 rotYFx, rotZFx, mtxFx, mtxFx2;
517
518 OS_Printf("==================== MTX_inv_43_test_ start ====================\n");
519 // For various rotation matrices, verifies the computation error of the inverse matrices
520 for (radY = 0; radY < MAX_RAD; radY += STEP_RAD)
521 {
522 MTX_RotY43(&rotYFx, FX_SinIdx(radY), FX_CosIdx(radY));
523 for (radZ = 0; radZ < MAX_RAD; radZ += STEP_RAD)
524 {
525 MTX_RotZ43(&rotZFx, FX_SinIdx(radZ), FX_CosIdx(radZ));
526 MTX_Concat43(&rotYFx, &rotZFx, &mtxFx);
527 MTX_TransApply43(&mtxFx, &mtxFx, FX32_ONE, FX32_ONE, FX32_ONE);
528 MTX_TransApply43(&mtxFx, &mtxFx2, FX32_ONE, FX32_ONE, FX32_ONE);
529
530 // Check whether it is valid
531 // Must be valid
532 MUST_SUCCEED_ASSERT(CheckInvMtxValid_(&mtxFx), 1);
533 }
534 }
535
536 // TODO: Something with the determinant close to zero
537
538 OS_Printf("==================== MTX_inv_43_test_ done ====================\n");
539 }
540