Added the Solid 3.5 sources to the blender source tree.
[blender-staging.git] / extern / solid / include / MT / Matrix3x3.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 MATRIX3X3_H
25 #define MATRIX3X3_H
26
27 #include <cassert>
28
29 #include "Vector3.h"
30 #include "Quaternion.h"
31
32 namespace MT {
33
34   // Row-major 3x3 matrix
35   
36         template <typename Scalar>
37         class Matrix3x3 {
38         public:
39                 Matrix3x3() {}
40                 
41                 template <typename Scalar2>
42                 explicit Matrix3x3(const Scalar2 *m) { setValue(m); }
43                 
44                 explicit Matrix3x3(const Quaternion<Scalar>& q) { setRotation(q); }
45                 
46                 template <typename Scalar2>
47                 Matrix3x3(const Scalar2& yaw, const Scalar2& pitch, const Scalar2& roll)
48                 { 
49                         setEuler(yaw, pitch, roll);
50                 }
51                 
52                 template <typename Scalar2>
53                 Matrix3x3(const Scalar2& xx, const Scalar2& xy, const Scalar2& xz,
54                                   const Scalar2& yx, const Scalar2& yy, const Scalar2& yz,
55                                   const Scalar2& zx, const Scalar2& zy, const Scalar2& zz)
56                 { 
57                         setValue(xx, xy, xz, 
58                                          yx, yy, yz, 
59                                          zx, zy, zz);
60                 }
61                 
62                 Vector3<Scalar>&  operator[](int i)
63                 { 
64                         assert(0 <= i && i < 3);
65                         return m_el[i]; 
66                 }
67                 
68                 const Vector3<Scalar>& operator[](int i) const
69                 {
70                         assert(0 <= i && i < 3);
71                         return m_el[i]; 
72                 }
73                 
74                 Matrix3x3<Scalar>& operator*=(const Matrix3x3<Scalar>& m); 
75                 
76                 template <typename Scalar2>
77                 void setValue(const Scalar2 *m)
78                 {
79                         m_el[0][0] = Scalar(m[0]); 
80                         m_el[1][0] = Scalar(m[1]); 
81                         m_el[2][0] = Scalar(m[2]);
82                         m_el[0][1] = Scalar(m[4]); 
83                         m_el[1][1] = Scalar(m[5]); 
84                         m_el[2][1] = Scalar(m[6]);
85                         m_el[0][2] = Scalar(m[8]); 
86                         m_el[1][2] = Scalar(m[9]); 
87                         m_el[2][2] = Scalar(m[10]);
88                 }
89
90                 template <typename Scalar2>
91                 void setValue(const Scalar2& xx, const Scalar2& xy, const Scalar2& xz, 
92                                           const Scalar2& yx, const Scalar2& yy, const Scalar2& yz, 
93                                           const Scalar2& zx, const Scalar2& zy, const Scalar2& zz)
94                 {
95                         m_el[0][0] = Scalar(xx); 
96                         m_el[0][1] = Scalar(xy); 
97                         m_el[0][2] = Scalar(xz);
98                         m_el[1][0] = Scalar(yx); 
99                         m_el[1][1] = Scalar(yy); 
100                         m_el[1][2] = Scalar(yz);
101                         m_el[2][0] = Scalar(zx); 
102                         m_el[2][1] = Scalar(zy); 
103                         m_el[2][2] = Scalar(zz);
104                 }
105   
106                 void setRotation(const Quaternion<Scalar>& q) 
107                 {
108                         Scalar d = q.length2();
109                         assert(d != Scalar(0.0));
110                         Scalar s = Scalar(2.0) / d;
111                         Scalar xs = q[0] * s,   ys = q[1] * s,   zs = q[2] * s;
112                         Scalar wx = q[3] * xs,  wy = q[3] * ys,  wz = q[3] * zs;
113                         Scalar xx = q[0] * xs,  xy = q[0] * ys,  xz = q[0] * zs;
114                         Scalar yy = q[1] * ys,  yz = q[1] * zs,  zz = q[2] * zs;
115                         setValue(Scalar(1.0) - (yy + zz), xy - wz, xz + wy,
116                                          xy + wz, Scalar(1.0) - (xx + zz), yz - wx,
117                                          xz - wy, yz + wx, Scalar(1.0) - (xx + yy));
118                 }
119                 
120                 template <typename Scalar2> 
121                 void setEuler(const Scalar2& yaw, const Scalar2& pitch, const Scalar2& roll) 
122                 {
123                         Scalar cy(Scalar_traits<Scalar>::cos(yaw)); 
124                         Scalar sy(Scalar_traits<Scalar>::sin(yaw)); 
125                         Scalar cp(Scalar_traits<Scalar>::cos(pitch)); 
126                         Scalar sp(Scalar_traits<Scalar>::sin(pitch)); 
127                         Scalar cr(Scalar_traits<Scalar>::cos(roll));
128                         Scalar sr(Scalar_traits<Scalar>::sin(roll));
129                         Scalar cc = cy * cr; 
130                         Scalar cs = cy * sr; 
131                         Scalar sc = sy * cr; 
132                         Scalar ss = sy * sr;
133                         setValue(cy * cp, -sc + sp * cs,  ss - sp * cc,
134                                          sy * cp,  cc + sp * ss, -cs + sp * sc,
135                                              -sp,       cp * sr,       cp * cr);
136                 }
137                 void setIdentity()
138                 { 
139                         setValue(Scalar(1.0), Scalar(0.0), Scalar(0.0), 
140                                          Scalar(0.0), Scalar(1.0), Scalar(0.0), 
141                                          Scalar(0.0), Scalar(0.0), Scalar(1.0)); 
142                 }
143     
144                 template <typename Scalar2>
145                 void getValue(Scalar2 *m) const 
146                 {
147                         m[0]  = Scalar2(m_el[0][0]); 
148                         m[1]  = Scalar2(m_el[1][0]);
149                         m[2]  = Scalar2(m_el[2][0]);
150                         m[3]  = Scalar2(0.0); 
151                         m[4]  = Scalar2(m_el[0][1]);
152                         m[5]  = Scalar2(m_el[1][1]);
153                         m[6]  = Scalar2(m_el[2][1]);
154                         m[7]  = Scalar2(0.0); 
155                         m[8]  = Scalar2(m_el[0][2]); 
156                         m[9]  = Scalar2(m_el[1][2]);
157                         m[10] = Scalar2(m_el[2][2]);
158                         m[11] = Scalar2(0.0); 
159                 }
160                 
161                 void getRotation(Quaternion<Scalar>& q) const
162                 {
163                         Scalar trace = m_el[0][0] + m_el[1][1] + m_el[2][2];
164                         
165                         if (trace > Scalar(0.0)) 
166                         {
167                                 Scalar s = Scalar_traits<Scalar>::sqrt(trace + Scalar(1.0));
168                                 q[3] = s * Scalar(0.5);
169                                 s = Scalar(0.5) / s;
170                                 
171                                 q[0] = (m_el[2][1] - m_el[1][2]) * s;
172                                 q[1] = (m_el[0][2] - m_el[2][0]) * s;
173                                 q[2] = (m_el[1][0] - m_el[0][1]) * s;
174                         } 
175                         else 
176                         {
177                                 int i = m_el[0][0] < m_el[1][1] ? 
178                                         (m_el[1][1] < m_el[2][2] ? 2 : 1) :
179                                         (m_el[0][0] < m_el[2][2] ? 2 : 0); 
180                                 int j = (i + 1) % 3;  
181                                 int k = (i + 2) % 3;
182                                 
183                                 Scalar s = Scalar_traits<Scalar>::sqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + Scalar(1.0));
184                                 q[i] = s * Scalar(0.5);
185                                 s = Scalar(0.5) / s;
186                                 
187                                 q[3] = (m_el[k][j] - m_el[j][k]) * s;
188                                 q[j] = (m_el[j][i] + m_el[i][j]) * s;
189                                 q[k] = (m_el[k][i] + m_el[i][k]) * s;
190                         }
191                 }
192
193
194                 
195                 template <typename Scalar2>
196                 void getEuler(Scalar2& yaw, Scalar2& pitch, Scalar2& roll) const
197                 {
198                         pitch = Scalar2(Scalar_traits<Scalar>::asin(-m_el[2][0]));
199                         if (pitch < Scalar_traits<Scalar2>::TwoTimesPi())
200                         {
201                                 if (pitch > Scalar_traits<Scalar2>::TwoTimesPi())
202                                 {
203                                         yaw = Scalar2(Scalar_traits<Scalar>::atan2(m_el[1][0], m_el[0][0]));
204                                         roll = Scalar2(Scalar_traits<Scalar>::atan2(m_el[2][1], m_el[2][2]));
205                                 }
206                                 else 
207                                 {
208                                         yaw = Scalar2(-Scalar_traits<Scalar>::atan2(-m_el[0][1], m_el[0][2]));
209                                         roll = Scalar2(0.0);
210                                 }
211                         }
212                         else
213                         {
214                                 yaw = Scalar2(Scalar_traits<Scalar>::atan2(-m_el[0][1], m_el[0][2]));
215                                 roll = Scalar2(0.0);
216                         }
217                 }
218
219                 Vector3<Scalar> getScaling() const
220                 {
221                         return Vector3<Scalar>(m_el[0][0] * m_el[0][0] + m_el[1][0] * m_el[1][0] + m_el[2][0] * m_el[2][0],
222                                                                    m_el[0][1] * m_el[0][1] + m_el[1][1] * m_el[1][1] + m_el[2][1] * m_el[2][1],
223                                                                    m_el[0][2] * m_el[0][2] + m_el[1][2] * m_el[1][2] + m_el[2][2] * m_el[2][2]);
224                 }
225                 
226                 
227                 Matrix3x3<Scalar> scaled(const Vector3<Scalar>& s) const
228                 {
229                         return Matrix3x3<Scalar>(m_el[0][0] * s[0], m_el[0][1] * s[1], m_el[0][2] * s[2],
230                                                                          m_el[1][0] * s[0], m_el[1][1] * s[1], m_el[1][2] * s[2],
231                                                                          m_el[2][0] * s[0], m_el[2][1] * s[1], m_el[2][2] * s[2]);
232                 }
233
234                 Scalar            determinant() const;
235                 Matrix3x3<Scalar> adjoint() const;
236                 Matrix3x3<Scalar> absolute() const;
237                 Matrix3x3<Scalar> transpose() const;
238                 Matrix3x3<Scalar> inverse() const; 
239                 
240                 Matrix3x3<Scalar> transposeTimes(const Matrix3x3<Scalar>& m) const;
241                 Matrix3x3<Scalar> timesTranspose(const Matrix3x3<Scalar>& m) const;
242                 
243                 Scalar tdot(int c, const Vector3<Scalar>& v) const 
244                 {
245                         return m_el[0][c] * v[0] + m_el[1][c] * v[1] + m_el[2][c] * v[2];
246                 }
247                 
248         protected:
249                 Scalar cofac(int r1, int c1, int r2, int c2) const 
250                 {
251                         return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
252                 }
253
254                 Vector3<Scalar> m_el[3];
255         };
256         
257         template <typename Scalar>
258         inline std::ostream& 
259         operator<<(std::ostream& os, const Matrix3x3<Scalar>& m)
260         {
261                 return os << m[0] << std::endl << m[1] << std::endl << m[2] << std::endl;
262         }
263         
264         template <typename Scalar>
265         inline Matrix3x3<Scalar>& 
266         Matrix3x3<Scalar>::operator*=(const Matrix3x3<Scalar>& m)
267         {
268                 setValue(m.tdot(0, m_el[0]), m.tdot(1, m_el[0]), m.tdot(2, m_el[0]),
269                                  m.tdot(0, m_el[1]), m.tdot(1, m_el[1]), m.tdot(2, m_el[1]),
270                                  m.tdot(0, m_el[2]), m.tdot(1, m_el[2]), m.tdot(2, m_el[2]));
271                 return *this;
272         }
273         
274         template <typename Scalar>
275         inline Scalar 
276         Matrix3x3<Scalar>::determinant() const
277         { 
278                 return triple((*this)[0], (*this)[1], (*this)[2]);
279         }
280         
281
282         template <typename Scalar>
283         inline Matrix3x3<Scalar> 
284         Matrix3x3<Scalar>::absolute() const
285         {
286                 return Matrix3x3<Scalar>(
287                         Scalar_traits<Scalar>::abs(m_el[0][0]), Scalar_traits<Scalar>::abs(m_el[0][1]), Scalar_traits<Scalar>::abs(m_el[0][2]),
288                         Scalar_traits<Scalar>::abs(m_el[1][0]), Scalar_traits<Scalar>::abs(m_el[1][1]), Scalar_traits<Scalar>::abs(m_el[1][2]),
289                         Scalar_traits<Scalar>::abs(m_el[2][0]), Scalar_traits<Scalar>::abs(m_el[2][1]), Scalar_traits<Scalar>::abs(m_el[2][2]));
290         }
291
292         template <typename Scalar>
293         inline Matrix3x3<Scalar> 
294         Matrix3x3<Scalar>::transpose() const 
295         {
296                 return Matrix3x3<Scalar>(m_el[0][0], m_el[1][0], m_el[2][0],
297                                                                  m_el[0][1], m_el[1][1], m_el[2][1],
298                                                                  m_el[0][2], m_el[1][2], m_el[2][2]);
299         }
300         
301         template <typename Scalar>
302         inline Matrix3x3<Scalar> 
303         Matrix3x3<Scalar>::adjoint() const 
304         {
305                 return Matrix3x3<Scalar>(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
306                                                                  cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
307                                                                  cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
308         }
309         
310         template <typename Scalar>
311         inline Matrix3x3<Scalar> 
312         Matrix3x3<Scalar>::inverse() const
313         {
314                 Vector3<Scalar> co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
315                 Scalar det = (*this)[0].dot(co);
316                 assert(det != Scalar(0.0));
317                 Scalar s = Scalar(1.0) / det;
318                 return Matrix3x3<Scalar>(co[0] * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
319                                                                  co[1] * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
320                                                                  co[2] * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
321         }
322         
323         template <typename Scalar>
324         inline Matrix3x3<Scalar> 
325         Matrix3x3<Scalar>::transposeTimes(const Matrix3x3<Scalar>& m) const
326         {
327                 return Matrix3x3<Scalar>(
328                         m_el[0][0] * m[0][0] + m_el[1][0] * m[1][0] + m_el[2][0] * m[2][0],
329                         m_el[0][0] * m[0][1] + m_el[1][0] * m[1][1] + m_el[2][0] * m[2][1],
330                         m_el[0][0] * m[0][2] + m_el[1][0] * m[1][2] + m_el[2][0] * m[2][2],
331                         m_el[0][1] * m[0][0] + m_el[1][1] * m[1][0] + m_el[2][1] * m[2][0],
332                         m_el[0][1] * m[0][1] + m_el[1][1] * m[1][1] + m_el[2][1] * m[2][1],
333                         m_el[0][1] * m[0][2] + m_el[1][1] * m[1][2] + m_el[2][1] * m[2][2],
334                         m_el[0][2] * m[0][0] + m_el[1][2] * m[1][0] + m_el[2][2] * m[2][0],
335                         m_el[0][2] * m[0][1] + m_el[1][2] * m[1][1] + m_el[2][2] * m[2][1],
336                         m_el[0][2] * m[0][2] + m_el[1][2] * m[1][2] + m_el[2][2] * m[2][2]);
337         }
338         
339         template <typename Scalar>
340         inline Matrix3x3<Scalar> 
341         Matrix3x3<Scalar>::timesTranspose(const Matrix3x3<Scalar>& m) const
342         {
343                 return Matrix3x3<Scalar>(
344                         m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
345                         m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
346                         m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
347                 
348         }
349
350         template <typename Scalar>
351         inline Vector3<Scalar> 
352         operator*(const Matrix3x3<Scalar>& m, const Vector3<Scalar>& v) 
353         {
354                 return Vector3<Scalar>(m[0].dot(v), m[1].dot(v), m[2].dot(v));
355         }
356         
357
358         template <typename Scalar>
359         inline Vector3<Scalar>
360         operator*(const Vector3<Scalar>& v, const Matrix3x3<Scalar>& m)
361         {
362                 return Vector3<Scalar>(m.tdot(0, v), m.tdot(1, v), m.tdot(2, v));
363         }
364
365         template <typename Scalar>
366         inline Matrix3x3<Scalar> 
367         operator*(const Matrix3x3<Scalar>& m1, const Matrix3x3<Scalar>& m2)
368         {
369                 return Matrix3x3<Scalar>(
370                         m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]),
371                         m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]),
372                         m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));
373         }
374 }
375
376 #endif