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

Generated on Wed Sep 5 19:04:00 2007 for funcLib by  doxygen 1.5.1