MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
matvec3.cc
Go to the documentation of this file.
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
53 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
68 Vec3::Tens(void) const
69 {
70  return Tens(*this);
71 }
72 
73 /* Prodotto vettore per matrice */
74 Mat3x3
75 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 
92 Vec3
93 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
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
122 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
133 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
152 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
166 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
186 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
199 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
255 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
279 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 
443 Mat3x3
444 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 
481 Vec3
482 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 
499 Mat3x3
500 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 
537 Mat3x3
538 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 
575 Mat3x3
576 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 
595 Mat3x3
596 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
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
629 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 
651 Vec3 operator - (const Vec3& v)
652 {
653  return Vec3(-v.pdVec[V1],
654  -v.pdVec[V2],
655  -v.pdVec[V3]);
656 }
657 
658 
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&
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&
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&
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&
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&
722 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&
738 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&
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
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
772 MatR2LinParam(const Mat3x3& m)
773 {
774  return m.Ax();
775 }
776 
777 /* Calcolo della matrice R a partire da due vettori sghembi */
778 
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
887 MatR2EulerAngles(const Mat3x3& R)
888 {
889  return MatR2EulerAngles123(R);
890 }
891 
892 Vec3
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
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
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 
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
1008 EulerAngles2MatR(const Vec3& v)
1009 {
1010  return EulerAngles123_2MatR(v);
1011 }
1012 
1013 Mat3x3
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
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
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
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
1124 IsNull(const doublereal& d)
1125 {
1126  return d == 0.;
1127 }
1128 
1129 template <>
1130 bool
1131 IsExactlySame(const doublereal& d1, const doublereal& d2)
1132 {
1133  return d1 == d2;
1134 }
1135 
1136 template <>
1137 bool
1138 IsSame(const doublereal& d1, const doublereal& d2, const doublereal& dTol)
1139 {
1140  return fabs(d1 - d2) <= dTol;
1141 }
1142 
1143 Vec3
1144 MultRV(const Vec3& v, const Mat3x3& R)
1145 {
1146  return R*v;
1147 }
1148 
1149 Mat3x3
1150 MultRM(const Mat3x3& m, const Mat3x3& R)
1151 {
1152  return R*m;
1153 }
1154 
1155 Mat3x3
1156 MultMRt(const Mat3x3& m, const Mat3x3& R)
1157 {
1158  return m.MulMT(R);
1159 }
1160 
1161 Mat3x3
1162 MultRMRt(const Mat3x3& m, const Mat3x3& R)
1163 {
1164  return R*m.MulMT(R);
1165 }
1166 
CGR_Rot::MatGm1
const MatGm1_Manip MatGm1
Definition: matvec3.cc:647
MultRMRt
Mat3x3 MultRMRt(const Mat3x3 &m, const Mat3x3 &R)
Definition: matvec3.cc:1162
Mat3x3Diag_Manip
Definition: matvec3.h:1478
Mat3x3::Ax
Vec3 Ax(void) const
Definition: matvec3.h:827
MatR2vec
Mat3x3 MatR2vec(unsigned short int ia, const Vec3 &va, unsigned short int ib, const Vec3 &vb)
Definition: matvec3.cc:779
MultRM
Mat3x3 MultRM(const Mat3x3 &m, const Mat3x3 &R)
Definition: matvec3.cc:1150
M_PI
#define M_PI
Definition: gradienttest.cc:67
Zero3
const Vec3 Zero3(0., 0., 0.)
Vec3::Cross
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
CGR_Rot::Param
const Param_Manip Param
Definition: matvec3.cc:644
Vec3::Write
std::ostream & Write(std::ostream &out, const char *sFill=" ") const
Definition: matvec3.cc:738
MultRV
Vec3 MultRV(const Vec3 &v, const Mat3x3 &R)
Definition: matvec3.cc:1144
Mat3x3::dGet
const doublereal & dGet(unsigned short int iRow, unsigned short int iCol) const
Definition: matvec3.h:770
CGR_Rot::MatGm1_Manip
Definition: matvec3.h:1730
MBDYN_EXCEPT_ARGS
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
Vec3
Definition: matvec3.h:98
grad::sin
GradientExpression< UnaryExpr< FuncSin, Expr > > sin(const GradientExpression< Expr > &u)
Definition: gradient.h:2977
MultMRt
Mat3x3 MultMRt(const Mat3x3 &m, const Mat3x3 &R)
Definition: matvec3.cc:1156
MatCross
const MatCross_Manip MatCross
Definition: matvec3.cc:639
Mat3x3::MulTMT
Mat3x3 MulTMT(const Mat3x3 &m) const
Definition: matvec3.cc:538
Vec3::Norm
doublereal Norm(void) const
Definition: matvec3.h:263
IsNull
bool IsNull(const doublereal &d)
Definition: matvec3.cc:1124
Mat3x3::Tr
doublereal Tr(void) const
Definition: matvec3.h:1361
Vec3::Dot
doublereal Dot(const Vec3 &v) const
Definition: matvec3.h:243
M22
Definition: matvec3.h:59
Mat3x3::Inv
Mat3x3 Inv(void) const
Definition: matvec3.cc:152
Eye3
const Mat3x3 Eye3(1., 0., 0., 0., 1., 0., 0., 0., 1.)
sForm
const char sForm[]
Definition: matvec3.cc:674
IsSame
bool IsSame(const doublereal &d1, const doublereal &d2, const doublereal &dTol)
Definition: matvec3.cc:1138
V2
Definition: matvec3.h:50
EulerAngles313_2MatR
Mat3x3 EulerAngles313_2MatR(const Vec3 &v)
Definition: matvec3.cc:1039
Vec3::pdVec
doublereal pdVec[3]
Definition: matvec3.h:109
MatCrossCross_Manip
Definition: matvec3.h:1518
grad::fabs
GradientExpression< UnaryExpr< FuncFabs, Expr > > fabs(const GradientExpression< Expr > &u)
Definition: gradient.h:2973
Mat3x3::GetVec
Vec3 GetVec(unsigned short int i) const
Definition: matvec3.h:893
Mat3x3DEye
const Mat3x3DEye_Manip Mat3x3DEye
Definition: matvec3.cc:637
CGR_Rot::Param_Manip
Definition: matvec3.h:1664
Zero1
const doublereal Zero1(0.)
M21
Definition: matvec3.h:58
Mat3x3Zero
const Mat3x3Zero_Manip Mat3x3Zero
Definition: matvec3.cc:636
Write
std::ostream & Write(std::ostream &out, const Mat3x3 &m, const char *s, const char *s2)
Definition: matvec3.cc:694
Mat3x3::Skew
Mat3x3 Skew(void) const
Definition: matvec3.cc:629
V3
Definition: matvec3.h:51
Mat3x3::MulTV
Vec3 MulTV(const Vec3 &v) const
Definition: matvec3.cc:482
M11
Definition: matvec3.h:55
Zero3x3
const Mat3x3 Zero3x3(0., 0., 0., 0., 0., 0., 0., 0., 0.)
MatR2gparam
Vec3 MatR2gparam(const Mat3x3 &m)
Definition: matvec3.cc:756
Mat3x3::Reset
void Reset(void)
Definition: matvec3.cc:613
EulerAngles123_2MatR
Mat3x3 EulerAngles123_2MatR(const Vec3 &v)
Definition: matvec3.cc:1014
Mat3x3::PutVec
void PutVec(unsigned short int i, const Vec3 &v)
Definition: matvec3.h:918
Mat3x3::Trace
doublereal Trace(void) const
Definition: matvec3.h:836
operator-
Vec3 operator-(const Vec3 &v)
Definition: matvec3.cc:651
M12
Definition: matvec3.h:56
Mat3x3::dDet
doublereal dDet(void) const
Definition: matvec3.cc:122
MatR2EulerAngles313
Vec3 MatR2EulerAngles313(const Mat3x3 &R)
Definition: matvec3.cc:927
Vec3::operator*
Vec3 operator*(const doublereal &d) const
Definition: matvec3.h:416
sDefFill
const char sDefFill[]
Definition: matvec3.cc:675
Mat3x3::MulTVCross
Mat3x3 MulTVCross(const Vec3 &v) const
Definition: matvec3.cc:596
CGR_Rot::MatG_Manip
Definition: matvec3.h:1712
M33
Definition: matvec3.h:63
Vec3::Vec3
Vec3(void)
Definition: matvec3.h:121
M32
Definition: matvec3.h:62
M31
Definition: matvec3.h:61
CGR_Rot::MatR_Manip
Definition: matvec3.h:1684
Mat3x3::IsDiag
bool IsDiag(void) const
Definition: matvec3.h:1284
Vec3::Reset
void Reset(void)
Definition: matvec3.cc:109
CGR_Rot::MatG
const MatG_Manip MatG
Definition: matvec3.cc:646
copysign
doublereal copysign(doublereal x, doublereal y)
Definition: gradient.h:97
M13
Definition: matvec3.h:57
DEBUGCOUT
#define DEBUGCOUT(msg)
Definition: myassert.h:232
MatR2EulerAngles123
Vec3 MatR2EulerAngles123(const Mat3x3 &R)
Definition: matvec3.cc:893
Mat3x3::Solve
Vec3 Solve(const Vec3 &v) const
Definition: matvec3.cc:186
dRaDegr
const doublereal dRaDegr
Definition: matvec3.cc:884
Mat3x3::EigSym
bool EigSym(Vec3 &EigenValues) const
Definition: matvec3.cc:255
CGR_Rot::MatR
const MatR_Manip MatR
Definition: matvec3.cc:645
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
EulerAngles321_2MatR
Mat3x3 EulerAngles321_2MatR(const Vec3 &v)
Definition: matvec3.cc:1064
grad::sqrt
GradientExpression< UnaryExpr< FuncSqrt, Expr > > sqrt(const GradientExpression< Expr > &u)
Definition: gradient.h:2974
Mat3x3::MulTM
Mat3x3 MulTM(const Mat3x3 &m) const
Definition: matvec3.cc:500
MatR2EulerAngles
Vec3 MatR2EulerAngles(const Mat3x3 &R)
Definition: matvec3.cc:887
MatCross_Manip
Definition: matvec3.h:1498
Mat3x3::LDLSolve
Vec3 LDLSolve(const Vec3 &v) const
Definition: matvec3.cc:199
Mat3x3::Vec3
friend class Vec3
Definition: matvec3.h:551
Mat3x3::IsSymmetric
bool IsSymmetric(void) const
Definition: matvec3.h:1260
Mat3x3::MulVCross
Mat3x3 MulVCross(const Vec3 &v) const
Definition: matvec3.cc:576
Unwrap
Vec3 Unwrap(const Vec3 &vPrev, const Vec3 &v)
Definition: matvec3.cc:1089
MatR2LinParam
Vec3 MatR2LinParam(const Mat3x3 &m)
Definition: matvec3.cc:772
EulerAngles2MatR
Mat3x3 EulerAngles2MatR(const Vec3 &v)
Definition: matvec3.cc:1008
MatCrossCross
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
Mat3x3::pGetMat
const doublereal * pGetMat(void) const
Definition: matvec3.h:743
Mat3x3Diag
const Mat3x3Diag_Manip Mat3x3Diag
Definition: matvec3.cc:638
V1
Definition: matvec3.h:49
Vec3::pGetVec
const doublereal * pGetVec(void) const
Definition: matvec3.h:192
Mat3x3::pdMat
doublereal pdMat[9]
Definition: matvec3.h:559
MatR2EulerAngles321
Vec3 MatR2EulerAngles321(const Mat3x3 &R)
Definition: matvec3.cc:941
M23
Definition: matvec3.h:60
grad::acos
GradientExpression< UnaryExpr< FuncAcos, Expr > > acos(const GradientExpression< Expr > &u)
Definition: gradient.h:2984
operator<<
std::ostream & operator<<(std::ostream &out, const Mat3x3 &m)
Definition: matvec3.cc:680
grad::cos
GradientExpression< UnaryExpr< FuncCos, Expr > > cos(const GradientExpression< Expr > &u)
Definition: gradient.h:2978
Mat3x3::MulMT
Mat3x3 MulMT(const Mat3x3 &m) const
Definition: matvec3.cc:444
grad::atan2
GradientExpression< BinaryExpr< FuncAtan2, LhsExpr, RhsExpr > > atan2(const GradientExpression< LhsExpr > &u, const GradientExpression< RhsExpr > &v)
Definition: gradient.h:2962
doublereal
double doublereal
Definition: colamd.c:52
Mat3x3::Write
std::ostream & Write(std::ostream &out, const char *sFill=" ", const char *sFill2=NULL) const
Definition: matvec3.cc:722
Mat3x3DEye_Manip
Definition: matvec3.h:1460
MatR2EulerParams
void MatR2EulerParams(const Mat3x3 &R, doublereal &e0, Vec3 &e)
Definition: matvec3.cc:954
Mat3x3::Mat3x3
Mat3x3(void)
Definition: matvec3.h:568
Vec3::Tens
Mat3x3 Tens(void) const
Definition: matvec3.cc:68
R
Mat3x3 R
Definition: mbdyn_util_oct.cc:74
Vec3::Mat3x3
friend class Mat3x3
Definition: matvec3.h:99
Mat3x3Zero_Manip
Definition: matvec3.h:1441
IsExactlySame
bool IsExactlySame(const doublereal &d1, const doublereal &d2)
Definition: matvec3.cc:1131