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 "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 */
ATTRIBUTE_ALIGNED16(class)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 /**@brief Normalize this vector
152 * x^2 + y^2 + z^2 = 1 */
153 SIMD_FORCE_INLINE btVector3& normalize()
154 {
155 return *this /= length();
156 }
157
158 /**@brief Return a normalized version of this vector */
159 SIMD_FORCE_INLINE btVector3 normalized() const;
160
161 /**@brief Rotate this vector
162 * @param wAxis The axis to rotate about
163 * @param angle The angle to rotate by */
164 SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle );
165
166 /**@brief Return the angle between this and another vector
167 * @param v The other vector */
168 SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const
169 {
170 btScalar s = btSqrt(length2() * v.length2());
171 btFullAssert(s != btScalar(0.0));
172 return btAcos(dot(v) / s);
173 }
174 /**@brief Return a vector will the absolute values of each element */
175 SIMD_FORCE_INLINE btVector3 absolute() const
176 {
177 return btVector3(
178 btFabs(m_floats[0]),
179 btFabs(m_floats[1]),
180 btFabs(m_floats[2]));
181 }
182 /**@brief Return the cross product between this and another vector
183 * @param v The other vector */
184 SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
185 {
186 return btVector3(
187 m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1],
188 m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
189 m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
190 }
191
192 SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
193 {
194 return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) +
195 m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) +
196 m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
197 }
198
199 /**@brief Return the axis with the smallest value
200 * Note return values are 0,1,2 for x, y, or z */
201 SIMD_FORCE_INLINE int minAxis() const
202 {
203 return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
204 }
205
206 /**@brief Return the axis with the largest value
207 * Note return values are 0,1,2 for x, y, or z */
208 SIMD_FORCE_INLINE int maxAxis() const
209 {
210 return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
211 }
212
213 SIMD_FORCE_INLINE int furthestAxis() const
214 {
215 return absolute().minAxis();
216 }
217
218 SIMD_FORCE_INLINE int closestAxis() const
219 {
220 return absolute().maxAxis();
221 }
222
223 SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
224 {
225 btScalar s = btScalar(1.0) - rt;
226 m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
227 m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
228 m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
229 //don't do the unused w component
230 // m_co[3] = s * v0[3] + rt * v1[3];
231 }
232
233 /**@brief Return the linear interpolation between this and another vector
234 * @param v The other vector
235 * @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
236 SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const
237 {
238 return btVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
239 m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
240 m_floats[2] + (v.m_floats[2] -m_floats[2]) * t);
241 }
242
243 /**@brief Elementwise multiply this vector by the other
244 * @param v The other vector */
245 SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
246 {
247 m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2];
248 return *this;
249 }
250
251 /**@brief Return the x value */
252 SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
253 /**@brief Return the y value */
254 SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
255 /**@brief Return the z value */
256 SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
257 /**@brief Set the x value */
258 SIMD_FORCE_INLINE void setX(btScalar x) { m_floats[0] = x;};
259 /**@brief Set the y value */
260 SIMD_FORCE_INLINE void setY(btScalar y) { m_floats[1] = y;};
261 /**@brief Set the z value */
262 SIMD_FORCE_INLINE void setZ(btScalar z) {m_floats[2] = z;};
263 /**@brief Set the w value */
264 SIMD_FORCE_INLINE void setW(btScalar w) { m_floats[3] = w;};
265 /**@brief Return the x value */
266 SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
267 /**@brief Return the y value */
268 SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
269 /**@brief Return the z value */
270 SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
271 /**@brief Return the w value */
272 SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
273
274 //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; }
275 //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
276 ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
277 SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; }
278 SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; }
279
280 SIMD_FORCE_INLINE bool operator==(const btVector3& other) const
281 {
282 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]));
283 }
284
285 SIMD_FORCE_INLINE bool operator!=(const btVector3& other) const
286 {
287 return !(*this == other);
288 }
289
290 /**@brief Set each element to the max of the current values and the values of another btVector3
291 * @param other The other btVector3 to compare with
292 */
293 SIMD_FORCE_INLINE void setMax(const btVector3& other)
294 {
295 btSetMax(m_floats[0], other.m_floats[0]);
296 btSetMax(m_floats[1], other.m_floats[1]);
297 btSetMax(m_floats[2], other.m_floats[2]);
298 btSetMax(m_floats[3], other.w());
299 }
300 /**@brief Set each element to the min of the current values and the values of another btVector3
301 * @param other The other btVector3 to compare with
302 */
303 SIMD_FORCE_INLINE void setMin(const btVector3& other)
304 {
305 btSetMin(m_floats[0], other.m_floats[0]);
306 btSetMin(m_floats[1], other.m_floats[1]);
307 btSetMin(m_floats[2], other.m_floats[2]);
308 btSetMin(m_floats[3], other.w());
309 }
310
311 SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z)
312 {
313 m_floats[0]=x;
314 m_floats[1]=y;
315 m_floats[2]=z;
316 m_floats[3] = btScalar(0.);
317 }
318
319 void getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const
320 {
321 v0->setValue(0. ,-z() ,y());
322 v1->setValue(z() ,0. ,-x());
323 v2->setValue(-y() ,x() ,0.);
324 }
325
326 void setZero()
327 {
328 setValue(btScalar(0.),btScalar(0.),btScalar(0.));
329 }
330
331 SIMD_FORCE_INLINE bool isZero() const
332 {
333 return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0);
334 }
335
336 SIMD_FORCE_INLINE bool fuzzyZero() const
337 {
338 return length2() < SIMD_EPSILON;
339 }
340
341 SIMD_FORCE_INLINE void serialize(struct btVector3Data& dataOut) const;
342
343 SIMD_FORCE_INLINE void deSerialize(const struct btVector3Data& dataIn);
344
345 SIMD_FORCE_INLINE void serializeFloat(struct btVector3FloatData& dataOut) const;
346
347 SIMD_FORCE_INLINE void deSerializeFloat(const struct btVector3FloatData& dataIn);
348
349 SIMD_FORCE_INLINE void serializeDouble(struct btVector3DoubleData& dataOut) const;
350
351 SIMD_FORCE_INLINE void deSerializeDouble(const struct btVector3DoubleData& dataIn);
352
353 };
354
355 /**@brief Return the sum of two vectors (Point symantics)*/
356 SIMD_FORCE_INLINE btVector3
357 operator+(const btVector3& v1, const btVector3& v2)
358 {
359 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]);
360 }
361
362 /**@brief Return the elementwise product of two vectors */
363 SIMD_FORCE_INLINE btVector3
364 operator*(const btVector3& v1, const btVector3& v2)
365 {
366 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]);
367 }
368
369 /**@brief Return the difference between two vectors */
370 SIMD_FORCE_INLINE btVector3
371 operator-(const btVector3& v1, const btVector3& v2)
372 {
373 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]);
374 }
375 /**@brief Return the negative of the vector */
376 SIMD_FORCE_INLINE btVector3
377 operator-(const btVector3& v)
378 {
379 return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
380 }
381
382 /**@brief Return the vector scaled by s */
383 SIMD_FORCE_INLINE btVector3
384 operator*(const btVector3& v, const btScalar& s)
385 {
386 return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
387 }
388
389 /**@brief Return the vector scaled by s */
390 SIMD_FORCE_INLINE btVector3
391 operator*(const btScalar& s, const btVector3& v)
392 {
393 return v * s;
394 }
395
396 /**@brief Return the vector inversely scaled by s */
397 SIMD_FORCE_INLINE btVector3
398 operator/(const btVector3& v, const btScalar& s)
399 {
400 btFullAssert(s != btScalar(0.0));
401 return v * (btScalar(1.0) / s);
402 }
403
404 /**@brief Return the vector inversely scaled by s */
405 SIMD_FORCE_INLINE btVector3
406 operator/(const btVector3& v1, const btVector3& v2)
407 {
408 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]);
409 }
410
411 /**@brief Return the dot product between two vectors */
412 SIMD_FORCE_INLINE btScalar
btDot(const btVector3 & v1,const btVector3 & v2)413 btDot(const btVector3& v1, const btVector3& v2)
414 {
415 return v1.dot(v2);
416 }
417
418
419 /**@brief Return the distance squared between two vectors */
420 SIMD_FORCE_INLINE btScalar
btDistance2(const btVector3 & v1,const btVector3 & v2)421 btDistance2(const btVector3& v1, const btVector3& v2)
422 {
423 return v1.distance2(v2);
424 }
425
426
427 /**@brief Return the distance between two vectors */
428 SIMD_FORCE_INLINE btScalar
btDistance(const btVector3 & v1,const btVector3 & v2)429 btDistance(const btVector3& v1, const btVector3& v2)
430 {
431 return v1.distance(v2);
432 }
433
434 /**@brief Return the angle between two vectors */
435 SIMD_FORCE_INLINE btScalar
btAngle(const btVector3 & v1,const btVector3 & v2)436 btAngle(const btVector3& v1, const btVector3& v2)
437 {
438 return v1.angle(v2);
439 }
440
441 /**@brief Return the cross product of two vectors */
442 SIMD_FORCE_INLINE btVector3
btCross(const btVector3 & v1,const btVector3 & v2)443 btCross(const btVector3& v1, const btVector3& v2)
444 {
445 return v1.cross(v2);
446 }
447
448 SIMD_FORCE_INLINE btScalar
btTriple(const btVector3 & v1,const btVector3 & v2,const btVector3 & v3)449 btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
450 {
451 return v1.triple(v2, v3);
452 }
453
454 /**@brief Return the linear interpolation between two vectors
455 * @param v1 One vector
456 * @param v2 The other vector
457 * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */
458 SIMD_FORCE_INLINE btVector3
lerp(const btVector3 & v1,const btVector3 & v2,const btScalar & t)459 lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
460 {
461 return v1.lerp(v2, t);
462 }
463
464
465
distance2(const btVector3 & v)466 SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
467 {
468 return (v - *this).length2();
469 }
470
distance(const btVector3 & v)471 SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
472 {
473 return (v - *this).length();
474 }
475
normalized()476 SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
477 {
478 return *this / length();
479 }
480
rotate(const btVector3 & wAxis,const btScalar angle)481 SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar angle )
482 {
483 // wAxis must be a unit lenght vector
484
485 btVector3 o = wAxis * wAxis.dot( *this );
486 btVector3 x = *this - o;
487 btVector3 y;
488
489 y = wAxis.cross( *this );
490
491 return ( o + x * btCos( angle ) + y * btSin( angle ) );
492 }
493
494 class btVector4 : public btVector3
495 {
496 public:
497
btVector4()498 SIMD_FORCE_INLINE btVector4() {}
499
500
btVector4(const btScalar & x,const btScalar & y,const btScalar & z,const btScalar & w)501 SIMD_FORCE_INLINE btVector4(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
502 : btVector3(x,y,z)
503 {
504 m_floats[3] = w;
505 }
506
507
absolute4()508 SIMD_FORCE_INLINE btVector4 absolute4() const
509 {
510 return btVector4(
511 btFabs(m_floats[0]),
512 btFabs(m_floats[1]),
513 btFabs(m_floats[2]),
514 btFabs(m_floats[3]));
515 }
516
517
518
getW()519 btScalar getW() const { return m_floats[3];}
520
521
maxAxis4()522 SIMD_FORCE_INLINE int maxAxis4() const
523 {
524 int maxIndex = -1;
525 btScalar maxVal = btScalar(-BT_LARGE_FLOAT);
526 if (m_floats[0] > maxVal)
527 {
528 maxIndex = 0;
529 maxVal = m_floats[0];
530 }
531 if (m_floats[1] > maxVal)
532 {
533 maxIndex = 1;
534 maxVal = m_floats[1];
535 }
536 if (m_floats[2] > maxVal)
537 {
538 maxIndex = 2;
539 maxVal =m_floats[2];
540 }
541 if (m_floats[3] > maxVal)
542 {
543 maxIndex = 3;
544 maxVal = m_floats[3];
545 }
546
547
548
549
550 return maxIndex;
551
552 }
553
554
minAxis4()555 SIMD_FORCE_INLINE int minAxis4() const
556 {
557 int minIndex = -1;
558 btScalar minVal = btScalar(BT_LARGE_FLOAT);
559 if (m_floats[0] < minVal)
560 {
561 minIndex = 0;
562 minVal = m_floats[0];
563 }
564 if (m_floats[1] < minVal)
565 {
566 minIndex = 1;
567 minVal = m_floats[1];
568 }
569 if (m_floats[2] < minVal)
570 {
571 minIndex = 2;
572 minVal =m_floats[2];
573 }
574 if (m_floats[3] < minVal)
575 {
576 minIndex = 3;
577 minVal = m_floats[3];
578 }
579
580 return minIndex;
581
582 }
583
584
closestAxis4()585 SIMD_FORCE_INLINE int closestAxis4() const
586 {
587 return absolute4().maxAxis4();
588 }
589
590
591
592
593 /**@brief Set x,y,z and zero w
594 * @param x Value of x
595 * @param y Value of y
596 * @param z Value of z
597 */
598
599
600 /* void getValue(btScalar *m) const
601 {
602 m[0] = m_floats[0];
603 m[1] = m_floats[1];
604 m[2] =m_floats[2];
605 }
606 */
607 /**@brief Set the values
608 * @param x Value of x
609 * @param y Value of y
610 * @param z Value of z
611 * @param w Value of w
612 */
setValue(const btScalar & x,const btScalar & y,const btScalar & z,const btScalar & w)613 SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
614 {
615 m_floats[0]=x;
616 m_floats[1]=y;
617 m_floats[2]=z;
618 m_floats[3]=w;
619 }
620
621
622 };
623
624
625 ///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
btSwapScalarEndian(const btScalar & sourceVal,btScalar & destVal)626 SIMD_FORCE_INLINE void btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal)
627 {
628 #ifdef BT_USE_DOUBLE_PRECISION
629 unsigned char* dest = (unsigned char*) &destVal;
630 unsigned char* src = (unsigned char*) &sourceVal;
631 dest[0] = src[7];
632 dest[1] = src[6];
633 dest[2] = src[5];
634 dest[3] = src[4];
635 dest[4] = src[3];
636 dest[5] = src[2];
637 dest[6] = src[1];
638 dest[7] = src[0];
639 #else
640 unsigned char* dest = (unsigned char*) &destVal;
641 unsigned char* src = (unsigned char*) &sourceVal;
642 dest[0] = src[3];
643 dest[1] = src[2];
644 dest[2] = src[1];
645 dest[3] = src[0];
646 #endif //BT_USE_DOUBLE_PRECISION
647 }
648 ///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
btSwapVector3Endian(const btVector3 & sourceVec,btVector3 & destVec)649 SIMD_FORCE_INLINE void btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec)
650 {
651 for (int i=0;i<4;i++)
652 {
653 btSwapScalarEndian(sourceVec[i],destVec[i]);
654 }
655
656 }
657
658 ///btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
btUnSwapVector3Endian(btVector3 & vector)659 SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3& vector)
660 {
661
662 btVector3 swappedVec;
663 for (int i=0;i<4;i++)
664 {
665 btSwapScalarEndian(vector[i],swappedVec[i]);
666 }
667 vector = swappedVec;
668 }
669
btPlaneSpace1(const btVector3 & n,btVector3 & p,btVector3 & q)670 SIMD_FORCE_INLINE void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q)
671 {
672 if (btFabs(n.z()) > SIMDSQRT12) {
673 // choose p in y-z plane
674 btScalar a = n[1]*n[1] + n[2]*n[2];
675 btScalar k = btRecipSqrt (a);
676 p.setValue(0,-n[2]*k,n[1]*k);
677 // set q = n x p
678 q.setValue(a*k,-n[0]*p[2],n[0]*p[1]);
679 }
680 else {
681 // choose p in x-y plane
682 btScalar a = n.x()*n.x() + n.y()*n.y();
683 btScalar k = btRecipSqrt (a);
684 p.setValue(-n.y()*k,n.x()*k,0);
685 // set q = n x p
686 q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k);
687 }
688 }
689
690
691 struct btVector3FloatData
692 {
693 float m_floats[4];
694 };
695
696 struct btVector3DoubleData
697 {
698 double m_floats[4];
699
700 };
701
serializeFloat(struct btVector3FloatData & dataOut)702 SIMD_FORCE_INLINE void btVector3::serializeFloat(struct btVector3FloatData& dataOut) const
703 {
704 ///could also do a memcpy, check if it is worth it
705 for (int i=0;i<4;i++)
706 dataOut.m_floats[i] = float(m_floats[i]);
707 }
708
deSerializeFloat(const struct btVector3FloatData & dataIn)709 SIMD_FORCE_INLINE void btVector3::deSerializeFloat(const struct btVector3FloatData& dataIn)
710 {
711 for (int i=0;i<4;i++)
712 m_floats[i] = btScalar(dataIn.m_floats[i]);
713 }
714
715
serializeDouble(struct btVector3DoubleData & dataOut)716 SIMD_FORCE_INLINE void btVector3::serializeDouble(struct btVector3DoubleData& dataOut) const
717 {
718 ///could also do a memcpy, check if it is worth it
719 for (int i=0;i<4;i++)
720 dataOut.m_floats[i] = double(m_floats[i]);
721 }
722
deSerializeDouble(const struct btVector3DoubleData & dataIn)723 SIMD_FORCE_INLINE void btVector3::deSerializeDouble(const struct btVector3DoubleData& dataIn)
724 {
725 for (int i=0;i<4;i++)
726 m_floats[i] = btScalar(dataIn.m_floats[i]);
727 }
728
729
serialize(struct btVector3Data & dataOut)730 SIMD_FORCE_INLINE void btVector3::serialize(struct btVector3Data& dataOut) const
731 {
732 ///could also do a memcpy, check if it is worth it
733 for (int i=0;i<4;i++)
734 dataOut.m_floats[i] = m_floats[i];
735 }
736
deSerialize(const struct btVector3Data & dataIn)737 SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3Data& dataIn)
738 {
739 for (int i=0;i<4;i++)
740 m_floats[i] = dataIn.m_floats[i];
741 }
742
743
744 #endif //SIMD__VECTOR3_H
745