1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/libraries/libmbmath/matvec3.cc,v 1.65 2017/01/12 14:43:54 masarati Exp $ */
2 /*
3 * MBDyn (C) is a multibody analysis code.
4 * http://www.mbdyn.org
5 *
6 * Copyright (C) 1996-2017
7 *
8 * Pierangelo Masarati <masarati@aero.polimi.it>
9 * Paolo Mantegazza <mantegazza@aero.polimi.it>
10 *
11 * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
12 * via La Masa, 34 - 20156 Milano, Italy
13 * http://www.aero.polimi.it
14 *
15 * Changing this copyright notice is forbidden.
16 *
17 * This program is free software; you can redistribute it and/or modify
18 * it under the terms of the GNU General Public License as published by
19 * the Free Software Foundation (version 2 of the License).
20 *
21 *
22 * This program is distributed in the hope that it will be useful,
23 * but WITHOUT ANY WARRANTY; without even the implied warranty of
24 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25 * GNU General Public License for more details.
26 *
27 * You should have received a copy of the GNU General Public License
28 * along with this program; if not, write to the Free Software
29 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30 */
31
32 /* vettori 3 e matrici 3x3 */
33
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35
36 #include <limits>
37 #include <cmath>
38 #include <cfloat>
39 #include <limits>
40
41 #include "matvec3.h"
42
43 /* noteworthy constant */
44 const doublereal Zero1(0.);
45 const Vec3 Zero3(0., 0., 0.);
46 const Mat3x3 Eye3(1., 0., 0., 0., 1., 0., 0., 0., 1.);
47 const Mat3x3 Zero3x3(0., 0., 0., 0., 0., 0., 0., 0., 0.);
48
49
50 /* Vec3 - begin */
51
52 Mat3x3
Tens(const Vec3 & v) const53 Vec3::Tens(const Vec3& v) const
54 {
55 return Mat3x3(pdVec[V1]*v.pdVec[V1],
56 pdVec[V2]*v.pdVec[V1],
57 pdVec[V3]*v.pdVec[V1],
58 pdVec[V1]*v.pdVec[V2],
59 pdVec[V2]*v.pdVec[V2],
60 pdVec[V3]*v.pdVec[V2],
61 pdVec[V1]*v.pdVec[V3],
62 pdVec[V2]*v.pdVec[V3],
63 pdVec[V3]*v.pdVec[V3]);
64 }
65
66 /* Prodotto "tensore". Restituisce se stesso per se stesso */
67 Mat3x3
Tens(void) const68 Vec3::Tens(void) const
69 {
70 return Tens(*this);
71 }
72
73 /* Prodotto vettore per matrice */
74 Mat3x3
Cross(const Mat3x3 & m) const75 Vec3::Cross(const Mat3x3& m) const {
76 return Mat3x3(pdVec[V2]*m.pdMat[M31]-pdVec[V3]*m.pdMat[M21],
77 pdVec[V3]*m.pdMat[M11]-pdVec[V1]*m.pdMat[M31],
78 pdVec[V1]*m.pdMat[M21]-pdVec[V2]*m.pdMat[M11],
79 pdVec[V2]*m.pdMat[M32]-pdVec[V3]*m.pdMat[M22],
80 pdVec[V3]*m.pdMat[M12]-pdVec[V1]*m.pdMat[M32],
81 pdVec[V1]*m.pdMat[M22]-pdVec[V2]*m.pdMat[M12],
82 pdVec[V2]*m.pdMat[M33]-pdVec[V3]*m.pdMat[M23],
83 pdVec[V3]*m.pdMat[M13]-pdVec[V1]*m.pdMat[M33],
84 pdVec[V1]*m.pdMat[M23]-pdVec[V2]*m.pdMat[M13]);
85 }
86
87
88 /**
89 * Scalar product
90 * multiplies self by matrix m; equivalent to m.Transpose() * this.
91 */
92 Vec3
operator *(const Mat3x3 & m) const93 Vec3::operator * (const Mat3x3& m) const
94 {
95 return
96 Vec3(
97 m.pdMat[M11]*pdVec[V1]
98 + m.pdMat[M21]*pdVec[V2]
99 + m.pdMat[M31]*pdVec[V3],
100 m.pdMat[M12]*pdVec[V1]
101 + m.pdMat[M22]*pdVec[V2]
102 + m.pdMat[M32]*pdVec[V3],
103 m.pdMat[M13]*pdVec[V1]
104 + m.pdMat[M23]*pdVec[V2]
105 + m.pdMat[M33]*pdVec[V3]);
106 }
107
108 void
Reset(void)109 Vec3::Reset(void)
110 {
111 pdVec[V1] = 0.;
112 pdVec[V2] = 0.;
113 pdVec[V3] = 0.;
114 }
115
116 /* Vec3 - end */
117
118 /* Mat3x3 - begin */
119
120 /* inversione */
121 doublereal
dDet(void) const122 Mat3x3::dDet(void) const
123 {
124 doublereal* p = (doublereal*)pdMat;
125
126 return p[M11]*(p[M22]*p[M33]-p[M23]*p[M32])
127 +p[M12]*(p[M23]*p[M31]-p[M21]*p[M33])
128 +p[M13]*(p[M21]*p[M32]-p[M22]*p[M31]);
129 }
130
131 /* inversione */
132 Mat3x3
Inv(const doublereal & d) const133 Mat3x3::Inv(const doublereal &d) const
134 {
135 ASSERT(fabs(d) > std::numeric_limits<doublereal>::epsilon());
136
137 doublereal* p = (doublereal*)pdMat;
138
139 return Mat3x3((p[M22]*p[M33]-p[M23]*p[M32])/d,
140 (p[M23]*p[M31]-p[M21]*p[M33])/d,
141 (p[M21]*p[M32]-p[M22]*p[M31])/d,
142 (p[M13]*p[M32]-p[M12]*p[M33])/d,
143 (p[M11]*p[M33]-p[M13]*p[M31])/d,
144 (p[M12]*p[M31]-p[M11]*p[M32])/d,
145 (p[M12]*p[M23]-p[M13]*p[M22])/d,
146 (p[M13]*p[M21]-p[M11]*p[M23])/d,
147 (p[M11]*p[M22]-p[M12]*p[M21])/d);
148 }
149
150 /* inversione */
151 Mat3x3
Inv(void) const152 Mat3x3::Inv(void) const
153 {
154 doublereal d = dDet();
155 if (fabs(d) < std::numeric_limits<doublereal>::epsilon()) {
156 silent_cerr("matrix is singular" << std::endl);
157 throw MatrixHandler::ErrMatrixIsSingular(MBDYN_EXCEPT_ARGS);
158 }
159
160 return Inv(d);
161 }
162
163
164 /* soluzione */
165 Vec3
Solve(const doublereal & d,const Vec3 & v) const166 Mat3x3::Solve(const doublereal& d, const Vec3& v) const
167 {
168 const doublereal* p = pdMat;
169 const doublereal* pv = v.pGetVec();
170
171 ASSERT(fabs(d) > std::numeric_limits<doublereal>::epsilon());
172
173 return Vec3((pv[V1]*(p[M22]*p[M33] - p[M23]*p[M32])
174 +pv[V2]*(p[M13]*p[M32] - p[M12]*p[M33])
175 +pv[V3]*(p[M12]*p[M23] - p[M13]*p[M22]))/d,
176 (pv[V1]*(p[M23]*p[M31] - p[M21]*p[M33])
177 +pv[V2]*(p[M11]*p[M33] - p[M13]*p[M31])
178 +pv[V3]*(p[M13]*p[M21] - p[M11]*p[M23]))/d,
179 (pv[V1]*(p[M21]*p[M32] - p[M22]*p[M31])
180 +pv[V2]*(p[M12]*p[M31] - p[M11]*p[M32])
181 +pv[V3]*(p[M11]*p[M22] - p[M12]*p[M21]))/d);
182 }
183
184 /* soluzione */
185 Vec3
Solve(const Vec3 & v) const186 Mat3x3::Solve(const Vec3& v) const
187 {
188 doublereal d = dDet();
189
190 if (fabs(d) < std::numeric_limits<doublereal>::epsilon()) {
191 silent_cerr("matrix is singular" << std::endl);
192 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
193 }
194
195 return Solve(d, v);
196 }
197
198 Vec3
LDLSolve(const Vec3 & v) const199 Mat3x3::LDLSolve(const Vec3& v) const
200 {
201 doublereal d1, d2, d3, l21 = 0., l31 = 0., l32 = 0.;
202
203 d1 = pdMat[M11];
204 ASSERT(d1 >= 0.);
205 if (d1 > std::numeric_limits<doublereal>::epsilon()) {
206 l21 = (pdMat[M21] + pdMat[M12])/(2.*d1);
207 l31 = (pdMat[M31] + pdMat[M13])/(2.*d1);
208 }
209
210 d2 = pdMat[M22] - l21*l21*d1;
211 ASSERT(d2 >= 0.);
212 if (d2 > std::numeric_limits<doublereal>::epsilon()) {
213 l32 = ((pdMat[M32] + pdMat[M23])/2. - l21*l31*d1)/d2;
214 }
215
216 d3 = pdMat[M33] - l31*l31*d1 - l32*l32*d2;
217
218 // L * D * L^T * x = v
219 // L^T * x = y
220 // D * y = z
221 // L * z = v
222
223 // z = L^-1 * v
224 doublereal z1 = v(1);
225 doublereal z2 = v(2) - l21*z1;
226 doublereal z3 = v(3) - l31*z1 - l32*z2;
227
228 // y = D^-1 * z
229 if (d1 > std::numeric_limits<doublereal>::epsilon()) {
230 z1 /= d1;
231 } else {
232 z1 = 0.;
233 }
234
235 if (d2 > std::numeric_limits<doublereal>::epsilon()) {
236 z2 /= d2;
237 } else {
238 z2 = 0.;
239 }
240
241 if (d3 > std::numeric_limits<doublereal>::epsilon()) {
242 z3 /= d3;
243 } else {
244 z3 = 0.;
245 }
246
247 // x = L^-T * y
248 z2 -= l32*z3;
249 z1 -= l21*z2 + l31*z3;
250
251 return Vec3(z1, z2, z3);
252 }
253
254 bool
EigSym(Vec3 & EigenValues) const255 Mat3x3::EigSym(Vec3& EigenValues) const
256 {
257 Mat3x3 EigenVectors;
258
259 return EigSym(EigenValues, EigenVectors);
260 }
261
262 #if 0
263 bool
264 Mat3x3::EigSym(Vec3& EigenValues,
265 Mat3x3& EigenVectors,
266 const Mat3x3& Hints) const
267 {
268 Mat3x3 Tmp;
269
270 if (!EigSym(EigenValues, Tmp)) {
271 return false;
272 }
273
274 /* TBC */
275 }
276 #endif
277
278 bool
EigSym(Vec3 & EigenValues,Mat3x3 & EigenVectors) const279 Mat3x3::EigSym(Vec3& EigenValues, Mat3x3& EigenVectors) const
280 {
281 // From:
282 // W.M. Scherzinger, C.R. Dohrmann,
283 // `A robust algorithm for finding the eigenvalues and eigenvectors
284 // of 3x3 symmetric matrices'
285 // Comput. Methods Appl. Mech. Engrg. 2008
286 // doi:10.1016/j.cma.2008.03.031
287
288 if (!IsSymmetric(std::numeric_limits<doublereal>::epsilon())) {
289 return false;
290 }
291
292 if (IsDiag(std::numeric_limits<doublereal>::epsilon())) {
293 EigenVectors = Eye3;
294 EigenValues = Vec3(pdMat[M11], pdMat[M22], pdMat[M33]);
295
296 return true;
297 }
298
299 Mat3x3 AA = *this;
300
301 doublereal trA_3 = Trace()/3;
302
303 AA(1, 1) -= trA_3;
304 AA(2, 2) -= trA_3;
305 AA(3, 3) -= trA_3;
306
307 doublereal J2 = (AA*AA).Trace()/2;
308
309 if (fabs(J2) < std::numeric_limits<doublereal>::epsilon()) {
310 EigenVectors = Eye3;
311 EigenValues = Vec3(pdMat[M11], pdMat[M22], pdMat[M33]);
312
313 return true;
314 }
315
316 doublereal J2_dmy = sqrt(J2/3.);
317 doublereal J3 = AA.dDet();
318 doublereal dmy = J3/2/(J2_dmy*J2_dmy*J2_dmy);
319 doublereal alpha;
320
321 // NOTE: we want real eigenvalues; this requires the matrix to be
322 // positive definite or semi-definite
323 if (dmy < -1.) {
324 dmy = -1.;
325 alpha = M_PI;
326
327 } else if (dmy > 1.) {
328 dmy = 1.;
329 alpha = 0.;
330
331 } else {
332 alpha = acos(dmy)/3;
333 }
334
335 int idx1;
336 if (alpha < M_PI/6.) {
337 idx1 = 1;
338
339 } else {
340 idx1 = 3;
341 }
342
343 doublereal eta1 = 2*J2_dmy*cos(alpha + 2./3.*M_PI*(idx1 - 1));
344 EigenValues(idx1) = eta1 + trA_3;
345
346 // NOTE: there's a typo in the original paper;
347 // AA must be used instead of A
348 Vec3 r1 = AA.GetVec(1);
349 r1(1) -= eta1;
350 Vec3 r2 = AA.GetVec(2);
351 r2(2) -= eta1;
352 Vec3 r3 = AA.GetVec(3);
353 r3(3) -= eta1;
354
355 doublereal
356 nr1 = r1.Norm(),
357 nr2 = r2.Norm(),
358 nr3 = r3.Norm();
359
360 int irmax = 1;
361 doublereal nrmax = nr1;
362
363 if (nr2 > nrmax) {
364 irmax = 2;
365 nrmax = nr2;
366 }
367
368 if (nr3 > nrmax) {
369 irmax = 3;
370 nrmax = nr3;
371 }
372
373 if (irmax == 2) {
374 Vec3 rtmp = r2;
375 r2 = r1;
376 nr2 = nr1;
377 r1 = rtmp;
378 nr1 = nrmax;
379
380 } else if (irmax == 3) {
381 Vec3 rtmp = r3;
382 r3 = r1;
383 nr3 = nr1;
384 r1 = rtmp;
385 nr1 = nrmax;
386 }
387
388 Vec3 s1, s2;
389 s1 = r1/nr1;
390 Vec3 t2 = r2 - s1*(s1*r2);
391 doublereal nt2 = t2.Norm();
392 Vec3 t3 = r3 - s1*(s1*r3);
393 doublereal nt3 = t3.Norm();
394
395 if (nt2 > nt3) {
396 s2 = t2/nt2;
397
398 } else {
399 s2 = t3/nt3;
400 }
401
402 Vec3 v1(s1.Cross(s2));
403 EigenVectors.PutVec(idx1, v1);
404
405 Mat3x3 AAA(eta1, 0., 0.,
406 0., s1*(AA*s1), s2*(AA*s1),
407 0., s1*(AA*s2), s2*(AA*s2));
408
409 int idx2 = idx1%3 + 1;
410 int idx3 = (idx1 + 1)%3 + 1;
411
412 doublereal AAA22p33 = AAA(2, 2) + AAA(3, 3);
413 doublereal AAA22m33 = AAA(2, 2) - AAA(3, 3);
414 doublereal eta2 = (AAA22p33 - copysign(1., AAA22m33)*sqrt(AAA22m33*AAA22m33 + 4*AAA(2, 3)*AAA(3, 2)))/2;
415 doublereal eta3 = AAA22p33 - eta2;
416
417 EigenValues(idx2) = eta2 + trA_3;
418 EigenValues(idx3) = eta3 + trA_3;
419
420 Vec3 u1 = AA*s1 - s1*eta2;
421 doublereal nu1 = u1.Norm();
422 Vec3 u2 = AA*s2 - s2*eta2;
423 doublereal nu2 = u2.Norm();
424
425 Vec3 w1;
426 if (nu1 > nu2) {
427 w1 = u1/nu1;
428
429 } else {
430 w1 = u2/nu2;
431 }
432
433 Vec3 v2(w1.Cross(v1));
434 EigenVectors.PutVec(idx2, v2);
435 EigenVectors.PutVec(idx3, v1.Cross(v2));
436
437 return true;
438 }
439
440 /**
441 * multiply by another matrix, transposed: this * m^T
442 */
443 Mat3x3
MulMT(const Mat3x3 & m) const444 Mat3x3::MulMT(const Mat3x3& m) const
445 {
446 return Mat3x3(
447 pdMat[M11]*m.pdMat[M11]
448 + pdMat[M12]*m.pdMat[M12]
449 + pdMat[M13]*m.pdMat[M13],
450 pdMat[M21]*m.pdMat[M11]
451 + pdMat[M22]*m.pdMat[M12]
452 + pdMat[M23]*m.pdMat[M13],
453 pdMat[M31]*m.pdMat[M11]
454 + pdMat[M32]*m.pdMat[M12]
455 + pdMat[M33]*m.pdMat[M13],
456
457 pdMat[M11]*m.pdMat[M21]
458 + pdMat[M12]*m.pdMat[M22]
459 + pdMat[M13]*m.pdMat[M23],
460 pdMat[M21]*m.pdMat[M21]
461 + pdMat[M22]*m.pdMat[M22]
462 + pdMat[M23]*m.pdMat[M23],
463 pdMat[M31]*m.pdMat[M21]
464 + pdMat[M32]*m.pdMat[M22]
465 + pdMat[M33]*m.pdMat[M23],
466
467 pdMat[M11]*m.pdMat[M31]
468 + pdMat[M12]*m.pdMat[M32]
469 + pdMat[M13]*m.pdMat[M33],
470 pdMat[M21]*m.pdMat[M31]
471 + pdMat[M22]*m.pdMat[M32]
472 + pdMat[M23]*m.pdMat[M33],
473 pdMat[M31]*m.pdMat[M31]
474 + pdMat[M32]*m.pdMat[M32]
475 + pdMat[M33]*m.pdMat[M33]);
476 }
477
478 /**
479 * multiply self transposed by a vector: this^T * v
480 */
481 Vec3
MulTV(const Vec3 & v) const482 Mat3x3::MulTV(const Vec3& v) const
483 {
484 return Vec3(
485 pdMat[M11]*v.pdVec[V1]
486 + pdMat[M21]*v.pdVec[V2]
487 + pdMat[M31]*v.pdVec[V3],
488 pdMat[M12]*v.pdVec[V1]
489 + pdMat[M22]*v.pdVec[V2]
490 + pdMat[M32]*v.pdVec[V3],
491 pdMat[M13]*v.pdVec[V1]
492 + pdMat[M23]*v.pdVec[V2]
493 + pdMat[M33]*v.pdVec[V3]);
494 }
495
496 /**
497 * multiply self transposed by another matrix: this^T * m
498 */
499 Mat3x3
MulTM(const Mat3x3 & m) const500 Mat3x3::MulTM(const Mat3x3& m) const
501 {
502 return Mat3x3(
503 pdMat[M11]*m.pdMat[M11]
504 + pdMat[M21]*m.pdMat[M21]
505 + pdMat[M31]*m.pdMat[M31],
506 pdMat[M12]*m.pdMat[M11]
507 + pdMat[M22]*m.pdMat[M21]
508 + pdMat[M32]*m.pdMat[M31],
509 pdMat[M13]*m.pdMat[M11]
510 + pdMat[M23]*m.pdMat[M21]
511 + pdMat[M33]*m.pdMat[M31],
512
513 pdMat[M11]*m.pdMat[M12]
514 + pdMat[M21]*m.pdMat[M22]
515 + pdMat[M31]*m.pdMat[M32],
516 pdMat[M12]*m.pdMat[M12]
517 + pdMat[M22]*m.pdMat[M22]
518 + pdMat[M32]*m.pdMat[M32],
519 pdMat[M13]*m.pdMat[M12]
520 + pdMat[M23]*m.pdMat[M22]
521 + pdMat[M33]*m.pdMat[M32],
522
523 pdMat[M11]*m.pdMat[M13]
524 + pdMat[M21]*m.pdMat[M23]
525 + pdMat[M31]*m.pdMat[M33],
526 pdMat[M12]*m.pdMat[M13]
527 + pdMat[M22]*m.pdMat[M23]
528 + pdMat[M32]*m.pdMat[M33],
529 pdMat[M13]*m.pdMat[M13]
530 + pdMat[M23]*m.pdMat[M23]
531 + pdMat[M33]*m.pdMat[M33]);
532 }
533
534 /**
535 * multiply self transposed by another matrix, transposed: this^T * m^T
536 */
537 Mat3x3
MulTMT(const Mat3x3 & m) const538 Mat3x3::MulTMT(const Mat3x3& m) const
539 {
540 return Mat3x3(
541 pdMat[M11]*m.pdMat[M11]
542 + pdMat[M21]*m.pdMat[M12]
543 + pdMat[M31]*m.pdMat[M13],
544 pdMat[M12]*m.pdMat[M11]
545 + pdMat[M22]*m.pdMat[M12]
546 + pdMat[M32]*m.pdMat[M13],
547 pdMat[M13]*m.pdMat[M11]
548 + pdMat[M23]*m.pdMat[M12]
549 + pdMat[M33]*m.pdMat[M13],
550
551 pdMat[M11]*m.pdMat[M21]
552 + pdMat[M21]*m.pdMat[M22]
553 + pdMat[M31]*m.pdMat[M23],
554 pdMat[M12]*m.pdMat[M21]
555 + pdMat[M22]*m.pdMat[M22]
556 + pdMat[M32]*m.pdMat[M23],
557 pdMat[M13]*m.pdMat[M21]
558 + pdMat[M23]*m.pdMat[M22]
559 + pdMat[M33]*m.pdMat[M23],
560
561 pdMat[M11]*m.pdMat[M31]
562 + pdMat[M21]*m.pdMat[M32]
563 + pdMat[M31]*m.pdMat[M33],
564 pdMat[M12]*m.pdMat[M31]
565 + pdMat[M22]*m.pdMat[M32]
566 + pdMat[M32]*m.pdMat[M33],
567 pdMat[M13]*m.pdMat[M31]
568 + pdMat[M23]*m.pdMat[M32]
569 + pdMat[M33]*m.pdMat[M33]);
570 }
571
572 /**
573 * multiply self times vCross
574 */
575 Mat3x3
MulVCross(const Vec3 & v) const576 Mat3x3::MulVCross(const Vec3& v) const
577 {
578 return Mat3x3(
579 pdMat[M12]*v.pdVec[V3] - pdMat[M13]*v.pdVec[V2],
580 pdMat[M22]*v.pdVec[V3] - pdMat[M23]*v.pdVec[V2],
581 pdMat[M32]*v.pdVec[V3] - pdMat[M33]*v.pdVec[V2],
582
583 pdMat[M13]*v.pdVec[V1] - pdMat[M11]*v.pdVec[V3],
584 pdMat[M23]*v.pdVec[V1] - pdMat[M21]*v.pdVec[V3],
585 pdMat[M33]*v.pdVec[V1] - pdMat[M31]*v.pdVec[V3],
586
587 pdMat[M11]*v.pdVec[V2] - pdMat[M12]*v.pdVec[V1],
588 pdMat[M21]*v.pdVec[V2] - pdMat[M22]*v.pdVec[V1],
589 pdMat[M31]*v.pdVec[V2] - pdMat[M32]*v.pdVec[V1]);
590 }
591
592 /**
593 * multiply self transposed times vCross
594 */
595 Mat3x3
MulTVCross(const Vec3 & v) const596 Mat3x3::MulTVCross(const Vec3& v) const
597 {
598 return Mat3x3(
599 pdMat[M21]*v.pdVec[V3] - pdMat[M31]*v.pdVec[V2],
600 pdMat[M22]*v.pdVec[V3] - pdMat[M32]*v.pdVec[V2],
601 pdMat[M23]*v.pdVec[V3] - pdMat[M33]*v.pdVec[V2],
602
603 pdMat[M31]*v.pdVec[V1] - pdMat[M11]*v.pdVec[V3],
604 pdMat[M32]*v.pdVec[V1] - pdMat[M12]*v.pdVec[V3],
605 pdMat[M33]*v.pdVec[V1] - pdMat[M13]*v.pdVec[V3],
606
607 pdMat[M11]*v.pdVec[V2] - pdMat[M21]*v.pdVec[V1],
608 pdMat[M12]*v.pdVec[V2] - pdMat[M22]*v.pdVec[V1],
609 pdMat[M13]*v.pdVec[V2] - pdMat[M23]*v.pdVec[V1]);
610 }
611
612 void
Reset(void)613 Mat3x3::Reset(void)
614 {
615 pdMat[M11] = 0.;
616 pdMat[M12] = 0.;
617 pdMat[M13] = 0.;
618
619 pdMat[M21] = 0.;
620 pdMat[M22] = 0.;
621 pdMat[M23] = 0.;
622
623 pdMat[M31] = 0.;
624 pdMat[M32] = 0.;
625 pdMat[M33] = 0.;
626 }
627
628 Mat3x3
Skew(void) const629 Mat3x3::Skew(void) const
630 {
631 return Mat3x3(MatCross, this->Ax());
632 }
633
634 /* Mat3x3 - end */
635
636 const Mat3x3Zero_Manip Mat3x3Zero;
637 const Mat3x3DEye_Manip Mat3x3DEye;
638 const Mat3x3Diag_Manip Mat3x3Diag;
639 const MatCross_Manip MatCross;
640 const MatCrossCross_Manip MatCrossCross;
641
642 /* Manipolatori */
643 namespace CGR_Rot {
644 const Param_Manip Param;
645 const MatR_Manip MatR;
646 const MatG_Manip MatG;
647 const MatGm1_Manip MatGm1;
648 };
649
650
operator -(const Vec3 & v)651 Vec3 operator - (const Vec3& v)
652 {
653 return Vec3(-v.pdVec[V1],
654 -v.pdVec[V2],
655 -v.pdVec[V3]);
656 }
657
658
operator -(const Mat3x3 & m)659 Mat3x3 operator - (const Mat3x3& m)
660 {
661 const doublereal* pdMat = m.pGetMat();
662 return Mat3x3(-pdMat[M11],
663 -pdMat[M21],
664 -pdMat[M31],
665 -pdMat[M12],
666 -pdMat[M22],
667 -pdMat[M32],
668 -pdMat[M13],
669 -pdMat[M23],
670 -pdMat[M33]);
671 }
672
673
674 const char sForm[] = "%15.6e%15.6e%15.6e%15.6e%15.6e%15.6e%15.6e%15.6e%15.6e";
675 const char sDefFill[] = " ";
676
677 /* output di matrici */
678
679 std::ostream&
operator <<(std::ostream & out,const Mat3x3 & m)680 operator << (std::ostream& out, const Mat3x3& m)
681 {
682 const doublereal* pd = m.pGetMat();
683
684 out
685 << pd[M11] << sDefFill << pd[M12] << sDefFill << pd[M13] << sDefFill
686 << pd[M21] << sDefFill << pd[M22] << sDefFill << pd[M23] << sDefFill
687 << pd[M31] << sDefFill << pd[M32] << sDefFill << pd[M33];
688
689 return out;
690 }
691
692
693 std::ostream&
Write(std::ostream & out,const Mat3x3 & m,const char * s,const char * s2)694 Write(std::ostream& out, const Mat3x3& m, const char* s, const char* s2)
695 {
696 return m.Write(out, s, s2);
697 }
698
699
700 /* output di vettori */
701
702 std::ostream&
operator <<(std::ostream & out,const Vec3 & v)703 operator << (std::ostream& out, const Vec3& v)
704 {
705 const doublereal* pd = v.pGetVec();
706
707 out << pd[0] << sDefFill << pd[1] << sDefFill << pd[2];
708
709 return out;
710 }
711
712
713 std::ostream&
Write(std::ostream & out,const Vec3 & v,const char * s)714 Write(std::ostream& out, const Vec3& v, const char* s)
715 {
716 return v.Write(out, s);
717 }
718
719
720 /* Output di matrici */
721 std::ostream&
Write(std::ostream & out,const char * sFill,const char * sFill2) const722 Mat3x3::Write(std::ostream& out, const char* sFill, const char* sFill2) const
723 {
724 char* sF2 = (char*)sFill2;
725 if (sFill2 == NULL) {
726 sF2 = (char*)sFill;
727 }
728 out
729 << pdMat[M11] << sFill << pdMat[M12] << sFill << pdMat[M13] << sF2
730 << pdMat[M21] << sFill << pdMat[M22] << sFill << pdMat[M23] << sF2
731 << pdMat[M31] << sFill << pdMat[M32] << sFill << pdMat[M33];
732
733 return out;
734 }
735
736
737 std::ostream&
Write(std::ostream & out,const char * sFill) const738 Vec3::Write(std::ostream& out, const char* sFill) const
739 {
740 out << pdVec[V1] << sFill << pdVec[V2] << sFill << pdVec[V3];
741
742 return out;
743 }
744
745
746 std::ostream&
Write(std::ostream & out,const doublereal & d,const char *)747 Write(std::ostream& out, const doublereal& d, const char*)
748 {
749 return out << d;
750 }
751
752
753 /* calcolo dei parametri di rotazione a partire dalla matrice R */
754
755 Vec3
MatR2gparam(const Mat3x3 & m)756 MatR2gparam(const Mat3x3& m)
757 {
758 /* test di singolarita' */
759 doublereal d = 1. + m.Trace();
760
761 if (fabs(d) < std::numeric_limits<doublereal>::epsilon()) {
762 silent_cerr("MatR2gparam(): divide by zero, "
763 "probably due to singularity in rotation parameters" << std::endl);
764 throw ErrDivideByZero(MBDYN_EXCEPT_ARGS);
765 }
766
767 return m.Ax()*(4./d);
768 }
769
770 /* So-called linear parametrization, consisting in ax(R - R') */
771 Vec3
MatR2LinParam(const Mat3x3 & m)772 MatR2LinParam(const Mat3x3& m)
773 {
774 return m.Ax();
775 }
776
777 /* Calcolo della matrice R a partire da due vettori sghembi */
778
MatR2vec(unsigned short int ia,const Vec3 & va,unsigned short int ib,const Vec3 & vb)779 Mat3x3 MatR2vec(unsigned short int ia, const Vec3& va,
780 unsigned short int ib, const Vec3& vb)
781 {
782 ASSERT(ia >= 1 && ia <= 3);
783 ASSERT(ib >= 1 && ib <= 3);
784
785 Vec3 r[3];
786
787 DEBUGCOUT("MatR2vec: ia = " << ia << " (" << va << "),"
788 << " ib = " << ib << " (" << vb << ")" << std::endl);
789
790 if (ia < 1 || ia > 3) {
791 silent_cerr("MatR2vec: first index is illegal"
792 << std::endl);
793 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
794 }
795
796 int i1 = ia-1;
797 int i2 = ia%3;
798 int i3 = (ia+1)%3;
799
800 if (ib == (ia%3)+1) {
801 doublereal d = va.Norm();
802 if (d <= std::numeric_limits<doublereal>::epsilon()) {
803 silent_cerr("MatR2vec: first vector must be non-null" << std::endl );
804 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
805 }
806 r[i1] = va/d;
807 d = vb.Norm();
808 if (d <= std::numeric_limits<doublereal>::epsilon()) {
809 silent_cerr("MatR2vec: second vector must be non-null" << std::endl );
810 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
811 }
812 r[i3] = r[i1].Cross(vb);
813 d = r[i3].Dot();
814 if (d <= std::numeric_limits<doublereal>::epsilon()) {
815 silent_cerr("MatR2vec: vectors must be distinct"
816 << std::endl);
817 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
818 }
819 d = sqrt(d);
820 r[i3] /= d;
821 r[i2] = r[i3].Cross(r[i1]);
822
823 DEBUGCOUT("R = " << Mat3x3(r[0], r[1], r[2]) << std::endl);
824
825 return Mat3x3(r[0], r[1], r[2]);
826 } else if (ib == ((ia+1)%3+1)) {
827 doublereal d = va.Norm();
828 if (d <= std::numeric_limits<doublereal>::epsilon()) {
829 silent_cerr("MatR2vec: first vector must be non-null" << std::endl );
830 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
831 }
832 r[i1] = va/d;
833 d = vb.Norm();
834 if (d <= std::numeric_limits<doublereal>::epsilon()) {
835 silent_cerr("MatR2vec: second vector must be non-null" << std::endl );
836 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
837 }
838 r[i2] = vb.Cross(r[i1]);
839 d = r[i2].Dot();
840 if (d <= std::numeric_limits<doublereal>::epsilon()) {
841 silent_cerr("MatR2vec: vectors must be distinct"
842 << std::endl);
843 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
844 }
845 d = sqrt(d);
846 r[i2] /= d;
847 r[i3] = r[i1].Cross(r[i2]);
848
849 DEBUGCOUT("R = " << Mat3x3(r[0], r[1], r[2]) << std::endl);
850
851 return Mat3x3(r[0], r[1], r[2]);
852 } else {
853 silent_cerr("MatR2vec: second index is illegal" << std::endl);
854 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
855 }
856
857 return ::Zero3x3; // phony call, not reachable
858 }
859
860
861 /* Subroutine che calcola gli angoli di Eulero a partire dalla matrice R
862 SUBROUTINE RPYPAR(ALFA, PHI)
863
864 IMPLICIT NONE
865 REAL*8 PHI(3), ALFA(3,3)
866
867 REAL*8 RADEGR
868 PARAMETER (RADEGR = 180.D0/3.141592653589793D0)
869
870 REAL*8 CA, SA
871
872 PHI(1) = ATAN2(- ALFA(2,3), ALFA(3,3))
873 CA = COS(PHI(1))
874 SA = SIN(PHI(1))
875 PHI(1) = PHI(1)*RADEGR
876 PHI(2) = ATAN2(ALFA(1,3), - SA*ALFA(2,3) + CA*ALFA(3,3))*RADEGR
877 PHI(3) = ATAN2(CA*ALFA(2,1) + SA*ALFA(3,1),
878 & CA*ALFA(2,2) + SA*ALFA(3,2))*RADEGR
879 RETURN
880 END
881
882 */
883
884 const doublereal dRaDegr = 180./M_PI;
885
886 Vec3
MatR2EulerAngles(const Mat3x3 & R)887 MatR2EulerAngles(const Mat3x3& R)
888 {
889 return MatR2EulerAngles123(R);
890 }
891
892 Vec3
MatR2EulerAngles123(const Mat3x3 & R)893 MatR2EulerAngles123(const Mat3x3& R)
894 {
895 doublereal dAlpha = atan2(-R(2, 3), R(3, 3));
896 doublereal dCosAlpha = cos(dAlpha);
897 doublereal dSinAlpha = sin(dAlpha);
898
899 return Vec3(dAlpha,
900 atan2(R(1, 3),
901 dCosAlpha*R(3, 3) - dSinAlpha*R(2, 3)),
902 atan2(dCosAlpha*R(2, 1) + dSinAlpha*R(3, 1),
903 dCosAlpha*R(2, 2) + dSinAlpha*R(3, 2)));
904 }
905
906 /*
907 R_313 = R_a * R_b * R_c
908
909 [ ca -sa 0 ]
910 R_a = [ sa ca 0 ]
911 [ 0 0 1 ]
912
913 [ 1 0 0 ]
914 R_a = [ 0 cb -sb ]
915 [ 0 sb cb ]
916
917 [ cc -sc 0 ]
918 R_c = [ sc cc 0 ]
919 [ 0 0 1 ]
920
921 [ ca*cc-cb*sa*sc -ca*sc-cb*cc*sa sa*sb ]
922 R_313 = [ cb*sa+ca*cb*sc ca*cb*cc-sa*sc -ca*sb ]
923 [ sb*sc cc*sb cb ]
924 */
925
926 Vec3
MatR2EulerAngles313(const Mat3x3 & R)927 MatR2EulerAngles313(const Mat3x3& R)
928 {
929 doublereal dAlpha = atan2(R(1, 3), -R(2, 3));
930 doublereal dCosAlpha = cos(dAlpha);
931 doublereal dSinAlpha = sin(dAlpha);
932
933 return Vec3(dAlpha,
934 atan2(dSinAlpha*R(1, 3) - dCosAlpha*R(2, 3),
935 R(3, 3)),
936 atan2(-dCosAlpha*R(1, 2) - dSinAlpha*R(2, 2),
937 dCosAlpha*R(1, 1) + dSinAlpha*R(2, 1)));
938 }
939
940 Vec3
MatR2EulerAngles321(const Mat3x3 & R)941 MatR2EulerAngles321(const Mat3x3& R)
942 {
943 doublereal dAlpha = atan2(R(2, 1), R(1, 1));
944 doublereal dCosAlpha = cos(dAlpha);
945 doublereal dSinAlpha = sin(dAlpha);
946
947 return Vec3(dAlpha,
948 atan2(-R(3, 1),
949 dCosAlpha*R(1, 1) + dSinAlpha*R(2, 1)),
950 atan2(dSinAlpha*R(1, 3) - dCosAlpha*R(2, 3),
951 -dSinAlpha*R(1, 2) + dCosAlpha*R(2, 2)));
952 }
953
MatR2EulerParams(const Mat3x3 & R,doublereal & e0,Vec3 & e)954 void MatR2EulerParams(const Mat3x3& R, doublereal& e0, Vec3& e)
955 {
956 doublereal t = R.Tr();
957 doublereal T[4];
958 T[0] = 1. + t;
959 T[1] = 1. - t + 2.*R.dGet(1, 1);
960 T[2] = 1. - t + 2.*R.dGet(2, 2);
961 T[3] = 1. - t + 2.*R.dGet(3, 3);
962
963 int k = 0;
964 for (int i = 1; i <= 3; i++) {
965 if (fabs(T[i]) >= fabs(T[k])) {
966 k = i;
967 }
968 }
969
970 switch (k) {
971 case 0:
972 e0 = .5*sqrt(T[0]);
973 e = Vec3(R.dGet(3, 2)-R.dGet(2, 3),
974 R.dGet(1, 3)-R.dGet(3, 1),
975 R.dGet(2, 1)-R.dGet(1, 2))/(4.*e0);
976 break;
977
978 case 1: {
979 doublereal e1 = .5*sqrt(T[1]);
980 e0 = (R.dGet(3, 2) - R.dGet(2, 3))/(4.*e1);
981 e = Vec3(T[1],
982 R.dGet(2, 1)+R.dGet(1, 2),
983 R.dGet(3, 1)+R.dGet(1, 3))/(4.*e1);
984 break;
985 }
986
987 case 2: {
988 doublereal e2 = .5*sqrt(T[2]);
989 e0 = (R.dGet(1, 3) - R.dGet(3, 1))/(4.*e2);
990 e = Vec3(R.dGet(1, 2)+R.dGet(2, 1),
991 T[2],
992 R.dGet(3, 2)+R.dGet(2, 3))/(4.*e2);
993 break;
994 }
995
996 case 3: {
997 doublereal e3 = .5*sqrt(T[3]);
998 e0 = (R.dGet(2, 1) - R.dGet(1, 2))/(4.*e3);
999 e = Vec3(R.dGet(1, 3)+R.dGet(3, 1),
1000 R.dGet(2, 3)+R.dGet(3, 2),
1001 T[3])/(4.*e3);
1002 break;
1003 }
1004 }
1005 }
1006
1007 Mat3x3
EulerAngles2MatR(const Vec3 & v)1008 EulerAngles2MatR(const Vec3& v)
1009 {
1010 return EulerAngles123_2MatR(v);
1011 }
1012
1013 Mat3x3
EulerAngles123_2MatR(const Vec3 & v)1014 EulerAngles123_2MatR(const Vec3& v)
1015 {
1016 doublereal d = v(1);
1017 doublereal dCosAlpha(cos(d));
1018 doublereal dSinAlpha(sin(d));
1019 d = v(2);
1020 doublereal dCosBeta(cos(d));
1021 doublereal dSinBeta(sin(d));
1022 d = v(3);
1023 doublereal dCosGamma(cos(d));
1024 doublereal dSinGamma(sin(d));
1025
1026 return Mat3x3(
1027 dCosBeta*dCosGamma,
1028 dCosAlpha*dSinGamma + dSinAlpha*dSinBeta*dCosGamma,
1029 dSinAlpha*dSinGamma - dCosAlpha*dSinBeta*dCosGamma,
1030 -dCosBeta*dSinGamma,
1031 dCosAlpha*dCosGamma - dSinAlpha*dSinBeta*dSinGamma,
1032 dSinAlpha*dCosGamma + dCosAlpha*dSinBeta*dSinGamma,
1033 dSinBeta,
1034 -dSinAlpha*dCosBeta,
1035 dCosAlpha*dCosBeta);
1036 };
1037
1038 Mat3x3
EulerAngles313_2MatR(const Vec3 & v)1039 EulerAngles313_2MatR(const Vec3& v)
1040 {
1041 doublereal d = v(1);
1042 doublereal dCosAlpha(cos(d));
1043 doublereal dSinAlpha(sin(d));
1044 d = v(2);
1045 doublereal dCosBeta(cos(d));
1046 doublereal dSinBeta(sin(d));
1047 d = v(3);
1048 doublereal dCosGamma(cos(d));
1049 doublereal dSinGamma(sin(d));
1050
1051 return Mat3x3(
1052 dCosAlpha*dCosGamma - dSinAlpha*dCosBeta*dSinGamma,
1053 dSinAlpha*dCosGamma + dCosAlpha*dCosBeta*dSinGamma,
1054 dSinBeta*dSinGamma,
1055 -dCosAlpha*dSinGamma - dSinAlpha*dCosBeta*dCosGamma,
1056 -dSinAlpha*dSinGamma + dCosAlpha*dCosBeta*dCosGamma,
1057 dSinBeta*dCosGamma,
1058 dSinAlpha*dSinBeta,
1059 -dCosAlpha*dSinBeta,
1060 dCosBeta);
1061 }
1062
1063 Mat3x3
EulerAngles321_2MatR(const Vec3 & v)1064 EulerAngles321_2MatR(const Vec3& v)
1065 {
1066 doublereal d = v(1);
1067 doublereal dCosAlpha(cos(d));
1068 doublereal dSinAlpha(sin(d));
1069 d = v(2);
1070 doublereal dCosBeta(cos(d));
1071 doublereal dSinBeta(sin(d));
1072 d = v(3);
1073 doublereal dCosGamma(cos(d));
1074 doublereal dSinGamma(sin(d));
1075
1076 return Mat3x3(
1077 dCosAlpha*dCosBeta,
1078 dSinAlpha*dCosBeta,
1079 -dSinBeta,
1080 -dSinAlpha*dCosGamma + dCosAlpha*dSinBeta*dSinGamma,
1081 dCosAlpha*dCosGamma + dSinAlpha*dSinBeta*dSinGamma,
1082 dCosBeta*dSinGamma,
1083 dSinAlpha*dSinGamma + dCosAlpha*dSinBeta*dCosGamma,
1084 -dCosAlpha*dSinGamma + dSinAlpha*dSinBeta*dCosGamma,
1085 dCosBeta*dCosGamma);
1086 }
1087
1088 Vec3
Unwrap(const Vec3 & vPrev,const Vec3 & v)1089 Unwrap(const Vec3& vPrev, const Vec3& v)
1090 {
1091 doublereal dTheta = v.Norm();
1092 if (dTheta > 0.) {
1093 doublereal dThetaPrev = vPrev.Norm();
1094 if (dThetaPrev > std::numeric_limits<doublereal>::epsilon()) {
1095 bool b(false);
1096
1097 if (vPrev*v < 0) {
1098 dTheta = -dTheta;
1099 }
1100
1101 doublereal dThetaOld = dTheta;
1102
1103 while (dTheta - dThetaPrev > M_PI) {
1104 dTheta -= 2.*M_PI;
1105 b = true;
1106 }
1107
1108 while (dThetaPrev - dTheta > M_PI) {
1109 dTheta += 2.*M_PI;
1110 b = true;
1111 }
1112
1113 if (b) {
1114 return v*(dTheta/dThetaOld);
1115 }
1116 }
1117 }
1118
1119 return v;
1120 }
1121
1122 template <>
1123 bool
IsNull(const doublereal & d)1124 IsNull(const doublereal& d)
1125 {
1126 return d == 0.;
1127 }
1128
1129 template <>
1130 bool
IsExactlySame(const doublereal & d1,const doublereal & d2)1131 IsExactlySame(const doublereal& d1, const doublereal& d2)
1132 {
1133 return d1 == d2;
1134 }
1135
1136 template <>
1137 bool
IsSame(const doublereal & d1,const doublereal & d2,const doublereal & dTol)1138 IsSame(const doublereal& d1, const doublereal& d2, const doublereal& dTol)
1139 {
1140 return fabs(d1 - d2) <= dTol;
1141 }
1142
1143 Vec3
MultRV(const Vec3 & v,const Mat3x3 & R)1144 MultRV(const Vec3& v, const Mat3x3& R)
1145 {
1146 return R*v;
1147 }
1148
1149 Mat3x3
MultRM(const Mat3x3 & m,const Mat3x3 & R)1150 MultRM(const Mat3x3& m, const Mat3x3& R)
1151 {
1152 return R*m;
1153 }
1154
1155 Mat3x3
MultMRt(const Mat3x3 & m,const Mat3x3 & R)1156 MultMRt(const Mat3x3& m, const Mat3x3& R)
1157 {
1158 return m.MulMT(R);
1159 }
1160
1161 Mat3x3
MultRMRt(const Mat3x3 & m,const Mat3x3 & R)1162 MultRMRt(const Mat3x3& m, const Mat3x3& R)
1163 {
1164 return R*m.MulMT(R);
1165 }
1166
1167