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 static void MTX_perspective_test_();
117
118 static void PrintMtxD_(const double *mtxArray, int numElement);
119
120 //----------------------------------------------------------------------------
121 // Concatenation test: entry
MTXTest_appended()122 void MTXTest_appended()
123 {
124 OS_Printf("**================== MTXTest_appended start ====================\n");
125 MTX_concat_43_test_();
126 MTX_concat_44_test_();
127 MTX_inv_43_test_();
128 MTX_perspective_test_();
129 OS_Printf("**================== MTXTest_appended done ====================\n");
130 }
131
132 //----------------------------------------------------------------------------
133 // Output the contents of MtxD to debug console
PrintMtxD_(const double * mtxArray,int numElement)134 static void PrintMtxD_(const double *mtxArray, int numElement)
135 {
136 int i = 0;
137 SDK_ASSERTMSG((numElement == NUM_MTX43_ELEMENT)
138 || (numElement == NUM_MTX44_ELEMENT), "Wrong Mtx Format");
139
140 for (i = 0; i < numElement; i++)
141 {
142 if (i % 4 == 0)
143 OutDetail("\n");
144
145 OutDetail(" %f", mtxArray[i]);
146 }
147 }
148
149 //----------------------------------------------------------------------------
MtxFxToD_(const fx32 * mf,double * md,int numElement)150 static void MtxFxToD_(const fx32 *mf, double *md, int numElement)
151 {
152 int i = 0;
153 SDK_NULL_ASSERT(mf);
154 SDK_NULL_ASSERT(md);
155 SDK_ASSERTMSG((numElement == NUM_MTX43_ELEMENT)
156 || (numElement == NUM_MTX44_ELEMENT), "Wrong Mtx Format");
157
158 for (i = 0; i < numElement; i++)
159 {
160 md[i] = (double)(FX_FX32_TO_F32(mf[i]));
161 }
162 }
163
164 //----------------------------------------------------------------------------
MTX_Concat43D_(const MtxFx43 * afx,const MtxFx43 * bfx,MtxD43 * ab)165 static void MTX_Concat43D_(const MtxFx43 *afx, const MtxFx43 *bfx, MtxD43 * ab)
166 {
167 MtxD43 a;
168 MtxD43 b;
169
170 register double x, y, z;
171 register double xx, yy, zz;
172
173 SDK_NULL_ASSERT(afx);
174 SDK_NULL_ASSERT(bfx);
175 SDK_NULL_ASSERT(ab);
176
177 MtxFxToD_(afx->a, a.a, NUM_MTX43_ELEMENT);
178 MtxFxToD_(bfx->a, b.a, NUM_MTX43_ELEMENT);
179 // Compute (a x b) -> p
180
181 // Row 0
182 x = a._00;
183 y = a._01;
184 z = a._02;
185
186 ab->_00 = x * b._00 + y * b._10 + z * b._20;
187 ab->_01 = x * b._01 + y * b._11 + z * b._21;
188
189 xx = b._02;
190 yy = b._12;
191 zz = b._22;
192
193 ab->_02 = x * xx + y * yy + z * zz;
194
195 // Row 1
196 x = a._10;
197 y = a._11;
198 z = a._12;
199
200 ab->_12 = x * xx + y * yy + z * zz;
201 ab->_11 = x * b._01 + y * b._11 + z * b._21;
202
203 xx = b._00;
204 yy = b._10;
205 zz = b._20;
206
207 ab->_10 = x * xx + y * yy + z * zz;
208
209 // Row 2
210 x = a._20;
211 y = a._21;
212 z = a._22;
213
214 ab->_20 = x * xx + y * yy + z * zz;
215 ab->_21 = x * b._01 + y * b._11 + z * b._21;
216 xx = b._02;
217 yy = b._12;
218 zz = b._22;
219
220 ab->_22 = x * xx + y * yy + z * zz;
221
222 // Row 3
223 x = a._30;
224 y = a._31;
225 z = a._32;
226
227 ab->_32 = x * xx + y * yy + z * zz + b._32;
228 ab->_31 = x * b._01 + y * b._11 + z * b._21 + b._31;
229 ab->_30 = x * b._00 + y * b._10 + z * b._20 + b._30;
230
231 }
232
233 //----------------------------------------------------------------------------
MTX_RotY43D_(MtxD43 * pDst,fx32 sinVal,fx32 cosVal)234 static void MTX_RotY43D_(MtxD43 * pDst, fx32 sinVal, fx32 cosVal)
235 {
236 SDK_NULL_ASSERT(pDst);
237
238 pDst->_00 = (double)FX_FX32_TO_F32(cosVal);
239 pDst->_01 = 0;
240 pDst->_02 = (double)FX_FX32_TO_F32(-sinVal);
241 pDst->_10 = 0;
242 pDst->_11 = 1.0;
243 pDst->_12 = 0;
244 pDst->_20 = (double)FX_FX32_TO_F32(sinVal);
245 pDst->_21 = 0;
246 pDst->_22 = (double)FX_FX32_TO_F32(cosVal);
247 pDst->_30 = 0;
248 pDst->_31 = 0;
249 pDst->_32 = 0;
250 }
251
252 //----------------------------------------------------------------------------
MTX_RotZ43D_(MtxD43 * pDst,fx32 sinVal,fx32 cosVal)253 static void MTX_RotZ43D_(MtxD43 * pDst, fx32 sinVal, fx32 cosVal)
254 {
255 SDK_NULL_ASSERT(pDst);
256
257 pDst->_00 = (double)FX_FX32_TO_F32(cosVal);
258 pDst->_01 = (double)FX_FX32_TO_F32(sinVal);
259 pDst->_02 = 0;
260 pDst->_10 = (double)FX_FX32_TO_F32(-sinVal);
261 pDst->_11 = (double)FX_FX32_TO_F32(cosVal);
262 pDst->_12 = 0;
263 pDst->_20 = 0;
264 pDst->_21 = 0;
265 pDst->_22 = 1.0;
266 pDst->_30 = 0;
267 pDst->_31 = 0;
268 pDst->_32 = 0;
269 }
270
271 //----------------------------------------------------------------------------
MTX_Translate43D_(MtxD43 * pDst,fx32 x,fx32 y,fx32 z)272 static void MTX_Translate43D_(MtxD43 * pDst, fx32 x, fx32 y, fx32 z)
273 {
274 SDK_NULL_ASSERT(pDst);
275
276 pDst->_30 = (double)FX_FX32_TO_F32(x);
277 pDst->_31 = (double)FX_FX32_TO_F32(y);
278 pDst->_32 = (double)FX_FX32_TO_F32(z);
279 }
280
281 //----------------------------------------------------------------------------
282 // 44
MTX_Concat44D_(const MtxFx44 * afx,const MtxFx44 * bfx,MtxD44 * ab)283 static void MTX_Concat44D_(const MtxFx44 *afx, const MtxFx44 *bfx, MtxD44 * ab)
284 {
285 MtxD44 a;
286 MtxD44 b;
287 MtxD44 *m = ab;
288
289 register double x, y, z, w;
290 register double xx, yy, zz, ww;
291
292
293 SDK_NULL_ASSERT(afx);
294 SDK_NULL_ASSERT(bfx);
295 SDK_NULL_ASSERT(ab);
296
297 MtxFxToD_(afx->a, a.a, NUM_MTX44_ELEMENT);
298 MtxFxToD_(bfx->a, b.a, NUM_MTX44_ELEMENT);
299
300
301
302 // Compute (a x b) -> p
303 // Row 0
304 x = a._00;
305 y = a._01;
306 z = a._02;
307 w = a._03;
308
309 ab->_00 = x * b._00 + y * b._10 + z * b._20 + w * b._30;
310 ab->_01 = x * b._01 + y * b._11 + z * b._21 + w * b._31;
311 ab->_03 = x * b._03 + y * b._13 + z * b._23 + w * b._33;
312
313 xx = b._02;
314 yy = b._12;
315 zz = b._22;
316 ww = b._32;
317
318 ab->_02 = x * xx + y * yy + z * zz + w * ww;
319
320 // Row 1
321 x = a._10;
322 y = a._11;
323 z = a._12;
324 w = a._13;
325
326 ab->_12 = x * xx + y * yy + z * zz + w * ww;
327 ab->_11 = x * b._01 + y * b._11 + z * b._21 + w * b._31;
328 ab->_13 = x * b._03 + y * b._13 + z * b._23 + w * b._33;
329
330 xx = b._00;
331 yy = b._10;
332 zz = b._20;
333 ww = b._30;
334
335 ab->_10 = x * xx + y * yy + z * zz + w * ww;
336
337 // Row 2
338 x = a._20;
339 y = a._21;
340 z = a._22;
341 w = a._23;
342
343 ab->_20 = x * xx + y * yy + z * zz + w * ww;
344 ab->_21 = x * b._01 + y * b._11 + z * b._21 + w * b._31;
345 ab->_23 = x * b._03 + y * b._13 + z * b._23 + w * b._33;
346
347 xx = b._02;
348 yy = b._12;
349 zz = b._22;
350 ww = b._32;
351
352 ab->_22 = x * xx + y * yy + z * zz + w * ww;
353
354 // Row 3
355 x = a._30;
356 y = a._31;
357 z = a._32;
358 w = a._33;
359
360 ab->_32 = x * xx + y * yy + z * zz + w * ww;
361 ab->_31 = x * b._01 + y * b._11 + z * b._21 + w * b._31;
362 ab->_30 = x * b._00 + y * b._10 + z * b._20 + w * b._30;
363 ab->_33 = x * b._03 + y * b._13 + z * b._23 + w * b._33;
364
365 }
366
367 //----------------------------------------------------------------------------
IsErrorMtxDValid_(const double * pM,int numElem)368 static BOOL IsErrorMtxDValid_(const double *pM, int numElem)
369 {
370 int i = 0;
371
372 SDK_NULL_ASSERT(pM);
373 SDK_ASSERTMSG((numElem == 12) || (numElem == 16), "Wrong Mtx Format");
374
375 for (i = 0; i < numElem; i++)
376 {
377
378 if (fabs(pM[i]) > CRITICAL_ERROR_THRESHOLD)
379 {
380 OutDetail("Error Mtx \n");
381 for (i = 0; i < numElem; i++)
382 {
383 OutDetail(" %f \n", pM[i]);
384 }
385 return FALSE;
386 }
387 }
388
389 return TRUE;
390 }
391
392 //----------------------------------------------------------------------------
GetDifferenceMtx_(const double * a,const double * b,double * diff,int numElement)393 static void GetDifferenceMtx_(const double *a, const double *b, double *diff, int numElement)
394 {
395 int i = 0;
396 SDK_NULL_ASSERT(a);
397 SDK_NULL_ASSERT(b);
398 SDK_NULL_ASSERT(diff);
399
400 SDK_ASSERTMSG((numElement == 12) || (numElement == 16), "Wrong Mtx Format");
401
402 for (i = 0; i < numElement; i++)
403 {
404 diff[i] = fabs(a[i] - b[i]);
405 }
406 }
407
408 //----------------------------------------------------------------------------
409 // Close to the normal matrix or not (within tolerance or not)
IsNearlyIdentityMtx_(const MtxFx43 * mtxIn)410 static BOOL IsNearlyIdentityMtx_(const MtxFx43 *mtxIn)
411 {
412 int i = 0;
413 MtxD43 mtxInD;
414 MtxD43 errorMtxD;
415
416 MtxFxToD_(mtxIn->a, mtxInD.a, NUM_MTX43_ELEMENT);
417
418 for (i = 0; i < NUM_MTX43_ELEMENT; i++)
419 {
420 errorMtxD.a[i] = fabs(s_mtxIdentity43D.a[i] - mtxInD.a[i]);
421 }
422
423 return IsErrorMtxDValid_(errorMtxD.a, NUM_MTX43_ELEMENT);
424 }
425
426 //----------------------------------------------------------------------------
CheckInvMtxValid_(const MtxFx43 * mtx)427 static BOOL CheckInvMtxValid_(const MtxFx43 *mtx)
428 {
429 MtxFx43 mtxInv, result;
430
431 int invResult = MTX_Inverse43(mtx, &mtxInv);
432
433
434 OutDetail("---------------- mtx\n");
435 print_mtx43(mtx);
436 OutDetail("---------------- Invmtx\n");
437 print_mtx43(&mtxInv);
438
439 SDK_ASSERTMSG(invResult == 0, "Failure in MTX_Inverse43() that must be successful");
440 MTX_Concat43(mtx, &mtxInv, &result);
441
442 // Close to identity matrix or not?
443 // Calculate the difference and check the absolute value
444 return IsNearlyIdentityMtx_(&result);
445 }
446
447 //----------------------------------------------------------------------------
MTX_concat_43_test_()448 static void MTX_concat_43_test_()
449 {
450 const MtxFx43 src43 = {
451 FX32_ONE * 1, FX32_ONE * 2, FX32_ONE * 3,
452 FX32_ONE * 4, FX32_ONE * 5, FX32_ONE * 6,
453 FX32_ONE * 7, FX32_ONE * 8, FX32_ONE * 9,
454 FX32_ONE * 10, FX32_ONE * 11, FX32_ONE * 12
455 };
456
457 MtxD43 resD;
458 MtxFx43 resFx;
459 MtxD43 resFxD;
460 MtxD43 resDiff;
461
462 OS_Printf("==================== MTX_concat_43_test_ start ====================\n");
463
464 MTX_Concat43D_(&src43, &src43, &resD);
465 MTX_Concat43(&src43, &src43, &resFx);
466
467 MtxFxToD_(resFx.a, resFxD.a, NUM_MTX43_ELEMENT);
468
469
470 PrintMtxD_(resFxD.a, NUM_MTX43_ELEMENT);
471 PrintMtxD_(resD.a, NUM_MTX43_ELEMENT);
472
473
474 GetDifferenceMtx_(resD.a, resFxD.a, resDiff.a, NUM_MTX43_ELEMENT);
475
476 MUST_SUCCEED_ASSERT(IsErrorMtxDValid_(resDiff.a, NUM_MTX43_ELEMENT), 1);
477 OS_Printf("==================== MTX_concat_43_test_ done ====================\n");
478 }
479
480 //----------------------------------------------------------------------------
MTX_concat_44_test_()481 static void MTX_concat_44_test_()
482 {
483 const MtxFx44 src44 = {
484 FX32_ONE * 1, FX32_ONE * 2, FX32_ONE * 3, FX32_ONE * 4,
485 FX32_ONE * 5, FX32_ONE * 6, FX32_ONE * 7, FX32_ONE * 8,
486 FX32_ONE * 9, FX32_ONE * 10, FX32_ONE * 11, FX32_ONE * 12,
487 FX32_ONE * 13, FX32_ONE * 14, FX32_ONE * 15, FX32_ONE * 16
488 };
489
490 MtxD44 resD;
491 MtxFx44 resFx;
492 MtxD44 resFxD;
493 MtxD44 resDiff;
494
495 OS_Printf("==================== MTX_concat_44_test_ start ====================\n");
496
497 MTX_Concat44D_(&src44, &src44, &resD);
498 MTX_Concat44(&src44, &src44, &resFx);
499
500 MtxFxToD_(resFx.a, resFxD.a, NUM_MTX44_ELEMENT);
501
502 GetDifferenceMtx_(resD.a, resFxD.a, resDiff.a, NUM_MTX44_ELEMENT);
503
504
505 MUST_SUCCEED_ASSERT(IsErrorMtxDValid_(resDiff.a, NUM_MTX44_ELEMENT), 1);
506 OS_Printf("==================== MTX_concat_44_test_ done ====================\n");
507 }
508
509 //----------------------------------------------------------------------------
MTX_inv_43_test_()510 static void MTX_inv_43_test_()
511 {
512 int radY;
513 int radZ;
514
515 const int STEP_RAD = 256;
516 const int MAX_RAD = 65535;
517
518 MtxFx43 rotYFx, rotZFx, mtxFx, mtxFx2;
519
520 OS_Printf("==================== MTX_inv_43_test_ start ====================\n");
521 // For various rotation matrices, verifies the computation error of the inverse matrices
522 for (radY = 0; radY < MAX_RAD; radY += STEP_RAD)
523 {
524 MTX_RotY43(&rotYFx, FX_SinIdx(radY), FX_CosIdx(radY));
525 for (radZ = 0; radZ < MAX_RAD; radZ += STEP_RAD)
526 {
527 MTX_RotZ43(&rotZFx, FX_SinIdx(radZ), FX_CosIdx(radZ));
528 MTX_Concat43(&rotYFx, &rotZFx, &mtxFx);
529 MTX_TransApply43(&mtxFx, &mtxFx, FX32_ONE, FX32_ONE, FX32_ONE);
530 MTX_TransApply43(&mtxFx, &mtxFx2, FX32_ONE, FX32_ONE, FX32_ONE);
531
532 // Check whether it is valid
533 // Must be valid
534 MUST_SUCCEED_ASSERT(CheckInvMtxValid_(&mtxFx), 1);
535 }
536 }
537
538 // TODO: Something with the determinant close to zero
539
540 OS_Printf("==================== MTX_inv_43_test_ done ====================\n");
541 }
542
543 //----------------------------------------------------------------------------
MTX_perspective_test_()544 static void MTX_perspective_test_()
545 {
546 const fx32 aspect = FX_F32_TO_FX32(1.3333f);
547 const fx32 n = FX_F32_TO_FX32(0.5f);
548 const fx32 f = FX_F32_TO_FX32(100.0f);
549 const fx32 scaleW = FX_F32_TO_FX32(64.0f);
550 const int idx = 65535 / 24; // 15 degrees
551 const MtxFx44 answer = {736941,0,0,0,0,982528,0,0,
552 0,0,-264779,-262144, 0, 0, -263461, 0};
553
554 fx32 fovySin, fovyCos;
555 MtxFx44 mtx;
556
557 fovySin = (fx32)FX_SinIdx(idx);
558 fovyCos = (fx32)FX_CosIdx(idx);
559
560 OS_Printf("==================== MTX_perspective_ start ====================\n");
561 MTX_PerspectiveW(fovySin, fovyCos, aspect, n, f, scaleW, &mtx);
562
563 MUST_SUCCEED_ASSERT(!MI_CpuComp8(&answer, &mtx, sizeof(MtxFx44)), 1);
564
565 OS_Printf("==================== MTX_perspective_ done ====================\n");
566 }
567