Branch data Line data Source code
1 : : /**
2 : : * MOAB, a Mesh-Oriented datABase, is a software component for creating,
3 : : * storing and accessing finite element mesh data.
4 : : *
5 : : * Copyright 2004 Sandia Corporation. Under the terms of Contract
6 : : * DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government
7 : : * retains certain rights in this software.
8 : : *
9 : : * This library is free software; you can redistribute it and/or
10 : : * modify it under the terms of the GNU Lesser General Public
11 : : * License as published by the Free Software Foundation; either
12 : : * version 2.1 of the License, or (at your option) any later version.
13 : : *
14 : : */
15 : :
16 : : #ifndef MOAB_HOMXFORM
17 : : #define MOAB_HOMXFORM
18 : :
19 : : /*
20 : : * \file HomXform.hpp
21 : : *
22 : : * \brief Representation and functions for homogeneous transforms
23 : : *
24 : : * A homogeneous transform is a 4x4 matrix for representing and manipulating
25 : : * homogeneous coordinates, which are x' = (x/h, y/h, z/h, h). See Mortenson,
26 : : * Geometric Modeling, or other texts on geometric modeling for details of homogeneous
27 : : * transforms.
28 : : */
29 : :
30 : : #define XFORM( a, b ) xForm[4 * a + b]
31 : : #define XFORM_INDEX( a, b ) 4 * a + b
32 : :
33 : : #include <math.h>
34 : : #include <ostream>
35 : :
36 : : #include "moab/win32_config.h"
37 : :
38 : : namespace moab
39 : : {
40 : :
41 : : class HomXform;
42 : :
43 : : /** \class HomCoord
44 : : * \brief Homogeneous coordinate vector
45 : : */
46 : : class HomCoord
47 : : {
48 : : private:
49 : : //! coordinate data
50 : : #if defined( __INTEL_COMPILER ) && ( __INTEL_COMPILER < 1310 )
51 : : // Hack Intel compiler 12 issues with -O2 optimization
52 : : int homCoord[5];
53 : : #else
54 : : int homCoord[4];
55 : : #endif
56 : :
57 : : public:
58 : : friend class HomXform;
59 : :
60 : : static MOAB_EXPORT HomCoord unitv[3];
61 : : static MOAB_EXPORT HomCoord IDENTITY;
62 : :
63 : : //! constructors
64 : : HomCoord();
65 : : HomCoord( const int coords[], const int num_coords = 4 );
66 : : HomCoord( const int coord0, const int coord1, const int coord2, const int coord3 );
67 : : HomCoord( const int coord0, const int coord1, const int coord2 );
68 : : HomCoord( const HomCoord& coord );
69 : :
70 : : //! set function
71 : : void set( const int coords[] );
72 : : void set( const int i, const int j, const int k, const int h = 1 );
73 : :
74 : : //! get function
75 : 9630698 : const int* hom_coord() const
76 : : {
77 : 9630698 : return homCoord;
78 : : }
79 : :
80 : : // Getters needed for share windows
81 : : static HomCoord& getUnitv( int c );
82 : :
83 : : //! parameter-based access functions
84 : 895833 : int i() const
85 : : {
86 : 895833 : return homCoord[0];
87 : : }
88 : 895340 : int j() const
89 : : {
90 : 895340 : return homCoord[1];
91 : : }
92 : 895159 : int k() const
93 : : {
94 : 895159 : return homCoord[2];
95 : : }
96 : 1 : int h() const
97 : : {
98 : 1 : return homCoord[3];
99 : : }
100 : :
101 : : //! squared length
102 : : int length_squared() const;
103 : :
104 : : //! length
105 : : int length() const;
106 : :
107 : : //! normalize
108 : : void normalize();
109 : :
110 : : //! operators
111 : : HomCoord& operator*=( const HomXform& rhs2 );
112 : : HomCoord operator*( const HomXform& rhs2 ) const;
113 : : HomCoord& operator*=( const int mult );
114 : : HomCoord operator*( const int mult ) const;
115 : : inline HomCoord& operator/=( const HomXform& rhs2 );
116 : : HomCoord operator/( const HomXform& rhs2 ) const;
117 : : inline HomCoord& operator/=( const int mult );
118 : : HomCoord operator/( const int mult ) const;
119 : : inline HomCoord& operator+=( const HomCoord& rhs1 );
120 : : HomCoord operator+( const HomCoord& rhs1 ) const;
121 : : inline HomCoord& operator-=( const HomCoord& rhs1 );
122 : : HomCoord operator-( const HomCoord& rhs1 ) const;
123 : : HomCoord& operator=( const HomCoord& rhs );
124 : :
125 : : // dot product
126 : : int operator%( const HomCoord& rhs ) const;
127 : :
128 : : // cross product
129 : : HomCoord operator*( const HomCoord& rhs ) const;
130 : : HomCoord& operator*=( const HomCoord& rhs );
131 : :
132 : : bool operator==( const HomCoord& rhs1 ) const;
133 : : bool operator!=( const HomCoord& rhs1 ) const;
134 : : bool operator>=( const HomCoord& rhs1 ) const;
135 : : bool operator<=( const HomCoord& rhs1 ) const;
136 : : bool operator>( const HomCoord& rhs1 ) const;
137 : : bool operator<( const HomCoord& rhs1 ) const;
138 : : inline int operator[]( const int& param ) const;
139 : : int& operator[]( const int& param );
140 : : };
141 : :
142 : : /** \class HomXform
143 : : * \brief Homogeneous coordinate transformation matrix
144 : : */
145 : : class HomXform
146 : : {
147 : :
148 : : private:
149 : : //! the matrix; don't bother storing the last column, since we assume for now it's
150 : : //! always unused
151 : : int xForm[16];
152 : :
153 : : public:
154 : : friend class HomCoord;
155 : :
156 : : static MOAB_EXPORT HomXform IDENTITY;
157 : :
158 : : //! constructor from matrix
159 : : HomXform( const int matrix[16] );
160 : :
161 : : //! bare constructor
162 : : HomXform();
163 : :
164 : : //! constructor from rotation, scaling, translation
165 : : HomXform( const int rotate[9], const int scale[3], const int translate[3] );
166 : :
167 : : //! constructor taking 16 ints, useful for efficient operators
168 : : HomXform( int i1, int i2, int i3, int i4, int i5, int i6, int i7, int i8, int i9, int i10, int i11, int i12,
169 : : int i13, int i14, int i15, int i16 );
170 : :
171 : : //! return this.inverse
172 : : inline HomXform inverse() const;
173 : :
174 : : //! compute a transform from three points
175 : : void three_pt_xform( const HomCoord& p1, const HomCoord& q1, const HomCoord& p2, const HomCoord& q2,
176 : : const HomCoord& p3, const HomCoord& q3 );
177 : :
178 : : //! operators
179 : : int operator[]( const int& count ) const;
180 : : int& operator[]( const int& count );
181 : : bool operator==( const HomXform& rhs ) const;
182 : : bool operator!=( const HomXform& rhs ) const;
183 : :
184 : : HomXform& operator=( const HomXform& rhs );
185 : : HomXform& operator*=( const HomXform& rhs );
186 : : HomXform operator*( const HomXform& rhs2 ) const;
187 : : };
188 : :
189 : 13608 : inline HomCoord::HomCoord()
190 : : {
191 : 13608 : homCoord[0] = 0;
192 : 13608 : homCoord[1] = 0;
193 : 13608 : homCoord[2] = 0;
194 : 13608 : homCoord[3] = 0;
195 : 13608 : }
196 : :
197 : 53346 : inline HomCoord::HomCoord( const int coords[], const int num_coords )
198 : : {
199 [ + + ]: 213385 : for( int tmpj = 0; tmpj < num_coords; tmpj++ )
200 : 160039 : homCoord[tmpj] = coords[tmpj];
201 [ + + ]: 53346 : if( num_coords != 4 ) homCoord[3] = 1;
202 : 53346 : }
203 : :
204 : 1415937 : inline HomCoord::HomCoord( const int coord0, const int coord1, const int coord2, const int coord3 )
205 : : {
206 : 1415937 : homCoord[0] = coord0;
207 : 1415937 : homCoord[1] = coord1;
208 : 1415937 : homCoord[2] = coord2;
209 : 1415937 : homCoord[3] = coord3;
210 : 1415937 : }
211 : :
212 : 1635822 : inline HomCoord::HomCoord( const int coord0, const int coord1, const int coord2 )
213 : : {
214 : 1635822 : homCoord[0] = coord0;
215 : 1635822 : homCoord[1] = coord1;
216 : 1635822 : homCoord[2] = coord2;
217 : 1635822 : homCoord[3] = 1;
218 : 1635822 : }
219 : :
220 : 8649779 : inline HomCoord::HomCoord( const HomCoord& coords )
221 : : {
222 : : #if defined( __INTEL_COMPILER ) && ( __INTEL_COMPILER < 1310 )
223 : : // Hack Intel compiler 12 issues with -O2 optimization
224 : : int coord0 = coords[0];
225 : : int coord1 = coords[1];
226 : : int coord2 = coords[2];
227 : : int coord3 = coords[3];
228 : : homCoord[0] = coord0;
229 : : homCoord[1] = coord1;
230 : : homCoord[2] = coord2;
231 : : homCoord[3] = coord3;
232 : : #else
233 [ + - ]: 8649779 : homCoord[0] = coords[0];
234 [ + - ]: 8649779 : homCoord[1] = coords[1];
235 [ + - ]: 8649779 : homCoord[2] = coords[2];
236 [ + - ]: 8649779 : homCoord[3] = coords[3];
237 : : #endif
238 : 8649779 : }
239 : :
240 : 1 : inline void HomCoord::set( const int coords[] )
241 : : {
242 : 1 : homCoord[0] = coords[0];
243 : 1 : homCoord[1] = coords[1];
244 : 1 : homCoord[2] = coords[2];
245 : 1 : homCoord[3] = coords[3];
246 : 1 : }
247 : :
248 : 15 : inline void HomCoord::set( const int ip, const int jp, const int kp, const int hp )
249 : : {
250 : 15 : homCoord[0] = ip;
251 : 15 : homCoord[1] = jp;
252 : 15 : homCoord[2] = kp;
253 : 15 : homCoord[3] = hp;
254 : 15 : }
255 : :
256 : 1416097 : inline HomCoord& HomCoord::operator=( const HomCoord& rhs1 )
257 : : {
258 : 1416097 : homCoord[0] = rhs1.homCoord[0];
259 : 1416097 : homCoord[1] = rhs1.homCoord[1];
260 : 1416097 : homCoord[2] = rhs1.homCoord[2];
261 : 1416097 : homCoord[3] = rhs1.homCoord[3];
262 : 1416097 : return *this;
263 : : }
264 : :
265 : : //! squared length
266 : 172 : inline int HomCoord::length_squared() const
267 : : {
268 : 172 : return homCoord[0] * homCoord[0] + homCoord[1] * homCoord[1] + homCoord[2] * homCoord[2];
269 : : }
270 : :
271 : : //! length
272 : 117 : inline int HomCoord::length() const
273 : : {
274 : 117 : return (int)sqrt( (float)length_squared() );
275 : : }
276 : :
277 : : //! normalize
278 : 116 : inline void HomCoord::normalize()
279 : : {
280 : 116 : *this /= length();
281 : 116 : }
282 : :
283 : : // dot product
284 : 114 : inline int HomCoord::operator%( const HomCoord& rhs ) const
285 : : {
286 : 114 : return homCoord[0] * rhs.homCoord[0] + homCoord[1] * rhs.homCoord[1] + homCoord[2] * rhs.homCoord[2];
287 : : }
288 : :
289 : : // cross product
290 : 52 : inline HomCoord HomCoord::operator*( const HomCoord& rhs ) const
291 : : {
292 : 52 : return HomCoord( homCoord[1] * rhs.homCoord[2] - homCoord[2] * rhs.homCoord[1],
293 : 52 : homCoord[2] * rhs.homCoord[0] - homCoord[0] * rhs.homCoord[2],
294 : 52 : homCoord[0] * rhs.homCoord[1] - homCoord[1] * rhs.homCoord[0] );
295 : : }
296 : :
297 : : inline HomCoord& HomCoord::operator*=( const HomCoord& rhs )
298 : : {
299 : : *this = HomCoord( homCoord[1] * rhs.homCoord[2] - homCoord[2] * rhs.homCoord[1],
300 : : homCoord[2] * rhs.homCoord[0] - homCoord[0] * rhs.homCoord[2],
301 : : homCoord[0] * rhs.homCoord[1] - homCoord[1] * rhs.homCoord[0] );
302 : :
303 : : return *this;
304 : : }
305 : :
306 : 2831580 : inline bool HomCoord::operator==( const HomCoord& rhs1 ) const
307 : : {
308 [ + + ][ + + ]: 2831580 : return ( homCoord[0] == rhs1.homCoord[0] && homCoord[1] == rhs1.homCoord[1] && homCoord[2] == rhs1.homCoord[2] &&
[ + - ][ + - ]
309 : 2831580 : homCoord[3] == rhs1.homCoord[3] );
310 : : }
311 : :
312 : 29 : inline bool HomCoord::operator!=( const HomCoord& rhs1 ) const
313 : : {
314 [ + - ][ + - ]: 29 : return ( homCoord[0] != rhs1.homCoord[0] || homCoord[1] != rhs1.homCoord[1] || homCoord[2] != rhs1.homCoord[2] ||
[ + - ][ - + ]
315 : 29 : homCoord[3] != rhs1.homCoord[3] );
316 : : }
317 : :
318 : 2898472 : inline bool HomCoord::operator>=( const HomCoord& rhs1 ) const
319 : : {
320 [ + + ][ + + ]: 2898472 : return ( homCoord[0] >= rhs1.homCoord[0] && homCoord[1] >= rhs1.homCoord[1] && homCoord[2] >= rhs1.homCoord[2] &&
[ + - ][ + - ]
321 : 2898472 : homCoord[3] == rhs1.homCoord[3] );
322 : : }
323 : :
324 : 2925722 : inline bool HomCoord::operator<=( const HomCoord& rhs1 ) const
325 : : {
326 [ + + ][ + + ]: 2925722 : return ( homCoord[0] <= rhs1.homCoord[0] && homCoord[1] <= rhs1.homCoord[1] && homCoord[2] <= rhs1.homCoord[2] &&
[ + - ][ + - ]
327 : 2925722 : homCoord[3] == rhs1.homCoord[3] );
328 : : }
329 : :
330 : 3 : inline bool HomCoord::operator<( const HomCoord& rhs1 ) const
331 : : {
332 [ + + ][ + - ]: 3 : return ( homCoord[0] < rhs1.homCoord[0] && homCoord[1] < rhs1.homCoord[1] && homCoord[2] < rhs1.homCoord[2] &&
[ + - ][ + - ]
333 : 3 : homCoord[3] == rhs1.homCoord[3] );
334 : : }
335 : :
336 : 3 : inline bool HomCoord::operator>( const HomCoord& rhs1 ) const
337 : : {
338 [ + + ][ + - ]: 3 : return ( homCoord[0] > rhs1.homCoord[0] && homCoord[1] > rhs1.homCoord[1] && homCoord[2] > rhs1.homCoord[2] &&
[ + - ][ + - ]
339 : 3 : homCoord[3] == rhs1.homCoord[3] );
340 : : }
341 : :
342 : 135 : inline HomCoord HomCoord::operator*( const HomXform& rhs2 ) const
343 : : {
344 : : return HomCoord(
345 : : // homCoord[0]*rhs2[4*0+0] + homCoord[1]*rhs2[4*1+0] +
346 : : // homCoord[2]*rhs2[4*2+0] + homCoord[3]*rhs2[4*3+0],
347 : 135 : homCoord[0] * rhs2.xForm[0] + homCoord[1] * rhs2.xForm[4] + homCoord[2] * rhs2.xForm[8] +
348 : 135 : homCoord[3] * rhs2.xForm[12],
349 : :
350 : : // homCoord[0]*rhs2.xForm[4*0+1] + homCoord[1]*rhs2.xForm[4*1+1] +
351 : : // homCoord[2]*rhs2.xForm[4*2+1] + homCoord[3]*rhs2.xForm[4*3+1],
352 : 135 : homCoord[0] * rhs2.xForm[1] + homCoord[1] * rhs2.xForm[5] + homCoord[2] * rhs2.xForm[9] +
353 : 135 : homCoord[3] * rhs2.xForm[13],
354 : :
355 : : // homCoord[0]*rhs2.xForm[4*0+2] + homCoord[1]*rhs2.xForm[4*1+2] +
356 : : // homCoord[2]*rhs2.xForm[4*2+2] + homCoord[3]*rhs2.xForm[4*3+2],
357 : 135 : homCoord[0] * rhs2.xForm[2] + homCoord[1] * rhs2.xForm[6] + homCoord[2] * rhs2.xForm[10] +
358 : 135 : homCoord[3] * rhs2.xForm[14],
359 : :
360 : : // homCoord[0]*rhs2.xForm[4*0+3] + homCoord[1]*rhs2.xForm[4*1+3] +
361 : : // homCoord[2]*rhs2.xForm[4*2+3] + homCoord[3]*rhs2.xForm[4*3+3]
362 : 135 : homCoord[0] * rhs2.xForm[3] + homCoord[1] * rhs2.xForm[7] + homCoord[2] * rhs2.xForm[11] +
363 : 135 : homCoord[3] * rhs2.xForm[15] );
364 : : }
365 : :
366 : 1415703 : inline HomCoord& HomCoord::operator*=( const HomXform& rhs2 )
367 : : {
368 : 1415703 : *this = HomCoord(
369 : : // homCoord[0]*rhs2.xForm[4*0+0] + homCoord[1]*rhs2.xForm[4*1+0] +
370 : : // homCoord[2]*rhs2.xForm[4*2+0] + homCoord[3]*rhs2.xForm[4*3+0],
371 : 1415703 : homCoord[0] * rhs2.xForm[0] + homCoord[1] * rhs2.xForm[4] + homCoord[2] * rhs2.xForm[8] +
372 : 1415703 : homCoord[3] * rhs2.xForm[12],
373 : :
374 : : // homCoord[0]*rhs2.xForm[4*0+1] + homCoord[1]*rhs2.xForm[4*1+1] +
375 : : // homCoord[2]*rhs2.xForm[4*2+1] + homCoord[3]*rhs2.xForm[4*3+1],
376 : 1415703 : homCoord[0] * rhs2.xForm[1] + homCoord[1] * rhs2.xForm[5] + homCoord[2] * rhs2.xForm[9] +
377 : 1415703 : homCoord[3] * rhs2.xForm[13],
378 : :
379 : : // homCoord[0]*rhs2.xForm[4*0+2] + homCoord[1]*rhs2.xForm[4*1+2] +
380 : : // homCoord[2]*rhs2.xForm[4*2+2] + homCoord[3]*rhs2.xForm[4*3+2],
381 : 1415703 : homCoord[0] * rhs2.xForm[2] + homCoord[1] * rhs2.xForm[6] + homCoord[2] * rhs2.xForm[10] +
382 : 1415703 : homCoord[3] * rhs2.xForm[14],
383 : :
384 : : // homCoord[0]*rhs2.xForm[4*0+3] + homCoord[1]*rhs2.xForm[4*1+3] +
385 : : // homCoord[2]*rhs2.xForm[4*2+3] + homCoord[3]*rhs2.xForm[4*3+3]
386 : 1415703 : homCoord[0] * rhs2.xForm[3] + homCoord[1] * rhs2.xForm[7] + homCoord[2] * rhs2.xForm[11] +
387 [ + - ]: 1415703 : homCoord[3] * rhs2.xForm[15] );
388 : 1415703 : return *this;
389 : : }
390 : :
391 : 9 : inline HomCoord HomCoord::operator*( const int mult ) const
392 : : {
393 : 9 : return HomCoord( mult * homCoord[0], mult * homCoord[1], mult * homCoord[2] );
394 : : }
395 : :
396 : : inline HomCoord& HomCoord::operator*=( const int mult )
397 : : {
398 : : homCoord[0] *= mult;
399 : : homCoord[1] *= mult;
400 : : homCoord[2] *= mult;
401 : : return *this;
402 : : }
403 : :
404 : : inline HomCoord HomCoord::operator/( const int div ) const
405 : : {
406 : : return HomCoord( homCoord[0] / div, homCoord[1] / div, homCoord[2] / div );
407 : : }
408 : :
409 : 116 : inline HomCoord& HomCoord::operator/=( const int div )
410 : : {
411 : 116 : homCoord[0] /= div;
412 : 116 : homCoord[1] /= div;
413 : 116 : homCoord[2] /= div;
414 : 116 : return *this;
415 : : }
416 : :
417 : 29354 : inline HomCoord HomCoord::operator-( const HomCoord& rhs2 ) const
418 : : {
419 [ + - ][ + - ]: 29354 : return HomCoord( *this ) -= rhs2;
420 : : }
421 : :
422 : 29354 : inline HomCoord& HomCoord::operator-=( const HomCoord& rhs2 )
423 : : {
424 [ + - ]: 29354 : homCoord[0] -= rhs2[0];
425 [ + - ]: 29354 : homCoord[1] -= rhs2[1];
426 [ + - ]: 29354 : homCoord[2] -= rhs2[2];
427 : 29354 : return *this;
428 : : }
429 : :
430 : 33524 : inline HomCoord HomCoord::operator+( const HomCoord& rhs2 ) const
431 : : {
432 [ + - ][ + - ]: 33524 : return HomCoord( *this ) += rhs2;
433 : : }
434 : :
435 : 33524 : inline HomCoord& HomCoord::operator+=( const HomCoord& rhs2 )
436 : : {
437 [ + - ]: 33524 : homCoord[0] += rhs2[0];
438 [ + - ]: 33524 : homCoord[1] += rhs2[1];
439 [ + - ]: 33524 : homCoord[2] += rhs2[2];
440 : 33524 : return *this;
441 : : }
442 : :
443 : 1415703 : inline HomCoord HomCoord::operator/( const HomXform& rhs2 ) const
444 : : {
445 [ + - ][ + - ]: 1415703 : return HomCoord( *this ) /= rhs2;
446 : : }
447 : :
448 : 1415703 : inline HomCoord& HomCoord::operator/=( const HomXform& rhs2 )
449 : : {
450 [ + - ]: 1415703 : HomXform inv = rhs2.inverse();
451 [ + - ]: 1415703 : *this *= inv;
452 : 1415703 : return *this;
453 : : }
454 : :
455 : 34872196 : inline int HomCoord::operator[]( const int& param ) const
456 : : {
457 : 34872196 : return homCoord[param];
458 : : }
459 : :
460 : 235359 : inline int& HomCoord::operator[]( const int& param )
461 : : {
462 : 235359 : return homCoord[param];
463 : : }
464 : :
465 : 0 : inline std::ostream& operator<<( std::ostream& str, const HomCoord& hc )
466 : : {
467 : 0 : str << "(" << hc.i() << "," << hc.j() << "," << hc.k() << ")";
468 : 0 : return str;
469 : : }
470 : :
471 : 59 : inline HomXform::HomXform( const int matrix[16] )
472 : : {
473 [ + + ]: 1003 : for( int i = 0; i < 16; i++ )
474 : 944 : xForm[i] = matrix[i];
475 : 59 : }
476 : :
477 : 40 : inline HomXform::HomXform() {}
478 : :
479 : 3 : inline HomXform::HomXform( const int rotate[9], const int scale[3], const int translate[3] )
480 : : {
481 : : int i, j;
482 [ + + ]: 12 : for( i = 0; i < 3; i++ )
483 : : {
484 [ + + ]: 36 : for( j = 0; j < 3; j++ )
485 : 27 : xForm[i * 4 + j] = rotate[i * 3 + j] * scale[j];
486 : :
487 : 9 : xForm[12 + i] = translate[i];
488 : : }
489 : 3 : xForm[3] = 0;
490 : 3 : xForm[7] = 0;
491 : 3 : xForm[11] = 0;
492 : 3 : xForm[15] = 1;
493 : 3 : }
494 : :
495 : 1415805 : inline HomXform::HomXform( int i1, int i2, int i3, int i4, int i5, int i6, int i7, int i8, int i9, int i10, int i11,
496 : : int i12, int i13, int i14, int i15, int i16 )
497 : : {
498 : 1415805 : xForm[0] = i1;
499 : 1415805 : xForm[1] = i2;
500 : 1415805 : xForm[2] = i3;
501 : 1415805 : xForm[3] = i4;
502 : 1415805 : xForm[4] = i5;
503 : 1415805 : xForm[5] = i6;
504 : 1415805 : xForm[6] = i7;
505 : 1415805 : xForm[7] = i8;
506 : 1415805 : xForm[8] = i9;
507 : 1415805 : xForm[9] = i10;
508 : 1415805 : xForm[10] = i11;
509 : 1415805 : xForm[11] = i12;
510 : 1415805 : xForm[12] = i13;
511 : 1415805 : xForm[13] = i14;
512 : 1415805 : xForm[14] = i15;
513 : 1415805 : xForm[15] = i16;
514 : 1415805 : }
515 : :
516 : 62 : inline HomXform& HomXform::operator=( const HomXform& rhs )
517 : : {
518 [ + + ]: 1054 : for( int i = 0; i < 16; i++ )
519 : 992 : xForm[i] = rhs.xForm[i];
520 : :
521 : 62 : return *this;
522 : : }
523 : :
524 : 2 : inline HomXform HomXform::operator*( const HomXform& rhs2 ) const
525 : : {
526 : : return HomXform(
527 : : // temp.XFORM(0,0)
528 : 2 : XFORM( 0, 0 ) * rhs2.XFORM( 0, 0 ) + XFORM( 0, 1 ) * rhs2.XFORM( 1, 0 ) + XFORM( 0, 2 ) * rhs2.XFORM( 2, 0 ) +
529 : 2 : XFORM( 0, 3 ) * rhs2.XFORM( 3, 0 ),
530 : : // temp.XFORM(0,1)
531 : 2 : XFORM( 0, 0 ) * rhs2.XFORM( 0, 1 ) + XFORM( 0, 1 ) * rhs2.XFORM( 1, 1 ) + XFORM( 0, 2 ) * rhs2.XFORM( 2, 1 ) +
532 : 2 : XFORM( 0, 3 ) * rhs2.XFORM( 3, 1 ),
533 : : // temp.XFORM(0,2)
534 : 2 : XFORM( 0, 0 ) * rhs2.XFORM( 0, 2 ) + XFORM( 0, 1 ) * rhs2.XFORM( 1, 2 ) + XFORM( 0, 2 ) * rhs2.XFORM( 2, 2 ) +
535 : 2 : XFORM( 0, 3 ) * rhs2.XFORM( 3, 2 ),
536 : : // temp.XFORM(0,3)
537 : 2 : XFORM( 0, 0 ) * rhs2.XFORM( 0, 3 ) + XFORM( 0, 1 ) * rhs2.XFORM( 1, 3 ) + XFORM( 0, 2 ) * rhs2.XFORM( 2, 3 ) +
538 : 2 : XFORM( 0, 3 ) * rhs2.XFORM( 3, 3 ),
539 : :
540 : : // temp.XFORM(1,0)
541 : 2 : XFORM( 1, 0 ) * rhs2.XFORM( 0, 0 ) + XFORM( 1, 1 ) * rhs2.XFORM( 1, 0 ) + XFORM( 1, 2 ) * rhs2.XFORM( 2, 0 ) +
542 : 2 : XFORM( 1, 3 ) * rhs2.XFORM( 3, 0 ),
543 : : // temp.XFORM(1,1)
544 : 2 : XFORM( 1, 0 ) * rhs2.XFORM( 0, 1 ) + XFORM( 1, 1 ) * rhs2.XFORM( 1, 1 ) + XFORM( 1, 2 ) * rhs2.XFORM( 2, 1 ) +
545 : 2 : XFORM( 1, 3 ) * rhs2.XFORM( 3, 1 ),
546 : : // temp.XFORM(1,2)
547 : 2 : XFORM( 1, 0 ) * rhs2.XFORM( 0, 2 ) + XFORM( 1, 1 ) * rhs2.XFORM( 1, 2 ) + XFORM( 1, 2 ) * rhs2.XFORM( 2, 2 ) +
548 : 2 : XFORM( 1, 3 ) * rhs2.XFORM( 3, 2 ),
549 : : // temp.XFORM(1,3)
550 : 2 : XFORM( 1, 0 ) * rhs2.XFORM( 0, 3 ) + XFORM( 1, 1 ) * rhs2.XFORM( 1, 3 ) + XFORM( 1, 2 ) * rhs2.XFORM( 2, 3 ) +
551 : 2 : XFORM( 1, 3 ) * rhs2.XFORM( 3, 3 ),
552 : :
553 : : // temp.XFORM(2,0)
554 : 2 : XFORM( 2, 0 ) * rhs2.XFORM( 0, 0 ) + XFORM( 2, 1 ) * rhs2.XFORM( 1, 0 ) + XFORM( 2, 2 ) * rhs2.XFORM( 2, 0 ) +
555 : 2 : XFORM( 2, 3 ) * rhs2.XFORM( 3, 0 ),
556 : : // temp.XFORM(2,1)
557 : 2 : XFORM( 2, 0 ) * rhs2.XFORM( 0, 1 ) + XFORM( 2, 1 ) * rhs2.XFORM( 1, 1 ) + XFORM( 2, 2 ) * rhs2.XFORM( 2, 1 ) +
558 : 2 : XFORM( 2, 3 ) * rhs2.XFORM( 3, 1 ),
559 : : // temp.XFORM(2,2)
560 : 2 : XFORM( 2, 0 ) * rhs2.XFORM( 0, 2 ) + XFORM( 2, 1 ) * rhs2.XFORM( 1, 2 ) + XFORM( 2, 2 ) * rhs2.XFORM( 2, 2 ) +
561 : 2 : XFORM( 2, 3 ) * rhs2.XFORM( 3, 2 ),
562 : : // temp.XFORM(2,3)
563 : 2 : XFORM( 2, 0 ) * rhs2.XFORM( 0, 3 ) + XFORM( 2, 1 ) * rhs2.XFORM( 1, 3 ) + XFORM( 2, 2 ) * rhs2.XFORM( 2, 3 ) +
564 : 2 : XFORM( 2, 3 ) * rhs2.XFORM( 3, 3 ),
565 : :
566 : : // temp.XFORM(3,0)
567 : : // xForm[12]*rhs2.xForm[0] + xForm[13]*rhs2.xForm[4] + xForm[14]*rhs2.xForm[8] +
568 : : // xForm[15]*rhs2.xForm[12]
569 : 2 : XFORM( 3, 0 ) * rhs2.XFORM( 0, 0 ) + XFORM( 3, 1 ) * rhs2.XFORM( 1, 0 ) + XFORM( 3, 2 ) * rhs2.XFORM( 2, 0 ) +
570 : 2 : XFORM( 3, 3 ) * rhs2.XFORM( 3, 0 ),
571 : : // temp.XFORM(3,1)
572 : : // xForm[12]*rhs2.xForm[1] + xForm[13]*rhs2.xForm[5] + xForm[14]*rhs2.xForm[9] +
573 : : // xForm[15]*rhs2.xForm[13]
574 : 2 : XFORM( 3, 0 ) * rhs2.XFORM( 0, 1 ) + XFORM( 3, 1 ) * rhs2.XFORM( 1, 1 ) + XFORM( 3, 2 ) * rhs2.XFORM( 2, 1 ) +
575 : 2 : XFORM( 3, 3 ) * rhs2.XFORM( 3, 1 ),
576 : : // temp.XFORM(3,2)
577 : : // xForm[12]*rhs2.xForm[2] + xForm[13]*rhs2.xForm[6] + xForm[14]*rhs2.xForm[10] +
578 : : // xForm[15]*rhs2.xForm[14]
579 : 2 : XFORM( 3, 0 ) * rhs2.XFORM( 0, 2 ) + XFORM( 3, 1 ) * rhs2.XFORM( 1, 2 ) + XFORM( 3, 2 ) * rhs2.XFORM( 2, 2 ) +
580 : 2 : XFORM( 3, 3 ) * rhs2.XFORM( 3, 2 ),
581 : : // temp.XFORM(3,3)
582 : : // xForm[12]*rhs2.xForm[3] + xForm[13]*rhs2.xForm[7] + xForm[14]*rhs2.xForm[11] +
583 : : // xForm[15]*rhs2.xForm[15]
584 : 2 : XFORM( 3, 0 ) * rhs2.XFORM( 0, 3 ) + XFORM( 3, 1 ) * rhs2.XFORM( 1, 3 ) + XFORM( 3, 2 ) * rhs2.XFORM( 2, 3 ) +
585 : 2 : XFORM( 3, 3 ) * rhs2.XFORM( 3, 3 ) );
586 : : }
587 : :
588 : 20 : inline HomXform& HomXform::operator*=( const HomXform& rhs2 )
589 : : {
590 : 20 : *this = HomXform(
591 : : // temp.XFORM(0,0)
592 : 20 : XFORM( 0, 0 ) * rhs2.XFORM( 0, 0 ) + XFORM( 0, 1 ) * rhs2.XFORM( 1, 0 ) + XFORM( 0, 2 ) * rhs2.XFORM( 2, 0 ) +
593 : 20 : XFORM( 0, 3 ) * rhs2.XFORM( 3, 0 ),
594 : : // temp.XFORM(0,1)
595 : 20 : XFORM( 0, 0 ) * rhs2.XFORM( 0, 1 ) + XFORM( 0, 1 ) * rhs2.XFORM( 1, 1 ) + XFORM( 0, 2 ) * rhs2.XFORM( 2, 1 ) +
596 : 20 : XFORM( 0, 3 ) * rhs2.XFORM( 3, 1 ),
597 : : // temp.XFORM(0,2)
598 : 20 : XFORM( 0, 0 ) * rhs2.XFORM( 0, 2 ) + XFORM( 0, 1 ) * rhs2.XFORM( 1, 2 ) + XFORM( 0, 2 ) * rhs2.XFORM( 2, 2 ) +
599 : 20 : XFORM( 0, 3 ) * rhs2.XFORM( 3, 2 ),
600 : : // temp.XFORM(0,3)
601 : 20 : XFORM( 0, 0 ) * rhs2.XFORM( 0, 3 ) + XFORM( 0, 1 ) * rhs2.XFORM( 1, 3 ) + XFORM( 0, 2 ) * rhs2.XFORM( 2, 3 ) +
602 : 20 : XFORM( 0, 3 ) * rhs2.XFORM( 3, 3 ),
603 : :
604 : : // temp.XFORM(1,0)
605 : 20 : XFORM( 1, 0 ) * rhs2.XFORM( 0, 0 ) + XFORM( 1, 1 ) * rhs2.XFORM( 1, 0 ) + XFORM( 1, 2 ) * rhs2.XFORM( 2, 0 ) +
606 : 20 : XFORM( 1, 3 ) * rhs2.XFORM( 3, 0 ),
607 : : // temp.XFORM(1,1)
608 : 20 : XFORM( 1, 0 ) * rhs2.XFORM( 0, 1 ) + XFORM( 1, 1 ) * rhs2.XFORM( 1, 1 ) + XFORM( 1, 2 ) * rhs2.XFORM( 2, 1 ) +
609 : 20 : XFORM( 1, 3 ) * rhs2.XFORM( 3, 1 ),
610 : : // temp.XFORM(1,2)
611 : 20 : XFORM( 1, 0 ) * rhs2.XFORM( 0, 2 ) + XFORM( 1, 1 ) * rhs2.XFORM( 1, 2 ) + XFORM( 1, 2 ) * rhs2.XFORM( 2, 2 ) +
612 : 20 : XFORM( 1, 3 ) * rhs2.XFORM( 3, 2 ),
613 : : // temp.XFORM(1,3)
614 : 20 : XFORM( 1, 0 ) * rhs2.XFORM( 0, 3 ) + XFORM( 1, 1 ) * rhs2.XFORM( 1, 3 ) + XFORM( 1, 2 ) * rhs2.XFORM( 2, 3 ) +
615 : 20 : XFORM( 1, 3 ) * rhs2.XFORM( 3, 3 ),
616 : :
617 : : // temp.XFORM(2,0)
618 : 20 : XFORM( 2, 0 ) * rhs2.XFORM( 0, 0 ) + XFORM( 2, 1 ) * rhs2.XFORM( 1, 0 ) + XFORM( 2, 2 ) * rhs2.XFORM( 2, 0 ) +
619 : 20 : XFORM( 2, 3 ) * rhs2.XFORM( 3, 0 ),
620 : : // temp.XFORM(2,1)
621 : 20 : XFORM( 2, 0 ) * rhs2.XFORM( 0, 1 ) + XFORM( 2, 1 ) * rhs2.XFORM( 1, 1 ) + XFORM( 2, 2 ) * rhs2.XFORM( 2, 1 ) +
622 : 20 : XFORM( 2, 3 ) * rhs2.XFORM( 3, 1 ),
623 : : // temp.XFORM(2,2)
624 : 20 : XFORM( 2, 0 ) * rhs2.XFORM( 0, 2 ) + XFORM( 2, 1 ) * rhs2.XFORM( 1, 2 ) + XFORM( 2, 2 ) * rhs2.XFORM( 2, 2 ) +
625 : 20 : XFORM( 2, 3 ) * rhs2.XFORM( 3, 2 ),
626 : : // temp.XFORM(2,3)
627 : 20 : XFORM( 2, 0 ) * rhs2.XFORM( 0, 3 ) + XFORM( 2, 1 ) * rhs2.XFORM( 1, 3 ) + XFORM( 2, 2 ) * rhs2.XFORM( 2, 3 ) +
628 : 20 : XFORM( 2, 3 ) * rhs2.XFORM( 3, 3 ),
629 : :
630 : : // temp.XFORM(3,0)
631 : 20 : XFORM( 3, 0 ) * rhs2.XFORM( 0, 0 ) + XFORM( 3, 1 ) * rhs2.XFORM( 1, 0 ) + XFORM( 3, 2 ) * rhs2.XFORM( 2, 0 ) +
632 : 20 : XFORM( 3, 3 ) * rhs2.XFORM( 3, 0 ),
633 : : // temp.XFORM(3,1)
634 : 20 : XFORM( 3, 0 ) * rhs2.XFORM( 0, 1 ) + XFORM( 3, 1 ) * rhs2.XFORM( 1, 1 ) + XFORM( 3, 2 ) * rhs2.XFORM( 2, 1 ) +
635 : 20 : XFORM( 3, 3 ) * rhs2.XFORM( 3, 1 ),
636 : : // temp.XFORM(3,2)
637 : 20 : XFORM( 3, 0 ) * rhs2.XFORM( 0, 2 ) + XFORM( 3, 1 ) * rhs2.XFORM( 1, 2 ) + XFORM( 3, 2 ) * rhs2.XFORM( 2, 2 ) +
638 : 20 : XFORM( 3, 3 ) * rhs2.XFORM( 3, 2 ),
639 : : // temp.XFORM(3,3)
640 : 20 : XFORM( 3, 0 ) * rhs2.XFORM( 0, 3 ) + XFORM( 3, 1 ) * rhs2.XFORM( 1, 3 ) + XFORM( 3, 2 ) * rhs2.XFORM( 2, 3 ) +
641 [ + - ]: 20 : XFORM( 3, 3 ) * rhs2.XFORM( 3, 3 ) );
642 : :
643 : 20 : return *this;
644 : : }
645 : :
646 : : inline int HomXform::operator[]( const int& count ) const
647 : : {
648 : : return xForm[count];
649 : : }
650 : :
651 : 1 : inline int& HomXform::operator[]( const int& count )
652 : : {
653 : 1 : return xForm[count];
654 : : }
655 : :
656 : 2 : inline bool HomXform::operator==( const HomXform& rhs ) const
657 : : {
658 [ + - ][ + - ]: 2 : return ( xForm[0] == rhs.xForm[0] && xForm[1] == rhs.xForm[1] && xForm[2] == rhs.xForm[2] &&
[ + - ]
659 [ + - ][ + - ]: 2 : xForm[3] == rhs.xForm[3] && xForm[4] == rhs.xForm[4] && xForm[5] == rhs.xForm[5] &&
[ + - ]
660 [ + - ][ + - ]: 2 : xForm[6] == rhs.xForm[6] && xForm[7] == rhs.xForm[7] && xForm[8] == rhs.xForm[8] &&
[ + - ]
661 [ + - ][ + - ]: 2 : xForm[9] == rhs.xForm[9] && xForm[10] == rhs.xForm[10] && xForm[11] == rhs.xForm[11] &&
[ + - ]
662 [ + - ][ + - ]: 4 : xForm[12] == rhs.xForm[12] && xForm[13] == rhs.xForm[13] && xForm[14] == rhs.xForm[14] &&
[ + - ][ + - ]
663 : 2 : xForm[15] == rhs.xForm[15] );
664 : : }
665 : :
666 : 6 : inline bool HomXform::operator!=( const HomXform& rhs ) const
667 : : {
668 [ + + ][ + - ]: 6 : return ( xForm[0] != rhs.xForm[0] || xForm[1] != rhs.xForm[1] || xForm[2] != rhs.xForm[2] ||
[ + - ]
669 [ + - ][ + - ]: 5 : xForm[3] != rhs.xForm[3] || xForm[4] != rhs.xForm[4] || xForm[5] != rhs.xForm[5] ||
[ + - ]
670 [ + - ][ + - ]: 5 : xForm[6] != rhs.xForm[6] || xForm[7] != rhs.xForm[7] || xForm[8] != rhs.xForm[8] ||
[ + - ]
671 [ + - ][ + - ]: 5 : xForm[9] != rhs.xForm[9] || xForm[10] != rhs.xForm[10] || xForm[11] != rhs.xForm[11] ||
[ + - ]
672 [ + - ][ + - ]: 12 : xForm[12] != rhs.xForm[12] || xForm[13] != rhs.xForm[13] || xForm[14] != rhs.xForm[14] ||
[ + - ][ - + ]
673 : 6 : xForm[15] != rhs.xForm[15] );
674 : : }
675 : :
676 : 1415743 : inline HomXform HomXform::inverse() const
677 : : {
678 : :
679 : : /*
680 : : // original code:
681 : :
682 : : HomXform tmp;
683 : :
684 : : // assign the diagonal
685 : : tmp[0] = xForm[0];
686 : : tmp[5] = xForm[5];
687 : : tmp[10] = xForm[10];
688 : : tmp[15] = xForm[15];
689 : :
690 : : // invert the rotation matrix
691 : : tmp[XFORM_INDEX(0,1)] = XFORM(1,0);
692 : : tmp[XFORM_INDEX(0,2)] = XFORM(2,0);
693 : : tmp[XFORM_INDEX(1,0)] = XFORM(0,1);
694 : : tmp[XFORM_INDEX(1,2)] = XFORM(2,1);
695 : : tmp[XFORM_INDEX(2,0)] = XFORM(0,2);
696 : : tmp[XFORM_INDEX(2,1)] = XFORM(1,2);
697 : :
698 : : // negative translate * Rinv
699 : : tmp[XFORM_INDEX(3,0)] = -(XFORM(3,0)*tmp.XFORM(0,0) + XFORM(3,1)*tmp.XFORM(1,0) +
700 : : XFORM(3,2)*tmp.XFORM(2,0)); tmp[XFORM_INDEX(3,1)] = -(XFORM(3,0)*tmp.XFORM(0,1) +
701 : : XFORM(3,1)*tmp.XFORM(1,1) + XFORM(3,2)*tmp.XFORM(2,1)); tmp[XFORM_INDEX(3,2)] =
702 : : -(XFORM(3,0)*tmp.XFORM(0,2) + XFORM(3,1)*tmp.XFORM(1,2) + XFORM(3,2)*tmp.XFORM(2,2));
703 : :
704 : : // zero last column
705 : : tmp[XFORM_INDEX(0,3)] = 0;
706 : : tmp[XFORM_INDEX(1,3)] = 0;
707 : : tmp[XFORM_INDEX(2,3)] = 0;
708 : :
709 : : // h factor
710 : : tmp[XFORM_INDEX(3,3)] = 1;
711 : :
712 : : return tmp;
713 : : */
714 : :
715 : : // more efficient, but somewhat confusing (remember, column-major):
716 : :
717 : : return HomXform(
718 : : // row 0
719 : 1415743 : xForm[0], XFORM( 1, 0 ), XFORM( 2, 0 ), 0,
720 : : // row 1
721 : 1415743 : XFORM( 0, 1 ), xForm[5], XFORM( 2, 1 ), 0,
722 : : // row 2
723 : 1415743 : XFORM( 0, 2 ), XFORM( 1, 2 ), xForm[10], 0,
724 : : // row 3
725 : 1415743 : -( XFORM( 3, 0 ) * xForm[0] + XFORM( 3, 1 ) * XFORM( 0, 1 ) + XFORM( 3, 2 ) * XFORM( 0, 2 ) ),
726 : 1415743 : -( XFORM( 3, 0 ) * XFORM( 1, 0 ) + XFORM( 3, 1 ) * xForm[5] + XFORM( 3, 2 ) * XFORM( 1, 2 ) ),
727 : 1415743 : -( XFORM( 3, 0 ) * XFORM( 2, 0 ) + XFORM( 3, 1 ) * XFORM( 2, 1 ) + XFORM( 3, 2 ) * xForm[10] ), 1 );
728 : : }
729 : :
730 : : } // namespace moab
731 : :
732 : : #endif
|
