Developer Documentation
Loading...
Searching...
No Matches
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
66namespace 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
76template <typename Scalar>
77Matrix4x4T<Scalar>
78Matrix4x4T<Scalar>::
79operator* (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
84 Matrix4x4T<Scalar> tmp;
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
106template <typename Scalar>
107Matrix4x4T<Scalar>&
108Matrix4x4T<Scalar>::
109operator*= (const Matrix4x4T<Scalar>& _rhs)
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
134template <typename Scalar>
135Matrix4x4T<Scalar>&
136Matrix4x4T<Scalar>::
137leftMult(const Matrix4x4T<Scalar>& _rhs)
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
158template <typename Scalar>
159template <typename T>
160VectorT<T,4>
161Matrix4x4T<Scalar>::
162operator*(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
175template <typename Scalar>
176Matrix4x4T<Scalar>
177Matrix4x4T<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
192template <typename Scalar>
193template <typename T>
194VectorT<T,3>
195Matrix4x4T<Scalar>::
196transform_point(const VectorT<T,3>& _v) const
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
215template <typename Scalar>
216template <typename T>
217VectorT<T,3>
218Matrix4x4T<Scalar>::
219transform_vector(const VectorT<T,3>& _v) const
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
231template <typename Scalar>
232void
233Matrix4x4T<Scalar>::
234clear()
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
247template <typename Scalar>
248void
249Matrix4x4T<Scalar>::
250identity()
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
263template <typename Scalar>
264void
265Matrix4x4T<Scalar>::
266transpose()
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 */
288template <typename Scalar>
289bool
290Matrix4x4T<Scalar>::
291invert()
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//=============================================================================
ACG::Matrix4x4T::operator*=
Matrix4x4T & operator*=(const Matrix4x4T< Scalar > &_rhs)
self *= _rhs
Definition Matrix4x4T_impl.hh:109
ACG::Matrix4x4T::leftMult
Matrix4x4T & leftMult(const Matrix4x4T< Scalar > &_rhs)
multiply from left: self = _rhs * self
Definition Matrix4x4T_impl.hh:137
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_impl.hh:219
ACG::Matrix4x4T::identity
void identity()
setup an identity matrix
Definition Matrix4x4T_impl.hh:250
ACG::Matrix4x4T::invert
bool invert()
matrix inversion (returns true on success)
Definition Matrix4x4T_impl.hh:291
ACG::Matrix4x4T::clear
void clear()
sets all elements to zero
Definition Matrix4x4T_impl.hh:234
ACG::Matrix4x4T::operator*
Matrix4x4T operator*(const Matrix4x4T< Scalar > &inst) const
self * _rhs
Definition Matrix4x4T_impl.hh:79
ACG::Matrix4x4T::transpose
void transpose()
transpose matrix
Definition Matrix4x4T_impl.hh:266
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_impl.hh:196
ACG
Namespace providing different geometric functions concerning angles.
Definition BaseObjectData.hh:68