Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

Last change on this file since 12005 was 8393, checked in by rgrieder, 14 years ago

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.
  • Property svn:eol-style set to native
File size: 20.4 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 BT_VECTOR3_H
18#define BT_VECTOR3_H
19
20
21#include "btScalar.h"
22#include "btMinMax.h"
23
24#ifdef BT_USE_DOUBLE_PRECISION
25#define btVector3Data btVector3DoubleData
26#define btVector3DataName "btVector3DoubleData"
27#else
28#define btVector3Data btVector3FloatData
29#define btVector3DataName "btVector3FloatData"
30#endif //BT_USE_DOUBLE_PRECISION
31
32
33
34
35/**@brief btVector3 can be used to represent 3D points and vectors.
36 * 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
37 * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
38 */
39ATTRIBUTE_ALIGNED16(class) btVector3
40{
41public:
42
43#if defined (__SPU__) && defined (__CELLOS_LV2__)
44                btScalar        m_floats[4];
45public:
46        SIMD_FORCE_INLINE const vec_float4&     get128() const
47        {
48                return *((const vec_float4*)&m_floats[0]);
49        }
50public:
51#else //__CELLOS_LV2__ __SPU__
52#ifdef BT_USE_SSE // _WIN32
53        union {
54                __m128 mVec128;
55                btScalar        m_floats[4];
56        };
57        SIMD_FORCE_INLINE       __m128  get128() const
58        {
59                return mVec128;
60        }
61        SIMD_FORCE_INLINE       void    set128(__m128 v128)
62        {
63                mVec128 = v128;
64        }
65#else
66        btScalar        m_floats[4];
67#endif
68#endif //__CELLOS_LV2__ __SPU__
69
70        public:
71
72  /**@brief No initialization constructor */
73        SIMD_FORCE_INLINE btVector3() {}
74
75 
76       
77  /**@brief Constructor from scalars
78   * @param x X value
79   * @param y Y value
80   * @param z Z value
81   */
82        SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z)
83        {
84                m_floats[0] = x;
85                m_floats[1] = y;
86                m_floats[2] = z;
87                m_floats[3] = btScalar(0.);
88        }
89
90       
91/**@brief Add a vector to this one
92 * @param The vector to add to this one */
93        SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v)
94        {
95
96                m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2];
97                return *this;
98        }
99
100
101  /**@brief Subtract a vector from this one
102   * @param The vector to subtract */
103        SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) 
104        {
105                m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2];
106                return *this;
107        }
108  /**@brief Scale the vector
109   * @param s Scale factor */
110        SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
111        {
112                m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s;
113                return *this;
114        }
115
116  /**@brief Inversely scale the vector
117   * @param s Scale factor to divide by */
118        SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) 
119        {
120                btFullAssert(s != btScalar(0.0));
121                return *this *= btScalar(1.0) / s;
122        }
123
124  /**@brief Return the dot product
125   * @param v The other vector in the dot product */
126        SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
127        {
128                return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2];
129        }
130
131  /**@brief Return the length of the vector squared */
132        SIMD_FORCE_INLINE btScalar length2() const
133        {
134                return dot(*this);
135        }
136
137  /**@brief Return the length of the vector */
138        SIMD_FORCE_INLINE btScalar length() const
139        {
140                return btSqrt(length2());
141        }
142
143  /**@brief Return the distance squared between the ends of this and another vector
144   * This is symantically treating the vector like a point */
145        SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;
146
147  /**@brief Return the distance between the ends of this and another vector
148   * This is symantically treating the vector like a point */
149        SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const;
150
151        SIMD_FORCE_INLINE btVector3& safeNormalize() 
152        {
153                btVector3 absVec = this->absolute();
154                int maxIndex = absVec.maxAxis();
155                if (absVec[maxIndex]>0)
156                {
157                        *this /= absVec[maxIndex];
158                        return *this /= length();
159                }
160                setValue(1,0,0);
161                return *this;
162        }
163
164  /**@brief Normalize this vector
165   * x^2 + y^2 + z^2 = 1 */
166        SIMD_FORCE_INLINE btVector3& normalize() 
167        {
168                return *this /= length();
169        }
170
171  /**@brief Return a normalized version of this vector */
172        SIMD_FORCE_INLINE btVector3 normalized() const;
173
174  /**@brief Return a rotated version of this vector
175   * @param wAxis The axis to rotate about
176   * @param angle The angle to rotate by */
177        SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle ) const;
178
179  /**@brief Return the angle between this and another vector
180   * @param v The other vector */
181        SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const 
182        {
183                btScalar s = btSqrt(length2() * v.length2());
184                btFullAssert(s != btScalar(0.0));
185                return btAcos(dot(v) / s);
186        }
187  /**@brief Return a vector will the absolute values of each element */
188        SIMD_FORCE_INLINE btVector3 absolute() const 
189        {
190                return btVector3(
191                        btFabs(m_floats[0]), 
192                        btFabs(m_floats[1]), 
193                        btFabs(m_floats[2]));
194        }
195  /**@brief Return the cross product between this and another vector
196   * @param v The other vector */
197        SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
198        {
199                return btVector3(
200                        m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1],
201                        m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
202                        m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
203        }
204
205        SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
206        {
207                return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + 
208                        m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + 
209                        m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
210        }
211
212  /**@brief Return the axis with the smallest value
213   * Note return values are 0,1,2 for x, y, or z */
214        SIMD_FORCE_INLINE int minAxis() const
215        {
216                return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
217        }
218
219  /**@brief Return the axis with the largest value
220   * Note return values are 0,1,2 for x, y, or z */
221        SIMD_FORCE_INLINE int maxAxis() const 
222        {
223                return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
224        }
225
226        SIMD_FORCE_INLINE int furthestAxis() const
227        {
228                return absolute().minAxis();
229        }
230
231        SIMD_FORCE_INLINE int closestAxis() const 
232        {
233                return absolute().maxAxis();
234        }
235
236        SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
237        {
238                btScalar s = btScalar(1.0) - rt;
239                m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
240                m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
241                m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
242                //don't do the unused w component
243                //              m_co[3] = s * v0[3] + rt * v1[3];
244        }
245
246  /**@brief Return the linear interpolation between this and another vector
247   * @param v The other vector
248   * @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
249        SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const 
250        {
251                return btVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
252                        m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
253                        m_floats[2] + (v.m_floats[2] -m_floats[2]) * t);
254        }
255
256  /**@brief Elementwise multiply this vector by the other
257   * @param v The other vector */
258        SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
259        {
260                m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2];
261                return *this;
262        }
263
264         /**@brief Return the x value */
265                SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
266  /**@brief Return the y value */
267                SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
268  /**@brief Return the z value */
269                SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
270  /**@brief Set the x value */
271                SIMD_FORCE_INLINE void  setX(btScalar x) { m_floats[0] = x;};
272  /**@brief Set the y value */
273                SIMD_FORCE_INLINE void  setY(btScalar y) { m_floats[1] = y;};
274  /**@brief Set the z value */
275                SIMD_FORCE_INLINE void  setZ(btScalar z) {m_floats[2] = z;};
276  /**@brief Set the w value */
277                SIMD_FORCE_INLINE void  setW(btScalar w) { m_floats[3] = w;};
278  /**@brief Return the x value */
279                SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
280  /**@brief Return the y value */
281                SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
282  /**@brief Return the z value */
283                SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
284  /**@brief Return the w value */
285                SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
286
287        //SIMD_FORCE_INLINE btScalar&       operator[](int i)       { return (&m_floats[0])[i]; }     
288        //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
289        ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
290        SIMD_FORCE_INLINE       operator       btScalar *()       { return &m_floats[0]; }
291        SIMD_FORCE_INLINE       operator const btScalar *() const { return &m_floats[0]; }
292
293        SIMD_FORCE_INLINE       bool    operator==(const btVector3& other) const
294        {
295                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]));
296        }
297
298        SIMD_FORCE_INLINE       bool    operator!=(const btVector3& other) const
299        {
300                return !(*this == other);
301        }
302
303         /**@brief Set each element to the max of the current values and the values of another btVector3
304   * @param other The other btVector3 to compare with
305   */
306                SIMD_FORCE_INLINE void  setMax(const btVector3& other)
307                {
308                        btSetMax(m_floats[0], other.m_floats[0]);
309                        btSetMax(m_floats[1], other.m_floats[1]);
310                        btSetMax(m_floats[2], other.m_floats[2]);
311                        btSetMax(m_floats[3], other.w());
312                }
313  /**@brief Set each element to the min of the current values and the values of another btVector3
314   * @param other The other btVector3 to compare with
315   */
316                SIMD_FORCE_INLINE void  setMin(const btVector3& other)
317                {
318                        btSetMin(m_floats[0], other.m_floats[0]);
319                        btSetMin(m_floats[1], other.m_floats[1]);
320                        btSetMin(m_floats[2], other.m_floats[2]);
321                        btSetMin(m_floats[3], other.w());
322                }
323
324                SIMD_FORCE_INLINE void  setValue(const btScalar& x, const btScalar& y, const btScalar& z)
325                {
326                        m_floats[0]=x;
327                        m_floats[1]=y;
328                        m_floats[2]=z;
329                        m_floats[3] = btScalar(0.);
330                }
331
332                void    getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const
333                {
334                        v0->setValue(0.         ,-z()           ,y());
335                        v1->setValue(z()        ,0.                     ,-x());
336                        v2->setValue(-y()       ,x()    ,0.);
337                }
338
339                void    setZero()
340                {
341                        setValue(btScalar(0.),btScalar(0.),btScalar(0.));
342                }
343
344                SIMD_FORCE_INLINE bool isZero() const 
345                {
346                        return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0);
347                }
348
349                SIMD_FORCE_INLINE bool fuzzyZero() const 
350                {
351                        return length2() < SIMD_EPSILON;
352                }
353
354                SIMD_FORCE_INLINE       void    serialize(struct        btVector3Data& dataOut) const;
355
356                SIMD_FORCE_INLINE       void    deSerialize(const struct        btVector3Data& dataIn);
357
358                SIMD_FORCE_INLINE       void    serializeFloat(struct   btVector3FloatData& dataOut) const;
359
360                SIMD_FORCE_INLINE       void    deSerializeFloat(const struct   btVector3FloatData& dataIn);
361
362                SIMD_FORCE_INLINE       void    serializeDouble(struct  btVector3DoubleData& dataOut) const;
363
364                SIMD_FORCE_INLINE       void    deSerializeDouble(const struct  btVector3DoubleData& dataIn);
365
366};
367
368/**@brief Return the sum of two vectors (Point symantics)*/
369SIMD_FORCE_INLINE btVector3
370operator+(const btVector3& v1, const btVector3& v2) 
371{
372        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]);
373}
374
375/**@brief Return the elementwise product of two vectors */
376SIMD_FORCE_INLINE btVector3
377operator*(const btVector3& v1, const btVector3& v2) 
378{
379        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]);
380}
381
382/**@brief Return the difference between two vectors */
383SIMD_FORCE_INLINE btVector3
384operator-(const btVector3& v1, const btVector3& v2)
385{
386        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]);
387}
388/**@brief Return the negative of the vector */
389SIMD_FORCE_INLINE btVector3
390operator-(const btVector3& v)
391{
392        return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
393}
394
395/**@brief Return the vector scaled by s */
396SIMD_FORCE_INLINE btVector3
397operator*(const btVector3& v, const btScalar& s)
398{
399        return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
400}
401
402/**@brief Return the vector scaled by s */
403SIMD_FORCE_INLINE btVector3
404operator*(const btScalar& s, const btVector3& v)
405{ 
406        return v * s; 
407}
408
409/**@brief Return the vector inversely scaled by s */
410SIMD_FORCE_INLINE btVector3
411operator/(const btVector3& v, const btScalar& s)
412{
413        btFullAssert(s != btScalar(0.0));
414        return v * (btScalar(1.0) / s);
415}
416
417/**@brief Return the vector inversely scaled by s */
418SIMD_FORCE_INLINE btVector3
419operator/(const btVector3& v1, const btVector3& v2)
420{
421        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]);
422}
423
424/**@brief Return the dot product between two vectors */
425SIMD_FORCE_INLINE btScalar
426btDot(const btVector3& v1, const btVector3& v2) 
427{ 
428        return v1.dot(v2); 
429}
430
431
432/**@brief Return the distance squared between two vectors */
433SIMD_FORCE_INLINE btScalar
434btDistance2(const btVector3& v1, const btVector3& v2) 
435{ 
436        return v1.distance2(v2); 
437}
438
439
440/**@brief Return the distance between two vectors */
441SIMD_FORCE_INLINE btScalar
442btDistance(const btVector3& v1, const btVector3& v2) 
443{ 
444        return v1.distance(v2); 
445}
446
447/**@brief Return the angle between two vectors */
448SIMD_FORCE_INLINE btScalar
449btAngle(const btVector3& v1, const btVector3& v2) 
450{ 
451        return v1.angle(v2); 
452}
453
454/**@brief Return the cross product of two vectors */
455SIMD_FORCE_INLINE btVector3
456btCross(const btVector3& v1, const btVector3& v2) 
457{ 
458        return v1.cross(v2); 
459}
460
461SIMD_FORCE_INLINE btScalar
462btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
463{
464        return v1.triple(v2, v3);
465}
466
467/**@brief Return the linear interpolation between two vectors
468 * @param v1 One vector
469 * @param v2 The other vector
470 * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */
471SIMD_FORCE_INLINE btVector3
472lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
473{
474        return v1.lerp(v2, t);
475}
476
477
478
479SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
480{
481        return (v - *this).length2();
482}
483
484SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
485{
486        return (v - *this).length();
487}
488
489SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
490{
491        return *this / length();
492} 
493
494SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar angle ) const
495{
496        // wAxis must be a unit lenght vector
497
498        btVector3 o = wAxis * wAxis.dot( *this );
499        btVector3 x = *this - o;
500        btVector3 y;
501
502        y = wAxis.cross( *this );
503
504        return ( o + x * btCos( angle ) + y * btSin( angle ) );
505}
506
507class btVector4 : public btVector3
508{
509public:
510
511        SIMD_FORCE_INLINE btVector4() {}
512
513
514        SIMD_FORCE_INLINE btVector4(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) 
515                : btVector3(x,y,z)
516        {
517                m_floats[3] = w;
518        }
519
520
521        SIMD_FORCE_INLINE btVector4 absolute4() const 
522        {
523                return btVector4(
524                        btFabs(m_floats[0]), 
525                        btFabs(m_floats[1]), 
526                        btFabs(m_floats[2]),
527                        btFabs(m_floats[3]));
528        }
529
530
531
532        btScalar        getW() const { return m_floats[3];}
533
534
535                SIMD_FORCE_INLINE int maxAxis4() const
536        {
537                int maxIndex = -1;
538                btScalar maxVal = btScalar(-BT_LARGE_FLOAT);
539                if (m_floats[0] > maxVal)
540                {
541                        maxIndex = 0;
542                        maxVal = m_floats[0];
543                }
544                if (m_floats[1] > maxVal)
545                {
546                        maxIndex = 1;
547                        maxVal = m_floats[1];
548                }
549                if (m_floats[2] > maxVal)
550                {
551                        maxIndex = 2;
552                        maxVal =m_floats[2];
553                }
554                if (m_floats[3] > maxVal)
555                {
556                        maxIndex = 3;
557                        maxVal = m_floats[3];
558                }
559               
560               
561               
562
563                return maxIndex;
564
565        }
566
567
568        SIMD_FORCE_INLINE int minAxis4() const
569        {
570                int minIndex = -1;
571                btScalar minVal = btScalar(BT_LARGE_FLOAT);
572                if (m_floats[0] < minVal)
573                {
574                        minIndex = 0;
575                        minVal = m_floats[0];
576                }
577                if (m_floats[1] < minVal)
578                {
579                        minIndex = 1;
580                        minVal = m_floats[1];
581                }
582                if (m_floats[2] < minVal)
583                {
584                        minIndex = 2;
585                        minVal =m_floats[2];
586                }
587                if (m_floats[3] < minVal)
588                {
589                        minIndex = 3;
590                        minVal = m_floats[3];
591                }
592               
593                return minIndex;
594
595        }
596
597
598        SIMD_FORCE_INLINE int closestAxis4() const 
599        {
600                return absolute4().maxAxis4();
601        }
602
603       
604 
605
606  /**@brief Set x,y,z and zero w
607   * @param x Value of x
608   * @param y Value of y
609   * @param z Value of z
610   */
611               
612
613/*              void getValue(btScalar *m) const
614                {
615                        m[0] = m_floats[0];
616                        m[1] = m_floats[1];
617                        m[2] =m_floats[2];
618                }
619*/
620/**@brief Set the values
621   * @param x Value of x
622   * @param y Value of y
623   * @param z Value of z
624   * @param w Value of w
625   */
626                SIMD_FORCE_INLINE void  setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
627                {
628                        m_floats[0]=x;
629                        m_floats[1]=y;
630                        m_floats[2]=z;
631                        m_floats[3]=w;
632                }
633
634
635};
636
637
638///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
639SIMD_FORCE_INLINE void  btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal)
640{
641        #ifdef BT_USE_DOUBLE_PRECISION
642        unsigned char* dest = (unsigned char*) &destVal;
643        unsigned char* src  = (unsigned char*) &sourceVal;
644        dest[0] = src[7];
645    dest[1] = src[6];
646    dest[2] = src[5];
647    dest[3] = src[4];
648    dest[4] = src[3];
649    dest[5] = src[2];
650    dest[6] = src[1];
651    dest[7] = src[0];
652#else
653        unsigned char* dest = (unsigned char*) &destVal;
654        unsigned char* src  = (unsigned char*) &sourceVal;
655        dest[0] = src[3];
656    dest[1] = src[2];
657    dest[2] = src[1];
658    dest[3] = src[0];
659#endif //BT_USE_DOUBLE_PRECISION
660}
661///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
662SIMD_FORCE_INLINE void  btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec)
663{
664        for (int i=0;i<4;i++)
665        {
666                btSwapScalarEndian(sourceVec[i],destVec[i]);
667        }
668
669}
670
671///btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
672SIMD_FORCE_INLINE void  btUnSwapVector3Endian(btVector3& vector)
673{
674
675        btVector3       swappedVec;
676        for (int i=0;i<4;i++)
677        {
678                btSwapScalarEndian(vector[i],swappedVec[i]);
679        }
680        vector = swappedVec;
681}
682
683template <class T>
684SIMD_FORCE_INLINE void btPlaneSpace1 (const T& n, T& p, T& q)
685{
686  if (btFabs(n[2]) > SIMDSQRT12) {
687    // choose p in y-z plane
688    btScalar a = n[1]*n[1] + n[2]*n[2];
689    btScalar k = btRecipSqrt (a);
690    p[0] = 0;
691        p[1] = -n[2]*k;
692        p[2] = n[1]*k;
693    // set q = n x p
694    q[0] = a*k;
695        q[1] = -n[0]*p[2];
696        q[2] = n[0]*p[1];
697  }
698  else {
699    // choose p in x-y plane
700    btScalar a = n[0]*n[0] + n[1]*n[1];
701    btScalar k = btRecipSqrt (a);
702    p[0] = -n[1]*k;
703        p[1] = n[0]*k;
704        p[2] = 0;
705    // set q = n x p
706    q[0] = -n[2]*p[1];
707        q[1] = n[2]*p[0];
708        q[2] = a*k;
709  }
710}
711
712
713struct  btVector3FloatData
714{
715        float   m_floats[4];
716};
717
718struct  btVector3DoubleData
719{
720        double  m_floats[4];
721
722};
723
724SIMD_FORCE_INLINE       void    btVector3::serializeFloat(struct        btVector3FloatData& dataOut) const
725{
726        ///could also do a memcpy, check if it is worth it
727        for (int i=0;i<4;i++)
728                dataOut.m_floats[i] = float(m_floats[i]);
729}
730
731SIMD_FORCE_INLINE void  btVector3::deSerializeFloat(const struct        btVector3FloatData& dataIn)
732{
733        for (int i=0;i<4;i++)
734                m_floats[i] = btScalar(dataIn.m_floats[i]);
735}
736
737
738SIMD_FORCE_INLINE       void    btVector3::serializeDouble(struct       btVector3DoubleData& dataOut) const
739{
740        ///could also do a memcpy, check if it is worth it
741        for (int i=0;i<4;i++)
742                dataOut.m_floats[i] = double(m_floats[i]);
743}
744
745SIMD_FORCE_INLINE void  btVector3::deSerializeDouble(const struct       btVector3DoubleData& dataIn)
746{
747        for (int i=0;i<4;i++)
748                m_floats[i] = btScalar(dataIn.m_floats[i]);
749}
750
751
752SIMD_FORCE_INLINE       void    btVector3::serialize(struct     btVector3Data& dataOut) const
753{
754        ///could also do a memcpy, check if it is worth it
755        for (int i=0;i<4;i++)
756                dataOut.m_floats[i] = m_floats[i];
757}
758
759SIMD_FORCE_INLINE void  btVector3::deSerialize(const struct     btVector3Data& dataIn)
760{
761        for (int i=0;i<4;i++)
762                m_floats[i] = dataIn.m_floats[i];
763}
764
765
766#endif //BT_VECTOR3_H
Note: See TracBrowser for help on using the repository browser.