6d709fae662d6234de51359aac23f02d092d9892
[blender.git] / intern / moto / include / MT_Matrix3x3.h
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
19  * All rights reserved.
20  *
21  * The Original Code is: all of this file.
22  *
23  * Contributor(s): none yet.
24  *
25  * ***** END GPL LICENSE BLOCK *****
26  */
27
28 /** \file moto/include/MT_Matrix3x3.h
29  *  \ingroup moto
30  */
31
32
33 /*
34
35  * Copyright (c) 2000 Gino van den Bergen <gino@acm.org>
36  *
37  * Permission to use, copy, modify, distribute and sell this software
38  * and its documentation for any purpose is hereby granted without fee,
39  * provided that the above copyright notice appear in all copies and
40  * that both that copyright notice and this permission notice appear
41  * in supporting documentation.  Gino van den Bergen makes no
42  * representations about the suitability of this software for any
43  * purpose.  It is provided "as is" without express or implied warranty.
44  *
45  */
46
47 #ifndef MT_MATRIX3X3_H
48 #define MT_MATRIX3X3_H
49
50 #include <MT_assert.h>
51
52 #include "MT_Vector3.h"
53 #include "MT_Quaternion.h"
54
55 class MT_Matrix3x3 {
56 public:
57     MT_Matrix3x3() {}
58     MT_Matrix3x3(const float *m) { setValue(m); }
59     MT_Matrix3x3(const double *m) { setValue(m); }
60     MT_Matrix3x3(const MT_Quaternion& q) { setRotation(q); }
61     
62         MT_Matrix3x3(const MT_Quaternion& q, const MT_Vector3& s) { 
63                 setRotation(q); 
64                 scale(s[0], s[1], s[2]);
65         }
66         
67         MT_Matrix3x3(const MT_Vector3& euler) { setEuler(euler); }
68         MT_Matrix3x3(const MT_Vector3& euler, const MT_Vector3& s) { 
69                 setEuler(euler); 
70                 scale(s[0], s[1], s[2]);
71         }
72         
73     MT_Matrix3x3(MT_Scalar xx, MT_Scalar xy, MT_Scalar xz,
74                  MT_Scalar yx, MT_Scalar yy, MT_Scalar yz,
75                  MT_Scalar zx, MT_Scalar zy, MT_Scalar zz) { 
76         setValue(xx, xy, xz, 
77                  yx, yy, yz, 
78                  zx, zy, zz);
79     }
80     
81     MT_Vector3&       operator[](int i)       { return m_el[i]; }
82     const MT_Vector3& operator[](int i) const { return m_el[i]; }
83
84         MT_Vector3 getColumn(int i) const {
85                 return MT_Vector3(m_el[0][i], m_el[1][i], m_el[2][i]);
86         }
87         void setColumn(int i, const MT_Vector3& v) {
88                 m_el[0][i] = v[0];
89                 m_el[1][i] = v[1];
90                 m_el[2][i] = v[2];
91         }
92     
93     void setValue(const float *m) {
94         m_el[0][0] = *m++; m_el[1][0] = *m++; m_el[2][0] = *m++; m++;
95         m_el[0][1] = *m++; m_el[1][1] = *m++; m_el[2][1] = *m++; m++;
96         m_el[0][2] = *m++; m_el[1][2] = *m++; m_el[2][2] = *m;
97     }
98
99     void setValue(const double *m) {
100         m_el[0][0] = *m++; m_el[1][0] = *m++; m_el[2][0] = *m++; m++;
101         m_el[0][1] = *m++; m_el[1][1] = *m++; m_el[2][1] = *m++; m++;
102         m_el[0][2] = *m++; m_el[1][2] = *m++; m_el[2][2] = *m;
103     }
104
105     void setValue3x3(const float *m) {
106         m_el[0][0] = *m++; m_el[1][0] = *m++; m_el[2][0] = *m++;
107         m_el[0][1] = *m++; m_el[1][1] = *m++; m_el[2][1] = *m++;
108         m_el[0][2] = *m++; m_el[1][2] = *m++; m_el[2][2] = *m;
109     }
110
111     void setValue3x3(const double *m) {
112         m_el[0][0] = *m++; m_el[1][0] = *m++; m_el[2][0] = *m++;
113         m_el[0][1] = *m++; m_el[1][1] = *m++; m_el[2][1] = *m++;
114         m_el[0][2] = *m++; m_el[1][2] = *m++; m_el[2][2] = *m;
115     }
116
117     void setValue(MT_Scalar xx, MT_Scalar xy, MT_Scalar xz, 
118                   MT_Scalar yx, MT_Scalar yy, MT_Scalar yz, 
119                   MT_Scalar zx, MT_Scalar zy, MT_Scalar zz) {
120         m_el[0][0] = xx; m_el[0][1] = xy; m_el[0][2] = xz;
121         m_el[1][0] = yx; m_el[1][1] = yy; m_el[1][2] = yz;
122         m_el[2][0] = zx; m_el[2][1] = zy; m_el[2][2] = zz;
123     }
124   
125     void setRotation(const MT_Quaternion& q) {
126         MT_Scalar d = q.length2();
127         MT_assert(!MT_fuzzyZero2(d));
128         MT_Scalar s = MT_Scalar(2.0) / d;
129         MT_Scalar xs = q[0] * s,   ys = q[1] * s,   zs = q[2] * s;
130         MT_Scalar wx = q[3] * xs,  wy = q[3] * ys,  wz = q[3] * zs;
131         MT_Scalar xx = q[0] * xs,  xy = q[0] * ys,  xz = q[0] * zs;
132         MT_Scalar yy = q[1] * ys,  yz = q[1] * zs,  zz = q[2] * zs;
133         setValue(MT_Scalar(1.0) - (yy + zz), xy - wz        ,         xz + wy,
134                  xy + wz        , MT_Scalar(1.0) - (xx + zz),         yz - wx,
135                  xz - wy        , yz + wx,         MT_Scalar(1.0) - (xx + yy));
136     }
137     
138         /**
139          * setEuler
140          * @param euler a const reference to a MT_Vector3 of euler angles
141          * These angles are used to produce a rotation matrix. The euler
142          * angles are applied in ZYX order. I.e a vector is first rotated 
143          * about X then Y and then Z
144          **/
145
146         void setEuler(const MT_Vector3& euler) {
147                 MT_Scalar ci = cos(euler[0]); 
148                 MT_Scalar cj = cos(euler[1]); 
149                 MT_Scalar ch = cos(euler[2]);
150                 MT_Scalar si = sin(euler[0]); 
151                 MT_Scalar sj = sin(euler[1]); 
152                 MT_Scalar sh = sin(euler[2]);
153                 MT_Scalar cc = ci * ch; 
154                 MT_Scalar cs = ci * sh; 
155                 MT_Scalar sc = si * ch; 
156                 MT_Scalar ss = si * sh;
157                 
158                 setValue(cj * ch, sj * sc - cs, sj * cc + ss,
159                                  cj * sh, sj * ss + cc, sj * cs - sc, 
160                                  -sj,      cj * si,      cj * ci);
161         }
162
163         void getEuler(MT_Scalar& yaw, MT_Scalar& pitch, MT_Scalar& roll) const
164                 {                       
165                         if (m_el[2][0] != -1.0 && m_el[2][0] != 1.0) {
166                                 pitch = MT_Scalar(-asin(m_el[2][0]));
167                                 yaw = MT_Scalar(atan2(m_el[2][1] / cos(pitch), m_el[2][2] / cos(pitch)));
168                                 roll = MT_Scalar(atan2(m_el[1][0] / cos(pitch), m_el[0][0] / cos(pitch)));                              
169                         }
170                         else {
171                                 roll = MT_Scalar(0);
172                                 if (m_el[2][0] == -1.0) {
173                                         pitch = MT_PI / 2.0;
174                                         yaw = MT_Scalar(atan2(m_el[0][1], m_el[0][2]));
175                                 }
176                                 else {
177                                         pitch = - MT_PI / 2.0;
178                                         yaw = MT_Scalar(atan2(m_el[0][1], m_el[0][2]));
179                                 }
180                         }
181                 }
182
183     void scale(MT_Scalar x, MT_Scalar y, MT_Scalar z) {
184         m_el[0][0] *= x; m_el[0][1] *= y; m_el[0][2] *= z;
185         m_el[1][0] *= x; m_el[1][1] *= y; m_el[1][2] *= z;
186         m_el[2][0] *= x; m_el[2][1] *= y; m_el[2][2] *= z;
187     }
188
189     MT_Matrix3x3 scaled(MT_Scalar x, MT_Scalar y, MT_Scalar z) const {
190         return MT_Matrix3x3(m_el[0][0] * x, m_el[0][1] * y, m_el[0][2] * z,
191                             m_el[1][0] * x, m_el[1][1] * y, m_el[1][2] * z,
192                             m_el[2][0] * x, m_el[2][1] * y, m_el[2][2] * z);
193     }
194     
195     void setIdentity() { 
196         setValue(MT_Scalar(1.0), MT_Scalar(0.0), MT_Scalar(0.0), 
197                  MT_Scalar(0.0), MT_Scalar(1.0), MT_Scalar(0.0), 
198                  MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(1.0)); 
199     }
200     
201     void getValue(float *m) const {
202         *m++ = (float) m_el[0][0]; *m++ = (float) m_el[1][0]; *m++ = (float) m_el[2][0]; *m++ = (float) 0.0;
203         *m++ = (float) m_el[0][1]; *m++ = (float) m_el[1][1]; *m++ = (float) m_el[2][1]; *m++ = (float) 0.0;
204         *m++ = (float) m_el[0][2]; *m++ = (float) m_el[1][2]; *m++ = (float) m_el[2][2]; *m   = (float) 0.0;
205     }
206
207     void getValue(double *m) const {
208         *m++ = m_el[0][0]; *m++ = m_el[1][0]; *m++ = m_el[2][0]; *m++ = 0.0;
209         *m++ = m_el[0][1]; *m++ = m_el[1][1]; *m++ = m_el[2][1]; *m++ = 0.0;
210         *m++ = m_el[0][2]; *m++ = m_el[1][2]; *m++ = m_el[2][2]; *m   = 0.0;
211     }
212
213     void getValue3x3(float *m) const {
214         *m++ = (float) m_el[0][0]; *m++ = (float) m_el[1][0]; *m++ = (float) m_el[2][0];
215         *m++ = (float) m_el[0][1]; *m++ = (float) m_el[1][1]; *m++ = (float) m_el[2][1];
216         *m++ = (float) m_el[0][2]; *m++ = (float) m_el[1][2]; *m++ = (float) m_el[2][2];
217     }
218
219     void getValue3x3(double *m) const {
220         *m++ = m_el[0][0]; *m++ = m_el[1][0]; *m++ = m_el[2][0];
221         *m++ = m_el[0][1]; *m++ = m_el[1][1]; *m++ = m_el[2][1];
222         *m++ = m_el[0][2]; *m++ = m_el[1][2]; *m++ = m_el[2][2];
223     }
224
225     MT_Quaternion getRotation() const;
226
227     MT_Matrix3x3& operator*=(const MT_Matrix3x3& m); 
228
229     MT_Scalar tdot(int c, const MT_Vector3& v) const {
230         return m_el[0][c] * v[0] + m_el[1][c] * v[1] + m_el[2][c] * v[2];
231     }
232   
233     MT_Scalar cofac(int r1, int c1, int r2, int c2) const {
234         return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
235     }
236
237     MT_Scalar    determinant() const;
238         MT_Matrix3x3 adjoint() const;
239
240     MT_Matrix3x3 absolute() const;
241
242     MT_Matrix3x3 transposed() const;
243     void         transpose();
244
245     MT_Matrix3x3 inverse() const; 
246         void         invert();
247   
248 protected:
249
250     MT_Vector3 m_el[3];
251 };
252
253 MT_Vector3   operator*(const MT_Matrix3x3& m, const MT_Vector3& v);
254 MT_Vector3   operator*(const MT_Vector3& v, const MT_Matrix3x3& m);
255 MT_Matrix3x3 operator*(const MT_Matrix3x3& m1, const MT_Matrix3x3& m2);
256
257 MT_Matrix3x3 MT_multTransposeLeft(const MT_Matrix3x3& m1, const MT_Matrix3x3& m2);
258 MT_Matrix3x3 MT_multTransposeRight(const MT_Matrix3x3& m1, const MT_Matrix3x3& m2);
259
260 inline MT_OStream& operator<<(MT_OStream& os, const MT_Matrix3x3& m) {
261     return os << m[0] << GEN_endl << m[1] << GEN_endl << m[2] << GEN_endl;
262 }
263
264 #ifdef GEN_INLINED
265 #include "MT_Matrix3x3.inl"
266 #endif
267
268 #endif
269