Making the C++ stuff work for the MipsPro 7.3 compiler.
[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 #if defined (__sgi)
28 #include <assert.h>
29 #else
30 #include <cassert>
31 #endif
32
33 #include "Tuple4.h"
34 #include "Vector3.h"
35
36 namespace MT {
37
38         template <typename Scalar>      
39         class Quaternion : public Tuple4<Scalar> {
40         public:
41                 Quaternion() {}
42                 
43                 template <typename Scalar2>
44                 explicit Quaternion(const Scalar2 *v) : Tuple4<Scalar>(v) {}
45                 
46                 template <typename Scalar2>
47                 Quaternion(const Scalar2& x, const Scalar2& y, const Scalar2& z, const Scalar2& w) 
48                         : Tuple4<Scalar>(x, y, z, w) 
49                 {}
50                 
51                 Quaternion(const Vector3<Scalar>& axis, const Scalar& angle) 
52                 { 
53                         setRotation(axis, angle); 
54                 }
55
56                 template <typename Scalar2>
57                 Quaternion(const Scalar2& yaw, const Scalar2& pitch, const Scalar2& roll)
58                 { 
59                         setEuler(yaw, pitch, roll); 
60                 }
61
62                 void setRotation(const Vector3<Scalar>& axis, const Scalar& angle)
63                 {
64                         Scalar d = axis.length();
65                         assert(d != Scalar(0.0));
66                         Scalar s = Scalar_traits<Scalar>::sin(angle * Scalar(0.5)) / d;
67                         setValue(axis[0] * s, axis[1] * s, axis[2] * s, 
68                                          Scalar_traits<Scalar>::cos(angle * Scalar(0.5)));
69                 }
70
71                 template <typename Scalar2>
72                 void setEuler(const Scalar2& yaw, const Scalar2& pitch, const Scalar2& roll)
73                 {
74                         Scalar halfYaw = Scalar(yaw) * Scalar(0.5);  
75                         Scalar halfPitch = Scalar(pitch) * Scalar(0.5);  
76                         Scalar halfRoll = Scalar(roll) * Scalar(0.5);  
77                         Scalar cosYaw = Scalar_traits<Scalar>::cos(halfYaw);
78                         Scalar sinYaw = Scalar_traits<Scalar>::sin(halfYaw);
79                         Scalar cosPitch = Scalar_traits<Scalar>::cos(halfPitch);
80                         Scalar sinPitch = Scalar_traits<Scalar>::sin(halfPitch);
81                         Scalar cosRoll = Scalar_traits<Scalar>::cos(halfRoll);
82                         Scalar sinRoll = Scalar_traits<Scalar>::sin(halfRoll);
83                         setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
84                                          cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
85                                          sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
86                                          cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
87                 }
88   
89                 Quaternion<Scalar>& operator+=(const Quaternion<Scalar>& q)
90                 {
91                         this->m_co[0] += q[0]; this->m_co[1] += q[1]; this->m_co[2] += q[2]; this->m_co[3] += q[3];
92                         return *this;
93                 }
94                 
95                 Quaternion<Scalar>& operator-=(const Quaternion<Scalar>& q) 
96                 {
97                         this->m_co[0] -= q[0]; this->m_co[1] -= q[1]; this->m_co[2] -= q[2]; this->m_co[3] -= q[3];
98                         return *this;
99                 }
100
101                 Quaternion<Scalar>& operator*=(const Scalar& s)
102                 {
103                         this->m_co[0] *= s; this->m_co[1] *= s; this->m_co[2] *= s; this->m_co[3] *= s;
104                         return *this;
105                 }
106                 
107                 Quaternion<Scalar>& operator/=(const Scalar& s) 
108                 {
109                         assert(s != Scalar(0.0));
110                         return *this *= Scalar(1.0) / s;
111                 }
112   
113                 Quaternion<Scalar>& operator*=(const Quaternion<Scalar>& q)
114                 {
115                         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],
116                                          this->m_co[3] * q[1] + this->m_co[1] * q[3] + this->m_co[2] * q[0] - this->m_co[0] * q[2],
117                                          this->m_co[3] * q[2] + this->m_co[2] * q[3] + this->m_co[0] * q[1] - this->m_co[1] * q[0],
118                                          this->m_co[3] * q[3] - this->m_co[0] * q[0] - this->m_co[1] * q[1] - this->m_co[2] * q[2]);
119                         return *this;
120                 }
121         
122                 Scalar dot(const Quaternion<Scalar>& q) const
123                 {
124                         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];
125                 }
126
127                 Scalar length2() const
128                 {
129                         return dot(*this);
130                 }
131
132                 Scalar length() const
133                 {
134                         return Scalar_traits<Scalar>::sqrt(length2());
135                 }
136
137                 Quaternion<Scalar>& normalize() 
138                 {
139                         return *this /= length();
140                 }
141                 
142                 Quaternion<Scalar> normalized() const 
143                 {
144                         return *this / length();
145                 } 
146
147                 Scalar angle(const Quaternion<Scalar>& q) const 
148                 {
149                         Scalar s = Scalar_traits<Scalar>::sqrt(length2() * q.length2());
150                         assert(s != Scalar(0.0));
151                         return Scalar_traits<Scalar>::acos(dot(q) / s);
152                 }
153    
154                 Quaternion<Scalar> conjugate() const 
155                 {
156                         return Quaternion<Scalar>(-this->m_co[0], -this->m_co[1], -this->m_co[2], this->m_co[3]);
157                 }
158
159                 Quaternion<Scalar> inverse() const
160                 {
161                         return conjugate / length2();
162                 }
163                 
164                 Quaternion<Scalar> slerp(const Quaternion<Scalar>& q, const Scalar& t) const
165                 {
166                         Scalar theta = angle(q);
167                         if (theta != Scalar(0.0))
168                         {
169                                 Scalar d = Scalar(1.0) / Scalar_traits<Scalar>::sin(theta);
170                                 Scalar s0 = Scalar_traits<Scalar>::sin((Scalar(1.0) - t) * theta);
171                                 Scalar s1 = Scalar_traits<Scalar>::sin(t * theta);   
172                                 return Quaternion<Scalar>((this->m_co[0] * s0 + q[0] * s1) * d,
173                                                                                   (this->m_co[1] * s0 + q[1] * s1) * d,
174                                                                                   (this->m_co[2] * s0 + q[2] * s1) * d,
175                                                                                   (this->m_co[3] * s0 + q[3] * s1) * d);
176                         }
177                         else
178                         {
179                                 return *this;
180                         }
181                 }
182
183                 static Quaternion<Scalar> random() 
184                 {
185                         // From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III, 
186             //       pg. 124-132
187                         Scalar x0 = Scalar_traits<Scalar>::random();
188                         Scalar r1 = Scalar_traits<Scalar>::sqrt(Scalar(1.0) - x0);
189                         Scalar r2 = Scalar_traits<Scalar>::sqrt(x0);
190                         Scalar t1 = Scalar_traits<Scalar>::TwoTimesPi() * Scalar_traits<Scalar>::random();
191                         Scalar t2 = Scalar_traits<Scalar>::TwoTimesPi() * Scalar_traits<Scalar>::random();
192                         Scalar c1 = Scalar_traits<Scalar>::cos(t1);
193                         Scalar s1 = Scalar_traits<Scalar>::sin(t1);
194                         Scalar c2 = Scalar_traits<Scalar>::cos(t2);
195                         Scalar s2 = Scalar_traits<Scalar>::sin(t2);
196                         return Quaternion<Scalar>(s1 * r1, c1 * r1, s2 * r2, c2 * r2);
197                 }
198
199         };
200
201         template <typename Scalar>
202         inline Quaternion<Scalar>
203         operator+(const Quaternion<Scalar>& q1, const Quaternion<Scalar>& q2)
204         {
205                 return Quaternion<Scalar>(q1[0] + q2[0], q1[1] + q2[1], q1[2] + q2[2], q1[3] + q2[3]);
206         }
207         
208         template <typename Scalar>
209         inline Quaternion<Scalar>
210         operator-(const Quaternion<Scalar>& q1, const Quaternion<Scalar>& q2)
211         {
212                 return Quaternion<Scalar>(q1[0] - q2[0], q1[1] - q2[1], q1[2] - q2[2], q1[3] - q2[3]);
213         }
214         
215         template <typename Scalar>
216         inline Quaternion<Scalar>
217         operator-(const Quaternion<Scalar>& q)
218         {
219                 return Quaternion<Scalar>(-q[0], -q[1], -q[2], -q[3]);
220         }
221
222         template <typename Scalar>
223         inline Quaternion<Scalar>
224         operator*(const Quaternion<Scalar>& q, const Scalar& s)
225         {
226                 return Quaternion<Scalar>(q[0] * s, q[1] * s, q[2] * s, q[3] * s);
227         }
228         
229         template <typename Scalar>
230         inline Quaternion<Scalar>
231         operator*(const Scalar& s, const Quaternion<Scalar>& q)
232         {
233                 return q * s;
234         }
235         
236         template <typename Scalar>
237         inline Quaternion<Scalar>
238         operator*(const Quaternion<Scalar>& q1, const Quaternion<Scalar>& q2) {
239                 return Quaternion<Scalar>(q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1],
240                                                                   q1[3] * q2[1] + q1[1] * q2[3] + q1[2] * q2[0] - q1[0] * q2[2],
241                                                                   q1[3] * q2[2] + q1[2] * q2[3] + q1[0] * q2[1] - q1[1] * q2[0],
242                                                                   q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2]); 
243         }
244         
245         template <typename Scalar>
246         inline Quaternion<Scalar>
247         operator*(const Quaternion<Scalar>& q, const Vector3<Scalar>& w)
248         {
249                 return Quaternion<Scalar>( q[3] * w[0] + q[1] * w[2] - q[2] * w[1],
250                                                                    q[3] * w[1] + q[2] * w[0] - q[0] * w[2],
251                                                                    q[3] * w[2] + q[0] * w[1] - q[1] * w[0],
252                                                                   -q[0] * w[0] - q[1] * w[1] - q[2] * w[2]); 
253         }
254         
255         template <typename Scalar>
256         inline Quaternion<Scalar>
257         operator*(const Vector3<Scalar>& w, const Quaternion<Scalar>& q)
258         {
259                 return Quaternion<Scalar>( w[0] * q[3] + w[1] * q[2] - w[2] * q[1],
260                                                                    w[1] * q[3] + w[2] * q[0] - w[0] * q[2],
261                                                                    w[2] * q[3] + w[0] * q[1] - w[1] * q[0],
262                                                                   -w[0] * q[0] - w[1] * q[1] - w[2] * q[2]); 
263         }
264         
265         template <typename Scalar>
266         inline Scalar 
267         dot(const Quaternion<Scalar>& q1, const Quaternion<Scalar>& q2) 
268         { 
269                 return q1.dot(q2); 
270         }
271
272         template <typename Scalar>
273         inline Scalar
274         length2(const Quaternion<Scalar>& q) 
275         { 
276                 return q.length2(); 
277         }
278
279         template <typename Scalar>
280         inline Scalar
281         length(const Quaternion<Scalar>& q) 
282         { 
283                 return q.length(); 
284         }
285
286         template <typename Scalar>
287         inline Scalar
288         angle(const Quaternion<Scalar>& q1, const Quaternion<Scalar>& q2) 
289         { 
290                 return q1.angle(q2); 
291         }
292
293         template <typename Scalar>
294         inline Quaternion<Scalar>
295         conjugate(const Quaternion<Scalar>& q) 
296         {
297                 return q.conjugate();
298         }
299
300         template <typename Scalar>
301         inline Quaternion<Scalar>
302         inverse(const Quaternion<Scalar>& q) 
303         {
304                 return q.inverse();
305         }
306
307         template <typename Scalar>
308         inline Quaternion<Scalar>
309         slerp(const Quaternion<Scalar>& q1, const Quaternion<Scalar>& q2, const Scalar& t) 
310         {
311                 return q1.slerp(q2, t);
312         }
313         
314 }
315
316 #endif