Some version were not merged from svn/missing --> svn merge -r 16370:16371 https...
[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 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
25 class   btVector3 : public btQuadWord {
26
27 public:
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
181 SIMD_FORCE_INLINE btVector3 
182 operator+(const btVector3& v1, const btVector3& v2) 
183 {
184         return btVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z());
185 }
186
187 SIMD_FORCE_INLINE btVector3 
188 operator*(const btVector3& v1, const btVector3& v2) 
189 {
190         return btVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z());
191 }
192
193 SIMD_FORCE_INLINE btVector3 
194 operator-(const btVector3& v1, const btVector3& v2)
195 {
196         return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z());
197 }
198
199 SIMD_FORCE_INLINE btVector3 
200 operator-(const btVector3& v)
201 {
202         return btVector3(-v.x(), -v.y(), -v.z());
203 }
204
205 SIMD_FORCE_INLINE btVector3 
206 operator*(const btVector3& v, const btScalar& s)
207 {
208         return btVector3(v.x() * s, v.y() * s, v.z() * s);
209 }
210
211 SIMD_FORCE_INLINE btVector3 
212 operator*(const btScalar& s, const btVector3& v)
213
214         return v * s; 
215 }
216
217 SIMD_FORCE_INLINE btVector3
218 operator/(const btVector3& v, const btScalar& s)
219 {
220         btFullAssert(s != btScalar(0.0));
221         return v * (btScalar(1.0) / s);
222 }
223
224 SIMD_FORCE_INLINE btVector3
225 operator/(const btVector3& v1, const btVector3& v2)
226 {
227         return btVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z());
228 }
229
230 SIMD_FORCE_INLINE btScalar 
231 dot(const btVector3& v1, const btVector3& v2) 
232
233         return v1.dot(v2); 
234 }
235
236
237
238 SIMD_FORCE_INLINE btScalar
239 distance2(const btVector3& v1, const btVector3& v2) 
240
241         return v1.distance2(v2); 
242 }
243
244
245 SIMD_FORCE_INLINE btScalar
246 distance(const btVector3& v1, const btVector3& v2) 
247
248         return v1.distance(v2); 
249 }
250
251 SIMD_FORCE_INLINE btScalar
252 angle(const btVector3& v1, const btVector3& v2) 
253
254         return v1.angle(v2); 
255 }
256
257 SIMD_FORCE_INLINE btVector3 
258 cross(const btVector3& v1, const btVector3& v2) 
259
260         return v1.cross(v2); 
261 }
262
263 SIMD_FORCE_INLINE btScalar
264 triple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
265 {
266         return v1.triple(v2, v3);
267 }
268
269 SIMD_FORCE_INLINE btVector3 
270 lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
271 {
272         return v1.lerp(v2, t);
273 }
274
275
276 SIMD_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
281 SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
282 {
283         return (v - *this).length2();
284 }
285
286 SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
287 {
288         return (v - *this).length();
289 }
290
291 SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
292 {
293         return *this / length();
294
295
296 SIMD_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
309 class btVector4 : public btVector3
310 {
311 public:
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
409 SIMD_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
432 SIMD_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
442 SIMD_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