Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/LinearMath/btVector3.h @ 6558

Last change on this file since 6558 was 5781, checked in by rgrieder, 16 years ago

Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).
  • Property svn:eol-style set to native
File size: 17.3 KB
Line 
1/*
2Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
3
4This software is provided 'as-is', without any express or implied warranty.
5In no event will the authors be held liable for any damages arising from the use of this software.
6Permission is granted to anyone to use this software for any purpose,
7including commercial applications, and to alter it and redistribute it freely,
8subject to the following restrictions:
9
101. 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.
112. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
123. 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
29ATTRIBUTE_ALIGNED16(class) btVector3
30{
31public:
32
33#if defined (__SPU__) && defined (__CELLOS_LV2__)
34        union {
35                vec_float4 mVec128;
36                btScalar        m_floats[4];
37        };
38public:
39        vec_float4      get128() const
40        {
41                return mVec128;
42        }
43public:
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)*/
322SIMD_FORCE_INLINE btVector3
323operator+(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 */
329SIMD_FORCE_INLINE btVector3
330operator*(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 */
336SIMD_FORCE_INLINE btVector3
337operator-(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 */
342SIMD_FORCE_INLINE btVector3
343operator-(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 */
349SIMD_FORCE_INLINE btVector3
350operator*(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 */
356SIMD_FORCE_INLINE btVector3
357operator*(const btScalar& s, const btVector3& v)
358{ 
359        return v * s; 
360}
361
362/**@brief Return the vector inversely scaled by s */
363SIMD_FORCE_INLINE btVector3
364operator/(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 */
371SIMD_FORCE_INLINE btVector3
372operator/(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 */
378SIMD_FORCE_INLINE btScalar
379dot(const btVector3& v1, const btVector3& v2) 
380{ 
381        return v1.dot(v2); 
382}
383
384
385/**@brief Return the distance squared between two vectors */
386SIMD_FORCE_INLINE btScalar
387distance2(const btVector3& v1, const btVector3& v2) 
388{ 
389        return v1.distance2(v2); 
390}
391
392
393/**@brief Return the distance between two vectors */
394SIMD_FORCE_INLINE btScalar
395distance(const btVector3& v1, const btVector3& v2) 
396{ 
397        return v1.distance(v2); 
398}
399
400/**@brief Return the angle between two vectors */
401SIMD_FORCE_INLINE btScalar
402angle(const btVector3& v1, const btVector3& v2) 
403{ 
404        return v1.angle(v2); 
405}
406
407/**@brief Return the cross product of two vectors */
408SIMD_FORCE_INLINE btVector3
409cross(const btVector3& v1, const btVector3& v2) 
410{ 
411        return v1.cross(v2); 
412}
413
414SIMD_FORCE_INLINE btScalar
415triple(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) */
424SIMD_FORCE_INLINE btVector3
425lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
426{
427        return v1.lerp(v2, t);
428}
429
430
431
432SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
433{
434        return (v - *this).length2();
435}
436
437SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
438{
439        return (v - *this).length();
440}
441
442SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
443{
444        return *this / length();
445} 
446
447SIMD_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
460class btVector4 : public btVector3
461{
462public:
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
594SIMD_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
617SIMD_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
627SIMD_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
Note: See TracBrowser for help on using the repository browser.