1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
2 | /** |
---|
3 | * Contains code for 3x3 matrices. |
---|
4 | * \file IceMatrix3x3.h |
---|
5 | * \author Pierre Terdiman |
---|
6 | * \date April, 4, 2000 |
---|
7 | */ |
---|
8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
9 | |
---|
10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
11 | // Include Guard |
---|
12 | #ifndef __ICEMATRIX3X3_H__ |
---|
13 | #define __ICEMATRIX3X3_H__ |
---|
14 | |
---|
15 | // Forward declarations |
---|
16 | class Quat; |
---|
17 | |
---|
18 | #define MATRIX3X3_EPSILON (1.0e-7f) |
---|
19 | |
---|
20 | class ICEMATHS_API Matrix3x3 |
---|
21 | { |
---|
22 | public: |
---|
23 | //! Empty constructor |
---|
24 | inline_ Matrix3x3() {} |
---|
25 | //! Constructor from 9 values |
---|
26 | inline_ Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22) |
---|
27 | { |
---|
28 | m[0][0] = m00; m[0][1] = m01; m[0][2] = m02; |
---|
29 | m[1][0] = m10; m[1][1] = m11; m[1][2] = m12; |
---|
30 | m[2][0] = m20; m[2][1] = m21; m[2][2] = m22; |
---|
31 | } |
---|
32 | //! Copy constructor |
---|
33 | inline_ Matrix3x3(const Matrix3x3& mat) { CopyMemory(m, &mat.m, 9*sizeof(float)); } |
---|
34 | //! Destructor |
---|
35 | inline_ ~Matrix3x3() {} |
---|
36 | |
---|
37 | //! Assign values |
---|
38 | inline_ void Set(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22) |
---|
39 | { |
---|
40 | m[0][0] = m00; m[0][1] = m01; m[0][2] = m02; |
---|
41 | m[1][0] = m10; m[1][1] = m11; m[1][2] = m12; |
---|
42 | m[2][0] = m20; m[2][1] = m21; m[2][2] = m22; |
---|
43 | } |
---|
44 | |
---|
45 | //! Sets the scale from a Point. The point is put on the diagonal. |
---|
46 | inline_ void SetScale(const Point& p) { m[0][0] = p.x; m[1][1] = p.y; m[2][2] = p.z; } |
---|
47 | |
---|
48 | //! Sets the scale from floats. Values are put on the diagonal. |
---|
49 | inline_ void SetScale(float sx, float sy, float sz) { m[0][0] = sx; m[1][1] = sy; m[2][2] = sz; } |
---|
50 | |
---|
51 | //! Scales from a Point. Each row is multiplied by a component. |
---|
52 | inline_ void Scale(const Point& p) |
---|
53 | { |
---|
54 | m[0][0] *= p.x; m[0][1] *= p.x; m[0][2] *= p.x; |
---|
55 | m[1][0] *= p.y; m[1][1] *= p.y; m[1][2] *= p.y; |
---|
56 | m[2][0] *= p.z; m[2][1] *= p.z; m[2][2] *= p.z; |
---|
57 | } |
---|
58 | |
---|
59 | //! Scales from floats. Each row is multiplied by a value. |
---|
60 | inline_ void Scale(float sx, float sy, float sz) |
---|
61 | { |
---|
62 | m[0][0] *= sx; m[0][1] *= sx; m[0][2] *= sx; |
---|
63 | m[1][0] *= sy; m[1][1] *= sy; m[1][2] *= sy; |
---|
64 | m[2][0] *= sz; m[2][1] *= sz; m[2][2] *= sz; |
---|
65 | } |
---|
66 | |
---|
67 | //! Copy from a Matrix3x3 |
---|
68 | inline_ void Copy(const Matrix3x3& source) { CopyMemory(m, source.m, 9*sizeof(float)); } |
---|
69 | |
---|
70 | // Row-column access |
---|
71 | //! Returns a row. |
---|
72 | inline_ void GetRow(const udword r, Point& p) const { p.x = m[r][0]; p.y = m[r][1]; p.z = m[r][2]; } |
---|
73 | //! Returns a row. |
---|
74 | inline_ const Point& GetRow(const udword r) const { return *(const Point*)&m[r][0]; } |
---|
75 | //! Returns a row. |
---|
76 | inline_ Point& GetRow(const udword r) { return *(Point*)&m[r][0]; } |
---|
77 | //! Sets a row. |
---|
78 | inline_ void SetRow(const udword r, const Point& p) { m[r][0] = p.x; m[r][1] = p.y; m[r][2] = p.z; } |
---|
79 | //! Returns a column. |
---|
80 | inline_ void GetCol(const udword c, Point& p) const { p.x = m[0][c]; p.y = m[1][c]; p.z = m[2][c]; } |
---|
81 | //! Sets a column. |
---|
82 | inline_ void SetCol(const udword c, const Point& p) { m[0][c] = p.x; m[1][c] = p.y; m[2][c] = p.z; } |
---|
83 | |
---|
84 | //! Computes the trace. The trace is the sum of the 3 diagonal components. |
---|
85 | inline_ float Trace() const { return m[0][0] + m[1][1] + m[2][2]; } |
---|
86 | //! Clears the matrix. |
---|
87 | inline_ void Zero() { ZeroMemory(&m, sizeof(m)); } |
---|
88 | //! Sets the identity matrix. |
---|
89 | inline_ void Identity() { Zero(); m[0][0] = m[1][1] = m[2][2] = 1.0f; } |
---|
90 | //! Checks for identity |
---|
91 | inline_ bool IsIdentity() const |
---|
92 | { |
---|
93 | if(IR(m[0][0])!=IEEE_1_0) return false; |
---|
94 | if(IR(m[0][1])!=0) return false; |
---|
95 | if(IR(m[0][2])!=0) return false; |
---|
96 | |
---|
97 | if(IR(m[1][0])!=0) return false; |
---|
98 | if(IR(m[1][1])!=IEEE_1_0) return false; |
---|
99 | if(IR(m[1][2])!=0) return false; |
---|
100 | |
---|
101 | if(IR(m[2][0])!=0) return false; |
---|
102 | if(IR(m[2][1])!=0) return false; |
---|
103 | if(IR(m[2][2])!=IEEE_1_0) return false; |
---|
104 | |
---|
105 | return true; |
---|
106 | } |
---|
107 | |
---|
108 | //! Checks matrix validity |
---|
109 | inline_ BOOL IsValid() const |
---|
110 | { |
---|
111 | for(udword j=0;j<3;j++) |
---|
112 | { |
---|
113 | for(udword i=0;i<3;i++) |
---|
114 | { |
---|
115 | if(!IsValidFloat(m[j][i])) return FALSE; |
---|
116 | } |
---|
117 | } |
---|
118 | return TRUE; |
---|
119 | } |
---|
120 | |
---|
121 | //! Makes a skew-symmetric matrix (a.k.a. Star(*) Matrix) |
---|
122 | //! [ 0.0 -a.z a.y ] |
---|
123 | //! [ a.z 0.0 -a.x ] |
---|
124 | //! [ -a.y a.x 0.0 ] |
---|
125 | //! This is also called a "cross matrix" since for any vectors A and B, |
---|
126 | //! A^B = Skew(A) * B = - B * Skew(A); |
---|
127 | inline_ void SkewSymmetric(const Point& a) |
---|
128 | { |
---|
129 | m[0][0] = 0.0f; |
---|
130 | m[0][1] = -a.z; |
---|
131 | m[0][2] = a.y; |
---|
132 | |
---|
133 | m[1][0] = a.z; |
---|
134 | m[1][1] = 0.0f; |
---|
135 | m[1][2] = -a.x; |
---|
136 | |
---|
137 | m[2][0] = -a.y; |
---|
138 | m[2][1] = a.x; |
---|
139 | m[2][2] = 0.0f; |
---|
140 | } |
---|
141 | |
---|
142 | //! Negates the matrix |
---|
143 | inline_ void Neg() |
---|
144 | { |
---|
145 | m[0][0] = -m[0][0]; m[0][1] = -m[0][1]; m[0][2] = -m[0][2]; |
---|
146 | m[1][0] = -m[1][0]; m[1][1] = -m[1][1]; m[1][2] = -m[1][2]; |
---|
147 | m[2][0] = -m[2][0]; m[2][1] = -m[2][1]; m[2][2] = -m[2][2]; |
---|
148 | } |
---|
149 | |
---|
150 | //! Neg from another matrix |
---|
151 | inline_ void Neg(const Matrix3x3& mat) |
---|
152 | { |
---|
153 | m[0][0] = -mat.m[0][0]; m[0][1] = -mat.m[0][1]; m[0][2] = -mat.m[0][2]; |
---|
154 | m[1][0] = -mat.m[1][0]; m[1][1] = -mat.m[1][1]; m[1][2] = -mat.m[1][2]; |
---|
155 | m[2][0] = -mat.m[2][0]; m[2][1] = -mat.m[2][1]; m[2][2] = -mat.m[2][2]; |
---|
156 | } |
---|
157 | |
---|
158 | //! Add another matrix |
---|
159 | inline_ void Add(const Matrix3x3& mat) |
---|
160 | { |
---|
161 | m[0][0] += mat.m[0][0]; m[0][1] += mat.m[0][1]; m[0][2] += mat.m[0][2]; |
---|
162 | m[1][0] += mat.m[1][0]; m[1][1] += mat.m[1][1]; m[1][2] += mat.m[1][2]; |
---|
163 | m[2][0] += mat.m[2][0]; m[2][1] += mat.m[2][1]; m[2][2] += mat.m[2][2]; |
---|
164 | } |
---|
165 | |
---|
166 | //! Sub another matrix |
---|
167 | inline_ void Sub(const Matrix3x3& mat) |
---|
168 | { |
---|
169 | m[0][0] -= mat.m[0][0]; m[0][1] -= mat.m[0][1]; m[0][2] -= mat.m[0][2]; |
---|
170 | m[1][0] -= mat.m[1][0]; m[1][1] -= mat.m[1][1]; m[1][2] -= mat.m[1][2]; |
---|
171 | m[2][0] -= mat.m[2][0]; m[2][1] -= mat.m[2][1]; m[2][2] -= mat.m[2][2]; |
---|
172 | } |
---|
173 | //! Mac |
---|
174 | inline_ void Mac(const Matrix3x3& a, const Matrix3x3& b, float s) |
---|
175 | { |
---|
176 | m[0][0] = a.m[0][0] + b.m[0][0] * s; |
---|
177 | m[0][1] = a.m[0][1] + b.m[0][1] * s; |
---|
178 | m[0][2] = a.m[0][2] + b.m[0][2] * s; |
---|
179 | |
---|
180 | m[1][0] = a.m[1][0] + b.m[1][0] * s; |
---|
181 | m[1][1] = a.m[1][1] + b.m[1][1] * s; |
---|
182 | m[1][2] = a.m[1][2] + b.m[1][2] * s; |
---|
183 | |
---|
184 | m[2][0] = a.m[2][0] + b.m[2][0] * s; |
---|
185 | m[2][1] = a.m[2][1] + b.m[2][1] * s; |
---|
186 | m[2][2] = a.m[2][2] + b.m[2][2] * s; |
---|
187 | } |
---|
188 | //! Mac |
---|
189 | inline_ void Mac(const Matrix3x3& a, float s) |
---|
190 | { |
---|
191 | m[0][0] += a.m[0][0] * s; m[0][1] += a.m[0][1] * s; m[0][2] += a.m[0][2] * s; |
---|
192 | m[1][0] += a.m[1][0] * s; m[1][1] += a.m[1][1] * s; m[1][2] += a.m[1][2] * s; |
---|
193 | m[2][0] += a.m[2][0] * s; m[2][1] += a.m[2][1] * s; m[2][2] += a.m[2][2] * s; |
---|
194 | } |
---|
195 | |
---|
196 | //! this = A * s |
---|
197 | inline_ void Mult(const Matrix3x3& a, float s) |
---|
198 | { |
---|
199 | m[0][0] = a.m[0][0] * s; m[0][1] = a.m[0][1] * s; m[0][2] = a.m[0][2] * s; |
---|
200 | m[1][0] = a.m[1][0] * s; m[1][1] = a.m[1][1] * s; m[1][2] = a.m[1][2] * s; |
---|
201 | m[2][0] = a.m[2][0] * s; m[2][1] = a.m[2][1] * s; m[2][2] = a.m[2][2] * s; |
---|
202 | } |
---|
203 | |
---|
204 | inline_ void Add(const Matrix3x3& a, const Matrix3x3& b) |
---|
205 | { |
---|
206 | m[0][0] = a.m[0][0] + b.m[0][0]; m[0][1] = a.m[0][1] + b.m[0][1]; m[0][2] = a.m[0][2] + b.m[0][2]; |
---|
207 | m[1][0] = a.m[1][0] + b.m[1][0]; m[1][1] = a.m[1][1] + b.m[1][1]; m[1][2] = a.m[1][2] + b.m[1][2]; |
---|
208 | m[2][0] = a.m[2][0] + b.m[2][0]; m[2][1] = a.m[2][1] + b.m[2][1]; m[2][2] = a.m[2][2] + b.m[2][2]; |
---|
209 | } |
---|
210 | |
---|
211 | inline_ void Sub(const Matrix3x3& a, const Matrix3x3& b) |
---|
212 | { |
---|
213 | m[0][0] = a.m[0][0] - b.m[0][0]; m[0][1] = a.m[0][1] - b.m[0][1]; m[0][2] = a.m[0][2] - b.m[0][2]; |
---|
214 | m[1][0] = a.m[1][0] - b.m[1][0]; m[1][1] = a.m[1][1] - b.m[1][1]; m[1][2] = a.m[1][2] - b.m[1][2]; |
---|
215 | m[2][0] = a.m[2][0] - b.m[2][0]; m[2][1] = a.m[2][1] - b.m[2][1]; m[2][2] = a.m[2][2] - b.m[2][2]; |
---|
216 | } |
---|
217 | |
---|
218 | //! this = a * b |
---|
219 | inline_ void Mult(const Matrix3x3& a, const Matrix3x3& b) |
---|
220 | { |
---|
221 | m[0][0] = a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[1][0] + a.m[0][2] * b.m[2][0]; |
---|
222 | m[0][1] = a.m[0][0] * b.m[0][1] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[2][1]; |
---|
223 | m[0][2] = a.m[0][0] * b.m[0][2] + a.m[0][1] * b.m[1][2] + a.m[0][2] * b.m[2][2]; |
---|
224 | m[1][0] = a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[1][2] * b.m[2][0]; |
---|
225 | m[1][1] = a.m[1][0] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[2][1]; |
---|
226 | m[1][2] = a.m[1][0] * b.m[0][2] + a.m[1][1] * b.m[1][2] + a.m[1][2] * b.m[2][2]; |
---|
227 | m[2][0] = a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[1][0] + a.m[2][2] * b.m[2][0]; |
---|
228 | m[2][1] = a.m[2][0] * b.m[0][1] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[2][1]; |
---|
229 | m[2][2] = a.m[2][0] * b.m[0][2] + a.m[2][1] * b.m[1][2] + a.m[2][2] * b.m[2][2]; |
---|
230 | } |
---|
231 | |
---|
232 | //! this = transpose(a) * b |
---|
233 | inline_ void MultAtB(const Matrix3x3& a, const Matrix3x3& b) |
---|
234 | { |
---|
235 | m[0][0] = a.m[0][0] * b.m[0][0] + a.m[1][0] * b.m[1][0] + a.m[2][0] * b.m[2][0]; |
---|
236 | m[0][1] = a.m[0][0] * b.m[0][1] + a.m[1][0] * b.m[1][1] + a.m[2][0] * b.m[2][1]; |
---|
237 | m[0][2] = a.m[0][0] * b.m[0][2] + a.m[1][0] * b.m[1][2] + a.m[2][0] * b.m[2][2]; |
---|
238 | m[1][0] = a.m[0][1] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[2][1] * b.m[2][0]; |
---|
239 | m[1][1] = a.m[0][1] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[2][1] * b.m[2][1]; |
---|
240 | m[1][2] = a.m[0][1] * b.m[0][2] + a.m[1][1] * b.m[1][2] + a.m[2][1] * b.m[2][2]; |
---|
241 | m[2][0] = a.m[0][2] * b.m[0][0] + a.m[1][2] * b.m[1][0] + a.m[2][2] * b.m[2][0]; |
---|
242 | m[2][1] = a.m[0][2] * b.m[0][1] + a.m[1][2] * b.m[1][1] + a.m[2][2] * b.m[2][1]; |
---|
243 | m[2][2] = a.m[0][2] * b.m[0][2] + a.m[1][2] * b.m[1][2] + a.m[2][2] * b.m[2][2]; |
---|
244 | } |
---|
245 | |
---|
246 | //! this = a * transpose(b) |
---|
247 | inline_ void MultABt(const Matrix3x3& a, const Matrix3x3& b) |
---|
248 | { |
---|
249 | m[0][0] = a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[0][1] + a.m[0][2] * b.m[0][2]; |
---|
250 | m[0][1] = a.m[0][0] * b.m[1][0] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[1][2]; |
---|
251 | m[0][2] = a.m[0][0] * b.m[2][0] + a.m[0][1] * b.m[2][1] + a.m[0][2] * b.m[2][2]; |
---|
252 | m[1][0] = a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[0][1] + a.m[1][2] * b.m[0][2]; |
---|
253 | m[1][1] = a.m[1][0] * b.m[1][0] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[1][2]; |
---|
254 | m[1][2] = a.m[1][0] * b.m[2][0] + a.m[1][1] * b.m[2][1] + a.m[1][2] * b.m[2][2]; |
---|
255 | m[2][0] = a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[0][1] + a.m[2][2] * b.m[0][2]; |
---|
256 | m[2][1] = a.m[2][0] * b.m[1][0] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[1][2]; |
---|
257 | m[2][2] = a.m[2][0] * b.m[2][0] + a.m[2][1] * b.m[2][1] + a.m[2][2] * b.m[2][2]; |
---|
258 | } |
---|
259 | |
---|
260 | //! Makes a rotation matrix mapping vector "from" to vector "to". |
---|
261 | Matrix3x3& FromTo(const Point& from, const Point& to); |
---|
262 | |
---|
263 | //! Set a rotation matrix around the X axis. |
---|
264 | //! 1 0 0 |
---|
265 | //! RX = 0 cx sx |
---|
266 | //! 0 -sx cx |
---|
267 | void RotX(float angle); |
---|
268 | //! Set a rotation matrix around the Y axis. |
---|
269 | //! cy 0 -sy |
---|
270 | //! RY = 0 1 0 |
---|
271 | //! sy 0 cy |
---|
272 | void RotY(float angle); |
---|
273 | //! Set a rotation matrix around the Z axis. |
---|
274 | //! cz sz 0 |
---|
275 | //! RZ = -sz cz 0 |
---|
276 | //! 0 0 1 |
---|
277 | void RotZ(float angle); |
---|
278 | //! cy sx.sy -sy.cx |
---|
279 | //! RY.RX 0 cx sx |
---|
280 | //! sy -sx.cy cx.cy |
---|
281 | void RotYX(float y, float x); |
---|
282 | |
---|
283 | //! Make a rotation matrix about an arbitrary axis |
---|
284 | Matrix3x3& Rot(float angle, const Point& axis); |
---|
285 | |
---|
286 | //! Transpose the matrix. |
---|
287 | void Transpose() |
---|
288 | { |
---|
289 | IR(m[1][0]) ^= IR(m[0][1]); IR(m[0][1]) ^= IR(m[1][0]); IR(m[1][0]) ^= IR(m[0][1]); |
---|
290 | IR(m[2][0]) ^= IR(m[0][2]); IR(m[0][2]) ^= IR(m[2][0]); IR(m[2][0]) ^= IR(m[0][2]); |
---|
291 | IR(m[2][1]) ^= IR(m[1][2]); IR(m[1][2]) ^= IR(m[2][1]); IR(m[2][1]) ^= IR(m[1][2]); |
---|
292 | } |
---|
293 | |
---|
294 | //! this = Transpose(a) |
---|
295 | void Transpose(const Matrix3x3& a) |
---|
296 | { |
---|
297 | m[0][0] = a.m[0][0]; m[0][1] = a.m[1][0]; m[0][2] = a.m[2][0]; |
---|
298 | m[1][0] = a.m[0][1]; m[1][1] = a.m[1][1]; m[1][2] = a.m[2][1]; |
---|
299 | m[2][0] = a.m[0][2]; m[2][1] = a.m[1][2]; m[2][2] = a.m[2][2]; |
---|
300 | } |
---|
301 | |
---|
302 | //! Compute the determinant of the matrix. We use the rule of Sarrus. |
---|
303 | float Determinant() const |
---|
304 | { |
---|
305 | return (m[0][0]*m[1][1]*m[2][2] + m[0][1]*m[1][2]*m[2][0] + m[0][2]*m[1][0]*m[2][1]) |
---|
306 | - (m[2][0]*m[1][1]*m[0][2] + m[2][1]*m[1][2]*m[0][0] + m[2][2]*m[1][0]*m[0][1]); |
---|
307 | } |
---|
308 | /* |
---|
309 | //! Compute a cofactor. Used for matrix inversion. |
---|
310 | float CoFactor(ubyte row, ubyte column) const |
---|
311 | { |
---|
312 | static sdword gIndex[3+2] = { 0, 1, 2, 0, 1 }; |
---|
313 | return (m[gIndex[row+1]][gIndex[column+1]]*m[gIndex[row+2]][gIndex[column+2]] - m[gIndex[row+2]][gIndex[column+1]]*m[gIndex[row+1]][gIndex[column+2]]); |
---|
314 | } |
---|
315 | */ |
---|
316 | //! Invert the matrix. Determinant must be different from zero, else matrix can't be inverted. |
---|
317 | Matrix3x3& Invert() |
---|
318 | { |
---|
319 | float Det = Determinant(); // Must be !=0 |
---|
320 | float OneOverDet = 1.0f / Det; |
---|
321 | |
---|
322 | Matrix3x3 Temp; |
---|
323 | Temp.m[0][0] = +(m[1][1] * m[2][2] - m[2][1] * m[1][2]) * OneOverDet; |
---|
324 | Temp.m[1][0] = -(m[1][0] * m[2][2] - m[2][0] * m[1][2]) * OneOverDet; |
---|
325 | Temp.m[2][0] = +(m[1][0] * m[2][1] - m[2][0] * m[1][1]) * OneOverDet; |
---|
326 | Temp.m[0][1] = -(m[0][1] * m[2][2] - m[2][1] * m[0][2]) * OneOverDet; |
---|
327 | Temp.m[1][1] = +(m[0][0] * m[2][2] - m[2][0] * m[0][2]) * OneOverDet; |
---|
328 | Temp.m[2][1] = -(m[0][0] * m[2][1] - m[2][0] * m[0][1]) * OneOverDet; |
---|
329 | Temp.m[0][2] = +(m[0][1] * m[1][2] - m[1][1] * m[0][2]) * OneOverDet; |
---|
330 | Temp.m[1][2] = -(m[0][0] * m[1][2] - m[1][0] * m[0][2]) * OneOverDet; |
---|
331 | Temp.m[2][2] = +(m[0][0] * m[1][1] - m[1][0] * m[0][1]) * OneOverDet; |
---|
332 | |
---|
333 | *this = Temp; |
---|
334 | |
---|
335 | return *this; |
---|
336 | } |
---|
337 | |
---|
338 | Matrix3x3& Normalize(); |
---|
339 | |
---|
340 | //! this = exp(a) |
---|
341 | Matrix3x3& Exp(const Matrix3x3& a); |
---|
342 | |
---|
343 | void FromQuat(const Quat &q); |
---|
344 | void FromQuatL2(const Quat &q, float l2); |
---|
345 | |
---|
346 | // Arithmetic operators |
---|
347 | //! Operator for Matrix3x3 Plus = Matrix3x3 + Matrix3x3; |
---|
348 | inline_ Matrix3x3 operator+(const Matrix3x3& mat) const |
---|
349 | { |
---|
350 | return Matrix3x3( |
---|
351 | m[0][0] + mat.m[0][0], m[0][1] + mat.m[0][1], m[0][2] + mat.m[0][2], |
---|
352 | m[1][0] + mat.m[1][0], m[1][1] + mat.m[1][1], m[1][2] + mat.m[1][2], |
---|
353 | m[2][0] + mat.m[2][0], m[2][1] + mat.m[2][1], m[2][2] + mat.m[2][2]); |
---|
354 | } |
---|
355 | |
---|
356 | //! Operator for Matrix3x3 Minus = Matrix3x3 - Matrix3x3; |
---|
357 | inline_ Matrix3x3 operator-(const Matrix3x3& mat) const |
---|
358 | { |
---|
359 | return Matrix3x3( |
---|
360 | m[0][0] - mat.m[0][0], m[0][1] - mat.m[0][1], m[0][2] - mat.m[0][2], |
---|
361 | m[1][0] - mat.m[1][0], m[1][1] - mat.m[1][1], m[1][2] - mat.m[1][2], |
---|
362 | m[2][0] - mat.m[2][0], m[2][1] - mat.m[2][1], m[2][2] - mat.m[2][2]); |
---|
363 | } |
---|
364 | |
---|
365 | //! Operator for Matrix3x3 Mul = Matrix3x3 * Matrix3x3; |
---|
366 | inline_ Matrix3x3 operator*(const Matrix3x3& mat) const |
---|
367 | { |
---|
368 | return Matrix3x3( |
---|
369 | m[0][0]*mat.m[0][0] + m[0][1]*mat.m[1][0] + m[0][2]*mat.m[2][0], |
---|
370 | m[0][0]*mat.m[0][1] + m[0][1]*mat.m[1][1] + m[0][2]*mat.m[2][1], |
---|
371 | m[0][0]*mat.m[0][2] + m[0][1]*mat.m[1][2] + m[0][2]*mat.m[2][2], |
---|
372 | |
---|
373 | m[1][0]*mat.m[0][0] + m[1][1]*mat.m[1][0] + m[1][2]*mat.m[2][0], |
---|
374 | m[1][0]*mat.m[0][1] + m[1][1]*mat.m[1][1] + m[1][2]*mat.m[2][1], |
---|
375 | m[1][0]*mat.m[0][2] + m[1][1]*mat.m[1][2] + m[1][2]*mat.m[2][2], |
---|
376 | |
---|
377 | m[2][0]*mat.m[0][0] + m[2][1]*mat.m[1][0] + m[2][2]*mat.m[2][0], |
---|
378 | m[2][0]*mat.m[0][1] + m[2][1]*mat.m[1][1] + m[2][2]*mat.m[2][1], |
---|
379 | m[2][0]*mat.m[0][2] + m[2][1]*mat.m[1][2] + m[2][2]*mat.m[2][2]); |
---|
380 | } |
---|
381 | |
---|
382 | //! Operator for Point Mul = Matrix3x3 * Point; |
---|
383 | inline_ Point operator*(const Point& v) const { return Point(GetRow(0)|v, GetRow(1)|v, GetRow(2)|v); } |
---|
384 | |
---|
385 | //! Operator for Matrix3x3 Mul = Matrix3x3 * float; |
---|
386 | inline_ Matrix3x3 operator*(float s) const |
---|
387 | { |
---|
388 | return Matrix3x3( |
---|
389 | m[0][0]*s, m[0][1]*s, m[0][2]*s, |
---|
390 | m[1][0]*s, m[1][1]*s, m[1][2]*s, |
---|
391 | m[2][0]*s, m[2][1]*s, m[2][2]*s); |
---|
392 | } |
---|
393 | |
---|
394 | //! Operator for Matrix3x3 Mul = float * Matrix3x3; |
---|
395 | inline_ friend Matrix3x3 operator*(float s, const Matrix3x3& mat) |
---|
396 | { |
---|
397 | return Matrix3x3( |
---|
398 | s*mat.m[0][0], s*mat.m[0][1], s*mat.m[0][2], |
---|
399 | s*mat.m[1][0], s*mat.m[1][1], s*mat.m[1][2], |
---|
400 | s*mat.m[2][0], s*mat.m[2][1], s*mat.m[2][2]); |
---|
401 | } |
---|
402 | |
---|
403 | //! Operator for Matrix3x3 Div = Matrix3x3 / float; |
---|
404 | inline_ Matrix3x3 operator/(float s) const |
---|
405 | { |
---|
406 | if (s) s = 1.0f / s; |
---|
407 | return Matrix3x3( |
---|
408 | m[0][0]*s, m[0][1]*s, m[0][2]*s, |
---|
409 | m[1][0]*s, m[1][1]*s, m[1][2]*s, |
---|
410 | m[2][0]*s, m[2][1]*s, m[2][2]*s); |
---|
411 | } |
---|
412 | |
---|
413 | //! Operator for Matrix3x3 Div = float / Matrix3x3; |
---|
414 | inline_ friend Matrix3x3 operator/(float s, const Matrix3x3& mat) |
---|
415 | { |
---|
416 | return Matrix3x3( |
---|
417 | s/mat.m[0][0], s/mat.m[0][1], s/mat.m[0][2], |
---|
418 | s/mat.m[1][0], s/mat.m[1][1], s/mat.m[1][2], |
---|
419 | s/mat.m[2][0], s/mat.m[2][1], s/mat.m[2][2]); |
---|
420 | } |
---|
421 | |
---|
422 | //! Operator for Matrix3x3 += Matrix3x3 |
---|
423 | inline_ Matrix3x3& operator+=(const Matrix3x3& mat) |
---|
424 | { |
---|
425 | m[0][0] += mat.m[0][0]; m[0][1] += mat.m[0][1]; m[0][2] += mat.m[0][2]; |
---|
426 | m[1][0] += mat.m[1][0]; m[1][1] += mat.m[1][1]; m[1][2] += mat.m[1][2]; |
---|
427 | m[2][0] += mat.m[2][0]; m[2][1] += mat.m[2][1]; m[2][2] += mat.m[2][2]; |
---|
428 | return *this; |
---|
429 | } |
---|
430 | |
---|
431 | //! Operator for Matrix3x3 -= Matrix3x3 |
---|
432 | inline_ Matrix3x3& operator-=(const Matrix3x3& mat) |
---|
433 | { |
---|
434 | m[0][0] -= mat.m[0][0]; m[0][1] -= mat.m[0][1]; m[0][2] -= mat.m[0][2]; |
---|
435 | m[1][0] -= mat.m[1][0]; m[1][1] -= mat.m[1][1]; m[1][2] -= mat.m[1][2]; |
---|
436 | m[2][0] -= mat.m[2][0]; m[2][1] -= mat.m[2][1]; m[2][2] -= mat.m[2][2]; |
---|
437 | return *this; |
---|
438 | } |
---|
439 | |
---|
440 | //! Operator for Matrix3x3 *= Matrix3x3 |
---|
441 | inline_ Matrix3x3& operator*=(const Matrix3x3& mat) |
---|
442 | { |
---|
443 | Point TempRow; |
---|
444 | |
---|
445 | GetRow(0, TempRow); |
---|
446 | m[0][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0]; |
---|
447 | m[0][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1]; |
---|
448 | m[0][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2]; |
---|
449 | |
---|
450 | GetRow(1, TempRow); |
---|
451 | m[1][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0]; |
---|
452 | m[1][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1]; |
---|
453 | m[1][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2]; |
---|
454 | |
---|
455 | GetRow(2, TempRow); |
---|
456 | m[2][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0]; |
---|
457 | m[2][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1]; |
---|
458 | m[2][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2]; |
---|
459 | return *this; |
---|
460 | } |
---|
461 | |
---|
462 | //! Operator for Matrix3x3 *= float |
---|
463 | inline_ Matrix3x3& operator*=(float s) |
---|
464 | { |
---|
465 | m[0][0] *= s; m[0][1] *= s; m[0][2] *= s; |
---|
466 | m[1][0] *= s; m[1][1] *= s; m[1][2] *= s; |
---|
467 | m[2][0] *= s; m[2][1] *= s; m[2][2] *= s; |
---|
468 | return *this; |
---|
469 | } |
---|
470 | |
---|
471 | //! Operator for Matrix3x3 /= float |
---|
472 | inline_ Matrix3x3& operator/=(float s) |
---|
473 | { |
---|
474 | if (s) s = 1.0f / s; |
---|
475 | m[0][0] *= s; m[0][1] *= s; m[0][2] *= s; |
---|
476 | m[1][0] *= s; m[1][1] *= s; m[1][2] *= s; |
---|
477 | m[2][0] *= s; m[2][1] *= s; m[2][2] *= s; |
---|
478 | return *this; |
---|
479 | } |
---|
480 | |
---|
481 | // Cast operators |
---|
482 | //! Cast a Matrix3x3 to a Matrix4x4. |
---|
483 | operator Matrix4x4() const; |
---|
484 | //! Cast a Matrix3x3 to a Quat. |
---|
485 | operator Quat() const; |
---|
486 | |
---|
487 | inline_ const Point& operator[](int row) const { return *(const Point*)&m[row][0]; } |
---|
488 | inline_ Point& operator[](int row) { return *(Point*)&m[row][0]; } |
---|
489 | |
---|
490 | public: |
---|
491 | |
---|
492 | float m[3][3]; |
---|
493 | }; |
---|
494 | |
---|
495 | #endif // __ICEMATRIX3X3_H__ |
---|
496 | |
---|