1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/libraries/libmbmath/matvec3.h,v 1.73 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 e matrici 3x3 - operazioni collegate */
33
34
35 #ifndef MATVEC3_H
36 #define MATVEC3_H
37
38 #include <iostream>
39 #include <limits>
40 #include "ac/f2c.h"
41
42 #include "myassert.h"
43 #include "except.h"
44 #include "solman.h"
45
46 #include "tpls.h"
47
48 enum {
49 V1 = 0,
50 V2 = 1,
51 V3 = 2
52 };
53
54 enum {
55 M11 = 0,
56 M12 = 3,
57 M13 = 6,
58 M21 = 1,
59 M22 = 4,
60 M23 = 7,
61 M31 = 2,
62 M32 = 5,
63 M33 = 8
64 };
65
66 /* classi principali definite in questo file */
67 class Vec3; /* vettore 3x1 */
68 class Mat3x3; /* matrice 3x3 */
69
70 class Mat3x3_Manip; /* manipolatori per la matrice 3x3 */
71
72 /* Vec3_Manip - begin */
73
74 class Vec3_Manip {
75 public:
76 /*
77 Metodo che trasforma la matrice m nel vettore v.
78 Viene usato da un costruttore di Vec3 che riceve come
79 argomenti un manipolatore e una matrice di rotazione.
80
81 NOTE: renamed from Make() to Manipulate() May 2009
82 */
83 virtual void Manipulate(Vec3& v, const Mat3x3& m) const = 0;
84
~Vec3_Manip(void)85 virtual ~Vec3_Manip(void) {
86 NO_OP;
87 };
88 };
89
90 /* Vec3_Manip - end */
91
92
93
94
95 /* Vec3 - begin */
96
97 // Vettori di dimensione 3
98 class Vec3 {
99 friend class Mat3x3;
100 friend Vec3 operator - (const Vec3& v);
101 // friend class Mat3x3_Manip;
102 friend class VecN;
103 friend class Mat3xN;
104 friend class MatNx3;
105
106
107 private:
108 //vettore di tre reali che contiene i coefficienti
109 doublereal pdVec[3];
110
111 protected:
112
113 public:
114 /*Costruttori */
115
116 /*
117 Costruttore banalissimo: non fa nulla. Attenzione che cosi' il
118 valore di Vec3 e' unpredictable. Se si desidera un vettore nullo
119 usare Zero3.
120 */
Vec3(void)121 Vec3(void) {
122 NO_OP;
123 };
124
125 /*
126 Assegna i tre valori.
127 Per azzerare il vettore, usare Zero3.
128 */
Vec3(const doublereal & v1,const doublereal & v2,const doublereal & v3)129 Vec3(const doublereal& v1,
130 const doublereal& v2,
131 const doublereal& v3) {
132 pdVec[V1] = v1;
133 pdVec[V2] = v2;
134 pdVec[V3] = v3;
135 };
136
137 /*
138 Costruttore di copia.
139 */
Vec3(const Vec3 & v)140 Vec3(const Vec3& v) {
141 pdVec[V1] = v.pdVec[V1];
142 pdVec[V2] = v.pdVec[V2];
143 pdVec[V3] = v.pdVec[V3];
144 };
145
146 /*
147 Costruttore da array di reali. Copia i primi 3 valori dell'array.
148 Si assume che l'array da pd sia lungo almeno 3.
149 */
Vec3(const doublereal * pd)150 Vec3(const doublereal* pd) {
151 ASSERT(pd != NULL);
152
153 pdVec[V1] = pd[0];
154 pdVec[V2] = pd[1];
155 pdVec[V3] = pd[2];
156 };
157
158 /*
159 Costruttore da VectorHandler. Prende i valori da iFirstIndex
160 a iFirstIndex+2. Nota: gli indici del VectorHandler partono da 1,
161 in stile FORTRAN.
162 */
Vec3(const VectorHandler & vh,integer iFirstIndex)163 Vec3(const VectorHandler& vh, integer iFirstIndex) {
164 ASSERT(iFirstIndex > 0 && iFirstIndex <= vh.iGetSize()-2);
165 pdVec[V1] = vh(iFirstIndex);
166 pdVec[V2] = vh(++iFirstIndex);
167 pdVec[V3] = vh(++iFirstIndex);
168 };
169
170 /*
171 Costruttore con manipolatore.
172 Invoca un metodo del manipolatore Manip che restituisce una matrice
173 a partire dal vettore v.
174 */
Vec3(const Vec3_Manip & Manip,const Mat3x3 & m)175 Vec3(const Vec3_Manip& Manip, const Mat3x3& m) {
176 Manip.Manipulate(*this, m);
177 };
178
179 /*
180 Distruttore banale: non fa nulla.
181 */
~Vec3(void)182 ~Vec3(void) {
183 NO_OP;
184 };
185
186
187 /*Metodi di servizio */
188
189 /*
190 Dirty job: restituisce il puntatore al vettore (deprecato).
191 */
pGetVec(void)192 const doublereal* pGetVec(void) const {
193 return pdVec;
194 };
195
pGetVec(void)196 doublereal* pGetVec(void) {
197 return pdVec;
198 };
199
200 /*Operatori su vettori e matrici */
201
202 /*
203 Prodotto "tensore".
204 Restituisce se stesso per v trasposto.
205 */
206 Mat3x3 Tens(const Vec3& v) const;
207
208 /*
209 Prodotto "tensore".
210 Restituisce se stesso per se stesso
211 */
212 Mat3x3 Tens(void) const;
213
214 /*
215 Prodotto vettore.
216 Restituisce il prodotto vettore tra se stesso e v in un temporaneo.
217 */
Cross(const Vec3 & v)218 Vec3 Cross(const Vec3& v) const {
219 return Vec3(pdVec[V2]*v.pdVec[V3]-pdVec[V3]*v.pdVec[V2],
220 pdVec[V3]*v.pdVec[V1]-pdVec[V1]*v.pdVec[V3],
221 pdVec[V1]*v.pdVec[V2]-pdVec[V2]*v.pdVec[V1]);
222 };
223
224 /*
225 * element by element product of vectors
226 */
EBEMult(const Vec3 & v)227 Vec3 EBEMult(const Vec3& v) const {
228 return Vec3(pdVec[V1]*v.pdVec[V1],
229 pdVec[V2]*v.pdVec[V2],
230 pdVec[V3]*v.pdVec[V3]);
231 };
232
233 /*
234 Prodotto vettore per matrice.
235 Restituisce il prodotto vettore tra se stesso e m in un temporaneo.
236 */
237 Mat3x3 Cross(const Mat3x3& m) const;
238
239 /*
240 Prodotto scalare.
241 Restituisce il prodotto scalare tra se e v
242 */
Dot(const Vec3 & v)243 doublereal Dot(const Vec3& v) const {
244 return
245 pdVec[V1]*v.pdVec[V1]+
246 pdVec[V2]*v.pdVec[V2]+
247 pdVec[V3]*v.pdVec[V3];
248 };
249
250 /*
251 Prodotto scalare per se stesso.
252 */
Dot(void)253 doublereal Dot(void) const {
254 return
255 pdVec[V1]*pdVec[V1]+
256 pdVec[V2]*pdVec[V2]+
257 pdVec[V3]*pdVec[V3];
258 };
259
260 /*
261 Norma: sqrt(Dot())
262 */
Norm(void)263 doublereal Norm(void) const {
264 return
265 sqrt(pdVec[V1]*pdVec[V1]+
266 pdVec[V2]*pdVec[V2]+
267 pdVec[V3]*pdVec[V3]);
268 };
269
270 /*Operazioni sui coefficienti */
271
272 /*
273 Assegnazione di un coefficiente.
274 Nota: l'indice ha base 1, in stile FORTRAN.
275 */
Put(unsigned short int iRow,const doublereal & dCoef)276 inline void Put(unsigned short int iRow, const doublereal& dCoef) {
277 ASSERT(iRow >= 1 && iRow <= 3);
278 pdVec[--iRow] = dCoef;
279 };
280
281 /*
282 Lettura di un coefficiente.
283 Nota: l'indice ha base 1, in stile FORTRAN.
284 */
dGet(unsigned short int iRow)285 inline const doublereal& dGet(unsigned short int iRow) const {
286 ASSERT(iRow >= 1 && iRow <= 3);
287 return pdVec[--iRow];
288 };
289
operator()290 inline doublereal& operator () (unsigned short int iRow) {
291 ASSERT(iRow >= 1 && iRow <= 3);
292 return pdVec[--iRow];
293 };
294
operator()295 inline const doublereal& operator () (unsigned short int iRow) const {
296 ASSERT(iRow >= 1 && iRow <= 3);
297 return pdVec[--iRow];
298 };
299
300 inline doublereal& operator [] (unsigned short int iRow) {
301 ASSERT(iRow <= 2);
302 return pdVec[iRow];
303 };
304
305 inline const doublereal& operator [] (unsigned short int iRow) const {
306 ASSERT(iRow <= 2);
307 return pdVec[iRow];
308 };
309
310
311 /*Operazioni con array di reali */
312
313 /*
314 Somma se stesso all'array pd.
315 Si assume che l'array pd sia lungo almeno 3
316 */
AddTo(doublereal * pd)317 void AddTo(doublereal* pd) const {
318 ASSERT(pd != NULL);
319 pd[0] += pdVec[V1];
320 pd[1] += pdVec[V2];
321 pd[2] += pdVec[V3];
322 };
323
324 /*
325 Sottrae se stesso dall'array pd.
326 Si assume che l'array pd sia lungo almeno 3
327 */
SubFrom(doublereal * pd)328 void SubFrom(doublereal* pd) const {
329 ASSERT(pd != NULL);
330 pd[0] -= pdVec[V1];
331 pd[1] -= pdVec[V2];
332 pd[2] -= pdVec[V3];
333 };
334
335 /*
336 Scrive se stesso sull'array pd.
337 Si assume che l'array pd sia lungo almeno 3
338 */
PutTo(doublereal * pd)339 void PutTo(doublereal* pd) const {
340 ASSERT(pd != NULL);
341 pd[0] = pdVec[V1];
342 pd[1] = pdVec[V2];
343 pd[2] = pdVec[V3];
344 };
345
346 /*
347 Si legge dall'array pd.
348 Si assume che l'array pd sia lungo almeno 3
349 */
GetFrom(const doublereal * pd)350 void GetFrom(const doublereal* pd) {
351 ASSERT(pd != NULL);
352 pdVec[V1] = pd[0];
353 pdVec[V2] = pd[1];
354 pdVec[V3] = pd[2];
355 };
356
357 /*Operatori */
358
359 /*
360 Operatore di assegnazione
361 */
362 Vec3& operator = (const Vec3& v) {
363 pdVec[V1] = v.pdVec[V1];
364 pdVec[V2] = v.pdVec[V2];
365 pdVec[V3] = v.pdVec[V3];
366
367 return *this;
368 };
369
370 /*
371 Operatore somma.
372 Restituisce v sommato a se stesso in un temporaneo.
373 */
374 Vec3 operator + (const Vec3& v) const {
375 return Vec3(pdVec[V1]+v.pdVec[V1],
376 pdVec[V2]+v.pdVec[V2],
377 pdVec[V3]+v.pdVec[V3]);
378 };
379
380 /*
381 Operatore somma e assegnazione.
382 Somma v a se stesso in loco.
383 */
384 Vec3& operator += (const Vec3& v) {
385 pdVec[V1] += v.pdVec[V1];
386 pdVec[V2] += v.pdVec[V2];
387 pdVec[V3] += v.pdVec[V3];
388 return *this;
389 };
390
391 /*
392 Operatore sottrazione.
393 Sottrae v da se stesso in un temporaneo.
394 */
395 Vec3 operator - (const Vec3& v) const {
396 return Vec3(pdVec[V1]-v.pdVec[V1],
397 pdVec[V2]-v.pdVec[V2],
398 pdVec[V3]-v.pdVec[V3]);
399 };
400
401 /*
402 Operatore sottrazione e assegnazione.
403 Sottrae v da se stesso in loco.
404 */
405 Vec3& operator -= (const Vec3& v) {
406 pdVec[V1] -= v.pdVec[V1];
407 pdVec[V2] -= v.pdVec[V2];
408 pdVec[V3] -= v.pdVec[V3];
409 return *this;
410 };
411
412 /*
413 Operatore prodotto per scalare.
414 Moltiplica se stesso per d in un temporaneo.
415 */
416 Vec3 operator * (const doublereal& d) const {
417 return Vec3(pdVec[V1]*d,
418 pdVec[V2]*d,
419 pdVec[V3]*d);
420 };
421
422 /*
423 Operatore prodotto e assegnazione per scalare.
424 Moltiplica se stesso per d in loco.
425 */
426 Vec3& operator *= (const doublereal& d) {
427 pdVec[V1] *= d;
428 pdVec[V2] *= d;
429 pdVec[V3] *= d;
430 return *this;
431 };
432
433 /*
434 Prodotto scalare.
435 Moltiplica se stesso per v.
436 */
437 doublereal operator * (const Vec3& v) const {
438 return pdVec[V1]*v.pdVec[V1]
439 +pdVec[V2]*v.pdVec[V2]
440 +pdVec[V3]*v.pdVec[V3];
441 };
442
443 /**
444 * Scalar product
445 * multiplies self by matrix m; equivalent to m.Transpose() * this.
446 */
447 Vec3 operator * (const Mat3x3& m) const;
448
449 /*
450 Operatore divisione per scalare.
451 Divide se stesso per d in un temporaneo.
452 */
453 Vec3 operator / (const doublereal& d) const {
454 ASSERT(d != 0.);
455 return Vec3(pdVec[V1]/d,
456 pdVec[V2]/d,
457 pdVec[V3]/d);
458 };
459
460 /*
461 Operatore divisione e assegnazione per scalare.
462 Divide se stesso per d in loco.
463 */
464 Vec3& operator /= (const doublereal& d) {
465 ASSERT(d != 0.);
466 pdVec[V1] /= d;
467 pdVec[V2] /= d;
468 pdVec[V3] /= d;
469 return *this;
470 };
471
IsNull(void)472 bool IsNull(void) const {
473 return (pdVec[V1] == 0. && pdVec[V2] == 0. && pdVec[V3] == 0.);
474 };
475
IsExactlySame(const Vec3 & v)476 bool IsExactlySame(const Vec3& v) const {
477 return (pdVec[V1] == v.pdVec[V1]
478 && pdVec[V2] == v.pdVec[V2]
479 && pdVec[V3] == v.pdVec[V3]);
480 };
481
IsSame(const Vec3 & v,const doublereal & dTol)482 bool IsSame(const Vec3& v, const doublereal& dTol) const {
483 doublereal d2 = 0.;
484
485 for (int i = 0; i < 3; i++) {
486 doublereal d = pdVec[i] - v.pdVec[i];
487 d2 += d*d;
488 }
489
490 return sqrt(d2) <= dTol;
491 };
492
493 void Reset(void);
494
495 /*Input/Output */
496
497 /*
498 Scrive se stesso sull'ostream out.
499 I coefficienti sono separati dalla stringa sFill (spazio di default).
500 */
501 std::ostream& Write(std::ostream& out, const char* sFill = " ") const;
502 };
503
504 /* Vec3 - end */
505
506
507 /* Mat3x3_Manip - begin */
508
509 /* classe virtuale dei manipolatori */
510 /*
511 Manipolatori per la costruzione di matrici 3x3.
512 Sono usati per ottenere le matrici di rotazione e lel loro derivate
513 dai parametri di rotazione
514 */
515 class Mat3x3_Manip {
516 public:
Manipulate(Mat3x3 & m)517 virtual void Manipulate(Mat3x3& m) const {
518 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
519 };
520
Manipulate(Mat3x3 & m,const doublereal d)521 virtual void Manipulate(Mat3x3& m, const doublereal d) const {
522 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
523 };
524
525 /*
526 Metodo che trasforma il vettore v nella matrice m.
527 Viene usato da un costruttore di Mat3x3 che riceve come
528 argomenti un manipolatore e un vettore di parametri di rotazione.
529
530 NOTE: renamed from Make() to Manipulate() May 2009
531 */
Manipulate(Mat3x3 & m,const Vec3 & v)532 virtual void Manipulate(Mat3x3& m, const Vec3& v) const {
533 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
534 };
535
Manipulate(Mat3x3 & m,const Vec3 & v1,const Vec3 & v2)536 virtual void Manipulate(Mat3x3& m, const Vec3& v1, const Vec3& v2) const {
537 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
538 };
539
~Mat3x3_Manip(void)540 virtual ~Mat3x3_Manip(void) {
541 NO_OP;
542 };
543 };
544
545 /* Mat3x3_Manip - end */
546
547
548 /* Mat3x3 - begin */
549 // Matrici 3x3
550 class Mat3x3 {
551 friend class Vec3;
552 friend class SparseSubMatrixHandler;
553 friend class Mat3x3_Manip;
554 friend class Mat3xN;
555 friend class MatNx3;
556
557 protected:
558 //Vettore di 9 reali che contiene i coefficienti
559 doublereal pdMat[9];
560
561 public:
562
563 /*Costruttori */
564 /*
565 Costruttore banale: non inizializza i coefficienti.
566 Per azzerare la matrice usare Zero3x3.
567 */
Mat3x3(void)568 Mat3x3(void) {
569 NO_OP;
570 };
571
572 // replaced by Mat3x3Zero_Manip when passed 0.
573 // replaced by Mat3x3DEye_Manip when passed a nonzero.
574 #if 0
575 Mat3x3(const doublereal& d);
576 #endif
577
578 /*
579 Costrutture completo.
580 */
Mat3x3(const doublereal & m11,const doublereal & m21,const doublereal & m31,const doublereal & m12,const doublereal & m22,const doublereal & m32,const doublereal & m13,const doublereal & m23,const doublereal & m33)581 Mat3x3(const doublereal& m11, const doublereal& m21, const doublereal& m31,
582 const doublereal& m12, const doublereal& m22, const doublereal& m32,
583 const doublereal& m13, const doublereal& m23, const doublereal& m33) {
584 pdMat[M11] = m11;
585 pdMat[M21] = m21;
586 pdMat[M31] = m31;
587 pdMat[M12] = m12;
588 pdMat[M22] = m22;
589 pdMat[M32] = m32;
590 pdMat[M13] = m13;
591 pdMat[M23] = m23;
592 pdMat[M33] = m33;
593 };
594
595 /*
596 Costruttore di copia.
597 */
Mat3x3(const Mat3x3 & m)598 Mat3x3(const Mat3x3& m) {
599 pdMat[M11] = m.pdMat[M11];
600 pdMat[M21] = m.pdMat[M21];
601 pdMat[M31] = m.pdMat[M31];
602 pdMat[M12] = m.pdMat[M12];
603 pdMat[M22] = m.pdMat[M22];
604 pdMat[M32] = m.pdMat[M32];
605 pdMat[M13] = m.pdMat[M13];
606 pdMat[M23] = m.pdMat[M23];
607 pdMat[M33] = m.pdMat[M33];
608 };
609
610 #if 0
611 // will be replaced by MatCross_Manip
612 /*
613 Costruttore prodotto vettore.
614 La matrice viene inizializzata con il vettore v disposto a dare
615 la matrice prodotto vettore
616 (ovvero la matrice che, moltiplicata per w, da' v vettor w).
617 */
618 Mat3x3(const Vec3& v) {
619 pdMat[M11] = 0.;
620 pdMat[M21] = v.pdVec[V3];
621 pdMat[M31] = -v.pdVec[V2];
622 pdMat[M12] = -v.pdVec[V3];
623 pdMat[M22] = 0.;
624 pdMat[M32] = v.pdVec[V1];
625 pdMat[M13] = v.pdVec[V2];
626 pdMat[M23] = -v.pdVec[V1];
627 pdMat[M33] = 0.;
628 };
629 #endif
630
631 #if 0
632 // will be replaced by MatCrossCross_Manip
633 /*
634 Costruttore doppio prodotto vettore.
635 restituisce la matrice data dal prodotto di due matrici prodotto vettore.
636 */
637 Mat3x3(const Vec3& a, const Vec3& b) {
638 pdMat[M11] = -a.pdVec[V2]*b.pdVec[V2]-a.pdVec[V3]*b.pdVec[V3];
639 pdMat[M21] = a.pdVec[V1]*b.pdVec[V2];
640 pdMat[M31] = a.pdVec[V1]*b.pdVec[V3];
641 pdMat[M12] = a.pdVec[V2]*b.pdVec[V1];
642 pdMat[M22] = -a.pdVec[V3]*b.pdVec[V3]-a.pdVec[V1]*b.pdVec[V1];
643 pdMat[M32] = a.pdVec[V2]*b.pdVec[V3];
644 pdMat[M13] = a.pdVec[V3]*b.pdVec[V1];
645 pdMat[M23] = a.pdVec[V3]*b.pdVec[V2];
646 pdMat[M33] = -a.pdVec[V1]*b.pdVec[V1]-a.pdVec[V2]*b.pdVec[V2];
647 };
648 #endif
649
650 /*
651 Costruttore che piglia tre vettori e li affianca a dare la matrice
652 */
Mat3x3(const Vec3 & v1,const Vec3 & v2,const Vec3 & v3)653 Mat3x3(const Vec3& v1, const Vec3& v2, const Vec3& v3) {
654 pdMat[M11] = v1.pdVec[V1];
655 pdMat[M21] = v1.pdVec[V2];
656 pdMat[M31] = v1.pdVec[V3];
657
658 pdMat[M12] = v2.pdVec[V1];
659 pdMat[M22] = v2.pdVec[V2];
660 pdMat[M32] = v2.pdVec[V3];
661
662 pdMat[M13] = v3.pdVec[V1];
663 pdMat[M23] = v3.pdVec[V2];
664 pdMat[M33] = v3.pdVec[V3];
665 };
666
667
668 /*
669 Costruttore di copia da array.
670 si assume che l'array pd sia lungo almeno 9
671 */
Mat3x3(const doublereal * pd,integer iSize)672 Mat3x3(const doublereal* pd, integer iSize) {
673 ASSERT(pd != NULL);
674 GetFrom(pd, iSize);
675 };
676
677 /*
678 Costruttore con manipolatore.
679 Invoca un metodo del manipolatore Manip che restituisce una matrice
680 a partire dal nulla.
681 */
Mat3x3(const Mat3x3_Manip & Manip)682 Mat3x3(const Mat3x3_Manip& Manip) {
683 Manip.Manipulate(*this);
684 };
685
686 /*
687 Costruttore con manipolatore.
688 Invoca un metodo del manipolatore Manip che restituisce una matrice
689 a partire dal reale d.
690 */
Mat3x3(const Mat3x3_Manip & Manip,const doublereal d)691 Mat3x3(const Mat3x3_Manip& Manip, const doublereal d) {
692 Manip.Manipulate(*this, d);
693 };
694
695 /*
696 Costruttore con manipolatore.
697 Invoca un metodo del manipolatore Manip che restituisce una matrice
698 a partire dal vettore v.
699 */
Mat3x3(const Mat3x3_Manip & Manip,const Vec3 & v)700 Mat3x3(const Mat3x3_Manip& Manip, const Vec3& v) {
701 Manip.Manipulate(*this, v);
702 };
703
704 /*
705 Costruttore con manipolatore.
706 Invoca un metodo del manipolatore Manip che restituisce una matrice
707 a partire dai vettori v1 e v2.
708 */
Mat3x3(const Mat3x3_Manip & Manip,const Vec3 & v1,const Vec3 & v2)709 Mat3x3(const Mat3x3_Manip& Manip, const Vec3& v1, const Vec3& v2) {
710 Manip.Manipulate(*this, v1, v2);
711 };
712
713 // FIXME: is this "safe"?
714 /*
715 Costruttore che genera la matrice identita' moltiplicata per d e sommata
716 alla matrice prodotto vettore ottenuta da v.
717 */
Mat3x3(const doublereal & d,const Vec3 & v)718 Mat3x3(const doublereal& d, const Vec3& v) {
719 pdMat[M11] = d;
720 pdMat[M21] = v.pdVec[V3];
721 pdMat[M31] = -v.pdVec[V2];
722 pdMat[M12] = -v.pdVec[V3];
723 pdMat[M22] = d;
724 pdMat[M32] = v.pdVec[V1];
725 pdMat[M13] = v.pdVec[V2];
726 pdMat[M23] = -v.pdVec[V1];
727 pdMat[M33] = d;
728 };
729
730 /*
731 Distruttore banale.
732 */
~Mat3x3(void)733 ~Mat3x3(void) {
734 NO_OP;
735 };
736
737
738 /*Metodi di servizio */
739
740 /*
741 Dirty job: restituisce il puntatore alla matrice (deprecato).
742 */
pGetMat(void)743 const doublereal* pGetMat(void) const {
744 return pdMat;
745 };
746
pGetMat(void)747 doublereal* pGetMat(void) {
748 return pdMat;
749 };
750
751
752 /*Operazioni sui coefficienti */
753
754 /*
755 Assegnazione di un coefficiente.
756 Nota: gli indici hanno base 1, in stile FORTRAN.
757 */
Put(unsigned short int iRow,unsigned short int iCol,const doublereal & dCoef)758 inline void Put(unsigned short int iRow,
759 unsigned short int iCol,
760 const doublereal& dCoef) {
761 ASSERT(iRow >= 1 && iRow <= 3);
762 ASSERT(iCol >= 1 && iCol <= 3);
763 pdMat[--iRow+3*--iCol] = dCoef;
764 };
765
766 /*
767 Lettura di un coefficiente.
768 Nota: gli indici hanno base 1, in stile FORTRAN.
769 */
dGet(unsigned short int iRow,unsigned short int iCol)770 inline const doublereal& dGet(unsigned short int iRow,
771 unsigned short int iCol) const {
772 ASSERT(iRow >= 1 && iRow <= 3);
773 ASSERT(iCol >= 1 && iCol <= 3);
774 return pdMat[--iRow+3*--iCol];
775 };
776
operator()777 inline doublereal& operator () (unsigned short int iRow,
778 unsigned short int iCol) {
779 ASSERT(iRow >= 1 && iRow <= 3);
780 ASSERT(iCol >= 1 && iCol <= 3);
781 return pdMat[--iRow+3*--iCol];
782 };
783
operator()784 inline const doublereal& operator () (unsigned short int iRow,
785 unsigned short int iCol) const {
786 ASSERT(iRow >= 1 && iRow <= 3);
787 ASSERT(iCol >= 1 && iCol <= 3);
788 return pdMat[--iRow+3*--iCol];
789 };
790
791
792 /*Operazioni su matrici e vettori */
793
794 /*
795 Prodotto di matrici prodotto vettore.
796 Setta se stessa pari al prodotto delle matrici prodotto vettore
797 ottenute dai vettori a e b.
798 */
Tens(const Vec3 & a,const Vec3 & b)799 const Mat3x3& Tens(const Vec3& a, const Vec3& b) {
800 pdMat[M11] = a.pdVec[V1]*b.pdVec[V1]; /* m(1,1) = a(1)*b(1) */
801 pdMat[M21] = a.pdVec[V2]*b.pdVec[V1]; /* m(2,1) = a(2)*b(1) */
802 pdMat[M31] = a.pdVec[V3]*b.pdVec[V1]; /* m(3,1) = a(3)*b(1) */
803 pdMat[M12] = a.pdVec[V1]*b.pdVec[V2]; /* m(1,2) = a(1)*b(2) */
804 pdMat[M22] = a.pdVec[V2]*b.pdVec[V2]; /* m(2,2) = a(2)*b(2) */
805 pdMat[M32] = a.pdVec[V3]*b.pdVec[V2]; /* m(3,2) = a(3)*b(2) */
806 pdMat[M13] = a.pdVec[V1]*b.pdVec[V3]; /* m(1,3) = a(1)*b(3) */
807 pdMat[M23] = a.pdVec[V2]*b.pdVec[V3]; /* m(2,3) = a(2)*b(3) */
808 pdMat[M33] = a.pdVec[V3]*b.pdVec[V3]; /* m(3,3) = a(3)*b(3) */
809 return *this;
810 };
811
812 /*
813 Traspone la matrice.
814 Restituisce la trasposta di se stessa in un temporaneo (poco efficiente).
815 */
Transpose(void)816 Mat3x3 Transpose(void) const {
817 /* La funzione non e' ottimizzata. Genera una nuova matrice che
818 * costruisce in modo opportuno. Se si ritiene di dover usare
819 * ripetutamente la matrice trasposta, conviene memorizzarla in
820 * una variabile di servizio */
821
822 return Mat3x3(pdMat[M11], pdMat[M12], pdMat[M13],
823 pdMat[M21], pdMat[M22], pdMat[M23],
824 pdMat[M31], pdMat[M32], pdMat[M33]);
825 };
826
Ax(void)827 Vec3 Ax(void) const {
828 /* assumendo che sia una matrice di rotazione ?!? */
829 return Vec3(
830 .5*(pdMat[M32]-pdMat[M23]),
831 .5*(pdMat[M13]-pdMat[M31]),
832 .5*(pdMat[M21]-pdMat[M12])
833 );
834 };
835
Trace(void)836 doublereal Trace(void) const {
837 return pdMat[M11]+pdMat[M22]+pdMat[M33];
838 };
839
Symm(void)840 Mat3x3 Symm(void) const {
841 doublereal m12 = .5*(pdMat[M21]+pdMat[M12]);
842 doublereal m13 = .5*(pdMat[M31]+pdMat[M13]);
843 doublereal m23 = .5*(pdMat[M32]+pdMat[M23]);
844
845 return Mat3x3(
846 pdMat[M11], m12, m13,
847 m12, pdMat[M22], m23,
848 m13, m23, pdMat[M33]
849 );
850 };
851
Symm2(void)852 Mat3x3 Symm2(void) const {
853 doublereal m12 = pdMat[M21]+pdMat[M12];
854 doublereal m13 = pdMat[M31]+pdMat[M13];
855 doublereal m23 = pdMat[M32]+pdMat[M23];
856
857 return Mat3x3(
858 2.*pdMat[M11], m12, m13,
859 m12, 2.*pdMat[M22], m23,
860 m13, m23, 2.*pdMat[M33]
861 );
862 };
863
Symm(const Mat3x3 & m)864 const Mat3x3& Symm(const Mat3x3& m) {
865 pdMat[M11] = m.pdMat[M11];
866 pdMat[M22] = m.pdMat[M22];
867 pdMat[M33] = m.pdMat[M33];
868 pdMat[M12] = pdMat[M21] = .5*(m.pdMat[M21]+m.pdMat[M12]);
869 pdMat[M13] = pdMat[M31] = .5*(m.pdMat[M31]+m.pdMat[M13]);
870 pdMat[M23] = pdMat[M32] = .5*(m.pdMat[M32]+m.pdMat[M23]);
871
872 return *this;
873 };
874
875 Mat3x3 Skew(void) const;
876
Skew(const Mat3x3 & m)877 const Mat3x3& Skew(const Mat3x3& m) {
878 pdMat[M11] = pdMat[M22] = pdMat[M13] = 0.;
879 pdMat[M12] = m.pdMat[M12] - m.pdMat[M21];
880 pdMat[M21] = -pdMat[M12];
881 pdMat[M13] = m.pdMat[M13] - m.pdMat[M31];
882 pdMat[M31] = -pdMat[M13];
883 pdMat[M23] = m.pdMat[M23] - m.pdMat[M32];
884 pdMat[M32] = -pdMat[M23];
885
886 return *this;
887 };
888
889 /*
890 Ottiene un sottovettore dalla matrice.
891 Nota: l'indice e' a base 1, in stile FORTRAN.
892 */
GetVec(unsigned short int i)893 Vec3 GetVec(unsigned short int i) const {
894 ASSERT(i >= 1 && i <= 3);
895 return Vec3(pdMat+3*--i);
896 };
897
898 /*
899 Ottiene un sottovettore dalla matrice.
900 Nota: l'indice e' a base 1, in stile FORTRAN.
901 Alias di GetVec()
902 */
GetCol(unsigned short int i)903 Vec3 GetCol(unsigned short int i) const {
904 ASSERT(i >= 1 && i <= 3);
905 return Vec3(pdMat + 3*--i);
906 };
907
908 /*
909 Ottiene un sottovettore dalla matrice.
910 Nota: l'indice e' a base 1, in stile FORTRAN.
911 */
GetRow(unsigned short int i)912 Vec3 GetRow(unsigned short int i) const {
913 ASSERT(i >= 1 && i <= 3);
914 --i;
915 return Vec3(pdMat[i], pdMat[3 + i], pdMat[6 + i]);
916 };
917
PutVec(unsigned short int i,const Vec3 & v)918 void PutVec(unsigned short int i, const Vec3& v) {
919 ASSERT(i >= 1 && i <= 3);
920
921 i--; i = 3*i;
922 pdMat[i++] = v.pdVec[V1];
923 pdMat[i++] = v.pdVec[V2];
924 pdMat[i] = v.pdVec[V3];
925 };
926
AddVec(unsigned short int i,const Vec3 & v)927 void AddVec(unsigned short int i, const Vec3& v) {
928 ASSERT(i >= 1 && i <= 3);
929
930 i--; i = 3*i;
931 pdMat[i++] += v.pdVec[V1];
932 pdMat[i++] += v.pdVec[V2];
933 pdMat[i] += v.pdVec[V3];
934 };
935
SubVec(unsigned short int i,const Vec3 & v)936 void SubVec(unsigned short int i, const Vec3& v) {
937 ASSERT(i >= 1 && i <= 3);
938
939 i--; i = 3*i;
940 pdMat[i++] -= v.pdVec[V1];
941 pdMat[i++] -= v.pdVec[V2];
942 pdMat[i] -= v.pdVec[V3];
943 };
944
945 /*
946 Inversione.
947 Restituisce l'inversa di se stessa in un temporaneo.
948 */
949 doublereal dDet(void) const;
950 Mat3x3 Inv(const doublereal& ddet) const;
951 Mat3x3 Inv(void) const;
952
953 /*
954 Soluzione.
955 Restituisce l'inversa di se stessa per v in un temporaneo.
956 */
957 Vec3 Solve(const Vec3& v) const;
958 Vec3 Solve(const doublereal& d, const Vec3& v) const;
959
960 Vec3 LDLSolve(const Vec3& v) const;
961
962 /*
963 Eigenvalues
964 */
965 bool EigSym(Vec3& EigenValues) const;
966 bool EigSym(Vec3& EigenValues, Mat3x3& EigenVectors) const;
967
968 /*Operazioni su arrays di reali */
969
970 /*
971 Si legge da un array.
972 Si assume che l'array pd sia lungo almeno 9.
973 @param iSize e' il numero di righe di dimensionamento dell'array,
974 in stile FORTRAN
975 */
GetFrom(const doublereal * pd,integer iSize)976 void GetFrom(const doublereal* pd, integer iSize) {
977 ASSERT(pd != NULL);
978 ASSERT(iSize >= 3);
979
980 doublereal* pdFrom = (doublereal*)pd;
981 pdMat[M11] = pdFrom[0];
982 pdMat[M21] = pdFrom[1];
983 pdMat[M31] = pdFrom[2];
984
985 pdFrom += iSize;
986 pdMat[M12] = pdFrom[0];
987 pdMat[M22] = pdFrom[1];
988 pdMat[M32] = pdFrom[2];
989
990 pdFrom += iSize;
991 pdMat[M13] = pdFrom[0];
992 pdMat[M23] = pdFrom[1];
993 pdMat[M33] = pdFrom[2];
994 };
995
996 /*
997 Somma se stesso ad un array con iNRows righe.
998 si assume che l'array sia lungo almeno 2*iNRows+3 a partire da pd.
999 @param iNRows e' il numero di righe di dimensionamento dell'array,
1000 in stile FORTRAN
1001 */
AddTo(doublereal * pd,integer iNRows)1002 void AddTo(doublereal* pd, integer iNRows) const {
1003 ASSERT(pd != NULL);
1004 ASSERT(iNRows >= 3);
1005
1006 doublereal* pdTo = pd;
1007
1008 pdTo[0] += pdMat[M11];
1009 pdTo[1] += pdMat[M21];
1010 pdTo[2] += pdMat[M31];
1011
1012 pdTo += iNRows;
1013 pdTo[0] += pdMat[M12];
1014 pdTo[1] += pdMat[M22];
1015 pdTo[2] += pdMat[M32];
1016
1017 pdTo += iNRows;
1018 pdTo[0] += pdMat[M13];
1019 pdTo[1] += pdMat[M23];
1020 pdTo[2] += pdMat[M33];
1021 };
1022
1023 /*
1024 Copia se stesso su un array con iNRows righe.
1025 Si assume che l'array sia lungo almeno 2*iNRows+3.
1026 @param iNRows e' il numero di righe di dimensionamento dell'array,
1027 in stile FORTRAN
1028 */
PutTo(doublereal * pd,integer iNRows)1029 void PutTo(doublereal* pd, integer iNRows) const {
1030 ASSERT(pd != NULL);
1031 ASSERT(iNRows >= 3);
1032
1033 doublereal* pdTo = pd;
1034
1035 pdTo[0] = pdMat[M11];
1036 pdTo[1] = pdMat[M21];
1037 pdTo[2] = pdMat[M31];
1038
1039 pdTo += iNRows;
1040 pdTo[0] = pdMat[M12];
1041 pdTo[1] = pdMat[M22];
1042 pdTo[2] = pdMat[M32];
1043
1044 pdTo += iNRows;
1045 pdTo[0] = pdMat[M13];
1046 pdTo[1] = pdMat[M23];
1047 pdTo[2] = pdMat[M33];
1048 };
1049
1050 /*Operatori */
1051
1052 /*
1053 Operatore di assegnazione.
1054 */
1055 Mat3x3& operator = (const Mat3x3& m) {
1056
1057 pdMat[M11] = m.pdMat[M11];
1058 pdMat[M21] = m.pdMat[M21];
1059 pdMat[M31] = m.pdMat[M31];
1060 pdMat[M12] = m.pdMat[M12];
1061 pdMat[M22] = m.pdMat[M22];
1062 pdMat[M32] = m.pdMat[M32];
1063 pdMat[M13] = m.pdMat[M13];
1064 pdMat[M23] = m.pdMat[M23];
1065 pdMat[M33] = m.pdMat[M33];
1066
1067 return *this;
1068 };
1069
1070 /*
1071 Operatore somma.
1072 Restituisce v sommato a se stesso in un temporaneo.
1073 */
1074 Mat3x3 operator + (const Mat3x3& m) const {
1075 return Mat3x3(pdMat[M11]+m.pdMat[M11],
1076 pdMat[M21]+m.pdMat[M21],
1077 pdMat[M31]+m.pdMat[M31],
1078 pdMat[M12]+m.pdMat[M12],
1079 pdMat[M22]+m.pdMat[M22],
1080 pdMat[M32]+m.pdMat[M32],
1081 pdMat[M13]+m.pdMat[M13],
1082 pdMat[M23]+m.pdMat[M23],
1083 pdMat[M33]+m.pdMat[M33]);
1084 };
1085
1086 /*
1087 Operatore somma e assegnazione.
1088 Somma v a se stesso in loco.
1089 */
1090 Mat3x3& operator += (const Mat3x3& m) {
1091 pdMat[M11] += m.pdMat[M11];
1092 pdMat[M21] += m.pdMat[M21];
1093 pdMat[M31] += m.pdMat[M31];
1094 pdMat[M12] += m.pdMat[M12];
1095 pdMat[M22] += m.pdMat[M22];
1096 pdMat[M32] += m.pdMat[M32];
1097 pdMat[M13] += m.pdMat[M13];
1098 pdMat[M23] += m.pdMat[M23];
1099 pdMat[M33] += m.pdMat[M33];
1100
1101 return *this;
1102 };
1103
1104 /*
1105 Operatore differenza.
1106 Restituisce v sottratto da se stesso in un temporaneo.
1107 */
1108 Mat3x3 operator - (const Mat3x3& m) const {
1109 return Mat3x3(pdMat[M11]-m.pdMat[M11],
1110 pdMat[M21]-m.pdMat[M21],
1111 pdMat[M31]-m.pdMat[M31],
1112 pdMat[M12]-m.pdMat[M12],
1113 pdMat[M22]-m.pdMat[M22],
1114 pdMat[M32]-m.pdMat[M32],
1115 pdMat[M13]-m.pdMat[M13],
1116 pdMat[M23]-m.pdMat[M23],
1117 pdMat[M33]-m.pdMat[M33]);
1118 };
1119
1120 /*
1121 Operatore differenza e assegnazione.
1122 Sottrae v da se stesso in loco.
1123 */
1124 Mat3x3& operator -= (const Mat3x3& m) {
1125 pdMat[M11] -= m.pdMat[M11];
1126 pdMat[M21] -= m.pdMat[M21];
1127 pdMat[M31] -= m.pdMat[M31];
1128 pdMat[M12] -= m.pdMat[M12];
1129 pdMat[M22] -= m.pdMat[M22];
1130 pdMat[M32] -= m.pdMat[M32];
1131 pdMat[M13] -= m.pdMat[M13];
1132 pdMat[M23] -= m.pdMat[M23];
1133 pdMat[M33] -= m.pdMat[M33];
1134
1135 return *this;
1136 };
1137
1138 /*
1139 Operatore moltiplicazione per scalare.
1140 Restituisce se stesso moltiplicato per d in un temporaneo.
1141 */
1142 Mat3x3 operator * (const doublereal& d) const {
1143 if (d != 1.) {
1144 return Mat3x3(pdMat[M11]*d,
1145 pdMat[M21]*d,
1146 pdMat[M31]*d,
1147 pdMat[M12]*d,
1148 pdMat[M22]*d,
1149 pdMat[M32]*d,
1150 pdMat[M13]*d,
1151 pdMat[M23]*d,
1152 pdMat[M33]*d);
1153 }
1154 return *this;
1155 };
1156
1157 /*
1158 Operatore moltiplicazione per scalare e assegnazione.
1159 Moltiplica se stesso per d in loco.
1160 */
1161 Mat3x3& operator *= (const doublereal& d) {
1162 if (d != 1.) {
1163 pdMat[M11] *= d;
1164 pdMat[M21] *= d;
1165 pdMat[M31] *= d;
1166 pdMat[M12] *= d;
1167 pdMat[M22] *= d;
1168 pdMat[M32] *= d;
1169 pdMat[M13] *= d;
1170 pdMat[M23] *= d;
1171 pdMat[M33] *= d;
1172 }
1173
1174 return *this;
1175 };
1176
1177 /*
1178 Operatore divisione per scalare.
1179 Restituisce se stesso diviso per d in un temporaneo.
1180 */
1181 Mat3x3 operator / (const doublereal& d) const {
1182 ASSERT(d != 0.);
1183 if (d != 1.) {
1184 return Mat3x3(pdMat[M11]/d,
1185 pdMat[M21]/d,
1186 pdMat[M31]/d,
1187 pdMat[M12]/d,
1188 pdMat[M22]/d,
1189 pdMat[M32]/d,
1190 pdMat[M13]/d,
1191 pdMat[M23]/d,
1192 pdMat[M33]/d);
1193 }
1194
1195 return *this;
1196 };
1197
1198 /*
1199 Operatore divisione per scalare e assegnazione.
1200 Divide se stesso per d in loco
1201 */
1202 Mat3x3& operator /= (const doublereal& d) {
1203 ASSERT(d != 0.);
1204 if (d != 1.) {
1205 pdMat[M11] /= d;
1206 pdMat[M21] /= d;
1207 pdMat[M31] /= d;
1208 pdMat[M12] /= d;
1209 pdMat[M22] /= d;
1210 pdMat[M32] /= d;
1211 pdMat[M13] /= d;
1212 pdMat[M23] /= d;
1213 pdMat[M33] /= d;
1214 }
1215
1216 return *this;
1217 };
1218
1219
1220 /*
1221 Operatore prodotto matrice vettore.
1222 Restituisce se stesso moltiplicato per v in un temporaneo.
1223 */
1224 Vec3 operator * (const Vec3& v) const {
1225
1226 return Vec3(pdMat[M11]*v.pdVec[V1]+pdMat[M12]*v.pdVec[V2]+pdMat[M13]*v.pdVec[V3],
1227 pdMat[M21]*v.pdVec[V1]+pdMat[M22]*v.pdVec[V2]+pdMat[M23]*v.pdVec[V3],
1228 pdMat[M31]*v.pdVec[V1]+pdMat[M32]*v.pdVec[V2]+pdMat[M33]*v.pdVec[V3]);
1229 };
1230
IsNull(void)1231 bool IsNull(void) const {
1232 return (pdMat[M11] == 0. && pdMat[M12] == 0. && pdMat[M13] == 0.
1233 && pdMat[M21] == 0. && pdMat[M22] == 0. && pdMat[M23] == 0.
1234 && pdMat[M31] == 0. && pdMat[M32] == 0. && pdMat[M33] == 0.);
1235 };
1236
IsExactlySame(const Mat3x3 & m)1237 bool IsExactlySame(const Mat3x3& m) const {
1238 return (pdMat[M11] == m.pdMat[M11]
1239 && pdMat[M12] == m.pdMat[M12]
1240 && pdMat[M13] == m.pdMat[M13]
1241 && pdMat[M21] == m.pdMat[M21]
1242 && pdMat[M22] == m.pdMat[M22]
1243 && pdMat[M23] == m.pdMat[M23]
1244 && pdMat[M31] == m.pdMat[M31]
1245 && pdMat[M32] == m.pdMat[M32]
1246 && pdMat[M33] == m.pdMat[M33]);
1247 };
1248
IsSame(const Mat3x3 & m,const doublereal & dTol)1249 bool IsSame(const Mat3x3& m, const doublereal& dTol) const {
1250 doublereal d2 = 0.;
1251
1252 for (int i = 0; i < 9; i++) {
1253 doublereal d = pdMat[i] - m.pdMat[i];
1254 d2 += d*d;
1255 }
1256
1257 return sqrt(d2) <= dTol;
1258 };
1259
IsSymmetric(void)1260 bool IsSymmetric(void) const {
1261 if (pdMat[M12] != pdMat[M21]
1262 || pdMat[M13] != pdMat[M31]
1263 || pdMat[M23] != pdMat[M32])
1264 {
1265 return false;
1266 }
1267
1268 return true;
1269 };
1270
IsSymmetric(const doublereal & dTol)1271 bool IsSymmetric(const doublereal& dTol) const {
1272 ASSERT(dTol > 0.);
1273
1274 if (fabs(pdMat[M12] - pdMat[M21]) > dTol
1275 || fabs(pdMat[M13] - pdMat[M31]) > dTol
1276 || fabs(pdMat[M23] - pdMat[M32]) > dTol)
1277 {
1278 return false;
1279 }
1280
1281 return true;
1282 };
1283
IsDiag(void)1284 bool IsDiag(void) const {
1285 if (pdMat[M12] != 0.
1286 || pdMat[M21] != 0.
1287 || pdMat[M13] != 0.
1288 || pdMat[M31] != 0.
1289 || pdMat[M23] != 0.
1290 || pdMat[M32] != 0.)
1291 {
1292 return false;
1293 }
1294
1295 return true;
1296 };
1297
IsDiag(const doublereal & dTol)1298 bool IsDiag(const doublereal& dTol) const {
1299 ASSERT(dTol > 0.);
1300
1301 if (fabs(pdMat[M12]) > dTol
1302 || fabs(pdMat[M21]) > dTol
1303 || fabs(pdMat[M13]) > dTol
1304 || fabs(pdMat[M31]) > dTol
1305 || fabs(pdMat[M23]) > dTol
1306 || fabs(pdMat[M32]) > dTol)
1307 {
1308 return false;
1309 }
1310
1311 return true;
1312 };
1313
1314 /*
1315 Prodotto matrice per matrice.
1316 Restituisce il prodotto di se stessa per m in un temporaneo.
1317 */
1318 Mat3x3 operator * (const Mat3x3& m) const
1319 {
1320 return Mat3x3(pdMat[M11]*m.pdMat[M11]+pdMat[M12]*m.pdMat[M21]+pdMat[M13]*m.pdMat[M31],
1321 pdMat[M21]*m.pdMat[M11]+pdMat[M22]*m.pdMat[M21]+pdMat[M23]*m.pdMat[M31],
1322 pdMat[M31]*m.pdMat[M11]+pdMat[M32]*m.pdMat[M21]+pdMat[M33]*m.pdMat[M31],
1323 pdMat[M11]*m.pdMat[M12]+pdMat[M12]*m.pdMat[M22]+pdMat[M13]*m.pdMat[M32],
1324 pdMat[M21]*m.pdMat[M12]+pdMat[M22]*m.pdMat[M22]+pdMat[M23]*m.pdMat[M32],
1325 pdMat[M31]*m.pdMat[M12]+pdMat[M32]*m.pdMat[M22]+pdMat[M33]*m.pdMat[M32],
1326 pdMat[M11]*m.pdMat[M13]+pdMat[M12]*m.pdMat[M23]+pdMat[M13]*m.pdMat[M33],
1327 pdMat[M21]*m.pdMat[M13]+pdMat[M22]*m.pdMat[M23]+pdMat[M23]*m.pdMat[M33],
1328 pdMat[M31]*m.pdMat[M13]+pdMat[M32]*m.pdMat[M23]+pdMat[M33]*m.pdMat[M33]);
1329 };
1330
1331 /**
1332 * multiply by another matrix, transposed: this * m^T
1333 */
1334 Mat3x3 MulMT(const Mat3x3& m) const;
1335
1336 /**
1337 * multiply self transposed by a vector: this^T * v
1338 */
1339 Vec3 MulTV(const Vec3& v) const;
1340
1341 /**
1342 * multiply self transposed by another matrix: this^T * m
1343 */
1344 Mat3x3 MulTM(const Mat3x3& m) const;
1345
1346 /**
1347 * multiply self transposed by another matrix, transposed: this^T * m^T
1348 */
1349 Mat3x3 MulTMT(const Mat3x3& m) const;
1350
1351 /**
1352 * multiply self times vCross
1353 */
1354 Mat3x3 MulVCross(const Vec3& v) const;
1355
1356 /**
1357 * multiply self transposed times vCross
1358 */
1359 Mat3x3 MulTVCross(const Vec3& v) const;
1360
Tr(void)1361 doublereal Tr(void) const {
1362 return pdMat[M11]+pdMat[M22]+pdMat[M33];
1363 };
1364
1365 void Reset(void);
1366
1367 /*Input/Output */
1368
1369 /*
1370 Scrittura su ostream della matrice.
1371 Scrive se stessa sull'ostream out usando come separatore tra colonne sFill
1372 e come separatore tra le righe sFill2.
1373 */
1374 std::ostream& Write(std::ostream& out,
1375 const char* sFill = " ",
1376 const char* sFill2 = NULL) const;
1377 };
1378
1379 /* Mat3x3 - end */
1380
1381 /*Operazioni esterne su Vec3 e Mat3x3 */
1382
1383 /*
1384 Operatore "meno" unario su Vec3.
1385 Restituisce l'opposto di se stesso in un temporaneo.
1386 */
1387 extern Vec3 operator - (const Vec3& v);
1388
1389 /*
1390 Operatore "meno" unario su Mat3x3.
1391 Restituisce l'opposto di se stesso in un temporaneo.
1392 */
1393 extern Mat3x3 operator - (const Mat3x3& v);
1394
1395 /*
1396 Operatore di scrittura di Vec3 su ostream.
1397 Nota: i coefficienti sono separati da spazi. Non c'e' endl al termine
1398 */
1399 extern std::ostream& operator << (std::ostream& out, const Vec3& v);
1400
1401 /*
1402 Operatore di scrittura di Mat3x3 su ostream.
1403 Nota: i coefficienti sono separati da spazi e sono scritti consecutivamente,
1404 per righe. Non c'e' endl al termine.
1405 */
1406 extern std::ostream& operator << (std::ostream& out, const Mat3x3& m);
1407
1408
1409 /*
1410 Funzione di Output di reali su ostream.
1411 Necessarie per poter generare i templates dei legami costitutivi.
1412 Il terzo parametro e' definito solo per compatibilita'.
1413 @param out ostream su cui avviene la scrittura.
1414 @param d valore da scrivere.
1415 */
1416 extern std::ostream& Write(std::ostream& out, const doublereal& d, const char*);
1417
1418 /*
1419 Funzione di Output di Vec3 su ostream.
1420 Necessarie per poter generare i templates dei legami costitutivi.
1421 @param out ostream su cui avviene la scrittura.
1422 @param v vettore da scrivere.
1423 @param s separatore tra i valori.
1424 */
1425 extern std::ostream& Write(std::ostream& out, const Vec3& v, const char* s = " ");
1426
1427 /*
1428 Funzione di Output di Mat3x3 su ostream.
1429 Necessarie per poter generare i templates dei legami costitutivi.
1430 @param out ostream su cui avviene la scrittura.
1431 @param d valore da scrivere.
1432 @param s separatore tra i valori (colonne).
1433 @param s2 separatore tra i valori (righe); se nullo, usa s.
1434 */
1435 extern std::ostream& Write(std::ostream& out,
1436 const Mat3x3& m,
1437 const char* s = " ",
1438 const char* s2 = NULL);
1439
1440 // replaces Mat3x3(const doublereal&) used as Mat3x3(0.)
1441 class Mat3x3Zero_Manip : public Mat3x3_Manip {
1442 public:
Mat3x3Zero_Manip()1443 Mat3x3Zero_Manip() {};
Manipulate(Mat3x3 & m)1444 inline void Manipulate(Mat3x3& m) const {
1445 doublereal *pdm = m.pGetMat();
1446
1447 pdm[M11] = 0.;
1448 pdm[M12] = 0.;
1449 pdm[M13] = 0.;
1450 pdm[M21] = 0.;
1451 pdm[M22] = 0.;
1452 pdm[M23] = 0.;
1453 pdm[M31] = 0.;
1454 pdm[M32] = 0.;
1455 pdm[M33] = 0.;
1456 };
1457 };
1458
1459 // replaces Mat3x3(const doublereal&) used as Mat3x3(d) with d != 0.
1460 class Mat3x3DEye_Manip : public Mat3x3_Manip {
1461 public:
Mat3x3DEye_Manip()1462 Mat3x3DEye_Manip() {};
Manipulate(Mat3x3 & m,const doublereal d)1463 inline void Manipulate(Mat3x3& m, const doublereal d) const {
1464 doublereal *pdm = m.pGetMat();
1465
1466 pdm[M11] = d;
1467 pdm[M12] = 0.;
1468 pdm[M13] = 0.;
1469 pdm[M21] = 0.;
1470 pdm[M22] = d;
1471 pdm[M23] = 0.;
1472 pdm[M31] = 0.;
1473 pdm[M32] = 0.;
1474 pdm[M33] = d;
1475 };
1476 };
1477
1478 class Mat3x3Diag_Manip : public Mat3x3_Manip {
1479 public:
Mat3x3Diag_Manip()1480 Mat3x3Diag_Manip() {};
Manipulate(Mat3x3 & m,const Vec3 & v)1481 inline void Manipulate(Mat3x3& m, const Vec3& v) const {
1482 doublereal *pdm = m.pGetMat();
1483 const doublereal *pdv = v.pGetVec();
1484
1485 pdm[M11] = pdv[V1];
1486 pdm[M12] = 0.;
1487 pdm[M13] = 0.;
1488 pdm[M21] = 0.;
1489 pdm[M22] = pdv[V2];
1490 pdm[M23] = 0.;
1491 pdm[M31] = 0.;
1492 pdm[M32] = 0.;
1493 pdm[M33] = pdv[V3];
1494 };
1495 };
1496
1497 // will replace Mat3x3(const Vec3&)
1498 class MatCross_Manip : public Mat3x3_Manip {
1499 public:
MatCross_Manip()1500 MatCross_Manip() {};
Manipulate(Mat3x3 & m,const Vec3 & v)1501 inline void Manipulate(Mat3x3& m, const Vec3& v) const {
1502 doublereal *pdm = m.pGetMat();
1503 const doublereal *pdv = v.pGetVec();
1504
1505 pdm[M11] = 0.;
1506 pdm[M12] = -pdv[V3];
1507 pdm[M13] = pdv[V2];
1508 pdm[M21] = pdv[V3];
1509 pdm[M22] = 0.;
1510 pdm[M23] = -pdv[V1];
1511 pdm[M31] = -pdv[V2];
1512 pdm[M32] = pdv[V1];
1513 pdm[M33] = 0.;
1514 };
1515 };
1516
1517 // will replace Mat3x3(const Vec3&, const Vec3&)
1518 class MatCrossCross_Manip : public Mat3x3_Manip {
1519 public:
MatCrossCross_Manip()1520 MatCrossCross_Manip() {};
Manipulate(Mat3x3 & m,const Vec3 & v1,const Vec3 & v2)1521 inline void Manipulate(Mat3x3& m, const Vec3& v1, const Vec3& v2) const {
1522 doublereal *pdm = m.pGetMat();
1523 const doublereal *pdv1 = v1.pGetVec();
1524 const doublereal *pdv2 = v2.pGetVec();
1525
1526 double d11 = pdv1[V1]*pdv2[V1];
1527 double d22 = pdv1[V2]*pdv2[V2];
1528 double d33 = pdv1[V3]*pdv2[V3];
1529
1530 pdm[M11] = -d22 - d33;
1531 pdm[M12] = pdv2[V1]*pdv1[V2];
1532 pdm[M13] = pdv2[V1]*pdv1[V3];
1533 pdm[M21] = pdv2[V2]*pdv1[V1];
1534 pdm[M22] = -d33 - d11;
1535 pdm[M23] = pdv2[V2]*pdv1[V3];
1536 pdm[M31] = pdv2[V3]*pdv1[V1];
1537 pdm[M32] = pdv2[V3]*pdv1[V2];
1538 pdm[M33] = -d11 - d22;
1539 };
1540 };
1541
1542 extern const Mat3x3Zero_Manip Mat3x3Zero;
1543 extern const Mat3x3DEye_Manip Mat3x3DEye;
1544 extern const Mat3x3Diag_Manip Mat3x3Diag;
1545 extern const MatCross_Manip MatCross;
1546 extern const MatCrossCross_Manip MatCrossCross;
1547
1548 extern const doublereal Zero1;
1549 extern const Vec3 Zero3;
1550 extern const Mat3x3 Zero3x3;
1551 extern const Mat3x3 Eye3;
1552
1553 template <>
1554 inline const doublereal& mb_zero<doublereal>(void)
1555 {
1556 return ::Zero1;
1557 }
1558
1559 template <>
1560 inline const Vec3& mb_zero<Vec3>(void)
1561 {
1562 return ::Zero3;
1563 }
1564
1565 template <>
1566 inline const Mat3x3& mb_zero<Mat3x3>(void)
1567 {
1568 return ::Zero3x3;
1569 }
1570
1571 template <>
1572 inline doublereal mb_deye<doublereal>(const doublereal d)
1573 {
1574 return d;
1575 }
1576
1577 template <>
1578 inline doublereal& mb_deye<doublereal>(doublereal& out, const doublereal d)
1579 {
1580 return out = d;
1581 }
1582
1583 template <>
1584 inline Mat3x3 mb_deye<Mat3x3>(const doublereal d)
1585 {
1586 return Mat3x3(Mat3x3DEye, d);
1587 }
1588
1589 template <>
1590 inline Mat3x3& mb_deye<Mat3x3>(Mat3x3& out, const doublereal d)
1591 {
1592 Mat3x3DEye.Manipulate(out, d);
1593 return out;
1594 }
1595
1596 /* known orientation descriptions */
1597 enum OrientationDescription {
1598 UNKNOWN_ORIENTATION_DESCRIPTION = -1,
1599 EULER_123 = 0,
1600 EULER_313,
1601 EULER_321,
1602 ORIENTATION_VECTOR,
1603 ORIENTATION_MATRIX,
1604
1605 LAST_ORIENTATION_DESCRIPTION
1606 };
1607
1608 /*
1609 Calcola i parametri di Rodrigues g a partire dalla matrice di rotazione R.
1610 Nota: i parametri devono essere definiti, ovvero R non deve rappresentare
1611 una rotazione a cui corrispondono parametri singolari.
1612 */
1613 extern Vec3 MatR2gparam(const Mat3x3& R);
1614
1615 /*
1616 Computes so-called linear parametrization
1617 */
1618 extern Vec3 MatR2LinParam(const Mat3x3& R);
1619
1620 /*
1621 Calcola la matrice di rotazione a partire da due vettori sghembi.
1622 @param ia indice del vettore va nel sistema locale.
1623 @param va vettore ia nel sistema locale.
1624 @param ib indice del vettore che risulta dall'elaborazione di vb.
1625 @param vb vettore che viene trasformato come segue:
1626 il vettore va viene assunto come asse ia nel sistema locale,
1627 mentre la componente di vb normale a va e giacente nel piano
1628 normale al prodotto vettore tra va e vb viene assunta come
1629 componente ib nel sistema locale.
1630 */
1631 extern Mat3x3 MatR2vec(unsigned short int ia,
1632 const Vec3& va,
1633 unsigned short int ib,
1634 const Vec3& vb);
1635
1636
1637 /*
1638 Calcola gli angoli di Eulero a partire dalla matrice di rotazione R.
1639 Nota: gli angoli di Eulero sono ritornati in gradi.
1640 */
1641 extern const doublereal dRaDegr;
1642 extern Vec3 MatR2EulerAngles(const Mat3x3& R);
1643 extern Vec3 MatR2EulerAngles123(const Mat3x3& R);
1644 extern Vec3 MatR2EulerAngles313(const Mat3x3& R);
1645 extern Vec3 MatR2EulerAngles321(const Mat3x3& R);
1646 extern void MatR2EulerParams(const Mat3x3& R, doublereal& e0, Vec3& e);
1647
1648 extern Vec3 Unwrap(const Vec3& vPrev, const Vec3& v);
1649
1650 /*
1651 Calcola la matrice di rotazione corrispondente agli angoli di Eulero v.
1652 Nota: gli angoli di Eulero vengono letti in radianti.
1653 */
1654 extern Mat3x3 EulerAngles2MatR(const Vec3& v);
1655 extern Mat3x3 EulerAngles123_2MatR(const Vec3& v);
1656 extern Mat3x3 EulerAngles313_2MatR(const Vec3& v);
1657 extern Mat3x3 EulerAngles321_2MatR(const Vec3& v);
1658
1659 /*
1660 * Cayley-Gibbs-Rodrigues rotation manipulation namespace
1661 */
1662 namespace CGR_Rot {
1663
1664 class Param_Manip : public Vec3_Manip {
1665 public:
Param_Manip()1666 Param_Manip() {};
Manipulate(Vec3 & v,const Mat3x3 & m)1667 inline void Manipulate(Vec3& v, const Mat3x3& m) const {
1668 // singularity test
1669 doublereal d = 1. + m.Trace();
1670
1671 if (fabs(d) < std::numeric_limits<doublereal>::epsilon()) {
1672 silent_cerr("Param_Manip(): divide by zero, "
1673 "probably due to singularity in rotation parameters" << std::endl);
1674 throw ErrDivideByZero(MBDYN_EXCEPT_ARGS);
1675 }
1676
1677 v = m.Ax()*(4./d);
1678 };
1679 };
1680
1681 /* MatR_Manip - begin */
1682
1683 // Manipolatore per matrice R con parametri di Rodrigues.
1684 class MatR_Manip : public Mat3x3_Manip {
1685 public:
MatR_Manip()1686 MatR_Manip() {}
1687 /*
1688 Crea in m la matrice R corrispondente ai parametri g.
1689 */
Manipulate(Mat3x3 & m,const Vec3 & g)1690 inline void Manipulate(Mat3x3& m, const Vec3& g) const {
1691 doublereal d = (4./(4. + g.Dot()));
1692
1693 /*
1694 m = Eye3;
1695 m += Mat3x3(g*d);
1696 */
1697
1698 /* E' piu' efficiente se creo contemporaneamente I+d*g/\ */
1699 m = Mat3x3(1., g*d);
1700
1701 /* Alla fine sommo il termine d/2*g/\g/\, che e' una matrice piena */
1702 m += Mat3x3(MatCrossCross, g, g*(d/2.));
1703 };
1704 };
1705
1706 /* MatR_Manip - end */
1707
1708
1709 /* MatG_Manip - begin */
1710
1711 // Manipolatore per matrice G con parametri di Rodrigues.
1712 class MatG_Manip : public Mat3x3_Manip {
1713 public:
MatG_Manip()1714 MatG_Manip() {};
1715 /*
1716 Crea in m la matrice G corrispondente ai parametri g.
1717 */
Manipulate(Mat3x3 & m,const Vec3 & g)1718 inline void Manipulate(Mat3x3& m, const Vec3& g) const {
1719 doublereal d = (4./(4.+g.Dot()));
1720 m = Mat3x3(d, g*(d/2.));
1721 };
1722 };
1723
1724 /* MatG_Manip - end */
1725
1726
1727 /* MatGm1_Manip - begin */
1728
1729 // Manipolatore per inversa della matrice G con parametri di Rodrigues */
1730 class MatGm1_Manip : public Mat3x3_Manip {
1731 public:
MatGm1_Manip()1732 MatGm1_Manip() {};
1733 /*
1734 Crea in m l'inversa della matrice G corrispondente ai parametri g.
1735 */
Manipulate(Mat3x3 & m,const Vec3 & g)1736 inline void Manipulate(Mat3x3& m, const Vec3& g) const {
1737 m = Mat3x3(1., g/(-2.));
1738 m += g.Tens()/4.;
1739 };
1740 };
1741
1742 /* MatGm1_Manip - end */
1743
1744
1745 /*
1746 Manipolatore per parametri di Cayley-Gibbs-Rodrigues
1747 */
1748 extern const Param_Manip Param;
1749
1750 /*
1751 Manipolatore per matrice R
1752 */
1753 extern const MatR_Manip MatR;
1754
1755 /*
1756 Manipolatore per matrice G
1757 */
1758 extern const MatG_Manip MatG;
1759
1760 /*
1761 Manipolatore per inversa della matrice G
1762 */
1763 extern const MatGm1_Manip MatGm1;
1764
1765 } // end of namespace CGR_Rot
1766
1767 /* test */
1768 template <class T>
1769 bool
IsNull(const T & t)1770 IsNull(const T& t)
1771 {
1772 return t.IsNull();
1773 }
1774
1775 template <class T>
1776 bool
IsExactlySame(const T & t1,const T & t2)1777 IsExactlySame(const T& t1, const T& t2)
1778 {
1779 return t1.IsExactlySame(t2);
1780 }
1781
1782 template <class T>
1783 bool
IsSame(const T & t1,const T & t2,const doublereal & dTol)1784 IsSame(const T& t1, const T& t2, const doublereal& dTol)
1785 {
1786 return t1.IsSame(t2, dTol);
1787 }
1788
1789 template <>
1790 bool
1791 IsNull(const doublereal& d);
1792
1793 template <>
1794 bool
1795 IsExactlySame(const doublereal& d1, const doublereal& d2);
1796
1797 template <>
1798 bool
1799 IsSame(const doublereal& d1, const doublereal& d2, const doublereal& dTol);
1800
1801 extern Vec3 MultRV(const Vec3& v, const Mat3x3& R);
1802
1803 extern Mat3x3 MultRM(const Mat3x3& m, const Mat3x3& R);
1804 extern Mat3x3 MultMRt(const Mat3x3& m, const Mat3x3& R);
1805 extern Mat3x3 MultRMRt(const Mat3x3& m, const Mat3x3& R);
1806
1807 #endif /* MATVEC3_H */
1808
1809