← Back to branch summary

~ubuntu-branches/ubuntu/vivid/emscripten/vivid

« back to all changes in this revision

Viewing changes to tests/bullet/src/LinearMath/btVector3.h

  • Committer: Package Import Robot
  • Author(s): Sylvestre Ledru
  • Date: 2013-05-02 13:11:51 UTC
  • Revision ID: package-import@ubuntu.com-20130502131151-q8dvteqr1ef2x7xz
Tags: upstream-1.4.1~20130504~adb56cb
Import upstream version 1.4.1~20130504~adb56cb

Show diffs side-by-side

added added

removed removed

Lines of Context:
tests/bullet/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