merging trunk 19093:19274
[blender.git] / extern / bullet2 / src / LinearMath / btVector3.h
1 /*
2 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
3
4 This software is provided 'as-is', without any express or implied warranty.
5 In no event will the authors be held liable for any damages arising from the use of this software.
6 Permission is granted to anyone to use this software for any purpose, 
7 including commercial applications, and to alter it and redistribute it freely, 
8 subject to the following restrictions:
9
10 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
12 3. This notice may not be removed or altered from any source distribution.
13 */
14
15
16
17 #ifndef SIMD__VECTOR3_H
18 #define SIMD__VECTOR3_H
19
20
21 #include "btScalar.h"
22 #include "btScalar.h"
23 #include "btMinMax.h"
24 /**@brief btVector3 can be used to represent 3D points and vectors.
25  * It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
26  * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
27  */
28
29 ATTRIBUTE_ALIGNED16(class) btVector3
30 {
31 public:
32
33 #if defined (__SPU__) && defined (__CELLOS_LV2__)
34         union {
35                 vec_float4 mVec128;
36                 btScalar        m_floats[4];
37         };
38 public:
39         vec_float4      get128() const
40         {
41                 return mVec128;
42         }
43 public:
44 #else //__CELLOS_LV2__ __SPU__
45 #ifdef BT_USE_SSE // WIN32
46         union {
47                 __m128 mVec128;
48                 btScalar        m_floats[4];
49         };
50         SIMD_FORCE_INLINE       __m128  get128() const
51         {
52                 return mVec128;
53         }
54         SIMD_FORCE_INLINE       void    set128(__m128 v128)
55         {
56                 mVec128 = v128;
57         }
58 #else
59         btScalar        m_floats[4];
60 #endif
61 #endif //__CELLOS_LV2__ __SPU__
62
63         public:
64
65   /**@brief No initialization constructor */
66         SIMD_FORCE_INLINE btVector3() {}
67
68  
69         
70   /**@brief Constructor from scalars 
71    * @param x X value
72    * @param y Y value 
73    * @param z Z value 
74    */
75         SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z)
76         {
77                 m_floats[0] = x;
78                 m_floats[1] = y;
79                 m_floats[2] = z;
80                 m_floats[3] = btScalar(0.);
81         }
82
83         
84 /**@brief Add a vector to this one 
85  * @param The vector to add to this one */
86         SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v)
87         {
88
89                 m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2];
90                 return *this;
91         }
92
93
94   /**@brief Subtract a vector from this one
95    * @param The vector to subtract */
96         SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) 
97         {
98                 m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2];
99                 return *this;
100         }
101   /**@brief Scale the vector
102    * @param s Scale factor */
103         SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
104         {
105                 m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s;
106                 return *this;
107         }
108
109   /**@brief Inversely scale the vector 
110    * @param s Scale factor to divide by */
111         SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) 
112         {
113                 btFullAssert(s != btScalar(0.0));
114                 return *this *= btScalar(1.0) / s;
115         }
116
117   /**@brief Return the dot product
118    * @param v The other vector in the dot product */
119         SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
120         {
121                 return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2];
122         }
123
124   /**@brief Return the length of the vector squared */
125         SIMD_FORCE_INLINE btScalar length2() const
126         {
127                 return dot(*this);
128         }
129
130   /**@brief Return the length of the vector */
131         SIMD_FORCE_INLINE btScalar length() const
132         {
133                 return btSqrt(length2());
134         }
135
136   /**@brief Return the distance squared between the ends of this and another vector
137    * This is symantically treating the vector like a point */
138         SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;
139
140   /**@brief Return the distance between the ends of this and another vector
141    * This is symantically treating the vector like a point */
142         SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const;
143
144   /**@brief Normalize this vector 
145    * x^2 + y^2 + z^2 = 1 */
146         SIMD_FORCE_INLINE btVector3& normalize() 
147         {
148                 return *this /= length();
149         }
150
151   /**@brief Return a normalized version of this vector */
152         SIMD_FORCE_INLINE btVector3 normalized() const;
153
154   /**@brief Rotate this vector 
155    * @param wAxis The axis to rotate about 
156    * @param angle The angle to rotate by */
157         SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle );
158
159   /**@brief Return the angle between this and another vector
160    * @param v The other vector */
161         SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const 
162         {
163                 btScalar s = btSqrt(length2() * v.length2());
164                 btFullAssert(s != btScalar(0.0));
165                 return btAcos(dot(v) / s);
166         }
167   /**@brief Return a vector will the absolute values of each element */
168         SIMD_FORCE_INLINE btVector3 absolute() const 
169         {
170                 return btVector3(
171                         btFabs(m_floats[0]), 
172                         btFabs(m_floats[1]), 
173                         btFabs(m_floats[2]));
174         }
175   /**@brief Return the cross product between this and another vector 
176    * @param v The other vector */
177         SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
178         {
179                 return btVector3(
180                         m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1],
181                         m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
182                         m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
183         }
184
185         SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
186         {
187                 return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + 
188                         m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + 
189                         m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
190         }
191
192   /**@brief Return the axis with the smallest value 
193    * Note return values are 0,1,2 for x, y, or z */
194         SIMD_FORCE_INLINE int minAxis() const
195         {
196                 return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
197         }
198
199   /**@brief Return the axis with the largest value 
200    * Note return values are 0,1,2 for x, y, or z */
201         SIMD_FORCE_INLINE int maxAxis() const 
202         {
203                 return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
204         }
205
206         SIMD_FORCE_INLINE int furthestAxis() const
207         {
208                 return absolute().minAxis();
209         }
210
211         SIMD_FORCE_INLINE int closestAxis() const 
212         {
213                 return absolute().maxAxis();
214         }
215
216         SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
217         {
218                 btScalar s = btScalar(1.0) - rt;
219                 m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
220                 m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
221                 m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
222                 //don't do the unused w component
223                 //              m_co[3] = s * v0[3] + rt * v1[3];
224         }
225
226   /**@brief Return the linear interpolation between this and another vector 
227    * @param v The other vector 
228    * @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
229         SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const 
230         {
231                 return btVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
232                         m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
233                         m_floats[2] + (v.m_floats[2] -m_floats[2]) * t);
234         }
235
236   /**@brief Elementwise multiply this vector by the other 
237    * @param v The other vector */
238         SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
239         {
240                 m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2];
241                 return *this;
242         }
243
244          /**@brief Return the x value */
245                 SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
246   /**@brief Return the y value */
247                 SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
248   /**@brief Return the z value */
249                 SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
250   /**@brief Set the x value */
251                 SIMD_FORCE_INLINE void  setX(btScalar x) { m_floats[0] = x;};
252   /**@brief Set the y value */
253                 SIMD_FORCE_INLINE void  setY(btScalar y) { m_floats[1] = y;};
254   /**@brief Set the z value */
255                 SIMD_FORCE_INLINE void  setZ(btScalar z) {m_floats[2] = z;};
256   /**@brief Set the w value */
257                 SIMD_FORCE_INLINE void  setW(btScalar w) { m_floats[3] = w;};
258   /**@brief Return the x value */
259                 SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
260   /**@brief Return the y value */
261                 SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
262   /**@brief Return the z value */
263                 SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
264   /**@brief Return the w value */
265                 SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
266
267         //SIMD_FORCE_INLINE btScalar&       operator[](int i)       { return (&m_floats[0])[i]; }      
268         //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
269         ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
270         SIMD_FORCE_INLINE       operator       btScalar *()       { return &m_floats[0]; }
271         SIMD_FORCE_INLINE       operator const btScalar *() const { return &m_floats[0]; }
272
273         SIMD_FORCE_INLINE       bool    operator==(const btVector3& other) const
274         {
275                 return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0]));
276         }
277
278         SIMD_FORCE_INLINE       bool    operator!=(const btVector3& other) const
279         {
280                 return !(*this == other);
281         }
282
283          /**@brief Set each element to the max of the current values and the values of another btVector3
284    * @param other The other btVector3 to compare with 
285    */
286                 SIMD_FORCE_INLINE void  setMax(const btVector3& other)
287                 {
288                         btSetMax(m_floats[0], other.m_floats[0]);
289                         btSetMax(m_floats[1], other.m_floats[1]);
290                         btSetMax(m_floats[2], other.m_floats[2]);
291                         btSetMax(m_floats[3], other.w());
292                 }
293   /**@brief Set each element to the min of the current values and the values of another btVector3
294    * @param other The other btVector3 to compare with 
295    */
296                 SIMD_FORCE_INLINE void  setMin(const btVector3& other)
297                 {
298                         btSetMin(m_floats[0], other.m_floats[0]);
299                         btSetMin(m_floats[1], other.m_floats[1]);
300                         btSetMin(m_floats[2], other.m_floats[2]);
301                         btSetMin(m_floats[3], other.w());
302                 }
303
304                 SIMD_FORCE_INLINE void  setValue(const btScalar& x, const btScalar& y, const btScalar& z)
305                 {
306                         m_floats[0]=x;
307                         m_floats[1]=y;
308                         m_floats[2]=z;
309                         m_floats[3] = 0.f;
310                 }
311
312                 void    getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const
313                 {
314                         v0->setValue(0.         ,-z()           ,y());
315                         v1->setValue(z()        ,0.                     ,-x());
316                         v2->setValue(-y()       ,x()    ,0.);
317                 }
318
319 };
320
321 /**@brief Return the sum of two vectors (Point symantics)*/
322 SIMD_FORCE_INLINE btVector3 
323 operator+(const btVector3& v1, const btVector3& v2) 
324 {
325         return btVector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]);
326 }
327
328 /**@brief Return the elementwise product of two vectors */
329 SIMD_FORCE_INLINE btVector3 
330 operator*(const btVector3& v1, const btVector3& v2) 
331 {
332         return btVector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]);
333 }
334
335 /**@brief Return the difference between two vectors */
336 SIMD_FORCE_INLINE btVector3 
337 operator-(const btVector3& v1, const btVector3& v2)
338 {
339         return btVector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]);
340 }
341 /**@brief Return the negative of the vector */
342 SIMD_FORCE_INLINE btVector3 
343 operator-(const btVector3& v)
344 {
345         return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
346 }
347
348 /**@brief Return the vector scaled by s */
349 SIMD_FORCE_INLINE btVector3 
350 operator*(const btVector3& v, const btScalar& s)
351 {
352         return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
353 }
354
355 /**@brief Return the vector scaled by s */
356 SIMD_FORCE_INLINE btVector3 
357 operator*(const btScalar& s, const btVector3& v)
358
359         return v * s; 
360 }
361
362 /**@brief Return the vector inversely scaled by s */
363 SIMD_FORCE_INLINE btVector3
364 operator/(const btVector3& v, const btScalar& s)
365 {
366         btFullAssert(s != btScalar(0.0));
367         return v * (btScalar(1.0) / s);
368 }
369
370 /**@brief Return the vector inversely scaled by s */
371 SIMD_FORCE_INLINE btVector3
372 operator/(const btVector3& v1, const btVector3& v2)
373 {
374         return btVector3(v1.m_floats[0] / v2.m_floats[0],v1.m_floats[1] / v2.m_floats[1],v1.m_floats[2] / v2.m_floats[2]);
375 }
376
377 /**@brief Return the dot product between two vectors */
378 SIMD_FORCE_INLINE btScalar 
379 dot(const btVector3& v1, const btVector3& v2) 
380
381         return v1.dot(v2); 
382 }
383
384
385 /**@brief Return the distance squared between two vectors */
386 SIMD_FORCE_INLINE btScalar
387 distance2(const btVector3& v1, const btVector3& v2) 
388
389         return v1.distance2(v2); 
390 }
391
392
393 /**@brief Return the distance between two vectors */
394 SIMD_FORCE_INLINE btScalar
395 distance(const btVector3& v1, const btVector3& v2) 
396
397         return v1.distance(v2); 
398 }
399
400 /**@brief Return the angle between two vectors */
401 SIMD_FORCE_INLINE btScalar
402 angle(const btVector3& v1, const btVector3& v2) 
403
404         return v1.angle(v2); 
405 }
406
407 /**@brief Return the cross product of two vectors */
408 SIMD_FORCE_INLINE btVector3 
409 cross(const btVector3& v1, const btVector3& v2) 
410
411         return v1.cross(v2); 
412 }
413
414 SIMD_FORCE_INLINE btScalar
415 triple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
416 {
417         return v1.triple(v2, v3);
418 }
419
420 /**@brief Return the linear interpolation between two vectors
421  * @param v1 One vector 
422  * @param v2 The other vector 
423  * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */
424 SIMD_FORCE_INLINE btVector3 
425 lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
426 {
427         return v1.lerp(v2, t);
428 }
429
430
431
432 SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
433 {
434         return (v - *this).length2();
435 }
436
437 SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
438 {
439         return (v - *this).length();
440 }
441
442 SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
443 {
444         return *this / length();
445
446
447 SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar angle )
448 {
449         // wAxis must be a unit lenght vector
450
451         btVector3 o = wAxis * wAxis.dot( *this );
452         btVector3 x = *this - o;
453         btVector3 y;
454
455         y = wAxis.cross( *this );
456
457         return ( o + x * btCos( angle ) + y * btSin( angle ) );
458 }
459
460 class btVector4 : public btVector3
461 {
462 public:
463
464         SIMD_FORCE_INLINE btVector4() {}
465
466
467         SIMD_FORCE_INLINE btVector4(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) 
468                 : btVector3(x,y,z)
469         {
470                 m_floats[3] = w;
471         }
472
473
474         SIMD_FORCE_INLINE btVector4 absolute4() const 
475         {
476                 return btVector4(
477                         btFabs(m_floats[0]), 
478                         btFabs(m_floats[1]), 
479                         btFabs(m_floats[2]),
480                         btFabs(m_floats[3]));
481         }
482
483
484
485         btScalar        getW() const { return m_floats[3];}
486
487
488                 SIMD_FORCE_INLINE int maxAxis4() const
489         {
490                 int maxIndex = -1;
491                 btScalar maxVal = btScalar(-1e30);
492                 if (m_floats[0] > maxVal)
493                 {
494                         maxIndex = 0;
495                         maxVal = m_floats[0];
496                 }
497                 if (m_floats[1] > maxVal)
498                 {
499                         maxIndex = 1;
500                         maxVal = m_floats[1];
501                 }
502                 if (m_floats[2] > maxVal)
503                 {
504                         maxIndex = 2;
505                         maxVal =m_floats[2];
506                 }
507                 if (m_floats[3] > maxVal)
508                 {
509                         maxIndex = 3;
510                         maxVal = m_floats[3];
511                 }
512                 
513                 
514                 
515
516                 return maxIndex;
517
518         }
519
520
521         SIMD_FORCE_INLINE int minAxis4() const
522         {
523                 int minIndex = -1;
524                 btScalar minVal = btScalar(1e30);
525                 if (m_floats[0] < minVal)
526                 {
527                         minIndex = 0;
528                         minVal = m_floats[0];
529                 }
530                 if (m_floats[1] < minVal)
531                 {
532                         minIndex = 1;
533                         minVal = m_floats[1];
534                 }
535                 if (m_floats[2] < minVal)
536                 {
537                         minIndex = 2;
538                         minVal =m_floats[2];
539                 }
540                 if (m_floats[3] < minVal)
541                 {
542                         minIndex = 3;
543                         minVal = m_floats[3];
544                 }
545                 
546                 return minIndex;
547
548         }
549
550
551         SIMD_FORCE_INLINE int closestAxis4() const 
552         {
553                 return absolute4().maxAxis4();
554         }
555
556         
557  
558
559   /**@brief Set x,y,z and zero w 
560    * @param x Value of x
561    * @param y Value of y
562    * @param z Value of z
563    */
564                 
565
566 /*              void getValue(btScalar *m) const 
567                 {
568                         m[0] = m_floats[0];
569                         m[1] = m_floats[1];
570                         m[2] =m_floats[2];
571                 }
572 */
573 /**@brief Set the values 
574    * @param x Value of x
575    * @param y Value of y
576    * @param z Value of z
577    * @param w Value of w
578    */
579                 SIMD_FORCE_INLINE void  setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
580                 {
581                         m_floats[0]=x;
582                         m_floats[1]=y;
583                         m_floats[2]=z;
584                         m_floats[3]=w;
585                 }
586
587
588  
589
590 };
591
592
593 ///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
594 SIMD_FORCE_INLINE void  btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal)
595 {
596         #ifdef BT_USE_DOUBLE_PRECISION
597         unsigned char* dest = (unsigned char*) &destVal;
598         unsigned char* src  = (unsigned char*) &sourceVal;
599         dest[0] = src[7];
600     dest[1] = src[6];
601     dest[2] = src[5];
602     dest[3] = src[4];
603     dest[4] = src[3];
604     dest[5] = src[2];
605     dest[6] = src[1];
606     dest[7] = src[0];
607 #else
608         unsigned char* dest = (unsigned char*) &destVal;
609         unsigned char* src  = (unsigned char*) &sourceVal;
610         dest[0] = src[3];
611     dest[1] = src[2];
612     dest[2] = src[1];
613     dest[3] = src[0];
614 #endif //BT_USE_DOUBLE_PRECISION
615 }
616 ///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
617 SIMD_FORCE_INLINE void  btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec)
618 {
619         for (int i=0;i<4;i++)
620         {
621                 btSwapScalarEndian(sourceVec[i],destVec[i]);
622         }
623
624 }
625
626 ///btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
627 SIMD_FORCE_INLINE void  btUnSwapVector3Endian(btVector3& vector)
628 {
629
630         btVector3       swappedVec;
631         for (int i=0;i<4;i++)
632         {
633                 btSwapScalarEndian(vector[i],swappedVec[i]);
634         }
635         vector = swappedVec;
636 }
637
638 #endif //SIMD__VECTOR3_H