?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
tinysmb devoptab fix - allow fread sizes > 7236
[libogc.git] / libogc / gu.c
blob53b9405d16b3f121a3933dbad2a8d014aad7f451
1 #include <gu.h>
2 #include <math.h>
3
4 extern void __ps_guMtxRotAxisRadInternal(register Mtx mt,const register Vector *axis,register f32 sT,register f32 cT);
5
6 void guFrustum(Mtx44 mt,f32 t,f32 b,f32 l,f32 r,f32 n,f32 f)
7 {
8 f32 tmp;
9
10 tmp = 1.0f/(r-l);
11 mt[0][0] = (2*n)*tmp;
12 mt[0][1] = 0.0f;
13 mt[0][2] = (r+l)*tmp;
14 mt[0][3] = 0.0f;
15
16 tmp = 1.0f/(t-b);
17 mt[1][0] = 0.0f;
18 mt[1][1] = (2*n)*tmp;
19 mt[1][2] = (t+b)*tmp;
20 mt[1][3] = 0.0f;
21
22 tmp = 1.0f/(f-n);
23 mt[2][0] = 0.0f;
24 mt[2][1] = 0.0f;
25 mt[2][2] = -(n)*tmp;
26 mt[2][3] = -(f*n)*tmp;
27
28 mt[3][0] = 0.0f;
29 mt[3][1] = 0.0f;
30 mt[3][2] = -1.0f;
31 mt[3][3] = 0.0f;
32 }
33
34 void guPerspective(Mtx44 mt,f32 fovy,f32 aspect,f32 n,f32 f)
35 {
36 f32 cot,angle,tmp;
37
38 angle = fovy*0.5f;
39 angle = DegToRad(angle);
40
41 cot = 1.0f/tanf(angle);
42
43 mt[0][0] = cot/aspect;
44 mt[0][1] = 0.0f;
45 mt[0][2] = 0.0f;
46 mt[0][3] = 0.0f;
47
48 mt[1][0] = 0.0f;
49 mt[1][1] = cot;
50 mt[1][2] = 0.0f;
51 mt[1][3] = 0.0f;
52
53 tmp = 1.0f/(f-n);
54 mt[2][0] = 0.0f;
55 mt[2][1] = 0.0f;
56 mt[2][2] = -(n)*tmp;
57 mt[2][3] = -(f*n)*tmp;
58
59 mt[3][0] = 0.0f;
60 mt[3][1] = 0.0f;
61 mt[3][2] = -1.0f;
62 mt[3][3] = 0.0f;
63 }
64
65 void guOrtho(Mtx44 mt,f32 t,f32 b,f32 l,f32 r,f32 n,f32 f)
66 {
67 f32 tmp;
68
69 tmp = 1.0f/(r-l);
70 mt[0][0] = 2.0f*tmp;
71 mt[0][1] = 0.0f;
72 mt[0][2] = 0.0f;
73 mt[0][3] = -(r+l)*tmp;
74
75 tmp = 1.0f/(t-b);
76 mt[1][0] = 0.0f;
77 mt[1][1] = 2.0f*tmp;
78 mt[1][2] = 0.0f;
79 mt[1][3] = -(t+b)*tmp;
80
81 tmp = 1.0f/(f-n);
82 mt[2][0] = 0.0f;
83 mt[2][1] = 0.0f;
84 mt[2][2] = -(1.0f)*tmp;
85 mt[2][3] = -(f)*tmp;
86
87 mt[3][0] = 0.0f;
88 mt[3][1] = 0.0f;
89 mt[3][2] = 0.0f;
90 mt[3][3] = 1.0f;
91 }
92
93 void guLightPerspective(Mtx mt,f32 fovY,f32 aspect,f32 scaleS,f32 scaleT,f32 transS,f32 transT)
94 {
95 f32 angle;
96 f32 cot;
97
98 angle = fovY*0.5f;
99 angle = DegToRad(angle);
100
101 cot = 1.0f/tanf(angle);
102
103 mt[0][0] = (cot / aspect) * scaleS;
104 mt[0][1] = 0.0f;
105 mt[0][2] = -transS;
106 mt[0][3] = 0.0f;
107
108 mt[1][0] = 0.0f;
109 mt[1][1] = cot * scaleT;
110 mt[1][2] = -transT;
111 mt[1][3] = 0.0f;
112
113 mt[2][0] = 0.0f;
114 mt[2][1] = 0.0f;
115 mt[2][2] = -1.0f;
116 mt[2][3] = 0.0f;
117 }
118
119 void guLightOrtho(Mtx mt,f32 t,f32 b,f32 l,f32 r,f32 scaleS,f32 scaleT,f32 transS,f32 transT)
120 {
121 f32 tmp;
122
123 tmp = 1.0f / (r - l);
124 mt[0][0] = (2.0f * tmp * scaleS);
125 mt[0][1] = 0.0f;
126 mt[0][2] = 0.0f;
127 mt[0][3] = ((-(r + l) * tmp) * scaleS) + transS;
128
129 tmp = 1.0f / (t - b);
130 mt[1][0] = 0.0f;
131 mt[1][1] = (2.0f * tmp) * scaleT;
132 mt[1][2] = 0.0f;
133 mt[1][3] = ((-(t + b) * tmp)* scaleT) + transT;
134
135 mt[2][0] = 0.0f;
136 mt[2][1] = 0.0f;
137 mt[2][2] = 0.0f;
138 mt[2][3] = 1.0f;
139 }
140
141 void guLightFrustum(Mtx mt,f32 t,f32 b,f32 l,f32 r,f32 n,f32 scaleS,f32 scaleT,f32 transS,f32 transT)
142 {
143 f32 tmp;
144
145 tmp = 1.0f / (r - l);
146 mt[0][0] = ((2*n) * tmp) * scaleS;
147 mt[0][1] = 0.0f;
148 mt[0][2] = (((r + l) * tmp) * scaleS) - transS;
149 mt[0][3] = 0.0f;
150
151 tmp = 1.0f / (t - b);
152 mt[1][0] = 0.0f;
153 mt[1][1] = ((2*n) * tmp) * scaleT;
154 mt[1][2] = (((t + b) * tmp) * scaleT) - transT;
155 mt[1][3] = 0.0f;
156
157 mt[2][0] = 0.0f;
158 mt[2][1] = 0.0f;
159 mt[2][2] = -1.0f;
160 mt[2][3] = 0.0f;
161 }
162
163 void guLookAt(Mtx mt,Vector *camPos,Vector *camUp,Vector *target)
164 {
165 Vector vLook,vRight,vUp;
166
167 vLook.x = camPos->x - target->x;
168 vLook.y = camPos->y - target->y;
169 vLook.z = camPos->z - target->z;
170 guVecNormalize(&vLook);
171
172 guVecCross(camUp,&vLook,&vRight);
173 guVecNormalize(&vRight);
174
175 guVecCross(&vLook,&vRight,&vUp);
176
177 mt[0][0] = vRight.x;
178 mt[0][1] = vRight.y;
179 mt[0][2] = vRight.z;
180 mt[0][3] = -( camPos->x * vRight.x + camPos->y * vRight.y + camPos->z * vRight.z );
181
182 mt[1][0] = vUp.x;
183 mt[1][1] = vUp.y;
184 mt[1][2] = vUp.z;
185 mt[1][3] = -( camPos->x * vUp.x + camPos->y * vUp.y + camPos->z * vUp.z );
186
187 mt[2][0] = vLook.x;
188 mt[2][1] = vLook.y;
189 mt[2][2] = vLook.z;
190 mt[2][3] = -( camPos->x * vLook.x + camPos->y * vLook.y + camPos->z * vLook.z );
191 }
192
193 void c_guMtxIdentity(Mtx mt)
194 {
195 s32 i,j;
196
197 for(i=0;i<3;i++) {
198 for(j=0;j<4;j++) {
199 if(i==j) mt[i][j] = 1.0;
200 else mt[i][j] = 0.0;
201 }
202 }
203 }
204
205 void c_guMtxRotRad(Mtx mt,const char axis,f32 rad)
206 {
207 f32 sinA,cosA;
208
209 sinA = sinf(rad);
210 cosA = cosf(rad);
211
212 c_guMtxRotTrig(mt,axis,sinA,cosA);
213 }
214
215 #ifdef GEKKO
216 void ps_guMtxRotRad(register Mtx mt,const register char axis,register f32 rad)
217 {
218 register f32 sinA,cosA;
219
220 sinA = sinf(rad);
221 cosA = cosf(rad);
222
223 ps_guMtxRotTrig(mt,axis,sinA,cosA);
224 }
225
226 void ps_guMtxRotAxisRad(Mtx mt,Vector *axis,f32 rad)
227 {
228 f32 sinT,cosT;
229
230 sinT = sinf(rad);
231 cosT = cosf(rad);
232
233 __ps_guMtxRotAxisRadInternal(mt,axis,sinT,cosT);
234 }
235
236 #endif
237
238 void c_guMtxRotTrig(Mtx mt,const char axis,f32 sinA,f32 cosA)
239 {
240 switch(axis) {
241 case 'x':
242 case 'X':
243 mt[0][0] = 1.0f; mt[0][1] = 0.0f; mt[0][2] = 0.0f; mt[0][3] = 0.0f;
244 mt[1][0] = 0.0f; mt[1][1] = cosA; mt[1][2] = -sinA; mt[1][3] = 0.0f;
245 mt[2][0] = 0.0f; mt[2][1] = sinA; mt[2][2] = cosA; mt[2][3] = 0.0f;
246 break;
247 case 'y':
248 case 'Y':
249 mt[0][0] = cosA; mt[0][1] = 0.0f; mt[0][2] = sinA; mt[0][3] = 0.0f;
250 mt[1][0] = 0.0f; mt[1][1] = 1.0f; mt[1][2] = 0.0f; mt[1][3] = 0.0f;
251 mt[2][0] = -sinA; mt[2][1] = 0.0f; mt[2][2] = cosA; mt[2][3] = 0.0f;
252 break;
253 case 'z':
254 case 'Z':
255 mt[0][0] = cosA; mt[0][1] = -sinA; mt[0][2] = 0.0f; mt[0][3] = 0.0f;
256 mt[1][0] = sinA; mt[1][1] = cosA; mt[1][2] = 0.0f; mt[1][3] = 0.0f;
257 mt[2][0] = 0.0f; mt[2][1] = 0.0f; mt[2][2] = 1.0f; mt[2][3] = 0.0f;
258 break;
259 default:
260 break;
261 }
262 }
263
264 void c_guMtxRotAxisRad(Mtx mt,Vector *axis,f32 rad)
265 {
266 f32 s,c;
267 f32 t;
268 f32 x,y,z;
269 f32 xSq,ySq,zSq;
270
271 s = sinf(rad);
272 c = cosf(rad);
273 t = 1.0f-c;
274
275 c_guVecNormalize(axis);
276
277 x = axis->x;
278 y = axis->y;
279 z = axis->z;
280
281 xSq = x*x;
282 ySq = y*y;
283 zSq = z*z;
284
285 mt[0][0] = ( t * xSq ) + ( c );
286 mt[0][1] = ( t * x * y ) - ( s * z );
287 mt[0][2] = ( t * x * z ) + ( s * y );
288 mt[0][3] = 0.0f;
289
290 mt[1][0] = ( t * x * y ) + ( s * z );
291 mt[1][1] = ( t * ySq ) + ( c );
292 mt[1][2] = ( t * y * z ) - ( s * x );
293 mt[1][3] = 0.0f;
294
295 mt[2][0] = ( t * x * z ) - ( s * y );
296 mt[2][1] = ( t * y * z ) + ( s * x );
297 mt[2][2] = ( t * zSq ) + ( c );
298 mt[2][3] = 0.0f;
299
300 }
301
302 void c_guMtxCopy(Mtx src,Mtx dst)
303 {
304 if(src==dst) return;
305
306 dst[0][0] = src[0][0]; dst[0][1] = src[0][1]; dst[0][2] = src[0][2]; dst[0][3] = src[0][3];
307 dst[1][0] = src[1][0]; dst[1][1] = src[1][1]; dst[1][2] = src[1][2]; dst[1][3] = src[1][3];
308 dst[2][0] = src[2][0]; dst[2][1] = src[2][1]; dst[2][2] = src[2][2]; dst[2][3] = src[2][3];
309 }
310
311 void c_guMtxConcat(Mtx a,Mtx b,Mtx ab)
312 {
313 Mtx tmp;
314 MtxP m;
315
316 if(ab==b || ab==a)
317 m = tmp;
318 else
319 m = ab;
320
321 m[0][0] = a[0][0]*b[0][0] + a[0][1]*b[1][0] + a[0][2]*b[2][0];
322 m[0][1] = a[0][0]*b[0][1] + a[0][1]*b[1][1] + a[0][2]*b[2][1];
323 m[0][2] = a[0][0]*b[0][2] + a[0][1]*b[1][2] + a[0][2]*b[2][2];
324 m[0][3] = a[0][0]*b[0][3] + a[0][1]*b[1][3] + a[0][2]*b[2][3] + a[0][3];
325
326 m[1][0] = a[1][0]*b[0][0] + a[1][1]*b[1][0] + a[1][2]*b[2][0];
327 m[1][1] = a[1][0]*b[0][1] + a[1][1]*b[1][1] + a[1][2]*b[2][1];
328 m[1][2] = a[1][0]*b[0][2] + a[1][1]*b[1][2] + a[1][2]*b[2][2];
329 m[1][3] = a[1][0]*b[0][3] + a[1][1]*b[1][3] + a[1][2]*b[2][3] + a[1][3];
330
331 m[2][0] = a[2][0]*b[0][0] + a[2][1]*b[1][0] + a[2][2]*b[2][0];
332 m[2][1] = a[2][0]*b[0][1] + a[2][1]*b[1][1] + a[2][2]*b[2][1];
333 m[2][2] = a[2][0]*b[0][2] + a[2][1]*b[1][2] + a[2][2]*b[2][2];
334 m[2][3] = a[2][0]*b[0][3] + a[2][1]*b[1][3] + a[2][2]*b[2][3] + a[2][3];
335
336 if(m==tmp)
337 c_guMtxCopy(tmp,ab);
338 }
339
340 void c_guMtxScale(Mtx mt,f32 xS,f32 yS,f32 zS)
341 {
342 mt[0][0] = xS; mt[0][1] = 0.0f; mt[0][2] = 0.0f; mt[0][3] = 0.0f;
343 mt[1][0] = 0.0f; mt[1][1] = yS; mt[1][2] = 0.0f; mt[1][3] = 0.0f;
344 mt[2][0] = 0.0f; mt[2][1] = 0.0f; mt[2][2] = zS; mt[2][3] = 0.0f;
345 }
346
347 void c_guMtxScaleApply(Mtx src,Mtx dst,f32 xS,f32 yS,f32 zS)
348 {
349 dst[0][0] = src[0][0] * xS; dst[0][1] = src[0][1] * xS;
350 dst[0][2] = src[0][2] * xS; dst[0][3] = src[0][3] * xS;
351
352 dst[1][0] = src[1][0] * yS; dst[1][1] = src[1][1] * yS;
353 dst[1][2] = src[1][2] * yS; dst[1][3] = src[1][3] * yS;
354
355 dst[2][0] = src[2][0] * zS; dst[2][1] = src[2][1] * zS;
356 dst[2][2] = src[2][2] * zS; dst[2][3] = src[2][3] * zS;
357 }
358
359 void c_guMtxApplyScale(Mtx src,Mtx dst,f32 xS,f32 yS,f32 zS)
360 {
361 dst[0][0] = src[0][0] * xS; dst[0][1] = src[0][1] * yS;
362 dst[0][2] = src[0][2] * zS; dst[0][3] = src[0][3];
363
364 dst[1][0] = src[1][0] * xS; dst[1][1] = src[1][1] * yS;
365 dst[1][2] = src[1][2] * zS; dst[1][3] = src[1][3];
366
367 dst[2][0] = src[2][0] * xS; dst[2][1] = src[2][1] * yS;
368 dst[2][2] = src[2][2] * zS; dst[2][3] = src[2][3];
369 }
370
371 void c_guMtxTrans(Mtx mt,f32 xT,f32 yT,f32 zT)
372 {
373 mt[0][0] = 1.0f; mt[0][1] = 0.0f; mt[0][2] = 0.0f; mt[0][3] = xT;
374 mt[1][0] = 0.0f; mt[1][1] = 1.0f; mt[1][2] = 0.0f; mt[1][3] = yT;
375 mt[2][0] = 0.0f; mt[2][1] = 0.0f; mt[2][2] = 1.0f; mt[2][3] = zT;
376 }
377
378 void c_guMtxTransApply(Mtx src,Mtx dst,f32 xT,f32 yT,f32 zT)
379 {
380 if ( src != dst )
381 {
382 dst[0][0] = src[0][0]; dst[0][1] = src[0][1]; dst[0][2] = src[0][2];
383 dst[1][0] = src[1][0]; dst[1][1] = src[1][1]; dst[1][2] = src[1][2];
384 dst[2][0] = src[2][0]; dst[2][1] = src[2][1]; dst[2][2] = src[2][2];
385 }
386
387 dst[0][3] = src[0][3] + xT;
388 dst[1][3] = src[1][3] + yT;
389 dst[2][3] = src[2][3] + zT;
390 }
391
392 void c_guMtxApplyTrans(Mtx src,Mtx dst,f32 xT,f32 yT,f32 zT)
393 {
394 if ( src != dst )
395 {
396 dst[0][0] = src[0][0]; dst[0][1] = src[0][1]; dst[0][2] = src[0][2];
397 dst[1][0] = src[1][0]; dst[1][1] = src[1][1]; dst[1][2] = src[1][2];
398 dst[2][0] = src[2][0]; dst[2][1] = src[2][1]; dst[2][2] = src[2][2];
399 }
400
401 dst[0][3] = src[0][0]*xT + src[0][1]*yT + src[0][2]*zT + src[0][3];
402 dst[1][3] = src[1][0]*xT + src[1][1]*yT + src[1][2]*zT + src[1][3];
403 dst[2][3] = src[2][0]*xT + src[2][1]*yT + src[2][2]*zT + src[2][3];
404 }
405
406 u32 c_guMtxInverse(Mtx src,Mtx inv)
407 {
408 Mtx mTmp;
409 MtxP m;
410 f32 det;
411
412 if(src==inv)
413 m = mTmp;
414 else
415 m = inv;
416
417
418 // compute the determinant of the upper 3x3 submatrix
419 det = src[0][0]*src[1][1]*src[2][2] + src[0][1]*src[1][2]*src[2][0] + src[0][2]*src[1][0]*src[2][1]
420 - src[2][0]*src[1][1]*src[0][2] - src[1][0]*src[0][1]*src[2][2] - src[0][0]*src[2][1]*src[1][2];
421
422
423 // check if matrix is singular
424 if(det==0.0f)return 0;
425
426
427 // compute the inverse of the upper submatrix:
428
429 // find the transposed matrix of cofactors of the upper submatrix
430 // and multiply by (1/det)
431
432 det = 1.0f / det;
433
434
435 m[0][0] = (src[1][1]*src[2][2] - src[2][1]*src[1][2]) * det;
436 m[0][1] = -(src[0][1]*src[2][2] - src[2][1]*src[0][2]) * det;
437 m[0][2] = (src[0][1]*src[1][2] - src[1][1]*src[0][2]) * det;
438
439 m[1][0] = -(src[1][0]*src[2][2] - src[2][0]*src[1][2]) * det;
440 m[1][1] = (src[0][0]*src[2][2] - src[2][0]*src[0][2]) * det;
441 m[1][2] = -(src[0][0]*src[1][2] - src[1][0]*src[0][2]) * det;
442
443 m[2][0] = (src[1][0]*src[2][1] - src[2][0]*src[1][1]) * det;
444 m[2][1] = -(src[0][0]*src[2][1] - src[2][0]*src[0][1]) * det;
445 m[2][2] = (src[0][0]*src[1][1] - src[1][0]*src[0][1]) * det;
446
447
448 // compute (invA)*(-C)
449 m[0][3] = -m[0][0]*src[0][3] - m[0][1]*src[1][3] - m[0][2]*src[2][3];
450 m[1][3] = -m[1][0]*src[0][3] - m[1][1]*src[1][3] - m[1][2]*src[2][3];
451 m[2][3] = -m[2][0]*src[0][3] - m[2][1]*src[1][3] - m[2][2]*src[2][3];
452
453 // copy back if needed
454 if( m == mTmp )
455 c_guMtxCopy(mTmp,inv);
456
457 return 1;
458 }
459
460 void c_guMtxTranspose(Mtx src,Mtx xPose)
461 {
462 Mtx mTmp;
463 MtxP m;
464
465 if(src==xPose)
466 m = mTmp;
467 else
468 m = xPose;
469
470
471 m[0][0] = src[0][0]; m[0][1] = src[1][0]; m[0][2] = src[2][0]; m[0][3] = 0.0f;
472 m[1][0] = src[0][1]; m[1][1] = src[1][1]; m[1][2] = src[2][1]; m[1][3] = 0.0f;
473 m[2][0] = src[0][2]; m[2][1] = src[1][2]; m[2][2] = src[2][2]; m[2][3] = 0.0f;
474
475
476 // copy back if needed
477 if(m==mTmp)
478 c_guMtxCopy(mTmp,xPose);
479 }
480
481 void c_guMtxReflect(Mtx m,Vector *p,Vector *n)
482 {
483 f32 vxy, vxz, vyz, pdotn;
484
485 vxy = -2.0f * n->x * n->y;
486 vxz = -2.0f * n->x * n->z;
487 vyz = -2.0f * n->y * n->z;
488 pdotn = 2.0f * c_guVecDotProduct(p,n);
489
490 m[0][0] = 1.0f - 2.0f * n->x * n->x;
491 m[0][1] = vxy;
492 m[0][2] = vxz;
493 m[0][3] = pdotn * n->x;
494
495 m[1][0] = vxy;
496 m[1][1] = 1.0f - 2.0f * n->y * n->y;
497 m[1][2] = vyz;
498 m[1][3] = pdotn * n->y;
499
500 m[2][0] = vxz;
501 m[2][1] = vyz;
502 m[2][2] = 1.0f - 2.0f * n->z * n->z;
503 m[2][3] = pdotn * n->z;
504 }
505
506
507 void c_guVecAdd(Vector *a,Vector *b,Vector *ab)
508 {
509 ab->x = a->x + b->x;
510 ab->y = a->y + b->y;
511 ab->z = a->z + b->z;
512 }
513
514 void c_guVecSub(Vector *a,Vector *b,Vector *ab)
515 {
516 ab->x = a->x - b->x;
517 ab->y = a->y - b->y;
518 ab->z = a->z - b->z;
519 }
520
521 void c_guVecScale(Vector *src,Vector *dst,f32 scale)
522 {
523 dst->x = src->x * scale;
524 dst->y = src->y * scale;
525 dst->z = src->z * scale;
526 }
527
528
529 void c_guVecNormalize(Vector *v)
530 {
531 f32 m;
532
533 m = ((v->x)*(v->x)) + ((v->y)*(v->y)) + ((v->z)*(v->z));
534 m = 1/sqrtf(m);
535 v->x *= m;
536 v->y *= m;
537 v->z *= m;
538 }
539
540 void c_guVecCross(Vector *a,Vector *b,Vector *axb)
541 {
542 Vector vTmp;
543
544 vTmp.x = (a->y*b->z)-(a->z*b->y);
545 vTmp.y = (a->z*b->x)-(a->x*b->z);
546 vTmp.z = (a->x*b->y)-(a->y*b->x);
547
548 axb->x = vTmp.x;
549 axb->y = vTmp.y;
550 axb->z = vTmp.z;
551 }
552
553 void c_guVecMultiply(Mtx mt,Vector *src,Vector *dst)
554 {
555 Vector tmp;
556
557 tmp.x = mt[0][0]*src->x + mt[0][1]*src->y + mt[0][2]*src->z + mt[0][3];
558 tmp.y = mt[1][0]*src->x + mt[1][1]*src->y + mt[1][2]*src->z + mt[1][3];
559 tmp.z = mt[2][0]*src->x + mt[2][1]*src->y + mt[2][2]*src->z + mt[2][3];
560
561 dst->x = tmp.x;
562 dst->y = tmp.y;
563 dst->z = tmp.z;
564 }
565
566 void c_guVecMultiplySR(Mtx mt,Vector *src,Vector *dst)
567 {
568 Vector tmp;
569
570 tmp.x = mt[0][0]*src->x + mt[0][1]*src->y + mt[0][2]*src->z;
571 tmp.y = mt[1][0]*src->x + mt[1][1]*src->y + mt[1][2]*src->z;
572 tmp.z = mt[2][0]*src->x + mt[2][1]*src->y + mt[2][2]*src->z;
573
574 // copy back
575 dst->x = tmp.x;
576 dst->y = tmp.y;
577 dst->z = tmp.z;
578 }
579
580 f32 c_guVecDotProduct(Vector *a,Vector *b)
581 {
582 f32 dot;
583
584 dot = (a->x * b->x) + (a->y * b->y) + (a->z * b->z);
585
586 return dot;
587 }
588
589 void c_guQuatAdd(Quaternion *a,Quaternion *b,Quaternion *ab)
590 {
591 ab->x = a->x + b->x;
592 ab->y = a->x + b->y;
593 ab->z = a->x + b->z;
594 ab->w = a->x + b->w;
595 }
596
597 #ifdef GEKKO
598 void ps_guQuatAdd(register Quaternion *a,register Quaternion *b,register Quaternion *ab)
599 {
600 register f32 tmp0,tmp1;
601
602 __asm__ __volatile__ (
603 "psq_l %0,0(%2),0,0\n" // [ax][ay]
604 "psq_l %1,0(%3),0,0\n" // [bx][by]
605 "ps_add %1,%0,%1\n" // [ax+bx][ay+by]
606 "psq_st %1,0(%4),0,0\n" // X = [ax+bx], Y = [ay+by]
607 "psq_l %0,8(%2),0,0\n" // [az][aw]
608 "psq_l %1,8(%3),0,0\n" // [bz][bw]
609 "ps_add %1,%0,%1\n" // [az+bz][aw+bw]
610 "psq_st %1,8(%4),0,0" // Z = [az+bz], W = [aw+bw]
611 : "=&f"(tmp0),"=&f"(tmp1)
612 : "b"(a),"b"(b),"b"(ab)
613 : "memory"
614 );
615 }
616 #endif
617
618 void c_guQuatSub(Quaternion *a,Quaternion *b,Quaternion *ab)
619 {
620 ab->x = a->x - b->x;
621 ab->y = a->x - b->y;
622 ab->z = a->x - b->z;
623 ab->w = a->x - b->w;
624 }
625
626 #ifdef GEKKO
627 void ps_guQuatSub(register Quaternion *a,register Quaternion *b,register Quaternion *ab)
628 {
629 register f32 tmp0,tmp1;
630
631 __asm__ __volatile__ (
632 "psq_l %0,0(%2),0,0\n" // [ax][ay]
633 "psq_l %1,0(%3),0,0\n" // [bx][by]
634 "ps_sub %1,%0,%1\n" // [ax-bx][ay-by]
635 "psq_st %1,0(%4),0,0\n" // X = [ax-bx], Y = [ay-by]
636 "psq_l %0,8(%2),0,0\n" // [az][aw]
637 "psq_l %1,8(%3),0,0\n" // [bz][bw]
638 "ps_sub %1,%0,%1\n" // [az-bz][aw-bw]
639 "psq_st %1,8(%4),0,0" // Z = [az-bz], W = [aw-bw]
640 : "=&f"(tmp0),"=&f"(tmp1)
641 : "b"(a),"b"(b),"b"(ab)
642 : "memory"
643 );
644 }
645 #endif
646
647 void c_guQuatMultiply(Quaternion *a,Quaternion *b,Quaternion *ab)
648 {
649 Quaternion *r;
650 Quaternion ab_tmp;
651
652 if(a==ab || b==ab) r = &ab_tmp;
653 else r = ab;
654
655 r->w = a->w*b->w - a->x*b->x - a->y*b->y - a->z*b->z;
656 r->x = a->w*b->x + a->x*b->w + a->y*b->z - a->z*b->y;
657 r->y = a->w*b->y + a->y*b->w + a->z*b->x - a->x*b->z;
658 r->z = a->w*b->z + a->z*b->w + a->x*b->y - a->y*b->x;
659
660 if(r==&ab_tmp) *ab = ab_tmp;
661 }
662
663 #ifdef GEKKO
664 void ps_guQuatMultiply(register Quaternion *a,register Quaternion *b,register Quaternion *ab)
665 {
666 register f32 aXY,aZW,bXY,bZW;
667 register f32 tmp0,tmp1,tmp2,tmp3,tmp4,tmp5,tmp6,tmp7;
668
669 __asm__ __volatile__ (
670 "psq_l %0,0(%12),0,0\n" // [px][py]
671 "psq_l %1,8(%12),0,0\n" // [pz][pw]
672 "psq_l %2,0(%13),0,0\n" // [qx][qy]
673 "ps_neg %4,%0\n" // [-px][-py]
674 "psq_l %3,8(%13),0,0\n" // [qz][qw]
675 "ps_neg %5,%1\n" // [-pz][-pw]
676 "ps_merge01 %6,%4,%0\n" // [-px][py]
677 "ps_muls0 %8,%1,%2\n" // [pz*qx][pw*qx]
678 "ps_muls0 %9,%4,%2\n" // [-px*qx][-py*qx]
679 "ps_merge01 %7,%5,%1\n" // [-pz][pw]
680 "ps_muls1 %11,%6,%2\n" // [-px*qy][py*qy]
681 "ps_madds0 %8,%6,%3,%8\n" // [-px*qz+pz*qx][py*qz+pw*qx]
682 "ps_muls1 %10,%7,%2\n" // [-pz*qy][pw*qy]
683 "ps_madds0 %9,%7,%3,%9\n" // [-pz*qz+-px*qx][pw*qz+-py*qx]
684 "ps_madds1 %11,%5,%3,%11\n" // [-pz*qw+-px*qy][-pw*qw+py*qy]
685 "ps_merge10 %8,%8,%8\n" // [py*qz+pw*qx][-px*qz+pz*qx]
686 "ps_madds1 %10,%0,%3,%10\n" // [px*qw+-pz*qy][py*qw+pw*qy]
687 "ps_merge10 %9,%9,%9\n" // [pw*qz+-py*qx][-pz*qz+-px*qx]
688 "ps_add %8,%8,%10\n" // [py*qz+pw*qx+px*qw+-pz*qy][-px*qz+pz*qx+py*qw+pw*qy]
689 "psq_st %8,0(%14),0,0\n" // X = [py*qz+pw*qx+px*qw+-pz*qy], Y = [-px*qz+pz*qx+py*qw+pw*qy]
690 "ps_sub %9,%9,%11\n" // [pw*qz+-py*qx--pz*qw+-px*qy][-pz*qz+-px*qx--pw*qw+py*qy]
691 "psq_st %9,8(%14),0,0" // Z = [pw*qz+-py*qx--pz*qw+-px*qy], W = [-pz*qz+-px*qx--pw*qw+py*qy]
692 : "=&f"(aXY),"=&f"(aZW),"=&f"(bXY),"=&f"(bZW),"=&f"(tmp0),"=&f"(tmp1),"=&f"(tmp2),"=&f"(tmp3),"=&f"(tmp4),"=&f"(tmp5),"=&f"(tmp6),"=&f"(tmp7)
693 : "b"(a),"b"(b),"b"(ab)
694 : "memory"
695 );
696 }
697 #endif
698
699 void c_guQuatNormalize(Quaternion *a,Quaternion *d)
700 {
701 f32 dot,scale;
702
703 dot = (a->x*a->x) + (a->y*a->y) + (a->z*a->z) + (a->w*a->w);
704 if(dot==0.0f) d->x = d->y = d->z = d->w = 0.0f;
705 else {
706 scale = 1.0f/sqrtf(dot);
707 d->x = a->x*scale;
708 d->y = a->y*scale;
709 d->z = a->z*scale;
710 d->w = a->w*scale;
711 }
712 }
713
714 #ifdef GEKKO
715 void ps_guQuatNormalize(register Quaternion *a,register Quaternion *d)
716 {
717 register f32 c_zero = 0.0f;
718 register f32 c_half = 0.5f;
719 register f32 c_three = 3.0f;
720 register f32 axy,azw,tmp0,tmp1,tmp2,tmp3;
721
722 __asm__ __volatile__ (
723 "psq_l %0,0(%6),0,0\n" // [ax][ay]
724 "ps_mul %2,%0,%0\n" // [ax*ax][ay*ay]
725 "psq_l %1,8(%6),0,0\n" // [az][aw]
726 "ps_madd %2,%1,%1,%2\n" // [az*az+ax*ax][aw*aw+ay*ay]
727 "ps_sum0 %2,%2,%2,%2\n" // [az*az+ax*ax+aw*aw+ay*ay][aw*aw+ay*ay]
728 "frsqrte %3,%2\n" // reciprocal sqrt estimated
729 //Newton-Raphson refinement 1 step: (E/2)*(3 - x*E*E)
730 "fmul %4,%3,%3\n" // E*E
731 "fmul %5,%3,%8\n" // E*0.5 = E/2
732 "fnmsub %4,%4,%2,%9\n" // -(E*E*x - 3) = (3 - x*E*E)
733 "fmul %3,%4,%5\n" // (E/2)*(3 - x*E*E)
734 "ps_sel %3,%2,%3,%10\n" // NaN check: if(mag==0.0f)
735 "ps_muls0 %0,%0,%3\n" // [ax*rsqmag][ay*rsqmag]
736 "ps_muls0 %1,%1,%3\n" // [az*rsqmag][aw*rsqmag]
737 "psq_st %0,0(%7),0,0\n" // X = [az*rsqmag], Y = [aw*rsqmag]
738 "psq_st %1,8(%7),0,0\n" // Z = [az*rsqmag], W = [aw*rsqmag]
739 : "=&f"(axy),"=&f"(azw),"=&f"(tmp0),"=&f"(tmp1),"=&f"(tmp2),"=&f"(tmp3)
740 : "b"(a),"b"(d),"f"(c_half),"f"(c_three),"f"(c_zero)
741 : "memory"
742 );
743 }
744 #endif
745
746 void c_guQuatInverse(Quaternion *a,Quaternion *d)
747 {
748 f32 mag,nrminv;
749
750 mag = (a->x*a->x) + (a->y*a->y) + (a->z*a->z) + (a->w*a->w);
751 if(mag==0.0f) mag = 1.0f;
752
753 nrminv = 1.0f/mag;
754 d->x = -a->x*nrminv;
755 d->y = -a->y*nrminv;
756 d->z = -a->z*nrminv;
757 d->w = a->w*nrminv;
758 }
759
760 #ifdef GEKKO
761 void ps_guQuatInverse(register Quaternion *a,register Quaternion *d)
762 {
763 register f32 c_one = 1.0f;
764 register f32 axy,azw,tmp0,tmp1,tmp2,tmp3,tmp4,tmp5;
765
766 __asm__ __volatile__ (
767 "psq_l %0,0(%8),0,0\n" // [ax][ay]
768 "ps_mul %2,%0,%0\n" // [ax*ax][ay*ay]
769 "ps_sub %3,%10,%10\n" // [1 - 1][1 - 1]
770 "psq_l %1,8(%8),0,0\n" // [az][aw]
771 "ps_madd %2,%1,%1,%2\n" // [az*az+ax*ax][aw*aw+ay*ay]
772 "ps_add %7,%0,%10\n" // [1 + 1][1 + 1]
773 "ps_sum0 %2,%2,%2,%2\n" // [az*az+ax*ax+aw*aw+ay*ay][aw*aw+ay*ay]
774 "fcmpu cr0,%2,%3\n" // [az*az+ax*ax+aw*aw+ay*ay] == 0.0f
775 "beq- 1f\n"
776 "fres %4,%2\n" // 1.0f/mag
777 "ps_neg %5,%2\n" // -mag
778 // Newton-Rapson refinement (x1) : E' = 2E-X*E*E
779 "ps_nmsub %6,%2,%4,%7\n" //
780 "ps_mul %4,%4,%6\n" //
781 "b 2f\n"
782 "1:\n"
783 "fmr %4,%10\n"
784 "2:\n"
785 "ps_neg %7,%4\n"
786 "ps_muls1 %5,%4,%1\n"
787 "ps_muls0 %0,%0,%7\n"
788 "psq_st %5,12(%9),1,0\n"
789 "ps_muls0 %6,%1,%7\n"
790 "psq_st %0,0(%9),0,0\n"
791 "psq_st %6,8(%9),1,0\n"
792 : "=&f"(axy),"=&f"(azw),"=&f"(tmp0),"=&f"(tmp1),"=&f"(tmp2),"=&f"(tmp3),"=&f"(tmp4),"=&f"(tmp5)
793 : "b"(a),"b"(d),"f"(c_one)
794 );
795 }
796 #endif
797
798 void c_guQuatMtx(Quaternion *a,Mtx m)
799 {
800 const f32 diag = guMtxRowCol(m,0,0) + guMtxRowCol(m,1,1) + guMtxRowCol(m,2,2) + 1;
801
802 if(diag>0.0f) {
803 const f32 scale = sqrtf(diag)*2.0f;
804
805 a->x = (guMtxRowCol(m,2,1) - guMtxRowCol(m,1,2))/scale;
806 a->y = (guMtxRowCol(m,0,2) - guMtxRowCol(m,2,0))/scale;
807 a->z = (guMtxRowCol(m,1,0) - guMtxRowCol(m,0,1))/scale;
808 a->w = 0.25f*scale;
809 } else {
810 if(guMtxRowCol(m,0,0)>guMtxRowCol(m,1,1) && guMtxRowCol(m,0,0)>guMtxRowCol(m,2,2)) {
811 const f32 scale = sqrtf(1.0f + guMtxRowCol(m,0,0) + guMtxRowCol(m,1,1) + guMtxRowCol(m,2,2))*2.0f;
812
813 a->x = 0.25f*scale;
814 a->y = (guMtxRowCol(m,0,1) + guMtxRowCol(m,1,0))/scale;
815 a->z = (guMtxRowCol(m,2,0) + guMtxRowCol(m,0,2))/scale;
816 a->w = (guMtxRowCol(m,2,1) - guMtxRowCol(m,1,2))/scale;
817 } else if(guMtxRowCol(m,1,1)>guMtxRowCol(m,2,2)) {
818 const f32 scale = sqrtf(1.0f + guMtxRowCol(m,0,0) + guMtxRowCol(m,1,1) + guMtxRowCol(m,2,2))*2.0f;
819
820 a->x = (guMtxRowCol(m,0,1) + guMtxRowCol(m,1,0))/scale;
821 a->y = 0.25f*scale;
822 a->z = (guMtxRowCol(m,1,2) + guMtxRowCol(m,2,1))/scale;
823 a->w = (guMtxRowCol(m,0,2) - guMtxRowCol(m,2,0))/scale;
824 } else {
825 const f32 scale = sqrtf(1.0f + guMtxRowCol(m,0,0) + guMtxRowCol(m,1,1) + guMtxRowCol(m,2,2))*2.0f;
826
827 a->x = (guMtxRowCol(m,0,2) + guMtxRowCol(m,2,0))/scale;
828 a->y = (guMtxRowCol(m,1,2) + guMtxRowCol(m,2,1))/scale;
829 a->z = 0.25f*scale;
830 a->w = (guMtxRowCol(m,1,0) - guMtxRowCol(m,0,1))/scale;
831 }
832 }
833 c_guQuatNormalize(a,a);
834 }
835
836 void c_guMtxQuat(Mtx m,Quaternion *a)
837 {
838 guMtxRowCol(m,0,0) = 1.0f - 2.0f*a->y*a->y - 2.0f*a->z*a->z;
839 guMtxRowCol(m,1,0) = 2.0f*a->x*a->y - 2.0f*a->z*a->w;
840 guMtxRowCol(m,2,0) = 2.0f*a->x*a->z + 2.0f*a->y*a->w;
841
842 guMtxRowCol(m,0,1) = 2.0f*a->x*a->y + 2.0f*a->z*a->w;
843 guMtxRowCol(m,1,1) = 1.0f - 2.0f*a->x*a->x - 2.0f*a->z*a->z;
844 guMtxRowCol(m,2,1) = 2.0f*a->z*a->y - 2.0f*a->x*a->w;
845
846 guMtxRowCol(m,0,2) = 2.0f*a->x*a->z - 2.0f*a->y*a->w;
847 guMtxRowCol(m,1,2) = 2.0f*a->z*a->y + 2.0f*a->x*a->w;
848 guMtxRowCol(m,2,2) = 1.0f - 2.0f*a->x*a->x - 2.0f*a->y*a->y;
849 }
850
851 void guVecHalfAngle(Vector *a,Vector *b,Vector *half)
852 {
853 Vector tmp1,tmp2,tmp3;
854
855 tmp1.x = -a->x;
856 tmp1.y = -a->y;
857 tmp1.z = -a->z;
858
859 tmp2.x = -b->x;
860 tmp2.y = -b->y;
861 tmp2.z = -b->z;
862
863 guVecNormalize(&tmp1);
864 guVecNormalize(&tmp2);
865
866 guVecAdd(&tmp1,&tmp2,&tmp3);
867 if(guVecDotProduct(&tmp3,&tmp3)>0.0f) guVecNormalize(&tmp3);
868
869 *half = tmp3;
870 }
inofficial libogc fork