MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
matvec6.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/libraries/libmbmath/matvec6.cc,v 1.26 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 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
33 
34 #include <matvec6.h>
35 
36 // NOTE: do not use Zero3, Zero3x3 or mb_zero<>()
37 // because they might be not initialized yet
38 const Vec6 Zero6(0., 0., 0., 0., 0., 0.);
39 const Mat6x6 Zero6x6(0., 0., 0., 0., 0., 0.,
40  0., 0., 0., 0., 0., 0.,
41  0., 0., 0., 0., 0., 0.,
42  0., 0., 0., 0., 0., 0.,
43  0., 0., 0., 0., 0., 0.,
44  0., 0., 0., 0., 0., 0.);
45 const Mat6x6 Eye6(1., 0., 0., 0., 0., 0.,
46  0., 1., 0., 0., 0., 0.,
47  0., 0., 1., 0., 0., 0.,
48  0., 0., 0., 1., 0., 0.,
49  0., 0., 0., 0., 1., 0.,
50  0., 0., 0., 0., 0., 1.);
51 
52 
53 std::ostream&
54 Vec6::Write(std::ostream& out, const char* sFill) const
55 {
56  return out
57  << v[0].dGet(1) << sFill
58  << v[0].dGet(2) << sFill
59  << v[0].dGet(3) << sFill
60  << v[1].dGet(1) << sFill
61  << v[1].dGet(2) << sFill
62  << v[1].dGet(3);
63 }
64 
65 void
67 {
68  v[0].Reset();
69  v[1].Reset();
70 }
71 
72 void
74 {
75  m[0][0].Reset();
76  m[0][1].Reset();
77  m[1][0].Reset();
78  m[1][1].Reset();
79 }
80 
82 {
83  return v;
84 }
85 
86 
88 {
89  return Vec6(-v.GetVec1(), -v.GetVec2());
90 }
91 
92 
93 std::ostream&
94 operator << (std::ostream& out, const Vec6& v)
95 {
96  const Vec3& v1 = v.GetVec1();
97  const Vec3& v2 = v.GetVec2();
98  return out
99  << v1.dGet(1) << " " << v1.dGet(2) << " " << v1.dGet(3) << " "
100  << v2.dGet(1) << " " << v2.dGet(2) << " " << v2.dGet(3);
101 }
102 
103 
104 std::ostream&
105 Write(std::ostream& out, const Vec6& v, const char* sFill)
106 {
107  return v.Write(out, sFill);
108 }
109 
110 
111 std::ostream&
112 Mat6x6::Write(std::ostream& out, const char* sFill, const char* sFill2) const
113 {
114  char* sF2 = (char*)sFill2;
115  if (sFill2 == NULL) {
116  sF2 = (char*)sFill;
117  }
118 
119  return out
120  << m[0][0].dGet(1,1) << sFill
121  << m[0][0].dGet(1,2) << sFill
122  << m[0][0].dGet(1,3) << sFill
123  << m[0][1].dGet(1,1) << sFill
124  << m[0][1].dGet(1,2) << sFill
125  << m[0][1].dGet(1,3) << sF2
126  << m[0][0].dGet(2,1) << sFill
127  << m[0][0].dGet(2,2) << sFill
128  << m[0][0].dGet(2,3) << sFill
129  << m[0][1].dGet(2,1) << sFill
130  << m[0][1].dGet(2,2) << sFill
131  << m[0][1].dGet(2,3) << sF2
132  << m[0][0].dGet(3,1) << sFill
133  << m[0][0].dGet(3,2) << sFill
134  << m[0][0].dGet(3,3) << sFill
135  << m[0][1].dGet(3,1) << sFill
136  << m[0][1].dGet(3,2) << sFill
137  << m[0][1].dGet(3,3) << sF2
138  << m[1][0].dGet(1,1) << sFill
139  << m[1][0].dGet(1,2) << sFill
140  << m[1][0].dGet(1,3) << sFill
141  << m[1][1].dGet(1,1) << sFill
142  << m[1][1].dGet(1,2) << sFill
143  << m[1][1].dGet(1,3) << sF2
144  << m[1][0].dGet(2,1) << sFill
145  << m[1][0].dGet(2,2) << sFill
146  << m[1][0].dGet(2,3) << sFill
147  << m[1][1].dGet(2,1) << sFill
148  << m[1][1].dGet(2,2) << sFill
149  << m[1][1].dGet(2,3) << sF2
150  << m[1][0].dGet(3,1) << sFill
151  << m[1][0].dGet(3,2) << sFill
152  << m[1][0].dGet(3,3) << sFill
153  << m[1][1].dGet(3,1) << sFill
154  << m[1][1].dGet(3,2) << sFill
155  << m[1][1].dGet(3,3);
156 }
157 
158 
159 std::ostream&
160 operator << (std::ostream& out, const Mat6x6& m)
161 {
162  const Mat3x3& m11 = m.GetMat11();
163  const Mat3x3& m12 = m.GetMat12();
164  const Mat3x3& m21 = m.GetMat21();
165  const Mat3x3& m22 = m.GetMat22();
166  return out
167  << m11.dGet(1, 1) << " " << m11.dGet(1, 2) << " " << m11.dGet(1,3) << " "
168  << m12.dGet(1, 1) << " " << m12.dGet(1, 2) << " " << m12.dGet(1,3) << std::endl
169  << m11.dGet(2, 1) << " " << m11.dGet(2, 2) << " " << m11.dGet(2,3) << " "
170  << m12.dGet(2, 1) << " " << m12.dGet(2, 2) << " " << m12.dGet(2,3) << std::endl
171  << m11.dGet(3, 1) << " " << m11.dGet(3, 2) << " " << m11.dGet(3,3) << " "
172  << m12.dGet(3, 1) << " " << m12.dGet(3, 2) << " " << m12.dGet(3,3) << std::endl
173  << m21.dGet(1, 1) << " " << m21.dGet(1, 2) << " " << m21.dGet(1,3) << " "
174  << m22.dGet(1, 1) << " " << m22.dGet(1, 2) << " " << m22.dGet(1,3) << std::endl
175  << m21.dGet(2, 1) << " " << m21.dGet(2, 2) << " " << m21.dGet(2,3) << " "
176  << m22.dGet(2, 1) << " " << m22.dGet(2, 2) << " " << m22.dGet(2,3) << std::endl
177  << m21.dGet(3, 1) << " " << m21.dGet(3, 2) << " " << m21.dGet(3,3) << " "
178  << m22.dGet(3, 1) << " " << m22.dGet(3, 2) << " " << m22.dGet(3,3);
179 }
180 
181 
182 std::ostream&
183 Write(std::ostream& out, const Mat6x6& m, const char* sFill,
184  const char* sFill2)
185 {
186  return m.Write(out, sFill, sFill2);
187 }
188 
189 
190 Vec6 MultRV(const Vec6& v, const Mat3x3& R)
191 {
192  return Vec6(R*v.GetVec1(), R*v.GetVec2());
193 }
194 
195 
196 Mat6x6 MultRM(const Mat6x6& m, const Mat3x3& R)
197 {
198  return Mat6x6(R*m.GetMat11(), R*m.GetMat21(),
199  R*m.GetMat12(), R*m.GetMat22());
200 }
201 
202 
203 Mat6x6 MultMRt(const Mat6x6& m, const Mat3x3& R)
204 {
205  return Mat6x6(m.GetMat11().MulMT(R), m.GetMat21().MulMT(R),
206  m.GetMat12().MulMT(R), m.GetMat22().MulMT(R));
207 }
208 
209 
210 Mat6x6 MultRMRt(const Mat6x6& m, const Mat3x3& R)
211 {
212  return Mat6x6(R*m.GetMat11().MulMT(R), R*m.GetMat21().MulMT(R),
213  R*m.GetMat12().MulMT(R), R*m.GetMat22().MulMT(R));
214 }
215 
216 Mat6x6 MultRMRt(const Mat6x6& m, const Mat3x3& R, const doublereal& c)
217 {
218  Mat3x3 Rc(R*c);
219  return Mat6x6(R*m.GetMat11().MulMT(Rc), R*m.GetMat21().MulMT(Rc),
220  R*m.GetMat12().MulMT(Rc), R*m.GetMat22().MulMT(Rc));
221 }
222 
223 
224 /* esegue l'operazione:
225  * [I 0] [ ]
226  * [ ] [ m ]
227  * [vx I] [ ] */
228 Mat6x6 MultVCrossMat(const Mat6x6& m, const Vec3& v)
229 {
230  return Mat6x6(m.GetMat11(),
231  v.Cross(m.GetMat11())+m.GetMat21(),
232  m.GetMat12(),
233  v.Cross(m.GetMat12())+m.GetMat22());
234 }
235 
236 
237 /* esegue l'operazione:
238  * [I vx] [ ]
239  * [ ] [ m ]
240  * [0 I] [ ] */
241 Mat6x6 MultVCrossTMat(const Mat6x6& m, const Vec3& v)
242 {
243  return Mat6x6(m.GetMat11()+v.Cross(m.GetMat21()),
244  m.GetMat21(),
245  m.GetMat12()+v.Cross(m.GetMat22()),
246  m.GetMat22());
247 }
248 
249 
250 /* esegue l'operazione:
251  * [ ] [I vx]
252  * [ m ] [ ]
253  * [ ] [0 I] */
254 Mat6x6 MultMatVCross(const Mat6x6& m, const Vec3& v)
255 {
256  Mat3x3 vCross(MatCross, v);
257 
258  return Mat6x6(m.GetMat11(),
259  m.GetMat21(),
260  m.GetMat11()*vCross + m.GetMat12(),
261  m.GetMat21()*vCross + m.GetMat22());
262 }
263 
264 
265 /* esegue l'operazione:
266  * [ ] [I 0]
267  * [ m ] [ ]
268  * [ ] [vx I] */
269 Mat6x6 MultMatVCrossT(const Mat6x6& m, const Vec3& v)
270 {
271  Mat3x3 vCross(MatCross, v);
272 
273  return Mat6x6(m.GetMat11() + m.GetMat12()*vCross,
274  m.GetMat21() + m.GetMat22()*vCross,
275  m.GetMat12(),
276  m.GetMat22());
277 }
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
const doublereal & dGet(unsigned short int iRow, unsigned short int iCol) const
Definition: matvec3.h:770
Mat3x3 GetMat12(void)
Definition: matvec6.h:328
Definition: matvec3.h:98
const Vec3 & GetVec2(void) const
Definition: matvec6.h:76
const MatCross_Manip MatCross
Definition: matvec3.cc:639
const Mat6x6 Zero6x6(0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.)
std::ostream & operator<<(std::ostream &out, const Vec6 &v)
Definition: matvec6.cc:94
Mat6x6 MultVCrossMat(const Mat6x6 &m, const Vec3 &v)
Definition: matvec6.cc:228
Mat3x3 GetMat21(void)
Definition: matvec6.h:324
Mat6x6 MultRM(const Mat6x6 &m, const Mat3x3 &R)
Definition: matvec6.cc:196
void Reset(void)
Definition: matvec6.cc:66
Mat3x3 GetMat22(void)
Definition: matvec6.h:332
Mat3x3 GetMat11(void)
Definition: matvec6.h:320
Mat6x6 MultMRt(const Mat6x6 &m, const Mat3x3 &R)
Definition: matvec6.cc:203
Definition: matvec6.h:37
void Reset(void)
Definition: matvec3.cc:613
const Vec3 & GetVec1(void) const
Definition: matvec6.h:72
Mat3x3 m[2][2]
Definition: matvec6.h:236
const Vec6 Zero6(0., 0., 0., 0., 0., 0.)
void Reset(void)
Definition: matvec3.cc:109
const doublereal & dGet(unsigned short int iRow) const
Definition: matvec3.h:285
std::ostream & Write(std::ostream &out, const char *sFill=" ", const char *sFill2=NULL) const
Definition: matvec6.cc:112
Vec6 MultRV(const Vec6 &v, const Mat3x3 &R)
Definition: matvec6.cc:190
Mat6x6 MultMatVCross(const Mat6x6 &m, const Vec3 &v)
Definition: matvec6.cc:254
Mat6x6 MultRMRt(const Mat6x6 &m, const Mat3x3 &R)
Definition: matvec6.cc:210
Vec6 operator+(const Vec6 &v)
Definition: matvec6.cc:81
Mat6x6 MultVCrossTMat(const Mat6x6 &m, const Vec3 &v)
Definition: matvec6.cc:241
Mat6x6 MultMatVCrossT(const Mat6x6 &m, const Vec3 &v)
Definition: matvec6.cc:269
static std::stack< cleanup * > c
Definition: cleanup.cc:59
const Mat6x6 Eye6(1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1.)
void Reset(void)
Definition: matvec6.cc:73
Vec3 v[2]
Definition: matvec6.h:39
std::ostream & Write(std::ostream &out, const char *sFill=" ") const
Definition: matvec6.cc:54
std::ostream & Write(std::ostream &out, const Vec6 &v, const char *sFill)
Definition: matvec6.cc:105
Mat3x3 MulMT(const Mat3x3 &m) const
Definition: matvec3.cc:444
Vec6 operator-(const Vec6 &v)
Definition: matvec6.cc:87
double doublereal
Definition: colamd.c:52
Mat3x3 R