1 // Vamos Automotive Simulator
2 // Copyright (C) 2001--2002 Sam Varner
3 //
4 // This program is free software; you can redistribute it and/or modify
5 // it under the terms of the GNU General Public License as published by
6 // the Free Software Foundation; either version 2 of the License, or
7 // (at your option) any later version.
8 //
9 // This program is distributed in the hope that it will be useful,
10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 // GNU General Public License for more details.
13 //
14 // You should have received a copy of the GNU General Public License
15 // along with this program; if not, write to the Free Software
16 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
17
18 #include "Three_Matrix.h"
19
20 #include <cmath>
21
22 using namespace Vamos_Geometry;
23
24 static void rotate_elements (double mat [3][3],
25 int i, int j, int k, int l,
26 double h, double s, double tau);
27
Three_Matrix()28 Three_Matrix::Three_Matrix ()
29 {
30 identity ();
31 }
32
33
Three_Matrix(const double mat_in[3][3])34 Three_Matrix::Three_Matrix (const double mat_in [3][3])
35 {
36 for (int i = 0; i < 3; i++)
37 {
38 for (int j = 0; j < 3; j++)
39 {
40 m_mat [i][j] = mat_in [i][j];
41 }
42 }
43 }
44
45
Three_Matrix(const Three_Matrix & mat)46 Three_Matrix::Three_Matrix (const Three_Matrix& mat)
47 {
48 copy_in (mat);
49 }
50
51 Three_Matrix&
operator =(const Three_Matrix & mat)52 Three_Matrix::operator = (const Three_Matrix& mat)
53 {
54 if (&mat != this)
55 {
56 copy_in (mat);
57 }
58 return *this;
59 }
60
61 bool
operator ==(const Three_Matrix & mat) const62 Three_Matrix::operator == (const Three_Matrix& mat) const
63 {
64 for (size_t i = 0; i < 3; i++)
65 {
66 for (size_t j = 0; j < 3; j++)
67 {
68 if (m_mat [i][j] != mat [i][j])
69 return false;
70 }
71 }
72 return true;
73 }
74
75 Three_Vector
unit(int index) const76 Three_Matrix::unit (int index) const
77 {
78 return Three_Vector (m_mat [0][index], m_mat [1][index], m_mat [2][index]);
79 }
80
81 void
set_diagonal(double diag)82 Three_Matrix::set_diagonal (double diag)
83 {
84 for (int i = 0; i < 3; i++)
85 {
86 m_mat [i][i] = diag;
87 }
88 }
89
90 void
identity()91 Three_Matrix::identity ()
92 {
93 zero ();
94 set_diagonal (1.0);
95 }
96
97 void
zero()98 Three_Matrix::zero ()
99 {
100 for (int i = 0; i < 3; i++)
101 {
102 for (int j = 0; j < 3; j++)
103 {
104 m_mat [i][j] = 0.0;
105 }
106 }
107 }
108
109 const Three_Matrix&
rotate(const Three_Vector & delta_theta)110 Three_Matrix::rotate (const Three_Vector& delta_theta)
111 {
112 const double angle = 0.5 * delta_theta.magnitude (); // in radians
113 if (angle == 0.0)
114 return *this;
115
116 const Three_Vector unit = delta_theta.unit () * sin (angle);
117 const double w = cos (angle);
118
119 // This tranformation matrix is derived from quaternion analysis.
120 Three_Matrix q_rot;
121 const double& x = unit.x;
122 const double& y = unit.y;
123 const double& z = unit.z;
124
125 const double wx = w*x;
126 const double wy = w*y;
127 const double wz = w*z;
128
129 const double xx = x*x;
130 const double xy = x*y;
131 const double xz = x*z;
132
133 const double yy = y*y;
134 const double yz = y*z;
135
136 const double zz = z*z;
137
138 q_rot [0][0] = 1.0 - 2.0 * (yy + zz);
139 q_rot [0][1] = 2.0 * (xy - wz);
140 q_rot [0][2] = 2.0 * (xz + wy);
141
142 q_rot [1][0] = 2.0 * (xy + wz);
143 q_rot [1][1] = 1.0 - 2.0 * (xx + zz);
144 q_rot [1][2] = 2.0 * (yz - wx);
145
146 q_rot [2][0] = 2.0 * (xz - wy);
147 q_rot [2][1] = 2.0 * (yz + wx);
148 q_rot [2][2] = 1.0 - 2.0 * (xx + yy);
149 // Note that the matrix is not symmetric. However, since x, y, and
150 // z are imaginary (although we treat them as real here) and w is
151 // real, the transformation matrix is Hermetian (equal to its
152 // complex conjugate transpose).
153
154 return *this *= q_rot;
155 }
156
157 void
diagonalize()158 Three_Matrix::diagonalize ()
159 {
160 const int dim = 3;
161
162 int rotations = 0;
163 double array_z [dim] = {0.0};
164 double array_b [dim];
165 double mat [dim][dim];
166
167 for (int i = 0; i < dim; i++)
168 {
169 for (int j = 0; j < dim; j++)
170 {
171 mat [i][j] = m_mat [i][j];
172 m_e_vec [i][j] = 0.0;
173 }
174 array_b [i] = m_mat [i][i];
175 m_e_val [i] = m_mat [i][i];
176 m_e_vec [i][i] = 1.0;
177 }
178
179 for (int iter = 0; iter < 50; iter++)
180 {
181 double off_diag = 0.0;
182 for (int i = 0; i < (dim - 1); i++)
183 {
184 for (int j = i + 1; j < dim; j++)
185 {
186 off_diag += mat [i][j];
187 }
188 }
189 if (off_diag == 0.0)
190 break;
191
192 double thresh = (iter < 4) ? (0.2 * off_diag / (dim * dim)) : 0.0;
193
194 for (int row = 0; row < (dim - 1); row++)
195 {
196 for (int col = row + 1; col < dim; col++)
197 {
198 double small = 100.0 * std::abs (mat [row][col]);
199 if ((iter > 4)
200 && (std::abs (m_e_val [row] + small)
201 == std::abs (m_e_val [row]))
202 && (std::abs (m_e_val [col] + small)
203 == std::abs (m_e_val [col])))
204 {
205 mat [row][col] = 0.0;
206 }
207 else if (std::abs (mat [row][col]) > thresh)
208 {
209 double param_h = m_e_val [col] - m_e_val [row];
210 double param_t;
211 if (std::abs (param_h) + small == std::abs (param_h))
212 {
213 param_t = mat [row][col] / param_h;
214 }
215 else
216 {
217 double theta = 0.5 * param_h / mat [row][col];
218 param_t = 1.0 / (std::abs (theta) +
219 sqrt (1.0 + theta * theta));
220 if (theta < 0.0)
221 param_t = -param_t;
222 }
223 double param_c = 1.0 / sqrt (1.0 + param_t * param_t);
224 double param_s = param_t * param_c;
225 double tau = param_s / (1.0 + param_c);
226
227 param_h = param_t * mat [row][col];
228 array_z [row] -= param_h;
229 array_z [col] += param_h;
230 m_e_val [row] -= param_h;
231 m_e_val [col] += param_h;
232 mat [row][col] = 0.0;
233
234 // Rotate the matrix.
235 for (int j = 0; j < row; j++)
236 rotate_elements (mat, j, row, j, col,
237 param_h, param_s, tau);
238 for (int j = row + 1; j < col; j++)
239 rotate_elements (mat, row, j, j, col,
240 param_h, param_s, tau);
241 for (int j = col + 1; j < dim; j++)
242 rotate_elements (mat, row, j, col, j,
243 param_h, param_s, tau);
244 // Rotate the eigenvectors.
245 for (int j = 0; j < dim; j++)
246 rotate_elements (m_e_vec, j, row, j, col,
247 param_h, param_s, tau);
248
249 rotations++;
250 }
251 }
252 }
253 for (int i = 0; i < dim; i++)
254 {
255 array_b [i] += array_z [i];
256 m_e_val [i] = array_b [i];
257 array_z [i] = 0.0;
258 }
259 }
260 }
261
262 void
rotate_elements(double mat[3][3],int i,int j,int k,int l,double param_h,double param_s,double tau)263 rotate_elements (double mat [3][3], int i, int j, int k, int l,
264 double param_h, double param_s, double tau)
265 {
266 double param_g = mat [i][j];
267 param_h = mat [k][l];
268 mat [i][j] = param_g - param_s * (param_h + param_g * tau);
269 mat [k][l] = param_h + param_s * (param_g - param_h * tau);
270 }
271
272 Three_Matrix
eigen(Three_Vector * out_vec)273 Three_Matrix::eigen (Three_Vector* out_vec)
274 {
275 diagonalize ();
276 if (out_vec)
277 *out_vec = Three_Vector (m_e_val [0],
278 m_e_val [1],
279 m_e_val [2]);
280 return Three_Matrix (m_e_vec).transpose ();
281 }
282
283 Three_Matrix
transpose() const284 Three_Matrix::transpose () const
285 {
286 Three_Matrix out_mat (m_mat);
287 out_mat [0][1] = m_mat [1][0];
288 out_mat [1][0] = m_mat [0][1];
289 out_mat [0][2] = m_mat [2][0];
290 out_mat [2][0] = m_mat [0][2];
291 out_mat [1][2] = m_mat [2][1];
292 out_mat [2][1] = m_mat [1][2];
293 return out_mat;
294 }
295
296 Three_Matrix
invert() const297 Three_Matrix::invert () const
298 {
299 double det
300 = m_mat [0][0] * m_mat [1][1] * m_mat [2][2]
301 + m_mat [0][1] * m_mat [1][2] * m_mat [2][0]
302 + m_mat [0][2] * m_mat [1][0] * m_mat [2][1]
303 - m_mat [0][2] * m_mat [1][1] * m_mat [2][0]
304 - m_mat [0][1] * m_mat [1][0] * m_mat [2][2]
305 - m_mat [0][0] * m_mat [1][2] * m_mat [2][1];
306
307 if (det == 0.0)
308 {
309 throw Singular_Matrix ();
310 }
311
312 // A^{-1} = C^T / det(A)
313 // A^{-1}_{ij} = C_{ji} / det(A)
314 // where C_{ji} is the cofactor of the A_{ji}.
315 // See Boas p.122-123.
316 Three_Matrix out_mat;
317 out_mat [0][0] =
318 (m_mat [1][1] * m_mat [2][2] - m_mat [1][2] * m_mat [2][1]) / det;
319 out_mat [1][0] =
320 (m_mat [1][2] * m_mat [2][0] - m_mat [1][0] * m_mat [2][2]) / det;
321 out_mat [2][0] =
322 (m_mat [1][0] * m_mat [2][1] - m_mat [1][1] * m_mat [2][0]) / det;
323 out_mat [0][1] =
324 (m_mat [2][1] * m_mat [0][2] - m_mat [2][2] * m_mat [0][1]) / det;
325 out_mat [1][1] =
326 (m_mat [2][2] * m_mat [0][0] - m_mat [2][0] * m_mat [0][2]) / det;
327 out_mat [2][1] =
328 (m_mat [2][0] * m_mat [0][1] - m_mat [2][1] * m_mat [0][0]) / det;
329 out_mat [0][2] =
330 (m_mat [0][1] * m_mat [1][2] - m_mat [0][2] * m_mat [1][1]) / det;
331 out_mat [1][2] =
332 (m_mat [0][2] * m_mat [1][0] - m_mat [0][0] * m_mat [1][2]) / det;
333 out_mat [2][2] =
334 (m_mat [0][0] * m_mat [1][1] - m_mat [0][1] * m_mat [1][0]) / det;
335
336 return out_mat;
337 }
338
339 namespace Vamos_Geometry
340 {
341 Three_Vector
operator *(const Three_Vector & vec,const Three_Matrix & mat)342 operator * (const Three_Vector& vec, const Three_Matrix& mat)
343 {
344 return Three_Vector
345 (vec.x * mat [0][0] + vec.y * mat [1][0] + vec.z * mat [2][0],
346 vec.x * mat [0][1] + vec.y * mat [1][1] + vec.z * mat [2][1],
347 vec.x * mat [0][2] + vec.y * mat [1][2] + vec.z * mat [2][2]);
348 }
349
350 Three_Matrix
operator *(const Three_Matrix & mat1,const Three_Matrix & mat2)351 operator * (const Three_Matrix& mat1, const Three_Matrix& mat2)
352 {
353 double out_mat [3][3] = {{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}};
354 for (int i = 0; i < 3; i++)
355 {
356 for (int j = 0; j < 3; j++)
357 {
358 for (int k = 0; k < 3; k++)
359 {
360 out_mat [i][j] += mat1 [i][k] * mat2 [k][j];
361 }
362 }
363 }
364 return Three_Matrix (out_mat);
365 }
366
367 Three_Matrix
operator *(double factor,const Three_Matrix & mat)368 operator * (double factor, const Three_Matrix& mat)
369 {
370 Three_Matrix out_mat = mat;
371 return out_mat *= factor;
372 }
373
374 Three_Matrix
operator *(const Three_Matrix & mat,double factor)375 operator * (const Three_Matrix& mat, double factor)
376 {
377 return factor * mat;
378 }
379
380 // Stream operators.
381 std::ostream&
operator <<(std::ostream & os,Three_Matrix mat)382 operator << (std::ostream& os, Three_Matrix mat)
383 {
384 os << "[[ " << mat [0][0]
385 << ",\t" << mat [0][1]
386 << ",\t" << mat [0][2] << "]\n"
387 << " [ " << mat [1][0]
388 << ",\t" << mat [1][1]
389 << ",\t" << mat [1][2] << "]\n"
390 << " [ " << mat [2][0]
391 << ",\t" << mat [2][1]
392 << ",\t" << mat [2][2] << "]]";
393 return os;
394 }
395
396 // Return the Euler angles about the x (PHI), y (THETA), and z (PSI)
397 // axes for the orientation matrix MAT.
398 void
euler_angles(const Three_Matrix & mat,double * phi,double * theta,double * psi)399 euler_angles (const Three_Matrix& mat,
400 double* phi, double* theta, double* psi)
401 {
402 *theta = asin (mat [2][0]);
403
404 if (std::abs (*theta) > 0.00001)
405 {
406 double cos_theta = cos (*theta);
407 double tr_x = mat [2][2] / cos_theta;
408 double tr_y = -mat [2][1] / cos_theta;
409
410 *phi = atan2 (tr_y, tr_x);
411
412 tr_x = mat [0][0] / cos_theta;
413 tr_y = mat [1][0] / cos_theta;
414
415 *psi = atan2 (tr_y, tr_x);
416 }
417 else
418 {
419 *phi = 0.0;
420
421 double tr_x = mat [1][1];
422 double tr_y = -mat [0][1];
423
424 *psi = atan2 (tr_y, tr_x);
425 }
426 }
427 }
428