Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/LinearMath/btVector3.h @ 2119

Last change on this file since 2119 was 1963, checked in by rgrieder, 16 years ago

Added Bullet physics engine.
  • Property svn:eol-style set to native
File size: 9.7 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///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
25class   btVector3 : public btQuadWord {
26
27public:
28        SIMD_FORCE_INLINE btVector3() {}
29
30        SIMD_FORCE_INLINE btVector3(const btQuadWordStorage& q) 
31                : btQuadWord(q)
32        {
33        }
34       
35
36        SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) 
37                :btQuadWord(x,y,z,btScalar(0.))
38        {
39        }
40
41//      SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
42//              : btQuadWord(x,y,z,w)
43//      {
44//      }
45
46       
47
48        SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v)
49        {
50
51                m_x += v.x(); m_y += v.y(); m_z += v.z();
52                return *this;
53        }
54
55
56
57        SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) 
58        {
59                m_x -= v.x(); m_y -= v.y(); m_z -= v.z();
60                return *this;
61        }
62
63        SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
64        {
65                m_x *= s; m_y *= s; m_z *= s;
66                return *this;
67        }
68
69        SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) 
70        {
71                btFullAssert(s != btScalar(0.0));
72                return *this *= btScalar(1.0) / s;
73        }
74
75        SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
76        {
77                return m_x * v.x() + m_y * v.y() + m_z * v.z();
78        }
79
80        SIMD_FORCE_INLINE btScalar length2() const
81        {
82                return dot(*this);
83        }
84
85        SIMD_FORCE_INLINE btScalar length() const
86        {
87                return btSqrt(length2());
88        }
89
90        SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;
91
92        SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const;
93
94        SIMD_FORCE_INLINE btVector3& normalize() 
95        {
96                return *this /= length();
97        }
98
99        SIMD_FORCE_INLINE btVector3 normalized() const;
100
101        SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle );
102
103        SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const 
104        {
105                btScalar s = btSqrt(length2() * v.length2());
106                btFullAssert(s != btScalar(0.0));
107                return btAcos(dot(v) / s);
108        }
109
110        SIMD_FORCE_INLINE btVector3 absolute() const 
111        {
112                return btVector3(
113                        btFabs(m_x), 
114                        btFabs(m_y), 
115                        btFabs(m_z));
116        }
117
118        SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
119        {
120                return btVector3(
121                        m_y * v.z() - m_z * v.y(),
122                        m_z * v.x() - m_x * v.z(),
123                        m_x * v.y() - m_y * v.x());
124        }
125
126        SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
127        {
128                return m_x * (v1.y() * v2.z() - v1.z() * v2.y()) + 
129                        m_y * (v1.z() * v2.x() - v1.x() * v2.z()) + 
130                        m_z * (v1.x() * v2.y() - v1.y() * v2.x());
131        }
132
133        SIMD_FORCE_INLINE int minAxis() const
134        {
135                return m_x < m_y ? (m_x < m_z ? 0 : 2) : (m_y < m_z ? 1 : 2);
136        }
137
138        SIMD_FORCE_INLINE int maxAxis() const 
139        {
140                return m_x < m_y ? (m_y < m_z ? 2 : 1) : (m_x < m_z ? 2 : 0);
141        }
142
143        SIMD_FORCE_INLINE int furthestAxis() const
144        {
145                return absolute().minAxis();
146        }
147
148        SIMD_FORCE_INLINE int closestAxis() const 
149        {
150                return absolute().maxAxis();
151        }
152
153        SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
154        {
155                btScalar s = btScalar(1.0) - rt;
156                m_x = s * v0.x() + rt * v1.x();
157                m_y = s * v0.y() + rt * v1.y();
158                m_z = s * v0.z() + rt * v1.z();
159                //don't do the unused w component
160                //              m_co[3] = s * v0[3] + rt * v1[3];
161        }
162
163        SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const 
164        {
165                return btVector3(m_x + (v.x() - m_x) * t,
166                        m_y + (v.y() - m_y) * t,
167                        m_z + (v.z() - m_z) * t);
168        }
169
170
171        SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
172        {
173                m_x *= v.x(); m_y *= v.y(); m_z *= v.z();
174                return *this;
175        }
176
177       
178
179};
180
181SIMD_FORCE_INLINE btVector3
182operator+(const btVector3& v1, const btVector3& v2) 
183{
184        return btVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z());
185}
186
187SIMD_FORCE_INLINE btVector3
188operator*(const btVector3& v1, const btVector3& v2) 
189{
190        return btVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z());
191}
192
193SIMD_FORCE_INLINE btVector3
194operator-(const btVector3& v1, const btVector3& v2)
195{
196        return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z());
197}
198
199SIMD_FORCE_INLINE btVector3
200operator-(const btVector3& v)
201{
202        return btVector3(-v.x(), -v.y(), -v.z());
203}
204
205SIMD_FORCE_INLINE btVector3
206operator*(const btVector3& v, const btScalar& s)
207{
208        return btVector3(v.x() * s, v.y() * s, v.z() * s);
209}
210
211SIMD_FORCE_INLINE btVector3
212operator*(const btScalar& s, const btVector3& v)
213{ 
214        return v * s; 
215}
216
217SIMD_FORCE_INLINE btVector3
218operator/(const btVector3& v, const btScalar& s)
219{
220        btFullAssert(s != btScalar(0.0));
221        return v * (btScalar(1.0) / s);
222}
223
224SIMD_FORCE_INLINE btVector3
225operator/(const btVector3& v1, const btVector3& v2)
226{
227        return btVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z());
228}
229
230SIMD_FORCE_INLINE btScalar
231dot(const btVector3& v1, const btVector3& v2) 
232{ 
233        return v1.dot(v2); 
234}
235
236
237
238SIMD_FORCE_INLINE btScalar
239distance2(const btVector3& v1, const btVector3& v2) 
240{ 
241        return v1.distance2(v2); 
242}
243
244
245SIMD_FORCE_INLINE btScalar
246distance(const btVector3& v1, const btVector3& v2) 
247{ 
248        return v1.distance(v2); 
249}
250
251SIMD_FORCE_INLINE btScalar
252angle(const btVector3& v1, const btVector3& v2) 
253{ 
254        return v1.angle(v2); 
255}
256
257SIMD_FORCE_INLINE btVector3
258cross(const btVector3& v1, const btVector3& v2) 
259{ 
260        return v1.cross(v2); 
261}
262
263SIMD_FORCE_INLINE btScalar
264triple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
265{
266        return v1.triple(v2, v3);
267}
268
269SIMD_FORCE_INLINE btVector3
270lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
271{
272        return v1.lerp(v2, t);
273}
274
275
276SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2) 
277{
278        return p1.x() == p2.x() && p1.y() == p2.y() && p1.z() == p2.z();
279}
280
281SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
282{
283        return (v - *this).length2();
284}
285
286SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
287{
288        return (v - *this).length();
289}
290
291SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
292{
293        return *this / length();
294} 
295
296SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar angle )
297{
298        // wAxis must be a unit lenght vector
299
300        btVector3 o = wAxis * wAxis.dot( *this );
301        btVector3 x = *this - o;
302        btVector3 y;
303
304        y = wAxis.cross( *this );
305
306        return ( o + x * btCos( angle ) + y * btSin( angle ) );
307}
308
309class btVector4 : public btVector3
310{
311public:
312
313        SIMD_FORCE_INLINE btVector4() {}
314
315
316        SIMD_FORCE_INLINE btVector4(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) 
317                : btVector3(x,y,z)
318        {
319                m_unusedW = w;
320        }
321
322
323        SIMD_FORCE_INLINE btVector4 absolute4() const 
324        {
325                return btVector4(
326                        btFabs(m_x), 
327                        btFabs(m_y), 
328                        btFabs(m_z),
329                        btFabs(m_unusedW));
330        }
331
332
333
334        btScalar        getW() const { return m_unusedW;}
335
336
337                SIMD_FORCE_INLINE int maxAxis4() const
338        {
339                int maxIndex = -1;
340                btScalar maxVal = btScalar(-1e30);
341                if (m_x > maxVal)
342                {
343                        maxIndex = 0;
344                        maxVal = m_x;
345                }
346                if (m_y > maxVal)
347                {
348                        maxIndex = 1;
349                        maxVal = m_y;
350                }
351                if (m_z > maxVal)
352                {
353                        maxIndex = 2;
354                        maxVal = m_z;
355                }
356                if (m_unusedW > maxVal)
357                {
358                        maxIndex = 3;
359                        maxVal = m_unusedW;
360                }
361               
362               
363               
364
365                return maxIndex;
366
367        }
368
369
370        SIMD_FORCE_INLINE int minAxis4() const
371        {
372                int minIndex = -1;
373                btScalar minVal = btScalar(1e30);
374                if (m_x < minVal)
375                {
376                        minIndex = 0;
377                        minVal = m_x;
378                }
379                if (m_y < minVal)
380                {
381                        minIndex = 1;
382                        minVal = m_y;
383                }
384                if (m_z < minVal)
385                {
386                        minIndex = 2;
387                        minVal = m_z;
388                }
389                if (m_unusedW < minVal)
390                {
391                        minIndex = 3;
392                        minVal = m_unusedW;
393                }
394               
395                return minIndex;
396
397        }
398
399
400        SIMD_FORCE_INLINE int closestAxis4() const 
401        {
402                return absolute4().maxAxis4();
403        }
404
405};
406
407
408///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
409SIMD_FORCE_INLINE void  btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal)
410{
411        #ifdef BT_USE_DOUBLE_PRECISION
412        unsigned char* dest = (unsigned char*) &destVal;
413        unsigned char* src  = (unsigned char*) &sourceVal;
414        dest[0] = src[7];
415    dest[1] = src[6];
416    dest[2] = src[5];
417    dest[3] = src[4];
418    dest[4] = src[3];
419    dest[5] = src[2];
420    dest[6] = src[1];
421    dest[7] = src[0];
422#else
423        unsigned char* dest = (unsigned char*) &destVal;
424        unsigned char* src  = (unsigned char*) &sourceVal;
425        dest[0] = src[3];
426    dest[1] = src[2];
427    dest[2] = src[1];
428    dest[3] = src[0];
429#endif //BT_USE_DOUBLE_PRECISION
430}
431///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
432SIMD_FORCE_INLINE void  btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec)
433{
434        for (int i=0;i<4;i++)
435        {
436                btSwapScalarEndian(sourceVec[i],destVec[i]);
437        }
438
439}
440
441///btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
442SIMD_FORCE_INLINE void  btUnSwapVector3Endian(btVector3& vector)
443{
444
445        btVector3       swappedVec;
446        for (int i=0;i<4;i++)
447        {
448                btSwapScalarEndian(vector[i],swappedVec[i]);
449        }
450        vector = swappedVec;
451}
452
453#endif //SIMD__VECTOR3_H
Note: See TracBrowser for help on using the repository browser.