DAVID 3D Scanning SDK for Sprout  1.8.6
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Groups Pages
FastVector.h
Go to the documentation of this file.
1 //=============================================================================
2 // See License in Related Pages
3 //=============================================================================
4 
5 #ifndef __FASTVECTOR_H__
6 #define __FASTVECTOR_H__
7 
8 /*! \file FastVector.h
9  \brief Basic vector/frame operations working on template<>-arrays.
10 */
11 
12 #include <math.h>
13 
14 #ifndef PI
15 const double PI = 3.14159265358979323846; //!< define pi if its not already defined
16 #endif
17 
18 //!set vector (3d)
19 template<class T1,class T2>
20 inline void Vset(T1 *Vr, T2 x, T2 y, T2 z)
21 {
22  Vr[0] = (T1)x; Vr[1] = (T1)y; Vr[2] = (T1)z;
23 }
24 
25 //! copy vector
26 template<class T1,class T2>
27 inline void VcV(T1 *Vr, const T2 *V)
28 {
29  Vr[0] = (T1)V[0]; Vr[1] = (T1)V[1]; Vr[2] = (T1)V[2];
30 }
31 
32 //! subtract vector
33 template<class T1, class T2, class T3>
34 inline void VmV(T1 *Vr, const T2 *V1, const T3 *V2)
35 {
36  Vr[0] = (T1)(V1[0] - V2[0]);
37  Vr[1] = (T1)(V1[1] - V2[1]);
38  Vr[2] = (T1)(V1[2] - V2[2]);
39 }
40 
41 //! substract vector inplace
42 template<class T1, class T2>
43 inline T1* VmV(T1 *Vr, const T2 *V1)
44 {
45  Vr[0] -= (T1)V1[0];
46  Vr[1] -= (T1)V1[1];
47  Vr[2] -= (T1)V1[2];
48  return Vr;
49 }
50 
51 //! add vector
52 template<class T1, class T2, class T3>
53 inline void VpV(T1 *Vr, const T2 *V1, const T3 *V2)
54 {
55  Vr[0] = (T1)(V1[0] + V2[0]);
56  Vr[1] = (T1)(V1[1] + V2[1]);
57  Vr[2] = (T1)(V1[2] + V2[2]);
58 }
59 
60 //! add vector inplace
61 template<class T1, class T2>
62 inline T1* VpV(T1 *Vr, const T2 *V1)
63 {
64  Vr[0] += (T1)V1[0];
65  Vr[1] += (T1)V1[1];
66  Vr[2] += (T1)V1[2];
67  return Vr;
68 }
69 
70 //! scale vector
71 template<class T1, class T2, class T3>
72 inline void VxS(T1 *Vr, const T2 *V, T3 s)
73 {
74  Vr[0] = (T1)(V[0] * s);
75  Vr[1] = (T1)(V[1] * s);
76  Vr[2] = (T1)(V[2] * s);
77 }
78 
79 //! scale vector inplace
80 template<class T1, class T2>
81 inline T1* VxS(T1 *Vr, T2 s)
82 {
83  Vr[0] *= s;
84  Vr[1] *= s;
85  Vr[2] *= s;
86  return Vr;
87 }
88 
89 //! cross(vector)-product
90 template<class T1, class T2, class T3>
91 inline void VcrossV(T1 *Vr, const T2 *V1, const T3 *V2)
92 {
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]);
96 }
97 
98 //! dot-product
99 template<class T1, class T2>
100 inline T1 VdotV(const T1 *V1, const T2 *V2)
101 {
102  return (V1[0]*V2[0] + V1[1]*V2[1] + V1[2]*V2[2]);
103 }
104 
105 //! length of a vector
106 template<class T1>
107 inline T1 Vlength(const T1 *V)
108 {
109  return sqrt(VdotV(V,V));
110 }
111 
112 
113 //! distance between two vectors (=length of difference vector)
114 template<class T1,class T2>
115 inline T1 VdistV(const T1 *V1, const T2 *V2)
116 {
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]) );
120 }
121 
122 //! square distance between two vectors (=length of difference vector)
123 template<class T1,class T2>
124 inline T1 VsdistV(const T1 *V1, const T2 *V2)
125 {
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]);
129 }
130 
131 //! cosine of angle between two vectors
132 template<class T1,class T2>
133 inline T1 VcosV(const T1 *V1, const T2 *V2)
134 {
135  return VdotV(V1,V2) / sqrt(VdotV(V1,V1)*VdotV(V2,V2));
136 }
137 
138 //! sinus of angle between two vectors
139 template<class T1,class T2>
140 inline T1 VsinV(const T1 *V1, const T2 *V2)
141 {
142  T1 tmpV[3];
143  VcrossV(tmpV,V1,V2);
144  return Vlength(tmpV) / sqrt(VdotV(V1,V1)*VdotV(V2,V2));
145 }
146 
147 
148 //!angle between two vectors
149 template<class T1, class T2>
150 inline T1 VangleV(const T1 *V1, const T2 *V2)
151 {
152  return acos(VcosV(V1,V2));
153 }
154 
155 //! calculates (sine,cosine)-vector of the angle between two given vectors
156 template <class T1, class T2, class T3>
157 inline void VV2SC(T1 *SC,const T2* V1,const T3* V2)
158 {
159  T1 d=sqrt(VdotV(V1,V1)*VdotV(V2,V2));
160  T1 tmpV[3];
161  VcrossV(tmpV,V1,V2);
162  SC[0] = Vlength(tmpV)/d; //sine
163  SC[1] = VdotV(V1,V2)/d; //cosine
164 }
165 
166 //! Calculates (sin(a+b),cos(a+b)) out of (sin(a),cos(a)) and (sin(b),cos(b))
167 template <class T1,class T2, class T3>
168 inline void SCpSC(T1 *SC_sum,const T2* SC_a, const T3* SC_b)
169 {
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];
172 }
173 
174 //! Calculates (sin(a+b),cos(a+b)) out of (sin(a),cos(a)) and (sin(b),cos(b)) inplace
175 template <class T1,class T2>
176 inline T1* SCpSC(T1* SC_r, const T2* SC_b)
177 {
178  T1 SC_r0 = SC_r[0];
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];
181  return SC_r;
182 }
183 
184 //! Calculates (sin(a),cos(a))
185 template <class T1, class T2>
186 inline void Angle2SC(T1 *SC, T2 angle)
187 {
188  SC[0] = sin(angle);
189  SC[1] = cos(angle);
190 }
191 
192 //! Calculates (sin(a-b),cos(a-b)) out of (sin(a),cos(a)) and (sin(b),cos(b))
193 template <class T1,class T2, class T3>
194 inline void SCmSC(T1 *SC_diff,const T2* SC_a, const T3* SC_b)
195 {
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];
198 }
199 
200 //! Calculates (sin(2*a),cos(2*a) out of (sin(a),cos(a))
201 template <class T1,class T2, class T3>
202 inline void SCdouble(T1 *SC_double,const T2* SC_a)
203 {
204  SC_double[0] = 2*SC_a[0]*SC_a[1];
205  SC_double[1] = 2*SC_a[1]*SC_a[1]-1;
206 }
207 
208 //! normalize vector
209 template <class T1, class T2>
210 inline void Vnormalize(T1 *Vr,const T2 *V)
211 {
212  T1 d = (T1)1.0 / Vlength(V);
213  VxS(Vr,V,d);
214 }
215 
216 //! normalize vector inplace
217 template <class T1>
218 inline T1* Vnormalize(T1 *V)
219 {
220  T1 d = (T1)1.0 / Vlength(V);
221  return VxS(V,d);
222 }
223 
224 //! normalize vector inplace but only if length is != 0
225 template <class T1>
226 inline T1* VnormalizeIf(T1 *V)
227 {
228  T1 length = (T1)Vlength(V);
229  if (length>0) VxS(V,T1(1)/length);
230  return V;
231 }
232 
233 //! determinant of matrix (in columns or rows)
234 template<class T1>
235 inline T1 detVVV(const T1 *V1, const T1 *V2, const T1 *V3)
236 {
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]);
239 
240 }
241 
242 //! determinant of matrix (in single entries)
243 template<class T1>
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)
247 {
248  return (a00*a11*a22) + (a02*a10*a21) + (a01*a12*a20) -
249  (a02*a11*a20) - (a00*a12*a21) - (a01*a10*a22);
250 }
251 
252 //! frame-vector product
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)
255 {
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);
259 }
260 
261 //! frame-vector product inplace
262 template <class Tr, class T1, class T3>
263 inline Tr* FxV( const T1 *F, Tr *V, const T3 V3)
264 {
265  Tr Vr[3];
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);
269  VcV( V, Vr);
270  return V;
271 }
272 
273 //! set vector4
274 template<class Tr,class T1>
275 inline void V4set(Tr *Vr, T1 x, T1 y, T1 z, T1 h)
276 {
277  Vr[0] = (Tr)x;
278  Vr[1] = (Tr)y;
279  Vr[2] = (Tr)z;
280  Vr[3] = (Tr)h;
281 }
282 
283 //! copy vector4
284 template <class Tr, class T1>
285 inline void V4cV4( Tr *Vr, const T1 *V)
286 {
287  Vr[0] = (Tr) V[0];
288  Vr[1] = (Tr) V[1];
289  Vr[2] = (Tr) V[2];
290  Vr[3] = (Tr) V[3];
291 }
292 
293 //! copy frame
294 template<class Tr,class T1>
295 inline void FcF( Tr *Fr, const T1 *F)
296 {
297  V4cV4( &Fr[0], &F[0] );
298  V4cV4( &Fr[4], &F[4] );
299  V4cV4( &Fr[8], &F[8] );
300  V4cV4( &Fr[12],&F[12]);
301 }
302 
303 //! frame-vector4 product
304 template <class Tr, class T1, class T2>
305 inline void FxV4( Tr *Vr, const T1 *F, const T2 *V)
306 {
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]);
311 }
312 
313 //! frame-frame product
314 template< class Tr, class T1, class T2>
315 inline void FxF( Tr *Fr, const T1 *F1, const T2 *F2)
316 {
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]);
321 }
322 
323 //! frame inversion (only for orthonormed frames!!)
324 template <class Tr>
325 inline void Finverse( Tr *Fr)
326 {
327  //neue position berechnen
328  Tr old_pos[4];
329  V4cV4( old_pos, &Fr[12]);
330 
331  Fr[12] = - VdotV( old_pos, &Fr[0]);
332  Fr[13] = - VdotV( old_pos, &Fr[4]);
333  Fr[14] = - VdotV( old_pos, &Fr[8]);
334 
335  //neue rotation ist einfach transponierte alte
336  Tr old_ny = Fr[1];
337  Tr old_nz = Fr[2];
338  Tr old_oz = Fr[6];
339  //nx bleibt
340  Fr[1] = Fr[4];
341  Fr[2] = Fr[8];
342  Fr[4] = old_ny;
343  //oy bleibt
344  Fr[6] = Fr[9];
345  Fr[8] = old_nz;
346  Fr[9] = old_oz;
347  //az bleibt
348 }
349 
350 //! load identity frame
351 template <class Tr>
352 inline void Fidentity( Tr *Fr)
353 {
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);
358 }
359 
360 
361 //! load x-rotation frame
362 template <class Tr, class T1>
363 inline void Frotx( Tr *Fr, const T1 angle)
364 {
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);
371 }
372 
373 
374 //! load y-rotation frame
375 template <class Tr, class T1>
376 inline void Froty( Tr *Fr, const T1 angle)
377 {
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);
384 }
385 
386 
387 //! load z-rotation frame
388 template <class Tr, class T1>
389 inline void Frotz( Tr *Fr, const T1 angle)
390 {
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);
397 }
398 
399 
400 //! load vector-rotation frame
401 template <class Tr, class T1, class T2>
402 inline void Frotv( Tr *Fr, const T1 *V, const T2 angle)
403 {
404  // see Fu, Gonzalez, Lee, ROBOTICS, p. 21
405  double vn[3];
406  VcV( vn, V);
407  Vnormalize(vn);
408 
409  double Sa = sin(angle);
410  double Ca = cos(angle);
411  double Va = 1.0 - Ca;
412 
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);
417 }
418 
419 //! set 6d-vector
420 template<class T1,class T2>
421 inline void V6set(T1 *Vr, T2 x, T2 y, T2 z,T2 nx,T2 ny,T2 nz)
422 {
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;
425 }
426 
427 //! copy 6d-vector
428 template<class T1,class T2>
429 inline void V6cV6(T1 *Vr, const T2 *V)
430 {
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];
433 }
434 
435 //! subtract 6d-vector
436 template<class T1, class T2, class T3>
437 inline void V6mV6(T1 *Vr, const T2 *V1, const T3 *V2)
438 {
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]);
445 }
446 
447 //! substract 6d-vector inplace
448 template<class T1, class T2>
449 inline T1* V6mV6(T1 *Vr, const T2 *V1)
450 {
451  Vr[0] -= (T1)V1[0];
452  Vr[1] -= (T1)V1[1];
453  Vr[2] -= (T1)V1[2];
454  Vr[3] -= (T1)V1[3];
455  Vr[4] -= (T1)V1[4];
456  Vr[5] -= (T1)V1[5];
457  return Vr;
458 }
459 
460 //! add 6d-vector
461 template<class T1, class T2, class T3>
462 inline void V6pV6(T1 *Vr, const T2 *V1, const T3 *V2)
463 {
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]);
470 }
471 
472 //! add 6d-vector inplace
473 template<class T1, class T2>
474 inline T1* V6pV6(T1 *Vr, const T2 *V1)
475 {
476  Vr[0] += (T1)V1[0];
477  Vr[1] += (T1)V1[1];
478  Vr[2] += (T1)V1[2];
479  Vr[3] += (T1)V1[3];
480  Vr[4] += (T1)V1[4];
481  Vr[5] += (T1)V1[5];
482  return Vr;
483 }
484 
485 //! scale 6d-vector
486 template<class T1, class T2, class T3>
487 inline void V6xS(T1 *Vr, const T2 *V, T3 s)
488 {
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);
495 }
496 
497 //! scale 6d-vector inplace
498 template<class T1, class T2>
499 inline T1* V6xS(T1 *Vr, T2 s)
500 {
501  Vr[0] *= s;
502  Vr[1] *= s;
503  Vr[2] *= s;
504  Vr[3] *= s;
505  Vr[4] *= s;
506  Vr[5] *= s;
507  return Vr;
508 }
509 
510 //! 6d-vector dot-product
511 template<class T1, class T2>
512 inline T1 V6dotV6(const T1 *V1, const T2 *V2)
513 {
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]);
515 }
516 
517 //! length of a 6d-vector
518 template<class T1>
519 inline T1 V6length(const T1 *V)
520 {
521  return sqrt(V6dotV6(V,V));
522 }
523 
524 
525 //! distance between two 6d-vectors (=length of difference vector)
526 template<class T1,class T2>
527 inline T1 V6distV6(const T1 *V1, const T2 *V2)
528 {
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]));
535 }
536 
537 //! square distance between two 6d-vectors (=length of difference vector)
538 template<class T1,class T2>
539 inline T1 V6sdistV6(const T1 *V1, const T2 *V2)
540 {
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]);
547 }
548 
549 //!angle between two 6d-vectors
550 template<class T1, class T2>
551 inline T1 V6angleV6(const T1 *V1, const T2 *V2)
552 {
553  return acos( V6dotV6(V1,V2) / sqrt(V6dotV6(V1,V1)*V6dotV6(V2,V2)) );
554 }
555 
556 
557 //! normalize 6d-vector
558 template <class T1, class T2>
559 inline void V6normalize(T1 *Vr,const T2 *V)
560 {
561  T1 d = (T1)1.0 / V6length(V);
562  V6xS(Vr,V,d);
563 }
564 
565 //! normalize 6d-vector inplace
566 template <class T1>
567 inline T1* V6normalize(T1 *V)
568 {
569  T1 d = (T1)1.0 / V6length(V);
570  return V6xS(V,d);
571 }
572 
573 //! 6x6Matrix-6d-vector product
574 template <class Tr,class T1, class T2>
575 inline void MxV6(Tr *Vr,const T1 *M,const T2 *V)
576 {
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]);
583 }
584 
585 //! copy vector
586 template<class T1,class T2>
587 inline void V0cV0(T1 *Vr, const T2 *V, const int dim)
588 {
589  for(int i=0; i < dim; ++i) Vr[i] = (T1)V[i];
590 }
591 
592 //! subtract vector
593 template<class T1, class T2, class T3>
594 inline void V0mV0(T1 *Vr, const T2 *V1, const T3 *V2, const int dim)
595 {
596  for(int i=0; i < dim; ++i) Vr[i] = (T1) (V1[i] - V2[i]);
597 }
598 
599 //! substract vector inplace
600 template<class T1, class T2>
601 inline T1* V0mV0(T1 *Vr, const T2 *V1, const int dim)
602 {
603  for(int i=0; i < dim; ++i) Vr[i] -= (T1) V1[i];
604  return Vr;
605 }
606 
607 //! add vector
608 template<class T1, class T2, class T3>
609 inline void V0pV0(T1 *Vr, const T2 *V1, const T3 *V2, const int dim)
610 {
611  for(int i=0; i < dim; ++i) Vr[i] = (T1) (V1[i] + V2[i]);
612 }
613 
614 //! add vector inplace
615 template<class T1, class T2>
616 inline T1* V0pV0(T1 *Vr, const T2 *V1, const int dim)
617 {
618  for(int i=0; i < dim; ++i) Vr[i] += (T1) V1[i];
619  return Vr;
620 }
621 
622 //! scale vector
623 template<class T1, class T2, class T3>
624 inline void V0xS(T1 *Vr, const T2 *V, T3 s, const int dim)
625 {
626  for(int i=0; i < dim; ++i) Vr[i] = (T1) (V[i]*s);
627 }
628 
629 //! scale vector inplace
630 template<class T1, class T2>
631 inline T1* V0xS(T1 *Vr, T2 s, const int dim)
632 {
633  for(int i=0; i < dim; ++i) Vr[i] *= s;
634  return Vr;
635 }
636 
637 //! vector dot-product
638 template<class T1, class T2>
639 inline T1 V0dotV0(const T1 *V1, const T2 *V2, const int dim)
640 {
641  T1 sum = 0;
642  for(int i=0; i < dim; ++i)
643  {
644  sum += V1[i] * V2[i];
645  }
646  return sum;
647 }
648 
649 //! length of a vector
650 template<class T1>
651 inline T1 V0length(const T1 *V, const int dim)
652 {
653  return sqrt(V0dotV0(V,V,dim));
654 }
655 
656 
657 //! distance between two vectors (=length of difference vector)
658 template<class T1,class T2>
659 inline T1 V0distV0(const T1 *V1, const T2 *V2, const int dim)
660 {
661  T1 sum = 0;
662  for(int i=0; i < dim; ++i)
663  {
664  const T1 dif = V1[i] - V2[i];
665  sum += dif*dif;
666  }
667  return sqrt(sum);
668 }
669 
670 //! square distance between two vectors (=length of difference vector)
671 template<class T1,class T2>
672 inline T1 V0sdistV0(const T1 *V1, const T2 *V2, const int dim)
673 {
674  T1 sum = 0;
675  for(int i=0; i < dim; ++i)
676  {
677  const T1 dif = V1[i] - V2[i];
678  sum += dif*dif;
679  }
680  return sum;
681 }
682 
683 //!angle between two vectors
684 template<class T1, class T2>
685 inline T1 V0angleV0(const T1 *V1, const T2 *V2, const int dim)
686 {
687  return acos(V0dotV0(V1,V2,dim) / sqrt(V0dotV0(V1,V1,dim)*V0dotV0(V2,V2,dim)));
688 }
689 
690 
691 //! normalize vector
692 template <class T1, class T2>
693 inline void V0normalize(T1 *Vr,const T2 *V, const int dim)
694 {
695  T1 d = (T1) 1.0 / V0length(V);
696  V0xS(Vr,V,d,dim);
697 }
698 
699 //! normalize vector inplace
700 template <class T1>
701 inline T1* V0normalize(T1 *V, const int dim)
702 {
703  T1 d = (T1)1.0 / V0length(V,dim);
704  return V0xS(V,d,dim);
705 }
706 
707 #endif