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:: 2010-04-16#$
14 $Rev: 11321 $
15 $Author: yada $
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 fx64 tmp1, tmp2;
156 SDK_NULL_ASSERT(a);
157 SDK_NULL_ASSERT(b);
158
159 tmp1 = (fx64)(a->x * b->x) + (a->y * b->y);
160 tmp2 = (fx64)(a->z * b->z) + (1 << (FX64_SHIFT - 1));
161 return (fx32)((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 fx64 x, y, z;
180 SDK_NULL_ASSERT(a);
181 SDK_NULL_ASSERT(b);
182 SDK_NULL_ASSERT(axb);
183
184 x = ((fx64)a->y * b->z - (fx64)a->z * b->y + (1 << (FX64_SHIFT - 1))) >> FX64_SHIFT;
185 y = ((fx64)a->z * b->x - (fx64)a->x * b->z + (1 << (FX64_SHIFT - 1))) >> FX64_SHIFT;
186 z = ((fx64)a->x * b->y - (fx64)a->y * b->x + (1 << (FX64_SHIFT - 1))) >> FX64_SHIFT;
187
188 SDK_WARNING(x <= FX32_MAX && x >= FX32_MIN &&
189 y <= FX32_MAX && y >= FX32_MIN &&
190 z <= FX32_MAX && z >= FX32_MIN,
191 "VEC_CrossProduct: Overflow (0x%x, 0x%x, 0x%x) x (0x%x, 0x%x, 0x%x).",
192 a->x, a->y, a->z, b->x, b->y, b->z);
193
194 axb->x = (fx32)x;
195 axb->y = (fx32)y;
196 axb->z = (fx32)z;
197 }
198
199
200 /*---------------------------------------------------------------------------*
201 Name: VEC_Fx16CrossProduct
202
203 Description: Computes the cross product of two vectors.
204
205 Arguments: a a pointer to a vector
206 b a pointer to a vector
207 axb a pointer to the result vector,
208 OK if axb == a or axb == b.
209
210 Returns: none
211 *---------------------------------------------------------------------------*/
VEC_Fx16CrossProduct(const VecFx16 * a,const VecFx16 * b,VecFx16 * axb)212 void VEC_Fx16CrossProduct(const VecFx16 *a, const VecFx16 *b, VecFx16 *axb)
213 {
214 fx32 x, y, z;
215 SDK_NULL_ASSERT(a);
216 SDK_NULL_ASSERT(b);
217 SDK_NULL_ASSERT(axb);
218
219 x = ((a->y * b->z - a->z * b->y + (FX16_ONE >> 1)) >> FX16_SHIFT);
220 y = ((a->z * b->x - a->x * b->z + (FX16_ONE >> 1)) >> FX16_SHIFT);
221 z = ((a->x * b->y - a->y * b->x + (FX16_ONE >> 1)) >> FX16_SHIFT);
222
223 SDK_WARNING(x <= FX16_MAX && x >= FX16_MIN &&
224 y <= FX16_MAX && y >= FX16_MIN &&
225 z <= FX16_MAX && z >= FX16_MIN,
226 "VEC_Fx16CrossProduct: Overflow (0x%x, 0x%x, 0x%x) x (0x%x, 0x%x, 0x%x).",
227 a->x, a->y, a->z, b->x, b->y, b->z);
228
229 axb->x = (fx16)x;
230 axb->y = (fx16)y;
231 axb->z = (fx16)z;
232 }
233
234
235 /*---------------------------------------------------------------------------*
236 Name: VEC_Mag
237
238 Description: Computes the magnitude of a vector
239
240 Arguments: pSrc a pointer to a vector
241
242 Returns: magnitude of the vector in fx32 format
243 *---------------------------------------------------------------------------*/
VEC_Mag(const VecFx32 * pSrc)244 fx32 VEC_Mag(const VecFx32 *pSrc)
245 {
246 u64 t; // always positive number
247 fx32 rval;
248
249 SDK_NULL_ASSERT(pSrc);
250 SDK_ASSERT(!CP_IsSqrtBusy());
251
252 t = (u64)((fx64)pSrc->x * pSrc->x);
253 t += (u64)((fx64)pSrc->y * pSrc->y);
254 t += (u64)((fx64)pSrc->z * pSrc->z);
255
256 SDK_WARNING(t <= FX64_MAX>>1,
257 "VEC_Mag: Overflow (0x%x, 0x%x, 0x%x).",
258 pSrc->x, pSrc->y, pSrc->z);
259
260 t <<= 2; // for better precision
261
262 CP_SetSqrt64(t);
263 rval = ((fx32)CP_GetSqrtResult32() + 1) >> 1;
264 return rval;
265 }
266
267
268 /*---------------------------------------------------------------------------*
269 Name: VEC_Fx16Mag
270
271 Description: Computes the magnitude of a vector
272
273 Arguments: pSrc a pointer to a vector
274
275 Returns: magnitude of the vector in fx32 format
276 *---------------------------------------------------------------------------*/
VEC_Fx16Mag(const VecFx16 * pSrc)277 fx32 VEC_Fx16Mag(const VecFx16 *pSrc)
278 {
279 fx64 t;
280 fx32 rval;
281
282 SDK_NULL_ASSERT(pSrc);
283 SDK_ASSERT(!CP_IsSqrtBusy());
284
285 t = pSrc->x * pSrc->x;
286 t += pSrc->y * pSrc->y;
287 t += pSrc->z * pSrc->z;
288
289 t <<= 2; // for better precision
290
291 CP_SetSqrt64((u64)t);
292 rval = ((fx32)CP_GetSqrtResult32() + 1) >> 1;
293 return rval;
294 }
295
296
297 /*---------------------------------------------------------------------------*
298 Name: VEC_Normalize
299
300 Description: Normalizes a vector
301
302 Arguments: pSrc a pointer to a vector
303 pDst a pointer to the normalized vector
304
305 Returns: none
306 *---------------------------------------------------------------------------*/
307 SDK_WEAK_SYMBOL
VEC_Normalize(const VecFx32 * pSrc,VecFx32 * pDst)308 void VEC_Normalize(const VecFx32 *pSrc, VecFx32 *pDst)
309 {
310 fx64 t;
311 s32 sqrt;
312
313 SDK_NULL_ASSERT(pSrc);
314 SDK_NULL_ASSERT(pDst);
315 SDK_ASSERT(!CP_IsDivBusy() && !CP_IsSqrtBusy());
316
317 t = (fx64)pSrc->x * pSrc->x;
318 t += (fx64)pSrc->y * pSrc->y;
319 t += (fx64)pSrc->z * pSrc->z; // in s39.24 format
320 SDK_TASSERTMSG(t > 0, "*pSrc is (0, 0, 0)");
321
322 SDK_WARNING(1LL << (FX32_SHIFT + FX32_SHIFT + FX64C_SHIFT) >= (u64)t, "*pSrc is too large!");
323
324 CP_SetDiv64_64(1LL << (FX32_SHIFT + FX32_SHIFT + FX64C_SHIFT), (u64)t);
325 CP_SetSqrt64((u64)(t << 2));
326
327 sqrt = (s32)CP_GetSqrtResult32(); // in s18.13 format
328 t = CP_GetDivResult64(); // in s31.32 format(fx64c)
329
330 t = t * sqrt; // in s18.45 format
331 pDst->x = (fx32)((t * pSrc->x + (1LL << (32 + FX32_SHIFT))) >> (32 + FX32_SHIFT + 1));
332 pDst->y = (fx32)((t * pSrc->y + (1LL << (32 + FX32_SHIFT))) >> (32 + FX32_SHIFT + 1));
333 pDst->z = (fx32)((t * pSrc->z + (1LL << (32 + FX32_SHIFT))) >> (32 + FX32_SHIFT + 1));
334 }
335
336
337 /*---------------------------------------------------------------------------*
338 Name: VEC_Fx16Normalize
339
340 Description: Normalizes a vector
341
342 Arguments: pSrc a pointer to a vector
343 pDst a pointer to the normalized vector
344
345 Returns: none
346 *---------------------------------------------------------------------------*/
347 SDK_WEAK_SYMBOL
VEC_Fx16Normalize(const VecFx16 * pSrc,VecFx16 * pDst)348 void VEC_Fx16Normalize(const VecFx16 *pSrc, VecFx16 *pDst)
349 {
350 fx64 t;
351 s32 sqrt;
352
353 SDK_NULL_ASSERT(pSrc);
354 SDK_NULL_ASSERT(pDst);
355 SDK_ASSERT(!CP_IsDivBusy() && !CP_IsSqrtBusy());
356
357 t = pSrc->x * pSrc->x;
358 t += pSrc->y * pSrc->y;
359 t += pSrc->z * pSrc->z; // in s39.24 format
360 SDK_TASSERTMSG(t > 0, "*pSrc is (0, 0, 0)");
361
362 CP_SetDiv64_64(1LL << (FX32_SHIFT + FX32_SHIFT + FX64C_SHIFT), (u64)t);
363 CP_SetSqrt64((u64)(t << 2));
364
365 sqrt = (s32)CP_GetSqrtResult32(); // in s18.13 format
366 t = CP_GetDivResult64(); // in s31.32 format(fx64c)
367
368 t = t * sqrt; // in s18.45 format
369 pDst->x = (fx16)((t * pSrc->x + (1LL << (32 + FX32_SHIFT))) >> (32 + FX32_SHIFT + 1));
370 pDst->y = (fx16)((t * pSrc->y + (1LL << (32 + FX32_SHIFT))) >> (32 + FX32_SHIFT + 1));
371 pDst->z = (fx16)((t * pSrc->z + (1LL << (32 + FX32_SHIFT))) >> (32 + FX32_SHIFT + 1));
372 }
373
374
375 /*---------------------------------------------------------------------------*
376 Name: VEC_MultAdd
377
378 Description: Computes 'a * v1 + v2'.
379
380 Arguments: a fixed point number(fx32)
381 v1 a pointer to a vector
382 v2 a pointer to a vector
383 pDest a pointer to a resultant vector
384
385 Returns: none
386 *---------------------------------------------------------------------------*/
VEC_MultAdd(fx32 a,const VecFx32 * v1,const VecFx32 * v2,VecFx32 * pDest)387 void VEC_MultAdd(fx32 a, const VecFx32 *v1, const VecFx32 *v2, VecFx32 *pDest)
388 {
389 SDK_NULL_ASSERT(v1);
390 SDK_NULL_ASSERT(v2);
391 SDK_NULL_ASSERT(pDest);
392
393 pDest->x = v2->x + (fx32)(((fx64)a * v1->x) >> FX32_SHIFT);
394 pDest->y = v2->y + (fx32)(((fx64)a * v1->y) >> FX32_SHIFT);
395 pDest->z = v2->z + (fx32)(((fx64)a * v1->z) >> FX32_SHIFT);
396 }
397
398
399 /*---------------------------------------------------------------------------*
400 Name: VEC_MultSubtract
401
402 Description: Computes 'a * v1 - v2'.
403
404 Arguments: a fixed point number(fx32)
405 v1 a pointer to a vector
406 v2 a pointer to a vector
407 pDest a pointer to a resultant vector
408
409 Returns: none
410 *---------------------------------------------------------------------------*/
VEC_MultSubtract(fx32 a,const VecFx32 * v1,const VecFx32 * v2,VecFx32 * pDest)411 void VEC_MultSubtract(fx32 a, const VecFx32 *v1, const VecFx32 *v2, VecFx32 *pDest)
412 {
413 SDK_NULL_ASSERT(v1);
414 SDK_NULL_ASSERT(v2);
415 SDK_NULL_ASSERT(pDest);
416
417 pDest->x = -v2->x + (fx32)(((fx64)a * v1->x) >> FX32_SHIFT);
418 pDest->y = -v2->y + (fx32)(((fx64)a * v1->y) >> FX32_SHIFT);
419 pDest->z = -v2->z + (fx32)(((fx64)a * v1->z) >> FX32_SHIFT);
420 }
421
422
423 /*---------------------------------------------------------------------------*
424 Name: VEC_Distance
425
426 Description: Computes |v1 - v2|
427
428 Arguments: v1 a pointer to a vector
429 v2 a pointer to a vector
430
431 Returns: distance between v1 and v2
432 *---------------------------------------------------------------------------*/
VEC_Distance(const VecFx32 * v1,const VecFx32 * v2)433 fx32 VEC_Distance(const VecFx32 *v1, const VecFx32 *v2)
434 {
435 u64 tmp; // always positive number
436 fx32 diff;
437
438 SDK_NULL_ASSERT(v1);
439 SDK_NULL_ASSERT(v2);
440
441 diff = v1->x - v2->x;
442 tmp = (u64)((fx64)diff *diff);
443
444 diff = v1->y - v2->y;
445 tmp += (u64)((fx64)diff *diff);
446
447 diff = v1->z - v2->z;
448 tmp += (u64)((fx64)diff *diff);
449
450 SDK_WARNING(tmp <= FX64_MAX>>1,
451 "VEC_Distance: Overflow (0x%x, 0x%x, 0x%x), (0x%x, 0x%x, 0x%x).",
452 v1->x, v1->y, v1->z, v2->x, v2->y, v2->z);
453
454 tmp <<= 2; // for better precision
455 CP_SetSqrt64(tmp);
456
457 return ((fx32)CP_GetSqrtResult32() + 1) >> 1;
458 }
459
460 /*---------------------------------------------------------------------------*
461 Name: VEC_Fx16Distance
462
463 Description: Computes |v1 - v2|
464
465 Arguments: v1 a pointer to a vector
466 v2 a pointer to a vector
467
468 Returns: distance between v1 and v2
469 *---------------------------------------------------------------------------*/
VEC_Fx16Distance(const VecFx16 * v1,const VecFx16 * v2)470 fx32 VEC_Fx16Distance(const VecFx16 *v1, const VecFx16 *v2)
471 {
472 u64 tmp;
473 fx32 diff;
474
475 SDK_NULL_ASSERT(v1);
476 SDK_NULL_ASSERT(v2);
477
478 diff = v1->x - v2->x;
479 tmp = (u32)diff * diff;
480
481 diff = v1->y - v2->y;
482 tmp += (u32)diff * diff;
483
484 diff = v1->z - v2->z;
485 tmp += (u32)diff * diff;
486
487 tmp <<= 2;
488 CP_SetSqrt64((u64)tmp);
489
490 return ((fx32)CP_GetSqrtResult32() + 1) >> 1;
491 }
492
493
494 #include <nitro/codereset.h>
495