1 /***************************************************************************
2 * This file is part of Tecorrec. *
3 * Copyright 2008 James Hogan <james@albanarts.com> *
5 * Tecorrec is free software: you can redistribute it and/or modify *
6 * it under the terms of the GNU General Public License as published by *
7 * the Free Software Foundation, either version 2 of the License, or *
8 * (at your option) any later version. *
10 * Tecorrec is distributed in the hope that it will be useful, *
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13 * GNU General Public License for more details. *
15 * You should have received a copy of the GNU General Public License *
16 * along with Tecorrec. If not, write to the Free Software Foundation, *
17 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. *
18 ***************************************************************************/
22 * @brief Square matrix class.
23 * @author James Hogan (james@albanarts.com)
24 * @note Copyright (C) 2007
26 * @version 1 : From Computer Graphics and Visualisation Open Assessment.
30 #include "Quaternion.h"
35 Matrix
<3,float>::Matrix(const Quaternion
<float> & q
)
44 setVector3(col
[0].v
, 1-2*(yy
+zz
), 2*(xy
+zw
), 2*(xz
-yw
));
48 setVector3(col
[1].v
, 2*(xy
-zw
), 1-2*(xx
+zz
), 2*(yz
+xw
));
49 setVector3(col
[2].v
, 2*(xz
+yw
), 2*(yz
-xw
), 1-2*(xx
+yy
));
52 /// Get an arbitrary matrix where the z axis points in the direction of Z.
53 Matrix
<3,float> getTbnMatrix(Vector
<3, float> vZ
)
55 Vector
<3,float> vY(0, 0, -1);
56 Vector
<3,float> vX(cross(vY
, vZ
));
63 return Matrix
<3,float>(vX
, vY
, vZ
);
66 // Invert the 3x3 matrix
69 Matrix
<3,float> & Matrix
<3,float>::invert(bool* singular
)
77 unsigned int rowMax
; // Points to max abs value row in this column
88 // Find the row with max value in this column
90 for (r
=c
+1; r
<3; r
++) {
91 if (fabs(a
[c
][r
]) > fabs(a
[c
][rowMax
])) {
96 // If the max value here is 0, we can't invert. Return identity.
97 if (a
[rowMax
][c
] == 0.0F
)
106 // Swap row "rowMax" with row "c"
107 for (cc
=0; cc
<3; cc
++)
110 a
[cc
][c
] = a
[cc
][rowMax
];
113 b
[cc
][c
] = b
[cc
][rowMax
];
117 // Now everything we do is on row "c".
118 // Set the max cell to 1 by dividing the entire row by that value
120 for (cc
=0; cc
<3; cc
++) {
125 // Now do the other rows, so that this column only has a 1 and 0's
126 for (row
= 0; row
< 3; row
++) {
129 for (cc
=0; cc
<3; cc
++) {
130 a
[cc
][row
] -= a
[cc
][c
] * tmp
;
131 b
[cc
][row
] -= b
[cc
][c
] * tmp
;
143 // Invert the Matrix<4,float>
145 Matrix
<4,float> & Matrix
<4,float>::invert(bool* singular
)
147 Matrix
<4,float> a(*this);
153 unsigned int rowMax
; // Points to max abs value row in this column
162 // Go through columns
163 for (c
=0; c
<4; c
++) {
164 // Find the row with max value in this column
166 for (r
=c
+1; r
<4; r
++) {
167 if (fabs(a
[c
][r
]) > fabs(a
[c
][rowMax
])) {
172 // If the max value here is 0, we can't invert. Return identity.
173 if (a
[rowMax
][c
] == 0.0F
)
182 // Swap row "rowMax" with row "c"
183 for (cc
=0; cc
<4; cc
++) {
185 a
[cc
][c
] = a
[cc
][rowMax
];
188 b
[cc
][c
] = b
[cc
][rowMax
];
192 // Now everything we do is on row "c".
193 // Set the max cell to 1 by dividing the entire row by that value
195 for (cc
=0; cc
<4; cc
++) {
200 // Now do the other rows, so that this column only has a 1 and 0's
201 for (row
= 0; row
< 4; row
++) {
204 for (cc
=0; cc
<4; cc
++) {
205 a
[cc
][row
] -= a
[cc
][c
] * tmp
;
206 b
[cc
][row
] -= b
[cc
][c
] * tmp
;
218 // Return a 3D axis-rotation maths::Matrix<4,float>
219 // Pass in 'x', 'y', or 'z' for the axis.
220 Matrix
<4,float> RotateRadMatrix44(char axis
, float rad
)
232 ret
[0][0] = 1.0F
; ret
[1][0] = 0.0F
; ret
[2][0] = 0.0F
;
233 ret
[0][1] = 0.0F
; ret
[1][1] = cosA
; ret
[2][1] = -sinA
;
234 ret
[0][2] = 0.0F
; ret
[1][2] = sinA
; ret
[2][2] = cosA
;
239 ret
[0][0] = cosA
; ret
[1][0] = 0.0F
; ret
[2][0] = sinA
;
240 ret
[0][1] = 0.0F
; ret
[1][1] = 1.0F
; ret
[2][1] = 0.0F
;
241 ret
[0][2] = -sinA
; ret
[1][2] = 0.0F
; ret
[2][2] = cosA
;
246 ret
[0][0] = cosA
; ret
[1][0] = -sinA
; ret
[2][0] = 0.0F
;
247 ret
[0][1] = sinA
; ret
[1][1] = cosA
; ret
[2][1] = 0.0F
;
248 ret
[0][2] = 0.0F
; ret
[1][2] = 0.0F
; ret
[2][2] = 1.0F
;
252 ret
[0][3] = 0.0F
; ret
[1][3] = 0.0F
; ret
[2][3] = 0.0F
;
261 // Return a 3D axis-rotation maths::Matrix<4,float>
262 // Pass in an arbitrary maths::Vector<3,float> axis.
263 Matrix
<4,float> RotateRadMatrix44(const Vector
<3,float> &axis
, float rad
)
268 Vector
<3,float> nrm
= axis
;
275 invCosA
= 1.0F
- cosA
;
285 ret
[0][0] = (invCosA
* xSq
) + (cosA
);
286 ret
[1][0] = (invCosA
* x
* y
) - (sinA
* z
);
287 ret
[2][0] = (invCosA
* x
* z
) + (sinA
* y
);
290 ret
[0][1] = (invCosA
* x
* y
) + (sinA
* z
);
291 ret
[1][1] = (invCosA
* ySq
) + (cosA
);
292 ret
[2][1] = (invCosA
* y
* z
) - (sinA
* x
);
295 ret
[0][2] = (invCosA
* x
* z
) - (sinA
* y
);
296 ret
[1][2] = (invCosA
* y
* z
) + (sinA
* x
);
297 ret
[2][2] = (invCosA
* zSq
) + (cosA
);
308 // Return a 3D translation Matrix<4,float>
309 Matrix
<4,float> TranslateMatrix44(const maths::Vector
<3,float> & v
)
314 ret
[3].slice
<0,3>() = v
;
319 // Return a 3D/4D scale Matrix<4,float>
320 Matrix
<4,float> ScaleMatrix44(const maths::Vector
<4,float> & v
)
325 for (int i
= 0; i
< 4; ++i
) {