mMath_C.cpp

Public Functions

default_matF_x_matF_C(const F32 * a, const F32 * b, F32 * mresult)

m_catmullrom_C(F32 t, F32 p0, F32 p1, F32 p2, F32 p3)

m_fmod_ASM(F32 val, F32 modulo)

m_fmodD_ASM(F64 val, F64 modulo)

m_matF_affineInverse_C(F32 * m)

m_matF_determinant_C(const F32 * m)

m_matF_identity_C(F32 * m)

m_matF_inverse_C(F32 * m)

m_matF_invert_to_C(const F32 * m, F32 * d)

m_matF_normalize_C(F32 * m)

m_matF_scale_C(F32 * m, const F32 * p)

m_matF_set_euler_C(const F32 * e, F32 * result)

m_matF_set_euler_point_C(const F32 * e, const F32 * p, F32 * result)

m_matF_transpose_C(F32 * m)

m_matF_x_box3F_C(const F32 * m, F32 * min, F32 * max)

m_matF_x_point4F_C(const F32 * m, const F32 * p, F32 * presult)

m_matF_x_scale_x_planeF_C(const F32 * m, const F32 * s, const F32 * p, F32 * presult)

m_mulDivS32_ASM(S32 a, S32 b, S32 c)

m_mulDivS32_C(S32 a, S32 b, S32 c)

m_mulDivU32_ASM(U32 a, U32 b, U32 c)

m_mulDivU32_C(S32 a, S32 b, U32 c)

m_point2D_normalize_C(F64 * p)

m_point2D_normalize_f_C(F64 * p, F64 val)

m_point2F_normalize_C(F32 * p)

m_point2F_normalize_f_C(F32 * p, F32 val)

m_point3D_interpolate_C(const F64 * from, const F64 * to, F64 factor, F64 * result)

m_point3D_normalize_C(F64 * p)

m_point3D_normalize_f_C(F64 * p, F64 val)

m_point3F_bulk_dot_C(const F32 * refVector, const F32 * dotPoints, const U32 numPoints, const U32 pointStride, F32 * output)

m_point3F_bulk_dot_indexed_C(const F32 * refVector, const F32 * dotPoints, const U32 numPoints, const U32 pointStride, const U32 * pointIndices, F32 * output)

m_point3F_interpolate_C(const F32 * from, const F32 * to, F32 factor, F32 * result)

m_point3F_normalize_C(F32 * p)

m_point3F_normalize_f_C(F32 * p, F32 val)

m_quatF_set_matF_C(F32 x, F32 y, F32 z, F32 w, F32 * m)

m_sincos_ASM(F32 angle, F32 * s, F32 * c)

m_sincos_C(F32 angle, F32 * s, F32 * c)

m_sincosD_ASM(F64 angle, F64 * s, F64 * c)

m_sincosD_C(F64 angle, F64 * s, F64 * c)

mInstallLibrary_C()

swap(F32 & a, F32 & b)

  1
  2//-----------------------------------------------------------------------------
  3// Copyright (c) 2012 GarageGames, LLC
  4//
  5// Permission is hereby granted, free of charge, to any person obtaining a copy
  6// of this software and associated documentation files (the "Software"), to
  7// deal in the Software without restriction, including without limitation the
  8// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
  9// sell copies of the Software, and to permit persons to whom the Software is
 10// furnished to do so, subject to the following conditions:
 11//
 12// The above copyright notice and this permission notice shall be included in
 13// all copies or substantial portions of the Software.
 14//
 15// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 16// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 17// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 18// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 19// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 20// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 21// IN THE SOFTWARE.
 22//-----------------------------------------------------------------------------
 23
 24#include "platform/platform.h"
 25#include "math/mMath.h"
 26#include "math/util/frustum.h"
 27#include <math.h>    // Caution!!! Possible platform specific include
 28
 29
 30//------------------------------------------------------------------------------
 31// C version of Math Library
 32
 33// asm externals
 34extern "C" {
 35
 36   S32 m_mulDivS32_ASM( S32 a, S32 b, S32 c );
 37   U32 m_mulDivU32_ASM( U32 a, U32 b, U32 c );
 38   F32 m_fmod_ASM(F32 val, F32 modulo);
 39   F32 m_fmodD_ASM(F64 val, F64 modulo);
 40   void m_sincos_ASM( F32 angle, F32 *s, F32 *c );
 41   void m_sincosD_ASM( F64 angle, F64 *s, F64 *c );
 42
 43}
 44//--------------------------------------
 45
 46static S32 m_mulDivS32_C(S32 a, S32 b, S32 c)
 47{
 48   // S64/U64 support in most 32-bit compilers generate
 49   // horrible code, the C version are here just for porting
 50   // assembly implementation is recommended
 51   return (S32) ((S64)a*(S64)b) / (S64)c;
 52}
 53
 54static U32 m_mulDivU32_C(S32 a, S32 b, U32 c)
 55{
 56   return (U32) ((S64)a*(S64)b) / (U64)c;
 57}
 58
 59
 60//--------------------------------------
 61static F32 m_catmullrom_C(F32 t, F32 p0, F32 p1, F32 p2, F32 p3)
 62{
 63   return 0.5f * ((3.0f*p1 - 3.0f*p2 + p3 - p0)*t*t*t
 64                  +  (2.0f*p0 - 5.0f*p1 + 4.0f*p2 - p3)*t*t
 65                  +  (p2-p0)*t
 66                  +  2.0f*p1);
 67}
 68
 69//--------------------------------------
 70static void m_sincos_C( F32 angle, F32 *s, F32 *c )
 71{
 72   *s = mSin( angle );
 73   *c = mCos( angle );
 74}
 75
 76static void m_sincosD_C( F64 angle, F64 *s, F64 *c )
 77{
 78   *s = mSin( angle );
 79   *c = mCos( angle );
 80}
 81
 82//--------------------------------------
 83static void m_point2F_normalize_C(F32 *p)
 84{
 85   F32 factor = 1.0f / mSqrt(p[0]*p[0] + p[1]*p[1] );
 86   p[0] *= factor;
 87   p[1] *= factor;
 88}
 89
 90//--------------------------------------
 91static void m_point2F_normalize_f_C(F32 *p, F32 val)
 92{
 93   F32 factor = val / mSqrt(p[0]*p[0] + p[1]*p[1] );
 94   p[0] *= factor;
 95   p[1] *= factor;
 96}
 97
 98//--------------------------------------
 99static void m_point2D_normalize_C(F64 *p)
