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