Compile fixes for gcc 3.4
[blender-staging.git] / extern / solid / include / MT / Quaternion.h
1 /*
2  * SOLID - Software Library for Interference Detection
3  * 
4  * Copyright (C) 2001-2003  Dtecta.  All rights reserved.
5  *
6  * This library may be distributed under the terms of the Q Public License
7  * (QPL) as defined by Trolltech AS of Norway and appearing in the file
8  * LICENSE.QPL included in the packaging of this file.
9  *
10  * This library may be distributed and/or modified under the terms of the
11  * GNU General Public License (GPL) version 2 as published by the Free Software
12  * Foundation and appearing in the file LICENSE.GPL included in the
13  * packaging of this file.
14  *
15  * This library is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
16  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
17  *
18  * Commercial use or any other use of this library not covered by either 
19  * the QPL or the GPL requires an additional license from Dtecta. 
20  * Please contact info@dtecta.com for enquiries about the terms of commercial
21  * use of this library.
22  */
23
24 #ifndef QUATERNION_H
25 #define QUATERNION_H
26
27 #include <cassert>
28
29 #include "Tuple4.h"
30 #include "Vector3.h"
31
32 namespace MT {
33
34         template <typename Scalar>      
35         class Quaternion : public Tuple4<Scalar> {
36         public:
37                 Quaternion() {}
38                 
39                 template <typename Scalar2>
40                 explicit Quaternion(const Scalar2 *v) : Tuple4<Scalar>(v) {}
41                 
42                 template <typename Scalar2>
43                 Quaternion(const Scalar2& x, const Scalar2& y, const Scalar2& z, const Scalar2& w) 
44                         : Tuple4<Scalar>(x, y, z, w) 
45                 {}
46                 
47                 Quaternion(const Vector3<Scalar>& axis, const Scalar& angle) 
48                 { 
49                         setRotation(axis, angle); 
50                 }
51
52                 template <typename Scalar2>
53                 Quaternion(const Scalar2& yaw, const Scalar2& pitch, const Scalar2& roll)
54                 { 
55                         setEuler(yaw, pitch, roll); 
56                 }
57
58                 void setRotation(const Vector3<Scalar>& axis, const Scalar& angle)
59                 {
60                         Scalar d = axis.length();
61                         assert(d != Scalar(0.0));
62                         Scalar s = Scalar_traits<Scalar>::sin(angle * Scalar(0.5)) / d;
63                         setValue(axis[0] * s, axis[1] * s, axis[2] * s, 
64                                          Scalar_traits<Scalar>::cos(angle * Scalar(0.5)));
65                 }
66
67                 template <typename Scalar2>
68                 void setEuler(const Scalar2& yaw, const Scalar2& pitch, const Scalar2& roll)
69                 {
70                         Scalar halfYaw = Scalar(yaw) * Scalar(0.5);  
71                         Scalar halfPitch = Scalar(pitch) * Scalar(0.5);  
72                         Scalar halfRoll = Scalar(roll) * Scalar(0.5);  
73                         Scalar cosYaw = Scalar_traits<Scalar>::cos(halfYaw);
74                         Scalar sinYaw = Scalar_traits<Scalar>::sin(halfYaw);
75                         Scalar cosPitch = Scalar_traits<Scalar>::cos(halfPitch);
76                         Scalar sinPitch = Scalar_traits<Scalar>::sin(halfPitch);
77                         Scalar cosRoll = Scalar_traits<Scalar>::cos(halfRoll);
78                         Scalar sinRoll = Scalar_traits<Scalar>::sin(halfRoll);
79                         setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
80                                          cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
81                                          sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
82                                          cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
83                 }
84   
85                 Quaternion<Scalar>& operator+=(const Quaternion<Scalar>& q)
86                 {
87                         this->m_co[0] += q[0]; this->m_co[1] += q[1]; this->m_co[2] += q[2]; this->m_co[3] += q[3];
88                         return *this;
89                 }
90                 
91                 Quaternion<Scalar>& operator-=(const Quaternion<Scalar>& q) 
92                 {
93                         this->m_co[0] -= q[0]; this->m_co[1] -= q[1]; this->m_co[2] -= q[2]; this->m_co[3] -= q[3];
94                         return *this;
95                 }
96
97                 Quaternion<Scalar>& operator*=(const Scalar& s)
98                 {
99                         this->m_co[0] *= s; this->m_co[1] *= s; this->m_co[2] *= s; this->m_co[3] *= s;
100                         return *this;
101                 }
102                 
103                 Quaternion<Scalar>& operator/=(const Scalar& s) 
104                 {
105                         assert(s != Scalar(0.0));
106                         return *this *= Scalar(1.0) / s;
107                 }
108   
109                 Quaternion<Scalar>& operator*=(const Quaternion<Scalar>& q)
110                 {
111                         setValue(this->m_co[3] * q[0] + this->m_co[0] * q[3] + this->m_co[1] * q[2] - this->m_co[2] * q[1],
112                                          this->m_co[3] * q[1] + this->m_co[1] * q[3] + this->m_co[2] * q[0] - this->m_co[0] * q[2],
113                                          this->m_co[3] * q[2] + this->m_co[2] * q[3] + this->m_co[0] * q[1] - this->m_co[1] * q[0],
114                                          this->m_co[3] * q[3] - this->m_co[0] * q[0] - this->m_co[1] * q[1] - this->m_co[2] * q[2]);
115                         return *this;
116                 }
117         
118                 Scalar dot(const Quaternion<Scalar>& q) const
119                 {
120                         return this->m_co[0] * q[0] + this->m_co[1] * q[1] + this->m_co[2] * q[2] + this->m_co[3] * q[3];
121                 }
122
123                 Scalar length2() const
124                 {
125                         return dot(*this);
126                 }
127
128                 Scalar length() const
129                 {
130                         return Scalar_traits<Scalar>::sqrt(length2());
131                 }
132
133                 Quaternion<Scalar>& normalize() 
134                 {
135                         return *this /= length();
136                 }
137                 
138                 Quaternion<Scalar> normalized() const 
139                 {
140                         return *this / length();
141                 } 
142
143                 Scalar angle(const Quaternion<Scalar>& q) const 
144                 {
145                         Scalar s = Scalar_traits<Scalar>::sqrt(length2() * q.length2());
146                         assert(s != Scalar(0.0));
147                         return Scalar_traits<Scalar>::acos(dot(q) / s);
148                 }
149    
150                 Quaternion<Scalar> conjugate() const 
151                 {
152                         return Quaternion<Scalar>(-this->m_co[0], -this->m_co[1], -this->m_co[2], this->m_co[3]);
153                 }
154
155                 Quaternion<Scalar> inverse() const
156                 {
157                         return conjugate / length2();
158                 }
159                 
160                 Quaternion<Scalar> slerp(const Quaternion<Scalar>& q, const Scalar& t) const
161                 {
162                         Scalar theta = angle(q);
163                         if (theta != Scalar(0.0))
164                         {
165                                 Scalar d = Scalar(1.0) / Scalar_traits<Scalar>::sin(theta);
166                                 Scalar s0 = Scalar_traits<Scalar>::sin((Scalar(1.0) - t) * theta);
167                                 Scalar s1 = Scalar_traits<Scalar>::sin(t * theta);   
168                                 return Quaternion<Scalar>((this->m_co[0] * s0 + q[0] * s1) * d,
169                                                                                   (this->m_co[1] * s0 + q[1] * s1) * d,
170                                                                                   (this->m_co[2] * s0 + q[2] * s1) * d,
171                                                                                   (this->m_co[3] * s0 + q[3] * s1) * d);
172                         }
173                         else
174                         {
175                                 return *this;
176                         }
177                 }
178
179                 static Quaternion<Scalar> random() 
180                 {
181                         // From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III, 
182             //       pg. 124-132
183                         Scalar x0 = Scalar_traits<Scalar>::random();
184                         Scalar r1 = Scalar_traits<Scalar>::sqrt(Scalar(1.0) - x0);
185                         Scalar r2 = Scalar_traits<Scalar>::sqrt(x0);
186                         Scalar t1 = Scalar_traits<Scalar>::TwoTimesPi() * Scalar_traits<Scalar>::random();
187                         Scalar t2 = Scalar_traits<Scalar>::TwoTimesPi() * Scalar_traits<Scalar>::random();
188                         Scalar c1 = Scalar_traits<Scalar>::cos(t1);
189                         Scalar s1 = Scalar_traits<Scalar>::sin(t1);
190                         Scalar c2 = Scalar_traits<Scalar>::cos(t2);
191                         Scalar s2 = Scalar_traits<Scalar>::sin(t2);
192                         return Quaternion<Scalar>(s1 * r1, c1 * r1, s2 * r2, c2 * r2);
193                 }
194
195         };
196
197         template <typename Scalar>
198         inline Quaternion<Scalar>
199         operator+(const Quaternion<Scalar>& q1, const Quaternion<Scalar>& q2)
200         {
201                 return Quaternion<Scalar>(q1[0] + q2[0], q1[1] + q2[1], q1[2] + q2[2], q1[3] + q2[3]);
202         }
203         
204         template <typename Scalar>
205         inline Quaternion<Scalar>
206         operator-(const Quaternion<Scalar>& q1, const Quaternion<Scalar>& q2)
207         {
208                 return Quaternion<Scalar>(q1[0] - q2[0], q1[1] - q2[1], q1[2] - q2[2], q1[3] - q2[3]);
209         }
210         
211         template <typename Scalar>
212         inline Quaternion<Scalar>
213         operator-(const Quaternion<Scalar>& q)
214         {
215                 return Quaternion<Scalar>(-q[0], -q[1], -q[2], -q[3]);
216         }
217
218         template <typename Scalar>
219         inline Quaternion<Scalar>
220         operator*(const Quaternion<Scalar>& q, const Scalar& s)
221         {
222                 return Quaternion<Scalar>(q[0] * s, q[1] * s, q[2] * s, q[3] * s);
223         }
224         
225         template <typename Scalar>
226         inline Quaternion<Scalar>
227         operator*(const Scalar& s, const Quaternion<Scalar>& q)
228         {
229                 return q * s;
230         }
231         
232         template <typename Scalar>
233         inline Quaternion<Scalar>
234         operator*(const Quaternion<Scalar>& q1, const Quaternion<Scalar>& q2) {
235                 return Quaternion<Scalar>(q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1],
236                                                                   q1[3] * q2[1] + q1[1] * q2[3] + q1[2] * q2[0] - q1[0] * q2[2],
237                                                                   q1[3] * q2[2] + q1[2] * q2[3] + q1[0] * q2[1] - q1[1] * q2[0],
238                                                                   q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2]); 
239         }
240         
241         template <typename Scalar>
242         inline Quaternion<Scalar>
243         operator*(const Quaternion<Scalar>& q, const Vector3<Scalar>& w)
244         {
245                 return Quaternion<Scalar>( q[3] * w[0] + q[1] * w[2] - q[2] * w[1],
246                                                                    q[3] * w[1] + q[2] * w[0] - q[0] * w[2],
247                                                                    q[3] * w[2] + q[0] * w[1] - q[1] * w[0],
248                                                                   -q[0] * w[0] - q[1] * w[1] - q[2] * w[2]); 
249         }
250         
251         template <typename Scalar>
252         inline Quaternion<Scalar>
253         operator*(const Vector3<Scalar>& w, const Quaternion<Scalar>& q)
254         {
255                 return Quaternion<Scalar>( w[0] * q[3] + w[1] * q[2] - w[2] * q[1],
256                                                                    w[1] * q[3] + w[2] * q[0] - w[0] * q[2],
257                                                                    w[2] * q[3] + w[0] * q[1] - w[1] * q[0],
258                                                                   -w[0] * q[0] - w[1] * q[1] - w[2] * q[2]); 
259         }
260         
261         template <typename Scalar>
262         inline Scalar 
263         dot(const Quaternion<Scalar>& q1, const Quaternion<Scalar>& q2) 
264         { 
265                 return q1.dot(q2); 
266         }
267
268         template <typename Scalar>
269         inline Scalar
270         length2(const Quaternion<Scalar>& q) 
271         { 
272                 return q.length2(); 
273         }
274
275         template <typename Scalar>
276         inline Scalar
277         length(const Quaternion<Scalar>& q) 
278         { 
279                 return q.length(); 
280         }
281
282         template <typename Scalar>
283         inline Scalar
284         angle(const Quaternion<Scalar>& q1, const Quaternion<Scalar>& q2) 
285         { 
286                 return q1.angle(q2); 
287         }
288
289         template <typename Scalar>
290         inline Quaternion<Scalar>
291         conjugate(const Quaternion<Scalar>& q) 
292         {
293                 return q.conjugate();
294         }
295
296         template <typename Scalar>
297         inline Quaternion<Scalar>
298         inverse(const Quaternion<Scalar>& q) 
299         {
300                 return q.inverse();
301         }
302
303         template <typename Scalar>
304         inline Quaternion<Scalar>
305         slerp(const Quaternion<Scalar>& q1, const Quaternion<Scalar>& q2, const Scalar& t) 
306         {
307                 return q1.slerp(q2, t);
308         }
309         
310 }
311
312 #endif