Developer Documentation
Matrix4x4T_impl.hh
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 
45 
46 //=============================================================================
47 //
48 // CLASS Matrix4x4T - IMPLEMENTATION
49 //
50 //=============================================================================
51 
52 
53 #define ACG_MATRIX4X4_C
54 
55 
56 //== INCLUDES =================================================================
57 
58 
59 #include "Matrix4x4T.hh"
60 #include "../Utils/NumLimitsT.hh"
61 
62 
63 //== IMPLEMENTATION ==========================================================
64 
65 
66 namespace ACG {
67 
68 
69 #define MAT(m,r,c) ((m)[(r)+((c)<<2)])
70 #define M(r,w) (MAT(mat_,r,w))
71 
72 
73 //-----------------------------------------------------------------------------
74 
75 
76 template <typename Scalar>
77 Matrix4x4T<Scalar>
79 operator* (const Matrix4x4T<Scalar>& _rhs) const
80 {
81 #define RHS(row,col) MAT(_rhs.mat_, row,col)
82 #define TMP(row,col) MAT(tmp.mat_, row,col)
83 
85  Scalar mi0, mi1, mi2, mi3;
86  int i;
87 
88  for (i = 0; i < 4; i++) {
89  mi0=M(i,0); mi1=M(i,1); mi2=M(i,2); mi3=M(i,3);
90  TMP(i,0) = mi0*RHS(0,0) + mi1*RHS(1,0) + mi2*RHS(2,0) + mi3*RHS(3,0);
91  TMP(i,1) = mi0*RHS(0,1) + mi1*RHS(1,1) + mi2*RHS(2,1) + mi3*RHS(3,1);
92  TMP(i,2) = mi0*RHS(0,2) + mi1*RHS(1,2) + mi2*RHS(2,2) + mi3*RHS(3,2);
93  TMP(i,3) = mi0*RHS(0,3) + mi1*RHS(1,3) + mi2*RHS(2,3) + mi3*RHS(3,3);
94  }
95 
96  return tmp;
97 
98 #undef RHS
99 #undef TMP
100 }
101 
102 
103 //-----------------------------------------------------------------------------
104 
105 
106 template <typename Scalar>
110 {
111 #define RHS(row,col) MAT(_rhs.mat_, row,col)
112 
113  int i;
114  Scalar mi0, mi1, mi2, mi3;
115 
116  for (i = 0; i < 4; i++)
117  {
118  mi0=M(i,0); mi1=M(i,1); mi2=M(i,2); mi3=M(i,3);
119  M(i,0) = mi0 * RHS(0,0) + mi1 * RHS(1,0) + mi2 * RHS(2,0) + mi3 * RHS(3,0);
120  M(i,1) = mi0 * RHS(0,1) + mi1 * RHS(1,1) + mi2 * RHS(2,1) + mi3 * RHS(3,1);
121  M(i,2) = mi0 * RHS(0,2) + mi1 * RHS(1,2) + mi2 * RHS(2,2) + mi3 * RHS(3,2);
122  M(i,3) = mi0 * RHS(0,3) + mi1 * RHS(1,3) + mi2 * RHS(2,3) + mi3 * RHS(3,3);
123  }
124 
125  return *this;
126 
127 #undef RHS
128 }
129 
130 
131 //-----------------------------------------------------------------------------
132 
133 
134 template <typename Scalar>
138 {
139 #define RHS(row,col) MAT(_rhs.mat_, row,col)
140  int i;
141  Scalar m0i, m1i, m2i, m3i;
142  for(i=0;i<4;i++)
143  {
144  m0i = M(0,i); m1i = M(1,i); m2i = M(2,i); m3i = M(3,i);
145  M(0,i) = RHS(0,0)*m0i + RHS(0,1)*m1i + RHS(0,2)*m2i + RHS(0,3)*m3i;
146  M(1,i) = RHS(1,0)*m0i + RHS(1,1)*m1i + RHS(1,2)*m2i + RHS(1,3)*m3i;
147  M(2,i) = RHS(2,0)*m0i + RHS(2,1)*m1i + RHS(2,2)*m2i + RHS(2,3)*m3i;
148  M(3,i) = RHS(3,0)*m0i + RHS(3,1)*m1i + RHS(3,2)*m2i + RHS(3,3)*m3i;
149  }
150  return *this;
151 #undef RHS
152 }
153 
154 
155 //-----------------------------------------------------------------------------
156 
157 
158 template <typename Scalar>
159 template <typename T>
162 operator*(const VectorT<T,4>& _v) const
163 {
164  return VectorT<T,4> (
165  M(0,0)*_v[0] + M(0,1)*_v[1] + M(0,2)*_v[2] + M(0,3)*_v[3],
166  M(1,0)*_v[0] + M(1,1)*_v[1] + M(1,2)*_v[2] + M(1,3)*_v[3],
167  M(2,0)*_v[0] + M(2,1)*_v[1] + M(2,2)*_v[2] + M(2,3)*_v[3],
168  M(3,0)*_v[0] + M(3,1)*_v[1] + M(3,2)*_v[2] + M(3,3)*_v[3]);
169 }
170 
171 
172 //-----------------------------------------------------------------------------
173 
174 
175 template <typename Scalar>
177 Matrix4x4T<Scalar>::operator*(const Scalar& scalar)
178 {
179  for (int i = 0; i < 4; ++i) {
180  for (int j = 0; j < 4; ++j) {
181  M(i,j) *= scalar;
182  }
183  }
184 
185  return *this;
186 }
187 
188 
189 //-----------------------------------------------------------------------------
190 
191 
192 template <typename Scalar>
193 template <typename T>
197 {
198  Scalar x = M(0,0)*_v[0] + M(0,1)*_v[1] + M(0,2)*_v[2] + M(0,3);
199  Scalar y = M(1,0)*_v[0] + M(1,1)*_v[1] + M(1,2)*_v[2] + M(1,3);
200  Scalar z = M(2,0)*_v[0] + M(2,1)*_v[1] + M(2,2)*_v[2] + M(2,3);
201  Scalar w = M(3,0)*_v[0] + M(3,1)*_v[1] + M(3,2)*_v[2] + M(3,3);
202 
203  if (w)
204  {
205  w = 1.0 / w;
206  return VectorT<T,3>(x*w, y*w, z*w);
207  }
208  else return VectorT<T,3>(x, y, z);
209 }
210 
211 
212 //-----------------------------------------------------------------------------
213 
214 
215 template <typename Scalar>
216 template <typename T>
220 {
221  Scalar x = M(0,0)*_v[0] + M(0,1)*_v[1] + M(0,2)*_v[2];
222  Scalar y = M(1,0)*_v[0] + M(1,1)*_v[1] + M(1,2)*_v[2];
223  Scalar z = M(2,0)*_v[0] + M(2,1)*_v[1] + M(2,2)*_v[2];
224  return VectorT<T,3>(x, y, z);
225 }
226 
227 
228 //-----------------------------------------------------------------------------
229 
230 
231 template <typename Scalar>
232 void
235 {
236  Scalar* m = mat_;
237  *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0;
238  *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0;
239  *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0;
240  *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m = 0.0;
241 }
242 
243 
244 //-----------------------------------------------------------------------------
245 
246 
247 template <typename Scalar>
248 void
251 {
252  Scalar* m = mat_;
253  *m++ = 1.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0;
254  *m++ = 0.0; *m++ = 1.0; *m++ = 0.0; *m++ = 0.0;
255  *m++ = 0.0; *m++ = 0.0; *m++ = 1.0; *m++ = 0.0;
256  *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m = 1.0;
257 }
258 
259 
260 //-----------------------------------------------------------------------------
261 
262 
263 template <typename Scalar>
264 void
267 {
268  Scalar tmp;
269  for( int i=0; i<4; i++ )
270  {
271  for( int j=i+1; j<4; j++ )
272  {
273  tmp = MAT(mat_,i,j);
274  MAT(mat_,i,j) = MAT(mat_,j,i);
275  MAT(mat_,j,i) = tmp;
276  }
277  }
278 }
279 
280 
281 //-----------------------------------------------------------------------------
282 
283 
284 /*
285  * Compute inverse of 4x4 transformation matrix.
286  * Taken from Mesa3.1
287  * Code contributed by Jacques Leroy jle@star.be */
288 template <typename Scalar>
289 bool
292 {
293 #define SWAP_ROWS(a, b) { Scalar *_tmp = a; (a)=(b); (b)=_tmp; }
294 
295  Scalar wtmp[4][8];
296  Scalar m0, m1, m2, m3, s;
297  Scalar *r0, *r1, *r2, *r3;
298 
299  r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
300 
301  r0[0] = M(0,0); r0[1] = M(0,1);
302  r0[2] = M(0,2); r0[3] = M(0,3);
303  r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0;
304 
305  r1[0] = M(1,0); r1[1] = M(1,1);
306  r1[2] = M(1,2); r1[3] = M(1,3);
307  r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0;
308 
309  r2[0] = M(2,0); r2[1] = M(2,1);
310  r2[2] = M(2,2); r2[3] = M(2,3);
311  r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0;
312 
313  r3[0] = M(3,0); r3[1] = M(3,1);
314  r3[2] = M(3,2); r3[3] = M(3,3);
315  r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0;
316 
317 
318  /* choose pivot - or die */
319  if (fabs(r3[0])>fabs(r2[0])) SWAP_ROWS(r3, r2);
320  if (fabs(r2[0])>fabs(r1[0])) SWAP_ROWS(r2, r1);
321  if (fabs(r1[0])>fabs(r0[0])) SWAP_ROWS(r1, r0);
322  if (0.0 == r0[0]) return false;
323 
324 
325  /* eliminate first variable */
326  m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0];
327  s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s;
328  s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s;
329  s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s;
330  s = r0[4];
331  if (s != 0.0) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; }
332  s = r0[5];
333  if (s != 0.0) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; }
334  s = r0[6];
335  if (s != 0.0) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; }
336  s = r0[7];
337  if (s != 0.0) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; }
338 
339 
340  /* choose pivot - or die */
341  if (fabs(r3[1])>fabs(r2[1])) SWAP_ROWS(r3, r2);
342  if (fabs(r2[1])>fabs(r1[1])) SWAP_ROWS(r2, r1);
343  if (0.0 == r1[1]) return false;
344 
345 
346  /* eliminate second variable */
347  m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1];
348  r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2];
349  r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3];
350  s = r1[4]; if (0.0 != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; }
351  s = r1[5]; if (0.0 != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; }
352  s = r1[6]; if (0.0 != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; }
353  s = r1[7]; if (0.0 != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; }
354 
355 
356  /* choose pivot - or die */
357  if (fabs(r3[2])>fabs(r2[2])) SWAP_ROWS(r3, r2);
358  if (0.0 == r2[2]) return false;
359 
360  /* eliminate third variable */
361  m3 = r3[2]/r2[2];
362  r3[3] -= m3 * r2[3];
363  r3[4] -= m3 * r2[4];
364  r3[5] -= m3 * r2[5];
365  r3[6] -= m3 * r2[6];
366  r3[7] -= m3 * r2[7];
367 
368  /* last check */
369  if (0.0 == r3[3]) return false;
370 
371  s = 1.0/r3[3]; /* now back substitute row 3 */
372  r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s;
373 
374  m2 = r2[3]; /* now back substitute row 2 */
375  s = 1.0/r2[2];
376  r2[4] = s * (r2[4] - r3[4] * m2); r2[5] = s * (r2[5] - r3[5] * m2);
377  r2[6] = s * (r2[6] - r3[6] * m2); r2[7] = s * (r2[7] - r3[7] * m2);
378  m1 = r1[3];
379  r1[4] -= r3[4] * m1; r1[5] -= r3[5] * m1;
380  r1[6] -= r3[6] * m1; r1[7] -= r3[7] * m1;
381  m0 = r0[3];
382  r0[4] -= r3[4] * m0; r0[5] -= r3[5] * m0;
383  r0[6] -= r3[6] * m0; r0[7] -= r3[7] * m0;
384 
385  m1 = r1[2]; /* now back substitute row 1 */
386  s = 1.0/r1[1];
387  r1[4] = s * (r1[4] - r2[4] * m1); r1[5] = s * (r1[5] - r2[5] * m1);
388  r1[6] = s * (r1[6] - r2[6] * m1); r1[7] = s * (r1[7] - r2[7] * m1);
389  m0 = r0[2];
390  r0[4] -= r2[4] * m0; r0[5] -= r2[5] * m0;
391  r0[6] -= r2[6] * m0; r0[7] -= r2[7] * m0;
392 
393  m0 = r0[1]; /* now back substitute row 0 */
394  s = 1.0/r0[0];
395  r0[4] = s * (r0[4] - r1[4] * m0); r0[5] = s * (r0[5] - r1[5] * m0);
396  r0[6] = s * (r0[6] - r1[6] * m0); r0[7] = s * (r0[7] - r1[7] * m0);
397 
398  M(0,0) = r0[4]; M(0,1) = r0[5];
399  M(0,2) = r0[6]; M(0,3) = r0[7];
400  M(1,0) = r1[4]; M(1,1) = r1[5];
401  M(1,2) = r1[6]; M(1,3) = r1[7];
402  M(2,0) = r2[4]; M(2,1) = r2[5];
403  M(2,2) = r2[6]; M(2,3) = r2[7];
404  M(3,0) = r3[4]; M(3,1) = r3[5];
405  M(3,2) = r3[6]; M(3,3) = r3[7];
406 
407  return true;
408 #undef SWAP_ROWS
409 }
410 
411 
412 //-----------------------------------------------------------------------------
413 
414 
415 #undef MAT
416 #undef M
417 
418 
419 //=============================================================================
420 } // namespace ACG
421 //=============================================================================
void clear()
sets all elements to zero
Namespace providing different geometric functions concerning angles.
void transpose()
transpose matrix
void identity()
setup an identity matrix
VectorT< T, 3 > transform_point(const VectorT< T, 3 > &_v) const
transform point (x&#39;,y&#39;,z&#39;,1) = M * (x,y,z,1)
bool invert()
matrix inversion (returns true on success)
VectorT< T, 3 > transform_vector(const VectorT< T, 3 > &_v) const
transform vector (x&#39;,y&#39;,z&#39;,0) = A * (x,y,z,0)
Matrix4x4T & operator*=(const Matrix4x4T< Scalar > &_rhs)
self *= _rhs
Matrix4x4T operator*(const Matrix4x4T< Scalar > &inst) const
self * _rhs
Matrix4x4T & leftMult(const Matrix4x4T< Scalar > &_rhs)
multiply from left: self = _rhs * self