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