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:: 2009-09-08#$
14 $Rev: 11043 $
15 $Author: yosizaki $
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 *---------------------------------------------------------------------------*/
299 SDK_WEAK_SYMBOL
VEC_Normalize(const VecFx32 * pSrc,VecFx32 * pDst)300 void VEC_Normalize(const VecFx32 *pSrc, VecFx32 *pDst)
301 {
302 fx64 t;
303 s32 sqrt;
304
305 SDK_NULL_ASSERT(pSrc);
306 SDK_NULL_ASSERT(pDst);
307 SDK_ASSERT(!CP_IsDivBusy() && !CP_IsSqrtBusy());
308
309 t = (fx64)pSrc->x * pSrc->x;
310 t += (fx64)pSrc->y * pSrc->y;
311 t += (fx64)pSrc->z * pSrc->z; // in s39.24 format
312 SDK_TASSERTMSG(t > 0, "*pSrc is (0, 0, 0)");
313
314 SDK_WARNING(1LL << (FX32_SHIFT + FX32_SHIFT + FX64C_SHIFT) >= (u64)t, "*pSrc is too large!");
315
316 CP_SetDiv64_64(1LL << (FX32_SHIFT + FX32_SHIFT + FX64C_SHIFT), (u64)t);
317 CP_SetSqrt64((u64)(t << 2));
318
319 sqrt = (s32)CP_GetSqrtResult32(); // in s18.13 format
320 t = CP_GetDivResult64(); // in s31.32 format(fx64c)
321
322 t = t * sqrt; // in s18.45 format
323 pDst->x = (fx32)((t * pSrc->x + (1LL << (32 + FX32_SHIFT))) >> (32 + FX32_SHIFT + 1));
324 pDst->y = (fx32)((t * pSrc->y + (1LL << (32 + FX32_SHIFT))) >> (32 + FX32_SHIFT + 1));
325 pDst->z = (fx32)((t * pSrc->z + (1LL << (32 + FX32_SHIFT))) >> (32 + FX32_SHIFT + 1));
326 }
327
328
329 /*---------------------------------------------------------------------------*
330 Name: VEC_Fx16Normalize
331
332 Description: Normalizes a vector
333
334 Arguments: pSrc a pointer to a vector
335 pDst a pointer to the normalized vector
336
337 Returns: none
338 *---------------------------------------------------------------------------*/
339 SDK_WEAK_SYMBOL
VEC_Fx16Normalize(const VecFx16 * pSrc,VecFx16 * pDst)340 void VEC_Fx16Normalize(const VecFx16 *pSrc, VecFx16 *pDst)
341 {
342 fx64 t;
343 s32 sqrt;
344
345 SDK_NULL_ASSERT(pSrc);
346 SDK_NULL_ASSERT(pDst);
347 SDK_ASSERT(!CP_IsDivBusy() && !CP_IsSqrtBusy());
348
349 t = pSrc->x * pSrc->x;
350 t += pSrc->y * pSrc->y;
351 t += pSrc->z * pSrc->z; // in s39.24 format
352 SDK_TASSERTMSG(t > 0, "*pSrc is (0, 0, 0)");
353
354 CP_SetDiv64_64(1LL << (FX32_SHIFT + FX32_SHIFT + FX64C_SHIFT), (u64)t);
355 CP_SetSqrt64((u64)(t << 2));
356
357 sqrt = (s32)CP_GetSqrtResult32(); // in s18.13 format
358 t = CP_GetDivResult64(); // in s31.32 format(fx64c)
359
360 t = t * sqrt; // in s18.45 format
361 pDst->x = (fx16)((t * pSrc->x + (1LL << (32 + FX32_SHIFT))) >> (32 + FX32_SHIFT + 1));
362 pDst->y = (fx16)((t * pSrc->y + (1LL << (32 + FX32_SHIFT))) >> (32 + FX32_SHIFT + 1));
363 pDst->z = (fx16)((t * pSrc->z + (1LL << (32 + FX32_SHIFT))) >> (32 + FX32_SHIFT + 1));
364 }
365
366
367 /*---------------------------------------------------------------------------*
368 Name: VEC_MultAdd
369
370 Description: Computes 'a * v1 + v2'.
371
372 Arguments: a fixed point number(fx32)
373 v1 a pointer to a vector
374 v2 a pointer to a vector
375 pDest a pointer to a resultant vector
376
377 Returns: none
378 *---------------------------------------------------------------------------*/
VEC_MultAdd(fx32 a,const VecFx32 * v1,const VecFx32 * v2,VecFx32 * pDest)379 void VEC_MultAdd(fx32 a, const VecFx32 *v1, const VecFx32 *v2, VecFx32 *pDest)
380 {
381 SDK_NULL_ASSERT(v1);
382 SDK_NULL_ASSERT(v2);
383 SDK_NULL_ASSERT(pDest);
384
385 pDest->x = v2->x + (fx32)(((fx64)a * v1->x) >> FX32_SHIFT);
386 pDest->y = v2->y + (fx32)(((fx64)a * v1->y) >> FX32_SHIFT);
387 pDest->z = v2->z + (fx32)(((fx64)a * v1->z) >> FX32_SHIFT);
388 }
389
390
391 /*---------------------------------------------------------------------------*
392 Name: VEC_MultSubtract
393
394 Description: Computes 'a * v1 - v2'.
395
396 Arguments: a fixed point number(fx32)
397 v1 a pointer to a vector
398 v2 a pointer to a vector
399 pDest a pointer to a resultant vector
400
401 Returns: none
402 *---------------------------------------------------------------------------*/
VEC_MultSubtract(fx32 a,const VecFx32 * v1,const VecFx32 * v2,VecFx32 * pDest)403 void VEC_MultSubtract(fx32 a, const VecFx32 *v1, const VecFx32 *v2, VecFx32 *pDest)
404 {
405 SDK_NULL_ASSERT(v1);
406 SDK_NULL_ASSERT(v2);
407 SDK_NULL_ASSERT(pDest);
408
409 pDest->x = -v2->x + (fx32)(((fx64)a * v1->x) >> FX32_SHIFT);
410 pDest->y = -v2->y + (fx32)(((fx64)a * v1->y) >> FX32_SHIFT);
411 pDest->z = -v2->z + (fx32)(((fx64)a * v1->z) >> FX32_SHIFT);
412 }
413
414
415 /*---------------------------------------------------------------------------*
416 Name: VEC_Distance
417
418 Description: Computes |v1 - v2|
419
420 Arguments: v1 a pointer to a vector
421 v2 a pointer to a vector
422
423 Returns: distance between v1 and v2
424 *---------------------------------------------------------------------------*/
VEC_Distance(const VecFx32 * v1,const VecFx32 * v2)425 fx32 VEC_Distance(const VecFx32 *v1, const VecFx32 *v2)
426 {
427 fx64 tmp;
428 fx32 diff;
429
430 SDK_NULL_ASSERT(v1);
431 SDK_NULL_ASSERT(v2);
432
433 diff = v1->x - v2->x;
434 tmp = (fx64)diff *diff;
435
436 diff = v1->y - v2->y;
437 tmp += (fx64)diff *diff;
438
439 diff = v1->z - v2->z;
440 tmp += (fx64)diff *diff;
441
442 tmp <<= 2;
443 CP_SetSqrt64((u64)tmp);
444
445 return ((fx32)CP_GetSqrtResult32() + 1) >> 1;
446 }
447
448 /*---------------------------------------------------------------------------*
449 Name: VEC_Fx16Distance
450
451 Description: Computes |v1 - v2|
452
453 Arguments: v1 a pointer to a vector
454 v2 a pointer to a vector
455
456 Returns: distance between v1 and v2
457 *---------------------------------------------------------------------------*/
VEC_Fx16Distance(const VecFx16 * v1,const VecFx16 * v2)458 fx32 VEC_Fx16Distance(const VecFx16 *v1, const VecFx16 *v2)
459 {
460 fx64 tmp;
461 fx32 diff;
462
463 SDK_NULL_ASSERT(v1);
464 SDK_NULL_ASSERT(v2);
465
466 diff = v1->x - v2->x;
467 tmp = diff * diff;
468
469 diff = v1->y - v2->y;
470 tmp += diff * diff;
471
472 diff = v1->z - v2->z;
473 tmp += diff * diff;
474
475 tmp <<= 2;
476 CP_SetSqrt64((u64)tmp);
477
478 return ((fx32)CP_GetSqrtResult32() + 1) >> 1;
479 }
480
481
482 #include <nitro/codereset.h>
483