- Main Page
- Classes
- Files

- File List
- File Members

# flMath.c

Go to the documentation of this file.

    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      /*#if FL_MATH_VFPU != 0
    00268      float tempOut;
    00269      __asm__ volatile (
    00270           "mtv     %1, S000\n"
    00271           "vcst.s  S001, VFPU_PI_2\n"
    00272           "vasin.s S000, S000\n"
    00273           "vocp.s  S000, S000\n"
    00274           "vmul.s  S000, S000, S001\n"
    00275           "mfv     %0, S000\n"
    00276           : "=r"(tempOut) : "r"(inAngle));
    00277      return tempOut;
    00278      #else*/
    00279      return atanf(inAngle);
    00280      //#endif
    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 /*float mathSqrtf(float inValue) {
    00319      #if FL_MATH_VFPU != 0
    00320      float tempOut;
    00321      __asm__ volatile (
    00322           "mtv     %1, S000\n"
    00323           "vsqrt.s  S000, S000\n"
    00324           "mfv     %0, S000\n"
    00325           : "=r"(tempOut) : "r"(inValue));
    00326      return tempOut;
    00327      #else
    00328      return sqrtf(inValue);
    00329      #endif
    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