Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

Last change on this file since 2835 was 2662, checked in by rgrieder, 16 years ago

Merged presentation branch back to trunk.

  • Property svn:eol-style set to native
File size: 13.2 KB
Line 
1/*
2Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
3
4This software is provided 'as-is', without any express or implied warranty.
5In no event will the authors be held liable for any damages arising from the use of this software.
6Permission is granted to anyone to use this software for any purpose,
7including commercial applications, and to alter it and redistribute it freely,
8subject to the following restrictions:
9
101. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
112. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
123. This notice may not be removed or altered from any source distribution.
13*/
14
15
16
17#ifndef SIMD__VECTOR3_H
18#define SIMD__VECTOR3_H
19
20#include "btQuadWord.h"
21
22/**@brief btVector3 can be used to represent 3D points and vectors.
23 * 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
24 * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
25 */
26class   btVector3 : public btQuadWord {
27
28public:
29  /**@brief No initialization constructor */
30        SIMD_FORCE_INLINE btVector3() {}
31
32  /**@brief Constructor from btQuadWordStorage (btVector3 inherits from this so is also valid)
33   * Note: Vector3 derives from btQuadWordStorage*/
34        SIMD_FORCE_INLINE btVector3(const btQuadWordStorage& q) 
35                : btQuadWord(q)
36        {
37        }
38       
39  /**@brief Constructor from scalars
40   * @param x X value
41   * @param y Y value
42   * @param z Z value
43   */
44        SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) 
45                :btQuadWord(x,y,z,btScalar(0.))
46        {
47        }
48
49//      SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
50//              : btQuadWord(x,y,z,w)
51//      {
52//      }
53
54       
55/**@brief Add a vector to this one
56 * @param The vector to add to this one */
57        SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v)
58        {
59
60                m_floats[0] += v.x(); m_floats[1] += v.y(); m_floats[2] += v.z();
61                return *this;
62        }
63
64
65  /**@brief Subtract a vector from this one
66   * @param The vector to subtract */
67        SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) 
68        {
69                m_floats[0] -= v.x(); m_floats[1] -= v.y(); m_floats[2] -= v.z();
70                return *this;
71        }
72  /**@brief Scale the vector
73   * @param s Scale factor */
74        SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
75        {
76                m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s;
77                return *this;
78        }
79
80  /**@brief Inversely scale the vector
81   * @param s Scale factor to divide by */
82        SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) 
83        {
84                btFullAssert(s != btScalar(0.0));
85                return *this *= btScalar(1.0) / s;
86        }
87
88  /**@brief Return the dot product
89   * @param v The other vector in the dot product */
90        SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
91        {
92                return m_floats[0] * v.x() + m_floats[1] * v.y() + m_floats[2] * v.z();
93        }
94
95  /**@brief Return the length of the vector squared */
96        SIMD_FORCE_INLINE btScalar length2() const
97        {
98                return dot(*this);
99        }
100
101  /**@brief Return the length of the vector */
102        SIMD_FORCE_INLINE btScalar length() const
103        {
104                return btSqrt(length2());
105        }
106
107  /**@brief Return the distance squared between the ends of this and another vector
108   * This is symantically treating the vector like a point */
109        SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;
110
111  /**@brief Return the distance between the ends of this and another vector
112   * This is symantically treating the vector like a point */
113        SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const;
114
115  /**@brief Normalize this vector
116   * x^2 + y^2 + z^2 = 1 */
117        SIMD_FORCE_INLINE btVector3& normalize() 
118        {
119                return *this /= length();
120        }
121
122  /**@brief Return a normalized version of this vector */
123        SIMD_FORCE_INLINE btVector3 normalized() const;
124
125  /**@brief Rotate this vector
126   * @param wAxis The axis to rotate about
127   * @param angle The angle to rotate by */
128        SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle );
129
130  /**@brief Return the angle between this and another vector
131   * @param v The other vector */
132        SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const 
133        {
134                btScalar s = btSqrt(length2() * v.length2());
135                btFullAssert(s != btScalar(0.0));
136                return btAcos(dot(v) / s);
137        }
138  /**@brief Return a vector will the absolute values of each element */
139        SIMD_FORCE_INLINE btVector3 absolute() const 
140        {
141                return btVector3(
142                        btFabs(m_floats[0]), 
143                        btFabs(m_floats[1]), 
144                        btFabs(m_floats[2]));
145        }
146  /**@brief Return the cross product between this and another vector
147   * @param v The other vector */
148        SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
149        {
150                return btVector3(
151                        m_floats[1] * v.z() - m_floats[2] * v.y(),
152                        m_floats[2] * v.x() - m_floats[0] * v.z(),
153                        m_floats[0] * v.y() - m_floats[1] * v.x());
154        }
155
156        SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
157        {
158                return m_floats[0] * (v1.y() * v2.z() - v1.z() * v2.y()) + 
159                        m_floats[1] * (v1.z() * v2.x() - v1.x() * v2.z()) + 
160                        m_floats[2] * (v1.x() * v2.y() - v1.y() * v2.x());
161        }
162
163  /**@brief Return the axis with the smallest value
164   * Note return values are 0,1,2 for x, y, or z */
165        SIMD_FORCE_INLINE int minAxis() const
166        {
167                return m_floats[0] < m_floats[1] ? (m_floats[0] < m_floats[2] ? 0 : 2) : (m_floats[1] < m_floats[2] ? 1 : 2);
168        }
169
170  /**@brief Return the axis with the largest value
171   * Note return values are 0,1,2 for x, y, or z */
172        SIMD_FORCE_INLINE int maxAxis() const 
173        {
174                return m_floats[0] < m_floats[1] ? (m_floats[1] < m_floats[2] ? 2 : 1) : (m_floats[0] < m_floats[2] ? 2 : 0);
175        }
176
177        SIMD_FORCE_INLINE int furthestAxis() const
178        {
179                return absolute().minAxis();
180        }
181
182        SIMD_FORCE_INLINE int closestAxis() const 
183        {
184                return absolute().maxAxis();
185        }
186
187        SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
188        {
189                btScalar s = btScalar(1.0) - rt;
190                m_floats[0] = s * v0.x() + rt * v1.x();
191                m_floats[1] = s * v0.y() + rt * v1.y();
192                m_floats[2] = s * v0.z() + rt * v1.z();
193                //don't do the unused w component
194                //              m_co[3] = s * v0[3] + rt * v1[3];
195        }
196
197  /**@brief Return the linear interpolation between this and another vector
198   * @param v The other vector
199   * @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
200        SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const 
201        {
202                return btVector3(m_floats[0] + (v.x() - m_floats[0]) * t,
203                        m_floats[1] + (v.y() - m_floats[1]) * t,
204                        m_floats[2] + (v.z() - m_floats[2]) * t);
205        }
206
207  /**@brief Elementwise multiply this vector by the other
208   * @param v The other vector */
209        SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
210        {
211                m_floats[0] *= v.x(); m_floats[1] *= v.y(); m_floats[2] *= v.z();
212                return *this;
213        }
214
215       
216
217};
218
219/**@brief Return the sum of two vectors (Point symantics)*/
220SIMD_FORCE_INLINE btVector3
221operator+(const btVector3& v1, const btVector3& v2) 
222{
223        return btVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z());
224}
225
226/**@brief Return the elementwise product of two vectors */
227SIMD_FORCE_INLINE btVector3
228operator*(const btVector3& v1, const btVector3& v2) 
229{
230        return btVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z());
231}
232
233/**@brief Return the difference between two vectors */
234SIMD_FORCE_INLINE btVector3
235operator-(const btVector3& v1, const btVector3& v2)
236{
237        return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z());
238}
239/**@brief Return the negative of the vector */
240SIMD_FORCE_INLINE btVector3
241operator-(const btVector3& v)
242{
243        return btVector3(-v.x(), -v.y(), -v.z());
244}
245
246/**@brief Return the vector scaled by s */
247SIMD_FORCE_INLINE btVector3
248operator*(const btVector3& v, const btScalar& s)
249{
250        return btVector3(v.x() * s, v.y() * s, v.z() * s);
251}
252
253/**@brief Return the vector scaled by s */
254SIMD_FORCE_INLINE btVector3
255operator*(const btScalar& s, const btVector3& v)
256{ 
257        return v * s; 
258}
259
260/**@brief Return the vector inversely scaled by s */
261SIMD_FORCE_INLINE btVector3
262operator/(const btVector3& v, const btScalar& s)
263{
264        btFullAssert(s != btScalar(0.0));
265        return v * (btScalar(1.0) / s);
266}
267
268/**@brief Return the vector inversely scaled by s */
269SIMD_FORCE_INLINE btVector3
270operator/(const btVector3& v1, const btVector3& v2)
271{
272        return btVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z());
273}
274
275/**@brief Return the dot product between two vectors */
276SIMD_FORCE_INLINE btScalar
277dot(const btVector3& v1, const btVector3& v2) 
278{ 
279        return v1.dot(v2); 
280}
281
282
283/**@brief Return the distance squared between two vectors */
284SIMD_FORCE_INLINE btScalar
285distance2(const btVector3& v1, const btVector3& v2) 
286{ 
287        return v1.distance2(v2); 
288}
289
290
291/**@brief Return the distance between two vectors */
292SIMD_FORCE_INLINE btScalar
293distance(const btVector3& v1, const btVector3& v2) 
294{ 
295        return v1.distance(v2); 
296}
297
298/**@brief Return the angle between two vectors */
299SIMD_FORCE_INLINE btScalar
300angle(const btVector3& v1, const btVector3& v2) 
301{ 
302        return v1.angle(v2); 
303}
304
305/**@brief Return the cross product of two vectors */
306SIMD_FORCE_INLINE btVector3
307cross(const btVector3& v1, const btVector3& v2) 
308{ 
309        return v1.cross(v2); 
310}
311
312SIMD_FORCE_INLINE btScalar
313triple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
314{
315        return v1.triple(v2, v3);
316}
317
318/**@brief Return the linear interpolation between two vectors
319 * @param v1 One vector
320 * @param v2 The other vector
321 * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */
322SIMD_FORCE_INLINE btVector3
323lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
324{
325        return v1.lerp(v2, t);
326}
327
328/**@brief Test if each element of the vector is equivalent */
329SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2) 
330{
331        return p1.x() == p2.x() && p1.y() == p2.y() && p1.z() == p2.z();
332}
333
334SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
335{
336        return (v - *this).length2();
337}
338
339SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
340{
341        return (v - *this).length();
342}
343
344SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
345{
346        return *this / length();
347} 
348
349SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar angle )
350{
351        // wAxis must be a unit lenght vector
352
353        btVector3 o = wAxis * wAxis.dot( *this );
354        btVector3 x = *this - o;
355        btVector3 y;
356
357        y = wAxis.cross( *this );
358
359        return ( o + x * btCos( angle ) + y * btSin( angle ) );
360}
361
362class btVector4 : public btVector3
363{
364public:
365
366        SIMD_FORCE_INLINE btVector4() {}
367
368
369        SIMD_FORCE_INLINE btVector4(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) 
370                : btVector3(x,y,z)
371        {
372                m_floats[3] = w;
373        }
374
375
376        SIMD_FORCE_INLINE btVector4 absolute4() const 
377        {
378                return btVector4(
379                        btFabs(m_floats[0]), 
380                        btFabs(m_floats[1]), 
381                        btFabs(m_floats[2]),
382                        btFabs(m_floats[3]));
383        }
384
385
386
387        btScalar        getW() const { return m_floats[3];}
388
389
390                SIMD_FORCE_INLINE int maxAxis4() const
391        {
392                int maxIndex = -1;
393                btScalar maxVal = btScalar(-1e30);
394                if (m_floats[0] > maxVal)
395                {
396                        maxIndex = 0;
397                        maxVal = m_floats[0];
398                }
399                if (m_floats[1] > maxVal)
400                {
401                        maxIndex = 1;
402                        maxVal = m_floats[1];
403                }
404                if (m_floats[2] > maxVal)
405                {
406                        maxIndex = 2;
407                        maxVal = m_floats[2];
408                }
409                if (m_floats[3] > maxVal)
410                {
411                        maxIndex = 3;
412                        maxVal = m_floats[3];
413                }
414               
415               
416               
417
418                return maxIndex;
419
420        }
421
422
423        SIMD_FORCE_INLINE int minAxis4() const
424        {
425                int minIndex = -1;
426                btScalar minVal = btScalar(1e30);
427                if (m_floats[0] < minVal)
428                {
429                        minIndex = 0;
430                        minVal = m_floats[0];
431                }
432                if (m_floats[1] < minVal)
433                {
434                        minIndex = 1;
435                        minVal = m_floats[1];
436                }
437                if (m_floats[2] < minVal)
438                {
439                        minIndex = 2;
440                        minVal = m_floats[2];
441                }
442                if (m_floats[3] < minVal)
443                {
444                        minIndex = 3;
445                        minVal = m_floats[3];
446                }
447               
448                return minIndex;
449
450        }
451
452
453        SIMD_FORCE_INLINE int closestAxis4() const 
454        {
455                return absolute4().maxAxis4();
456        }
457
458};
459
460
461///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
462SIMD_FORCE_INLINE void  btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal)
463{
464        #ifdef BT_USE_DOUBLE_PRECISION
465        unsigned char* dest = (unsigned char*) &destVal;
466        unsigned char* src  = (unsigned char*) &sourceVal;
467        dest[0] = src[7];
468    dest[1] = src[6];
469    dest[2] = src[5];
470    dest[3] = src[4];
471    dest[4] = src[3];
472    dest[5] = src[2];
473    dest[6] = src[1];
474    dest[7] = src[0];
475#else
476        unsigned char* dest = (unsigned char*) &destVal;
477        unsigned char* src  = (unsigned char*) &sourceVal;
478        dest[0] = src[3];
479    dest[1] = src[2];
480    dest[2] = src[1];
481    dest[3] = src[0];
482#endif //BT_USE_DOUBLE_PRECISION
483}
484///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
485SIMD_FORCE_INLINE void  btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec)
486{
487        for (int i=0;i<4;i++)
488        {
489                btSwapScalarEndian(sourceVec[i],destVec[i]);
490        }
491
492}
493
494///btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
495SIMD_FORCE_INLINE void  btUnSwapVector3Endian(btVector3& vector)
496{
497
498        btVector3       swappedVec;
499        for (int i=0;i<4;i++)
500        {
501                btSwapScalarEndian(vector[i],swappedVec[i]);
502        }
503        vector = swappedVec;
504}
505
506#endif //SIMD__VECTOR3_H
Note: See TracBrowser for help on using the repository browser.