100{
101   F64 factor = 1.0f / mSqrtD(p[0]*p[0] + p[1]*p[1] );
102   p[0] *= factor;
103   p[1] *= factor;
104}
105
106//--------------------------------------
107static void m_point2D_normalize_f_C(F64 *p, F64 val)
108{
109   F64 factor = val / mSqrtD(p[0]*p[0] + p[1]*p[1] );
110   p[0] *= factor;
111   p[1] *= factor;
112}
113
114//--------------------------------------
115static void m_point3D_normalize_f_C(F64 *p, F64 val)
116{
117   F64 factor = val / mSqrtD(p[0]*p[0] + p[1]*p[1] + p[2]*p[2]);
118   p[0] *= factor;
119   p[1] *= factor;
120   p[2] *= factor;
121}
122
123//--------------------------------------
124static void m_point3F_normalize_C(F32 *p)
125{
126   F32 squared = p[0]*p[0] + p[1]*p[1] + p[2]*p[2];
127   // This can happen in Container::castRay -> ForceFieldBare::castRay
128   //AssertFatal(squared != 0.0, "Error, zero length vector normalized!");
129   if (squared != 0.0f) {
130      F32 factor = 1.0f / mSqrt(squared);
131      p[0] *= factor;
132      p[1] *= factor;
133      p[2] *= factor;
134   } else {
135      p[0] = 0.0f;
136      p[1] = 0.0f;
137      p[2] = 1.0f;
138   }
139}
140
141//--------------------------------------
142static void m_point3F_normalize_f_C(F32 *p, F32 val)
143{
144   F32 factor = val / mSqrt(p[0]*p[0] + p[1]*p[1] + p[2]*p[2] );
145   p[0] *= factor;
146   p[1] *= factor;
147   p[2] *= factor;
148}
149
150//--------------------------------------
151static void m_point3F_interpolate_C(const F32 *from, const F32 *to, F32 factor, F32 *result )
152{
153#ifdef TORQUE_COMPILER_GCC
154// remove possibility of aliases
155   const F32 inverse = 1.0f - factor;
156   const F32   from0 = from[0], from1 = from[1], from2 = from[2];
157   const F32   to0 = to[0], to1 = to[1], to2 = to[2];
158   
159   result[0] = from0 * inverse + to0 * factor;
160   result[1] = from1 * inverse + to1 * factor;
161   result[2] = from2 * inverse + to2 * factor;
162#else
163   F32 inverse = 1.0f - factor;
164   result[0] = from[0] * inverse + to[0] * factor;
165   result[1] = from[1] * inverse + to[1] * factor;
166   result[2] = from[2] * inverse + to[2] * factor;
167#endif
168}
169
170//--------------------------------------
171static void m_point3D_normalize_C(F64 *p)
172{
173   F64 factor = 1.0f / mSqrtD(p[0]*p[0] + p[1]*p[1] + p[2]*p[2] );
174   p[0] *= factor;
175   p[1] *= factor;
176   p[2] *= factor;
177}
178
179
180//--------------------------------------
181static void m_point3D_interpolate_C(const F64 *from, const F64 *to, F64 factor, F64 *result )
182{
183#ifdef TORQUE_COMPILER_GCC
184// remove possibility of aliases
185   const F64 inverse = 1.0f - factor;
186   const F64   from0 = from[0], from1 = from[1], from2 = from[2];
187   const F64   to0 = to[0], to1 = to[1], to2 = to[2];
188   
189   result[0] = from0 * inverse + to0 * factor;
190   result[1] = from1 * inverse + to1 * factor;
191   result[2] = from2 * inverse + to2 * factor;
192#else
193   F64 inverse = 1.0f - factor;
194   result[0] = from[0] * inverse + to[0] * factor;
195   result[1] = from[1] * inverse + to[1] * factor;
196   result[2] = from[2] * inverse + to[2] * factor;
197#endif
198}
199
200
201static void m_quatF_set_matF_C( F32 x, F32 y, F32 z, F32 w, F32* m )
202{
203#define qidx(r,c) (r*4 + c)
204      F32 xs = x * 2.0f;
205      F32 ys = y * 2.0f;
206      F32 zs = z * 2.0f;
207      F32 wx = w * xs;
208      F32 wy = w * ys;
209      F32 wz = w * zs;
210      F32 xx = x * xs;
211      F32 xy = x * ys;
212      F32 xz = x * zs;
213      F32 yy = y * ys;
214      F32 yz = y * zs;
215      F32 zz = z * zs;
216      m[qidx(0,0)] = 1.0f - (yy + zz);
217      m[qidx(1,0)] = xy - wz;
218      m[qidx(2,0)] = xz + wy;
219      m[qidx(3,0)] = 0.0f;
220      m[qidx(0,1)] = xy + wz;
221      m[qidx(1,1)] = 1.0f - (xx + zz);
222      m[qidx(2,1)] = yz - wx;
223      m[qidx(3,1)] = 0.0f;
224      m[qidx(0,2)] = xz - wy;
225      m[qidx(1,2)] = yz + wx;
226      m[qidx(2,2)] = 1.0f - (xx + yy);
227      m[qidx(3,2)] = 0.0f;
228
229      m[qidx(0,3)] = 0.0f;
230      m[qidx(1,3)] = 0.0f;
231      m[qidx(2,3)] = 0.0f;
232      m[qidx(3,3)] = 1.0f;
233#undef qidx
234}
235
236
237//--------------------------------------
238static void m_matF_set_euler_C(const F32 *e, F32 *result)
239{
240   enum {
241      AXIS_X   = (1<<0),
242      AXIS_Y   = (1<<1),
243      AXIS_Z   = (1<<2)
244   };
245
246   U32 axis = 0;
247   if (e[0] != 0.0f) axis |= AXIS_X;
248   if (e[1] != 0.0f) axis |= AXIS_Y;
249   if (e[2] != 0.0f) axis |= AXIS_Z;
250
251   switch (axis)
252   {
253      case 0:
254         m_matF_identity(result);
255         break;
256
257      case AXIS_X:
258      {
259         F32 cx,sx;
260         mSinCos( e[0], sx, cx );
261
262         result[0] = 1.0f;
263         result[1] = 0.0f;
264         result[2] = 0.0f;
265         result[3] = 0.0f;
266
267         result[4] = 0.0f;
268         result[5] = cx;
269         result[6] = sx;
270         result[7] = 0.0f;
271
272         result[8] = 0.0f;
273         result[9] = -sx;
274         result[10]= cx;
275         result[11]= 0.0f;
276
277         result[12]= 0.0f;
278         result[13]= 0.0f;
279         result[14]= 0.0f;
280         result[15]= 1.0f;
281         break;
282      }
283
284      case AXIS_Y:
285      {
286         F32 cy,sy;
287         mSinCos( e[1], sy, cy );
288
289         result[0] = cy;
290         result[1] = 0.0f;
291         result[2] = -sy;
292         result[3] = 0.0f;
293
294         result[4] = 0.0f;
295         result[5] = 1.0f;
296         result[6] = 0.0f;
297         result[7] = 0.0f;
298
299         result[8] = sy;
300         result[9] = 0.0f;
301         result[10]= cy;
302         result[11]= 0.0f;
303
304         result[12]= 0.0f;
305         result[13]= 0.0f;
306         result[14]= 0.0f;
307         result[15]= 1.0f;
308         break;
309      }
310
311      case AXIS_Z:
312      {
313         // the matrix looks like this:
314         //  r1 - (r4 * sin(x))     r2 + (r3 * sin(x))   -cos(x) * sin(y)
315         //  -cos(x) * sin(z)       cos(x) * cos(z)      sin(x)
316         //  r3 + (r2 * sin(x))     r4 - (r1 * sin(x))   cos(x) * cos(y)
317         //
318         // where:
319         //  r1 = cos(y) * cos(z)
320         //  r2 = cos(y) * sin(z)
321         //  r3 = sin(y) * cos(z)
322         //  r4 = sin(y) * sin(z)
323         F32 cz,sz;
324         mSinCos( e[2], sz, cz );
325
326         result[0] = cz;
327         result[1] = sz;
328         result[2] = 0.0f;
329         result[3] = 0.0f;
330
331         result[4] = -sz;
332         result[5] = cz;
333         result[6] = 0.0f;
334         result[7] = 0.0f;
335
336         result[8] = 0.0f;
337         result[9] = 0.0f;
338         result[10]= 1.0f;
339         result[11]= 0.0f;
340
341         result[12]= 0.0f;
342         result[13]= 0.0f;
343         result[14]= 0.0f;
344         result[15]= 1.0f;
345         break;
346      }
347
348      default:
349         // the matrix looks like this:
350         //  r1 - (r4 * sin(x))     r2 + (r3 * sin(x))   -cos(x) * sin(y)
351         //  -cos(x) * sin(z)       cos(x) * cos(z)      sin(x)
352         //  r3 + (r2 * sin(x))     r4 - (r1 * sin(x))   cos(x) * cos(y)
353         //
354         // where:
355         //  r1 = cos(y) * cos(z)
356         //  r2 = cos(y) * sin(z)
357         //  r3 = sin(y) * cos(z)
358         //  r4 = sin(y) * sin(z)
359         F32 cx,sx;
360         mSinCos( e[0], sx, cx );
361         F32 cy,sy;
362         mSinCos( e[1], sy, cy );
363         F32 cz,sz;
364         mSinCos( e[2], sz, cz );
365         F32 r1 = cy * cz;
366         F32 r2 = cy * sz;
367         F32 r3 = sy * cz;
368         F32 r4 = sy * sz;
369
370         result[0] = r1 - (r4 * sx);
371         result[1] = r2 + (r3 * sx);
372         result[2] = -cx * sy;
373         result[3] = 0.0f;
374
375         result[4] = -cx * sz;
376         result[5] = cx * cz;
377         result[6] = sx;
378         result[7] = 0.0f;
379
380         result[8] = r3 + (r2 * sx);
381         result[9] = r4 - (r1 * sx);
382         result[10]= cx * cy;
383         result[11]= 0.0f;
384
385         result[12]= 0.0f;
386         result[13]= 0.0f;
387         result[14]= 0.0f;
388         result[15]= 1.0f;
389         break;
390   }
391}
392
393
394//--------------------------------------
395static void m_matF_set_euler_point_C(const F32 *e, const F32 *p, F32 *result)
396{
397   m_matF_set_euler(e, result);
398   result[3] = p[0];
399   result[7] = p[1];
400   result[11]= p[2];
401}
402
403//--------------------------------------
404static void m_matF_identity_C(F32 *m)
405{
406   *m++ = 1.0f;
407   *m++ = 0.0f;
408   *m++ = 0.0f;
409   *m++ = 0.0f;
410
411   *m++ = 0.0f;
412   *m++ = 1.0f;
413   *m++ = 0.0f;
414   *m++ = 0.0f;
415
416   *m++ = 0.0f;
417   *m++ = 0.0f;
418   *m++ = 1.0f;
419   *m++ = 0.0f;
420
421   *m++ = 0.0f;
422   *m++ = 0.0f;
423   *m++ = 0.0f;
424   *m   = 1.0f;
425}
426
427#if 0
428// Compile this out till we hook it up.  It's a more efficient matrix
429// inverse than what we have (it uses intermediate results of determinant
430// to same about 1/4 of the operations.
431static void affineInvertTo(const F32 * out)
432{
433#define idx(r,c) (r*4 + c)
434   idx(1,2)];
435   idx(1,0)];
436   idx(1,1)];
437
438   idx(0,2)] * d3);
439
440   idx(0,0)] * invDet; 
441   idx(0,1)] * invDet; 
442   idx(0,2)] * invDet;
443
444   F32 * result = out;
445   *out++ = d1 * invDet;
446   *out++ = m02 * m[idx(2,2)];
447   *out++ = m01 * m[idx(1,1)];
448   *out++ = 0.0f;
449
450   *out++ = d2 * invDet;
451   *out++ = m00 * m[idx(2,0)];
452   *out++ = m02 * m[idx(1,2)];
453   *out++ = 0.0f;
454
455   *out++ = d3 * invDet;
456   *out++ = m01 * m[idx(2,1)];
457   *out++ = m00 * m[idx(1,0)];
458   *out++ = 0.0f;
459
460   *out++ = -result[idx(2,3)];
461   *out++ = -result[idx(2,3)];
462   *out++ = -result[idx(2,3)];
463   *out++ = 1.0f;
464#undef idx
465}
466#endif
467
468//--------------------------------------
469static void m_matF_inverse_C(F32 *m)
470{
471   // using Cramers Rule find the Inverse
472   // Minv = (1/det(M)) * adjoint(M)
473   F32 det = m_matF_determinant( m );
474   AssertFatal( det != 0.0f, "MatrixF::inverse: non-singular matrix, no inverse.");
475
476   F32 invDet = 1.0f/det;
477   F32 temp[16];
478
479   temp[0] = (m[5] * m[10]- m[6] * m[9]) * invDet;
480   temp[1] = (m[9] * m[2] - m[10]* m[1]) * invDet;
481   temp[2] = (m[1] * m[6] - m[2] * m[5]) * invDet;
482
483   temp[4] = (m[6] * m[8] - m[4] * m[10])* invDet;
484   temp[5] = (m[10]* m[0] - m[8] * m[2]) * invDet;
485   temp[6] = (m[2] * m[4] - m[0] * m[6]) * invDet;
486
487   temp[8] = (m[4] * m[9] - m[5] * m[8]) * invDet;
488   temp[9] = (m[8] * m[1] - m[9] * m[0]) * invDet;
489   temp[10]= (m[0] * m[5] - m[1] * m[4]) * invDet;
490
491   m[0] = temp[0];
492   m[1] = temp[1];
493   m[2] = temp[2];
494
495   m[4] = temp[4];
496   m[5] = temp[5];
497   m[6] = temp[6];
498
499   m[8] = temp[8];
500   m[9] = temp[9];
501   m[10] = temp[10];
502
503   // invert the translation
504   temp[0] = -m[3];
505   temp[1] = -m[7];
506   temp[2] = -m[11];
507   m_matF_x_vectorF(m, temp, &temp[4]);
508   m[3] = temp[4];
509   m[7] = temp[5];
510   m[11]= temp[6];
511}
512
513static void m_matF_invert_to_C(const F32 *m, F32 *d)
514{
515   // using Cramers Rule find the Inverse
516   // Minv = (1/det(M)) * adjoint(M)
517   F32 det = m_matF_determinant( m );
518   AssertFatal( det != 0.0f, "MatrixF::inverse: non-singular matrix, no inverse.");
519
520   F32 invDet = 1.0f/det;
521
522   d[0] = (m[5] * m[10]- m[6] * m[9]) * invDet;
523   d[1] = (m[9] * m[2] - m[10]* m[1]) * invDet;
524   d[2] = (m[1] * m[6] - m[2] * m[5]) * invDet;
525
526   d[4] = (m[6] * m[8] - m[4] * m[10])* invDet;
527   d[5] = (m[10]* m[0] - m[8] * m[2]) * invDet;
528   d[6] = (m[2] * m[4] - m[0] * m[6]) * invDet;
529
530   d[8] = (m[4] * m[9] - m[5] * m[8]) * invDet;
531   d[9] = (m[8] * m[1] - m[9] * m[0]) * invDet;
532   d[10]= (m[0] * m[5] - m[1] * m[4]) * invDet;
533
534   // invert the translation
535   F32 temp[6];
536   temp[0] = -m[3];
537   temp[1] = -m[7];
538   temp[2] = -m[11];
539   m_matF_x_vectorF(d, temp, &temp[3]);
540   d[3] = temp[3];
541   d[7] = temp[4];
542   d[11]= temp[5];
543   d[ 12 ] = m[ 12 ];
544   d[ 13 ] = m[ 13 ];
545   d[ 14 ] = m[ 14 ];
546   d[ 15 ] = m[ 15 ];
547}
548
549//--------------------------------------
550static void m_matF_affineInverse_C(F32 *m)
551{
552   // Matrix class checks to make sure this is an affine transform before calling
553   //  this function, so we can proceed assuming it is...
554   F32 temp[16];
555   dMemcpy(temp, m, 16 * sizeof(F32));
556
557   // Transpose rotation
558   m[1] = temp[4];
559   m[4] = temp[1];
560   m[2] = temp[8];
561   m[8] = temp[2];
562   m[6] = temp[9];
563   m[9] = temp[6];
564
565   m[3]  = -(temp[0]*temp[3] + temp[4]*temp[7] + temp[8]*temp[11]);
566   m[7]  = -(temp[1]*temp[3] + temp[5]*temp[7] + temp[9]*temp[11]);
567   m[11] = -(temp[2]*temp[3] + temp[6]*temp[7] + temp[10]*temp[11]);
568}
569
570inline void swap(F32 &a, F32 &b)
571{
572   F32 temp = a;
573   a = b;
574   b = temp;
575}
576
577//--------------------------------------
578static void m_matF_transpose_C(F32 *m)
579{
580   swap(m[1], m[4]);
581   swap(m[2], m[8]);
582   swap(m[3], m[12]);
583   swap(m[6], m[9]);
584   swap(m[7], m[13]);
585   swap(m[11],m[14]);
586}
587
588//--------------------------------------
589static void m_matF_scale_C(F32 *m,const F32 *p)
590{
591   // Note, doesn't allow scaling w...
592
593   m[0]  *= p[0];  m[1]  *= p[1];  m[2]  *= p[2];
594   m[4]  *= p[0];  m[5]  *= p[1];  m[6]  *= p[2];
595   m[8]  *= p[0];  m[9]  *= p[1];  m[10] *= p[2];
596   m[12] *= p[0];  m[13] *= p[1];  m[14] *= p[2];
597}
598
599//--------------------------------------
600static void m_matF_normalize_C(F32 *m)
601{
602   F32 col0[3], col1[3], col2[3];
603   // extract columns 0 and 1
604   col0[0] = m[0];
605   col0[1] = m[4];
606   col0[2] = m[8];
607
608   col1[0] = m[1];
609   col1[1] = m[5];
610   col1[2] = m[9];
611
612   // assure their relationships to one another
613   mCross(*(Point3F*)col0, *(Point3F*)col1, (Point3F*)col2);
614   mCross(*(Point3F*)col2, *(Point3F*)col0, (Point3F*)col1);
615
616   // assure their length is 1.0f
617   m_point3F_normalize( col0 );
618   m_point3F_normalize( col1 );
619   m_point3F_normalize( col2 );
620
621   // store the normalized columns
622   m[0] = col0[0];
623   m[4] = col0[1];
624   m[8] = col0[2];
625
626   m[1] = col1[0];
627   m[5] = col1[1];
628   m[9] = col1[2];
629
630   m[2] = col2[0];
631   m[6] = col2[1];
632   m[10]= col2[2];
633}
634
635
636//--------------------------------------
637static F32 m_matF_determinant_C(const F32 *m)
638{
639   return m[0] * (m[5] * m[10] - m[6] * m[9])  +
640      m[4] * (m[2] * m[9]  - m[1] * m[10]) +
641      m[8] * (m[1] * m[6]  - m[2] * m[5])  ;
642}
643
644
645//--------------------------------------
646// Removed static in order to write benchmarking code (that compares against
647// specialized SSE/AMD versions) elsewhere.
648void default_matF_x_matF_C(const F32 *a, const F32 *b, F32 *mresult)
649{
650   mresult[0] = a[0]*b[0] + a[1]*b[4] + a[2]*b[8]  + a[3]*b[12];
651   mresult[1] = a[0]*b[1] + a[1]*b[5] + a[2]*b[9]  + a[3]*b[13];
652   mresult[2] = a[0]*b[2] + a[1]*b[6] + a[2]*b[10] + a[3]*b[14];
653   mresult[3] = a[0]*b[3] + a[1]*b[7] + a[2]*b[11] + a[3]*b[15];
654
655   mresult[4] = a[4]*b[0] + a[5]*b[4] + a[6]*b[8]  + a[7]*b[12];
656   mresult[5] = a[4]*b[1] + a[5]*b[5] + a[6]*b[9]  + a[7]*b[13];
657   mresult[6] = a[4]*b[2] + a[5]*b[6] + a[6]*b[10] + a[7]*b[14];
658   mresult[7] = a[4]*b[3] + a[5]*b[7] + a[6]*b[11] + a[7]*b[15];
659
660   mresult[8] = a[8]*b[0] + a[9]*b[4] + a[10]*b[8] + a[11]*b[12];
661   mresult[9] = a[8]*b[1] + a[9]*b[5] + a[10]*b[9] + a[11]*b[13];
662   mresult[10]= a[8]*b[2] + a[9]*b[6] + a[10]*b[10]+ a[11]*b[14];
663   mresult[11]= a[8]*b[3] + a[9]*b[7] + a[10]*b[11]+ a[11]*b[15];
664
665   mresult[12]= a[12]*b[0]+ a[13]*b[4]+ a[14]*b[8] + a[15]*b[12];
666   mresult[13]= a[12]*b[1]+ a[13]*b[5]+ a[14]*b[9] + a[15]*b[13];
667   mresult[14]= a[12]*b[2]+ a[13]*b[6]+ a[14]*b[10]+ a[15]*b[14];
668   mresult[15]= a[12]*b[3]+ a[13]*b[7]+ a[14]*b[11]+ a[15]*b[15];
669}
670
671
672// //--------------------------------------
673// static void m_matF_x_point3F_C(const F32 *m, const F32 *p, F32 *presult)
674// {
675//    AssertFatal(p != presult, "Error, aliasing matrix mul pointers not allowed here!");
676//    presult[0] = m[0]*p[0] + m[1]*p[1] + m[2]*p[2]  + m[3];
677//    presult[1] = m[4]*p[0] + m[5]*p[1] + m[6]*p[2]  + m[7];
678//    presult[2] = m[8]*p[0] + m[9]*p[1] + m[10]*p[2] + m[11];
679// }
680
681
682// //--------------------------------------
683// static void m_matF_x_vectorF_C(const F32 *m, const F32 *v, F32 *vresult)
684// {
685//    AssertFatal(v != vresult, "Error, aliasing matrix mul pointers not allowed here!");
686//    vresult[0] = m[0]*v[0] + m[1]*v[1] + m[2]*v[2];
687//    vresult[1] = m[4]*v[0] + m[5]*v[1] + m[6]*v[2];
688//    vresult[2] = m[8]*v[0] + m[9]*v[1] + m[10]*v[2];
689// }
690
691
692//--------------------------------------
693static void m_matF_x_point4F_C(const F32 *m, const F32 *p, F32 *presult)
694{
695   AssertFatal(p != presult, "Error, aliasing matrix mul pointers not allowed here!");
696   presult[0] = m[0]*p[0] + m[1]*p[1] + m[2]*p[2]  + m[3]*p[3];
697   presult[1] = m[4]*p[0] + m[5]*p[1] + m[6]*p[2]  + m[7]*p[3];
698   presult[2] = m[8]*p[0] + m[9]*p[1] + m[10]*p[2] + m[11]*p[3];
699   presult[3] = m[12]*p[0]+ m[13]*p[1]+ m[14]*p[2] + m[15]*p[3];
700}
701
702//--------------------------------------
703static void m_matF_x_scale_x_planeF_C(const F32* m, const F32* s, const F32* p, F32* presult)
704{
705   // We take in a matrix, a scale factor, and a plane equation.  We want to output
706   //  the resultant normal
707   // We have T = m*s
708   // To multiply the normal, we want Inv(Tr(m*s))
709   //  Inv(Tr(ms)) = Inv(Tr(s) * Tr(m))
710   //              = Inv(Tr(m)) * Inv(Tr(s))
711   //
712   //  Inv(Tr(s)) = Inv(s) = [ 1/x   0   0  0]
713   //                        [   0 1/y   0  0]
714   //                        [   0   0 1/z  0]
715   //                        [   0   0   0  1]
716   //
717   // Since m is an affine matrix,
718   //  Tr(m) = [ [       ] 0 ]
719   //          [ [   R   ] 0 ]
720   //          [ [       ] 0 ]
721   //          [ [ x y z ] 1 ]
722   //
723   // Inv(Tr(m)) = [ [    -1 ] 0 ]
724   //              [ [   R   ] 0 ]
725   //              [ [       ] 0 ]
726   //              [ [ A B C ] 1 ]
727   // Where:
728   //
729   //  P = (x, y, z)
730   //  A = -(Row(0, r) * P);
731   //  B = -(Row(1, r) * P);
732   //  C = -(Row(2, r) * P);
733
734   MatrixF invScale(true);
735   F32* pScaleElems = invScale;
736   pScaleElems[MatrixF::idx(0, 0)] = 1.0f / s[0];
737   pScaleElems[MatrixF::idx(1, 1)] = 1.0f / s[1];
738   pScaleElems[MatrixF::idx(2, 2)] = 1.0f / s[2];
739
740   const Point3F shear( m[MatrixF::idx(3, 0)], m[MatrixF::idx(3, 1)], m[MatrixF::idx(3, 2)] );
741
742   const Point3F row0(m[MatrixF::idx(0, 0)], m[MatrixF::idx(0, 1)], m[MatrixF::idx(0, 2)]);
743   const Point3F row1(m[MatrixF::idx(1, 0)], m[MatrixF::idx(1, 1)], m[MatrixF::idx(1, 2)]);
744   const Point3F row2(m[MatrixF::idx(2, 0)], m[MatrixF::idx(2, 1)], m[MatrixF::idx(2, 2)]);
745
746   const F32 A = -mDot(row0, shear);
747   const F32 B = -mDot(row1, shear);
748   const F32 C = -mDot(row2, shear);
749
750   MatrixF invTrMatrix(true);
751   F32* destMat = invTrMatrix;
752   destMat[MatrixF::idx(0, 0)] = m[MatrixF::idx(0, 0)];
753   destMat[MatrixF::idx(1, 0)] = m[MatrixF::idx(1, 0)];
754   destMat[MatrixF::idx(2, 0)] = m[MatrixF::idx(2, 0)];
755   destMat[MatrixF::idx(0, 1)] = m[MatrixF::idx(0, 1)];
756   destMat[MatrixF::idx(1, 1)] = m[MatrixF::idx(1, 1)];
757   destMat[MatrixF::idx(2, 1)] = m[MatrixF::idx(2, 1)];
758   destMat[MatrixF::idx(0, 2)] = m[MatrixF::idx(0, 2)];
759   destMat[MatrixF::idx(1, 2)] = m[MatrixF::idx(1, 2)];
760   destMat[MatrixF::idx(2, 2)] = m[MatrixF::idx(2, 2)];
761   destMat[MatrixF::idx(0, 3)] = A;
762   destMat[MatrixF::idx(1, 3)] = B;
763   destMat[MatrixF::idx(2, 3)] = C;
764   invTrMatrix.mul(invScale);
765
766   Point3F norm(p[0], p[1], p[2]);
767   Point3F point = norm * -p[3];
768   invTrMatrix.mulP(norm);
769   norm.normalize();
770
771   MatrixF temp;
772   dMemcpy(temp, m, sizeof(F32) * 16);
773   point.x *= s[0];
774   point.y *= s[1];
775   point.z *= s[2];
776   temp.mulP(point);
777
778   PlaneF resultPlane(point, norm);
779   presult[0] = resultPlane.x;
780   presult[1] = resultPlane.y;
781   presult[2] = resultPlane.z;
782   presult[3] = resultPlane.d;
783}
784
785static void m_matF_x_box3F_C(const F32 *m, F32* min, F32* max)
786{
787   // Algorithm for axis aligned bounding box adapted from
788   //  Graphic Gems I, pp 548-550
789   //
790   F32 originalMin[3];
791   F32 originalMax[3];
792   originalMin[0] = min[0];
793   originalMin[1] = min[1];
794   originalMin[2] = min[2];
795   originalMax[0] = max[0];
796   originalMax[1] = max[1];
797   originalMax[2] = max[2];
798
799   min[0] = max[0] = m[3];
800   min[1] = max[1] = m[7];
801   min[2] = max[2] = m[11];
802
803   const F32 * row = &m[0];
804   for (U32 i = 0; i < 3; i++)
805   {
806      #define  Do_One_Row(j)   {                         \
807         F32    a = (row[j] * originalMin[j]);           \
808         F32    b = (row[j] * originalMax[j]);           \
809         if (a < b) { *min += a;  *max += b; }           \
810         else       { *min += b;  *max += a; }     }
811
812      // Simpler addressing (avoiding things like [ecx+edi*4]) might be worthwhile (LH):
813      Do_One_Row(0);
814      Do_One_Row(1);
815      Do_One_Row(2);
816      row += 4;
817      min++;
818      max++;
819   }
820}
821
822
823void m_point3F_bulk_dot_C(const F32* refVector,
824                          const F32* dotPoints,
825                          const U32  numPoints,
826                          const U32  pointStride,
827                          F32*       output)
828{
829   for (U32 i = 0; i < numPoints; i++)
830   {
831      F32* pPoint = (F32*)(((U8*)dotPoints) + (pointStride * i));
832      output[i] = ((refVector[0] * pPoint[0]) +
833                   (refVector[1] * pPoint[1]) +
834                   (refVector[2] * pPoint[2]));
835   }
836}
837
838void m_point3F_bulk_dot_indexed_C(const F32* refVector,
839                                  const F32* dotPoints,
840                                  const U32  numPoints,
841                                  const U32  pointStride,
842                                  const U32* pointIndices,
843                                  F32*       output)
844{
845   for (U32 i = 0; i < numPoints; i++)
846   {
847      F32* pPoint = (F32*)(((U8*)dotPoints) + (pointStride * pointIndices[i]));
848      output[i] = ((refVector[0] * pPoint[0]) +
849                   (refVector[1] * pPoint[1]) +
850                   (refVector[2] * pPoint[2]));
851   }
852}
853
854//------------------------------------------------------------------------------
855// Math function pointer declarations
856
857S32  (*m_mulDivS32)(S32 a, S32 b, S32 c) = m_mulDivS32_C;
858U32  (*m_mulDivU32)(S32 a, S32 b, U32 c) = m_mulDivU32_C;
859
860F32  (*m_catmullrom)(F32 t, F32 p0, F32 p1, F32 p2, F32 p3) = m_catmullrom_C;
861
862void (*m_sincos)( F32 angle, F32 *s, F32 *c ) = m_sincos_C;
863void (*m_sincosD)( F64 angle, F64 *s, F64 *c ) = m_sincosD_C;
864
865void (*m_point2F_normalize)(F32 *p) = m_point2F_normalize_C;
866void (*m_point2F_normalize_f)(F32 *p, F32 val) = m_point2F_normalize_f_C;
867void (*m_point2D_normalize)(F64 *p) = m_point2D_normalize_C;
868void (*m_point2D_normalize_f)(F64 *p, F64 val) = m_point2D_normalize_f_C;
869void (*m_point3D_normalize_f)(F64 *p, F64 val) = m_point3D_normalize_f_C;
870void (*m_point3F_normalize)(F32 *p) = m_point3F_normalize_C;
871void (*m_point3F_normalize_f)(F32 *p, F32 len) = m_point3F_normalize_f_C;
872void (*m_point3F_interpolate)(const F32 *from, const F32 *to, F32 factor, F32 *result) = m_point3F_interpolate_C;
873
874void (*m_point3D_normalize)(F64 *p) = m_point3D_normalize_C;
875void (*m_point3D_interpolate)(const F64 *from, const F64 *to, F64 factor, F64 *result) = m_point3D_interpolate_C;
876
877void (*m_point3F_bulk_dot)(const F32* refVector,
878                           const F32* dotPoints,
879                           const U32  numPoints,
880                           const U32  pointStride,
881                           F32*       output) = m_point3F_bulk_dot_C;
882void (*m_point3F_bulk_dot_indexed)(const F32* refVector,
883                                   const F32* dotPoints,
884                                   const U32  numPoints,
885                                   const U32  pointStride,
886                                   const U32* pointIndices,
887                                   F32*       output) = m_point3F_bulk_dot_indexed_C;
888
889void (*m_quatF_set_matF)( F32 x, F32 y, F32 z, F32 w, F32* m ) = m_quatF_set_matF_C;
890
891void (*m_matF_set_euler)(const F32 *e, F32 *result) = m_matF_set_euler_C;
892void (*m_matF_set_euler_point)(const F32 *e, const F32 *p, F32 *result) = m_matF_set_euler_point_C;
893void (*m_matF_identity)(F32 *m)  = m_matF_identity_C;
894void (*m_matF_inverse)(F32 *m)   = m_matF_inverse_C;
895void (*m_matF_affineInverse)(F32 *m)   = m_matF_affineInverse_C;
896void (*m_matF_invert_to)(const F32 *m, F32 *d) = m_matF_invert_to_C;
897void (*m_matF_transpose)(F32 *m) = m_matF_transpose_C;
898void (*m_matF_scale)(F32 *m,const F32* p) = m_matF_scale_C;
899void (*m_matF_normalize)(F32 *m) = m_matF_normalize_C;
900F32  (*m_matF_determinant)(const F32 *m) = m_matF_determinant_C;
901void (*m_matF_x_matF)(const F32 *a, const F32 *b, F32 *mresult)    = default_matF_x_matF_C;
902void (*m_matF_x_matF_aligned)(const F32 *a, const F32 *b, F32 *mresult)    = default_matF_x_matF_C;
903// void (*m_matF_x_point3F)(const F32 *m, const F32 *p, F32 *presult) = m_matF_x_point3F_C;
904// void (*m_matF_x_vectorF)(const F32 *m, const F32 *v, F32 *vresult) = m_matF_x_vectorF_C;
905void (*m_matF_x_point4F)(const F32 *m, const F32 *p, F32 *presult) = m_matF_x_point4F_C;
906void (*m_matF_x_scale_x_planeF)(const F32 *m, const F32* s, const F32 *p, F32 *presult) = m_matF_x_scale_x_planeF_C;
907void (*m_matF_x_box3F)(const F32 *m, F32 *min, F32 *max)    = m_matF_x_box3F_C;
908
909//------------------------------------------------------------------------------
910void mInstallLibrary_C()
911{
912   m_mulDivS32             = m_mulDivS32_C;
913   m_mulDivU32             = m_mulDivU32_C;
914
915   m_catmullrom            = m_catmullrom_C;
916
917   m_sincos = m_sincos_C;
918   m_sincosD = m_sincosD_C;
919
920   m_point2F_normalize     = m_point2F_normalize_C;
921   m_point2F_normalize_f   = m_point2F_normalize_f_C;
922   m_point2D_normalize     = m_point2D_normalize_C;
923   m_point2D_normalize_f   = m_point2D_normalize_f_C;
924   m_point3F_normalize     = m_point3F_normalize_C;
925   m_point3F_normalize_f   = m_point3F_normalize_f_C;
926   m_point3F_interpolate   = m_point3F_interpolate_C;
927
928   m_point3D_normalize     = m_point3D_normalize_C;
929   m_point3D_interpolate   = m_point3D_interpolate_C;
930
931   m_point3F_bulk_dot      = m_point3F_bulk_dot_C;
932   m_point3F_bulk_dot_indexed = m_point3F_bulk_dot_indexed_C;
933
934   m_quatF_set_matF        = m_quatF_set_matF_C;
935
936   m_matF_set_euler        = m_matF_set_euler_C;
937   m_matF_set_euler_point  = m_matF_set_euler_point_C;
938   m_matF_identity         = m_matF_identity_C;
939   m_matF_inverse          = m_matF_inverse_C;
940   m_matF_affineInverse    = m_matF_affineInverse_C;
941   m_matF_invert_to        = m_matF_invert_to_C;
942   m_matF_transpose        = m_matF_transpose_C;
943   m_matF_scale            = m_matF_scale_C;
944   m_matF_normalize        = m_matF_normalize_C;
945   m_matF_determinant      = m_matF_determinant_C;
946   m_matF_x_matF           = default_matF_x_matF_C;
947   m_matF_x_matF_aligned   = default_matF_x_matF_C;
948//    m_matF_x_point3F        = m_matF_x_point3F_C;
949//    m_matF_x_vectorF        = m_matF_x_vectorF_C;
950   m_matF_x_point4F        = m_matF_x_point4F_C;
951   m_matF_x_scale_x_planeF = m_matF_x_scale_x_planeF_C;
952   m_matF_x_box3F          = m_matF_x_box3F_C;
953}
954
955