MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
hbeam_interp.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/hbeam_interp.cc,v 1.27 2017/01/12 14:46:43 masarati Exp $ */
2 /*
3  * HmFe (C) is a FEM analysis code.
4  *
5  * Copyright (C) 1996-2017
6  *
7  * Marco Morandini <morandini@aero.polimi.it>
8  * Teodoro Merlini <merlini@aero.polimi.it>
9  *
10  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
11  * via La Masa, 34 - 20156 Milano, Italy
12  * http://www.aero.polimi.it
13  *
14  * Changing this copyright notice is forbidden.
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
18  *
19  */
20 /*
21  * MBDyn (C) is a multibody analysis code.
22  * http://www.mbdyn.org
23  *
24  * Copyright (C) 1996-2017
25  *
26  * This code is a partial merge of HmFe and MBDyn.
27  *
28  * Pierangelo Masarati <masarati@aero.polimi.it>
29  * Paolo Mantegazza <mantegazza@aero.polimi.it>
30  *
31  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
32  * via La Masa, 34 - 20156 Milano, Italy
33  * http://www.aero.polimi.it
34  *
35  * Changing this copyright notice is forbidden.
36  *
37  * This program is free software; you can redistribute it and/or modify
38  * it under the terms of the GNU General Public License as published by
39  * the Free Software Foundation (version 2 of the License).
40  *
41  *
42  * This program is distributed in the hope that it will be useful,
43  * but WITHOUT ANY WARRANTY; without even the implied warranty of
44  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
45  * GNU General Public License for more details.
46  *
47  * You should have received a copy of the GNU General Public License
48  * along with this program; if not, write to the Free Software
49  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
50  */
51 
52 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
53 
54 #include "matvecexp.h"
55 #include "Rot.hh"
56 #include "hbeam_interp.h"
57 
58 // Input:
59 // node_pos array delle posizioni attuali dei due nodi piu' offset
60 // node_or array delle orientazioni attuali dei due nodi
61 // node_f array degli offset attuali (R*f_tilde) dei due nodi
62 // xi posizione del punto, da 0 a 1
63 // dexi_des derivata della posizione del punto rispetto all'ascissa
64 // curvilinea
65 //
66 // Output:
67 // pos posizione del punto interpolato
68 // orient orientazione del punto interpolato
69 // or_delta_w_or or_delta in funzione di or_delta nodali
70 // delta_pos_w_or delta_pos in funzione di or_delta nodali
71 // delta_pos_w_pos delta_pos in funzione di delta_pos nodali
72 // F derivata della posizione rispetto all'ascissa curvilinea
73 // om assiale di d(orient)/d(s)*or.Transpose()
74 // delta_om_ws_or delta_om in funzione di or_delta nodali
75 // delta_F_ws_or delta_F in funzione di or_delta nodali
76 // delta_F_ws_pos delta_F in funzione di delta_pos nodali
77 //
78 // Nota:
79 // Dopo l'uscita bisogna convertire gli or_delta nodali
80 // in delta_parametri_or
81 
82 /*
83  * Enum puramente locale
84  */
85 enum {
86  NODE1 = 0,
87  NODE2 = 1,
88  NUMNODES = 2 /* serve eventualmente per dimensionare arrays */
89 };
90 
91 
92 inline void Compute(const Vec3 *const node_pos,
93  const Mat3x3 *const node_or,
94  const Vec3 *const node_f,
95  const doublereal xi,
96  const doublereal dexi_des,
97  Vec3 &pos,
98  Mat3x3 &orient,
99  Vec3 &F,
100  Vec3 &om,
101  MatExp& A1,
102  MatExp& A2,
103  MatExp& A12,
104  MatExp& A_tilde,
105  MatExp& Theta_r_I,
106  MatExp& Theta_tilde,
107  MatExp& A_xi,
108  VecExp& eta_r,
109  VecExp& kappa,
110  VecExp& eta_tilde
111  ) {
112  A1 = MatExp(node_or[NODE1], node_pos[NODE1].Cross(node_or[NODE1]));
113  A2 = MatExp(node_or[NODE2], node_pos[NODE2].Cross(node_or[NODE2]));
114  A12 = A2*(A1.Transpose());
115  //VecExp e1(0.);
116  eta_r = RoTrManip::Helix(A12);
117  Theta_r_I = RoTrManip::DRoTr_I(eta_r);
118  eta_tilde = eta_r*xi;
119  RoTrManip::RoTrAndDRoTr(eta_tilde,A_tilde,Theta_tilde);
120  A_xi = A_tilde*A1;
121  //extract interpolated orientation and position
122  orient = A_xi.GetVec();
123  pos = (A_xi.GetMom()*(orient.Transpose())).Ax();
124 
125  kappa = Theta_tilde*eta_r*dexi_des;
126  //extract interpolated curvature and def. gradient
127  om = kappa.GetVec();
128  F = kappa.GetMom();
129  F -= pos.Cross(om); /* :) */
130 };
131 
132 void ComputeInterpolation(const Vec3 *const node_pos,
133  const Mat3x3 *const node_or,
134  const Vec3 *const node_f,
135  const doublereal xi,
136  const doublereal dexi_des,
137  Vec3 &pos,
138  Mat3x3 &orient,
139  Vec3 &F,
140  Vec3 &om) {
141  MatExp A1, A2, A12, A_tilde, Theta_r_I, Theta_tilde, A_xi;
142  VecExp eta_r, kappa, eta_tilde;
143  Compute(node_pos,node_or, node_f,xi,dexi_des,pos,orient,F,om,
144  A1,A2,A12,A_tilde,Theta_r_I,Theta_tilde,A_xi,eta_r,
145  kappa,eta_tilde);
146 };
147 
148 void ComputeFullInterpolation(const Vec3 *const node_pos,
149  const Mat3x3 *const node_or,
150  const Vec3 *const node_f,
151  const doublereal xi,
152  const doublereal dexi_des,
153  Vec3 &pos,
154  Mat3x3 &orient,
155  Mat3x3 *const or_delta_w_or,
156  Mat3x3 *const delta_pos_w_or,
157  Mat3x3 *const delta_pos_w_pos,
158  Vec3 &F,
159  Vec3 &om,
160  Mat3x3 *const delta_om_ws_or,
161  Mat3x3 *const delta_F_ws_or,
162  Mat3x3 *const delta_F_ws_pos) {
163  MatExp A1, A2, A12, A_tilde, Theta_r_I, Theta_tilde, A_xi;
164  VecExp eta_r, kappa, eta_tilde;
165  Compute(node_pos,node_or, node_f,xi,dexi_des,pos,orient,F,om,
166  A1,A2,A12,A_tilde,Theta_r_I,Theta_tilde,A_xi,eta_r,
167  kappa,eta_tilde);
168 
169  MatExp eta_xi_delta_2(Theta_tilde*Theta_r_I*xi);
170  MatExp eta_xi_delta_1(A_tilde);
171  eta_xi_delta_1-=Theta_tilde*Theta_r_I*A2*xi;
172  //compute traslation wrenches at xi and at the nodes:
173  MatExp Trasl_n[NUMNODES];
174  Trasl_n[NODE1] = MatExp(Eye3, Mat3x3(MatCross, node_pos[NODE1]));
175  Trasl_n[NODE2] = MatExp(Eye3, Mat3x3(MatCross, node_pos[NODE2]));
176  MatExp Trasl_xi_bak(Eye3, Mat3x3(MatCross, -pos));
177  //compute delta_kappa_i
178  MatExp Lambda(RoTrManip::Elle(eta_tilde,eta_r*dexi_des));
179  MatExp delta_kappa_2(Lambda*xi);
180  delta_kappa_2+=Theta_tilde*dexi_des;
181  delta_kappa_2 = delta_kappa_2*Theta_r_I;
182  MatExp delta_kappa_1(delta_kappa_2*A2*-1);
183 
184  //transform eta_xi_delta_i to deltapos_ordelta_xi_i
185  MatExp dpos_ord_xi_1(Trasl_xi_bak*eta_xi_delta_1);
186  MatExp dpos_ord_xi_2(Trasl_xi_bak*eta_xi_delta_2);
187  //project to nodes delta_pos, or_delta:
188  MatExp dpos_ord_dpos_ord_1(dpos_ord_xi_1*Trasl_n[NODE1]);
189  MatExp dpos_ord_dpos_ord_2(dpos_ord_xi_2*Trasl_n[NODE2]);
190  //extract or_delta_w_or, delta_pos_w_or and delta_pos_w_pos
191  or_delta_w_or[NODE1] = dpos_ord_dpos_ord_1.GetVec();
192  or_delta_w_or[NODE2] = dpos_ord_dpos_ord_2.GetVec();
193  delta_pos_w_or[NODE1] = dpos_ord_dpos_ord_1.GetMom();
194  delta_pos_w_or[NODE2] = dpos_ord_dpos_ord_2.GetMom();
195  delta_pos_w_pos[NODE1] = dpos_ord_dpos_ord_1.GetVec();
196  delta_pos_w_pos[NODE2] = dpos_ord_dpos_ord_2.GetVec();
197  //project delta_kappa_1 to nodes delta_pos, or_delta:
198  MatExp dkappa_dpos_ord_1(delta_kappa_1*Trasl_n[NODE1]);
199  MatExp dkappa_dpos_ord_2(delta_kappa_2*Trasl_n[NODE2]);
200  //extract delta_om_ws_or, delta_F_ws_or and delta_F_ws_pos
201  delta_om_ws_or[NODE1] = dkappa_dpos_ord_1.GetVec();
202  delta_om_ws_or[NODE2] = dkappa_dpos_ord_2.GetVec();
203  //delta kappa.Mom()
204  delta_F_ws_or[NODE1] = dkappa_dpos_ord_1.GetMom();
205  delta_F_ws_pos[NODE1] = dkappa_dpos_ord_1.GetVec();
206  delta_F_ws_or[NODE2] = dkappa_dpos_ord_2.GetMom();
207  delta_F_ws_pos[NODE2] = dkappa_dpos_ord_2.GetVec();
208  //x cross delta kappa.Vec()
209  delta_F_ws_or[NODE1] -= pos.Cross(dkappa_dpos_ord_1.GetVec());
210  delta_F_ws_or[NODE2] -= pos.Cross(dkappa_dpos_ord_2.GetVec());
211  //kappa.Vec() x delta_pos
212  delta_F_ws_or[NODE1] += kappa.GetVec().Cross(delta_pos_w_or[NODE1]);
213  delta_F_ws_pos[NODE1] += kappa.GetVec().Cross(delta_pos_w_pos[NODE1]);
214  delta_F_ws_or[NODE2] += kappa.GetVec().Cross(delta_pos_w_or[NODE2]);
215  delta_F_ws_pos[NODE2] += kappa.GetVec().Cross(delta_pos_w_pos[NODE2]);
216 
217  //tarocco per l'aggiunta dell'offset f
218  //le curvature e le posizioni invece sono giuste perche'
219  //viene passato node_pos=x_node+node_or*f
220  Mat3x3 fodo;
221 
222  fodo = node_f[NODE1].Cross(or_delta_w_or[NODE1]);
223  delta_pos_w_or[NODE1] -= delta_pos_w_pos[NODE1]*fodo;
224  delta_F_ws_or[NODE1] -= delta_F_ws_pos[NODE1]*fodo;
225 
226  fodo = node_f[NODE2].Cross(or_delta_w_or[NODE2]);
227  delta_pos_w_or[NODE2] -= delta_pos_w_pos[NODE2]*fodo;
228  delta_F_ws_or[NODE2] -= delta_F_ws_pos[NODE2]*fodo;
229 }
230 
const Vec3 & GetVec(void) const
Definition: matvecexp.h:198
void ComputeInterpolation(const Vec3 *const node_pos, const Mat3x3 *const node_or, const Vec3 *const node_f, const doublereal xi, const doublereal dexi_des, Vec3 &pos, Mat3x3 &orient, Vec3 &F, Vec3 &om)
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
const Mat3x3 & GetVec(void) const
Definition: matvecexp.h:356
Definition: matvec3.h:98
const MatCross_Manip MatCross
Definition: matvec3.cc:639
MatExp DRoTr_I(const VecExp &eta)
Definition: Rot.cc:255
const Mat3x3 Eye3(1., 0., 0., 0., 1., 0., 0., 0., 1.)
const Vec3 & GetMom(void) const
Definition: matvecexp.h:202
void Compute(const Vec3 *const node_pos, const Mat3x3 *const node_or, const Vec3 *const node_f, const doublereal xi, const doublereal dexi_des, Vec3 &pos, Mat3x3 &orient, Vec3 &F, Vec3 &om, MatExp &A1, MatExp &A2, MatExp &A12, MatExp &A_tilde, MatExp &Theta_r_I, MatExp &Theta_tilde, MatExp &A_xi, VecExp &eta_r, VecExp &kappa, VecExp &eta_tilde)
Definition: hbeam_interp.cc:92
VecExp Helix(const MatExp &H)
Definition: Rot.cc:282
VectorExpression< VectorCrossExpr< VectorLhsExpr, VectorRhsExpr >, 3 > Cross(const VectorExpression< VectorLhsExpr, 3 > &u, const VectorExpression< VectorRhsExpr, 3 > &v)
Definition: matvec.h:3248
Mat3x3 Transpose(void) const
Definition: matvec3.h:816
MatExp Transpose(void) const
Definition: matvecexp.h:407
double doublereal
Definition: colamd.c:52
const Mat3x3 & GetMom(void) const
Definition: matvecexp.h:360
void RoTrAndDRoTr(const VecExp &eta, MatExp &H, MatExp &Th)
Definition: Rot.cc:230
void ComputeFullInterpolation(const Vec3 *const node_pos, const Mat3x3 *const node_or, const Vec3 *const node_f, const doublereal xi, const doublereal dexi_des, Vec3 &pos, Mat3x3 &orient, Mat3x3 *const or_delta_w_or, Mat3x3 *const delta_pos_w_or, Mat3x3 *const delta_pos_w_pos, Vec3 &F, Vec3 &om, Mat3x3 *const delta_om_ws_or, Mat3x3 *const delta_F_ws_or, Mat3x3 *const delta_F_ws_pos)
MatExp Elle(const VecExp &phi, const VecExp &a)
Definition: Rot.cc:194