Minor changes in Bullet:
[blender-staging.git] / extern / bullet / LinearMath / SimdQuaternion.h
1 /*
2
3 Copyright (c) 2005 Gino van den Bergen / Erwin Coumans http://continuousphysics.com
4
5 Permission is hereby granted, free of charge, to any person or organization
6 obtaining a copy of the software and accompanying documentation covered by
7 this license (the "Software") to use, reproduce, display, distribute,
8 execute, and transmit the Software, and to prepare derivative works of the
9 Software, and to permit third-parties to whom the Software is furnished to
10 do so, all subject to the following:
11
12 The copyright notices in the Software and this entire statement, including
13 the above license grant, this restriction and the following disclaimer,
14 must be included in all copies of the Software, in whole or in part, and
15 all derivative works of the Software, unless such copies or derivative
16 works are solely in the form of machine-executable object code generated by
17 a source language processor.
18
19 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
22 SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
23 FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
24 ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
25 DEALINGS IN THE SOFTWARE.
26 */
27
28
29 #ifndef SIMD__QUATERNION_H_
30 #define SIMD__QUATERNION_H_
31
32 #include "SimdVector3.h"
33
34 class SimdQuaternion : public SimdQuadWord {
35 public:
36         SimdQuaternion() {}
37
38         //              template <typename SimdScalar>
39         //              explicit Quaternion(const SimdScalar *v) : Tuple4<SimdScalar>(v) {}
40
41         SimdQuaternion(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z, const SimdScalar& w) 
42                 : SimdQuadWord(x, y, z, w) 
43         {}
44
45         SimdQuaternion(const SimdVector3& axis, const SimdScalar& angle) 
46         { 
47                 setRotation(axis, angle); 
48         }
49
50         SimdQuaternion(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll)
51         { 
52                 setEuler(yaw, pitch, roll); 
53         }
54
55         void setRotation(const SimdVector3& axis, const SimdScalar& angle)
56         {
57                 SimdScalar d = axis.length();
58                 assert(d != SimdScalar(0.0));
59                 SimdScalar s = SimdSin(angle * SimdScalar(0.5)) / d;
60                 setValue(axis.x() * s, axis.y() * s, axis.z() * s, 
61                         SimdCos(angle * SimdScalar(0.5)));
62         }
63
64         void setEuler(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll)
65         {
66                 SimdScalar halfYaw = SimdScalar(yaw) * SimdScalar(0.5);  
67                 SimdScalar halfPitch = SimdScalar(pitch) * SimdScalar(0.5);  
68                 SimdScalar halfRoll = SimdScalar(roll) * SimdScalar(0.5);  
69                 SimdScalar cosYaw = SimdCos(halfYaw);
70                 SimdScalar sinYaw = SimdSin(halfYaw);
71                 SimdScalar cosPitch = SimdCos(halfPitch);
72                 SimdScalar sinPitch = SimdSin(halfPitch);
73                 SimdScalar cosRoll = SimdCos(halfRoll);
74                 SimdScalar sinRoll = SimdSin(halfRoll);
75                 setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
76                         cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
77                         sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
78                         cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
79         }
80
81         SimdQuaternion& operator+=(const SimdQuaternion& q)
82         {
83                 m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q[3];
84                 return *this;
85         }
86
87         SimdQuaternion& operator-=(const SimdQuaternion& q) 
88         {
89                 m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q[3];
90                 return *this;
91         }
92
93         SimdQuaternion& operator*=(const SimdScalar& s)
94         {
95                 m_x *= s; m_y *= s; m_z *= s; m_unusedW *= s;
96                 return *this;
97         }
98
99
100         SimdQuaternion& operator*=(const SimdQuaternion& q)
101         {
102                 setValue(m_unusedW * q.x() + m_x * q[3] + m_y * q.z() - m_z * q.y(),
103                         m_unusedW * q.y() + m_y * q[3] + m_z * q.x() - m_x * q.z(),
104                         m_unusedW * q.z() + m_z * q[3] + m_x * q.y() - m_y * q.x(),
105                         m_unusedW * q[3] - m_x * q.x() - m_y * q.y() - m_z * q.z());
106                 return *this;
107         }
108
109         SimdScalar dot(const SimdQuaternion& q) const
110         {
111                 return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q[3];
112         }
113
114         SimdScalar length2() const
115         {
116                 return dot(*this);
117         }
118
119         SimdScalar length() const
120         {
121                 return SimdSqrt(length2());
122         }
123
124         SimdQuaternion& normalize() 
125         {
126                 return *this /= length();
127         }
128
129         SIMD_FORCE_INLINE SimdQuaternion
130         operator*(const SimdScalar& s) const
131         {
132                 return SimdQuaternion(x() * s, y() * s, z() * s, m_unusedW * s);
133         }
134
135
136
137         SimdQuaternion operator/(const SimdScalar& s) const
138         {
139                 assert(s != SimdScalar(0.0));
140                 return *this * (SimdScalar(1.0) / s);
141         }
142
143
144         SimdQuaternion& operator/=(const SimdScalar& s) 
145         {
146                 assert(s != SimdScalar(0.0));
147                 return *this *= SimdScalar(1.0) / s;
148         }
149
150
151         SimdQuaternion normalized() const 
152         {
153                 return *this / length();
154         } 
155
156         SimdScalar angle(const SimdQuaternion& q) const 
157         {
158                 SimdScalar s = SimdSqrt(length2() * q.length2());
159                 assert(s != SimdScalar(0.0));
160                 return SimdAcos(dot(q) / s);
161         }
162
163         SimdScalar getAngle() const 
164         {
165                 SimdScalar s = 2.f * SimdAcos(m_unusedW);
166                 return s;
167         }
168
169
170
171         SimdQuaternion inverse() const
172         {
173                 return SimdQuaternion(m_x, m_y, m_z, -m_unusedW);
174         }
175
176         SIMD_FORCE_INLINE SimdQuaternion
177         operator+(const SimdQuaternion& q2) const
178         {
179                 const SimdQuaternion& q1 = *this;
180                 return SimdQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1[3] + q2[3]);
181         }
182
183         SIMD_FORCE_INLINE SimdQuaternion
184         operator-(const SimdQuaternion& q2) const
185         {
186                 const SimdQuaternion& q1 = *this;
187                 return SimdQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1[3] - q2[3]);
188         }
189
190         SIMD_FORCE_INLINE SimdQuaternion operator-() const
191         {
192                 const SimdQuaternion& q2 = *this;
193                 return SimdQuaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2[3]);
194         }
195
196         SIMD_FORCE_INLINE SimdQuaternion farthest( const SimdQuaternion& qd) const 
197         {
198                 SimdQuaternion diff,sum;
199                 diff = *this - qd;
200                 sum = *this + qd;
201                 if( diff.dot(diff) > sum.dot(sum) )
202                         return qd;
203                 return (-qd);
204         }
205
206         SimdQuaternion slerp(const SimdQuaternion& q, const SimdScalar& t) const
207         {
208                 SimdScalar theta = angle(q);
209                 if (theta != SimdScalar(0.0))
210                 {
211                         SimdScalar d = SimdScalar(1.0) / SimdSin(theta);
212                         SimdScalar s0 = SimdSin((SimdScalar(1.0) - t) * theta);
213                         SimdScalar s1 = SimdSin(t * theta);   
214                         return SimdQuaternion((m_x * s0 + q.x() * s1) * d,
215                                 (m_y * s0 + q.y() * s1) * d,
216                                 (m_z * s0 + q.z() * s1) * d,
217                                 (m_unusedW * s0 + q[3] * s1) * d);
218                 }
219                 else
220                 {
221                         return *this;
222                 }
223         }
224
225         
226
227 };
228
229
230
231 SIMD_FORCE_INLINE SimdQuaternion
232 operator-(const SimdQuaternion& q)
233 {
234         return SimdQuaternion(-q.x(), -q.y(), -q.z(), -q[3]);
235 }
236
237
238
239
240 SIMD_FORCE_INLINE SimdQuaternion
241 operator*(const SimdQuaternion& q1, const SimdQuaternion& q2) {
242         return SimdQuaternion(q1[3] * q2.x() + q1.x() * q2[3] + q1.y() * q2.z() - q1.z() * q2.y(),
243                 q1[3] * q2.y() + q1.y() * q2[3] + q1.z() * q2.x() - q1.x() * q2.z(),
244                 q1[3] * q2.z() + q1.z() * q2[3] + q1.x() * q2.y() - q1.y() * q2.x(),
245                 q1[3] * q2[3] - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); 
246 }
247
248 SIMD_FORCE_INLINE SimdQuaternion
249 operator*(const SimdQuaternion& q, const SimdVector3& w)
250 {
251         return SimdQuaternion( q[3] * w.x() + q.y() * w.z() - q.z() * w.y(),
252                 q[3] * w.y() + q.z() * w.x() - q.x() * w.z(),
253                 q[3] * w.z() + q.x() * w.y() - q.y() * w.x(),
254                 -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); 
255 }
256
257 SIMD_FORCE_INLINE SimdQuaternion
258 operator*(const SimdVector3& w, const SimdQuaternion& q)
259 {
260         return SimdQuaternion( w.x() * q[3] + w.y() * q.z() - w.z() * q.y(),
261                 w.y() * q[3] + w.z() * q.x() - w.x() * q.z(),
262                 w.z() * q[3] + w.x() * q.y() - w.y() * q.x(),
263                 -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); 
264 }
265
266 SIMD_FORCE_INLINE SimdScalar 
267 dot(const SimdQuaternion& q1, const SimdQuaternion& q2) 
268
269         return q1.dot(q2); 
270 }
271
272
273 SIMD_FORCE_INLINE SimdScalar
274 length(const SimdQuaternion& q) 
275
276         return q.length(); 
277 }
278
279 SIMD_FORCE_INLINE SimdScalar
280 angle(const SimdQuaternion& q1, const SimdQuaternion& q2) 
281
282         return q1.angle(q2); 
283 }
284
285
286 SIMD_FORCE_INLINE SimdQuaternion
287 inverse(const SimdQuaternion& q) 
288 {
289         return q.inverse();
290 }
291
292 SIMD_FORCE_INLINE SimdQuaternion
293 slerp(const SimdQuaternion& q1, const SimdQuaternion& q2, const SimdScalar& t) 
294 {
295         return q1.slerp(q2, t);
296 }
297
298
299 #endif
300
301
302