svn merge ^/trunk/blender -r47023:HEAD
[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 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  */
39 ATTRIBUTE_ALIGNED16(class) btVector3
40 {
41 public:
42
43 #if defined (__SPU__) && defined (__CELLOS_LV2__)
44                 btScalar        m_floats[4];
45 public:
46         SIMD_FORCE_INLINE const vec_float4&     get128() const
47         {
48                 return *((const vec_float4*)&m_floats[0]);
49         }
50 public:
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)*/
369 SIMD_FORCE_INLINE btVector3 
370 operator+(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 */
376 SIMD_FORCE_INLINE btVector3 
377 operator*(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 */
383 SIMD_FORCE_INLINE btVector3 
384 operator-(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 */
389 SIMD_FORCE_INLINE btVector3 
390 operator-(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 */
396 SIMD_FORCE_INLINE btVector3 
397 operator*(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 */
403 SIMD_FORCE_INLINE btVector3 
404 operator*(const btScalar& s, const btVector3& v)
405
406         return v * s; 
407 }
408
409 /**@brief Return the vector inversely scaled by s */
410 SIMD_FORCE_INLINE btVector3
411 operator/(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 */
418 SIMD_FORCE_INLINE btVector3
419 operator/(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 */
425 SIMD_FORCE_INLINE btScalar 
426 btDot(const btVector3& v1, const btVector3& v2) 
427
428         return v1.dot(v2); 
429 }
430
431
432 /**@brief Return the distance squared between two vectors */
433 SIMD_FORCE_INLINE btScalar
434 btDistance2(const btVector3& v1, const btVector3& v2) 
435
436         return v1.distance2(v2); 
437 }
438
439
440 /**@brief Return the distance between two vectors */
441 SIMD_FORCE_INLINE btScalar
442 btDistance(const btVector3& v1, const btVector3& v2) 
443
444         return v1.distance(v2); 
445 }
446
447 /**@brief Return the angle between two vectors */
448 SIMD_FORCE_INLINE btScalar
449 btAngle(const btVector3& v1, const btVector3& v2) 
450
451         return v1.angle(v2); 
452 }
453
454 /**@brief Return the cross product of two vectors */
455 SIMD_FORCE_INLINE btVector3 
456 btCross(const btVector3& v1, const btVector3& v2) 
457
458         return v1.cross(v2); 
459 }
460
461 SIMD_FORCE_INLINE btScalar
462 btTriple(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) */
471 SIMD_FORCE_INLINE btVector3 
472 lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
473 {
474         return v1.lerp(v2, t);
475 }
476
477
478
479 SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
480 {
481         return (v - *this).length2();
482 }
483
484 SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
485 {
486         return (v - *this).length();
487 }
488
489 SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
490 {
491         return *this / length();
492
493
494 SIMD_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
507 class btVector4 : public btVector3
508 {
509 public:
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
639 SIMD_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
662 SIMD_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
672 SIMD_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
683 template <class T>
684 SIMD_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
713 struct  btVector3FloatData
714 {
715         float   m_floats[4];
716 };
717
718 struct  btVector3DoubleData
719 {
720         double  m_floats[4];
721
722 };
723
724 SIMD_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
731 SIMD_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
738 SIMD_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
745 SIMD_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
752 SIMD_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
759 SIMD_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