1 /*---------------------------------------------------------------------------*
2 Project: TwlSDK - GX -
3 File: fx_vec.c
4
5 Copyright 2003-2008 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 $Date:: 2008-09-18#$
14 $Rev: 8573 $
15 $Author: okubata_ryoma $
16 *---------------------------------------------------------------------------*/
17
18
19
20 #include <nitro/fx/fx_vec.h>
21 #include <nitro/fx/fx_cp.h>
22 #include <nitro/fx/fx_const.h>
23 #include <nitro/cp/divider.h>
24 #include <nitro/cp/sqrt.h>
25
26
27 /*---------------------------------------------------------------------*
28 Name: VEC_Add
29
30 Description: add two vectors.
31
32 Arguments: a a pointer to first vector
33 b a pointer to second vector
34 ab a pointer to resultant vector (a + b).
35 ok if ab == a or ab == b.
36
37 Return: none.
38 *---------------------------------------------------------------------*/
VEC_Add(const VecFx32 * a,const VecFx32 * b,VecFx32 * ab)39 void VEC_Add(const VecFx32 *a, const VecFx32 *b, VecFx32 *ab)
40 {
41 SDK_NULL_ASSERT(a);
42 SDK_NULL_ASSERT(b);
43 SDK_NULL_ASSERT(ab);
44
45 ab->x = a->x + b->x;
46 ab->y = a->y + b->y;
47 ab->z = a->z + b->z;
48 }
49
50
51 /*---------------------------------------------------------------------*
52 Name: VEC_Subtract
53
54 Description: subtract one vector from another.
55
56 Arguments: a a pointer to first vector
57 b a pointer to second vector.
58 a_b a pointer to resultant vector (a - b).
59 ok if a_b == a or a_b == b.
60
61 Return: none.
62 *---------------------------------------------------------------------*/
VEC_Subtract(const VecFx32 * a,const VecFx32 * b,VecFx32 * ab)63 void VEC_Subtract(const VecFx32 *a, const VecFx32 *b, VecFx32 *ab)
64 {
65 SDK_NULL_ASSERT(a);
66 SDK_NULL_ASSERT(b);
67 SDK_NULL_ASSERT(ab);
68
69 ab->x = a->x - b->x;
70 ab->y = a->y - b->y;
71 ab->z = a->z - b->z;
72 }
73
74
75 /*---------------------------------------------------------------------*
76 Name: VEC_Fx16Add
77
78 Description: add two vectors.
79
80 Arguments: a a pointer to first vector
81 b a pointer to second vector
82 ab a pointer to resultant vector (a + b).
83 ok if ab == a or ab == b.
84
85 Return: none.
86 *---------------------------------------------------------------------*/
VEC_Fx16Add(const VecFx16 * a,const VecFx16 * b,VecFx16 * ab)87 void VEC_Fx16Add(const VecFx16 *a, const VecFx16 *b, VecFx16 *ab)
88 {
89 SDK_NULL_ASSERT(a);
90 SDK_NULL_ASSERT(b);
91 SDK_NULL_ASSERT(ab);
92
93 ab->x = (fx16)(a->x + b->x);
94 ab->y = (fx16)(a->y + b->y);
95 ab->z = (fx16)(a->z + b->z);
96 }
97
98
99 /*---------------------------------------------------------------------*
100 Name: VEC_Fx16Subtract
101
102 Description: subtract one vector from another.
103
104 Arguments: a a pointer to first vector
105 b a pointer to second vector.
106 a_b a pointer to resultant vector (a - b).
107 ok if a_b == a or a_b == b.
108
109 Return: none.
110 *---------------------------------------------------------------------*/
VEC_Fx16Subtract(const VecFx16 * a,const VecFx16 * b,VecFx16 * ab)111 void VEC_Fx16Subtract(const VecFx16 *a, const VecFx16 *b, VecFx16 *ab)
112 {
113 SDK_NULL_ASSERT(a);
114 SDK_NULL_ASSERT(b);
115 SDK_NULL_ASSERT(ab);
116
117 ab->x = (fx16)(a->x - b->x);
118 ab->y = (fx16)(a->y - b->y);
119 ab->z = (fx16)(a->z - b->z);
120 }
121
122
123 #include <nitro/code32.h> // Always generate ARM binary for efficiency
124 /*---------------------------------------------------------------------------*
125 Name: VEC_DotProduct
126
127 Description: Computes the dot product of two vectors.
128
129 Arguments: a a pointer to a vector
130 b a pointer to a vector
131
132 Returns: dot product value in fx32 format
133 *---------------------------------------------------------------------------*/
VEC_DotProduct(const VecFx32 * a,const VecFx32 * b)134 fx32 VEC_DotProduct(const VecFx32 *a, const VecFx32 *b)
135 {
136 SDK_NULL_ASSERT(a);
137 SDK_NULL_ASSERT(b);
138 return (fx32)(((fx64)a->x * b->x +
139 (fx64)a->y * b->y + (fx64)a->z * b->z + (1 << (FX64_SHIFT - 1))) >> FX64_SHIFT);
140 }
141
142
143 /*---------------------------------------------------------------------------*
144 Name: VEC_Fx16DotProduct
145
146 Description: Computes the dot product of two vectors.
147
148 Arguments: a a pointer to a vector
149 b a pointer to a vector
150
151 Returns: dot product value in fx32 format
152 *---------------------------------------------------------------------------*/
VEC_Fx16DotProduct(const VecFx16 * a,const VecFx16 * b)153 fx32 VEC_Fx16DotProduct(const VecFx16 *a, const VecFx16 *b)
154 {
155 fx32 tmp1, tmp2;
156 SDK_NULL_ASSERT(a);
157 SDK_NULL_ASSERT(b);
158
159 tmp1 = (a->x * b->x) + (a->y * b->y);
160 tmp2 = (a->z * b->z) + (1 << (FX64_SHIFT - 1));
161 return (fx32)(((fx64)tmp1 + tmp2) >> FX64_SHIFT);
162 }
163
164
165 /*---------------------------------------------------------------------------*
166 Name: VEC_CrossProduct
167
168 Description: Computes the cross product of two vectors.
169
170 Arguments: a a pointer to a vector
171 b a pointer to a vector
172 axb a pointer to the result vector,
173 OK if axb == a or axb == b.
174
175 Returns: none
176 *---------------------------------------------------------------------------*/
VEC_CrossProduct(const VecFx32 * a,const VecFx32 * b,VecFx32 * axb)177 void VEC_CrossProduct(const VecFx32 *a, const VecFx32 *b, VecFx32 *axb)
178 {
179 fx32 x, y, z;
180 SDK_NULL_ASSERT(a);
181 SDK_NULL_ASSERT(b);
182 SDK_NULL_ASSERT(axb);
183
184 x = (fx32)(((fx64)a->y * b->z - (fx64)a->z * b->y + (1 << (FX64_SHIFT - 1))) >> FX64_SHIFT);
185
186 y = (fx32)(((fx64)a->z * b->x - (fx64)a->x * b->z + (1 << (FX64_SHIFT - 1))) >> FX64_SHIFT);
187
188 z = (fx32)(((fx64)a->x * b->y - (fx64)a->y * b->x + (1 << (FX64_SHIFT - 1))) >> FX32_SHIFT);
189
190 axb->x = x;
191 axb->y = y;
192 axb->z = z;
193 }
194
195
196 /*---------------------------------------------------------------------------*
197 Name: VEC_Fx16CrossProduct
198
199 Description: Computes the cross product of two vectors.
200
201 Arguments: a a pointer to a vector
202 b a pointer to a vector
203 axb a pointer to the result vector,
204 OK if axb == a or axb == b.
205
206 Returns: none
207 *---------------------------------------------------------------------------*/
VEC_Fx16CrossProduct(const VecFx16 * a,const VecFx16 * b,VecFx16 * axb)208 void VEC_Fx16CrossProduct(const VecFx16 *a, const VecFx16 *b, VecFx16 *axb)
209 {
210 fx32 x, y, z;
211 SDK_NULL_ASSERT(a);
212 SDK_NULL_ASSERT(b);
213 SDK_NULL_ASSERT(axb);
214
215 x = ((a->y * b->z - a->z * b->y + (FX16_ONE >> 1)) >> FX16_SHIFT);
216 y = ((a->z * b->x - a->x * b->z + (FX16_ONE >> 1)) >> FX16_SHIFT);
217 z = ((a->x * b->y - a->y * b->x + (FX16_ONE >> 1)) >> FX16_SHIFT);
218
219 SDK_WARNING(x <= FX16_MAX && x >= FX16_MIN &&
220 y <= FX16_MAX && y >= FX16_MIN &&
221 z <= FX16_MAX && z >= FX16_MIN,
222 "VEC_Fx16CrossProduct: Overflow (0x%x, 0x%x, 0x%x) x (0x%x, 0x%x, 0x%x).",
223 a->x, a->y, a->z, b->x, b->y, b->z);
224
225 axb->x = (fx16)x;
226 axb->y = (fx16)y;
227 axb->z = (fx16)z;
228 }
229
230
231 /*---------------------------------------------------------------------------*
232 Name: VEC_Mag
233
234 Description: Computes the magnitude of a vector
235
236 Arguments: pSrc a pointer to a vector
237
238 Returns: magnitude of the vector in fx32 format
239 *---------------------------------------------------------------------------*/
VEC_Mag(const VecFx32 * pSrc)240 fx32 VEC_Mag(const VecFx32 *pSrc)
241 {
242 fx64 t;
243 fx32 rval;
244
245 SDK_NULL_ASSERT(pSrc);
246 SDK_ASSERT(!CP_IsSqrtBusy());
247
248 t = (fx64)pSrc->x * pSrc->x;
249 t += (fx64)pSrc->y * pSrc->y;
250 t += (fx64)pSrc->z * pSrc->z;
251
252 t <<= 2; // for better precision
253
254 CP_SetSqrt64((u64)t);
255 rval = ((fx32)CP_GetSqrtResult32() + 1) >> 1;
256 return rval;
257 }
258
259
260 /*---------------------------------------------------------------------------*
261 Name: VEC_Fx16Mag
262
263 Description: Computes the magnitude of a vector
264
265 Arguments: pSrc a pointer to a vector
266
267 Returns: magnitude of the vector in fx32 format
268 *---------------------------------------------------------------------------*/
VEC_Fx16Mag(const VecFx16 * pSrc)269 fx32 VEC_Fx16Mag(const VecFx16 *pSrc)
270 {
271 fx64 t;
272 fx32 rval;
273
274 SDK_NULL_ASSERT(pSrc);
275 SDK_ASSERT(!CP_IsSqrtBusy());
276
277 t = pSrc->x * pSrc->x;
278 t += pSrc->y * pSrc->y;
279 t += pSrc->z * pSrc->z;
280
281 t <<= 2; // for better precision
282
283 CP_SetSqrt64((u64)t);
284 rval = ((fx32)CP_GetSqrtResult32() + 1) >> 1;
285 return rval;
286 }
287
288
289 /*---------------------------------------------------------------------------*
290 Name: VEC_Normalize
291
292 Description: Normalizes a vector
293
294 Arguments: pSrc a pointer to a vector
295 pDst a pointer to the normalized vector
296
297 Returns: none
298 *---------------------------------------------------------------------------*/
VEC_Normalize(const VecFx32 * pSrc,VecFx32 * pDst)299 void VEC_Normalize(const VecFx32 *pSrc, VecFx32 *pDst)
300 {
301 fx64 t;
302 s32 sqrt;
303
304 SDK_NULL_ASSERT(pSrc);
305 SDK_NULL_ASSERT(pDst);
306 SDK_ASSERT(!CP_IsDivBusy() && !CP_IsSqrtBusy());
307
308 t = (fx64)pSrc->x * pSrc->x;
309 t += (fx64)pSrc->y * pSrc->y;
310 t += (fx64)pSrc->z * pSrc->z; // in s39.24 format
311 SDK_ASSERTMSG(t > 0, "*pSrc is (0, 0, 0)");
312
313 SDK_WARNING(1LL << (FX32_SHIFT + FX32_SHIFT + FX64C_SHIFT) >= (u64)t, "*pSrc is too large!");
314
315 CP_SetDiv64_64(1LL << (FX32_SHIFT + FX32_SHIFT + FX64C_SHIFT), (u64)t);
316 CP_SetSqrt64((u64)(t << 2));
317
318 sqrt = (s32)CP_GetSqrtResult32(); // in s18.13 format
319 t = CP_GetDivResult64(); // in s31.32 format(fx64c)
320
321 t = t * sqrt; // in s18.45 format
322 pDst->x = (fx32)((t * pSrc->x + (1LL << (32 + FX32_SHIFT))) >> (32 + FX32_SHIFT + 1));
323 pDst->y = (fx32)((t * pSrc->y + (1LL << (32 + FX32_SHIFT))) >> (32 + FX32_SHIFT + 1));
324 pDst->z = (fx32)((t * pSrc->z + (1LL << (32 + FX32_SHIFT))) >> (32 + FX32_SHIFT + 1));
325 }
326
327
328 /*---------------------------------------------------------------------------*
329 Name: VEC_Fx16Normalize
330
331 Description: Normalizes a vector
332
333 Arguments: pSrc a pointer to a vector
334 pDst a pointer to the normalized vector
335
336 Returns: none
337 *---------------------------------------------------------------------------*/
VEC_Fx16Normalize(const VecFx16 * pSrc,VecFx16 * pDst)338 void VEC_Fx16Normalize(const VecFx16 *pSrc, VecFx16 *pDst)
339 {
340 fx64 t;
341 s32 sqrt;
342
343 SDK_NULL_ASSERT(pSrc);
344 SDK_NULL_ASSERT(pDst);
345 SDK_ASSERT(!CP_IsDivBusy() && !CP_IsSqrtBusy());
346
347 t = pSrc->x * pSrc->x;
348 t += pSrc->y * pSrc->y;
349 t += pSrc->z * pSrc->z; // in s39.24 format
350 SDK_ASSERTMSG(t > 0, "*pSrc is (0, 0, 0)");
351
352 CP_SetDiv64_64(1LL << (FX32_SHIFT + FX32_SHIFT + FX64C_SHIFT), (u64)t);
353 CP_SetSqrt64((u64)(t << 2));
354
355 sqrt = (s32)CP_GetSqrtResult32(); // in s18.13 format
356 t = CP_GetDivResult64(); // in s31.32 format(fx64c)
357
358 t = t * sqrt; // in s18.45 format
359 pDst->x = (fx16)((t * pSrc->x + (1LL << (32 + FX32_SHIFT))) >> (32 + FX32_SHIFT + 1));
360 pDst->y = (fx16)((t * pSrc->y + (1LL << (32 + FX32_SHIFT))) >> (32 + FX32_SHIFT + 1));
361 pDst->z = (fx16)((t * pSrc->z + (1LL << (32 + FX32_SHIFT))) >> (32 + FX32_SHIFT + 1));
362 }
363
364
365 /*---------------------------------------------------------------------------*
366 Name: VEC_MultAdd
367
368 Description: Computes 'a * v1 + v2'.
369
370 Arguments: a fixed point number(fx32)
371 v1 a pointer to a vector
372 v2 a pointer to a vector
373 pDest a pointer to a resultant vector
374
375 Returns: none
376 *---------------------------------------------------------------------------*/
VEC_MultAdd(fx32 a,const VecFx32 * v1,const VecFx32 * v2,VecFx32 * pDest)377 void VEC_MultAdd(fx32 a, const VecFx32 *v1, const VecFx32 *v2, VecFx32 *pDest)
378 {
379 SDK_NULL_ASSERT(v1);
380 SDK_NULL_ASSERT(v2);
381 SDK_NULL_ASSERT(pDest);
382
383 pDest->x = v2->x + (fx32)(((fx64)a * v1->x) >> FX32_SHIFT);
384 pDest->y = v2->y + (fx32)(((fx64)a * v1->y) >> FX32_SHIFT);
385 pDest->z = v2->z + (fx32)(((fx64)a * v1->z) >> FX32_SHIFT);
386 }
387
388
389 /*---------------------------------------------------------------------------*
390 Name: VEC_MultSubtract
391
392 Description: Computes 'a * v1 - v2'.
393
394 Arguments: a fixed point number(fx32)
395 v1 a pointer to a vector
396 v2 a pointer to a vector
397 pDest a pointer to a resultant vector
398
399 Returns: none
400 *---------------------------------------------------------------------------*/
VEC_MultSubtract(fx32 a,const VecFx32 * v1,const VecFx32 * v2,VecFx32 * pDest)401 void VEC_MultSubtract(fx32 a, const VecFx32 *v1, const VecFx32 *v2, VecFx32 *pDest)
402 {
403 SDK_NULL_ASSERT(v1);
404 SDK_NULL_ASSERT(v2);
405 SDK_NULL_ASSERT(pDest);
406
407 pDest->x = -v2->x + (fx32)(((fx64)a * v1->x) >> FX32_SHIFT);
408 pDest->y = -v2->y + (fx32)(((fx64)a * v1->y) >> FX32_SHIFT);
409 pDest->z = -v2->z + (fx32)(((fx64)a * v1->z) >> FX32_SHIFT);
410 }
411
412
413 /*---------------------------------------------------------------------------*
414 Name: VEC_Distance
415
416 Description: Computes |v1 - v2|
417
418 Arguments: v1 a pointer to a vector
419 v2 a pointer to a vector
420
421 Returns: distance between v1 and v2
422 *---------------------------------------------------------------------------*/
VEC_Distance(const VecFx32 * v1,const VecFx32 * v2)423 fx32 VEC_Distance(const VecFx32 *v1, const VecFx32 *v2)
424 {
425 fx64 tmp;
426 fx32 diff;
427
428 SDK_NULL_ASSERT(v1);
429 SDK_NULL_ASSERT(v2);
430
431 diff = v1->x - v2->x;
432 tmp = (fx64)diff *diff;
433
434 diff = v1->y - v2->y;
435 tmp += (fx64)diff *diff;
436
437 diff = v1->z - v2->z;
438 tmp += (fx64)diff *diff;
439
440 tmp <<= 2;
441 CP_SetSqrt64((u64)tmp);
442
443 return ((fx32)CP_GetSqrtResult32() + 1) >> 1;
444 }
445
446 /*---------------------------------------------------------------------------*
447 Name: VEC_Fx16Distance
448
449 Description: Computes |v1 - v2|
450
451 Arguments: v1 a pointer to a vector
452 v2 a pointer to a vector
453
454 Returns: distance between v1 and v2
455 *---------------------------------------------------------------------------*/
VEC_Fx16Distance(const VecFx16 * v1,const VecFx16 * v2)456 fx32 VEC_Fx16Distance(const VecFx16 *v1, const VecFx16 *v2)
457 {
458 fx64 tmp;
459 fx32 diff;
460
461 SDK_NULL_ASSERT(v1);
462 SDK_NULL_ASSERT(v2);
463
464 diff = v1->x - v2->x;
465 tmp = diff * diff;
466
467 diff = v1->y - v2->y;
468 tmp += diff * diff;
469
470 diff = v1->z - v2->z;
471 tmp += diff * diff;
472
473 tmp <<= 2;
474 CP_SetSqrt64((u64)tmp);
475
476 return ((fx32)CP_GetSqrtResult32() + 1) >> 1;
477 }
478
479
480 #include <nitro/codereset.h>
481