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