Synchronized Bullet physics to latest version.
[blender-staging.git] / extern / bullet / LinearMath / SimdQuaternion.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__QUATERNION_H_
18 #define SIMD__QUATERNION_H_
19
20 #include "SimdVector3.h"
21
22 class SimdQuaternion : public SimdQuadWord {
23 public:
24         SimdQuaternion() {}
25
26         //              template <typename SimdScalar>
27         //              explicit Quaternion(const SimdScalar *v) : Tuple4<SimdScalar>(v) {}
28
29         SimdQuaternion(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z, const SimdScalar& w) 
30                 : SimdQuadWord(x, y, z, w) 
31         {}
32
33         SimdQuaternion(const SimdVector3& axis, const SimdScalar& angle) 
34         { 
35                 setRotation(axis, angle); 
36         }
37
38         SimdQuaternion(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll)
39         { 
40                 setEuler(yaw, pitch, roll); 
41         }
42
43         void setRotation(const SimdVector3& axis, const SimdScalar& angle)
44         {
45                 SimdScalar d = axis.length();
46                 assert(d != SimdScalar(0.0));
47                 SimdScalar s = SimdSin(angle * SimdScalar(0.5)) / d;
48                 setValue(axis.x() * s, axis.y() * s, axis.z() * s, 
49                         SimdCos(angle * SimdScalar(0.5)));
50         }
51
52         void setEuler(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll)
53         {
54                 SimdScalar halfYaw = SimdScalar(yaw) * SimdScalar(0.5);  
55                 SimdScalar halfPitch = SimdScalar(pitch) * SimdScalar(0.5);  
56                 SimdScalar halfRoll = SimdScalar(roll) * SimdScalar(0.5);  
57                 SimdScalar cosYaw = SimdCos(halfYaw);
58                 SimdScalar sinYaw = SimdSin(halfYaw);
59                 SimdScalar cosPitch = SimdCos(halfPitch);
60                 SimdScalar sinPitch = SimdSin(halfPitch);
61                 SimdScalar cosRoll = SimdCos(halfRoll);
62                 SimdScalar sinRoll = SimdSin(halfRoll);
63                 setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
64                         cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
65                         sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
66                         cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
67         }
68
69         SimdQuaternion& operator+=(const SimdQuaternion& q)
70         {
71                 m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q[3];
72                 return *this;
73         }
74
75         SimdQuaternion& operator-=(const SimdQuaternion& q) 
76         {
77                 m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q[3];
78                 return *this;
79         }
80
81         SimdQuaternion& operator*=(const SimdScalar& s)
82         {
83                 m_x *= s; m_y *= s; m_z *= s; m_unusedW *= s;
84                 return *this;
85         }
86
87
88         SimdQuaternion& operator*=(const SimdQuaternion& q)
89         {
90                 setValue(m_unusedW * q.x() + m_x * q[3] + m_y * q.z() - m_z * q.y(),
91                         m_unusedW * q.y() + m_y * q[3] + m_z * q.x() - m_x * q.z(),
92                         m_unusedW * q.z() + m_z * q[3] + m_x * q.y() - m_y * q.x(),
93                         m_unusedW * q[3] - m_x * q.x() - m_y * q.y() - m_z * q.z());
94                 return *this;
95         }
96
97         SimdScalar dot(const SimdQuaternion& q) const
98         {
99                 return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q[3];
100         }
101
102         SimdScalar length2() const
103         {
104                 return dot(*this);
105         }
106
107         SimdScalar length() const
108         {
109                 return SimdSqrt(length2());
110         }
111
112         SimdQuaternion& normalize() 
113         {
114                 return *this /= length();
115         }
116
117         SIMD_FORCE_INLINE SimdQuaternion
118         operator*(const SimdScalar& s) const
119         {
120                 return SimdQuaternion(x() * s, y() * s, z() * s, m_unusedW * s);
121         }
122
123
124
125         SimdQuaternion operator/(const SimdScalar& s) const
126         {
127                 assert(s != SimdScalar(0.0));
128                 return *this * (SimdScalar(1.0) / s);
129         }
130
131
132         SimdQuaternion& operator/=(const SimdScalar& s) 
133         {
134                 assert(s != SimdScalar(0.0));
135                 return *this *= SimdScalar(1.0) / s;
136         }
137
138
139         SimdQuaternion normalized() const 
140         {
141                 return *this / length();
142         } 
143
144         SimdScalar angle(const SimdQuaternion& q) const 
145         {
146                 SimdScalar s = SimdSqrt(length2() * q.length2());
147                 assert(s != SimdScalar(0.0));
148                 return SimdAcos(dot(q) / s);
149         }
150
151         SimdScalar getAngle() const 
152         {
153                 SimdScalar s = 2.f * SimdAcos(m_unusedW);
154                 return s;
155         }
156
157
158
159         SimdQuaternion inverse() const
160         {
161                 return SimdQuaternion(m_x, m_y, m_z, -m_unusedW);
162         }
163
164         SIMD_FORCE_INLINE SimdQuaternion
165         operator+(const SimdQuaternion& q2) const
166         {
167                 const SimdQuaternion& q1 = *this;
168                 return SimdQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1[3] + q2[3]);
169         }
170
171         SIMD_FORCE_INLINE SimdQuaternion
172         operator-(const SimdQuaternion& q2) const
173         {
174                 const SimdQuaternion& q1 = *this;
175                 return SimdQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1[3] - q2[3]);
176         }
177
178         SIMD_FORCE_INLINE SimdQuaternion operator-() const
179         {
180                 const SimdQuaternion& q2 = *this;
181                 return SimdQuaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2[3]);
182         }
183
184         SIMD_FORCE_INLINE SimdQuaternion farthest( const SimdQuaternion& qd) const 
185         {
186                 SimdQuaternion diff,sum;
187                 diff = *this - qd;
188                 sum = *this + qd;
189                 if( diff.dot(diff) > sum.dot(sum) )
190                         return qd;
191                 return (-qd);
192         }
193
194         SimdQuaternion slerp(const SimdQuaternion& q, const SimdScalar& t) const
195         {
196                 SimdScalar theta = angle(q);
197                 if (theta != SimdScalar(0.0))
198                 {
199                         SimdScalar d = SimdScalar(1.0) / SimdSin(theta);
200                         SimdScalar s0 = SimdSin((SimdScalar(1.0) - t) * theta);
201                         SimdScalar s1 = SimdSin(t * theta);   
202                         return SimdQuaternion((m_x * s0 + q.x() * s1) * d,
203                                 (m_y * s0 + q.y() * s1) * d,
204                                 (m_z * s0 + q.z() * s1) * d,
205                                 (m_unusedW * s0 + q[3] * s1) * d);
206                 }
207                 else
208                 {
209                         return *this;
210                 }
211         }
212
213         
214
215 };
216
217
218
219 SIMD_FORCE_INLINE SimdQuaternion
220 operator-(const SimdQuaternion& q)
221 {
222         return SimdQuaternion(-q.x(), -q.y(), -q.z(), -q[3]);
223 }
224
225
226
227
228 SIMD_FORCE_INLINE SimdQuaternion
229 operator*(const SimdQuaternion& q1, const SimdQuaternion& q2) {
230         return SimdQuaternion(q1[3] * q2.x() + q1.x() * q2[3] + q1.y() * q2.z() - q1.z() * q2.y(),
231                 q1[3] * q2.y() + q1.y() * q2[3] + q1.z() * q2.x() - q1.x() * q2.z(),
232                 q1[3] * q2.z() + q1.z() * q2[3] + q1.x() * q2.y() - q1.y() * q2.x(),
233                 q1[3] * q2[3] - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); 
234 }
235
236 SIMD_FORCE_INLINE SimdQuaternion
237 operator*(const SimdQuaternion& q, const SimdVector3& w)
238 {
239         return SimdQuaternion( q[3] * w.x() + q.y() * w.z() - q.z() * w.y(),
240                 q[3] * w.y() + q.z() * w.x() - q.x() * w.z(),
241                 q[3] * w.z() + q.x() * w.y() - q.y() * w.x(),
242                 -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); 
243 }
244
245 SIMD_FORCE_INLINE SimdQuaternion
246 operator*(const SimdVector3& w, const SimdQuaternion& q)
247 {
248         return SimdQuaternion( w.x() * q[3] + w.y() * q.z() - w.z() * q.y(),
249                 w.y() * q[3] + w.z() * q.x() - w.x() * q.z(),
250                 w.z() * q[3] + w.x() * q.y() - w.y() * q.x(),
251                 -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); 
252 }
253
254 SIMD_FORCE_INLINE SimdScalar 
255 dot(const SimdQuaternion& q1, const SimdQuaternion& q2) 
256
257         return q1.dot(q2); 
258 }
259
260
261 SIMD_FORCE_INLINE SimdScalar
262 length(const SimdQuaternion& q) 
263
264         return q.length(); 
265 }
266
267 SIMD_FORCE_INLINE SimdScalar
268 angle(const SimdQuaternion& q1, const SimdQuaternion& q2) 
269
270         return q1.angle(q2); 
271 }
272
273
274 SIMD_FORCE_INLINE SimdQuaternion
275 inverse(const SimdQuaternion& q) 
276 {
277         return q.inverse();
278 }
279
280 SIMD_FORCE_INLINE SimdQuaternion
281 slerp(const SimdQuaternion& q1, const SimdQuaternion& q2, const SimdScalar& t) 
282 {
283         return q1.slerp(q2, t);
284 }
285
286
287 #endif
288
289
290