00001 #include "flGlobal.h"
00002 #if FL_MATH != 0
00003 #include <stdlib.h>
00004 #include <stdio.h>
00005 #include <math.h>
00006
00007 #if FL_MATH_RANDTYPE != FL_MATH_RAND_VFPU
00008 #include <psprtc.h>
00009 #endif
00010
00011 #if FL_INCLUDE_ALL_C == 0
00012 #include "flMath.h"
00013 #endif
00014
00015 #if FL_MATH_TRIGCACHE > 0
00016 MathTrigCache* mathTrigCacheSin = NULL;
00017 bool mathTrigCacheInitialized = false;
00018 u64 mathTrigCacheCalls = 0;
00019 #endif
00020
00021 #if FL_MATH_RANDTYPE == FL_MATH_RAND_MERSENNETWISTER
00022 SceKernelUtilsMt19937Context mathMtContext;
00023 #endif
00024
00025 void mathInit() {
00026 pspTime tempTime;
00027 sceRtcGetCurrentClockLocalTime(&tempTime);
00028 mathRandSeed(tempTime.microseconds * tempTime.seconds);
00029
00030 #if FL_MATH_TRIGCACHE > 0
00031 mathTrigCacheInit();
00032 #endif
00033 }
00034
00035 void mathRandSeed(u32 inSeed) {
00036 #if FL_MATH_RANDTYPE == FL_MATH_RAND_VFPU
00037 __asm__ volatile (
00038 "mtv %0, S000\n"
00039 "vrnds.s S000"
00040 : : "r"(inSeed));
00041 #else
00042 #if FL_MATH_RANDTYPE == FL_MATH_RAND_MERSENNETWISTER
00043 sceKernelUtilsMt19937Init(&mathMtContext, inSeed);
00044 #else
00045 srand(inSeed);
00046 #endif
00047 #endif
00048 }
00049
00050 float mathRandf(float inSize) {
00051 #if FL_MATH_RANDTYPE == FL_MATH_RAND_VFPU
00052 float tempOut;
00053 __asm__ volatile (
00054 "mtv %1, S000\n"
00055 "vrndf1.s S000\n"
00056 "vone.s S002\n"
00057 "vsub.s S000, S000, S002\n"
00058 "vmul.s S000, S000, S001\n"
00059 "mfv %0, S000\n"
00060 : "=r"(tempOut) : "r"(inSize));
00061 return tempOut;
00062 #else
00063 #if FL_MATH_RANDTYPE == FL_MATH_RAND_MERSENNETWISTER
00064 float tempOut = sceKernelUtilsMt19937UInt(&mathMtContext);
00065 return (((float)tempOut * inSize) / (float)MAX_INT);
00066 #else
00067 #if FL_MATH_RANDOMSEED != 0
00068 int tempRand = ((rand() * FL_MATH_RANDOMSEED_FREQUENCY) / RAND_MAX);
00069 if(tempRand == 0) {
00070 pspTime tempTime;
00071 sceRtcGetCurrentClockLocalTime(&tempTime);
00072 srand(tempTime.microseconds * tempTime.seconds);
00073 }
00074 #endif
00075 return (((float)rand() * inSize) / (float)RAND_MAX);
00076 #endif
00077 #endif
00078 }
00079
00080 float mathRandFractf() {
00081 #if FL_MATH_RANDTYPE == FL_MATH_RAND_VFPU
00082 float tempOut;
00083 __asm__ volatile (
00084 "vrndf1.s S000\n"
00085 "vone.s S001\n"
00086 "vsub.s S000, S000, S001\n"
00087 "mfv %0, S000\n"
00088 : "=r"(tempOut) :);
00089 return tempOut;
00090 #else
00091 #if FL_MATH_RANDTYPE == FL_MATH_RAND_MERSENNETWISTER
00092 float tempOut = sceKernelUtilsMt19937UInt(&mathMtContext);
00093 return ((float)tempOut / (float)MAX_INT);
00094 #else
00095 #if FL_MATH_RANDOMSEED != 0
00096 int tempRand = ((rand() * FL_MATH_RANDOMSEED_FREQUENCY) / RAND_MAX);
00097 if(tempRand == 0) {
00098 pspTime tempTime;
00099 sceRtcGetCurrentClockLocalTime(&tempTime);
00100 srand(tempTime.microseconds * tempTime.seconds);
00101 }
00102 #endif
00103 return ((float)rand() / (float)RAND_MAX);
00104 #endif
00105 #endif
00106 }
00107
00108 unsigned int mathRandi(unsigned int inSize) {
00109 #if FL_MATH_RANDTYPE == FL_MATH_RAND_VFPU
00110 unsigned int tempOut;
00111 __asm__ volatile (
00112 "vrndi.s S000\n"
00113 "mfv %0, S000\n"
00114 : "=r"(tempOut) :);
00115 tempOut = tempOut % inSize;
00116 return tempOut;
00117 #else
00118 #if FL_MATH_RANDTYPE == FL_MATH_RAND_MERSENNETWISTER
00119 unsigned int tempOut = sceKernelUtilsMt19937UInt(&mathMtContext);
00120 return (tempOut % inSize);
00121 #else
00122 #if FL_MATH_RANDOMSEED != 0
00123 int tempRand = ((rand() * FL_MATH_RANDOMSEED_FREQUENCY) / RAND_MAX);
00124 if(tempRand == 0) {
00125 pspTime tempTime;
00126 sceRtcGetCurrentClockLocalTime(&tempTime);
00127 srand(tempTime.microseconds * tempTime.seconds);
00128 }
00129 #endif
00130 return (rand() % inSize);
00131 #endif
00132 #endif
00133 }
00134
00135 s64 mathRandl(s64 inSize) {
00136 long tempOut = ((s64)mathRandi((int)(inSize >> 32))) << 32;
00137 tempOut += (s64)mathRandi((int)(inSize & 0xFFFFFFFF));
00138 return tempOut;
00139 }
00140
00141 inline float mathFixDegRange(float inDeg) {
00142 while(inDeg >= 360)
00143 inDeg -= 360;
00144 while(inDeg < 0)
00145 inDeg += 360;
00146 return inDeg;
00147 }
00148
00149 inline float mathFixRadRange(float inRad) {
00150 while(inRad >= (MATH_PI * 2.0f))
00151 inRad -= (MATH_PI * 2.0f);
00152 while(inRad < 0)
00153 inRad += (MATH_PI * 2.0f);
00154 return inRad;
00155 }
00156
00157 float mathDegToRad(float inDeg) {
00158 inDeg = mathFixDegRange(inDeg);
00159 #if FL_MATH_VFPU != 0
00160 float tempOut;
00161 __asm__ volatile (
00162 "mtv %1, S000\n"
00163 "mtv %2, S001\n"
00164 "vcst.s S002, VFPU_PI\n"
00165 "vmul.s S000, S000, S002\n"
00166 "vrcp.s S001, S001\n"
00167 "vmul.s S000, S000, S001\n"
00168 "mfv %0, S000\n"
00169 : "=r"(tempOut) : "r"(inDeg), "r"(180.0f));
00170 return tempOut;
00171 #else
00172 return ((inDeg * MATH_PI) / 180.0f);
00173 #endif
00174 }
00175
00176 float mathRadToDeg(float inRad) {
00177 inRad = mathFixRadRange(inRad);
00178 #if FL_MATH_VFPU != 0
00179 float tempOut;
00180 __asm__ volatile (
00181 "mtv %1, S000\n"
00182 "mtv %2, S001\n"
00183 "vmul.s S000, S000, S001\n"
00184 "vcst.s S001, VFPU_PI\n"
00185 "vrcp.s S001, S001\n"
00186 "vmul.s S000, S000, S001\n"
00187 "mfv %0, S000\n"
00188 : "=r"(tempOut) : "r"(inRad), "r"(180.0f));
00189 return tempOut;
00190 #else
00191 return ((inRad * 180.0f) / MATH_PI);
00192 #endif
00193 }
00194
00195 float mathSinf(float inAngle) {
00196 #if FL_MATH_TRIGCACHE > 0
00197 float* tempCacheAns = mathTrigCacheCheckSin(inAngle);
00198 if(tempCacheAns)
00199 return *tempCacheAns;
00200 #endif
00201
00202 float tempOut;
00203 #if FL_MATH_VFPU != 0
00204 __asm__ volatile (
00205 "mtv %1, S000\n"
00206 "vcst.s S001, VFPU_2_PI\n"
00207 "vmul.s S000, S000, S001\n"
00208 "vsin.s S000, S000\n"
00209 "mfv %0, S000\n"
00210 : "=r"(tempOut) : "r"(inAngle));
00211 #else
00212 tempOut = sinf(inAngle);
00213 #endif
00214
00215 #if FL_MATH_TRIGCACHE > 0
00216 mathTrigCacheAddSin(inAngle, tempOut);
00217 #endif
00218
00219 return tempOut;
00220 }
00221
00222 float mathCosf(float inAngle) {
00223 #if FL_MATH_TRIGCACHE > 0
00224 float* tempCacheAns = mathTrigCacheCheckCos(inAngle);
00225 if(tempCacheAns)
00226 return *tempCacheAns;
00227 #endif
00228
00229 float tempOut;
00230 #if FL_MATH_VFPU != 0
00231 __asm__ volatile (
00232 "mtv %1, S000\n"
00233 "vcst.s S001, VFPU_2_PI\n"
00234 "vmul.s S000, S000, S001\n"
00235 "vcos.s S000, S000\n"
00236 "mfv %0, S000\n"
00237 : "=r"(tempOut) : "r"(inAngle));
00238 #else
00239 tempOut = cosf(inAngle);
00240 #endif
00241
00242 #if FL_MATH_TRIGCACHE > 0
00243 mathTrigCacheAddCos(inAngle, tempOut);
00244 #endif
00245
00246 return tempOut;
00247 }
00248
00249 float mathACosf(float inAngle) {
00250 #if FL_MATH_VFPU != 0
00251 float tempOut;
00252 __asm__ volatile (
00253 "mtv %1, S000\n"
00254 "vcst.s S001, VFPU_PI_2\n"
00255 "vasin.s S000, S000\n"
00256 "vocp.s S000, S000\n"
00257 "vmul.s S000, S000, S001\n"
00258 "mfv %0, S000\n"
00259 : "=r"(tempOut) : "r"(inAngle));
00260 return tempOut;
00261 #else
00262 return acosf(inAngle);
00263 #endif
00264 }
00265
00266 float mathATanf(float inAngle) {
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279 return atanf(inAngle);
00280
00281 }
00282
00283
00284 float mathExpnf(float inValue) {
00285 #if FL_MATH_VFPU != 0
00286 float tempOut;
00287 __asm__ volatile (
00288 "mtv %1, S000\n"
00289 "vcst.s S001, VFPU_LN2\n"
00290 "vrcp.s S001, S001\n"
00291 "vmul.s S000, S000, S001\n"
00292 "vexp2.s S000, S000\n"
00293 "mfv %0, S000\n"
00294 : "=r"(tempOut) : "r"(inValue));
00295 return tempOut;
00296 #else
00297 return expf(inValue);
00298 #endif
00299 }
00300
00301 float mathLnf(float inValue) {
00302 #if FL_MATH_VFPU != 0
00303 float tempOut;
00304 __asm__ volatile (
00305 "mtv %1, S000\n"
00306 "vcst.s S001, VFPU_LOG2E\n"
00307 "vrcp.s S001, S001\n"
00308 "vlog2.s S000, S000\n"
00309 "vmul.s S000, S000, S001\n"
00310 "mfv %0, S000\n"
00311 : "=r"(tempOut) : "r"(inValue));
00312 return tempOut;
00313 #else
00314 return logf(inValue);
00315 #endif
00316 }
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332 float mathPythag2f(float inX, float inY) {
00333 #if FL_MATH_VFPU != 0
00334 float tempOut;
00335 __asm__ volatile (
00336 "mtv %1, S000\n"
00337 "mtv %2, S001\n"
00338 "vmul.s S000, S000, S000\n"
00339 "vmul.s S001, S001, S001\n"
00340 "vadd.s S000, S000, S001\n"
00341 "mfv %0, S000\n"
00342 : "=r"(tempOut) : "r"(inX), "r"(inY));
00343 return sqrtf(tempOut);
00344 #else
00345 return sqrtf((inX * inX) + (inY * inY));
00346 #endif
00347 }
00348
00349 float mathPythag3f(float inX, float inY, float inZ) {
00350 #if FL_MATH_VFPU != 0
00351 float tempOut;
00352 __asm__ volatile (
00353 "mtv %1, S000\n"
00354 "mtv %2, S001\n"
00355 "mtv %3, S002\n"
00356 "vmul.s S000, S000, S000\n"
00357 "vmul.s S001, S001, S001\n"
00358 "vmul.s S002, S002, S002\n"
00359 "vadd.s S000, S000, S001\n"
00360 "vadd.s S000, S000, S002\n"
00361 "mfv %0, S000\n"
00362 : "=r"(tempOut) : "r"(inX), "r"(inY), "r"(inZ));
00363 return sqrtf(tempOut);
00364 #else
00365 return sqrtf((inX * inX) + (inY * inY) + (inZ * inZ));
00366 #endif
00367 }
00368
00369 float mathDotProdN(float* inValue0, float* inValue1, u8 inCount) {
00370 if(!inCount)
00371 return 0;
00372 float tempOut = 0.0f;
00373 int i;
00374 #if FL_MATH_VFPU != 0
00375 for(i = 0; i < inCount; i++) {
00376 __asm__ volatile (
00377 "mtv %1, S000\n"
00378 "mtv %2, S001\n"
00379 "mtv %3, S002\n"
00380 "vmul.s S000, S000, S001\n"
00381 "vadd.s S000, S000, S002\n"
00382 "mfv %0, S000\n"
00383 : "=r"(tempOut) : "r"(inValue0[i]), "r"(inValue1[i]), "r"(tempOut));
00384 }
00385 #else
00386 for(i = 0; i < inCount; i++)
00387 tempOut += (inValue0[i] * inValue1[i]);
00388 #endif
00389 return tempOut;
00390 }
00391
00392 float mathQuatLength(quat4f inQuat) {
00393 #if FL_MATH_VFPU != 0
00394 float tempOut = 0.0f;
00395 __asm__ volatile (
00396 "mtv %1, S000\n"
00397 "mtv %2, S001\n"
00398 "mtv %3, S002\n"
00399 "mtv %4, S003\n"
00400 "vmul.s S000, S000, S000\n"
00401 "vmul.s S001, S001, S001\n"
00402 "vmul.s S002, S002, S002\n"
00403 "vmul.s S003, S003, S003\n"
00404 "vadd.s S000, S000, S001\n"
00405 "vadd.s S001, S002, S003\n"
00406 "vadd.s S000, S000, S001\n"
00407 "mfv %0, S000\n"
00408 : "=r"(tempOut) : "r"(inQuat.x), "r"(inQuat.y), "r"(inQuat.z), "r"(inQuat.w));
00409 return sqrtf(tempOut);
00410 #else
00411 return sqrtf((inQuat.x * inQuat.x) + (inQuat.y * inQuat.z) + (inQuat.y * inQuat.z) + (inQuat.w * inQuat.w));
00412 #endif
00413 }
00414
00415 quat4f* mathQuatNormalize(quat4f* inQuat) {
00416 float tempLength = mathQuatLength(*inQuat);
00417 #if FL_MATH_VFPU != 0
00418 __asm__ volatile (
00419 "mtv %4, S000\n"
00420 "mtv %5, S001\n"
00421 "mtv %6, S002\n"
00422 "mtv %7, S003\n"
00423 "mtv %8, S010\n"
00424 "vrcp.s S010, S010\n"
00425 "vmul.s S000, S000, S010\n"
00426 "vmul.s S001, S001, S010\n"
00427 "vmul.s S002, S002, S010\n"
00428 "vmul.s S003, S003, S010\n"
00429 "mfv %0, S000\n"
00430 "mfv %1, S001\n"
00431 "mfv %2, S002\n"
00432 "mfv %3, S003\n"
00433 : "=r"(inQuat->x), "=r"(inQuat->y), "=r"(inQuat->z), "=r"(inQuat->w) : "r"(inQuat->x), "r"(inQuat->y), "r"(inQuat->z), "r"(inQuat->w), "r"(tempLength));
00434 return inQuat;
00435 #else
00436 inQuat->x /= tempLength;
00437 inQuat->y /= tempLength;
00438 inQuat->z /= tempLength;
00439 inQuat->w /= tempLength;
00440 return inQuat;
00441 #endif
00442 }
00443
00444 quat4f mathQuatNormal(quat4f inQuat) {
00445 quat4f tempOut = inQuat;
00446 mathQuatNormalize(&tempOut);
00447 return tempOut;
00448 }
00449
00450 quat4f* mathQuatNegate(quat4f* inQuat) {
00451 inQuat->x = -inQuat->x;
00452 inQuat->y = -inQuat->y;
00453 inQuat->z = -inQuat->z;
00454 inQuat->w = -inQuat->w;
00455 return inQuat;
00456 }
00457
00458 quat4f mathQuatNegation(quat4f inQuat) {
00459 quat4f tempOut = inQuat;
00460 mathQuatNegate(&tempOut);
00461 return tempOut;
00462 }
00463
00464 quat4f mathQuatMult(quat4f inQuat0, quat4f inQuat1) {
00465 quat4f tempOut;
00466 #if FL_MATH_VFPU != 0
00467 __asm__ volatile (
00468 "mtv %4, S000\n"
00469 "mtv %5, S001\n"
00470 "mtv %6, S002\n"
00471 "mtv %7, S003\n"
00472 "mtv %8, S010\n"
00473 "mtv %9, S011\n"
00474 "mtv %10, S012\n"
00475 "mtv %11, S013\n"
00476
00477 "vmul.s S020, S003, S010\n"
00478 "vmul.s S021, S000, S013\n"
00479 "vmul.s S022, S001, S012\n"
00480 "vmul.s S023, S002, S011\n"
00481 "vadd.s S020, S020, S021\n"
00482 "vadd.s S020, S020, S010\n"
00483 "vsub.s S020, S020, S023\n"
00484 "mfv %0, S020\n"
00485
00486 "vmul.s S020, S003, S011\n"
00487 "vmul.s S021, S000, S012\n"
00488 "vmul.s S010, S001, S013\n"
00489 "vmul.s S023, S002, S010\n"
00490 "vsub.s S020, S020, S021\n"
00491 "vadd.s S020, S020, S010\n"
00492 "vadd.s S020, S020, S023\n"
00493 "mfv %1, S020\n"
00494
00495 "vmul.s S020, S003, S012\n"
00496 "vmul.s S021, S000, S011\n"
00497 "vmul.s S010, S001, S013\n"
00498 "vmul.s S023, S002, S010\n"
00499 "vadd.s S020, S020, S021\n"
00500 "vsub.s S020, S020, S010\n"
00501 "vadd.s S020, S020, S023\n"
00502 "mfv %2, S020\n"
00503
00504 "vmul.s S020, S003, S013\n"
00505 "vmul.s S021, S000, S010\n"
00506 "vmul.s S010, S001, S011\n"
00507 "vmul.s S023, S002, S012\n"
00508 "vsub.s S020, S020, S021\n"
00509 "vsub.s S020, S020, S010\n"
00510 "vsub.s S020, S020, S023\n"
00511 "mfv %3, S020\n"
00512 : "=r"(tempOut.x), "=r"(tempOut.y), "=r"(tempOut.z), "=r"(tempOut.w) : "r"(inQuat0.x), "r"(inQuat0.y), "r"(inQuat0.z), "r"(inQuat0.w), "r"(inQuat1.x), "r"(inQuat1.y), "r"(inQuat1.z), "r"(inQuat1.w));
00513 return tempOut;
00514 #else
00515 tempOut.x = ((inQuat0.w * inQuat1.x) + (inQuat0.x * inQuat1.w) + (inQuat0.y * inQuat1.z) - (inQuat0.z * inQuat1.y));
00516 tempOut.y = ((inQuat0.w * inQuat1.y) - (inQuat0.x * inQuat1.z) + (inQuat0.y * inQuat1.w) + (inQuat0.z * inQuat1.x));
00517 tempOut.z = ((inQuat0.w * inQuat1.z) + (inQuat0.x * inQuat1.y) - (inQuat0.y * inQuat1.x) + (inQuat0.z * inQuat1.w));
00518 tempOut.w = ((inQuat0.w * inQuat1.w) - (inQuat0.x * inQuat1.x) - (inQuat0.y * inQuat1.y) - (inQuat0.z * inQuat1.z));
00519 return tempOut;
00520 #endif
00521 }
00522
00523 vect3f mathVect3fRotate(vect3f inVect, vect3f inAxis, float inAngle) {
00524 quat4f tempView = { inVect.x, inVect.y, inVect.z, 0 };
00525 quat4f tempRot;
00526
00527 #if FL_MATH_VFPU != 0
00528 __asm__ volatile (
00529 "mtv %4, S000\n"
00530 "mtv %5, S001\n"
00531 "mtv %6, S002\n"
00532 "mtv %7, S010\n"
00533 "mtv %8, S011\n"
00534
00535 "vcst.s S012, VFPU_2_PI\n"
00536 "vmul.s S012, S010, S012\n"
00537 "vsin.s S012, S012\n"
00538 "vmul.s S012, S012, S011\n"
00539
00540 "vmul.s S000, S000, S012\n"
00541 "vmul.s S001, S001, S012\n"
00542 "vmul.s S002, S002, S012\n"
00543
00544 "vcst.s S012, VFPU_2_PI\n"
00545 "vmul.s S012, S010, S012\n"
00546 "vcos.s S012, S012\n"
00547 "vmul.s S003, S012, S011\n"
00548
00549 "mfv %0, S000\n"
00550 "mfv %1, S001\n"
00551 "mfv %2, S002\n"
00552 "mfv %3, S003\n"
00553 : "=r"(tempRot.x), "=r"(tempRot.y), "=r"(tempRot.z), "=r"(tempRot.w) : "r"(inAxis.x), "r"(inAxis.y), "r"(inAxis.z), "r"(inAngle), "r"(0.5f));
00554 #else
00555 float tempSin = sinf(inAngle * 0.5f);
00556 tempRot.x = inAxis.x * tempSin;
00557 tempRot.y = inAxis.y * tempSin;
00558 tempRot.z = inAxis.z * tempSin;
00559 tempRot.w = cosf(inAngle * 0.5f);
00560 #endif
00561
00562 quat4f tempResult = mathQuatMult(tempRot, tempView);
00563 mathQuatNegate(&tempRot);
00564 tempResult = mathQuatMult(tempResult, tempRot);
00565
00566 return (vect3f){ tempResult.x, tempResult.y, tempResult.z };
00567 }
00568
00569 vect3f mathVect3fCrossProd(vect3f inVect0, vect3f inVect1) {
00570 vect3f tempOut;
00571 #if FL_MATH_VFPU != 0
00572 __asm__ volatile (
00573 "mtv %3, S000\n"
00574 "mtv %4, S001\n"
00575 "mtv %5, S002\n"
00576
00577 "mtv %6, S010\n"
00578 "mtv %7, S011\n"
00579 "mtv %8, S012\n"
00580
00581 "vmul.s S020, S001, S012\n"
00582 "vmul.s S021, S002, S011\n"
00583 "vsub.s S020, S020, S021\n"
00584 "mfv %0, S020\n"
00585
00586 "vmul.s S020, S002, S010\n"
00587 "vmul.s S021, S000, S012\n"
00588 "vsub.s S020, S020, S021\n"
00589 "mfv %1, S020\n"
00590
00591 "vmul.s S020, S000, S011\n"
00592 "vmul.s S021, S001, S010\n"
00593 "vsub.s S020, S020, S021\n"
00594 "mfv %2, S020\n"
00595 : "=r"(tempOut.x), "=r"(tempOut.y), "=r"(tempOut.z) : "r"(inVect0.x), "r"(inVect0.y), "r"(inVect0.z), "r"(inVect1.x), "r"(inVect1.y), "r"(inVect1.z));
00596 #else
00597 tempOut.x = ((inVect0.y * inVect1.z) - (inVect0.z * inVect1.y));
00598 tempOut.y = ((inVect0.z * inVect1.x) - (inVect0.x * inVect1.z));
00599 tempOut.z = ((inVect0.x * inVect1.y) - (inVect0.y * inVect1.x));
00600 #endif
00601 return tempOut;
00602 }
00603
00604 float mathVect3fLength(vect3f inVect) {
00605 #if FL_MATH_VFPU != 0
00606 float tempOut = 0.0f;
00607 __asm__ volatile (
00608 "mtv %1, S000\n"
00609 "mtv %2, S001\n"
00610 "mtv %3, S002\n"
00611 "vmul.s S000, S000, S000\n"
00612 "vmul.s S001, S001, S001\n"
00613 "vmul.s S002, S002, S002\n"
00614 "vadd.s S000, S000, S001\n"
00615 "vadd.s S000, S000, S002\n"
00616 "mfv %0, S000\n"
00617 : "=r"(tempOut) : "r"(inVect.x), "r"(inVect.y), "r"(inVect.z));
00618 return sqrtf(tempOut);
00619 #else
00620 return sqrtf((inVect.x * inVect.x) + (inVect.y * inVect.z) + (inVect.y * inVect.z));
00621 #endif
00622 }
00623
00624 vect3f* mathVect3fNormalize(vect3f* inVect) {
00625 float tempLength = mathVect3fLength(*inVect);
00626 #if FL_MATH_VFPU != 0
00627 __asm__ volatile (
00628 "mtv %3, S000\n"
00629 "mtv %4, S001\n"
00630 "mtv %5, S002\n"
00631 "mtv %6, S003\n"
00632 "vrcp.s S003, S003\n"
00633 "vmul.s S000, S000, S003\n"
00634 "vmul.s S001, S001, S003\n"
00635 "vmul.s S002, S002, S003\n"
00636 "mfv %0, S000\n"
00637 "mfv %1, S001\n"
00638 "mfv %2, S002\n"
00639 : "=r"(inVect->x), "=r"(inVect->y), "=r"(inVect->z) : "r"(inVect->x), "r"(inVect->y), "r"(inVect->z), "r"(tempLength));
00640 return inVect;
00641 #else
00642 inVect->x /= tempLength;
00643 inVect->y /= tempLength;
00644 inVect->z /= tempLength;
00645 return inVect;
00646 #endif
00647 }
00648
00649 vect3f mathVect3fNormal(vect3f inVect) {
00650 vect3f tempOut = inVect;
00651 mathVect3fNormalize(&tempOut);
00652 return tempOut;
00653 }
00654
00655 #if FL_MATH_TRIGCACHE > 0
00656 void mathTrigCacheInit() {
00657 if(mathTrigCacheInitialized)
00658 return;
00659 mathTrigCacheSin = (MathTrigCache*)memCalloc(sizeof(MathTrigCache), FL_MATH_TRIGCACHE);
00660 mathTrigCacheInitialized = true;
00661 }
00662
00663 float* mathTrigCacheCheckSin(float inAngle) {
00664 if(!mathTrigCacheInitialized)
00665 return NULL;
00666 inAngle = mathFixRadRange(inAngle);
00667
00668 mathTrigCacheCalls++;
00669
00670 u32 i;
00671 for(i = 0; i < FL_MATH_TRIGCACHE; i++) {
00672 if(pos(inAngle - mathTrigCacheSin[i].mtgIn) < 0.001f) {
00673 mathTrigCacheSin[i].mtgLastUsed = mathTrigCacheCalls;
00674 return &mathTrigCacheSin[i].mtgOut;
00675 }
00676 }
00677
00678 return NULL;
00679 }
00680
00681 void mathTrigCacheAddSin(float inAngle, float inAnswer) {
00682 if(!mathTrigCacheInitialized)
00683 return;
00684
00685 mathTrigCacheCalls++;
00686
00687 u32 tempDrop = 0;
00688 u64 tempDropAge = 0;
00689 u64 tempAge;
00690
00691 u32 i;
00692 for(i = 0; i < FL_MATH_TRIGCACHE; i++) {
00693 tempAge = (mathTrigCacheCalls - mathTrigCacheSin[i].mtgLastUsed);
00694 if(!mathTrigCacheSin[i].mtgLastUsed) {
00695 tempDrop = i;
00696 break;
00697 }
00698 if((tempAge > tempDropAge) || !i) {
00699 tempDrop = i;
00700 tempDropAge = tempAge;
00701 }
00702 }
00703
00704 mathTrigCacheSin[tempDrop].mtgLastUsed = mathTrigCacheCalls;
00705 mathTrigCacheSin[tempDrop].mtgIn = mathFixRadRange(inAngle);
00706 mathTrigCacheSin[tempDrop].mtgOut = inAnswer;
00707 }
00708
00709 float* mathTrigCacheCheckCos(float inAngle) {
00710 return mathTrigCacheCheckSin(inAngle + (MATH_PI / 2.0f));
00711 }
00712
00713 void mathTrigCacheAddCos(float inAngle, float inAnswer) {
00714 return mathTrigCacheAddSin((inAngle + (MATH_PI / 2.0f)), inAnswer);
00715 }
00716 #endif
00717
00718 #endif