Developer Documentation
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Modules Pages
Matrix4x4T.cc
1 /*===========================================================================*\
2  * *
3  * OpenFlipper *
4  * Copyright (c) 2001-2015, RWTH-Aachen University *
5  * Department of Computer Graphics and Multimedia *
6  * All rights reserved. *
7  * www.openflipper.org *
8  * *
9  *---------------------------------------------------------------------------*
10  * This file is part of OpenFlipper. *
11  *---------------------------------------------------------------------------*
12  * *
13  * Redistribution and use in source and binary forms, with or without *
14  * modification, are permitted provided that the following conditions *
15  * are met: *
16  * *
17  * 1. Redistributions of source code must retain the above copyright notice, *
18  * this list of conditions and the following disclaimer. *
19  * *
20  * 2. Redistributions in binary form must reproduce the above copyright *
21  * notice, this list of conditions and the following disclaimer in the *
22  * documentation and/or other materials provided with the distribution. *
23  * *
24  * 3. Neither the name of the copyright holder nor the names of its *
25  * contributors may be used to endorse or promote products derived from *
26  * this software without specific prior written permission. *
27  * *
28  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS *
29  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED *
30  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A *
31  * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER *
32  * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, *
33  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, *
34  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR *
35  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF *
36  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING *
37  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS *
38  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
39  * *
40 \*===========================================================================*/
41 
42 /*===========================================================================*\
43  * *
44  * $Revision$ *
45  * $Author$ *
46  * $Date$ *
47  * *
48 \*===========================================================================*/
49 
50 
51 
52 //=============================================================================
53 //
54 // CLASS Matrix4x4T - IMPLEMENTATION
55 //
56 //=============================================================================
57 
58 
59 #define ACG_MATRIX4X4_C
60 
61 
62 //== INCLUDES =================================================================
63 
64 
65 #include "Matrix4x4T.hh"
66 #include "../Utils/NumLimitsT.hh"
67 
68 
69 //== IMPLEMENTATION ==========================================================
70 
71 
72 namespace ACG {
73 
74 
75 #define MAT(m,r,c) ((m)[(r)+((c)<<2)])
76 #define M(r,w) (MAT(mat_,r,w))
77 
78 
79 //-----------------------------------------------------------------------------
80 
81 
82 template <typename Scalar>
83 Matrix4x4T<Scalar>
84 Matrix4x4T<Scalar>::
85 operator* (const Matrix4x4T<Scalar>& _rhs) const
86 {
87 #define RHS(row,col) MAT(_rhs.mat_, row,col)
88 #define TMP(row,col) MAT(tmp.mat_, row,col)
89 
90  Matrix4x4T<Scalar> tmp;
91  Scalar mi0, mi1, mi2, mi3;
92  int i;
93 
94  for (i = 0; i < 4; i++) {
95  mi0=M(i,0); mi1=M(i,1); mi2=M(i,2); mi3=M(i,3);
96  TMP(i,0) = mi0*RHS(0,0) + mi1*RHS(1,0) + mi2*RHS(2,0) + mi3*RHS(3,0);
97  TMP(i,1) = mi0*RHS(0,1) + mi1*RHS(1,1) + mi2*RHS(2,1) + mi3*RHS(3,1);
98  TMP(i,2) = mi0*RHS(0,2) + mi1*RHS(1,2) + mi2*RHS(2,2) + mi3*RHS(3,2);
99  TMP(i,3) = mi0*RHS(0,3) + mi1*RHS(1,3) + mi2*RHS(2,3) + mi3*RHS(3,3);
100  }
101 
102  return tmp;
103 
104 #undef RHS
105 #undef TMP
106 }
107 
108 
109 //-----------------------------------------------------------------------------
110 
111 
112 template <typename Scalar>
113 Matrix4x4T<Scalar>&
114 Matrix4x4T<Scalar>::
115 operator*= (const Matrix4x4T<Scalar>& _rhs)
116 {
117 #define RHS(row,col) MAT(_rhs.mat_, row,col)
118 
119  int i;
120  Scalar mi0, mi1, mi2, mi3;
121 
122  for (i = 0; i < 4; i++)
123  {
124  mi0=M(i,0); mi1=M(i,1); mi2=M(i,2); mi3=M(i,3);
125  M(i,0) = mi0 * RHS(0,0) + mi1 * RHS(1,0) + mi2 * RHS(2,0) + mi3 * RHS(3,0);
126  M(i,1) = mi0 * RHS(0,1) + mi1 * RHS(1,1) + mi2 * RHS(2,1) + mi3 * RHS(3,1);
127  M(i,2) = mi0 * RHS(0,2) + mi1 * RHS(1,2) + mi2 * RHS(2,2) + mi3 * RHS(3,2);
128  M(i,3) = mi0 * RHS(0,3) + mi1 * RHS(1,3) + mi2 * RHS(2,3) + mi3 * RHS(3,3);
129  }
130 
131  return *this;
132 
133 #undef RHS
134 }
135 
136 
137 //-----------------------------------------------------------------------------
138 
139 
140 template <typename Scalar>
141 Matrix4x4T<Scalar>&
142 Matrix4x4T<Scalar>::
143 leftMult(const Matrix4x4T<Scalar>& _rhs)
144 {
145 #define RHS(row,col) MAT(_rhs.mat_, row,col)
146  int i;
147  Scalar m0i, m1i, m2i, m3i;
148  for(i=0;i<4;i++)
149  {
150  m0i = M(0,i); m1i = M(1,i); m2i = M(2,i); m3i = M(3,i);
151  M(0,i) = RHS(0,0)*m0i + RHS(0,1)*m1i + RHS(0,2)*m2i + RHS(0,3)*m3i;
152  M(1,i) = RHS(1,0)*m0i + RHS(1,1)*m1i + RHS(1,2)*m2i + RHS(1,3)*m3i;
153  M(2,i) = RHS(2,0)*m0i + RHS(2,1)*m1i + RHS(2,2)*m2i + RHS(2,3)*m3i;
154  M(3,i) = RHS(3,0)*m0i + RHS(3,1)*m1i + RHS(3,2)*m2i + RHS(3,3)*m3i;
155  }
156  return *this;
157 #undef RHS
158 }
159 
160 
161 //-----------------------------------------------------------------------------
162 
163 
164 template <typename Scalar>
165 template <typename T>
166 VectorT<T,4>
167 Matrix4x4T<Scalar>::
168 operator*(const VectorT<T,4>& _v) const
169 {
170  return VectorT<T,4> (
171  M(0,0)*_v[0] + M(0,1)*_v[1] + M(0,2)*_v[2] + M(0,3)*_v[3],
172  M(1,0)*_v[0] + M(1,1)*_v[1] + M(1,2)*_v[2] + M(1,3)*_v[3],
173  M(2,0)*_v[0] + M(2,1)*_v[1] + M(2,2)*_v[2] + M(2,3)*_v[3],
174  M(3,0)*_v[0] + M(3,1)*_v[1] + M(3,2)*_v[2] + M(3,3)*_v[3]);
175 }
176 
177 
178 //-----------------------------------------------------------------------------
179 
180 
181 template <typename Scalar>
182 Matrix4x4T<Scalar>
183 Matrix4x4T<Scalar>::operator*(const Scalar& scalar)
184 {
185  for (int i = 0; i < 4; ++i) {
186  for (int j = 0; j < 4; ++j) {
187  M(i,j) *= scalar;
188  }
189  }
190 
191  return *this;
192 }
193 
194 
195 //-----------------------------------------------------------------------------
196 
197 
198 template <typename Scalar>
199 template <typename T>
200 VectorT<T,3>
201 Matrix4x4T<Scalar>::
202 transform_point(const VectorT<T,3>& _v) const
203 {
204  Scalar x = M(0,0)*_v[0] + M(0,1)*_v[1] + M(0,2)*_v[2] + M(0,3);
205  Scalar y = M(1,0)*_v[0] + M(1,1)*_v[1] + M(1,2)*_v[2] + M(1,3);
206  Scalar z = M(2,0)*_v[0] + M(2,1)*_v[1] + M(2,2)*_v[2] + M(2,3);
207  Scalar w = M(3,0)*_v[0] + M(3,1)*_v[1] + M(3,2)*_v[2] + M(3,3);
208 
209  if (w)
210  {
211  w = 1.0 / w;
212  return VectorT<T,3>(x*w, y*w, z*w);
213  }
214  else return VectorT<T,3>(x, y, z);
215 }
216 
217 
218 //-----------------------------------------------------------------------------
219 
220 
221 template <typename Scalar>
222 template <typename T>
223 VectorT<T,3>
224 Matrix4x4T<Scalar>::
225 transform_vector(const VectorT<T,3>& _v) const
226 {
227  Scalar x = M(0,0)*_v[0] + M(0,1)*_v[1] + M(0,2)*_v[2];
228  Scalar y = M(1,0)*_v[0] + M(1,1)*_v[1] + M(1,2)*_v[2];
229  Scalar z = M(2,0)*_v[0] + M(2,1)*_v[1] + M(2,2)*_v[2];
230  return VectorT<T,3>(x, y, z);
231 }
232 
233 
234 //-----------------------------------------------------------------------------
235 
236 
237 template <typename Scalar>
238 void
239 Matrix4x4T<Scalar>::
240 clear()
241 {
242  Scalar* m = mat_;
243  *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0;
244  *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0;
245  *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0;
246  *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m = 0.0;
247 }
248 
249 
250 //-----------------------------------------------------------------------------
251 
252 
253 template <typename Scalar>
254 void
255 Matrix4x4T<Scalar>::
256 identity()
257 {
258  Scalar* m = mat_;
259  *m++ = 1.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0;
260  *m++ = 0.0; *m++ = 1.0; *m++ = 0.0; *m++ = 0.0;
261  *m++ = 0.0; *m++ = 0.0; *m++ = 1.0; *m++ = 0.0;
262  *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m = 1.0;
263 }
264 
265 
266 //-----------------------------------------------------------------------------
267 
268 
269 template <typename Scalar>
270 void
271 Matrix4x4T<Scalar>::
272 transpose()
273 {
274  Scalar tmp;
275  for( int i=0; i<4; i++ )
276  {
277  for( int j=i+1; j<4; j++ )
278  {
279  tmp = MAT(mat_,i,j);
280  MAT(mat_,i,j) = MAT(mat_,j,i);
281  MAT(mat_,j,i) = tmp;
282  }
283  }
284 }
285 
286 
287 //-----------------------------------------------------------------------------
288 
289 
290 /*
291  * Compute inverse of 4x4 transformation matrix.
292  * Taken from Mesa3.1
293  * Code contributed by Jacques Leroy jle@star.be */
294 template <typename Scalar>
295 bool
296 Matrix4x4T<Scalar>::
297 invert()
298 {
299 #define SWAP_ROWS(a, b) { Scalar *_tmp = a; (a)=(b); (b)=_tmp; }
300 
301  Scalar wtmp[4][8];
302  Scalar m0, m1, m2, m3, s;
303  Scalar *r0, *r1, *r2, *r3;
304 
305  r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
306 
307  r0[0] = M(0,0); r0[1] = M(0,1);
308  r0[2] = M(0,2); r0[3] = M(0,3);
309  r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0;
310 
311  r1[0] = M(1,0); r1[1] = M(1,1);
312  r1[2] = M(1,2); r1[3] = M(1,3);
313  r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0;
314 
315  r2[0] = M(2,0); r2[1] = M(2,1);
316  r2[2] = M(2,2); r2[3] = M(2,3);
317  r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0;
318 
319  r3[0] = M(3,0); r3[1] = M(3,1);
320  r3[2] = M(3,2); r3[3] = M(3,3);
321  r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0;
322 
323 
324  /* choose pivot - or die */
325  if (fabs(r3[0])>fabs(r2[0])) SWAP_ROWS(r3, r2);
326  if (fabs(r2[0])>fabs(r1[0])) SWAP_ROWS(r2, r1);
327  if (fabs(r1[0])>fabs(r0[0])) SWAP_ROWS(r1, r0);
328  if (0.0 == r0[0]) return false;
329 
330 
331  /* eliminate first variable */
332  m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0];
333  s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s;
334  s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s;
335  s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s;
336  s = r0[4];
337  if (s != 0.0) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; }
338  s = r0[5];
339  if (s != 0.0) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; }
340  s = r0[6];
341  if (s != 0.0) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; }
342  s = r0[7];
343  if (s != 0.0) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; }
344 
345 
346  /* choose pivot - or die */
347  if (fabs(r3[1])>fabs(r2[1])) SWAP_ROWS(r3, r2);
348  if (fabs(r2[1])>fabs(r1[1])) SWAP_ROWS(r2, r1);
349  if (0.0 == r1[1]) return false;
350 
351 
352  /* eliminate second variable */
353  m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1];
354  r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2];
355  r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3];
356  s = r1[4]; if (0.0 != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; }
357  s = r1[5]; if (0.0 != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; }
358  s = r1[6]; if (0.0 != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; }
359  s = r1[7]; if (0.0 != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; }
360 
361 
362  /* choose pivot - or die */
363  if (fabs(r3[2])>fabs(r2[2])) SWAP_ROWS(r3, r2);
364  if (0.0 == r2[2]) return false;
365 
366  /* eliminate third variable */
367  m3 = r3[2]/r2[2];
368  r3[3] -= m3 * r2[3];
369  r3[4] -= m3 * r2[4];
370  r3[5] -= m3 * r2[5];
371  r3[6] -= m3 * r2[6];
372  r3[7] -= m3 * r2[7];
373 
374  /* last check */
375  if (0.0 == r3[3]) return false;
376 
377  s = 1.0/r3[3]; /* now back substitute row 3 */
378  r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s;
379 
380  m2 = r2[3]; /* now back substitute row 2 */
381  s = 1.0/r2[2];
382  r2[4] = s * (r2[4] - r3[4] * m2); r2[5] = s * (r2[5] - r3[5] * m2);
383  r2[6] = s * (r2[6] - r3[6] * m2); r2[7] = s * (r2[7] - r3[7] * m2);
384  m1 = r1[3];
385  r1[4] -= r3[4] * m1; r1[5] -= r3[5] * m1;
386  r1[6] -= r3[6] * m1; r1[7] -= r3[7] * m1;
387  m0 = r0[3];
388  r0[4] -= r3[4] * m0; r0[5] -= r3[5] * m0;
389  r0[6] -= r3[6] * m0; r0[7] -= r3[7] * m0;
390 
391  m1 = r1[2]; /* now back substitute row 1 */
392  s = 1.0/r1[1];
393  r1[4] = s * (r1[4] - r2[4] * m1); r1[5] = s * (r1[5] - r2[5] * m1);
394  r1[6] = s * (r1[6] - r2[6] * m1); r1[7] = s * (r1[7] - r2[7] * m1);
395  m0 = r0[2];
396  r0[4] -= r2[4] * m0; r0[5] -= r2[5] * m0;
397  r0[6] -= r2[6] * m0; r0[7] -= r2[7] * m0;
398 
399  m0 = r0[1]; /* now back substitute row 0 */
400  s = 1.0/r0[0];
401  r0[4] = s * (r0[4] - r1[4] * m0); r0[5] = s * (r0[5] - r1[5] * m0);
402  r0[6] = s * (r0[6] - r1[6] * m0); r0[7] = s * (r0[7] - r1[7] * m0);
403 
404  M(0,0) = r0[4]; M(0,1) = r0[5];
405  M(0,2) = r0[6]; M(0,3) = r0[7];
406  M(1,0) = r1[4]; M(1,1) = r1[5];
407  M(1,2) = r1[6]; M(1,3) = r1[7];
408  M(2,0) = r2[4]; M(2,1) = r2[5];
409  M(2,2) = r2[6]; M(2,3) = r2[7];
410  M(3,0) = r3[4]; M(3,1) = r3[5];
411  M(3,2) = r3[6]; M(3,3) = r3[7];
412 
413  return true;
414 #undef SWAP_ROWS
415 }
416 
417 
418 //-----------------------------------------------------------------------------
419 
420 
421 #undef MAT
422 #undef M
423 
424 
425 //=============================================================================
426 } // namespace ACG
427 //=============================================================================
ACG::Matrix4x4T::operator*=
Matrix4x4T & operator*=(const Matrix4x4T< Scalar > &_rhs)
self *= _rhs
Definition: Matrix4x4T.cc:115
ACG
Namespace providing different geometric functions concerning angles.
Definition: DBSCANT.cc:51
ACG::Matrix4x4T::transform_vector
VectorT< T, 3 > transform_vector(const VectorT< T, 3 > &_v) const
transform vector (x',y',z',0) = A * (x,y,z,0)
Definition: Matrix4x4T.cc:225
ACG::Matrix4x4T::leftMult
Matrix4x4T & leftMult(const Matrix4x4T< Scalar > &_rhs)
multiply from left: self = _rhs * self
Definition: Matrix4x4T.cc:143
ACG::Matrix4x4T::operator*
Matrix4x4T operator*(const Matrix4x4T< Scalar > &inst) const
self * _rhs
Definition: Matrix4x4T.cc:85
ACG::Matrix4x4T::clear
void clear()
sets all elements to zero
Definition: Matrix4x4T.cc:240
ACG::Matrix4x4T::identity
void identity()
setup an identity matrix
Definition: Matrix4x4T.cc:256
ACG::Matrix4x4T::transform_point
VectorT< T, 3 > transform_point(const VectorT< T, 3 > &_v) const
transform point (x',y',z',1) = M * (x,y,z,1)
Definition: Matrix4x4T.cc:202
ACG::Matrix4x4T::transpose
void transpose()
transpose matrix
Definition: Matrix4x4T.cc:272
ACG::Matrix4x4T::invert
bool invert()
matrix inversion (returns true on success)
Definition: Matrix4x4T.cc:297