5 #ifndef __FASTVECTOR_H__
6 #define __FASTVECTOR_H__
15 const double PI = 3.14159265358979323846;
19 template<
class T1,
class T2>
20 inline void Vset(T1 *Vr, T2 x, T2 y, T2 z)
22 Vr[0] = (T1)x; Vr[1] = (T1)y; Vr[2] = (T1)z;
26 template<
class T1,
class T2>
27 inline void VcV(T1 *Vr,
const T2 *V)
29 Vr[0] = (T1)V[0]; Vr[1] = (T1)V[1]; Vr[2] = (T1)V[2];
33 template<
class T1,
class T2,
class T3>
34 inline void VmV(T1 *Vr,
const T2 *V1,
const T3 *V2)
36 Vr[0] = (T1)(V1[0] - V2[0]);
37 Vr[1] = (T1)(V1[1] - V2[1]);
38 Vr[2] = (T1)(V1[2] - V2[2]);
42 template<
class T1,
class T2>
43 inline T1*
VmV(T1 *Vr,
const T2 *V1)
52 template<
class T1,
class T2,
class T3>
53 inline void VpV(T1 *Vr,
const T2 *V1,
const T3 *V2)
55 Vr[0] = (T1)(V1[0] + V2[0]);
56 Vr[1] = (T1)(V1[1] + V2[1]);
57 Vr[2] = (T1)(V1[2] + V2[2]);
61 template<
class T1,
class T2>
62 inline T1*
VpV(T1 *Vr,
const T2 *V1)
71 template<
class T1,
class T2,
class T3>
72 inline void VxS(T1 *Vr,
const T2 *V, T3 s)
74 Vr[0] = (T1)(V[0] * s);
75 Vr[1] = (T1)(V[1] * s);
76 Vr[2] = (T1)(V[2] * s);
80 template<
class T1,
class T2>
81 inline T1*
VxS(T1 *Vr, T2 s)
90 template<
class T1,
class T2,
class T3>
91 inline void VcrossV(T1 *Vr,
const T2 *V1,
const T3 *V2)
93 Vr[0] = (T1)(V1[1]*V2[2] - V1[2]*V2[1]);
94 Vr[1] = (T1)(V1[2]*V2[0] - V1[0]*V2[2]);
95 Vr[2] = (T1)(V1[0]*V2[1] - V1[1]*V2[0]);
99 template<
class T1,
class T2>
100 inline T1
VdotV(
const T1 *V1,
const T2 *V2)
102 return (V1[0]*V2[0] + V1[1]*V2[1] + V1[2]*V2[2]);
109 return sqrt(
VdotV(V,V));
114 template<
class T1,
class T2>
115 inline T1
VdistV(
const T1 *V1,
const T2 *V2)
117 return sqrt((V1[0]-V2[0])*(V1[0]-V2[0]) +
118 (V1[1]-V2[1])*(V1[1]-V2[1]) +
119 (V1[2]-V2[2])*(V1[2]-V2[2]) );
123 template<
class T1,
class T2>
126 return (V1[0]-V2[0])*(V1[0]-V2[0]) +
127 (V1[1]-V2[1])*(V1[1]-V2[1]) +
128 (V1[2]-V2[2])*(V1[2]-V2[2]);
132 template<
class T1,
class T2>
133 inline T1
VcosV(
const T1 *V1,
const T2 *V2)
139 template<
class T1,
class T2>
140 inline T1
VsinV(
const T1 *V1,
const T2 *V2)
149 template<
class T1,
class T2>
152 return acos(
VcosV(V1,V2));
156 template <
class T1,
class T2,
class T3>
157 inline void VV2SC(T1 *SC,
const T2* V1,
const T3* V2)
163 SC[1] =
VdotV(V1,V2)/d;
167 template <
class T1,
class T2,
class T3>
168 inline void SCpSC(T1 *SC_sum,
const T2* SC_a,
const T3* SC_b)
170 SC_sum[0] = SC_a[0]*SC_b[1]+SC_b[0]*SC_a[1];
171 SC_sum[1] = SC_a[1]*SC_b[1]-SC_a[0]*SC_b[0];
175 template <
class T1,
class T2>
176 inline T1*
SCpSC(T1* SC_r,
const T2* SC_b)
179 SC_r[0] = SC_r[0]*SC_b[1]+SC_b[0]*SC_r[1];
180 SC_r[1] = SC_r[1]*SC_b[1]-SC_r0*SC_b[0];
185 template <
class T1,
class T2>
193 template <
class T1,
class T2,
class T3>
194 inline void SCmSC(T1 *SC_diff,
const T2* SC_a,
const T3* SC_b)
196 SC_diff[0] = SC_a[0]*SC_b[1]-SC_b[0]*SC_a[1];
197 SC_diff[1] = SC_a[1]*SC_b[1]+SC_a[0]*SC_b[0];
201 template <
class T1,
class T2,
class T3>
204 SC_double[0] = 2*SC_a[0]*SC_a[1];
205 SC_double[1] = 2*SC_a[1]*SC_a[1]-1;
209 template <
class T1,
class T2>
229 if (length>0)
VxS(V,T1(1)/length);
235 inline T1
detVVV(
const T1 *V1,
const T1 *V2,
const T1 *V3)
237 return (V1[0]*V2[1]*V3[2]) + (V1[2]*V2[0]*V3[1]) + (V1[1]*V2[2]*V3[0]) -
238 (V1[2]*V2[1]*V3[0]) - (V1[0]*V2[2]*V3[1]) - (V1[1]*V2[0]*V3[2]);
244 inline T1
det3x3(
const T1 a00,
const T1 a01,
const T1 a02,
245 const T1 a10,
const T1 a11,
const T1 a12,
246 const T1 a20,
const T1 a21,
const T1 a22)
248 return (a00*a11*a22) + (a02*a10*a21) + (a01*a12*a20) -
249 (a02*a11*a20) - (a00*a12*a21) - (a01*a10*a22);
253 template <
class Tr,
class T1,
class T2,
class T3>
254 inline void FxV( Tr *Vr,
const T1 *F,
const T2 *V,
const T3 V3)
256 Vr[0] = (Tr)( F[0]*V[0] + F[4]*V[1] + F[8]*V[2] + F[12]*V3);
257 Vr[1] = (Tr)( F[1]*V[0] + F[5]*V[1] + F[9]*V[2] + F[13]*V3);
258 Vr[2] = (Tr)( F[2]*V[0] + F[6]*V[1] + F[10]*V[2] + F[14]*V3);
262 template <
class Tr,
class T1,
class T3>
263 inline Tr*
FxV(
const T1 *F, Tr *V,
const T3 V3)
266 Vr[0] = (Tr)( F[0]*V[0] + F[4]*V[1] + F[8]*V[2] + F[12]*V3);
267 Vr[1] = (Tr)( F[1]*V[0] + F[5]*V[1] + F[9]*V[2] + F[13]*V3);
268 Vr[2] = (Tr)( F[2]*V[0] + F[6]*V[1] + F[10]*V[2] + F[14]*V3);
274 template<
class Tr,
class T1>
275 inline void V4set(Tr *Vr, T1 x, T1 y, T1 z, T1 h)
284 template <
class Tr,
class T1>
285 inline void V4cV4( Tr *Vr,
const T1 *V)
294 template<
class Tr,
class T1>
295 inline void FcF( Tr *Fr,
const T1 *F)
297 V4cV4( &Fr[0], &F[0] );
298 V4cV4( &Fr[4], &F[4] );
299 V4cV4( &Fr[8], &F[8] );
300 V4cV4( &Fr[12],&F[12]);
304 template <
class Tr,
class T1,
class T2>
305 inline void FxV4( Tr *Vr,
const T1 *F,
const T2 *V)
307 Vr[0] = (Tr)( F[0]*V[0] + F[4]*V[1] + F[8]*V[2] + F[12]*V[3]);
308 Vr[1] = (Tr)( F[1]*V[0] + F[5]*V[1] + F[9]*V[2] + F[13]*V[3]);
309 Vr[2] = (Tr)( F[2]*V[0] + F[6]*V[1] + F[10]*V[2] + F[14]*V[3]);
310 Vr[3] = (Tr)( F[3]*V[0] + F[7]*V[1] + F[11]*V[2] + F[15]*V[3]);
314 template<
class Tr,
class T1,
class T2>
315 inline void FxF( Tr *Fr,
const T1 *F1,
const T2 *F2)
317 FxV4( &Fr[0], F1, &F2[0] );
318 FxV4( &Fr[4], F1, &F2[4] );
319 FxV4( &Fr[8], F1, &F2[8] );
320 FxV4( &Fr[12],F1, &F2[12]);
329 V4cV4( old_pos, &Fr[12]);
331 Fr[12] = -
VdotV( old_pos, &Fr[0]);
332 Fr[13] = -
VdotV( old_pos, &Fr[4]);
333 Fr[14] = -
VdotV( old_pos, &Fr[8]);
354 V4set( &Fr[0], 1.0, 0.0, 0.0, 0.0);
355 V4set( &Fr[4], 0.0, 1.0, 0.0, 0.0);
356 V4set( &Fr[8], 0.0, 0.0, 1.0, 0.0);
357 V4set( &Fr[12],0.0, 0.0, 0.0, 1.0);
362 template <
class Tr,
class T1>
363 inline void Frotx( Tr *Fr,
const T1 angle)
365 double Sa = sin(angle);
366 double Ca = cos(angle);
367 V4set( &Fr[0], 1.0, 0.0, 0.0, 0.0);
368 V4set( &Fr[4], 0.0, Ca , Sa , 0.0);
369 V4set( &Fr[8], 0.0, -Sa, Ca , 0.0);
370 V4set( &Fr[12],0.0, 0.0, 0.0, 1.0);
375 template <
class Tr,
class T1>
376 inline void Froty( Tr *Fr,
const T1 angle)
378 double Sa = sin(angle);
379 double Ca = cos(angle);
380 V4set( &Fr[0], Ca , 0.0, -Sa, 0.0);
381 V4set( &Fr[4], 0.0, 1.0, 0.0, 0.0);
382 V4set( &Fr[8], Sa , 0.0, Ca , 0.0);
383 V4set( &Fr[12],0.0, 0.0, 0.0, 1.0);
388 template <
class Tr,
class T1>
389 inline void Frotz( Tr *Fr,
const T1 angle)
391 double Sa = sin(angle);
392 double Ca = cos(angle);
393 V4set( &Fr[0], Ca , Sa , 0.0, 0.0);
394 V4set( &Fr[4], -Sa, Ca , 0.0, 0.0);
395 V4set( &Fr[8], 0.0, 0.0, 1.0, 0.0);
396 V4set( &Fr[12],0.0, 0.0, 0.0, 1.0);
401 template <
class Tr,
class T1,
class T2>
402 inline void Frotv( Tr *Fr,
const T1 *V,
const T2 angle)
409 double Sa = sin(angle);
410 double Ca = cos(angle);
411 double Va = 1.0 - Ca;
413 V4set( &Fr[0], vn[0]*vn[0]*Va + Ca, vn[0]*vn[1]*Va + vn[2]*Sa, vn[0]*vn[2]*Va - vn[1]*Sa, 0.0 );
414 V4set( &Fr[4], vn[1]*vn[0]*Va - vn[2]*Sa, vn[1]*vn[1]*Va + Ca, vn[1]*vn[2]*Va + vn[0]*Sa, 0.0 );
415 V4set( &Fr[8], vn[2]*vn[0]*Va + vn[1]*Sa, vn[2]*vn[1]*Va - vn[0]*Sa, vn[2]*vn[2]*Va + Ca, 0.0 );
416 V4set( &Fr[12], 0.0, 0.0, 0.0, 1.0);
420 template<
class T1,
class T2>
421 inline void V6set(T1 *Vr, T2 x, T2 y, T2 z,T2 nx,T2 ny,T2 nz)
423 Vr[0] = (T1)x; Vr[1] = (T1)y; Vr[2] = (T1)z;
424 Vr[3] = (T1)nx; Vr[4] = (T1)ny; Vr[5] = (T1)nz;
428 template<
class T1,
class T2>
429 inline void V6cV6(T1 *Vr,
const T2 *V)
431 Vr[0] = (T1)V[0]; Vr[1] = (T1)V[1]; Vr[2] = (T1)V[2];
432 Vr[3] = (T1)V[3]; Vr[4] = (T1)V[4]; Vr[5] = (T1)V[5];
436 template<
class T1,
class T2,
class T3>
437 inline void V6mV6(T1 *Vr,
const T2 *V1,
const T3 *V2)
439 Vr[0] = (T1)(V1[0] - V2[0]);
440 Vr[1] = (T1)(V1[1] - V2[1]);
441 Vr[2] = (T1)(V1[2] - V2[2]);
442 Vr[3] = (T1)(V1[3] - V2[3]);
443 Vr[4] = (T1)(V1[4] - V2[4]);
444 Vr[5] = (T1)(V1[5] - V2[5]);
448 template<
class T1,
class T2>
449 inline T1*
V6mV6(T1 *Vr,
const T2 *V1)
461 template<
class T1,
class T2,
class T3>
462 inline void V6pV6(T1 *Vr,
const T2 *V1,
const T3 *V2)
464 Vr[0] = (T1)(V1[0] + V2[0]);
465 Vr[1] = (T1)(V1[1] + V2[1]);
466 Vr[2] = (T1)(V1[2] + V2[2]);
467 Vr[3] = (T1)(V1[3] + V2[3]);
468 Vr[4] = (T1)(V1[4] + V2[4]);
469 Vr[5] = (T1)(V1[5] + V2[5]);
473 template<
class T1,
class T2>
474 inline T1*
V6pV6(T1 *Vr,
const T2 *V1)
486 template<
class T1,
class T2,
class T3>
487 inline void V6xS(T1 *Vr,
const T2 *V, T3 s)
489 Vr[0] = (T1)(V[0] * s);
490 Vr[1] = (T1)(V[1] * s);
491 Vr[2] = (T1)(V[2] * s);
492 Vr[3] = (T1)(V[3] * s);
493 Vr[4] = (T1)(V[4] * s);
494 Vr[5] = (T1)(V[5] * s);
498 template<
class T1,
class T2>
511 template<
class T1,
class T2>
514 return (V1[0]*V2[0] + V1[1]*V2[1] + V1[2]*V2[2] + V1[3]*V2[3] + V1[4]*V2[4] + V1[5]*V2[5]);
526 template<
class T1,
class T2>
529 return sqrt((V1[0]-V2[0])*(V1[0]-V2[0]) +
530 (V1[1]-V2[1])*(V1[1]-V2[1]) +
531 (V1[2]-V2[2])*(V1[2]-V2[2]) +
532 (V1[3]-V2[3])*(V1[3]-V2[3]) +
533 (V1[4]-V2[4])*(V1[4]-V2[4]) +
534 (V1[5]-V2[5])*(V1[5]-V2[5]));
538 template<
class T1,
class T2>
541 return (V1[0]-V2[0])*(V1[0]-V2[0]) +
542 (V1[1]-V2[1])*(V1[1]-V2[1]) +
543 (V1[2]-V2[2])*(V1[2]-V2[2]) +
544 (V1[3]-V2[3])*(V1[3]-V2[3]) +
545 (V1[4]-V2[4])*(V1[4]-V2[4]) +
546 (V1[5]-V2[5])*(V1[5]-V2[5]);
550 template<
class T1,
class T2>
558 template <
class T1,
class T2>
574 template <
class Tr,
class T1,
class T2>
575 inline void MxV6(Tr *Vr,
const T1 *M,
const T2 *V)
577 Vr[0] = (Tr)( M[0]*V[0] + M[6]*V[1] + M[12]*V[2] + M[18]*V[3] + M[24]*V[4] + M[30]*V[5]);
578 Vr[1] = (Tr)( M[1]*V[0] + M[7]*V[1] + M[13]*V[2] + M[19]*V[3] + M[25]*V[4] + M[31]*V[5]);
579 Vr[2] = (Tr)( M[2]*V[0] + M[8]*V[1] + M[14]*V[2] + M[20]*V[3] + M[26]*V[4] + M[32]*V[5]);
580 Vr[3] = (Tr)( M[3]*V[0] + M[9]*V[1] + M[15]*V[2] + M[21]*V[3] + M[27]*V[4] + M[33]*V[5]);
581 Vr[4] = (Tr)( M[4]*V[0] + M[10]*V[1] + M[16]*V[2] + M[22]*V[3] + M[28]*V[4] + M[34]*V[5]);
582 Vr[5] = (Tr)( M[5]*V[0] + M[11]*V[1] + M[17]*V[2] + M[23]*V[3] + M[29]*V[4] + M[35]*V[5]);
586 template<
class T1,
class T2>
587 inline void V0cV0(T1 *Vr,
const T2 *V,
const int dim)
589 for(
int i=0; i < dim; ++i) Vr[i] = (T1)V[i];
593 template<
class T1,
class T2,
class T3>
594 inline void V0mV0(T1 *Vr,
const T2 *V1,
const T3 *V2,
const int dim)
596 for(
int i=0; i < dim; ++i) Vr[i] = (T1) (V1[i] - V2[i]);
600 template<
class T1,
class T2>
601 inline T1*
V0mV0(T1 *Vr,
const T2 *V1,
const int dim)
603 for(
int i=0; i < dim; ++i) Vr[i] -= (T1) V1[i];
608 template<
class T1,
class T2,
class T3>
609 inline void V0pV0(T1 *Vr,
const T2 *V1,
const T3 *V2,
const int dim)
611 for(
int i=0; i < dim; ++i) Vr[i] = (T1) (V1[i] + V2[i]);
615 template<
class T1,
class T2>
616 inline T1*
V0pV0(T1 *Vr,
const T2 *V1,
const int dim)
618 for(
int i=0; i < dim; ++i) Vr[i] += (T1) V1[i];
623 template<
class T1,
class T2,
class T3>
624 inline void V0xS(T1 *Vr,
const T2 *V, T3 s,
const int dim)
626 for(
int i=0; i < dim; ++i) Vr[i] = (T1) (V[i]*s);
630 template<
class T1,
class T2>
631 inline T1*
V0xS(T1 *Vr, T2 s,
const int dim)
633 for(
int i=0; i < dim; ++i) Vr[i] *= s;
638 template<
class T1,
class T2>
639 inline T1
V0dotV0(
const T1 *V1,
const T2 *V2,
const int dim)
642 for(
int i=0; i < dim; ++i)
644 sum += V1[i] * V2[i];
658 template<
class T1,
class T2>
659 inline T1
V0distV0(
const T1 *V1,
const T2 *V2,
const int dim)
662 for(
int i=0; i < dim; ++i)
664 const T1 dif = V1[i] - V2[i];
671 template<
class T1,
class T2>
672 inline T1
V0sdistV0(
const T1 *V1,
const T2 *V2,
const int dim)
675 for(
int i=0; i < dim; ++i)
677 const T1 dif = V1[i] - V2[i];
684 template<
class T1,
class T2>
685 inline T1
V0angleV0(
const T1 *V1,
const T2 *V2,
const int dim)
692 template <
class T1,
class T2>
704 return V0xS(V,d,dim);