4 extern void __ps_guMtxRotAxisRadInternal(register Mtx mt
,const register guVector
*axis
,register f32 sT
,register f32 cT
);
6 void guFrustum(Mtx44 mt
,f32 t
,f32 b
,f32 l
,f32 r
,f32 n
,f32 f
)
26 mt
[2][3] = -(f
*n
)*tmp
;
34 void guPerspective(Mtx44 mt
,f32 fovy
,f32 aspect
,f32 n
,f32 f
)
39 angle
= DegToRad(angle
);
41 cot
= 1.0f
/tanf(angle
);
43 mt
[0][0] = cot
/aspect
;
57 mt
[2][3] = -(f
*n
)*tmp
;
65 void guOrtho(Mtx44 mt
,f32 t
,f32 b
,f32 l
,f32 r
,f32 n
,f32 f
)
73 mt
[0][3] = -(r
+l
)*tmp
;
79 mt
[1][3] = -(t
+b
)*tmp
;
84 mt
[2][2] = -(1.0f
)*tmp
;
93 void guLightPerspective(Mtx mt
,f32 fovY
,f32 aspect
,f32 scaleS
,f32 scaleT
,f32 transS
,f32 transT
)
99 angle
= DegToRad(angle
);
101 cot
= 1.0f
/tanf(angle
);
103 mt
[0][0] = (cot
/ aspect
) * scaleS
;
109 mt
[1][1] = cot
* scaleT
;
119 void guLightOrtho(Mtx mt
,f32 t
,f32 b
,f32 l
,f32 r
,f32 scaleS
,f32 scaleT
,f32 transS
,f32 transT
)
123 tmp
= 1.0f
/ (r
- l
);
124 mt
[0][0] = (2.0f
* tmp
* scaleS
);
127 mt
[0][3] = ((-(r
+ l
) * tmp
) * scaleS
) + transS
;
129 tmp
= 1.0f
/ (t
- b
);
131 mt
[1][1] = (2.0f
* tmp
) * scaleT
;
133 mt
[1][3] = ((-(t
+ b
) * tmp
)* scaleT
) + transT
;
141 void guLightFrustum(Mtx mt
,f32 t
,f32 b
,f32 l
,f32 r
,f32 n
,f32 scaleS
,f32 scaleT
,f32 transS
,f32 transT
)
145 tmp
= 1.0f
/ (r
- l
);
146 mt
[0][0] = ((2*n
) * tmp
) * scaleS
;
148 mt
[0][2] = (((r
+ l
) * tmp
) * scaleS
) - transS
;
151 tmp
= 1.0f
/ (t
- b
);
153 mt
[1][1] = ((2*n
) * tmp
) * scaleT
;
154 mt
[1][2] = (((t
+ b
) * tmp
) * scaleT
) - transT
;
163 void guLookAt(Mtx mt
,guVector
*camPos
,guVector
*camUp
,guVector
*target
)
165 guVector vLook
,vRight
,vUp
;
167 vLook
.x
= camPos
->x
- target
->x
;
168 vLook
.y
= camPos
->y
- target
->y
;
169 vLook
.z
= camPos
->z
- target
->z
;
170 guVecNormalize(&vLook
);
172 guVecCross(camUp
,&vLook
,&vRight
);
173 guVecNormalize(&vRight
);
175 guVecCross(&vLook
,&vRight
,&vUp
);
180 mt
[0][3] = -( camPos
->x
* vRight
.x
+ camPos
->y
* vRight
.y
+ camPos
->z
* vRight
.z
);
185 mt
[1][3] = -( camPos
->x
* vUp
.x
+ camPos
->y
* vUp
.y
+ camPos
->z
* vUp
.z
);
190 mt
[2][3] = -( camPos
->x
* vLook
.x
+ camPos
->y
* vLook
.y
+ camPos
->z
* vLook
.z
);
193 void c_guMtxIdentity(Mtx mt
)
199 if(i
==j
) mt
[i
][j
] = 1.0;
205 void c_guMtxRotRad(Mtx mt
,const char axis
,f32 rad
)
212 c_guMtxRotTrig(mt
,axis
,sinA
,cosA
);
216 void ps_guMtxRotRad(register Mtx mt
,const register char axis
,register f32 rad
)
218 register f32 sinA
,cosA
;
223 ps_guMtxRotTrig(mt
,axis
,sinA
,cosA
);
226 void ps_guMtxRotAxisRad(Mtx mt
,guVector
*axis
,f32 rad
)
233 __ps_guMtxRotAxisRadInternal(mt
,axis
,sinT
,cosT
);
238 void c_guMtxRotTrig(Mtx mt
,const char axis
,f32 sinA
,f32 cosA
)
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
;
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
;
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
;
264 void c_guMtxRotAxisRad(Mtx mt
,guVector
*axis
,f32 rad
)
275 c_guVecNormalize(axis
);
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
);
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
);
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
);
302 void c_guMtxCopy(Mtx src
,Mtx dst
)
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];
311 void c_guMtxConcat(Mtx a
,Mtx b
,Mtx ab
)
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];
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];
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];
340 void c_guMtxScale(Mtx mt
,f32 xS
,f32 yS
,f32 zS
)
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
;
347 void c_guMtxScaleApply(Mtx src
,Mtx dst
,f32 xS
,f32 yS
,f32 zS
)
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
;
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
;
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
;
359 void c_guMtxApplyScale(Mtx src
,Mtx dst
,f32 xS
,f32 yS
,f32 zS
)
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];
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];
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];
371 void c_guMtxTrans(Mtx mt
,f32 xT
,f32 yT
,f32 zT
)
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
;
378 void c_guMtxTransApply(Mtx src
,Mtx dst
,f32 xT
,f32 yT
,f32 zT
)
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];
387 dst
[0][3] = src
[0][3] + xT
;
388 dst
[1][3] = src
[1][3] + yT
;
389 dst
[2][3] = src
[2][3] + zT
;
392 void c_guMtxApplyTrans(Mtx src
,Mtx dst
,f32 xT
,f32 yT
,f32 zT
)
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];
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];
406 u32
c_guMtxInverse(Mtx src
,Mtx inv
)
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];
423 // check if matrix is singular
424 if(det
==0.0f
)return 0;
427 // compute the inverse of the upper submatrix:
429 // find the transposed matrix of cofactors of the upper submatrix
430 // and multiply by (1/det)
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
;
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
;
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
;
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];
453 // copy back if needed
455 c_guMtxCopy(mTmp
,inv
);
460 void c_guMtxTranspose(Mtx src
,Mtx xPose
)
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
;
476 // copy back if needed
478 c_guMtxCopy(mTmp
,xPose
);
481 void c_guMtxReflect(Mtx m
,guVector
*p
,guVector
*n
)
483 f32 vxy
, vxz
, vyz
, pdotn
;
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
);
490 m
[0][0] = 1.0f
- 2.0f
* n
->x
* n
->x
;
493 m
[0][3] = pdotn
* n
->x
;
496 m
[1][1] = 1.0f
- 2.0f
* n
->y
* n
->y
;
498 m
[1][3] = pdotn
* n
->y
;
502 m
[2][2] = 1.0f
- 2.0f
* n
->z
* n
->z
;
503 m
[2][3] = pdotn
* n
->z
;
507 void c_guVecAdd(guVector
*a
,guVector
*b
,guVector
*ab
)
514 void c_guVecSub(guVector
*a
,guVector
*b
,guVector
*ab
)
521 void c_guVecScale(guVector
*src
,guVector
*dst
,f32 scale
)
523 dst
->x
= src
->x
* scale
;
524 dst
->y
= src
->y
* scale
;
525 dst
->z
= src
->z
* scale
;
529 void c_guVecNormalize(guVector
*v
)
533 m
= ((v
->x
)*(v
->x
)) + ((v
->y
)*(v
->y
)) + ((v
->z
)*(v
->z
));
540 void c_guVecCross(guVector
*a
,guVector
*b
,guVector
*axb
)
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
);
553 void c_guVecMultiply(Mtx mt
,guVector
*src
,guVector
*dst
)
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];
566 void c_guVecMultiplySR(Mtx mt
,guVector
*src
,guVector
*dst
)
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
;
580 f32
c_guVecDotProduct(guVector
*a
,guVector
*b
)
584 dot
= (a
->x
* b
->x
) + (a
->y
* b
->y
) + (a
->z
* b
->z
);
589 void c_guQuatAdd(guQuaternion
*a
,guQuaternion
*b
,guQuaternion
*ab
)
598 void ps_guQuatAdd(register guQuaternion
*a
,register guQuaternion
*b
,register guQuaternion
*ab
)
600 register f32 tmp0
,tmp1
;
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
)
618 void c_guQuatSub(guQuaternion
*a
,guQuaternion
*b
,guQuaternion
*ab
)
627 void ps_guQuatSub(register guQuaternion
*a
,register guQuaternion
*b
,register guQuaternion
*ab
)
629 register f32 tmp0
,tmp1
;
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
)
647 void c_guQuatMultiply(guQuaternion
*a
,guQuaternion
*b
,guQuaternion
*ab
)
652 if(a
==ab
|| b
==ab
) r
= &ab_tmp
;
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
;
660 if(r
==&ab_tmp
) *ab
= ab_tmp
;
664 void ps_guQuatMultiply(register guQuaternion
*a
,register guQuaternion
*b
,register guQuaternion
*ab
)
666 register f32 aXY
,aZW
,bXY
,bZW
;
667 register f32 tmp0
,tmp1
,tmp2
,tmp3
,tmp4
,tmp5
,tmp6
,tmp7
;
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
)
699 void c_guQuatNormalize(guQuaternion
*a
,guQuaternion
*d
)
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
;
706 scale
= 1.0f
/sqrtf(dot
);
715 void ps_guQuatNormalize(register guQuaternion
*a
,register guQuaternion
*d
)
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
;
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
)
746 void c_guQuatInverse(guQuaternion
*a
,guQuaternion
*d
)
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
;
761 void ps_guQuatInverse(register guQuaternion
*a
,register guQuaternion
*d
)
763 register f32 c_one
= 1.0f
;
764 register f32 axy
,azw
,tmp0
,tmp1
,tmp2
,tmp3
,tmp4
,tmp5
;
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
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" //
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
)
798 void c_guQuatMtx(guQuaternion
*a
,Mtx m
)
800 const f32 diag
= guMtxRowCol(m
,0,0) + guMtxRowCol(m
,1,1) + guMtxRowCol(m
,2,2) + 1;
803 const f32 scale
= sqrtf(diag
)*2.0f
;
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
;
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
;
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
;
820 a
->x
= (guMtxRowCol(m
,0,1) + guMtxRowCol(m
,1,0))/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
;
825 const f32 scale
= sqrtf(1.0f
+ guMtxRowCol(m
,0,0) + guMtxRowCol(m
,1,1) + guMtxRowCol(m
,2,2))*2.0f
;
827 a
->x
= (guMtxRowCol(m
,0,2) + guMtxRowCol(m
,2,0))/scale
;
828 a
->y
= (guMtxRowCol(m
,1,2) + guMtxRowCol(m
,2,1))/scale
;
830 a
->w
= (guMtxRowCol(m
,1,0) - guMtxRowCol(m
,0,1))/scale
;
833 c_guQuatNormalize(a
,a
);
836 void c_guMtxQuat(Mtx m
,guQuaternion
*a
)
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
;
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
;
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
;
851 void guVecHalfAngle(guVector
*a
,guVector
*b
,guVector
*half
)
853 guVector tmp1
,tmp2
,tmp3
;
863 guVecNormalize(&tmp1
);
864 guVecNormalize(&tmp2
);
866 guVecAdd(&tmp1
,&tmp2
,&tmp3
);
867 if(guVecDotProduct(&tmp3
,&tmp3
)>0.0f
) guVecNormalize(&tmp3
);