Branch data Line data Source code
1 : : // Class: CubitTransformMatrix
2 : : //
3 : : // Description: A 4-Dimensional Matrix. Essentially the same as
4 : : // a generic CubitMatrix, except that it has some
5 : : // extra 3D transformation functions.
6 : : //
7 : : // All transformations are pre-multiplications,
8 : : // meaning that M*V will transform a point V
9 : : // in the same order transformations are applied to M.
10 : : //
11 : : // Owner: Darryl Melander
12 : :
13 : : #include "CubitTransformMatrix.hpp"
14 : : #include "CubitMessage.hpp"
15 : :
16 : 884 : CubitTransformMatrix::CubitTransformMatrix()
17 : 884 : : CubitMatrix(4)
18 : : {
19 : : // Just creates a 4x4 matrix set to identity
20 : 884 : }
21 : :
22 : 411 : CubitTransformMatrix::CubitTransformMatrix(const CubitTransformMatrix& from)
23 : 411 : : CubitMatrix(from)
24 : : {
25 : : // Just copies it
26 : 411 : }
27 : :
28 : :
29 : 2392 : CubitTransformMatrix::~CubitTransformMatrix()
30 : : {
31 : : // Just deletes it
32 [ - + ]: 1196 : }
33 : :
34 : :
35 : :
36 : 433 : CubitTransformMatrix& CubitTransformMatrix::translate(const CubitVector& v)
37 : : {
38 : 433 : return translate(v.x(), v.y(), v.z());
39 : : }
40 : :
41 : :
42 : 455 : CubitTransformMatrix& CubitTransformMatrix::translate (double x, double y, double z)
43 : : {
44 : 455 : set (0, 3, get(0, 3) + x);
45 : 455 : set (1, 3, get(1, 3) + y);
46 : 455 : set (2, 3, get(2, 3) + z);
47 : :
48 : 455 : return *this;
49 : : }
50 : :
51 : 11 : CubitTransformMatrix& CubitTransformMatrix::rotate(double degrees,
52 : : const CubitVector& vector)
53 : : {
54 : : double angle, ct, st;
55 : : // Make a copy of vector so we don't have to change it
56 [ + - ]: 11 : CubitVector axis = vector;
57 : :
58 : : // Convert degrees to radians
59 : 11 : angle = -degrees * CUBIT_PI/180.0;
60 : :
61 : : // Take sin and cos
62 : 11 : ct = cos(angle);
63 : 11 : st = sin(angle);
64 : :
65 : : // Normalize the axis vector
66 [ + - ]: 11 : axis.normalize();
67 : :
68 : : // Make an identity matrix
69 [ + - ]: 11 : CubitTransformMatrix mat;
70 : :
71 : : // Setup some calculations that occur repeatedly
72 : 11 : double one_minus_cos = 1.0 - ct;
73 [ + - ]: 11 : double dx_ct = axis.x() * one_minus_cos;
74 [ + - ]: 11 : double dy_ct = axis.y() * one_minus_cos;
75 [ + - ]: 11 : double dz_ct = axis.z() * one_minus_cos;
76 [ + - ]: 11 : double dx_st = axis.x() * st;
77 [ + - ]: 11 : double dy_st = axis.y() * st;
78 [ + - ]: 11 : double dz_st = axis.z() * st;
79 : :
80 : : // Set the values in the matrix
81 [ + - ][ + - ]: 11 : mat.set(0, 0, ct + axis.x() * dx_ct);
82 [ + - ][ + - ]: 11 : mat.set(1, 0, -dz_st + axis.x() * dy_ct);
83 [ + - ][ + - ]: 11 : mat.set(2, 0, dy_st + axis.x() * dz_ct);
84 : :
85 [ + - ][ + - ]: 11 : mat.set(0, 1, dz_st + axis.y() * dx_ct);
86 [ + - ][ + - ]: 11 : mat.set(1, 1, ct + axis.y() * dy_ct);
87 [ + - ][ + - ]: 11 : mat.set(2, 1, -dx_st + axis.y() * dz_ct);
88 : :
89 [ + - ][ + - ]: 11 : mat.set(0, 2, -dy_st + axis.z() * dx_ct);
90 [ + - ][ + - ]: 11 : mat.set(1, 2, dx_st + axis.z() * dy_ct);
91 [ + - ][ + - ]: 11 : mat.set(2, 2, ct + axis.z() * dz_ct);
92 : :
93 : : // Premultiply the matrix
94 [ + - ][ + - ]: 11 : *this = mat * *this;
[ + - ]
95 : : // Return
96 [ + - ]: 11 : return *this;
97 : : }
98 : :
99 : 0 : CubitTransformMatrix& CubitTransformMatrix::rotate(double degrees, char axis)
100 : : {
101 [ # # ][ # # ]: 0 : if(axis != 'x' && axis != 'y' && axis != 'z')
[ # # ]
102 [ # # ][ # # ]: 0 : throw std::invalid_argument("Invalid Axis: must be X, Y, or Z");
103 : : //assert (axis == 'x' || axis == 'y' || axis == 'z');
104 : :
105 : : // Convert to Radians, Get the sine and cosine
106 : 0 : double angle = degrees * CUBIT_PI/180.;
107 : : double s, c;
108 : 0 : s = sin(angle);
109 : 0 : c = cos(angle);
110 : :
111 : : // Make an Identity matrix
112 [ # # ]: 0 : CubitTransformMatrix mat;
113 : : // Place values in appropriate places
114 [ # # # # ]: 0 : switch (axis)
115 : : {
116 : : case 'x':
117 [ # # ]: 0 : mat.set(1, 1, c);
118 [ # # ]: 0 : mat.set(2, 2, c);
119 [ # # ]: 0 : mat.set(1, 2, -s);
120 [ # # ]: 0 : mat.set(2, 1, s);
121 : 0 : break;
122 : : case 'y':
123 [ # # ]: 0 : mat.set(0, 0, c);
124 [ # # ]: 0 : mat.set(0, 2, s);
125 [ # # ]: 0 : mat.set(2, 0, -s);
126 [ # # ]: 0 : mat.set(2, 2, c);
127 : 0 : break;
128 : : case 'z':
129 [ # # ]: 0 : mat.set(0, 0, c);
130 [ # # ]: 0 : mat.set(1, 0, s);
131 [ # # ]: 0 : mat.set(0, 1, -s);
132 [ # # ]: 0 : mat.set(1, 1, c);
133 : 0 : break;
134 : : }
135 : :
136 : : // Perform Pre-Multiplication
137 [ # # ][ # # ]: 0 : *this = mat * *this;
[ # # ]
138 : :
139 [ # # ]: 0 : return *this;
140 : : }
141 : :
142 : 22 : CubitTransformMatrix& CubitTransformMatrix::reflect(const CubitVector& vector)
143 : : {
144 : : double a, b, c, d;
145 [ + - ]: 22 : CubitVector axis = vector;
146 [ + - ]: 22 : axis.normalize();
147 : :
148 [ + - ]: 22 : a = axis.x();
149 [ + - ]: 22 : b = axis.y();
150 [ + - ]: 22 : c = axis.z();
151 : :
152 : 22 : d = sqrt(b*b + c*c);
153 : :
154 : : // Make an Identity matrix
155 [ + - ]: 22 : CubitTransformMatrix mat;
156 [ + - ]: 22 : if(d)
157 : : {
158 : : // Place values in appropriate places for negative rotate about x
159 [ + - ]: 22 : mat.set(1, 1, c/d);
160 [ + - ]: 22 : mat.set(1, 2, -b/d);
161 [ + - ]: 22 : mat.set(2, 1, b/d);
162 [ + - ]: 22 : mat.set(2, 2, c/d);
163 : :
164 : : // Perform Pre-Multiplication
165 [ + - ][ + - ]: 22 : *this = mat * *this;
[ + - ]
166 [ + - ]: 22 : mat.set_to_identity();
167 : : }
168 : :
169 : : // Place values in appropriate places for negative rotate about y
170 [ + - ]: 22 : mat.set(0, 0, d);
171 [ + - ]: 22 : mat.set(0, 2, -a);
172 [ + - ]: 22 : mat.set(2, 0, a);
173 [ + - ]: 22 : mat.set(2, 2, d);
174 : :
175 : :
176 : : // Perform Pre-Multiplication
177 [ + - ][ + - ]: 22 : *this = mat * *this;
[ + - ]
178 [ + - ]: 22 : mat.set_to_identity();
179 : :
180 : : // Place values in appropriate places for reflect across z
181 [ + - ]: 22 : mat.set(2, 2, -1);
182 : :
183 : :
184 : : // Perform Pre-Multiplication
185 [ + - ][ + - ]: 22 : *this = mat * *this;
[ + - ]
186 [ + - ]: 22 : mat.set_to_identity();
187 : :
188 : : // Place values in appropriate places for rotate about y
189 [ + - ]: 22 : mat.set(0, 0, d);
190 [ + - ]: 22 : mat.set(0, 2, a);
191 [ + - ]: 22 : mat.set(2, 0, -a);
192 [ + - ]: 22 : mat.set(2, 2, d);
193 : :
194 : :
195 : : // Perform Pre-Multiplication
196 [ + - ][ + - ]: 22 : *this = mat * *this;
[ + - ]
197 [ + - ]: 22 : mat.set_to_identity();
198 : :
199 [ + - ]: 22 : if(d)
200 : : {
201 : : // Place values in appropriate places for rotate about x
202 [ + - ]: 22 : mat.set(1, 1, c/d);
203 [ + - ]: 22 : mat.set(1, 2, b/d);
204 [ + - ]: 22 : mat.set(2, 1, -b/d);
205 [ + - ]: 22 : mat.set(2, 2, c/d);
206 : :
207 : : // Perform Pre-Multiplication
208 [ + - ][ + - ]: 22 : *this = mat * *this;
[ + - ]
209 [ + - ]: 22 : mat.set_to_identity();
210 : : }
211 : :
212 [ + - ]: 22 : return *this;
213 : :
214 : : }
215 : :
216 : 0 : CubitTransformMatrix& CubitTransformMatrix::rotate(double degrees,
217 : : const CubitVector& axis_from,
218 : : const CubitVector& axis_to)
219 : : {
220 : : // Translate so that axis_from is at origin
221 [ # # ]: 0 : translate (-axis_from);
222 : :
223 : : // Rotate about specified axis
224 [ # # ]: 0 : rotate (degrees, axis_to - axis_from);
225 : :
226 : : // Translate back
227 : 0 : translate (axis_from);
228 : :
229 : 0 : return *this;
230 : : }
231 : :
232 : 11 : CubitTransformMatrix& CubitTransformMatrix::scale_about_origin (const CubitVector& scale)
233 : : {
234 : 11 : return scale_about_origin(scale.x(), scale.y(), scale.z());
235 : : }
236 : :
237 : 11 : CubitTransformMatrix& CubitTransformMatrix::scale_about_origin (double x, double y, double z)
238 : : {
239 [ + - ]: 11 : CubitTransformMatrix mat;
240 [ + - ]: 11 : mat.set(0, 0, x);
241 [ + - ]: 11 : mat.set(1, 1, y);
242 [ + - ]: 11 : mat.set(2, 2, z);
243 : :
244 : : // Perform Pre-Multiplication
245 [ + - ][ + - ]: 11 : *this = mat * *this;
[ + - ]
246 : :
247 [ + - ]: 11 : return *this;
248 : : }
249 : :
250 : 0 : CubitTransformMatrix& CubitTransformMatrix::scale_about_origin (double scale)
251 : : {
252 : 0 : return scale_about_origin(scale, scale, scale);
253 : : }
254 : :
255 : 0 : CubitTransformMatrix& CubitTransformMatrix::inverse()
256 : : {
257 [ # # ]: 0 : CubitMatrix matrix = *this;
258 [ # # ][ # # ]: 0 : matrix = matrix.inverse();
[ # # ][ # # ]
259 [ # # ]: 0 : for( int ii = 0; ii < 4; ii++ )
260 : : {
261 [ # # ]: 0 : for( int jj = 0; jj < 4; jj++ )
262 : : {
263 [ # # ][ # # ]: 0 : set( ii, jj, matrix.get(ii,jj) );
264 : : }
265 : : }
266 [ # # ]: 0 : return *this;
267 : : }
268 : :
269 : :
270 : : // Post-multiplication of a point (M*V)
271 : 154 : CubitVector CubitTransformMatrix::operator* (const CubitVector& point) const
272 : : {
273 : : // Make a sub-matrix, multiply the point by it.
274 [ + - ]: 154 : CubitVector vec = (this->sub_matrix(3, 3))*point;
275 : : // Handle the fourth column here
276 : 154 : vec.x(vec.x() + get(0, 3));
277 : 154 : vec.y(vec.y() + get(1, 3));
278 : 154 : vec.z(vec.z() + get(2, 3));
279 : :
280 : 154 : return vec;
281 : : }
282 : :
283 : :
284 : : // point * matrix
285 : : CUBIT_UTIL_EXPORT
286 : 0 : CubitVector operator* (const CubitVector& point,
287 : : const CubitTransformMatrix& matrix)
288 : : {
289 : : // Make a 1x4 matrix, multiply matrix by it.
290 [ # # ]: 0 : CubitMatrix m1(1,4);
291 [ # # ][ # # ]: 0 : m1.set(0, 0, point.x());
292 [ # # ][ # # ]: 0 : m1.set(0, 1, point.y());
293 [ # # ][ # # ]: 0 : m1.set(0, 2, point.z());
294 [ # # ]: 0 : m1.set(0, 3, 1);
295 : :
296 : : // Perform the multiplication
297 [ # # ][ # # ]: 0 : m1 = m1 * matrix;
[ # # ][ # # ]
298 : : // The result is a 1x4
299 : :
300 : : // Put the results into a vector (dividing by w), and return
301 [ # # ]: 0 : double w = m1.get(0,3);
302 [ # # ][ # # ]: 0 : return CubitVector(m1.get(0,0)/w, m1.get(0,1)/w, m1.get(0,2)/w);
[ # # ][ # # ]
[ # # ]
303 : : }
304 : :
305 : 154 : CubitTransformMatrix CubitTransformMatrix::operator*(
306 : : const CubitTransformMatrix& matrix) const
307 : : {
308 [ + - ]: 154 : CubitTransformMatrix rv;
309 [ + - ][ + - ]: 308 : CubitMatrix temp(4);
310 [ + - ][ + - ]: 154 : temp = CubitMatrix::operator*(matrix);
[ + - ][ + - ]
311 [ + + ]: 770 : for (int i = 0; i < 4; i++)
312 [ + + ]: 3080 : for (int j = 0; j < 4; j++)
313 [ + - ][ + - ]: 2464 : rv.set(i, j, temp.get(i,j));
314 : 308 : return rv;
315 : : }
316 : :
317 : 0 : CubitMatrix CubitTransformMatrix::operator*(const CubitMatrix& matrix) const
318 : : {
319 : 0 : return CubitMatrix::operator*(matrix);
320 : : }
321 : :
322 : 0 : CubitTransformMatrix CubitTransformMatrix::operator*(double val) const
323 : : {
324 : 0 : CubitTransformMatrix rv;
325 [ # # ]: 0 : for (int i = 0; i < 4; i++)
326 [ # # ]: 0 : for (int j = 0; j < 4; j++)
327 [ # # ][ # # ]: 0 : rv.set(i, j, this->get(i,j)*val);
328 : 0 : return rv;
329 : : }
330 : :
331 : 0 : void CubitTransformMatrix::print_me() const
332 : : {
333 [ # # ]: 0 : PRINT_INFO("%8.4f %8.4f %8.4f %8.4f\n",
334 [ # # ]: 0 : get(0,0), get(0,1), get(0,2), get(0,3));
335 [ # # ]: 0 : PRINT_INFO("%8.4f %8.4f %8.4f %8.4f\n",
336 [ # # ]: 0 : get(1,0), get(1,1), get(1,2), get(1,3));
337 [ # # ]: 0 : PRINT_INFO("%8.4f %8.4f %8.4f %8.4f\n",
338 [ # # ]: 0 : get(2,0), get(2,1), get(2,2), get(2,3));
339 [ # # ]: 0 : PRINT_INFO("%8.4f %8.4f %8.4f %8.4f\n",
340 [ # # ]: 0 : get(3,0), get(3,1), get(3,2), get(3,3));
341 : 0 : }
342 : :
343 : : //! return the origin of this system
344 : 0 : CubitVector CubitTransformMatrix::origin() const
345 : : {
346 : 0 : return CubitVector(this->get(0,3), this->get(1,3), this->get(2,3));
347 : : }
348 : :
349 : : //! return the x-axis
350 : 0 : CubitVector CubitTransformMatrix::x_axis() const
351 : : {
352 [ # # ]: 0 : CubitMatrix tmp1(4,1);
353 [ # # ]: 0 : tmp1.set(0,0,1);
354 [ # # ]: 0 : tmp1.set(1,0,0);
355 [ # # ]: 0 : tmp1.set(2,0,0);
356 [ # # ]: 0 : tmp1.set(3,0,0);
357 [ # # ][ # # ]: 0 : CubitMatrix tmp2 = (*this) * tmp1;
358 [ # # ][ # # ]: 0 : return CubitVector(tmp2.get(0,0), tmp2.get(1,0), tmp2.get(2,0));
[ # # ][ # # ]
[ # # ]
359 : : }
360 : :
361 : : //! return the y-axis
362 : 0 : CubitVector CubitTransformMatrix::y_axis() const
363 : : {
364 [ # # ]: 0 : CubitMatrix tmp1(4,1);
365 [ # # ]: 0 : tmp1.set(0,0,0);
366 [ # # ]: 0 : tmp1.set(1,0,1);
367 [ # # ]: 0 : tmp1.set(2,0,0);
368 [ # # ]: 0 : tmp1.set(3,0,0);
369 [ # # ][ # # ]: 0 : CubitMatrix tmp2 = (*this) * tmp1;
370 [ # # ][ # # ]: 0 : return CubitVector(tmp2.get(0,0), tmp2.get(1,0), tmp2.get(2,0));
[ # # ][ # # ]
[ # # ]
371 : : }
372 : :
373 : : //! return the z-axis
374 : 0 : CubitVector CubitTransformMatrix::z_axis() const
375 : : {
376 [ # # ]: 0 : CubitMatrix tmp1(4,1);
377 [ # # ]: 0 : tmp1.set(0,0,0);
378 [ # # ]: 0 : tmp1.set(1,0,0);
379 [ # # ]: 0 : tmp1.set(2,0,1);
380 [ # # ]: 0 : tmp1.set(3,0,0);
381 [ # # ][ # # ]: 0 : CubitMatrix tmp2 = (*this) * tmp1;
382 [ # # ][ # # ]: 0 : return CubitVector(tmp2.get(0,0), tmp2.get(1,0), tmp2.get(2,0));
[ # # ][ # # ]
[ # # ]
383 : : }
384 : :
385 : :
386 : 0 : CubitTransformMatrix CubitTransformMatrix::construct_matrix(const CubitVector& origin,
387 : : const CubitVector& x_axis,
388 : : const CubitVector& y_axis)
389 : : {
390 [ # # ]: 0 : CubitTransformMatrix mat;
391 [ # # ][ # # ]: 0 : double angle = x_axis.interior_angle(CubitVector(1,0,0));
392 [ # # ][ # # ]: 0 : CubitVector axis = angle == 180 ? CubitVector(0,1,0) : CubitVector(1,0,0) * x_axis;
[ # # ][ # # ]
[ # # ][ # # ]
393 [ # # ]: 0 : mat.rotate(angle, axis);
394 [ # # ][ # # ]: 0 : CubitVector y = mat * CubitVector(0,1,0);
395 [ # # ]: 0 : angle = y_axis.interior_angle(y);
396 [ # # ][ # # ]: 0 : axis = angle == 180 ? CubitVector(0,0,1) : y * y_axis;
[ # # ][ # # ]
397 [ # # ]: 0 : mat.rotate(angle, axis);
398 [ # # ]: 0 : mat.translate(origin);
399 : 0 : return mat;
400 : : }
401 : :
402 : 0 : void CubitTransformMatrix::get_rotation_axis_and_angle(CubitVector &rotation_axis, double &angle)
403 : : {
404 : 0 : double cos_theta = (this->get(0,0) + this->get(1,1) + this->get(2,2) - 1) / 2;
405 : :
406 : 0 : double x = 0;
407 : 0 : double y = 0;
408 : 0 : double z = 0;
409 : :
410 [ # # ]: 0 : if (fabs(cos_theta - 1) < .0001)
411 : : {
412 : : // theta is 1 or almost 1
413 : 0 : angle = 0;
414 : 0 : x = 0;
415 : 0 : y = 0;
416 : 0 : z = 1;
417 : : }
418 [ # # ]: 0 : else if (fabs(cos_theta + 1) > .0001)
419 : : {
420 : : // theta is NOT -1 or almost -1
421 : 0 : angle = acos(cos_theta);
422 : 0 : angle = (angle * 180.0) / CUBIT_PI; // convert to degrees
423 : 0 : double sin_theta = sqrt(1 - cos_theta * cos_theta);
424 : 0 : x = ( (this->get(2,1) - this->get(1,2)) / 2 ) / sin_theta;
425 : 0 : y = ( (this->get(0,2) - this->get(2,0)) / 2 ) / sin_theta;
426 : 0 : z = ( (this->get(1,0) - this->get(0,1)) / 2 ) / sin_theta;
427 : : }
428 : : else
429 : : {
430 : 0 : angle = 180;
431 [ # # ]: 0 : if (this->get(0,0) >= this->get(1,1))
432 : : {
433 : :
434 [ # # ]: 0 : if (this->get(0,0) >= this->get(2,2))
435 : : {
436 : : // 0,0 is maximal diagonal term
437 : 0 : x = sqrt(this->get(0,0) - this->get(1,1) - this->get(2,2) + 1) / 2;
438 : 0 : double half_inverse = 1 / (2 * x);
439 : 0 : y = half_inverse * this->get(0,1);
440 : 0 : z = half_inverse * this->get(0,2);
441 : : }
442 : : else
443 : : {
444 : : // 2,2 is maximal diagonal term
445 : 0 : z = sqrt(this->get(2,2) - this->get(0,0) - this->get(1,1) + 1) / 2;
446 : 0 : double half_inverse = 1 / (2 * z);
447 : 0 : x = half_inverse * this->get(0,2);
448 : 0 : y = half_inverse * this->get(1,2);
449 : : }
450 : : }
451 : : else
452 : : {
453 [ # # ]: 0 : if (this->get(1,1) >= this->get(2,2))
454 : : {
455 : : // 1,1 is maximal diagonal term
456 : 0 : y = sqrt(this->get(1,1) - this->get(0,0) - this->get(2,2) + 1) / 2;
457 : 0 : double half_inverse = 1 / (2 * y);
458 : 0 : x = half_inverse * this->get(0,1);
459 : 0 : z = half_inverse * this->get(1,2);
460 : : }
461 : : else
462 : : {
463 : : // 2,2 is maximal diagonal term
464 : 0 : z = sqrt(this->get(2,2) - this->get(0,0) - this->get(1,1) + 1) / 2;
465 : 0 : double half_inverse = 1 / (2 * z);
466 : 0 : x = half_inverse * this->get(0,2);
467 : 0 : y = half_inverse * this->get(1,2);
468 : : }
469 : : }
470 : :
471 : : }
472 : :
473 : 0 : rotation_axis.x(x);
474 : 0 : rotation_axis.y(y);
475 : 0 : rotation_axis.z(z);
476 : 0 : }
|
