20 #ifndef OPENRAVE_MATHEXTRA_H
21 #define OPENRAVE_MATHEXTRA_H
28 #define MATH_ASSERT BOOST_ASSERT
31 #define MATH_ASSERT assert
42 #define distinctRoots 0 // roots r0 < r1 < r2
43 #define singleRoot 1 // root r0
44 #define floatRoot01 2 // roots r0 = r1 < r2
45 #define floatRoot12 4 // roots r0 < r1 = r2
46 #define tripleRoot 6 // roots r0 = r1 = r2
49 inline float*
mult4(
float* pfres,
const float* pf1,
const float* pf2);
52 inline float*
multtrans3(
float* pfres,
const float* pf1,
const float* pf2);
53 inline float*
multtrans4(
float* pfres,
const float* pf1,
const float* pf2);
54 inline float*
transnorm3(
float* pfout,
const float* pfmat,
const float* pf);
56 inline float*
transpose3(
const float* pf,
float* pfres);
57 inline float*
transpose4(
const float* pf,
float* pfres);
59 inline float dot2(
const float* pf1,
const float* pf2);
60 inline float dot3(
const float* pf1,
const float* pf2);
61 inline float dot4(
const float* pf1,
const float* pf2);
67 inline float*
normalize2(
float* pfout,
const float* pf);
68 inline float*
normalize3(
float* pfout,
const float* pf);
69 inline float*
normalize4(
float* pfout,
const float* pf);
71 inline float*
cross3(
float* pfout,
const float* pf1,
const float* pf2);
74 inline float*
mult3_s4(
float* pfres,
const float* pf1,
const float* pf2);
75 inline float*
mult3_s3(
float* pfres,
const float* pf1,
const float* pf2);
77 inline float*
inv3(
const float* pf,
float* pfres,
float* pfdet,
int stride);
78 inline float*
inv4(
const float* pf,
float* pfres);
81 inline double*
mult4(
double* pfres,
const double* pf1,
const double* pf2);
84 inline double*
multtrans3(
double* pfres,
const double* pf1,
const double* pf2);
85 inline double*
multtrans4(
double* pfres,
const double* pf1,
const double* pf2);
86 inline double*
transnorm3(
double* pfout,
const double* pfmat,
const double* pf);
88 inline double*
transpose3(
const double* pf,
double* pfres);
89 inline double*
transpose4(
const double* pf,
double* pfres);
91 inline double dot2(
const double* pf1,
const double* pf2);
92 inline double dot3(
const double* pf1,
const double* pf2);
93 inline double dot4(
const double* pf1,
const double* pf2);
99 inline double*
normalize2(
double* pfout,
const double* pf);
100 inline double*
normalize3(
double* pfout,
const double* pf);
101 inline double*
normalize4(
double* pfout,
const double* pf);
103 inline double*
cross3(
double* pfout,
const double* pf1,
const double* pf2);
106 inline double*
mult3_s4(
double* pfres,
const double* pf1,
const double* pf2);
107 inline double*
mult3_s3(
double* pfres,
const double* pf1,
const double* pf2);
109 inline double*
inv3(
const double* pf,
double* pfres,
double* pfdet,
int stride);
110 inline double*
inv4(
const double* pf,
double* pfres);
119 template <
typename T>
120 inline bool eig2(
const T* pfmat, T* peigs, T& fv1x, T& fv1y, T& fv2x, T& fv2y);
125 template <
typename T,
typename S>
void Tridiagonal3 (S* mat, T* diag, T* subd);
133 template <
typename T>
140 T f =
dot3(vbasis[0],vbasis[1]);
141 vbasis[1][0] -= vbasis[0][0]*f; vbasis[1][1] -= vbasis[0][1]*f; vbasis[1][2] -= vbasis[0][2]*f;
143 cross3(vbasis[2],vbasis[0],vbasis[1]);
152 template <
typename T>
inline void svd3(
const T* A, T* U, T* D, T* V);
154 template <
typename T>
inline void mult(T* pf, T fa,
int r)
163 template <
typename T>
int Min(T* pts,
int stride,
int numPts);
164 template <
typename T>
int Max(T* pts,
int stride,
int numPts);
167 template <
typename T>
inline void mult(T* pf, T fa,
int r);
171 template <
typename T,
typename R,
typename S>
172 inline S*
mult(T* pf1, R* pf2,
int r1,
int c1,
int c2, S* pfres,
bool badd =
false);
177 template <
typename T,
typename R,
typename S>
178 inline S*
multtrans(T* pf1, R* pf2,
int r1,
int c1,
int c2, S* pfres,
bool badd =
false);
184 template <
typename T,
typename R,
typename S>
185 inline S*
multtrans_to2(T* pf1, R* pf2,
int r1,
int c1,
int r2, S* pfres,
bool badd =
false);
191 template <
typename T>
inline T*
multto1(T* pf1, T* pf2,
int r1,
int c1, T* pftemp = NULL);
195 template <
typename T,
typename S>
inline T*
multto2(T* pf1, S* pf2,
int r2,
int c2, S* pftemp = NULL);
198 template <
typename T>
inline void sub(T* pf1, T* pf2,
int r);
199 template <
typename T>
inline T
normsqr(
const T* pf1,
int r);
200 template <
typename T>
inline T
lengthsqr(
const T* pf1,
const T* pf2,
int length);
201 template <
typename T>
inline T
dot(T* pf1, T* pf2,
int length);
203 template <
typename T>
inline T
sum(T* pf,
int length);
206 template <
typename T>
inline bool inv2(T* pf, T* pfres);
211 template <
typename T>
212 bool eig2(
const T* pfmat, T* peigs, T& fv1x, T& fv1y, T& fv2x, T& fv2y)
216 b = -(pfmat[0] + pfmat[3]);
217 c = pfmat[0] * pfmat[3] - pfmat[1] * pfmat[2];
218 d = b * b - 4.0f * c + 1e-16f;
228 c = 1 / sqrt(fv1x*fv1x + fv1y*fv1y);
241 c = 1 / sqrt(fv1x*fv1x + fv1y*fv1y);
248 c = 1 / sqrt(fv2x*fv2x + fv2y*fv2y);
255 template <
typename T>
258 T d = b * b - (T)4 * c * a + (T)1e-16;
263 r1 = r2 = (T)-0.5 * b / a;
273 #define MULT3(stride) { \
274 pfres2[0*stride+0] = pf1[0*stride+0]*pf2[0*stride+0]+pf1[0*stride+1]*pf2[1*stride+0]+pf1[0*stride+2]*pf2[2*stride+0]; \
275 pfres2[0*stride+1] = pf1[0*stride+0]*pf2[0*stride+1]+pf1[0*stride+1]*pf2[1*stride+1]+pf1[0*stride+2]*pf2[2*stride+1]; \
276 pfres2[0*stride+2] = pf1[0*stride+0]*pf2[0*stride+2]+pf1[0*stride+1]*pf2[1*stride+2]+pf1[0*stride+2]*pf2[2*stride+2]; \
277 pfres2[1*stride+0] = pf1[1*stride+0]*pf2[0*stride+0]+pf1[1*stride+1]*pf2[1*stride+0]+pf1[1*stride+2]*pf2[2*stride+0]; \
278 pfres2[1*stride+1] = pf1[1*stride+0]*pf2[0*stride+1]+pf1[1*stride+1]*pf2[1*stride+1]+pf1[1*stride+2]*pf2[2*stride+1]; \
279 pfres2[1*stride+2] = pf1[1*stride+0]*pf2[0*stride+2]+pf1[1*stride+1]*pf2[1*stride+2]+pf1[1*stride+2]*pf2[2*stride+2]; \
280 pfres2[2*stride+0] = pf1[2*stride+0]*pf2[0*stride+0]+pf1[2*stride+1]*pf2[1*stride+0]+pf1[2*stride+2]*pf2[2*stride+0]; \
281 pfres2[2*stride+1] = pf1[2*stride+0]*pf2[0*stride+1]+pf1[2*stride+1]*pf2[1*stride+1]+pf1[2*stride+2]*pf2[2*stride+1]; \
282 pfres2[2*stride+2] = pf1[2*stride+0]*pf2[0*stride+2]+pf1[2*stride+1]*pf2[1*stride+2]+pf1[2*stride+2]*pf2[2*stride+2]; \
286 template <
typename T>
287 inline T*
_mult3_s4(T* pfres,
const T* pf1,
const T* pf2)
289 MATH_ASSERT( pf1 != NULL && pf2 != NULL && pfres != NULL );
292 if((pfres == pf1)||(pfres == pf2)) pfres2 = (T*)alloca(12 *
sizeof(T));
296 if( pfres2 != pfres ) memcpy(pfres, pfres2, 12*
sizeof(T));
301 template <
typename T>
302 inline T*
_mult3_s3(T* pfres,
const T* pf1,
const T* pf2)
304 MATH_ASSERT( pf1 != NULL && pf2 != NULL && pfres != NULL );
307 if((pfres == pf1)||(pfres == pf2)) pfres2 = (T*)alloca(9 *
sizeof(T));
312 if( pfres2 != pfres ) memcpy(pfres, pfres2, 9*
sizeof(T));
318 template <
typename T>
319 inline T*
_mult4(T* pfres,
const T* p1,
const T* p2)
321 MATH_ASSERT( pfres != NULL && p1 != NULL && p2 != NULL );
324 if((pfres == p1)||(pfres == p2)) pfres2 = (T*)alloca(16 *
sizeof(T));
327 pfres2[0*4+0] = p1[0*4+0]*p2[0*4+0] + p1[0*4+1]*p2[1*4+0] + p1[0*4+2]*p2[2*4+0] + p1[0*4+3]*p2[3*4+0];
328 pfres2[0*4+1] = p1[0*4+0]*p2[0*4+1] + p1[0*4+1]*p2[1*4+1] + p1[0*4+2]*p2[2*4+1] + p1[0*4+3]*p2[3*4+1];
329 pfres2[0*4+2] = p1[0*4+0]*p2[0*4+2] + p1[0*4+1]*p2[1*4+2] + p1[0*4+2]*p2[2*4+2] + p1[0*4+3]*p2[3*4+2];
330 pfres2[0*4+3] = p1[0*4+0]*p2[0*4+3] + p1[0*4+1]*p2[1*4+3] + p1[0*4+2]*p2[2*4+3] + p1[0*4+3]*p2[3*4+3];
332 pfres2[1*4+0] = p1[1*4+0]*p2[0*4+0] + p1[1*4+1]*p2[1*4+0] + p1[1*4+2]*p2[2*4+0] + p1[1*4+3]*p2[3*4+0];
333 pfres2[1*4+1] = p1[1*4+0]*p2[0*4+1] + p1[1*4+1]*p2[1*4+1] + p1[1*4+2]*p2[2*4+1] + p1[1*4+3]*p2[3*4+1];
334 pfres2[1*4+2] = p1[1*4+0]*p2[0*4+2] + p1[1*4+1]*p2[1*4+2] + p1[1*4+2]*p2[2*4+2] + p1[1*4+3]*p2[3*4+2];
335 pfres2[1*4+3] = p1[1*4+0]*p2[0*4+3] + p1[1*4+1]*p2[1*4+3] + p1[1*4+2]*p2[2*4+3] + p1[1*4+3]*p2[3*4+3];
337 pfres2[2*4+0] = p1[2*4+0]*p2[0*4+0] + p1[2*4+1]*p2[1*4+0] + p1[2*4+2]*p2[2*4+0] + p1[2*4+3]*p2[3*4+0];
338 pfres2[2*4+1] = p1[2*4+0]*p2[0*4+1] + p1[2*4+1]*p2[1*4+1] + p1[2*4+2]*p2[2*4+1] + p1[2*4+3]*p2[3*4+1];
339 pfres2[2*4+2] = p1[2*4+0]*p2[0*4+2] + p1[2*4+1]*p2[1*4+2] + p1[2*4+2]*p2[2*4+2] + p1[2*4+3]*p2[3*4+2];
340 pfres2[2*4+3] = p1[2*4+0]*p2[0*4+3] + p1[2*4+1]*p2[1*4+3] + p1[2*4+2]*p2[2*4+3] + p1[2*4+3]*p2[3*4+3];
342 pfres2[3*4+0] = p1[3*4+0]*p2[0*4+0] + p1[3*4+1]*p2[1*4+0] + p1[3*4+2]*p2[2*4+0] + p1[3*4+3]*p2[3*4+0];
343 pfres2[3*4+1] = p1[3*4+0]*p2[0*4+1] + p1[3*4+1]*p2[1*4+1] + p1[3*4+2]*p2[2*4+1] + p1[3*4+3]*p2[3*4+1];
344 pfres2[3*4+2] = p1[3*4+0]*p2[0*4+2] + p1[3*4+1]*p2[1*4+2] + p1[3*4+2]*p2[2*4+2] + p1[3*4+3]*p2[3*4+2];
345 pfres2[3*4+3] = p1[3*4+0]*p2[0*4+3] + p1[3*4+1]*p2[1*4+3] + p1[3*4+2]*p2[2*4+3] + p1[3*4+3]*p2[3*4+3];
347 if( pfres != pfres2 ) memcpy(pfres, pfres2,
sizeof(T)*16);
351 template <
typename T>
355 if( pfres == pf1 ) pfres2 = (T*)alloca(9 *
sizeof(T));
358 pfres2[0] = pf1[0]*pf2[0]+pf1[3]*pf2[3]+pf1[6]*pf2[6];
359 pfres2[1] = pf1[0]*pf2[1]+pf1[3]*pf2[4]+pf1[6]*pf2[7];
360 pfres2[2] = pf1[0]*pf2[2]+pf1[3]*pf2[5]+pf1[6]*pf2[8];
362 pfres2[3] = pf1[1]*pf2[0]+pf1[4]*pf2[3]+pf1[7]*pf2[6];
363 pfres2[4] = pf1[1]*pf2[1]+pf1[4]*pf2[4]+pf1[7]*pf2[7];
364 pfres2[5] = pf1[1]*pf2[2]+pf1[4]*pf2[5]+pf1[7]*pf2[8];
366 pfres2[6] = pf1[2]*pf2[0]+pf1[5]*pf2[3]+pf1[8]*pf2[6];
367 pfres2[7] = pf1[2]*pf2[1]+pf1[5]*pf2[4]+pf1[8]*pf2[7];
368 pfres2[8] = pf1[2]*pf2[2]+pf1[5]*pf2[5]+pf1[8]*pf2[8];
370 if( pfres2 != pfres ) memcpy(pfres, pfres2, 9*
sizeof(T));
375 template <
typename T>
379 if( pfres == pf1 ) pfres2 = (T*)alloca(16 *
sizeof(T));
382 for(
int i = 0; i < 4; ++i) {
383 for(
int j = 0; j < 4; ++j) {
384 pfres2[4*i+j] = pf1[i] * pf2[j] + pf1[i+4] * pf2[j+4] + pf1[i+8] * pf2[j+8] + pf1[i+12] * pf2[j+12];
388 if( pfres2 != pfres ) memcpy(pfres, pfres2, 16*
sizeof(T));
394 template <
typename T>
inline T
matrixdet3(
const T* pf,
int stride)
396 return pf[0*stride+2] * (pf[1*stride + 0] * pf[2*stride + 1] - pf[1*stride + 1] * pf[2*stride + 0]) +
397 pf[1*stride+2] * (pf[0*stride + 1] * pf[2*stride + 0] - pf[0*stride + 0] * pf[2*stride + 1]) +
398 pf[2*stride+2] * (pf[0*stride + 0] * pf[1*stride + 1] - pf[0*stride + 1] * pf[1*stride + 0]);
408 template <
typename T>
409 inline T*
_inv3(
const T* pf, T* pfres, T* pfdet,
int stride)
412 if( pfres == pf ) pfres2 = (T*)alloca(3 * stride *
sizeof(T));
418 pfres2[0*stride + 0] = pf[1*stride + 1] * pf[2*stride + 2] - pf[1*stride + 2] * pf[2*stride + 1];
419 pfres2[0*stride + 1] = pf[0*stride + 2] * pf[2*stride + 1] - pf[0*stride + 1] * pf[2*stride + 2];
420 pfres2[0*stride + 2] = pf[0*stride + 1] * pf[1*stride + 2] - pf[0*stride + 2] * pf[1*stride + 1];
421 pfres2[1*stride + 0] = pf[1*stride + 2] * pf[2*stride + 0] - pf[1*stride + 0] * pf[2*stride + 2];
422 pfres2[1*stride + 1] = pf[0*stride + 0] * pf[2*stride + 2] - pf[0*stride + 2] * pf[2*stride + 0];
423 pfres2[1*stride + 2] = pf[0*stride + 2] * pf[1*stride + 0] - pf[0*stride + 0] * pf[1*stride + 2];
424 pfres2[2*stride + 0] = pf[1*stride + 0] * pf[2*stride + 1] - pf[1*stride + 1] * pf[2*stride + 0];
425 pfres2[2*stride + 1] = pf[0*stride + 1] * pf[2*stride + 0] - pf[0*stride + 0] * pf[2*stride + 1];
426 pfres2[2*stride + 2] = pf[0*stride + 0] * pf[1*stride + 1] - pf[0*stride + 1] * pf[1*stride + 0];
428 T fdet = pf[0*stride + 2] * pfres2[2*stride + 0] + pf[1*stride + 2] * pfres2[2*stride + 1] +
429 pf[2*stride + 2] * pfres2[2*stride + 2];
434 if( fabs(fdet) < 1e-12 ) {
442 pfres[0*stride+0] *= fdet; pfres[0*stride+1] *= fdet; pfres[0*stride+2] *= fdet;
443 pfres[1*stride+0] *= fdet; pfres[1*stride+1] *= fdet; pfres[1*stride+2] *= fdet;
444 pfres[2*stride+0] *= fdet; pfres[2*stride+1] *= fdet; pfres[2*stride+2] *= fdet;
448 pfres[0*stride+0] = pfres2[0*stride+0] * fdet;
449 pfres[0*stride+1] = pfres2[0*stride+1] * fdet;
450 pfres[0*stride+2] = pfres2[0*stride+2] * fdet;
451 pfres[1*stride+0] = pfres2[1*stride+0] * fdet;
452 pfres[1*stride+1] = pfres2[1*stride+1] * fdet;
453 pfres[1*stride+2] = pfres2[1*stride+2] * fdet;
454 pfres[2*stride+0] = pfres2[2*stride+0] * fdet;
455 pfres[2*stride+1] = pfres2[2*stride+1] * fdet;
456 pfres[2*stride+2] = pfres2[2*stride+2] * fdet;
461 template <
typename T>
462 inline T*
_inv4(
const T* pf, T* pfres)
465 if( pfres == pf ) pfres2 = (T*)alloca(16 *
sizeof(T));
475 fd0 = pf[2*4 + 0] * pf[3*4 + 1] - pf[2*4 + 1] * pf[3*4 + 0];
476 fd1 = pf[2*4 + 1] * pf[3*4 + 2] - pf[2*4 + 2] * pf[3*4 + 1];
477 fd2 = pf[2*4 + 2] * pf[3*4 + 3] - pf[2*4 + 3] * pf[3*4 + 2];
479 f1 = pf[2*4 + 1] * pf[3*4 + 3] - pf[2*4 + 3] * pf[3*4 + 1];
480 f2 = pf[2*4 + 0] * pf[3*4 + 3] - pf[2*4 + 3] * pf[3*4 + 0];
481 f3 = pf[2*4 + 0] * pf[3*4 + 2] - pf[2*4 + 2] * pf[3*4 + 0];
483 pfres2[0*4 + 0] = pf[1*4 + 1] * fd2 - pf[1*4 + 2] * f1 + pf[1*4 + 3] * fd1;
484 pfres2[0*4 + 1] = -(pf[0*4 + 1] * fd2 - pf[0*4 + 2] * f1 + pf[0*4 + 3] * fd1);
486 pfres2[1*4 + 0] = -(pf[1*4 + 0] * fd2 - pf[1*4 + 2] * f2 + pf[1*4 + 3] * f3);
487 pfres2[1*4 + 1] = pf[0*4 + 0] * fd2 - pf[0*4 + 2] * f2 + pf[0*4 + 3] * f3;
489 pfres2[2*4 + 0] = pf[1*4 + 0] * f1 - pf[1*4 + 1] * f2 + pf[1*4 + 3] * fd0;
490 pfres2[2*4 + 1] = -(pf[0*4 + 0] * f1 - pf[0*4 + 1] * f2 + pf[0*4 + 3] * fd0);
492 pfres2[3*4 + 0] = -(pf[1*4 + 0] * fd1 - pf[1*4 + 1] * f3 + pf[1*4 + 2] * fd0);
493 pfres2[3*4 + 1] = pf[0*4 + 0] * fd1 - pf[0*4 + 1] * f3 + pf[0*4 + 2] * fd0;
496 fd0 = pf[0*4 + 0] * pf[1*4 + 1] - pf[0*4 + 1] * pf[1*4 + 0];
497 fd1 = pf[0*4 + 1] * pf[1*4 + 2] - pf[0*4 + 2] * pf[1*4 + 1];
498 fd2 = pf[0*4 + 2] * pf[1*4 + 3] - pf[0*4 + 3] * pf[1*4 + 2];
500 f1 = pf[0*4 + 1] * pf[1*4 + 3] - pf[0*4 + 3] * pf[1*4 + 1];
501 f2 = pf[0*4 + 0] * pf[1*4 + 3] - pf[0*4 + 3] * pf[1*4 + 0];
502 f3 = pf[0*4 + 0] * pf[1*4 + 2] - pf[0*4 + 2] * pf[1*4 + 0];
504 pfres2[0*4 + 2] = pf[3*4 + 1] * fd2 - pf[3*4 + 2] * f1 + pf[3*4 + 3] * fd1;
505 pfres2[0*4 + 3] = -(pf[2*4 + 1] * fd2 - pf[2*4 + 2] * f1 + pf[2*4 + 3] * fd1);
507 pfres2[1*4 + 2] = -(pf[3*4 + 0] * fd2 - pf[3*4 + 2] * f2 + pf[3*4 + 3] * f3);
508 pfres2[1*4 + 3] = pf[2*4 + 0] * fd2 - pf[2*4 + 2] * f2 + pf[2*4 + 3] * f3;
510 pfres2[2*4 + 2] = pf[3*4 + 0] * f1 - pf[3*4 + 1] * f2 + pf[3*4 + 3] * fd0;
511 pfres2[2*4 + 3] = -(pf[2*4 + 0] * f1 - pf[2*4 + 1] * f2 + pf[2*4 + 3] * fd0);
513 pfres2[3*4 + 2] = -(pf[3*4 + 0] * fd1 - pf[3*4 + 1] * f3 + pf[3*4 + 2] * fd0);
514 pfres2[3*4 + 3] = pf[2*4 + 0] * fd1 - pf[2*4 + 1] * f3 + pf[2*4 + 2] * fd0;
516 T fdet = pf[0*4 + 3] * pfres2[3*4 + 0] + pf[1*4 + 3] * pfres2[3*4 + 1] +
517 pf[2*4 + 3] * pfres2[3*4 + 2] + pf[3*4 + 3] * pfres2[3*4 + 3];
519 if( fabs(fdet) < 1e-9)
return NULL;
524 if( pfres2 == pfres ) {
525 mult(pfres, fdet, 16);
531 pfres[i] = pfres2[i] * fdet;
539 template <
typename T>
545 std::swap(pfres[1], pfres[3]);
546 std::swap(pfres[2], pfres[6]);
547 std::swap(pfres[5], pfres[7]);
551 pfres[0] = pf[0]; pfres[1] = pf[3]; pfres[2] = pf[6];
552 pfres[3] = pf[1]; pfres[4] = pf[4]; pfres[5] = pf[7];
553 pfres[6] = pf[2]; pfres[7] = pf[5]; pfres[8] = pf[8];
559 template <
typename T>
565 std::swap(pfres[1], pfres[4]);
566 std::swap(pfres[2], pfres[8]);
567 std::swap(pfres[3], pfres[12]);
568 std::swap(pfres[6], pfres[9]);
569 std::swap(pfres[7], pfres[13]);
570 std::swap(pfres[11], pfres[15]);
574 pfres[0] = pf[0]; pfres[1] = pf[4]; pfres[2] = pf[8]; pfres[3] = pf[12];
575 pfres[4] = pf[1]; pfres[5] = pf[5]; pfres[6] = pf[9]; pfres[7] = pf[13];
576 pfres[8] = pf[2]; pfres[9] = pf[6]; pfres[10] = pf[10]; pfres[11] = pf[14];
577 pfres[12] = pf[3]; pfres[13] = pf[7]; pfres[14] = pf[11]; pfres[15] = pf[15];
581 template <
typename T>
582 inline T
_dot2(
const T* pf1,
const T* pf2)
585 return pf1[0]*pf2[0] + pf1[1]*pf2[1];
588 template <
typename T>
589 inline T
_dot3(
const T* pf1,
const T* pf2)
592 return pf1[0]*pf2[0] + pf1[1]*pf2[1] + pf1[2]*pf2[2];
595 template <
typename T>
596 inline T
_dot4(
const T* pf1,
const T* pf2)
599 return pf1[0]*pf2[0] + pf1[1]*pf2[1] + pf1[2]*pf2[2] + pf1[3] * pf2[3];
602 template <
typename T>
606 return pf[0] * pf[0] + pf[1] * pf[1];
609 template <
typename T>
613 return pf[0] * pf[0] + pf[1] * pf[1] + pf[2] * pf[2];
616 template <
typename T>
620 return pf[0] * pf[0] + pf[1] * pf[1] + pf[2] * pf[2] + pf[3] * pf[3];
623 template <
typename T>
628 T f = pf[0]*pf[0] + pf[1]*pf[1];
630 pfout[0] = pf[0] * f;
631 pfout[1] = pf[1] * f;
636 template <
typename T>
641 T f = pf[0]*pf[0] + pf[1]*pf[1] + pf[2]*pf[2];
644 pfout[0] = pf[0] * f;
645 pfout[1] = pf[1] * f;
646 pfout[2] = pf[2] * f;
651 template <
typename T>
656 T f = pf[0]*pf[0] + pf[1]*pf[1] + pf[2]*pf[2] + pf[3]*pf[3];
659 pfout[0] = pf[0] * f;
660 pfout[1] = pf[1] * f;
661 pfout[2] = pf[2] * f;
662 pfout[3] = pf[3] * f;
667 template <
typename T>
668 inline T*
_cross3(T* pfout,
const T* pf1,
const T* pf2)
670 MATH_ASSERT( pfout != NULL && pf1 != NULL && pf2 != NULL );
673 temp[0] = pf1[1] * pf2[2] - pf1[2] * pf2[1];
674 temp[1] = pf1[2] * pf2[0] - pf1[0] * pf2[2];
675 temp[2] = pf1[0] * pf2[1] - pf1[1] * pf2[0];
677 pfout[0] = temp[0]; pfout[1] = temp[1]; pfout[2] = temp[2];
681 template <
typename T>
684 MATH_ASSERT( pfout != NULL && pf != NULL && pfmat != NULL );
687 T* pfreal = (pfout == pf) ? dummy : pfout;
689 pfreal[0] = pf[0] * pfmat[0] + pf[1] * pfmat[1] + pf[2] * pfmat[2];
690 pfreal[1] = pf[0] * pfmat[3] + pf[1] * pfmat[4] + pf[2] * pfmat[5];
691 pfreal[2] = pf[0] * pfmat[6] + pf[1] * pfmat[7] + pf[2] * pfmat[8];
694 pfout[0] = pfreal[0];
695 pfout[1] = pfreal[1];
696 pfout[2] = pfreal[2];
702 inline float*
mult4(
float* pfres,
const float* pf1,
const float* pf2) {
703 return _mult4<float>(pfres, pf1, pf2);
706 inline float*
multtrans3(
float* pfres,
const float* pf1,
const float* pf2) {
707 return _multtrans3<float>(pfres, pf1, pf2);
709 inline float*
multtrans4(
float* pfres,
const float* pf1,
const float* pf2) {
710 return _multtrans4<float>(pfres, pf1, pf2);
712 inline float*
transnorm3(
float* pfout,
const float* pfmat,
const float* pf) {
713 return _transnorm3<float>(pfout, pfmat, pf);
717 return _transpose3<float>(pf, pfres);
720 return _transpose4<float>(pf, pfres);
723 inline float dot2(
const float* pf1,
const float* pf2) {
724 return _dot2<float>(pf1, pf2);
726 inline float dot3(
const float* pf1,
const float* pf2) {
727 return _dot3<float>(pf1, pf2);
729 inline float dot4(
const float* pf1,
const float* pf2) {
730 return _dot4<float>(pf1, pf2);
734 return _lengthsqr2<float>(pf);
737 return _lengthsqr3<float>(pf);
740 return _lengthsqr4<float>(pf);
744 return _normalize2<float>(pfout, pf);
747 return _normalize3<float>(pfout, pf);
750 return _normalize4<float>(pfout, pf);
753 inline float*
cross3(
float* pfout,
const float* pf1,
const float* pf2) {
754 return _cross3<float>(pfout, pf1, pf2);
758 inline float*
mult3_s4(
float* pfres,
const float* pf1,
const float* pf2) {
759 return _mult3_s4<float>(pfres, pf1, pf2);
761 inline float*
mult3_s3(
float* pfres,
const float* pf1,
const float* pf2) {
762 return _mult3_s3<float>(pfres, pf1, pf2);
765 inline float*
inv3(
const float* pf,
float* pfres,
float* pfdet,
int stride) {
766 return _inv3<float>(pf, pfres, pfdet, stride);
768 inline float*
inv4(
const float* pf,
float* pfres) {
769 return _inv4<float>(pf, pfres);
773 inline double*
mult4(
double* pfres,
const double* pf1,
const double* pf2) {
774 return _mult4<double>(pfres, pf1, pf2);
777 inline double*
multtrans3(
double* pfres,
const double* pf1,
const double* pf2) {
778 return _multtrans3<double>(pfres, pf1, pf2);
780 inline double*
multtrans4(
double* pfres,
const double* pf1,
const double* pf2) {
781 return _multtrans4<double>(pfres, pf1, pf2);
783 inline double*
transnorm3(
double* pfout,
const double* pfmat,
const double* pf) {
784 return _transnorm3<double>(pfout, pfmat, pf);
788 return _transpose3<double>(pf, pfres);
791 return _transpose4<double>(pf, pfres);
794 inline double dot2(
const double* pf1,
const double* pf2) {
795 return _dot2<double>(pf1, pf2);
797 inline double dot3(
const double* pf1,
const double* pf2) {
798 return _dot3<double>(pf1, pf2);
800 inline double dot4(
const double* pf1,
const double* pf2) {
801 return _dot4<double>(pf1, pf2);
805 return _lengthsqr2<double>(pf);
808 return _lengthsqr3<double>(pf);
811 return _lengthsqr4<double>(pf);
815 return _normalize2<double>(pfout, pf);
818 return _normalize3<double>(pfout, pf);
821 return _normalize4<double>(pfout, pf);
824 inline double*
cross3(
double* pfout,
const double* pf1,
const double* pf2) {
825 return _cross3<double>(pfout, pf1, pf2);
829 inline double*
mult3_s4(
double* pfres,
const double* pf1,
const double* pf2) {
830 return _mult3_s4<double>(pfres, pf1, pf2);
832 inline double*
mult3_s3(
double* pfres,
const double* pf1,
const double* pf2) {
833 return _mult3_s3<double>(pfres, pf1, pf2);
836 inline double*
inv3(
const double* pf,
double* pfres,
double* pfdet,
int stride) {
837 return _inv3<double>(pf, pfres, pfdet, stride);
839 inline double*
inv4(
const double* pf,
double* pfres) {
840 return _inv4<double>(pf, pfres);
843 template <
typename T,
typename R,
typename S>
844 inline S*
mult(T* pf1, R* pf2,
int r1,
int c1,
int c2, S* pfres,
bool badd)
846 MATH_ASSERT( pf1 != NULL && pf2 != NULL && pfres != NULL);
849 if( !badd ) memset(pfres, 0,
sizeof(S) * r1 * c2);
858 pfres[j] += (S)(pf1[k] * pf2[k*c2 + j]);
871 template <
typename T,
typename R,
typename S>
872 inline S*
multtrans(T* pf1, R* pf2,
int r1,
int c1,
int c2, S* pfres,
bool badd)
874 MATH_ASSERT( pf1 != NULL && pf2 != NULL && pfres != NULL);
877 if( !badd ) memset(pfres, 0,
sizeof(S) * c1 * c2);
887 pfres[j] += (S)(pf1[k*c1] * pf2[k*c2 + j]);
902 template <
typename T,
typename R,
typename S>
903 inline S*
multtrans_to2(T* pf1, R* pf2,
int r1,
int c1,
int r2, S* pfres,
bool badd)
905 MATH_ASSERT( pf1 != NULL && pf2 != NULL && pfres != NULL);
908 if( !badd ) memset(pfres, 0,
sizeof(S) * r1 * r2);
917 pfres[j] += (S)(pf1[k] * pf2[j*c1 + k]);
930 template <
typename T>
inline T*
multto1(T* pf1, T* pf2,
int r,
int c, T* pftemp)
937 if( pftemp == NULL ) {
952 pftemp[j] += pf1[k] * pf2[k*c + j];
958 memcpy(pf1, pftemp, c *
sizeof(T));
962 if( bdel )
delete[] pftemp;
967 template <
typename T,
typename S>
inline T*
multto2(T* pf1, S* pf2,
int r2,
int c2, S* pftemp)
974 if( pftemp == NULL ) {
989 pftemp[i] += pf1[i*r2 + k] * pf2[k*c2 + j];
997 *(pf2+i*c2+j) = pftemp[i];
1004 if( bdel )
delete[] pftemp;
1009 template <
typename T>
inline void add(T* pf1, T* pf2,
int r)
1019 template <
typename T>
inline void sub(T* pf1, T* pf2,
int r)
1029 template <
typename T>
inline T
normsqr(
const T* pf1,
int r)
1036 d += pf1[r] * pf1[r];
1042 template <
typename T>
inline T
lengthsqr(
const T* pf1,
const T* pf2,
int length)
1047 T t = pf1[length] - pf2[length];
1054 template <
typename T>
inline T
dot(T* pf1, T* pf2,
int length)
1059 d += pf1[length] * pf2[length];
1065 template <
typename T>
inline T
sum(T* pf,
int length)
1076 template <
typename T>
inline bool inv2(T* pf, T* pfres)
1078 T fdet = pf[0] * pf[3] - pf[1] * pf[2];
1080 if( fabs(fdet) < 1e-16 )
return false;
1086 pfres[0] = fdet * pf[3]; pfres[1] = -fdet * pf[1];
1087 pfres[2] = -fdet * pf[2]; pfres[3] = fdet * pf[0];
1092 pfres[0] = pf[3] * fdet;
1095 pfres[3] = ftemp * pf[0];
1100 template <
typename T,
typename S>
1103 T a, b, c, d, e, f, ell, q;
1115 ell = (T)sqrt(b*b+c*c);
1123 mat[0*3+0] = (S)1; mat[0*3+1] = (S)0; mat[0*3+2] = (T)0;
1124 mat[1*3+0] = (S)0; mat[1*3+1] = b; mat[1*3+2] = c;
1125 mat[2*3+0] = (S)0; mat[2*3+1] = c; mat[2*3+2] = -b;
1132 mat[0*3+0] = (S)1; mat[0*3+1] = (S)0; mat[0*3+2] = (S)0;
1133 mat[1*3+0] = (S)0; mat[1*3+1] = (S)1; mat[1*3+2] = (S)0;
1134 mat[2*3+0] = (S)0; mat[2*3+1] = (S)0; mat[2*3+2] = (S)1;
1138 template <
typename T>
1139 inline void svd3(
const T* A, T* U, T* D, T* V)
1146 std::copy(&VVt,&VVt[9],&V[0]);
1161 for(
int i = 0; i < 3; ++i) {
1162 D[i] = sqrt(eigenvalues[i]);
1169 if( D[1] > D[maxval] ) {
1172 if( D[2] > D[maxval] ) {
1177 swap(U[0], U[maxval]);
1178 swap(U[3], U[3+maxval]);
1179 swap(U[6], U[6+maxval]);
1180 swap(V[0], V[maxval]);
1181 swap(V[3], V[3+maxval]);
1182 swap(V[6], V[6+maxval]);
1183 swap(D[0], D[maxval]);
1196 template <
typename T>
1197 int Min(T* pts,
int stride,
int numPts)
1199 MATH_ASSERT( pts != NULL && numPts > 0 && stride > 0 );
1201 int best = pts[0]; pts += stride;
1202 for(
int i = 1; i < numPts; ++i, pts += stride) {
1210 template <
typename T>
1211 int Max(T* pts,
int stride,
int numPts)
1213 MATH_ASSERT( pts != NULL && numPts > 0 && stride > 0 );
1215 int best = pts[0]; pts += stride;
1216 for(
int i = 1; i < numPts; ++i, pts += stride) {