1 //#**************************************************************
2 //#
3 //# filename: ANCFBeam3D.cpp
4 //#
5 //# author: Gerstmayr Johannes
6 //#
7 //# generated: 9. November 2004
8 //# description: ANCF beam formulation according to Yakoub and Shabana
9 //# Elastic line approach according to Schwab and Meijaard
10 //# remarks:
11 //#
12 //# Copyright (c) 2003-2013 Johannes Gerstmayr, Linz Center of Mechatronics GmbH, Austrian
13 //# Center of Competence in Mechatronics GmbH, Institute of Technical Mechanics at the
14 //# Johannes Kepler Universitaet Linz, Austria. All rights reserved.
15 //#
16 //# This file is part of HotInt.
17 //# HotInt is free software: you can redistribute it and/or modify it under the terms of
18 //# the HOTINT license. See folder 'licenses' for more details.
19 //#
20 //# bug reports are welcome!!!
21 //# WWW: www.hotint.org
22 //# email: bug_reports@hotint.org or support@hotint.org
23 //#***************************************************************************************
24
25 #include "ANCFBeam3D.h"
26 #include "elementDataAccess.h"
27 #include "Material.h"
28 #include "node.h"
29 #include "femathHelperFunctions.h"
30 #include "graphicsConstants.h"
31
32 const int usetangentframe = 0;
33
ANCFBeam3D(MBS * mbsi,const Vector & xc1,const Vector & xc2,double rhoi,double Emi,double nui,const Vector3D & si,const Vector3D & coli,int beammodel)34 ANCFBeam3D::ANCFBeam3D(MBS* mbsi, const Vector& xc1, const Vector& xc2, double rhoi, double Emi, double nui,
35 const Vector3D& si, const Vector3D& coli, int beammodel):
36 Body3D(mbsi),massmatrix(), Hmatrix(), SV(), DS(), x1(), x2(), x3(), w1(), w2(), w3(),
37 T1(), T2(), T(), xg(), xgd(), e0()
38 {
39 elasticforce_beam = beammodel;
40 n1=0; n2=0; sos2=SOS();
41 size = si;
42 lx = si.X(); ly = si.Y(); lz = si.Z();
43
44 mass = lx*ly*lz*rhoi;
45 //lx=1;ly=1;lz=1;
46
47 x_init = xc1.Append(xc2);
48 xg = xc1.Append(xc2);
49
50 nu = nui;
51 Em = Emi;
52 //rho = rhoi; //$ DR 2013-02-04 deleted rho from class element, do not use it here!
53 col = coli;
54
55 SetRectangularBeamParameters();
56 InitConstructor();
57 BuildDSMatrices();
58 Matrix Tinv = T;
59 if (!Tinv.Invert2()) {UO() << "ERROR: T matrix not invertible!!!\n";}
60 e0 = x_init;
61 x_init = Tinv * x_init;
62 x_init = x_init.Append(Vector(SOS())); //Velocity initial conditions can also be transformed by Tinv!!!
63
64 SetInitialCurvatures(); //must be done after x_init!!!
65
66 elementname = GetElementSpec();
67 };
68
69 //Element shares nodes with other elements, n1 and n2 are nodenumbers; element sets initial conditions for nodes
ANCFBeam3D(MBS * mbsi,const Vector & xc1,const Vector & xc2,int n1i,int n2i,double rhoi,double Emi,double nui,const Vector3D & si,const Vector3D & coli,int beammodel)70 ANCFBeam3D::ANCFBeam3D(MBS* mbsi, const Vector& xc1, const Vector& xc2, int n1i, int n2i, double rhoi, double Emi, double nui,
71 const Vector3D& si, const Vector3D& coli, int beammodel):
72 Body3D(mbsi),massmatrix(), Hmatrix(), SV(), DS(), x1(), x2(), x3(), w1(), w2(), w3(),
73 T1(), T2(), T(), xg(), xgd(), e0()
74 {
75 elasticforce_beam = beammodel;
76 n1=n1i; n2=n2i; sos2=0;
77 size = si;
78 lx = si.X(); ly = si.Y(); lz = si.Z();
79
80 //global_uo << "beam(" << n1i << "," << n2i << ")\n";
81
82 mass = lx*ly*lz*rhoi;
83 //lx=1;ly=1;lz=1;
84
85 x_init = xc1.Append(xc2);
86 xg = xc1.Append(xc2);
87
88 nu = nui;
89 Em = Emi;
90 //rho = rhoi; //$ DR 2013-02-04 deleted rho from class element, do not use it here!
91 col = coli;
92 SetRectangularBeamParameters();
93
94 InitConstructor();
95 BuildDSMatrices();
96 Matrix Tinv = T;
97 if (!Tinv.Invert2()) {UO() << "ERROR: T matrix not invertible!!!\n";}
98 e0 = x_init;
99 x_init = Tinv * x_init;
100 x_init = x_init.Append(Vector(SOS())); //Velocity initial conditions can also be transformed by Tinv!!!
101
102 SetInitialCurvatures(); //must be done after x_init!!!
103
104 elementname = GetElementSpec();
105 };
106
107 //Element shares nodes with other elements, n1 and n2 are nodenumbers; element sets initial conditions for nodes
ANCFBeam3D(MBS * mbsi,const Vector & xc1,const Vector & xc2,const Vector & vc1,const Vector & vc2,int n1i,int n2i,double rhoi,double Emi,double nui,const Vector3D & si,const Vector3D & coli,int beammodel)108 ANCFBeam3D::ANCFBeam3D(MBS* mbsi, const Vector& xc1, const Vector& xc2, const Vector& vc1, const Vector& vc2, int n1i, int n2i, double rhoi, double Emi, double nui,
109 const Vector3D& si, const Vector3D& coli, int beammodel):
110 Body3D(mbsi),massmatrix(), Hmatrix(), SV(), DS(), x1(), x2(), x3(), w1(), w2(), w3(),
111 T1(), T2(), T(), xg(), xgd(), e0()
112 {
113 elasticforce_beam = beammodel;
114 n1=n1i; n2=n2i; sos2=0;
115 size = si;
116 lx = si.X(); ly = si.Y(); lz = si.Z();
117
118 mass = lx*ly*lz*rhoi;
119 //lx=1;ly=1;lz=1;
120
121 x_init = xc1.Append(xc2);
122 xg = xc1.Append(xc2);
123 Vector v_init = vc1.Append(vc2);
124
125 nu = nui;
126 Em = Emi;
127 rho = rhoi;
128 col = coli;
129 SetRectangularBeamParameters();
130
131 InitConstructor();
132 BuildDSMatrices();
133 Matrix Tinv = T;
134 if (!Tinv.Invert2()) {UO() << "ERROR: T matrix not invertible!!!\n";}
135 e0 = x_init;
136 x_init = Tinv * x_init;
137 v_init = Tinv * v_init;
138 x_init = x_init.Append(v_init); //Velocity initial conditions can also be transformed by Tinv!!!
139
140 SetInitialCurvatures(); //must be done after x_init!!!
141
142 elementname = GetElementSpec();
143 };
144
145
146
GetElementData(ElementDataContainer & edc)147 void ANCFBeam3D::GetElementData(ElementDataContainer& edc) //fill in all element data
148 {
149
150 Body3D::GetElementData(edc);
151
152 ElementData ed;
153
154 ed.SetDouble(Em, "Youngs_modulus"); edc.Add(ed);
155 ed.SetDouble(nu, "Poisson_ratio"); edc.Add(ed);
156 ed.SetVector3D(lx, ly, lz, "Beam_dimensions"); edc.Add(ed);
157
158 ed.SetInt(n1, "Node_number1", 1, GetMBS()->NNodes()); edc.Add(ed);
159 ed.SetInt(n2, "Node_number2", 1, GetMBS()->NNodes()); edc.Add(ed);
160
161 ed.SetVector2D(concentratedmass1, concentratedmass2, "Node_masses"); edc.Add(ed);
162
163 ed.SetBool(elasticforce_beam!=0, "Elastic_line_model"); edc.Add(ed);
164
165 ed.SetDouble(beamEA, "Axial_stiffness"); edc.Add(ed);
166 ed.SetVector2D(beamEIy, beamEIz, "Bending_stiffness"); edc.Add(ed);
167 ed.SetVector2D(beamGAky, beamGAkz, "Shear_stiffness"); edc.Add(ed);
168 ed.SetDouble(beamGJkx, "Torsional_stiffness"); edc.Add(ed);
169
170 Matrix Tinv = T;
171 Vector xinit = x_init.SubVector(1, SOS());
172 Vector vinit = x_init.SubVector(SOS()+1, 2*SOS());
173 xinit = T*xinit;
174 vinit = T*vinit;
175
176 ed.SetVector3D(xinit( 1), xinit( 2), xinit( 3), "Node1_r"); edc.Add(ed);
177 ed.SetVector3D(xinit( 4), xinit( 5), xinit( 6), "Node1_rx"); edc.Add(ed);
178 ed.SetVector3D(xinit( 7), xinit( 8), xinit( 9), "Node1_ry"); edc.Add(ed);
179 ed.SetVector3D(xinit(10), xinit(11), xinit(12), "Node1_rz"); edc.Add(ed);
180
181 ed.SetVector3D(xinit( 1+12), xinit( 2+12), xinit( 3+12), "Node2_r"); edc.Add(ed);
182 ed.SetVector3D(xinit( 4+12), xinit( 5+12), xinit( 6+12), "Node2_rx"); edc.Add(ed);
183 ed.SetVector3D(xinit( 7+12), xinit( 8+12), xinit( 9+12), "Node2_ry"); edc.Add(ed);
184 ed.SetVector3D(xinit(10+12), xinit(11+12), xinit(12+12), "Node2_rz"); edc.Add(ed);
185
186 ed.SetVector3D(vinit( 1), vinit( 2), vinit( 3), "Node1_v"); edc.Add(ed);
187 ed.SetVector3D(vinit( 4), vinit( 5), vinit( 6), "Node1_vx"); edc.Add(ed);
188 ed.SetVector3D(vinit( 7), vinit( 8), vinit( 9), "Node1_vy"); edc.Add(ed);
189 ed.SetVector3D(vinit(10), vinit(11), vinit(12), "Node1_vz"); edc.Add(ed);
190
191 ed.SetVector3D(vinit( 1+12), vinit( 2+12), vinit( 3+12), "Node2_v"); edc.Add(ed);
192 ed.SetVector3D(vinit( 4+12), vinit( 5+12), vinit( 6+12), "Node2_vx"); edc.Add(ed);
193 ed.SetVector3D(vinit( 7+12), vinit( 8+12), vinit( 9+12), "Node2_vy"); edc.Add(ed);
194 ed.SetVector3D(vinit(10+12), vinit(11+12), vinit(12+12), "Node2_vz"); edc.Add(ed);
195
196 }
197
SetElementData(ElementDataContainer & edc)198 int ANCFBeam3D::SetElementData(ElementDataContainer& edc) //set element data according to ElementDataContainer
199 {
200 int rv = Body3D::SetElementData(edc);
201
202 InitConstructor();
203 SetRectangularBeamParameters();
204
205 sos2 = 0;
206 Em = 0;
207 elasticforce_beam = 0;
208
209 GetElemDataDouble(GetMBS(), edc, "Youngs_modulus", Em, 1);
210 GetElemDataDouble(GetMBS(), edc, "Poisson_ratio", nu, 1);
211
212 GetElemDataVector3D(GetMBS(), edc, "Beam_dimensions", lx, ly, lz, 1);
213
214 GetElemDataInt(GetMBS(), edc, "Node_number1", n1, 1);
215 GetElemDataInt(GetMBS(), edc, "Node_number2", n2, 1);
216
217
218 if (n1 < 1 || n1 > GetMBS()->NNodes() || n2 < 1 || n2 > GetMBS()->NNodes())
219 {
220 GetMBS()->EDCError("Invalid node numbers in ANCF beam element. Element has not been created!");
221 return 0;
222 }
223 /*
224 if (GetMBS()->GetNode(n1).SOS() != 12)
225 {
226 GetMBS()->GetNode(n1).SOS() = 12;
227 GetMBS()->EDCError("ANCFbeam Node does not have 12 coordinates; the size has been corrected");
228 }
229 if (GetMBS()->GetNode(n2).SOS() != 12)
230 {
231 GetMBS()->GetNode(n2).SOS() = 12;
232 GetMBS()->EDCError("ANCFbeam Node does not have 12 coordinates; the size has been corrected");
233 }*/
234
235 GetElemDataVector2D(GetMBS(), edc, "Node_masses", concentratedmass1, concentratedmass2, 0);
236
237 GetElemDataBool(GetMBS(), edc, "Elastic_line_model", elasticforce_beam, 0);
238 if (elasticforce_beam) elasticforce_beam = 3;
239
240 GetElemDataDouble(GetMBS(), edc, "Axial_stiffness", beamEA, 1);
241 GetElemDataVector2D(GetMBS(), edc, "Bending_stiffness", beamEIy, beamEIz, 1);
242 GetElemDataVector2D(GetMBS(), edc, "Shear_stiffness", beamGAky, beamGAkz, 1);
243 GetElemDataDouble(GetMBS(), edc, "Torsional_stiffness", beamGJkx, 1);
244
245 Vector xinit(SOS());
246 Vector vinit(SOS());
247
248 GetElemDataVector3D(GetMBS(), edc, "Node1_r" , xinit( 1), xinit( 2), xinit( 3), 1);
249 GetElemDataVector3D(GetMBS(), edc, "Node1_rx", xinit( 4), xinit( 5), xinit( 6), 1);
250 GetElemDataVector3D(GetMBS(), edc, "Node1_ry", xinit( 7), xinit( 8), xinit( 9), 1);
251 GetElemDataVector3D(GetMBS(), edc, "Node1_rz", xinit(10), xinit(11), xinit(12), 1);
252
253 GetElemDataVector3D(GetMBS(), edc, "Node2_r" , xinit( 1+12), xinit( 2+12), xinit( 3+12), 1);
254 GetElemDataVector3D(GetMBS(), edc, "Node2_rx", xinit( 4+12), xinit( 5+12), xinit( 6+12), 1);
255 GetElemDataVector3D(GetMBS(), edc, "Node2_ry", xinit( 7+12), xinit( 8+12), xinit( 9+12), 1);
256 GetElemDataVector3D(GetMBS(), edc, "Node2_rz", xinit(10+12), xinit(11+12), xinit(12+12), 1);
257
258 GetElemDataVector3D(GetMBS(), edc, "Node1_v" , vinit( 1), vinit( 2), vinit( 3), 1);
259 GetElemDataVector3D(GetMBS(), edc, "Node1_vx", vinit( 4), vinit( 5), vinit( 6), 1);
260 GetElemDataVector3D(GetMBS(), edc, "Node1_vy", vinit( 7), vinit( 8), vinit( 9), 1);
261 GetElemDataVector3D(GetMBS(), edc, "Node1_vz", vinit(10), vinit(11), vinit(12), 1);
262
263 GetElemDataVector3D(GetMBS(), edc, "Node2_v" , vinit( 1+12), vinit( 2+12), vinit( 3+12), 1);
264 GetElemDataVector3D(GetMBS(), edc, "Node2_vx", vinit( 4+12), vinit( 5+12), vinit( 6+12), 1);
265 GetElemDataVector3D(GetMBS(), edc, "Node2_vy", vinit( 7+12), vinit( 8+12), vinit( 9+12), 1);
266 GetElemDataVector3D(GetMBS(), edc, "Node2_vz", vinit(10+12), vinit(11+12), vinit(12+12), 1);
267
268 size = Vector3D(lx,ly,lz);
269 mass = lx*ly*lz*rho;
270
271
272 BuildDSMatrices();
273 Matrix Tinv = T;
274 if (!Tinv.Invert2()) {UO() << "ERROR: T matrix not invertible!!!\n";}
275 e0 = xinit;
276 xinit = Tinv * xinit;
277 vinit = Tinv * vinit;
278 x_init = xinit.Append(vinit); //Velocity initial conditions can also be transformed by Tinv!!!
279
280 SetInitialCurvatures(); //must be done after x_init!!!
281
282
283 return rv;
284 }
285
286
CheckConsistency(mystr & errorstr)287 int ANCFBeam3D::CheckConsistency(mystr& errorstr) //rv==0 --> OK, rv==1 --> can not compute, rv==2 --> can not draw and not compute
288 {
289 int rv = Element::CheckConsistency(errorstr);
290 if (rv) return rv;
291
292
293 if (n1 && n2)
294 {
295 if (GetMBS()->GetNode(n1).SOS() != 12)
296 {
297 GetMBS()->GetNode(n1).SOS() = 12;
298 errorstr += mystr("ANCFbeam ")+ mystr(GetOwnNum()) + mystr(": Node does not have 12 coordinates; the size has been corrected");
299 }
300 if (GetMBS()->GetNode(n2).SOS() != 12)
301 {
302 GetMBS()->GetNode(n2).SOS() = 12;
303 errorstr += mystr("ANCFbeam ")+ mystr(GetOwnNum()) + mystr(": Node does not have 12 coordinates; the size has been corrected");
304 }
305 }
306
307 return rv;
308 }
309
310
311
LinkToElements()312 void ANCFBeam3D::LinkToElements()
313 {
314 if (SOSowned() == 0)
315 {
316 const Node& node1 = GetMBS()->GetNode(n1);
317 const Node& node2 = GetMBS()->GetNode(n2);
318 for (int i=1; i <= node1.SOS(); i++)
319 {
320 AddLTG(node1.Get(i));
321 }
322 for (int i=1; i <= node2.SOS(); i++)
323 {
324 AddLTG(node2.Get(i));
325 }
326 for (int i=1; i <= node1.SOS(); i++)
327 {
328 AddLTG(node1.Get(i+node1.SOS()));
329 }
330 for (int i=1; i <= node2.SOS(); i++)
331 {
332 AddLTG(node2.Get(i+node2.SOS()));
333 }
334 }
335 }
336
BuildDSMatrices()337 void ANCFBeam3D::BuildDSMatrices()
338 {
339
340 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
341 //for original ANCF:
342 orderx = 9; //max 9x5, sonst array grad zu klein!!!!
343 orderyz = 5; //9x5 und 7x3 ergibt fast das gleiche!!!
344
345 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
346 //for Arend Schwab ANCF
347 //for all integration points:
348 orderWl = 5; //5; 5 and 6 coincide with 8 up to 10 digits, 7=8
349 orderWs = 2; //2; more than 2 gives locking, 1 is rather inaccurate
350 orderWsHR=4; //4; for Hellinger Reissner, 4 is exact
351 orderWt = 2; //2; 1=2=4, 1 does not work in L-shape!!!
352 orderWb = 6; //3; 3=4=6, 2 gives shear locking
353
354 factstiffWl = 1; //reduce stiffness of thickness modes, standard==1
355
356 GetIntegrationRuleLobatto(x1,w1,orderWl); int nipWl = x1.Length();
357 //GetIntegrationRuleLobatto(x1,w1,orderWs); int nipWs = x1.Length();
358 GetIntegrationRuleLobatto(x1,w1,orderWsHR); int nipWsHR = x1.Length();
359 GetIntegrationRule(x1,w1,orderWt); int nipWt = x1.Length();
360 GetIntegrationRule(x1,w1,orderWb); int nipWb = x1.Length();
361
362 Wlepsx0.SetLen(nipWl);
363 Wlepsy0.SetLen(nipWl);
364 Wlepsz0.SetLen(nipWl);
365 Wlgamyz0.SetLen(nipWl);
366 WsHRk10.SetLen(nipWsHR);
367 WsHRk20.SetLen(nipWsHR);
368 Wtkapx0.SetLen(nipWt);
369 Wbkapy0.SetLen(nipWb);
370 Wbkapz0.SetLen(nipWb);
371
372 Wlepsx0.SetAll(0);
373 Wlepsy0.SetAll(0);
374 Wlepsz0.SetAll(0);
375 Wlgamyz0.SetAll(0);
376 WsHRk10.SetAll(0);
377 WsHRk20.SetAll(0);
378 Wtkapx0.SetAll(0);
379 Wbkapy0.SetAll(0);
380 Wbkapz0.SetAll(0);
381
382 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
383
384 GetIntegrationRule(x1,w1,orderx);
385 GetIntegrationRule(x2,w2,orderyz);
386 GetIntegrationRule(x3,w3,orderyz);
387 Matrix3D jac, jacinv;
388 int kx1 = x2.Length()*x3.Length();
389 int kx2 = x3.Length();
390
391 //UO() << "Initialize Element\n";
392
393 for (int i1=1; i1<=x1.GetLen(); i1++)
394 {
395 for (int i2=1; i2<=x2.GetLen(); i2++)
396 {
397 for (int i3=1; i3<=x3.GetLen(); i3++)
398 {
399 int ind = (i1-1)*kx1+(i2-1)*kx2+(i3-1);
400 //UO() << "ind=" << ind << "\n";
401
402 Vector3D p(x1(i1),x2(i2),x3(i3));
403
404 GetDSMatrix0(p,DS);
405
406 GetJacobi(jac,p,DS,x_init);
407 jacdet[ind] = jac.Det();
408 jac.GetInverse(jacinv);
409 jacinv = jacinv.GetTp();
410
411 grad[ind].Init();
412 grad[ind].SetSize(3,NS());
413 Mult(jacinv, DS, grad[ind]);
414 }
415 }
416 }
417 //Build T-matrices:
418 T1.Init();
419 T2.Init();
420 T1.SetSize(Dim()*NS()/2,Dim()*NS()/2);
421 T1.SetAll(0);
422 T2=T1;
423 Vector3D p1(-1,0,0);
424 Vector3D p2( 1,0,0);
425
426 GetDSMatrix0(p1,DS);
427 GetJacobi(jac1,p1,DS,x_init);
428 jac1 = jac1.GetTp();
429 GetDSMatrix0(p2,DS);
430 GetJacobi(jac2,p2,DS,x_init);
431 jac2 = jac2.GetTp();
432
433 //UO() << "jac1A=\n" << jac1 << "\n";
434 //UO() << "jac2A=\n" << jac2 << "\n";
435
436 for (int i=1; i <= Dim(); i++)
437 {
438 for (int j=1; j <= Dim(); j++)
439 {
440 jac1(i,j) = x_init((i-1)*3+j+3);
441 jac2(i,j) = x_init((i-1)*3+j+12+3);
442 }
443 }
444
445 T1(1,1) = 1; T1(2,2) = 1; T1(3,3) = 1;
446 T2 = T1;
447 for (int i=1; i <= Dim(); i++)
448 {
449 for (int j=1; j <= Dim(); j++)
450 {
451 T1(i*3+1,j*3+1) = jac1(i,j);
452 T1(i*3+2,j*3+2) = jac1(i,j);
453 T1(i*3+3,j*3+3) = jac1(i,j);
454 T2(i*3+1,j*3+1) = jac2(i,j);
455 T2(i*3+2,j*3+2) = jac2(i,j);
456 T2(i*3+3,j*3+3) = jac2(i,j);
457 }
458 }
459
460
461 T.Init();
462 T.SetSize(SOS(),SOS());
463 T.SetAll(0);
464 T.SetSubmatrix(T1,1,1);
465 T.SetSubmatrix(T2,1+T1.Getrows(),1+T1.Getcols());
466
467 //UO() << "T=" << T << "\n";
468 };
469
SetBeamParameters(double EA,double EIw,double EIv,double GJ,double GA)470 void ANCFBeam3D::SetBeamParameters(double EA, double EIw, double EIv, double GJ, double GA)
471 {
472 //Em and nu already set
473 //old function that assumes rectangular cross-section
474 double ky = 10*(1+nu)/(12.+11.*nu); //shear correction factors (square): 10*(1+nu)/(12.+11.*nu)
475 double kz = 10*(1+nu)/(12.+11.*nu); //shear correction factors (square): 10*(1+nu)/(12.+11.*nu)
476 double kx = 0.8436; //shear correction factor torsion : 0.8436 (square)
477
478 double A = ly*lz;
479 double Gm = Em / (2.*(1+nu));
480
481 beamEA = EA;
482 beamEIy = EIw;
483 beamEIz = EIv;
484 beamGJkx = GJ*kx;
485 beamGAky = GA*ky;
486 beamGAkz = GA*kz;
487
488 SetInitialCurvatures();
489 }
490
SetBeamParameters2(double beamEAi,double beamEIyi,double beamEIzi,double beamGJkxi,double beamGAkyi,double beamGAkzi)491 void ANCFBeam3D::SetBeamParameters2(double beamEAi, double beamEIyi, double beamEIzi, double beamGJkxi,
492 double beamGAkyi, double beamGAkzi)
493 {
494 beamEA = beamEAi;
495 beamEIy = beamEIyi;
496 beamEIz = beamEIzi;
497 beamGJkx = beamGJkxi;
498 beamGAky = beamGAkyi;
499 beamGAkz = beamGAkzi;
500
501 SetInitialCurvatures();
502 }
503
SetRectangularBeamParameters()504 void ANCFBeam3D::SetRectangularBeamParameters() //set beam parameters for rectangular cross-section, Schwab-Meijaard model
505 {
506 double A = ly*lz;
507 double Gm = Em / (2.*(1+nu));
508 double GA = Gm*A;
509
510 beamEIy = Em * ly*Cub(lz)/12.;
511 beamEIz = Em * lz*Cub(ly)/12.;
512 beamEA = Em * A;
513
514 double GIT = Gm * ly*lz*(Sqr(ly) + Sqr(lz))/12.; //Schwab/Meijaard
515 //GIT = Gm * 0.140 * Sqr(ly)*Sqr(lz); //acc. to Mang or Gieck for ly==lz
516
517 double ky = 10*(1+nu)/(12.+11.*nu); //shear correction factors (square): 10*(1+nu)/(12.+11.*nu)
518 double kz = 10*(1+nu)/(12.+11.*nu); //shear correction factors (square): 10*(1+nu)/(12.+11.*nu)
519 double kx = 0.8436; //shear correction factor torsion : 0.8436 (square)
520
521 beamGJkx = GIT * kx;
522 beamGAky = GA*ky;
523 beamGAkz = GA*kz;
524
525 //UO() << "GAky=" << beamGAky << ", GAkz=" << beamGAkz << "\n";
526 }
527
528
529
ApplyT(Vector & v) const530 void ANCFBeam3D::ApplyT(Vector& v) const
531 {
532 static Vector x;
533 x = v;
534 for (int i=1; i<=3; i++)
535 {
536 for (int j=1; j<=3; j++)
537 {
538 v((i-1)*3+j+3) = jac1(i,1)*x(0+j+3) + jac1(i,2)*x(3+j+3) + jac1(i,3)*x(6+j+3);
539 v((i-1)*3+j+3+12) = jac2(i,1)*x(0+j+3+12) + jac2(i,2)*x(3+j+3+12) + jac2(i,3)*x(6+j+3+12);
540 }
541 }
542 //global_uo << "T*v2=" << v << "\n";
543 }
544
ApplyTD(Vector & v) const545 void ANCFBeam3D::ApplyTD(Vector& v) const
546 {
547 static Vector x;
548 x = v;
549 for (int i=1; i<=3; i++)
550 {
551 for (int j=1; j<=3; j++)
552 {
553 v((i-1)*3+j+3) = jac1(i,1)*x(0+j+3) + jac1(i,2)*x(3+j+3) + jac1(i,3)*x(6+j+3);
554 v((i-1)*3+j+3+12) = jac2(i,1)*x(0+j+3+12) + jac2(i,2)*x(3+j+3+12) + jac2(i,3)*x(6+j+3+12);
555 }
556 }
557 }
558
ApplyTtp(Vector & v) const559 void ANCFBeam3D::ApplyTtp(Vector& v) const
560 {
561 //v=T.GetTp()*v; return;
562 static Vector x;
563 x = v;
564 for (int i=1; i<=3; i++)
565 {
566 for (int j=1; j<=3; j++)
567 {
568 v((i-1)*3+j+3) = jac1(1,i)*x(0+j+3) + jac1(2,i)*x(3+j+3) + jac1(3,i)*x(6+j+3);
569 v((i-1)*3+j+3+12) = jac2(1,i)*x(0+j+3+12) + jac2(2,i)*x(3+j+3+12) + jac2(3,i)*x(6+j+3+12);
570 }
571 }
572 }
573
ApplyTtp(Matrix & m) const574 void ANCFBeam3D::ApplyTtp(Matrix& m) const
575 {
576 //v=T.GetTp()*v; return;
577 static Vector x;
578 int sns = SOS();
579 x.SetLen(sns);
580
581 for (int k = 1; k <= m.Getcols(); k++)
582 {
583 for (int i = 1+3; i <= sns; i++)
584 {
585 x(i) = m(i,k);
586 }
587 for (int i=1; i<=3; i++)
588 {
589 for (int j=1; j<=3; j++)
590 {
591 m((i-1)*3+j+3,k) = jac1(1,i)*x(0+j+3) + jac1(2,i)*x(3+j+3) + jac1(3,i)*x(6+j+3);
592 m((i-1)*3+j+3+12,k) = jac2(1,i)*x(0+j+3+12) + jac2(2,i)*x(3+j+3+12) + jac2(3,i)*x(6+j+3+12);
593 }
594 }
595 }
596 }
597
GetS0(Vector & sf,const Vector3D & ploc) const598 void ANCFBeam3D::GetS0(Vector& sf, const Vector3D& ploc) const
599 {
600 double xb = ploc.X();
601 double yb = ploc.Y();
602 double zb = ploc.Z();
603 sf.SetLen(8);
604 double xb2 = xb*xb;
605 double xb3 = xb2*xb;
606 sf(1) = 1.0/2.0-3.0/4.0*xb+xb3/4.0;
607 sf(2) = lx*(1.0-xb-xb2+xb3)/8.0;
608 sf(3) = -yb*ly*(-1.0+xb)/4.0;
609 sf(4) = -zb*lz*(-1.0+xb)/4.0;
610 sf(5) = 1.0/2.0+3.0/4.0*xb-xb3/4.0;
611 sf(6) = lx*(1.0+xb)*(1.0+xb)*(-1.0+xb)/8.0;
612 sf(7) = (1.0+xb)*yb*ly/4.0;
613 sf(8) = (1.0+xb)*zb*lz/4.0;
614 }
615
GetDSMatrix0(const Vector3D & ploc,Matrix & sf) const616 void ANCFBeam3D::GetDSMatrix0(const Vector3D& ploc, Matrix& sf) const
617 {
618 double xb = ploc.X();
619 double yb = ploc.Y();
620 double zb = ploc.Z();
621 sf.SetSize(3,8);
622
623 sf(1,1) = -3.0/4.0+3.0/4.0*xb*xb;
624 sf(1,2) = lx*(-1.0-2.0*xb+3.0*xb*xb)/8.0;
625 sf(1,3) = -yb*ly/4.0;
626 sf(1,4) = -zb*lz/4.0;
627 sf(1,5) = 3.0/4.0-3.0/4.0*xb*xb;
628 sf(1,6) = lx*(1.0+xb)*(-1.0+xb)/4.0+lx*(1.0+xb)*(1.0+xb)/8.0;
629 sf(1,7) = yb*ly/4.0;
630 sf(1,8) = zb*lz/4.0;
631 sf(2,1) = 0.0;
632 sf(2,2) = 0.0;
633 sf(2,3) = -ly*(-1.0+xb)/4.0;
634 sf(2,4) = 0.0;
635 sf(2,5) = 0.0;
636 sf(2,6) = 0.0;
637 sf(2,7) = (1.0+xb)*ly/4.0;
638 sf(2,8) = 0.0;
639 sf(3,1) = 0.0;
640 sf(3,2) = 0.0;
641 sf(3,3) = 0.0;
642 sf(3,4) = -lz*(-1.0+xb)/4.0;
643 sf(3,5) = 0.0;
644 sf(3,6) = 0.0;
645 sf(3,7) = 0.0;
646 sf(3,8) = (1.0+xb)*lz/4.0;
647
648 }
649
GetS0x(Vector & sfx,const Vector3D & ploc) const650 void ANCFBeam3D::GetS0x(Vector& sfx, const Vector3D& ploc) const
651 {
652 //at y=z=0 !!!!!!!!!!!!!!!!!!!!!
653 double xb = ploc.X();
654 sfx.SetLen(NS());
655 double f = 2./lx;
656
657 sfx(1) = f*(-3.0/4.0+3.0/4.0*xb*xb);
658 sfx(2) = f*(lx*(-1.0-2.0*xb+3.0*xb*xb)/8.0);
659 sfx(3) = 0.;
660 sfx(4) = 0.;
661 sfx(5) = f*(3.0/4.0-3.0/4.0*xb*xb);
662 sfx(6) = f*(lx*(1.0+xb)*(-1.0+xb)/4.0+lx*(1.0+xb)*(1.0+xb)/8.0);
663 sfx(7) = 0.;
664 sfx(8) = 0.;
665
666 }
667
GetS0y(Vector & sfx,const Vector3D & ploc) const668 void ANCFBeam3D::GetS0y(Vector& sfx, const Vector3D& ploc) const
669 {
670 double xb = ploc.X();
671 sfx.SetLen(NS());
672 double f = 2./ly;
673 sfx(1) = 0.0;
674 sfx(2) = 0.0;
675 sfx(3) = -f*ly*(-1.0+xb)/4.0;
676 sfx(4) = 0.0;
677 sfx(5) = 0.0;
678 sfx(6) = 0.0;
679 sfx(7) = f*(1.0+xb)*ly/4.0;
680 sfx(8) = 0.0;
681 }
682
GetS0z(Vector & sfx,const Vector3D & ploc) const683 void ANCFBeam3D::GetS0z(Vector& sfx, const Vector3D& ploc) const
684 {
685 double xb = ploc.X();
686 sfx.SetLen(NS());
687 double f = 2./lz;
688
689 sfx(1) = 0.0;
690 sfx(2) = 0.0;
691 sfx(3) = 0.0;
692 sfx(4) = -f*lz*(-1.0+xb)/4.0;
693 sfx(5) = 0.0;
694 sfx(6) = 0.0;
695 sfx(7) = 0.0;
696 sfx(8) = f*(1.0+xb)*lz/4.0;
697 }
698
GetS0yx(Vector & sfx,const Vector3D & ploc) const699 void ANCFBeam3D::GetS0yx(Vector& sfx, const Vector3D& ploc) const
700 {
701 double xb = ploc.X();
702 sfx.SetLen(NS());
703 double f = 4./(ly*lx);
704 sfx(1) = 0.0;
705 sfx(2) = 0.0;
706 sfx(3) = -f*ly/4.0;
707 sfx(4) = 0.0;
708 sfx(5) = 0.0;
709 sfx(6) = 0.0;
710 sfx(7) = f*ly/4.0;
711 sfx(8) = 0.0;
712 }
713
GetS0zx(Vector & sfx,const Vector3D & ploc) const714 void ANCFBeam3D::GetS0zx(Vector& sfx, const Vector3D& ploc) const
715 {
716 double xb = ploc.X();
717 sfx.SetLen(NS());
718 double f = 4./(lz*lx);
719 sfx(1) = 0.0;
720 sfx(2) = 0.0;
721 sfx(3) = 0.0;
722 sfx(4) = -f*lz/4.0;
723 sfx(5) = 0.0;
724 sfx(6) = 0.0;
725 sfx(7) = 0.0;
726 sfx(8) = f*lz/4.0;
727 }
728
GetS0xx(Vector & sfxx,const Vector3D & ploc) const729 void ANCFBeam3D::GetS0xx(Vector& sfxx, const Vector3D& ploc) const
730 {
731 double xb = ploc.X();
732 sfxx.SetLen(NS());
733 double f = 4./Sqr(lx);
734 sfxx(1) = f*3.0/2.0*xb;
735 sfxx(2) = f*(-1.0+3.0*xb)*lx/4.0;
736 sfxx(3) = 0.0;
737 sfxx(4) = 0.0;
738 sfxx(5) = -f*3.0/2.0*xb;
739 sfxx(6) = f*(1.0+3.0*xb)*lx/4.0;
740 sfxx(7) = 0.0;
741 sfxx(8) = 0.0;
742
743 }
744
745
746
747
GetRot(Matrix3D & rot,const Vector & xg) const748 void ANCFBeam3D::GetRot(Matrix3D& rot, const Vector& xg) const
749 {
750
751 Vector3D r1(xg(13)-xg(1),xg(14)-xg(2),xg(15)-xg(3)); //r_B - r_A
752 Vector3D r2(0.5*(xg(12+7)+xg(7)),0.5*(xg(12+8)+xg(8)),0.5*(xg(12+9)+xg(9))); //0.5*(r,y_B + r,y_A)
753 Vector3D r3;
754
755 r1.Normalize();
756 double h = r2*r1;
757 r2 -= h*r1;
758 r2.Normalize();
759 r3 = r1.Cross(r2);
760
761 rot.Set(r1,r2,r3);
762 }
763
GetPos(const Vector3D & p_loc) const764 Vector3D ANCFBeam3D::GetPos(const Vector3D& p_loc) const
765 {
766 static Vector xg;
767 xg.SetLen(SOS());
768 GetCoordinates(xg);
769 ApplyT(xg);
770 Vector3D p0=p_loc;
771 p0.Scale(0.5*lx,0.5*ly,0.5*lz);
772 static Vector SV;
773 GetS0(SV, p0);
774 Vector3D p(0.,0.,0.);
775 for (int i = 1; i <= 3; i++)
776 {
777 for (int j = 1; j <= 8; j++)
778 {
779 p(i) += SV(j)*xg((j-1)*3+i);
780 }
781 }
782 return p;
783 };
784
GetVel(const Vector3D & p_loc) const785 Vector3D ANCFBeam3D::GetVel(const Vector3D& p_loc) const
786 {
787 static Vector xg;
788 xg.SetLen(SOS());
789 GetCoordinatesP(xg);
790 ApplyT(xg);
791 Vector3D p0=p_loc;
792 p0.Scale(0.5*lx,0.5*ly,0.5*lz);
793 static Vector SV;
794 GetS0(SV, p0);
795 Vector3D p(0.,0.,0.);
796 for (int i = 1; i <= 3; i++)
797 {
798 for (int j = 1; j <= 8; j++)
799 {
800 p(i) += SV(j)*xg((j-1)*3+i);
801 }
802 }
803 return p;
804 };
805
GetPosD(const Vector3D & p_loc) const806 Vector3D ANCFBeam3D::GetPosD(const Vector3D& p_loc) const
807 {
808 static Vector xgd;
809 xgd.SetLen(SOS());
810 GetDrawCoordinates(xgd);
811 xgd = T*xgd;
812 Vector3D p0=p_loc;
813 p0.Scale(0.5*lx,0.5*ly,0.5*lz);
814 static Vector SV;
815 GetS0(SV, p0);
816 Vector3D p(0.,0.,0.);
817 for (int i = 1; i <= 3; i++)
818 {
819 for (int j = 1; j <= 8; j++)
820 {
821 p(i) += SV(j)*xgd((j-1)*3+i);
822 }
823 }
824 return p;
825 };
826
827 //in reference element coordinates (-1..1)
GetPos0D(const Vector3D & p_loc,double def_scale) const828 Vector3D ANCFBeam3D::GetPos0D(const Vector3D& p_loc, double def_scale) const
829 {
830 static Vector xgd;
831 xgd.SetLen(SOS());
832 GetDrawCoordinates(xgd);
833 //xgd = T*xgd;
834 ApplyTD(xgd);
835
836 int i;
837 for (i=1; i <=SOS(); i++)
838 {
839 xgd(i) = def_scale*(xgd(i)-x_init(i))+x_init(i);
840 }
841
842 static Vector SV;
843 GetS0(SV, p_loc);
844 Vector3D p(0.,0.,0.);
845 for (i = 1; i <= 3; i++)
846 {
847 for (int j = 1; j <= 8; j++)
848 {
849 p(i) += SV(j)*xgd((j-1)*3+i);
850 }
851 }
852 return p;
853 };
854
GetVelD(const Vector3D & p_loc) const855 Vector3D ANCFBeam3D::GetVelD(const Vector3D& p_loc) const
856 {
857 static Vector xgd;
858 xgd.SetLen(SOS());
859 GetDrawCoordinatesP(xgd);
860 xgd = T*xgd;
861 Vector3D p0=p_loc;
862 p0.Scale(0.5*lx,0.5*ly,0.5*lz);
863 static Vector SV;
864 GetS0(SV, p0);
865 Vector3D p(0.,0.,0.);
866 for (int i = 1; i <= 3; i++)
867 {
868 for (int j = 1; j <= 8; j++)
869 {
870 p(i) += SV(j)*xgd((j-1)*3+i);
871 }
872 }
873 return p;
874 };
875
876
GetPosx(const Vector3D & p_loc) const877 Vector3D ANCFBeam3D::GetPosx(const Vector3D& p_loc) const
878 {
879 static Vector xg;
880 xg.SetLen(SOS());
881 GetCoordinates(xg);
882 ApplyT(xg);
883 Vector3D p0=p_loc;
884 p0.Scale(0.5*lx,0.5*ly,0.5*lz);
885 static Vector SV;
886 GetS0x(SV, p0);
887 Vector3D p(0.,0.,0.);
888 for (int i = 1; i <= 3; i++)
889 {
890 for (int j = 1; j <= 8; j++)
891 {
892 p(i) += SV(j)*xg((j-1)*3+i);
893 }
894 }
895 return p;
896 };
897
GetDisplacement(const Vector3D & p_loc) const898 Vector3D ANCFBeam3D::GetDisplacement(const Vector3D& p_loc) const
899 {
900 static Vector xg;
901 xg.SetLen(SOS());
902 GetCoordinates(xg);
903
904 for (int i=1; i <= SOS(); i++)
905 xg(i) -= x_init(i);
906
907 ApplyT(xg);
908 Vector3D p0=p_loc;
909 p0.Scale(0.5*lx,0.5*ly,0.5*lz);
910 static Vector SV;
911 GetS0(SV, p0);
912 Vector3D p(0.,0.,0.);
913 for (int i = 1; i <= 3; i++)
914 {
915 for (int j = 1; j <= 8; j++)
916 {
917 p(i) += SV(j)*xg((j-1)*3+i);
918 }
919 }
920 return p;
921 };
922
GetDisplacementD(const Vector3D & p_loc) const923 Vector3D ANCFBeam3D::GetDisplacementD(const Vector3D& p_loc) const
924 {
925 static Vector xgd;
926 xgd.SetLen(SOS());
927 GetDrawCoordinates(xgd);
928
929 for (int i=1; i <= SOS(); i++)
930 xgd(i) -= x_init(i);
931
932 xgd = T*xgd;
933 Vector3D p0=p_loc;
934 p0.Scale(0.5*lx,0.5*ly,0.5*lz);
935 static Vector SV;
936 GetS0(SV, p0);
937 Vector3D p(0.,0.,0.);
938 for (int i = 1; i <= 3; i++)
939 {
940 for (int j = 1; j <= 8; j++)
941 {
942 p(i) += SV(j)*xgd((j-1)*3+i);
943 }
944 }
945 return p;
946 };
947
948
949
GetH(Matrix & H)950 void ANCFBeam3D::GetH(Matrix& H)
951 {
952 if (Hmatrix.Getrows() == SOS())
953 {
954 H = Hmatrix;
955 return;
956 }
957 else
958 {
959 int dim = Dim();
960 int ns = NS();
961
962 H.SetSize(ns*dim,dim);
963 H.SetAll(0);
964 Matrix3D jac;
965
966 DS.SetSize(3,ns);
967 SV.SetLen(8);
968
969 GetIntegrationRule(x1,w1,3); //3x1x1 !!!!!
970 GetIntegrationRule(x2,w2,1);
971 GetIntegrationRule(x3,w3,1);
972
973 for (int i1=1; i1<=x1.GetLen(); i1++)
974 {
975 for (int i2=1; i2<=x2.GetLen(); i2++)
976 {
977 for (int i3=1; i3<=x3.GetLen(); i3++)
978 {
979 Vector3D p(x1(i1),x2(i2),x3(i3));
980 GetS0(SV,p);
981
982 GetDSMatrix0(p,DS);
983 GetJacobi(jac,p,DS, e0);
984 double jacdet = jac.Det();
985 double fact = fabs (jacdet) * w1(i1)*w2(i2)*w3(i3);
986
987 for (int i=0; i<ns; i++)
988 {
989 for (int j=1; j<=dim; j++)
990 {
991 H(i*dim+j,j)+=fact*SV(i+1);
992 }
993 }
994 }
995 }
996 }
997 ApplyTtp(H);
998 //H=T.GetTp()*H;
999 Hmatrix = H;
1000 }
1001 }
1002
EvalM(Matrix & m,double t)1003 void ANCFBeam3D::EvalM(Matrix& m, double t)
1004 {
1005 if (massmatrix.Getcols() == SOS())
1006 {
1007 m = massmatrix;
1008 return;
1009 }
1010 else
1011 {
1012 int dim = Dim();
1013 int ns = NS();
1014
1015 SV.SetLen(ns);
1016 Matrix3D jac;
1017
1018 Matrix HL(ns*dim,dim);
1019 DS.SetSize(3,ns);
1020
1021 Vector x1,x2,x3,w1,w2,w3;
1022
1023 GetIntegrationRule(x1,w1,6); //optimal: 6x3x3, 6x1x1 geht auch!!!!
1024 GetIntegrationRule(x2,w2,3);
1025 GetIntegrationRule(x3,w3,3);
1026 //UO() << "intrule x1=" << x1 << ", w1=" << w1 << "\n";
1027 //UO() << "intrule x2=" << x2 << ", w2=" << w2 << "\n";
1028
1029 for (int i1=1; i1<=x1.GetLen(); i1++)
1030 {
1031 for (int i2=1; i2<=x2.GetLen(); i2++)
1032 {
1033 for (int i3=1; i3<=x3.GetLen(); i3++)
1034 {
1035 Vector3D p(x1(i1),x2(i2),x3(i3));
1036 GetS0(SV,p);
1037
1038 for (int i=0; i<ns; i++)
1039 {
1040 for (int j=1; j<=dim; j++)
1041 {
1042 HL(i*dim+j,j)=SV(i+1);
1043 }
1044 }
1045
1046 GetDSMatrix0(p,DS);
1047 //UO() << "DS=\n" << DS << "\n";
1048 GetJacobi(jac,p,DS,e0);
1049 //UO() << "jac=\n" << jac << "\n";
1050
1051 double jacdet = jac.Det();
1052 //UO() << "jacdet=\n" << jacdet << ", rho=" << rho << "\n";
1053 m += fabs (jacdet) * rho * w1(i1)*w2(i2)*w3(i3) * (HL*HL.GetTp());
1054 }
1055 }
1056 }
1057 m(1,1) += concentratedmass1;
1058 m(2,2) += concentratedmass1;
1059 m(3,3) += concentratedmass1;
1060 m(1+12,1+12) += concentratedmass2;
1061 m(2+12,2+12) += concentratedmass2;
1062 m(3+12,3+12) += concentratedmass2;
1063
1064 massmatrix = T.GetTp()*m*T;
1065 m = massmatrix;
1066 }
1067 //Matrix minv = m;
1068 //UO() << "Mass matrix invertable=" << minv.Invert2() << "\n";
1069 //UO() << "m=" << m << "\n";
1070 };
1071
GetTau(const Vector & xg) const1072 double ANCFBeam3D::GetTau(const Vector& xg) const
1073 {
1074 //simplified torsion:
1075 Vector3D ryA(xg(7),xg(8),xg(9));
1076 Vector3D ryB(xg(12+7),xg(12+8),xg(12+9));
1077 Vector3D rA(xg(1),xg(2),xg(3));
1078 Vector3D rB(xg(12+1),xg(12+2),xg(12+3));
1079 Vector3D rAB = rB-rA;
1080
1081 //Project into normal plane:
1082 rAB.GramSchmidt(ryA);
1083 rAB.GramSchmidt(ryB);
1084
1085 //torsion
1086 double ryAryB = (ryA.Cross(ryB)).Norm();
1087 double val = ryAryB/(ryA.Norm()*ryB.Norm());
1088 double tau;
1089 if (fabs(val) >= 1) tau = val;
1090 //else tau = acos(ryAryB/(ryA.Norm()*ryB.Norm()));
1091 else tau = asin(val);
1092 return tau;
1093 }
1094
GetDeltaTau(const Vector & xg,Vector & deltatau) const1095 void ANCFBeam3D::GetDeltaTau(const Vector& xg, Vector& deltatau) const
1096 {
1097 static Vector xgdiff;
1098 xgdiff = xg;
1099
1100 deltatau.SetLen(SOS());
1101 double tau0 = GetTau(xgdiff);
1102 double eps = 1e-7;
1103 for (int i = 1; i <= SOS(); i++)
1104 {
1105 xgdiff(i) += eps;
1106 deltatau(i) = 1./eps*(GetTau(xgdiff) - tau0);
1107 xgdiff(i) -= eps;
1108 }
1109 }
1110
1111
GetDeltaKappa(const double & x,const Vector & xg,Vector & dkappa,double & kappa) const1112 void ANCFBeam3D::GetDeltaKappa(const double& x, const Vector& xg, Vector& dkappa, double& kappa) const
1113 {
1114 int dim = Dim();
1115 int ns = NS();
1116
1117 static Vector SVx;
1118 static Vector SVxx;
1119 SVx.SetLen(NS());
1120 SVxx.SetLen(NS());
1121
1122 GetS0x(SVx,x);
1123 GetS0xx(SVxx,x);
1124
1125 Vector3D rx(0.,0.,0.);
1126 for (int i = 1; i <= Dim(); i++)
1127 {
1128 for (int j = 1; j <= NS(); j++)
1129 {
1130 rx(i) += SVx(j)*xg((j-1)*3+i);
1131 }
1132 }
1133
1134 Vector3D rxx(0.,0.,0.);
1135 for (int i = 1; i <= Dim(); i++)
1136 {
1137 for (int j = 1; j <= NS(); j++)
1138 {
1139 rxx(i) += SVxx(j)*xg((j-1)*3+i);
1140 }
1141 }
1142
1143 //Vector3D rx = GetPosx0(x,xg);
1144 //Vector3D rxx = GetPosxx0(x,xg);
1145 double rxn = rx.Norm();
1146
1147 if (rxn == 0) {dkappa *= 0; return;}
1148 Vector3D rxcrxx = rx.Cross(rxx);
1149 double f = (rxcrxx).Norm();
1150 double g = Cub(rxn);
1151
1152 if (rxn == 0) kappa=0;
1153 else kappa = f/g;
1154
1155 double g2inv = 1./Sqr(g);
1156 double fn = f*g2inv*(3*rxn);
1157 double gn = g*g2inv;
1158 if (f != 0) {gn/=f;}
1159
1160
1161 Vector3D t1;
1162 double df, dg;
1163
1164 for (int i=1; i <= dim; i++)
1165 {
1166 for (int j=1; j <= ns; j++)
1167 {
1168 //dr,x/de x r,xx + r,x x dr,xx/de:
1169 switch (i) {
1170 case 1:
1171 t1.X() = 0;
1172 t1.Y() =-(SVx(j)*rxx.Z()-SVxx(j)*rx.Z());
1173 t1.Z() = SVx(j)*rxx.Y()-SVxx(j)*rx.Y(); break; //dy=dz=0
1174 case 2:
1175 t1.X() = SVx(j)*rxx.Z()-SVxx(j)*rx.Z();
1176 t1.Y() = 0;
1177 t1.Z() =-(SVx(j)*rxx.X()-SVxx(j)*rx.X()); break; //dx=dz=0
1178 case 3:
1179 t1.X() =-(SVx(j)*rxx.Y()-SVxx(j)*rx.Y());
1180 t1.Y() = SVx(j)*rxx.X()-SVxx(j)*rx.X();
1181 t1.Z() = 0; break; //dx=dy=0
1182 //case 1: t1.X() = 0; t1.Y() =-dx*rz; t1.Z() = dx*ry; break; //dy=dz=0
1183 //case 2: t1.X() = dy*rz; t1.Y() = 0; t1.Z() =-dy*rx; break; //dx=dz=0
1184 //case 3: t1.X() =-dz*ry; t1.Y() = dz*rx; t1.Z() = 0; break; //dx=dy=0
1185 default: ;
1186 }
1187 //dg = (3*rxn)*(rx(i)*SVx(j));
1188 dg = (rx(i)*SVx(j)); //normed
1189
1190 //df = (rxcrxx*t1);
1191 df = (rxcrxx*t1); //normed
1192
1193 //dkappa((j-1)*dim+i) = (df*g-f*dg)/Sqr(g);
1194 dkappa((j-1)*dim+i) = df*gn-fn*dg; //normed
1195 }
1196 }
1197 //oldkappa = dkappa;
1198 }
1199
1200
1201
1202
FastStiffnessMatrix() const1203 int ANCFBeam3D::FastStiffnessMatrix() const
1204 {
1205 return 0; //1==Position derivatives done analytically, damping added with numerical differentiation
1206 }
1207
1208
StiffnessMatrix(Matrix & m)1209 void ANCFBeam3D::StiffnessMatrix(Matrix& m) //fill in sos x sos components, m might be larger
1210 {
1211 //size of m given!!!!!!! DO NOT CHANGE size, m contains derivatives w.r.t. velocity coordinates, etc.
1212 int sos= SOS();
1213 int dim = Dim();
1214 int ns = NS();
1215
1216 TMStartTimer(24);
1217
1218 if (elasticforce_beam == 4 || elasticforce_beam == 0)
1219 {
1220
1221 static Vector u;
1222 u.SetLen(sos);
1223 int useu = 1;
1224 if (useu)
1225 {
1226 for (int i=1; i <= sos; i++)
1227 u(i) = xg(i) - x_init(i);
1228 }
1229 else
1230 {
1231 for (int i=1; i <= sos; i++)
1232 u(i) = xg(i);
1233 }
1234 ApplyT(u); // u = T*u
1235 //UO() << "u=" << u << "\n";
1236
1237 double la=Em * nu / ((1.+nu)*(1.-2.*nu));
1238 double mu=Em / 2. / (1.+nu);
1239 double ks = 10.*(1+nu)/(12+11*nu); //shear correction factor
1240
1241 Matrix3D strain, piola1, F, stress;
1242 Matrix3D jac, jacinv;
1243 double jacdet;
1244
1245 static Matrix Dmat;
1246 static Matrix Dmat2;
1247 Dmat.SetSize(6,6);
1248 Dmat2.SetSize(6,6);
1249 ConstVector<6> strainvec(6);
1250 ConstVector<6> stressvec(6);
1251
1252 for (int i=1; i <= sos; i++)
1253 {
1254 for (int j=1; j <= sos; j++)
1255 {
1256 m(i,j) = 0;
1257 }
1258 }
1259
1260 GetDMatrix(Dmat, nu, Em);
1261
1262 int kkend = 1;
1263 if (elasticforce_beam == 4)
1264 {
1265 kkend = 2;
1266 Dmat(4,4) *= ks;
1267 Dmat(5,5) *= ks;
1268 Dmat(6,6) *= ks;
1269
1270 Dmat2.SetAll(0);
1271 Dmat2(1,1) = Em;
1272 Dmat2(2,2) = Em;
1273 Dmat2(3,3) = Em;
1274 Dmat2(4,4) = Dmat(4,4);
1275 Dmat2(5,5) = Dmat(5,5);
1276 Dmat2(6,6) = Dmat(6,6);
1277
1278 Dmat -= Dmat2;
1279 }
1280
1281 int reduceorder = 2; //reduce order of stiffness matrix
1282 Matrix3D A2;
1283 Matrix3D A1;
1284 Matrix3D dStress;
1285 Matrix3D A3;
1286
1287 for (int kk=1; kk <= kkend; kk++)
1288 {
1289 //kk=1: just poisson effect at element line
1290 //kk=2: no poisson effect, whole body
1291
1292 if (kk == 2 || kkend == 1)
1293 {
1294 GetIntegrationRule(x1,w1,orderx-reduceorder); //x_i ... integration points, w_i ... integration weights
1295 GetIntegrationRule(x2,w2,orderyz-reduceorder);
1296 GetIntegrationRule(x3,w3,orderyz-reduceorder);
1297 }
1298 else
1299 {
1300 GetIntegrationRule(x1,w1,orderx-reduceorder); //x_i ... integration points, w_i ... integration weights
1301 GetIntegrationRule(x2,w2,1);
1302 GetIntegrationRule(x3,w3,1);
1303 }
1304
1305 int kx1 = x2.Length()*x3.Length();
1306 int kx2 = x3.Length();
1307
1308 for (int ip1=1; ip1<=x1.GetLen(); ip1++)
1309 {
1310 for (int ip2=1; ip2<=x2.GetLen(); ip2++)
1311 {
1312 for (int ip3=1; ip3<=x3.GetLen(); ip3++)
1313 {
1314 //TMStartTimer(20);
1315 int i,j,k;
1316 int ind = (ip1-1)*kx1+(ip2-1)*kx2+(ip3-1);
1317 Vector3D p(x1(ip1),x2(ip2),x3(ip3));
1318
1319 // compute F
1320 F.SetAll(0);
1321 int l;
1322
1323 static Matrix agrad; //\bar S^D
1324 GetDSMatrix0(p,DS);
1325 GetJacobi(jac,p,DS,x_init);
1326 jacdet = jac.Det();
1327 jac.GetInverse(jacinv);
1328 jacinv = jacinv.GetTp();
1329
1330 agrad.SetSize(3,NS());
1331 Mult(jacinv, DS, agrad);
1332
1333 for (j = 1; j <= dim; j++)
1334 {
1335 for (i = 1; i <= ns; i++)
1336 {
1337 l = (i-1)*dim+j;
1338 //u = xg(l) - x_init(l);
1339 for (k = 1; k <= dim; k++)
1340 {
1341 F(j,k) += agrad(k,i)*u(l);
1342 //G(j,k) += DS(k,i)*u((i-1)*dim+j);
1343 }
1344 }
1345 if (useu) F(j,j) += 1;
1346 }
1347
1348 // Green-Lagrange strain tensor
1349 //strain = 0.5 * (F.GetTp() * F - I);
1350 F.GetATA2(strain);
1351 strain(1,1) -= 0.5; strain(2,2) -= 0.5; strain(3,3) -= 0.5;
1352
1353 strainvec(1) = strain(1,1);
1354 strainvec(2) = strain(2,2);
1355 strainvec(3) = strain(3,3);
1356 strainvec(4) = 2.*strain(2,3);
1357 strainvec(5) = 2.*strain(3,1);
1358 strainvec(6) = 2.*strain(1,2);
1359
1360 if (kk == 1)
1361 Mult(Dmat,strainvec,stressvec);
1362 else
1363 Mult(Dmat2,strainvec,stressvec);
1364
1365 //piola1 = F * ((2*mu) * strain + Matrix3D(la * strain.Trace()));
1366 stress(1,1) = stressvec(1);
1367 stress(2,2) = stressvec(2);
1368 stress(3,3) = stressvec(3);
1369 stress(2,3) = stressvec(4);
1370 stress(3,1) = stressvec(5);
1371 stress(1,2) = stressvec(6);
1372 stress(3,2) = stressvec(4);
1373 stress(1,3) = stressvec(5);
1374 stress(2,1) = stressvec(6);
1375
1376 //loop for all components of stiffness matrix:
1377 for (int i1=1; i1 <= ns; i1++)
1378 {
1379 for (int j1=1; j1 <= dim; j1++)
1380 {
1381 for (int i2=1; i2 <= ns; i2++)
1382 {
1383 for (int j2=1; j2 <= dim; j2++)
1384 {
1385 double dKij = 0;
1386 //dKij = dS/dqj : F^T dF/dqi + S : dF^T/dqj dF/dqi
1387 // = 0.5*(F^T dF/dqj + dF/dqj^T F) : D : F^T dF/dqi + S : dF^T/dqj dF/dqi
1388 // = 0.5*(A1 + A1^T ) : D : A2 + S : A3
1389
1390 //A2(r,s) = F(r,t)^T dF(t,s)/dq(i) = F(r,t)^T dF(t,s)/dq((i1-1)*3+j1) = F(j1,r)*agrad(s,i1)
1391 //A1(r,s) = F(r,t)^T dF(t,s)/dq(j) = F(r,t)^T dF(t,s)/dq((i2-1)*3+j2) = F(j2,r)*agrad(s,i2)
1392 for (int r=1; r <= dim; r++)
1393 for (int s=1; s <= dim; s++)
1394 {
1395 A2(r,s) = F(j1, r) * agrad(s,i1);
1396 A1(r,s) = F(j2, r) * agrad(s,i2);
1397 }
1398
1399 A1.MakeSym();
1400
1401 //dStress = D:A1
1402 /*
1403 double laA1tr = la * A1.Trace();
1404 A1 *= 2*mu;
1405 A1(1,1) += laA1tr;
1406 A1(2,2) += laA1tr;
1407 A1(3,3) += laA1tr;
1408 dKij += A2.DoubleContract(A1);
1409 */
1410
1411 strainvec(1) = A1(1,1);
1412 strainvec(2) = A1(2,2);
1413 strainvec(3) = A1(3,3);
1414 strainvec(4) = 2.*A1(2,3);
1415 strainvec(5) = 2.*A1(3,1);
1416 strainvec(6) = 2.*A1(1,2);
1417
1418 if (kk == 1)
1419 {
1420 //Mult(Dmat,strainvec,stressvec);
1421
1422 stressvec(1) = Dmat(1,1)*strainvec(1) + Dmat(1,2)*strainvec(2) + Dmat(1,3)*strainvec(3);
1423 stressvec(2) = Dmat(2,1)*strainvec(1) + Dmat(2,2)*strainvec(2) + Dmat(2,3)*strainvec(3);
1424 stressvec(3) = Dmat(3,1)*strainvec(1) + Dmat(3,2)*strainvec(2) + Dmat(3,3)*strainvec(3);
1425 stressvec(4) = Dmat(4,4)*strainvec(4);
1426 stressvec(5) = Dmat(5,5)*strainvec(5);
1427 stressvec(6) = Dmat(6,6)*strainvec(6);
1428
1429 }
1430 else
1431 {
1432 //Mult(Dmat2,strainvec,stressvec);
1433
1434 stressvec(1) = Dmat2(1,1)*strainvec(1);
1435 stressvec(2) = Dmat2(2,2)*strainvec(2);
1436 stressvec(3) = Dmat2(3,3)*strainvec(3);
1437 stressvec(4) = Dmat2(4,4)*strainvec(4);
1438 stressvec(5) = Dmat2(5,5)*strainvec(5);
1439 stressvec(6) = Dmat2(6,6)*strainvec(6);
1440 }
1441
1442 //piola1 = F * ((2*mu) * strain + Matrix3D(la * strain.Trace()));
1443 dStress(1,1) = stressvec(1);
1444 dStress(2,2) = stressvec(2);
1445 dStress(3,3) = stressvec(3);
1446 dStress(2,3) = stressvec(4);
1447 dStress(3,1) = stressvec(5);
1448 dStress(1,2) = stressvec(6);
1449 dStress(3,2) = stressvec(4);
1450 dStress(1,3) = stressvec(5);
1451 dStress(2,1) = stressvec(6);
1452 //dKij += A2.DoubleContract(dStress);
1453 //* YV 17.11.2010: the previous line was commented out as the function Matrix3D::DoubleContract
1454 // used to have a strange meaning, and now it is unclear what should appear there:
1455 // either dStress : dStress or A2 : dStress.
1456 GetMBS()->UO() << "Warning: reached a point in ANCFBeam3D::StiffnessMatrix() in which a decision is yet to be made, the results may be incorrect.";
1457
1458
1459
1460
1461 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1462 //geometric stiffening:
1463 //A3(r,s) = dF(k,r)/dqj dF(k,s)/dqi = {DS(r,i1) * DS(s,i2) if (j1==j2), 0 else}
1464 if (j1 == j2)
1465 {
1466 //compute this once for all j1 and j2 !!!!
1467 /*
1468 for (int r=1; r <= dim; r++)
1469 for (int s=1; s <= dim; s++)
1470 A3(r,s) = agrad(r,i1) * agrad(s,i2);
1471 dKij += stress.DoubleContract(A3);
1472 */
1473 for (int r=1; r <= dim; r++)
1474 for (int s=1; s <= dim; s++)
1475 {
1476 dKij += agrad(r,i1) * agrad(s,i2) * stress(r,s);
1477 }
1478 }
1479
1480 m((i1-1)*3+j1,(i2-1)*3+j2) -= dKij * fabs (jacdet) * w1(ip1)*w2(ip2)*w3(ip3); //negative, because K is put on right-hand-side
1481 }
1482 }
1483 }
1484 }
1485 }
1486 }
1487 }
1488 }
1489 }
1490
1491
1492 TMStopTimer(24);
1493
1494
1495 if (0)
1496 {
1497 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1498 //Compute initial stiffness matrix K0 at first step!!!!!
1499 //u = u_0 + R*(X+u_f^*) - X
1500 //R is rotation from initial position to actual position, R=Ortho([r_x r_y r_z])
1501 //K_T = R*K0*R^T
1502 if (K0.Getrows() != sos)
1503 {
1504 K0.SetSize(sos,sos);
1505 static Vector locjacf20;
1506 locjacf20.SetLen(sos);
1507 locjacf20.SetAll(0);
1508 static Vector locjacf21;
1509 locjacf21.SetLen(sos);
1510
1511 double numdiffepsi = GetMBS()->NumSolver().NumDiffepsi();
1512 double eps;
1513
1514 double t = GetMBS()->GetTime();
1515 EvalF2(locjacf20,t);
1516 double storex;
1517
1518
1519 for (int i = 1; i <= sos; i++)
1520 {
1521 eps = numdiffepsi*Maximum(1e-2,fabs(XG(i)));
1522
1523 storex = XG(i);
1524 XG(i) += 2*eps;
1525 locjacf21.SetAll(0);
1526 EvalF2(locjacf21,t);
1527 XG(i) = storex;
1528
1529 for (int j=1; j<=sos;j++)
1530 {
1531 K0(j,i)= (locjacf21(j)-locjacf20(j))/(2.*eps);
1532 }
1533 }
1534 //UO() << "K0=" << K0 << "\n";
1535 }
1536 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1537
1538 SetComputeCoordinates();
1539 ApplyT(xg); //xg is transformed version of coordinates
1540
1541 //compute R 3x3
1542
1543 //???try if two different rotation matrices at each node are better???
1544 int mode = 2; //1= single rotation matrix, 2=two rotation matrices
1545
1546 Matrix3D R1, R1T, R2, R2T;
1547 if (mode == 1)
1548 {
1549 //average of 2 gradients:
1550 Vector3D x1(0.5*(xg(4)), 0.5*(xg(5)), 0.5*(xg(6)));
1551 Vector3D y1(0.5*(xg(7)), 0.5*(xg(8)), 0.5*(xg(9)));
1552 Vector3D z1(0.5*(xg(10)),0.5*(xg(11)),0.5*(xg(12)));
1553 //Vector3D x1(0.5*(xg(4)+ xg(4+12)), 0.5*(xg(5)+ xg(5+12)), 0.5*(xg(6)+ xg(6+12)));
1554 //Vector3D y1(0.5*(xg(7)+ xg(7+12)), 0.5*(xg(8)+ xg(8+12)), 0.5*(xg(9)+ xg(9+12)));
1555 //Vector3D z1(0.5*(xg(10)+xg(10+12)),0.5*(xg(11)+xg(11+12)),0.5*(xg(12)+xg(12+12)));
1556
1557 x1.Normalize();
1558 Vector3D y1a = y1;
1559 Vector3D z1a = z1;
1560 x1.GramSchmidt(y1a);
1561 x1.GramSchmidt(z1);
1562 y1 = -1*x1.Cross(z1);
1563 y1 = 0.5*(y1+y1a);
1564 x1.GramSchmidt(y1);
1565 z1 = x1.Cross(y1);
1566
1567 //??? Rotationsmatrix stimmt nicht!!!
1568
1569 R1 = Matrix3D(x1.X(),y1.X(),z1.X(),
1570 x1.Y(),y1.Y(),z1.Y(),
1571 x1.Z(),y1.Z(),z1.Z());
1572 R1T = R1.GetTp();
1573 }
1574 else
1575 {
1576 //first node:
1577 Vector3D x1(0.5*(xg(4)), 0.5*(xg(5)), 0.5*(xg(6)));
1578 Vector3D y1(0.5*(xg(7)), 0.5*(xg(8)), 0.5*(xg(9)));
1579 Vector3D z1(0.5*(xg(10)),0.5*(xg(11)),0.5*(xg(12)));
1580
1581 x1.Normalize();
1582 Vector3D y1a = y1;
1583 Vector3D z1a = z1;
1584 x1.GramSchmidt(y1a);
1585 x1.GramSchmidt(z1);
1586 y1 = -1.*x1.Cross(z1);
1587 y1 = 0.5*(y1+y1a);
1588 x1.GramSchmidt(y1);
1589 z1 = x1.Cross(y1);
1590
1591 //??? Rotationsmatrix stimmt nicht!!!
1592
1593 R1 = Matrix3D(x1.X(),y1.X(),z1.X(),
1594 x1.Y(),y1.Y(),z1.Y(),
1595 x1.Z(),y1.Z(),z1.Z());
1596 R1T = R1.GetTp();
1597
1598 //second node:
1599 x1 = Vector3D(0.5*(xg(4+12)), 0.5*(xg(5+12)), 0.5*(xg(6+12)));
1600 y1 = Vector3D(0.5*(xg(7+12)), 0.5*(xg(8+12)), 0.5*(xg(9+12)));
1601 z1 = Vector3D(0.5*(xg(10+12)),0.5*(xg(11+12)),0.5*(xg(12+12)));
1602 x1.Normalize();
1603 y1a = y1;
1604 z1a = z1;
1605 x1.GramSchmidt(y1a);
1606 x1.GramSchmidt(z1);
1607 y1 = -1.*x1.Cross(z1);
1608 y1 = 0.5*(y1+y1a);
1609 x1.GramSchmidt(y1);
1610 z1 = x1.Cross(y1);
1611
1612 //??? Rotationsmatrix stimmt nicht!!!
1613
1614 R2 = Matrix3D(x1.X(),y1.X(),z1.X(),
1615 x1.Y(),y1.Y(),z1.Y(),
1616 x1.Z(),y1.Z(),z1.Z());
1617 R2T = R2.GetTp();
1618 }
1619
1620 //UO() << "R=" << R << "\n";
1621 //UO() << "R1=" << R1 << "\n";
1622 //UO() << "RT=" << RT << "\n";
1623
1624 //write K0 in m
1625 //transform = R*K0(3x3)*R^T
1626 for (int i=0; i<sos/3; i++)
1627 {
1628 for (int j=0; j<sos/3; j++)
1629 {
1630 int io = i*3;
1631 int jo = j*3;
1632
1633 Matrix3D Ksub;
1634 for (int i0=1; i0<=3; i0++)
1635 {
1636 for (int j0=1; j0<=3; j0++)
1637 {
1638 Ksub(i0,j0) = K0(io+i0, jo+j0);
1639 }
1640 }
1641 if (mode == 1)
1642 {
1643 Ksub = (R1*Ksub)*R1T;
1644 }
1645 else
1646 {
1647 //Ksub = (R2*Ksub)*R2T;
1648
1649 if (i < sos/6 && j < sos/6)
1650 {
1651 Ksub = (R1*Ksub)*R1T;
1652 }
1653 else if (i >=sos/6 && j < sos/6)
1654 {
1655 Ksub = (R2*Ksub)*R1T;
1656 }
1657 else if (i < sos/6 && j >=sos/6)
1658 {
1659 Ksub = (R1*Ksub)*R2T;
1660 }
1661 else if (i >=sos/6 && j >=sos/6)
1662 {
1663 Ksub = (R2*Ksub)*R2T;
1664 }
1665 }
1666 for (int i0=1; i0<=3; i0++)
1667 {
1668 for (int j0=1; j0<=3; j0++)
1669 {
1670 m(io+i0, jo+j0) = Ksub(i0,j0);
1671 }
1672 }
1673 }
1674 }
1675 //UO() << "RKR=" << m << "\n";
1676 }
1677 }
1678
1679 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1680 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1681 //compute curvature, stretch, shear and torsion for initial values in order to make precurved beams possible
SetInitialCurvatures()1682 void ANCFBeam3D::SetInitialCurvatures()
1683 {
1684 if (elasticforce_beam != 3) return;
1685
1686 int sos = SOS();
1687 //SetComputeCoordinates(); //only for EvalF2
1688 //--> here: set xg = x_init;
1689 xg = x_init;
1690
1691 int ns = NS();
1692 int dim = 3;
1693
1694 ApplyT(xg);
1695
1696
1697 Vector Sx; Sx.SetLen(NS()); Sx.SetAll(0); //rx
1698 Vector Sy; Sy.SetLen(NS()); Sy.SetAll(0); //ry
1699 Vector Sz; Sz.SetLen(NS()); Sz.SetAll(0); //rz
1700 Vector Sxx; Sxx.SetLen(NS()); Sxx.SetAll(0); //rxx
1701 Vector Syx; Syx.SetLen(NS()); Syx.SetAll(0); //ryx
1702 Vector Szx; Szx.SetLen(NS()); Szx.SetAll(0); //rzx
1703
1704
1705 Vector3D rx, ry, rz, rxx, ryx, rzx;
1706 //integration of elongation and cross-section change:
1707 GetIntegrationRuleLobatto(x1,w1,orderWl);
1708 for (int i1=1; i1<=x1.GetLen(); i1++)
1709 {
1710 double x = x1(i1);
1711 double x2 = x*x;
1712
1713 //rx:
1714 //GetS0x(Sx,Vector3D(x,0,0));
1715
1716 Sx(1) = (-1.5+1.5*x2)/lx;
1717 Sx(2) = (-0.25-0.5*x+0.75*x2);
1718 Sx(5) = -Sx(1);//(1.5-1.5*x2)/lx;
1719 Sx(6) = Sx(2)+x;//(-0.25+0.5*x+0.75*x2);
1720
1721 rx.X() = Sx(1)*xg(3*0+1);
1722 rx.Y() = Sx(1)*xg(3*0+2);
1723 rx.Z() = Sx(1)*xg(3*0+3);
1724 rx.X() += Sx(2)*xg(3*1+1);
1725 rx.Y() += Sx(2)*xg(3*1+2);
1726 rx.Z() += Sx(2)*xg(3*1+3);
1727 rx.X() += Sx(5)*xg(3*4+1);
1728 rx.Y() += Sx(5)*xg(3*4+2);
1729 rx.Z() += Sx(5)*xg(3*4+3);
1730 rx.X() += Sx(6)*xg(3*5+1);
1731 rx.Y() += Sx(6)*xg(3*5+2);
1732 rx.Z() += Sx(6)*xg(3*5+3);
1733
1734 //GetS0y(Sy,Vector3D(x,0,0));
1735 Sy(3) = -0.5*(-1.0+x);
1736 Sy(7) = 0.5*(1.0+x);
1737 ry.X() = Sy(3)*xg(3*2+1);
1738 ry.Y() = Sy(3)*xg(3*2+2);
1739 ry.Z() = Sy(3)*xg(3*2+3);
1740 ry.X() += Sy(7)*xg(3*6+1);
1741 ry.Y() += Sy(7)*xg(3*6+2);
1742 ry.Z() += Sy(7)*xg(3*6+3);
1743
1744 //GetS0z(Sz,Vector3D(x,0,0));
1745 Sz(4) = -0.5*(-1.0+x);
1746 Sz(8) = 0.5*(1.0+x);
1747 rz.X() = Sz(4)*xg(3*3+1);
1748 rz.Y() = Sz(4)*xg(3*3+2);
1749 rz.Z() = Sz(4)*xg(3*3+3);
1750 rz.X() += Sz(8)*xg(3*7+1);
1751 rz.Y() += Sz(8)*xg(3*7+2);
1752 rz.Z() += Sz(8)*xg(3*7+3);
1753
1754
1755 //integration factors:
1756 double fact = lx * 0.5 * w1(i1);
1757
1758 Wlepsx0(i1) = 0.5*(rx*rx-1.);
1759 Wlepsy0(i1) = 0.5*(ry*ry-1.)*factstiffWl;
1760 Wlepsz0(i1) = 0.5*(rz*rz-1.)*factstiffWl;
1761 Wlgamyz0(i1)= ry*rz*factstiffWl ;
1762
1763 }
1764
1765
1766 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1767 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1768 //Hellinger Reissner principle:
1769 //note that xi=0..1 in the paper of Schwab and x = -1..+1
1770
1771 //1. compute Wxz1, Wxz2, Wxy1, Wxy2
1772 //2. compute delta Wx... times Wx... and sum up in temp
1773 //UO() << "Hellinger Reisner\n";
1774
1775 double Wxz1f = 0;
1776 double Wxz2f = 0;
1777 double Wxy1f = 0;
1778 double Wxy2f = 0;
1779 //shear deformation:
1780 GetIntegrationRuleLobatto(x1,w1,orderWsHR);
1781 for (int i1=1; i1<=x1.GetLen(); i1++)
1782 {
1783 double x = x1(i1);
1784
1785 double x2 = x*x;
1786 //rx:
1787 //GetS0x(Sx,Vector3D(x,0,0));
1788 Sx(1) = (-1.5+1.5*x2)/lx;
1789 Sx(2) = (-0.25-0.5*x+0.75*x2);
1790 Sx(5) = -Sx(1);//(1.5-1.5*x2)/lx;
1791 Sx(6) = Sx(2)+x;//(-0.25+0.5*x+0.75*x2);
1792 rx.X() = Sx(1)*xg(3*0+1);
1793 rx.Y() = Sx(1)*xg(3*0+2);
1794 rx.Z() = Sx(1)*xg(3*0+3);
1795 rx.X() += Sx(2)*xg(3*1+1);
1796 rx.Y() += Sx(2)*xg(3*1+2);
1797 rx.Z() += Sx(2)*xg(3*1+3);
1798 rx.X() += Sx(5)*xg(3*4+1);
1799 rx.Y() += Sx(5)*xg(3*4+2);
1800 rx.Z() += Sx(5)*xg(3*4+3);
1801 rx.X() += Sx(6)*xg(3*5+1);
1802 rx.Y() += Sx(6)*xg(3*5+2);
1803 rx.Z() += Sx(6)*xg(3*5+3);
1804
1805 //GetS0y(Sy,Vector3D(x,0,0));
1806 Sy(3) = -0.5*(-1.0+x);
1807 Sy(7) = 0.5*(1.0+x);
1808 ry.X() = Sy(3)*xg(3*2+1);
1809 ry.Y() = Sy(3)*xg(3*2+2);
1810 ry.Z() = Sy(3)*xg(3*2+3);
1811 ry.X() += Sy(7)*xg(3*6+1);
1812 ry.Y() += Sy(7)*xg(3*6+2);
1813 ry.Z() += Sy(7)*xg(3*6+3);
1814
1815 //GetS0z(Sz,Vector3D(x,0,0));
1816 Sz(4) = -0.5*(-1.0+x);
1817 Sz(8) = 0.5*(1.0+x);
1818 rz.X() = Sz(4)*xg(3*3+1);
1819 rz.Y() = Sz(4)*xg(3*3+2);
1820 rz.Z() = Sz(4)*xg(3*3+3);
1821 rz.X() += Sz(8)*xg(3*7+1);
1822 rz.Y() += Sz(8)*xg(3*7+2);
1823 rz.Z() += Sz(8)*xg(3*7+3);
1824
1825
1826 //integration factors:
1827 double fact = lx * 0.5 * w1(i1);
1828
1829 double gamxy= fact*rx*ry;
1830 double gamxz= fact*rx*rz;
1831 //UO() << "rx=" << rx << "\n";
1832 //UO() << "ry=" << ry << "\n";
1833 //UO() << "gamxy=" << gamxy << "\n";
1834 //UO() << "gamxz=" << gamxz << "\n";
1835
1836 Wxy1f += 0.5*(1-x) * gamxy;
1837 Wxy2f += 0.5*(1+x) * gamxy;
1838 Wxz1f += 0.5*(1-x) * gamxz;
1839 Wxz2f += 0.5*(1+x) * gamxz;
1840
1841 }
1842
1843 GetIntegrationRuleLobatto(x1,w1,orderWsHR);
1844 //now compute variations:
1845 for (int i1=1; i1<=x1.GetLen(); i1++)
1846 {
1847 double x = x1(i1);
1848
1849 double x2 = x*x;
1850 //rx:
1851 //GetS0x(Sx,Vector3D(x,0,0));
1852 Sx(1) = (-1.5+1.5*x2)/lx;
1853 Sx(2) = (-0.25-0.5*x+0.75*x2);
1854 Sx(5) = -Sx(1);//(1.5-1.5*x2)/lx;
1855 Sx(6) = Sx(2)+x;//(-0.25+0.5*x+0.75*x2);
1856 rx.X() = Sx(1)*xg(3*0+1);
1857 rx.Y() = Sx(1)*xg(3*0+2);
1858 rx.Z() = Sx(1)*xg(3*0+3);
1859 rx.X() += Sx(2)*xg(3*1+1);
1860 rx.Y() += Sx(2)*xg(3*1+2);
1861 rx.Z() += Sx(2)*xg(3*1+3);
1862 rx.X() += Sx(5)*xg(3*4+1);
1863 rx.Y() += Sx(5)*xg(3*4+2);
1864 rx.Z() += Sx(5)*xg(3*4+3);
1865 rx.X() += Sx(6)*xg(3*5+1);
1866 rx.Y() += Sx(6)*xg(3*5+2);
1867 rx.Z() += Sx(6)*xg(3*5+3);
1868
1869 //GetS0y(Sy,Vector3D(x,0,0));
1870 Sy(3) = -0.5*(-1.0+x);
1871 Sy(7) = 0.5*(1.0+x);
1872 ry.X() = Sy(3)*xg(3*2+1);
1873 ry.Y() = Sy(3)*xg(3*2+2);
1874 ry.Z() = Sy(3)*xg(3*2+3);
1875 ry.X() += Sy(7)*xg(3*6+1);
1876 ry.Y() += Sy(7)*xg(3*6+2);
1877 ry.Z() += Sy(7)*xg(3*6+3);
1878
1879 //GetS0z(Sz,Vector3D(x,0,0));
1880 Sz(4) = -0.5*(-1.0+x);
1881 Sz(8) = 0.5*(1.0+x);
1882 rz.X() = Sz(4)*xg(3*3+1);
1883 rz.Y() = Sz(4)*xg(3*3+2);
1884 rz.Z() = Sz(4)*xg(3*3+3);
1885 rz.X() += Sz(8)*xg(3*7+1);
1886 rz.Y() += Sz(8)*xg(3*7+2);
1887 rz.Z() += Sz(8)*xg(3*7+3);
1888
1889
1890 //integration factors:
1891 double fact = 0.5 * w1(i1);
1892
1893 //factor for delta gamxy components:
1894 WsHRk10(i1) = beamGAky * fact*(0.5*(1-x)*(4.*Wxy1f - 2.*Wxy2f) + 0.5*(1+x)*(-2.*Wxy1f + 4.*Wxy2f));
1895 //factor for delta gamxz components:
1896 WsHRk20(i1) = beamGAkz * fact*(0.5*(1-x)*(4.*Wxz1f - 2.*Wxz2f) + 0.5*(1+x)*(-2.*Wxz1f + 4.*Wxz2f));
1897
1898 }
1899
1900
1901 //torsion:
1902 GetIntegrationRule(x1,w1,orderWt);
1903 temp.SetAll(0);
1904 for (int i1=1; i1<=x1.GetLen(); i1++)
1905 {
1906 double x = x1(i1);
1907 //double x2 = x*x;
1908 //GetS0y(Sy,Vector3D(x,0,0));
1909 Sy(3) = -0.5*(-1.0+x);
1910 Sy(7) = 0.5*(1.0+x);
1911 ry.X() = Sy(3)*xg(3*2+1);
1912 ry.Y() = Sy(3)*xg(3*2+2);
1913 ry.Z() = Sy(3)*xg(3*2+3);
1914 ry.X() += Sy(7)*xg(3*6+1);
1915 ry.Y() += Sy(7)*xg(3*6+2);
1916 ry.Z() += Sy(7)*xg(3*6+3);
1917
1918 //GetS0z(Sz,Vector3D(x,0,0));
1919 Sz(4) = -0.5*(-1.0+x);
1920 Sz(8) = 0.5*(1.0+x);
1921 rz.X() = Sz(4)*xg(3*3+1);
1922 rz.Y() = Sz(4)*xg(3*3+2);
1923 rz.Z() = Sz(4)*xg(3*3+3);
1924 rz.X() += Sz(8)*xg(3*7+1);
1925 rz.Y() += Sz(8)*xg(3*7+2);
1926 rz.Z() += Sz(8)*xg(3*7+3);
1927
1928 //ryx:
1929 //GetS0yx(Syx,Vector3D(x,0,0));
1930 Syx(3) = -1./lx;
1931 Syx(7) = 1./lx;
1932 ryx.X() = Syx(3)*xg(3*2+1);
1933 ryx.Y() = Syx(3)*xg(3*2+2);
1934 ryx.Z() = Syx(3)*xg(3*2+3);
1935 ryx.X() += Syx(7)*xg(3*6+1);
1936 ryx.Y() += Syx(7)*xg(3*6+2);
1937 ryx.Z() += Syx(7)*xg(3*6+3);
1938
1939 //rzx:
1940 //GetS0zx(Szx,Vector3D(x,0,0));
1941 Szx(4) = -1./lx;
1942 Szx(8) = 1./lx;
1943 rzx.X() = Szx(4)*xg(3*3+1);
1944 rzx.Y() = Szx(4)*xg(3*3+2);
1945 rzx.Z() = Szx(4)*xg(3*3+3);
1946 rzx.X() += Szx(8)*xg(3*7+1);
1947 rzx.Y() += Szx(8)*xg(3*7+2);
1948 rzx.Z() += Szx(8)*xg(3*7+3);
1949
1950
1951 //integration factors:
1952 double fact = beamGJkx * lx * 0.5 * w1(i1);
1953
1954 Wtkapx0(i1) = 0.5 * (rz*ryx - ry*rzx);
1955 }
1956
1957 //bending:
1958 GetIntegrationRule(x1,w1,orderWb);
1959 for (int i1=1; i1<=x1.GetLen(); i1++)
1960 {
1961 double x = x1(i1);
1962
1963 double x2 = x*x;
1964 //GetS0y(Sy,Vector3D(x,0,0));
1965 Sy(3) = -0.5*(-1.0+x);
1966 Sy(7) = 0.5*(1.0+x);
1967 ry.X() = Sy(3)*xg(3*2+1);
1968 ry.Y() = Sy(3)*xg(3*2+2);
1969 ry.Z() = Sy(3)*xg(3*2+3);
1970 ry.X() += Sy(7)*xg(3*6+1);
1971 ry.Y() += Sy(7)*xg(3*6+2);
1972 ry.Z() += Sy(7)*xg(3*6+3);
1973
1974 //GetS0z(Sz,Vector3D(x,0,0));
1975 Sz(4) = -0.5*(-1.0+x);
1976 Sz(8) = 0.5*(1.0+x);
1977 rz.X() = Sz(4)*xg(3*3+1);
1978 rz.Y() = Sz(4)*xg(3*3+2);
1979 rz.Z() = Sz(4)*xg(3*3+3);
1980 rz.X() += Sz(8)*xg(3*7+1);
1981 rz.Y() += Sz(8)*xg(3*7+2);
1982 rz.Z() += Sz(8)*xg(3*7+3);
1983
1984 //rxx:
1985 //GetS0xx(Sxx,Vector3D(x,0,0));
1986 Sxx(1) = 4./Sqr(lx)*3.0/2.0*x;
1987 Sxx(2) = 1./lx*(-1.0+3.0*x);
1988 Sxx(5) = -Sxx(1);
1989 Sxx(6) = 1./lx*(1.0+3.0*x);
1990 rxx.X() = Sxx(1)*xg(3*(1-1)+1);
1991 rxx.Y() = Sxx(1)*xg(3*(1-1)+2);
1992 rxx.Z() = Sxx(1)*xg(3*(1-1)+3);
1993 rxx.X() += Sxx(2)*xg(3*(2-1)+1);
1994 rxx.Y() += Sxx(2)*xg(3*(2-1)+2);
1995 rxx.Z() += Sxx(2)*xg(3*(2-1)+3);
1996 rxx.X() += Sxx(5)*xg(3*(5-1)+1);
1997 rxx.Y() += Sxx(5)*xg(3*(5-1)+2);
1998 rxx.Z() += Sxx(5)*xg(3*(5-1)+3);
1999 rxx.X() += Sxx(6)*xg(3*(6-1)+1);
2000 rxx.Y() += Sxx(6)*xg(3*(6-1)+2);
2001 rxx.Z() += Sxx(6)*xg(3*(6-1)+3);
2002
2003
2004 //integration factors:
2005 double facty = beamEIy * lx * 0.5 * w1(i1);
2006 double factz = beamEIz * lx * 0.5 * w1(i1);
2007
2008 Wbkapy0(i1) = rz*rxx;
2009 Wbkapz0(i1) =-(ry*rxx);
2010 }
2011
2012
2013 }
2014
2015 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2016 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2017
EvalF2(Vector & f,double t)2018 void ANCFBeam3D::EvalF2(Vector& f, double t)
2019 {
2020 Body3D::EvalF2(f,t);
2021
2022 int sos = SOS();
2023 SetComputeCoordinates();
2024 static Vector fadd;
2025
2026 int ns = NS();
2027 int dim = 3;
2028 //double u;
2029
2030 if (!((elasticforce_beam == 4 || elasticforce_beam == 0) && GetMBS()->IsJacobianComputation() && FastStiffnessMatrix() != 0 && GetMBS()->NumSolver().UseSparseSolver()))
2031 {
2032 TMStartTimer(22);
2033
2034 //0 is Original ANCF
2035 //3 Arend Schwab && Jaap Meijaard
2036 //4 Poisson locking eliminated in original ANCF
2037 //5 Corotational
2038
2039 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2040 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2041 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2042 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2043 if (elasticforce_beam == 5)
2044 {
2045 int includedeltaterms = 1;
2046 //elastic line linearized
2047 mystatic Vector temp2;
2048 temp2.SetLen(SOS());
2049
2050 temp.SetLen(SOS());
2051 fadd.SetLen(SOS());
2052 fadd.SetAll(0);
2053 //optimized version:
2054
2055 ApplyT(xg);
2056
2057 Matrix3D A, AT;
2058 Vector3D pref;
2059 ComputeCorotationalFrame(pref, A);
2060 AT = A.GetTp();
2061
2062
2063 mystatic Vector Sx; Sx.SetLen(NS()); Sx.SetAll(0); //rx
2064 mystatic Vector Sy; Sy.SetLen(NS()); Sy.SetAll(0); //ry
2065 mystatic Vector Sz; Sz.SetLen(NS()); Sz.SetAll(0); //rz
2066 mystatic Vector Sxx; Sxx.SetLen(NS()); Sxx.SetAll(0); //rxx
2067 mystatic Vector Syx; Syx.SetLen(NS()); Syx.SetAll(0); //ryx
2068 mystatic Vector Szx; Szx.SetLen(NS()); Szx.SetAll(0); //rzx
2069
2070 Vector3D rx, ry, rz, rxx, ryx, rzx;
2071 double /*epsx,*/ epsy, epsz, gamyz, gamxy, gamxz;
2072 double depsy, depsz;
2073
2074 Vector3D tx[2]; //rx vectors at both ends
2075 Vector3D ty[2]; //ry vectors at both ends
2076 Vector3D tz[2]; //rz vectors at both ends
2077
2078 Vector3D txlin[2]; //rx linearized
2079 Vector3D tylin[2]; //rx linearized
2080 Vector3D tzlin[2]; //rx linearized
2081 txlin[0] = 0.;
2082 txlin[1] = 0.;
2083 tylin[0] = 0.;
2084 tylin[1] = 0.;
2085 tzlin[0] = 0.;
2086 tzlin[1] = 0.;
2087
2088 ConstMatrix<72> temp3(3,SOS());
2089 ConstVector<8> txlin_delta[2];
2090 txlin_delta[0].SetLen(8);
2091 txlin_delta[1].SetLen(8);
2092 txlin_delta[0].SetAll(0.);
2093 txlin_delta[1].SetAll(0.);
2094
2095
2096 Vector3D rxx_int(0.);
2097 ConstVector<8> rxx_int_delta(8);
2098 rxx_int_delta.SetAll(0.);
2099
2100 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2101 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2102 //apply penalty terms for thickness deformation and cross section distorsion at both sides
2103 //shear deformation included with reduced integration (Lobatto)
2104 double ix;
2105 int ind = 0;
2106 temp.SetAll(0.);
2107 for (ix = -1; ix <= 1; ix += 2)
2108 {
2109 double x = ix;
2110 double x2 = x*x;
2111
2112 //GetS0y(Sy,Vector3D(x,0,0));
2113 Sy(3) = -0.5*(-1.0+x);
2114 Sy(7) = 0.5*(1.0+x);
2115 ry.X() = Sy(3)*xg(3*2+1);
2116 ry.Y() = Sy(3)*xg(3*2+2);
2117 ry.Z() = Sy(3)*xg(3*2+3);
2118 ry.X() += Sy(7)*xg(3*6+1);
2119 ry.Y() += Sy(7)*xg(3*6+2);
2120 ry.Z() += Sy(7)*xg(3*6+3);
2121
2122 //GetS0z(Sz,Vector3D(x,0,0));
2123 Sz(4) = -0.5*(-1.0+x);
2124 Sz(8) = 0.5*(1.0+x);
2125 rz.X() = Sz(4)*xg(3*3+1);
2126 rz.Y() = Sz(4)*xg(3*3+2);
2127 rz.Z() = Sz(4)*xg(3*3+3);
2128 rz.X() += Sz(8)*xg(3*7+1);
2129 rz.Y() += Sz(8)*xg(3*7+2);
2130 rz.Z() += Sz(8)*xg(3*7+3);
2131
2132 Sx(1) = (-1.5+1.5*x2)/lx;
2133 Sx(2) = (-0.25-0.5*x+0.75*x2);
2134 Sx(5) = -Sx(1);//(1.5-1.5*x2)/lx;
2135 Sx(6) = Sx(2)+x;//(-0.25+0.5*x+0.75*x2);
2136
2137 rx.X() = Sx(1)*xg(3*0+1);
2138 rx.Y() = Sx(1)*xg(3*0+2);
2139 rx.Z() = Sx(1)*xg(3*0+3);
2140 rx.X() += Sx(2)*xg(3*1+1);
2141 rx.Y() += Sx(2)*xg(3*1+2);
2142 rx.Z() += Sx(2)*xg(3*1+3);
2143 rx.X() += Sx(5)*xg(3*4+1);
2144 rx.Y() += Sx(5)*xg(3*4+2);
2145 rx.Z() += Sx(5)*xg(3*4+3);
2146 rx.X() += Sx(6)*xg(3*5+1);
2147 rx.Y() += Sx(6)*xg(3*5+2);
2148 rx.Z() += Sx(6)*xg(3*5+3);
2149
2150
2151 tx[ind ] = rx;
2152 ty[ind ] = ry;
2153 tz[ind++] = rz;
2154
2155 epsy = 0.5*(ry*ry-1.);
2156 epsz = 0.5*(rz*rz-1.);
2157 gamyz= ry*rz;
2158 gamxy= rx*ry;
2159 gamxz= rx*rz;
2160 double GAcs = beamEA; //use beam cross-section to define this factor!
2161 double EAcs = GAcs; //stiffness factor for cross-section deformation and axial elongation
2162
2163 double fact = 0.5*lx;
2164 //UO() << "EAcs=" << EAcs << "\n";
2165
2166 for (int i=1; i <= dim; i++)
2167 {
2168 //j=3,7: Sx(j)=Sz(j)=depsx=depsz=0
2169 depsy = Sy(3)*ry(i);
2170 temp((3-1)*dim+i) = epsy*depsy*EAcs + gamyz * GAcs * Sy(3)*rz(i);
2171 depsy = Sy(7)*ry(i);
2172 temp((7-1)*dim+i) = epsy*depsy*EAcs + gamyz * GAcs * Sy(7)*rz(i);
2173
2174 //j=4,8: Sx(j)=Sy(j)=depsx=depsy=0
2175 depsz = Sz(4)*rz(i);
2176 temp((4-1)*dim+i) = epsz*depsz*EAcs + gamyz * GAcs * (Sz(4)*ry(i));
2177 depsz = Sz(8)*rz(i);
2178 temp((8-1)*dim+i) = epsz*depsz*EAcs + gamyz * GAcs * (Sz(8)*ry(i));
2179
2180 }
2181 fadd += fact*temp;
2182 }
2183
2184 Vector3D rxs[2];
2185 //Vector3D rxslin[2];
2186 Vector3D rys[2];
2187 Vector3D rzs[2];
2188 double gammay[2]; //shear angle, rotation around y
2189 double gammaz[2]; //shear angle, rotation around z
2190
2191 const int uselin = 0;
2192 const int altmode = 0; //use average gamma + relative gamma
2193
2194 for (int i=0; i<2; i++)
2195 {
2196 rxs[i] = AT * tx[i];
2197
2198 rys[i] = AT * ty[i];
2199 rzs[i] = AT * tz[i];
2200
2201 gammaz[i] =( rxs[i].Y() + rys[i].X()); //in fact would be -1*
2202 gammay[i] =( rxs[i].Z() + rzs[i].X());
2203 }
2204
2205 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2206 //integration of elongation and bending:
2207
2208 GetIntegrationRule(x1,w1,orderWl);
2209 for (int i1=1; i1<=x1.GetLen(); i1++)
2210 {
2211 double x = x1(i1);
2212 double x2 = x*x;
2213
2214 //rx:
2215 //GetS0x(Sx,Vector3D(x,0,0));
2216
2217 Sx(1) = (-1.5+1.5*x2)/lx;
2218 Sx(2) = (-0.25-0.5*x+0.75*x2);
2219 Sx(5) = -Sx(1);//(1.5-1.5*x2)/lx;
2220 Sx(6) = Sx(2)+x;//(-0.25+0.5*x+0.75*x2);
2221
2222 rx.X() = Sx(1)*xg(3*0+1);
2223 rx.Y() = Sx(1)*xg(3*0+2);
2224 rx.Z() = Sx(1)*xg(3*0+3);
2225 rx.X() += Sx(2)*xg(3*1+1);
2226 rx.Y() += Sx(2)*xg(3*1+2);
2227 rx.Z() += Sx(2)*xg(3*1+3);
2228 rx.X() += Sx(5)*xg(3*4+1);
2229 rx.Y() += Sx(5)*xg(3*4+2);
2230 rx.Z() += Sx(5)*xg(3*4+3);
2231 rx.X() += Sx(6)*xg(3*5+1);
2232 rx.Y() += Sx(6)*xg(3*5+2);
2233 rx.Z() += Sx(6)*xg(3*5+3);
2234
2235 //rxx:
2236 //GetS0xx(Sxx,Vector3D(x,0,0));
2237 Sxx(1) = 4./Sqr(lx)*3.0/2.0*x;
2238 Sxx(2) = 1./lx*(-1.0+3.0*x);
2239 Sxx(5) = -Sxx(1);
2240 Sxx(6) = 1./lx*(1.0+3.0*x);
2241 rxx.X() = Sxx(1)*xg(3*(1-1)+1);
2242 rxx.Y() = Sxx(1)*xg(3*(1-1)+2);
2243 rxx.Z() = Sxx(1)*xg(3*(1-1)+3);
2244 rxx.X() += Sxx(2)*xg(3*(2-1)+1);
2245 rxx.Y() += Sxx(2)*xg(3*(2-1)+2);
2246 rxx.Z() += Sxx(2)*xg(3*(2-1)+3);
2247 rxx.X() += Sxx(5)*xg(3*(5-1)+1);
2248 rxx.Y() += Sxx(5)*xg(3*(5-1)+2);
2249 rxx.Z() += Sxx(5)*xg(3*(5-1)+3);
2250 rxx.X() += Sxx(6)*xg(3*(6-1)+1);
2251 rxx.Y() += Sxx(6)*xg(3*(6-1)+2);
2252 rxx.Z() += Sxx(6)*xg(3*(6-1)+3);
2253
2254 //for shear deformation:
2255 txlin[0] += rx*(1.-x)*0.5 * w1(i1);
2256 txlin[1] += rx*(1.+x)*0.5 * w1(i1);
2257
2258 for (int i = 1; i <= NS(); i++)
2259 {
2260 txlin_delta[0](i) += Sx(i)*(1.-x)*0.5 * w1(i1);
2261 txlin_delta[1](i) += Sx(i)*(1.+x)*0.5 * w1(i1);
2262 }
2263
2264 rxx_int += rxx * 0.5 * w1(i1) * lx;
2265
2266 for (int i = 1; i <= NS(); i++)
2267 {
2268 rxx_int_delta(i) += Sxx(i)* 0.5 * w1(i1) * lx;
2269 }
2270
2271
2272 //GetS0y(Sy,Vector3D(x,0,0));
2273 Sy(3) = -0.5*(-1.0+x);
2274 Sy(7) = 0.5*(1.0+x);
2275 ry.X() = Sy(3)*xg(3*2+1);
2276 ry.Y() = Sy(3)*xg(3*2+2);
2277 ry.Z() = Sy(3)*xg(3*2+3);
2278 ry.X() += Sy(7)*xg(3*6+1);
2279 ry.Y() += Sy(7)*xg(3*6+2);
2280 ry.Z() += Sy(7)*xg(3*6+3);
2281
2282 //GetS0z(Sz,Vector3D(x,0,0));
2283 Sz(4) = -0.5*(-1.0+x);
2284 Sz(8) = 0.5*(1.0+x);
2285 rz.X() = Sz(4)*xg(3*3+1);
2286 rz.Y() = Sz(4)*xg(3*3+2);
2287 rz.Z() = Sz(4)*xg(3*3+3);
2288 rz.X() += Sz(8)*xg(3*7+1);
2289 rz.Y() += Sz(8)*xg(3*7+2);
2290 rz.Z() += Sz(8)*xg(3*7+3);
2291
2292 tylin[0] += ry*(1.-x)*0.5 * w1(i1);
2293 tylin[1] += ry*(1.+x)*0.5 * w1(i1);
2294 tzlin[0] += rz*(1.-x)*0.5 * w1(i1);
2295 tzlin[1] += rz*(1.+x)*0.5 * w1(i1);
2296
2297
2298
2299
2300
2301 Vector3D rxxs = AT*rxx;
2302 double wxxs = rxxs(3)-(gammay[1]-gammay[0])/lx;
2303 double vxxs = rxxs(2)+(gammaz[1]-gammaz[0])/lx;
2304
2305 Vector3D rxs = AT*rx;
2306 double uxs = rxs(1)-1.;
2307
2308 //integration factors:
2309 double facty = beamEIy * lx * 0.5 * w1(i1);
2310 double factz = beamEIz * lx * 0.5 * w1(i1);
2311
2312 //integration factors:
2313 double factx = lx * 0.5 * w1(i1) * beamEA;
2314 //epsx = 0.5*(rx*rx-1.);
2315
2316 temp.SetAll(0.);
2317 for (int i=1; i <= dim; i++)
2318 {
2319 //j=1,2,5,6:
2320 //temp((1-1)*dim+i) += factx*Sx(1)*rx(i)*(epsx);
2321 //temp((2-1)*dim+i) += factx*Sx(2)*rx(i)*(epsx);
2322 //temp((5-1)*dim+i) += factx*Sx(5)*rx(i)*(epsx);
2323 //temp((6-1)*dim+i) += factx*Sx(6)*rx(i)*(epsx);
2324 temp((1-1)*dim+i) += factx*uxs*Sx(1)*AT(1,i);
2325 temp((2-1)*dim+i) += factx*uxs*Sx(2)*AT(1,i);
2326 temp((5-1)*dim+i) += factx*uxs*Sx(5)*AT(1,i);
2327 temp((6-1)*dim+i) += factx*uxs*Sx(6)*AT(1,i);
2328
2329
2330 //j=1,2,5,6:
2331 temp((1-1)*dim+i) += facty*wxxs*Sxx(1)*AT(3,i);
2332 temp((2-1)*dim+i) += facty*wxxs*Sxx(2)*AT(3,i);
2333 temp((5-1)*dim+i) += facty*wxxs*Sxx(5)*AT(3,i);
2334 temp((6-1)*dim+i) += facty*wxxs*Sxx(6)*AT(3,i);
2335
2336 temp((1-1)*dim+i) += factz*vxxs*Sxx(1)*AT(2,i);
2337 temp((2-1)*dim+i) += factz*vxxs*Sxx(2)*AT(2,i);
2338 temp((5-1)*dim+i) += factz*vxxs*Sxx(5)*AT(2,i);
2339 temp((6-1)*dim+i) += factz*vxxs*Sxx(6)*AT(2,i);
2340
2341 for (int k=0; k <=1; k++)
2342 {
2343 double sign = 1;
2344 if (k==1) sign = -1;
2345 //gammaz[i] =( rxs[i].Y() + rys[i].X()); //in fact would be -1*
2346 //gammay[i] =( rxs[i].Z() + rzs[i].X());
2347 //double wxxs = rxxs(3)-(gammay[1]-gammay[0])/lx;
2348 //double vxxs = rxxs(2)+(gammaz[1]-gammaz[0])/lx;
2349
2350 //delta rx:
2351 temp(3+i+12*k) += -1.*factz*vxxs*AT(2,i)/lx*sign; //y-component of AT
2352 temp(3+i+12*k) += facty*wxxs*AT(3,i)/lx*sign; //z-component of AT
2353
2354 //delta ry:
2355 temp(6+i+12*k)+= -1.*factz*vxxs*AT(1,i)/lx*sign; //x-component of AT
2356
2357 //delta rz:
2358 temp(9+i+12*k)+= facty*wxxs*AT(1,i)/lx*sign; //x-component of AT
2359 }
2360
2361 }
2362
2363 //include terms with delta AT
2364 if (includedeltaterms)
2365 {
2366 ComputeCorotationalFrameDATvdq(rx,temp3);
2367 for (int i=1; i <= SOS(); i++)
2368 {
2369 temp(i) += temp3(1,i)*uxs*factx;
2370 }
2371
2372 ComputeCorotationalFrameDATvdq(rxx,temp3);
2373 for (int i=1; i <= SOS(); i++)
2374 {
2375 temp(i) += temp3(2,i)*vxxs*factz;
2376 temp(i) += temp3(3,i)*wxxs*facty;
2377 }
2378 }
2379
2380 fadd += temp;
2381 }
2382
2383 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2384 //Torsion:
2385 //linearized angle of tz[0]=rz(1) and tz[1]=rz(2)
2386 //theta = [AT*(tz[0]-tz[1])].Y
2387 Vector3D rz1rz2 = tz[0] - tz[1];
2388 Vector3D ATrz1rz2 = AT * rz1rz2;
2389
2390 double theta = ATrz1rz2.Y();
2391 //delta AT * rz1rz2:
2392 if (includedeltaterms)
2393 {
2394 ComputeCorotationalFrameDATvdq(rz1rz2, temp3);
2395 for (int i=1; i <= SOS(); i++)
2396 {
2397 temp(i) = temp3(2,i); //take y-component of difference of rz2 and rz1
2398 }
2399 }
2400 else
2401 {
2402 temp.SetAll(0.);
2403 }
2404 //AT * delta rz1rz2 = AT * (delta[q10,q11,q12] - delta[q22,q23,q24]):
2405
2406 for (int i = 1; i <= Dim(); i++)
2407 {
2408 temp(9+i) += AT(2,i); //y-component of AT
2409 temp(12+9+i) +=-AT(2,i); //y-component of AT
2410 }
2411
2412 double fact_theta = beamGJkx / lx * theta;
2413 temp *= fact_theta;
2414 fadd += temp;
2415
2416
2417 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2418 //shear deformation:
2419
2420 for (int i=0; i<2; i++)
2421 {
2422 /*if (GetMBS()->GetTime() > 0.9)
2423 {
2424 UO() << "x=" << x_init(1) << ", ";
2425 UO() << "rzlin=" << tzlin[i] << ",rz=" << tz[i]
2426 << "rx=" << tx[i] << ",rx=" << tx[i]<< ", gammay=" << gammay[i] << "\n";
2427 //UO() << "beamGAky=" << beamGAky << ", beamGAkz=" << beamGAkz << "\n";
2428 }*/
2429
2430 //if (GetMBS()->GetTime() > 0.02 && i == 0)
2431 // UO() << "gammay=" << gammay[i] << ", rxz=" << rxs[i].Z() << ", rzx=" << rzs[i].X() << "\n";
2432 //UO() << "beamGAky=" << beamGAky << ", beamGAkz=" << beamGAkz << "\n";
2433 //UO() << "beamEIy=" << beamEIy << ", beamEIz=" << beamEIz << "\n";
2434
2435 double facty = 0.5 * lx * beamGAky * gammay[i];
2436 double factz = 0.5 * lx * beamGAkz * gammaz[i];
2437 if (altmode)
2438 {
2439 facty = 0.5*lx * beamGAky * (gammay[1] + gammay[2]);
2440 factz = 0.5*lx * beamGAkz * (gammaz[1] + gammaz[2]);
2441 }
2442
2443 temp.SetAll(0.); //not necessary!
2444
2445
2446 //compute terms with delta AT
2447 if (includedeltaterms && 0)
2448 {
2449 ComputeCorotationalFrameDATvdq(tx[i], temp3);
2450 for (int j=1; j <= SOS(); j++)
2451 {
2452 temp(j)+= factz*temp3(2,j);
2453 temp(j)+= facty*temp3(3,j);
2454 }
2455
2456 ComputeCorotationalFrameDATvdq(ty[i], temp3);
2457 for (int j=1; j <= SOS(); j++)
2458 {
2459 temp(j)+= factz*temp3(1,j);
2460 }
2461
2462 ComputeCorotationalFrameDATvdq(tz[i], temp3);
2463 for (int j=1; j <= SOS(); j++)
2464 {
2465 temp(j)+= facty*temp3(1,j);
2466 }
2467 }
2468
2469 //compute terms with delta rx, delta ry, delta rz
2470 for (int j = 1; j <= Dim(); j++)
2471 {
2472 //delta rx:
2473 temp(3+j+12*i) += factz*AT(2,j); //y-component of AT
2474 temp(3+j+12*i) += facty*AT(3,j); //z-component of AT
2475
2476 //delta ry:
2477 temp(6+j+12*i)+= factz*AT(1,j); //x-component of AT
2478
2479 //delta rz:
2480 temp(9+j+12*i)+= facty*AT(1,j); //x-component of AT
2481 }
2482 fadd += temp;
2483 }
2484 }
2485
2486 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2487 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2488 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2489 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2490 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2491 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2492 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2493 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2494 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2495 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2496 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2497 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2498 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2499 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2500 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2501 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2502 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2503 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2504 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2505 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2506 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2507 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2508 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2509 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2510
2511 if (elasticforce_beam == 3)
2512 {
2513 //elastic line approach from Arend Schwab
2514 //Version of elastic forces in Hiro paper/Sound and Vibration
2515 temp.SetLen(SOS());
2516 fadd.SetLen(SOS());
2517 fadd.SetAll(0);
2518 //optimized version:
2519
2520 ApplyT(xg);
2521
2522
2523 static Vector Sx; Sx.SetLen(NS()); Sx.SetAll(0); //rx
2524 static Vector Sy; Sy.SetLen(NS()); Sy.SetAll(0); //ry
2525 static Vector Sz; Sz.SetLen(NS()); Sz.SetAll(0); //rz
2526 static Vector Sxx; Sxx.SetLen(NS()); Sxx.SetAll(0); //rxx
2527 static Vector Syx; Syx.SetLen(NS()); Syx.SetAll(0); //ryx
2528 static Vector Szx; Szx.SetLen(NS()); Szx.SetAll(0); //rzx
2529
2530 double epsx;
2531 double epsy;
2532 double epsz;
2533 double gamyz;
2534 double depsy;
2535 double depsz;
2536
2537 Vector3D rx, ry, rz, rxx, ryx, rzx;
2538 //integration of elongation and cross-section change:
2539 GetIntegrationRuleLobatto(x1,w1,orderWl);
2540 for (int i1=1; i1<=x1.GetLen(); i1++)
2541 {
2542 double x = x1(i1);
2543 double x2 = x*x;
2544
2545 //rx:
2546 //GetS0x(Sx,Vector3D(x,0,0));
2547
2548 Sx(1) = (-1.5+1.5*x2)/lx;
2549 Sx(2) = (-0.25-0.5*x+0.75*x2);
2550 Sx(5) = -Sx(1);//(1.5-1.5*x2)/lx;
2551 Sx(6) = Sx(2)+x;//(-0.25+0.5*x+0.75*x2);
2552
2553 rx.X() = Sx(1)*xg(3*0+1);
2554 rx.Y() = Sx(1)*xg(3*0+2);
2555 rx.Z() = Sx(1)*xg(3*0+3);
2556 rx.X() += Sx(2)*xg(3*1+1);
2557 rx.Y() += Sx(2)*xg(3*1+2);
2558 rx.Z() += Sx(2)*xg(3*1+3);
2559 rx.X() += Sx(5)*xg(3*4+1);
2560 rx.Y() += Sx(5)*xg(3*4+2);
2561 rx.Z() += Sx(5)*xg(3*4+3);
2562 rx.X() += Sx(6)*xg(3*5+1);
2563 rx.Y() += Sx(6)*xg(3*5+2);
2564 rx.Z() += Sx(6)*xg(3*5+3);
2565
2566 //GetS0y(Sy,Vector3D(x,0,0));
2567 Sy(3) = -0.5*(-1.0+x);
2568 Sy(7) = 0.5*(1.0+x);
2569 ry.X() = Sy(3)*xg(3*2+1);
2570 ry.Y() = Sy(3)*xg(3*2+2);
2571 ry.Z() = Sy(3)*xg(3*2+3);
2572 ry.X() += Sy(7)*xg(3*6+1);
2573 ry.Y() += Sy(7)*xg(3*6+2);
2574 ry.Z() += Sy(7)*xg(3*6+3);
2575
2576 //GetS0z(Sz,Vector3D(x,0,0));
2577 Sz(4) = -0.5*(-1.0+x);
2578 Sz(8) = 0.5*(1.0+x);
2579 rz.X() = Sz(4)*xg(3*3+1);
2580 rz.Y() = Sz(4)*xg(3*3+2);
2581 rz.Z() = Sz(4)*xg(3*3+3);
2582 rz.X() += Sz(8)*xg(3*7+1);
2583 rz.Y() += Sz(8)*xg(3*7+2);
2584 rz.Z() += Sz(8)*xg(3*7+3);
2585
2586
2587 //integration factors:
2588 double fact = lx * 0.5 * w1(i1);
2589
2590 epsx = 0.5*(rx*rx-1.) - Wlepsx0(i1);
2591 epsy = 0.5*(ry*ry-1.)*factstiffWl - Wlepsy0(i1);
2592 epsz = 0.5*(rz*rz-1.)*factstiffWl - Wlepsz0(i1);
2593 gamyz= ry*rz*factstiffWl - Wlgamyz0(i1);
2594
2595 double GAcs = beamEA / (2.*(1+nu)); //use beam cross-section to define this factor!
2596 double gf = 2.*GAcs/(1.-2.*nu); //stiffness factor for cross-section deformation and axial elongation
2597
2598
2599 //delta eps:
2600 double sumepsi = epsx+epsy+epsz;
2601 for (int i=1; i <= dim; i++)
2602 {
2603 //j=1,2,5,6: Sy(j)=Sz(j)=depsy=depsz=0
2604 temp((1-1)*dim+i) = gf*Sx(1)*rx(i)*(sumepsi*nu+epsx*(1.-2.*nu));
2605 temp((2-1)*dim+i) = gf*Sx(2)*rx(i)*(sumepsi*nu+epsx*(1.-2.*nu));
2606 temp((5-1)*dim+i) = gf*Sx(5)*rx(i)*(sumepsi*nu+epsx*(1.-2.*nu));
2607 temp((6-1)*dim+i) = gf*Sx(6)*rx(i)*(sumepsi*nu+epsx*(1.-2.*nu));
2608
2609 //j=3,7: Sx(j)=Sz(j)=depsx=depsz=0
2610 depsy = Sy(3)*ry(i);
2611 temp((3-1)*dim+i) = sumepsi*nu*gf*depsy+epsy*depsy*(1.-2.*nu)*gf+gamyz * GAcs * Sy(3)*rz(i);
2612 depsy = Sy(7)*ry(i);
2613 temp((7-1)*dim+i) = sumepsi*nu*gf*depsy+epsy*depsy*(1.-2.*nu)*gf+gamyz * GAcs * Sy(7)*rz(i);
2614
2615 //j=4,8: Sx(j)=Sy(j)=depsx=depsy=0
2616 depsz = Sz(4)*rz(i);
2617 temp((4-1)*dim+i) = sumepsi * nu*gf*depsz+epsz*depsz*(1.-2.*nu)*gf+gamyz * GAcs * (Sz(4)*ry(i));
2618 depsz = Sz(8)*rz(i);
2619 temp((8-1)*dim+i) = sumepsi * nu*gf*depsz+epsz*depsz*(1.-2.*nu)*gf+gamyz * GAcs * (Sz(8)*ry(i));
2620
2621 }
2622 temp *= fact;
2623 fadd += temp;
2624 }
2625
2626
2627 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2628 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2629 //Hellinger Reissner principle:
2630 //note that xi=0..1 in the paper of Schwab and x = -1..+1
2631
2632 //1. compute Wxz1, Wxz2, Wxy1, Wxy2
2633 //2. compute delta Wx... times Wx... and sum up in temp
2634 //UO() << "Hellinger Reisner\n";
2635
2636 double Wxz1f = 0;
2637 double Wxz2f = 0;
2638 double Wxy1f = 0;
2639 double Wxy2f = 0;
2640 //shear deformation:
2641 GetIntegrationRuleLobatto(x1,w1,orderWsHR);
2642 for (int i1=1; i1<=x1.GetLen(); i1++)
2643 {
2644 double x = x1(i1);
2645
2646 double x2 = x*x;
2647 //rx:
2648 //GetS0x(Sx,Vector3D(x,0,0));
2649 Sx(1) = (-1.5+1.5*x2)/lx;
2650 Sx(2) = (-0.25-0.5*x+0.75*x2);
2651 Sx(5) = -Sx(1);//(1.5-1.5*x2)/lx;
2652 Sx(6) = Sx(2)+x;//(-0.25+0.5*x+0.75*x2);
2653 rx.X() = Sx(1)*xg(3*0+1);
2654 rx.Y() = Sx(1)*xg(3*0+2);
2655 rx.Z() = Sx(1)*xg(3*0+3);
2656 rx.X() += Sx(2)*xg(3*1+1);
2657 rx.Y() += Sx(2)*xg(3*1+2);
2658 rx.Z() += Sx(2)*xg(3*1+3);
2659 rx.X() += Sx(5)*xg(3*4+1);
2660 rx.Y() += Sx(5)*xg(3*4+2);
2661 rx.Z() += Sx(5)*xg(3*4+3);
2662 rx.X() += Sx(6)*xg(3*5+1);
2663 rx.Y() += Sx(6)*xg(3*5+2);
2664 rx.Z() += Sx(6)*xg(3*5+3);
2665
2666 //GetS0y(Sy,Vector3D(x,0,0));
2667 Sy(3) = -0.5*(-1.0+x);
2668 Sy(7) = 0.5*(1.0+x);
2669 ry.X() = Sy(3)*xg(3*2+1);
2670 ry.Y() = Sy(3)*xg(3*2+2);
2671 ry.Z() = Sy(3)*xg(3*2+3);
2672 ry.X() += Sy(7)*xg(3*6+1);
2673 ry.Y() += Sy(7)*xg(3*6+2);
2674 ry.Z() += Sy(7)*xg(3*6+3);
2675
2676 //GetS0z(Sz,Vector3D(x,0,0));
2677 Sz(4) = -0.5*(-1.0+x);
2678 Sz(8) = 0.5*(1.0+x);
2679 rz.X() = Sz(4)*xg(3*3+1);
2680 rz.Y() = Sz(4)*xg(3*3+2);
2681 rz.Z() = Sz(4)*xg(3*3+3);
2682 rz.X() += Sz(8)*xg(3*7+1);
2683 rz.Y() += Sz(8)*xg(3*7+2);
2684 rz.Z() += Sz(8)*xg(3*7+3);
2685
2686
2687 //integration factors:
2688 double fact = lx * 0.5 * w1(i1);
2689
2690 double gamxy= fact*rx*ry;
2691 double gamxz= fact*rx*rz;
2692
2693 Wxy1f += 0.5*(1-x) * gamxy;
2694 Wxy2f += 0.5*(1+x) * gamxy;
2695 Wxz1f += 0.5*(1-x) * gamxz;
2696 Wxz2f += 0.5*(1+x) * gamxz;
2697
2698 }
2699
2700 GetIntegrationRuleLobatto(x1,w1,orderWsHR);
2701 //now compute variations:
2702 for (int i1=1; i1<=x1.GetLen(); i1++)
2703 {
2704 double x = x1(i1);
2705
2706 double x2 = x*x;
2707 //rx:
2708 //GetS0x(Sx,Vector3D(x,0,0));
2709 Sx(1) = (-1.5+1.5*x2)/lx;
2710 Sx(2) = (-0.25-0.5*x+0.75*x2);
2711 Sx(5) = -Sx(1);//(1.5-1.5*x2)/lx;
2712 Sx(6) = Sx(2)+x;//(-0.25+0.5*x+0.75*x2);
2713 rx.X() = Sx(1)*xg(3*0+1);
2714 rx.Y() = Sx(1)*xg(3*0+2);
2715 rx.Z() = Sx(1)*xg(3*0+3);
2716 rx.X() += Sx(2)*xg(3*1+1);
2717 rx.Y() += Sx(2)*xg(3*1+2);
2718 rx.Z() += Sx(2)*xg(3*1+3);
2719 rx.X() += Sx(5)*xg(3*4+1);
2720 rx.Y() += Sx(5)*xg(3*4+2);
2721 rx.Z() += Sx(5)*xg(3*4+3);
2722 rx.X() += Sx(6)*xg(3*5+1);
2723 rx.Y() += Sx(6)*xg(3*5+2);
2724 rx.Z() += Sx(6)*xg(3*5+3);
2725
2726 //GetS0y(Sy,Vector3D(x,0,0));
2727 Sy(3) = -0.5*(-1.0+x);
2728 Sy(7) = 0.5*(1.0+x);
2729 ry.X() = Sy(3)*xg(3*2+1);
2730 ry.Y() = Sy(3)*xg(3*2+2);
2731 ry.Z() = Sy(3)*xg(3*2+3);
2732 ry.X() += Sy(7)*xg(3*6+1);
2733 ry.Y() += Sy(7)*xg(3*6+2);
2734 ry.Z() += Sy(7)*xg(3*6+3);
2735
2736 //GetS0z(Sz,Vector3D(x,0,0));
2737 Sz(4) = -0.5*(-1.0+x);
2738 Sz(8) = 0.5*(1.0+x);
2739 rz.X() = Sz(4)*xg(3*3+1);
2740 rz.Y() = Sz(4)*xg(3*3+2);
2741 rz.Z() = Sz(4)*xg(3*3+3);
2742 rz.X() += Sz(8)*xg(3*7+1);
2743 rz.Y() += Sz(8)*xg(3*7+2);
2744 rz.Z() += Sz(8)*xg(3*7+3);
2745
2746
2747 //integration factors:
2748 double fact = 0.5 * w1(i1); // length cancels out with length omitted in Fy and Fz, see A.Schwab
2749
2750 //factor for delta gamxy components:
2751 double k1 = beamGAky * fact*(0.5*(1-x)*(4.*Wxy1f - 2.*Wxy2f) + 0.5*(1+x)*(-2.*Wxy1f + 4.*Wxy2f)) - WsHRk10(i1);
2752 //factor for delta gamxz components:
2753 double k2 = beamGAkz * fact*(0.5*(1-x)*(4.*Wxz1f - 2.*Wxz2f) + 0.5*(1+x)*(-2.*Wxz1f + 4.*Wxz2f)) - WsHRk20(i1);
2754
2755 //delta eps:
2756 for (int i=1; i <= dim; i++)
2757 {
2758 //j=1,2,5,6: Sy=Sz=0;
2759 temp((1-1)*dim+i) = (k1 * (Sx(1)*ry(i)) + k2 * (Sx(1)*rz(i)));
2760 temp((2-1)*dim+i) = (k1 * (Sx(2)*ry(i)) + k2 * (Sx(2)*rz(i)));
2761 temp((5-1)*dim+i) = (k1 * (Sx(5)*ry(i)) + k2 * (Sx(5)*rz(i)));
2762 temp((6-1)*dim+i) = (k1 * (Sx(6)*ry(i)) + k2 * (Sx(6)*rz(i)));
2763 //j=3,7: Sx=Sz=0;
2764 temp((3-1)*dim+i) = k1 * Sy(3)*rx(i);
2765 temp((7-1)*dim+i) = k1 * Sy(7)*rx(i);
2766 //j=4,8: Sx=Sy=0;
2767 temp((4-1)*dim+i) = k2 * Sz(4)*rx(i);
2768 temp((8-1)*dim+i) = k2 * Sz(8)*rx(i);
2769 }
2770 fadd += temp;
2771 }
2772
2773
2774 //torsion:
2775 GetIntegrationRule(x1,w1,orderWt);
2776 temp.SetAll(0);
2777 for (int i1=1; i1<=x1.GetLen(); i1++)
2778 {
2779 double x = x1(i1);
2780 //double x2 = x*x;
2781 //GetS0y(Sy,Vector3D(x,0,0));
2782 Sy(3) = -0.5*(-1.0+x);
2783 Sy(7) = 0.5*(1.0+x);
2784 ry.X() = Sy(3)*xg(3*2+1);
2785 ry.Y() = Sy(3)*xg(3*2+2);
2786 ry.Z() = Sy(3)*xg(3*2+3);
2787 ry.X() += Sy(7)*xg(3*6+1);
2788 ry.Y() += Sy(7)*xg(3*6+2);
2789 ry.Z() += Sy(7)*xg(3*6+3);
2790
2791 //GetS0z(Sz,Vector3D(x,0,0));
2792 Sz(4) = -0.5*(-1.0+x);
2793 Sz(8) = 0.5*(1.0+x);
2794 rz.X() = Sz(4)*xg(3*3+1);
2795 rz.Y() = Sz(4)*xg(3*3+2);
2796 rz.Z() = Sz(4)*xg(3*3+3);
2797 rz.X() += Sz(8)*xg(3*7+1);
2798 rz.Y() += Sz(8)*xg(3*7+2);
2799 rz.Z() += Sz(8)*xg(3*7+3);
2800
2801 //ryx:
2802 //GetS0yx(Syx,Vector3D(x,0,0));
2803 Syx(3) = -1./lx;
2804 Syx(7) = 1./lx;
2805 ryx.X() = Syx(3)*xg(3*2+1);
2806 ryx.Y() = Syx(3)*xg(3*2+2);
2807 ryx.Z() = Syx(3)*xg(3*2+3);
2808 ryx.X() += Syx(7)*xg(3*6+1);
2809 ryx.Y() += Syx(7)*xg(3*6+2);
2810 ryx.Z() += Syx(7)*xg(3*6+3);
2811
2812 //rzx:
2813 //GetS0zx(Szx,Vector3D(x,0,0));
2814 Szx(4) = -1./lx;
2815 Szx(8) = 1./lx;
2816 rzx.X() = Szx(4)*xg(3*3+1);
2817 rzx.Y() = Szx(4)*xg(3*3+2);
2818 rzx.Z() = Szx(4)*xg(3*3+3);
2819 rzx.X() += Szx(8)*xg(3*7+1);
2820 rzx.Y() += Szx(8)*xg(3*7+2);
2821 rzx.Z() += Szx(8)*xg(3*7+3);
2822
2823
2824 //integration factors:
2825 double fact = beamGJkx * lx * 0.5 * w1(i1);
2826
2827 double kapx= 0.5 * (rz*ryx - ry*rzx) - Wtkapx0(i1);
2828
2829 //delta eps:
2830 for (int i=1; i <= dim; i++)
2831 {
2832 //j=3,7: Sx=Sz=0;
2833 temp((3-1)*dim+i) = 0.5*(Syx(3)*rz(i) - Sy(3)*rzx(i));
2834 temp((7-1)*dim+i) = 0.5*(Syx(7)*rz(i) - Sy(7)*rzx(i));
2835 //j=4,8: Sx=Sy=0;
2836 temp((4-1)*dim+i) = 0.5*(Sz(4)*ryx(i) - Szx(4)*ry(i));
2837 temp((8-1)*dim+i) = 0.5*(Sz(8)*ryx(i) - Szx(8)*ry(i));
2838
2839 /*
2840 for (int j=1; j <= ns; j++)
2841 {
2842 temp((j-1)*dim+i) = 0.5*(Sz(j)*ryx(i)+Syx(j)*rz(i) - (Sy(j)*rzx(i)+Szx(j)*ry(i)));
2843 }*/
2844 }
2845 temp *= fact*kapx;
2846 fadd += temp;
2847 }
2848
2849 //bending:
2850 GetIntegrationRule(x1,w1,orderWb);
2851 for (int i1=1; i1<=x1.GetLen(); i1++)
2852 {
2853 double x = x1(i1);
2854
2855 double x2 = x*x;
2856 //GetS0y(Sy,Vector3D(x,0,0));
2857 Sy(3) = -0.5*(-1.0+x);
2858 Sy(7) = 0.5*(1.0+x);
2859 ry.X() = Sy(3)*xg(3*2+1);
2860 ry.Y() = Sy(3)*xg(3*2+2);
2861 ry.Z() = Sy(3)*xg(3*2+3);
2862 ry.X() += Sy(7)*xg(3*6+1);
2863 ry.Y() += Sy(7)*xg(3*6+2);
2864 ry.Z() += Sy(7)*xg(3*6+3);
2865
2866 //GetS0z(Sz,Vector3D(x,0,0));
2867 Sz(4) = -0.5*(-1.0+x);
2868 Sz(8) = 0.5*(1.0+x);
2869 rz.X() = Sz(4)*xg(3*3+1);
2870 rz.Y() = Sz(4)*xg(3*3+2);
2871 rz.Z() = Sz(4)*xg(3*3+3);
2872 rz.X() += Sz(8)*xg(3*7+1);
2873 rz.Y() += Sz(8)*xg(3*7+2);
2874 rz.Z() += Sz(8)*xg(3*7+3);
2875
2876 //rxx:
2877 //GetS0xx(Sxx,Vector3D(x,0,0));
2878 Sxx(1) = 4./Sqr(lx)*3.0/2.0*x;
2879 Sxx(2) = 1./lx*(-1.0+3.0*x);
2880 Sxx(5) = -Sxx(1);
2881 Sxx(6) = 1./lx*(1.0+3.0*x);
2882 rxx.X() = Sxx(1)*xg(3*(1-1)+1);
2883 rxx.Y() = Sxx(1)*xg(3*(1-1)+2);
2884 rxx.Z() = Sxx(1)*xg(3*(1-1)+3);
2885 rxx.X() += Sxx(2)*xg(3*(2-1)+1);
2886 rxx.Y() += Sxx(2)*xg(3*(2-1)+2);
2887 rxx.Z() += Sxx(2)*xg(3*(2-1)+3);
2888 rxx.X() += Sxx(5)*xg(3*(5-1)+1);
2889 rxx.Y() += Sxx(5)*xg(3*(5-1)+2);
2890 rxx.Z() += Sxx(5)*xg(3*(5-1)+3);
2891 rxx.X() += Sxx(6)*xg(3*(6-1)+1);
2892 rxx.Y() += Sxx(6)*xg(3*(6-1)+2);
2893 rxx.Z() += Sxx(6)*xg(3*(6-1)+3);
2894
2895
2896 //integration factors:
2897 double facty = beamEIy * lx * 0.5 * w1(i1);
2898 double factz = beamEIz * lx * 0.5 * w1(i1);
2899
2900 double kapy= rz*rxx - Wbkapy0(i1);
2901 double kapz=-(ry*rxx) - Wbkapz0(i1);
2902
2903 //delta eps:
2904 for (int i=1; i <= dim; i++)
2905 {
2906 //j=1,2,5,6: Sy=Sz=0;
2907 temp((1-1)*dim+i) = facty*kapy*(Sxx(1)*rz(i)) + factz*kapz*(-Sxx(1)*ry(i));
2908 temp((2-1)*dim+i) = facty*kapy*(Sxx(2)*rz(i)) + factz*kapz*(-Sxx(2)*ry(i));
2909 temp((5-1)*dim+i) = facty*kapy*(Sxx(5)*rz(i)) + factz*kapz*(-Sxx(5)*ry(i));
2910 temp((6-1)*dim+i) = facty*kapy*(Sxx(6)*rz(i)) + factz*kapz*(-Sxx(6)*ry(i));
2911 //j=3,7: Sx=Sz=0;
2912 temp((3-1)*dim+i) = factz*kapz*(-Sy(3)*rxx(i));
2913 temp((7-1)*dim+i) = factz*kapz*(-Sy(7)*rxx(i));
2914 //j=4,8: Sx=Sy=0;
2915 temp((4-1)*dim+i) = facty*kapy*(Sz(4)*rxx(i));
2916 temp((8-1)*dim+i) = facty*kapy*(Sz(8)*rxx(i));
2917
2918
2919 /* for (int j=1; j <= ns; j++)
2920 {
2921 temp((j-1)*dim+i) = facty*kapy*(Sz(j)*rxx(i)+Sxx(j)*rz(i)) + factz*kapz*(-Sy(j)*rxx(i)-Sxx(j)*ry(i));
2922 }*/
2923 }
2924 //temp *= 1;
2925 fadd += temp;
2926 }
2927
2928 }
2929 else if (elasticforce_beam < 3)
2930 {
2931 //UO() << "standard ANCForig\n";
2932 static Vector u;
2933 u.SetLen(sos);
2934 int useu = 1;
2935 if (useu)
2936 {
2937 for (int i=1; i <= sos; i++)
2938 u(i) = xg(i) - x_init(i);
2939 }
2940 else
2941 {
2942 for (int i=1; i <= sos; i++)
2943 u(i) = xg(i);
2944 }
2945 ApplyT(u); // u = T*u
2946 //UO() << "u=" << u << "\n";
2947
2948 double la=Em * nu / ((1.+nu)*(1.-2.*nu));
2949 double mu=Em / 2. / (1.+nu);
2950
2951 Matrix3D strain, piola1, F;
2952
2953 temp.SetLen(SOS());
2954 fadd.SetLen(SOS());
2955 fadd.SetAll(0);
2956
2957 GetIntegrationRule(x1,w1,orderx); //x_i ... integration points, w_i ... integration weights
2958 GetIntegrationRule(x2,w2,orderyz);
2959 GetIntegrationRule(x3,w3,orderyz);
2960 int kx1 = x2.Length()*x3.Length();
2961 int kx2 = x3.Length();
2962
2963 for (int i1=1; i1<=x1.GetLen(); i1++)
2964 {
2965 for (int i2=1; i2<=x2.GetLen(); i2++)
2966 {
2967 for (int i3=1; i3<=x3.GetLen(); i3++)
2968 {
2969 //TMStartTimer(20);
2970 int i,j,k;
2971 int ind = (i1-1)*kx1+(i2-1)*kx2+(i3-1);
2972
2973 // compute F
2974 F.SetAll(0);
2975 int l;
2976 const Matrix& agrad = grad[ind]; //\bar S^D
2977 for (j = 1; j <= dim; j++)
2978 {
2979 for (i = 1; i <= ns; i++)
2980 {
2981 l = (i-1)*dim+j;
2982 //u = xg(l) - x_init(l);
2983 for (k = 1; k <= dim; k++)
2984 {
2985 F(j,k) += grad[ind](k,i)*u(l);
2986 //G(j,k) += DS(k,i)*u((i-1)*dim+j);
2987 }
2988 }
2989 if (useu) F(j,j) += 1;
2990 }
2991
2992 //jacinv = jacinv.GetTp();
2993 //G = G * jacinv; //also possible!!!
2994 //F = Matrix3D(1) + G;
2995
2996 int linear=0;
2997 if (linear)
2998 {
2999 F += Matrix3D(-1.);
3000 strain = 0.5*(F+F.GetTp());
3001 piola1 = ((2*mu) * strain + Matrix3D(la * strain.Trace()));
3002 }
3003 else
3004 {
3005 // Green-Lagrange strain tensor
3006 //strain = 0.5 * (F.GetTp() * F - I);
3007 F.GetATA2(strain);
3008 strain(1,1) -= 0.5; strain(2,2) -= 0.5; strain(3,3) -= 0.5;
3009
3010 // Matrix3D Eplast = ....
3011 // strain -= Eplast; //strain = strain - Eplast;
3012
3013 // Matrix3D piola2 = (2*mu) * strain + Matrix3D(la * strain.Trace());
3014 // piola1 = F*piola2;
3015 piola1 = F * ((2*mu) * strain + Matrix3D(la * strain.Trace()));
3016 }
3017
3018 for (int j=1; j <= 3; j++)
3019 {
3020 for (int i = 0; i < ns; i++)
3021 {
3022 temp(3*i+j) = grad[ind](1, i+1)*piola1(j,1)
3023 + grad[ind](2, i+1)*piola1(j,2)
3024 + grad[ind](3, i+1)*piola1(j,3);
3025 }
3026 }
3027
3028 //temp *= fabs (jacdet[ind]) * w1(i1)*w2(i2)*w3(i3);
3029 //fadd += temp;
3030 fadd.MultAdd(fabs (jacdet[ind]) * w1(i1)*w2(i2)*w3(i3),temp);
3031 //f -= fabs (jacdet[ind]) * w1(i1)*w2(i2)*w3(i3) * (B*piola1v);
3032 //TMStopTimer(21);
3033 }
3034 }
3035 }
3036 }
3037 else if (elasticforce_beam == 4)
3038 {
3039 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
3040 //Poisson locking eliminated
3041
3042 static Vector u;
3043 u.SetLen(sos);
3044 int useu = 1;
3045 if (useu)
3046 {
3047 for (int i=1; i <= sos; i++)
3048 u(i) = xg(i) - x_init(i);
3049 }
3050 else
3051 {
3052 for (int i=1; i <= sos; i++)
3053 u(i) = xg(i);
3054 }
3055 //ApplyT(u); // u = T*u
3056 //UO() << "u=" << u << "\n";
3057
3058 double la=Em * nu / ((1.+nu)*(1.-2.*nu));
3059 double mu=Em / 2. / (1.+nu);
3060 //double ksT = 1./1.248424513011470; //shear correction for h=0.02, b=0.01
3061 //double ksT = 3.51176216306567e-009/1.4857142857142857e-8;//10.*(1+nu)/(12+11*nu); //shear correction for h=0.02, b=0.005
3062 double ksT = (beamGJkx) / (mu * ly*lz*(Sqr(ly) + Sqr(lz))/12.);//10.*(1+nu)/(12+11*nu); //shear correction for h=0.02, b=0.005
3063 double ks = 1;//10.*(1+nu)/(12+11*nu); //shear correction factor GA
3064
3065
3066 Matrix3D strain, piola1, F;
3067 Matrix3D jac, jacinv;
3068 double jacdet;
3069
3070 static Matrix Dmat;
3071 static Matrix Dmat2;
3072 Dmat.SetSize(6,6);
3073 Dmat2.SetSize(6,6);
3074 static Vector strainvec;
3075 strainvec.SetLen(6);
3076 static Vector stressvec;
3077 stressvec.SetLen(6);
3078
3079 GetDMatrix(Dmat, nu, Em);
3080 Dmat(4,4) *= 1; //no shear correction for Syz necessary
3081 Dmat(5,5) *= ksT;
3082 Dmat(6,6) *= ksT;
3083
3084 Dmat2.SetAll(0);
3085 Dmat2(1,1) = Em;
3086 Dmat2(2,2) = Em;
3087 Dmat2(3,3) = Em;
3088 Dmat2(4,4) = Dmat(4,4);
3089 Dmat2(5,5) = Dmat(5,5);
3090 Dmat2(6,6) = Dmat(6,6);
3091
3092 Dmat -= Dmat2;
3093 Dmat(5,5) = ks-ksT; //at elastic line
3094 Dmat(6,6) = ks-ksT; //at elastic line
3095
3096 //GetDMatrix(Dmat2,nu,Em);
3097
3098 temp.SetLen(SOS());
3099 fadd.SetLen(SOS());
3100 fadd.SetAll(0);
3101
3102 for (int kk=1; kk <= 2; kk++)
3103 {
3104 //kk=1: just poisson effect at element line
3105 //kk=2: no poisson effect, whole body
3106
3107 if (kk == 2)
3108 {
3109 GetIntegrationRule(x1,w1,orderx); //x_i ... integration points, w_i ... integration weights
3110 GetIntegrationRule(x2,w2,orderyz);
3111 GetIntegrationRule(x3,w3,orderyz);
3112 }
3113 else
3114 {
3115 GetIntegrationRule(x1,w1,orderx); //x_i ... integration points, w_i ... integration weights
3116 GetIntegrationRule(x2,w2,1);
3117 GetIntegrationRule(x3,w3,1);
3118 }
3119
3120 int kx1 = x2.Length()*x3.Length();
3121 int kx2 = x3.Length();
3122
3123 for (int i1=1; i1<=x1.GetLen(); i1++)
3124 {
3125 for (int i2=1; i2<=x2.GetLen(); i2++)
3126 {
3127 for (int i3=1; i3<=x3.GetLen(); i3++)
3128 {
3129 //TMStartTimer(20);
3130 int i,j,k;
3131 int ind = (i1-1)*kx1+(i2-1)*kx2+(i3-1);
3132 Vector3D p(x1(i1),x2(i2),x3(i3));
3133
3134 // compute F
3135 F.SetAll(0);
3136 int l;
3137 static Matrix agrad; //\bar S^D
3138
3139 GetDSMatrix0(p,DS);
3140
3141 GetJacobi(jac,p,DS,x_init);
3142 jacdet = jac.Det();
3143 jac.GetInverse(jacinv);
3144 jacinv = jacinv.GetTp();
3145
3146 agrad.SetSize(3,NS());
3147 Mult(jacinv, DS, agrad);
3148
3149 for (j = 1; j <= dim; j++)
3150 {
3151 for (i = 1; i <= ns; i++)
3152 {
3153 l = (i-1)*dim+j;
3154 //u = xg(l) - x_init(l);
3155 for (k = 1; k <= dim; k++)
3156 {
3157 F(j,k) += agrad(k,i)*u(l);
3158 //G(j,k) += DS(k,i)*u((i-1)*dim+j);
3159 }
3160 }
3161 if (useu) F(j,j) += 1;
3162 }
3163
3164 // Green-Lagrange strain tensor
3165 //strain = 0.5 * (F.GetTp() * F - I);
3166 F.GetATA2(strain);
3167 strain(1,1) -= 0.5; strain(2,2) -= 0.5; strain(3,3) -= 0.5;
3168
3169 strainvec(1) = strain(1,1);
3170 strainvec(2) = strain(2,2);
3171 strainvec(3) = strain(3,3);
3172 strainvec(4) = 2.*strain(2,3);
3173 strainvec(5) = 2.*strain(3,1);
3174 strainvec(6) = 2.*strain(1,2);
3175
3176 if (kk == 1)
3177 Mult(Dmat,strainvec,stressvec);
3178 else
3179 Mult(Dmat2,strainvec,stressvec);
3180
3181 //piola1 = F * ((2*mu) * strain + Matrix3D(la * strain.Trace()));
3182 piola1(1,1) = stressvec(1);
3183 piola1(2,2) = stressvec(2);
3184 piola1(3,3) = stressvec(3);
3185 piola1(2,3) = stressvec(4);
3186 piola1(3,1) = stressvec(5);
3187 piola1(1,2) = stressvec(6);
3188 piola1(3,2) = stressvec(4);
3189 piola1(1,3) = stressvec(5);
3190 piola1(2,1) = stressvec(6);
3191
3192 piola1 = F*piola1;
3193
3194 for (int j=1; j <= 3; j++)
3195 {
3196 for (int i = 0; i < ns; i++)
3197 {
3198 temp(3*i+j) = agrad(1, i+1)*piola1(j,1)
3199 + agrad(2, i+1)*piola1(j,2)
3200 + agrad(3, i+1)*piola1(j,3);
3201 }
3202 }
3203
3204 //temp *= fabs (jacdet[ind]) * w1(i1)*w2(i2)*w3(i3);
3205 //fadd += temp;
3206 fadd.MultAdd(fabs (jacdet) * w1(i1)*w2(i2)*w3(i3),temp);
3207 //f -= fabs (jacdet[ind]) * w1(i1)*w2(i2)*w3(i3) * (B*piola1v);
3208 //TMStopTimer(21);
3209 }
3210 }
3211 }
3212 }
3213 }
3214
3215
3216
3217 if (!(elasticforce_beam == 4)) ApplyTtp(fadd);
3218
3219 f -= fadd;
3220 TMStopTimer(22);
3221
3222 }
3223
3224 if (GetMassDamping() != 0)
3225 {
3226 // +++++++ damping: +++++++
3227 for (int i = 1; i <= SOS(); i++)
3228 xg(i) = XGP(i);
3229
3230 if (massmatrix.Getcols() == SOS())
3231 {
3232 Mult(massmatrix,xg,temp);
3233 }
3234 else
3235 {
3236 Matrix dmat;
3237 EvalM(dmat,t);
3238 Mult(dmat,xg,temp);
3239 }
3240 temp *= GetMassDamping();
3241 f -= temp;
3242 }
3243 };
3244
GraduD(const Vector3D & ploc,const Vector & u,Matrix3D & gradu) const3245 void ANCFBeam3D::GraduD(const Vector3D& ploc, const Vector& u, Matrix3D& gradu) const
3246 {
3247 static Matrix DS;
3248 GetDSMatrix0(ploc,DS);
3249
3250 Matrix3D jac, jacinv;
3251 GetJacobi(jac,ploc,DS,e0);
3252
3253 jac.GetInverse(jacinv);
3254 jacinv = jacinv.GetTp();
3255
3256 static Matrix grad;
3257 grad.SetSize(3,NS());
3258 Mult(jacinv, DS, grad);
3259
3260 gradu.SetAll(0);
3261 int dim = Dim();
3262 int l;
3263 for (int j = 1; j <= dim; j++)
3264 {
3265 for (int i = 1; i <= NS(); i++)
3266 {
3267 l = (i-1)*dim+j;
3268 for (int k = 1; k <= dim; k++)
3269 {
3270 gradu(j,k) += grad(k,i)*u(l);
3271 }
3272 }
3273 }
3274 }
3275
GetAvailableFieldVariables(TArray<FieldVariableDescriptor> & variables)3276 void ANCFBeam3D::GetAvailableFieldVariables(TArray<FieldVariableDescriptor> & variables)
3277 {
3278 FieldVariableDescriptor::AddTypeIntoArray(variables, FieldVariableDescriptor::FVT_displacement,
3279 FieldVariableDescriptor::FVCI_z);
3280 FieldVariableDescriptor::AddTypeIntoArray(variables, FieldVariableDescriptor::FVT_total_strain,
3281 FieldVariableDescriptor::FVCI_z, true);
3282 FieldVariableDescriptor::AddTypeIntoArray(variables, FieldVariableDescriptor::FVT_stress,
3283 FieldVariableDescriptor::FVCI_z, true);
3284 FieldVariableDescriptor::AddTypeIntoArray(variables, FieldVariableDescriptor::FVT_stress_mises);
3285 }
3286
GetFieldVariableValue(const FieldVariableDescriptor & fvd,const Vector3D & local_position,bool flagD)3287 double ANCFBeam3D::GetFieldVariableValue(const FieldVariableDescriptor & fvd, const Vector3D & local_position, bool flagD)
3288 {
3289 if (!flagD)
3290 return FIELD_VARIABLE_NO_VALUE; //not implemented for computation!
3291 if (fvd.VariableType() == FieldVariableDescriptor::FVT_displacement) //show displacements
3292 {
3293 Vector3D u;
3294 Vector3D p0 = local_position;
3295 p0.Scale(2./lx,2./ly,2./lz);
3296 if(flagD)
3297 u = GetDisplacementD(p0);
3298 else
3299 u = GetDisplacement(p0);
3300
3301 return fvd.GetComponent(u);
3302 }
3303
3304 if (elasticforce_beam != 3 || fvd.VariableType() == FieldVariableDescriptor::FVT_total_strain)
3305 {
3306 xgd.SetLen(SOS());
3307 GetDrawCoordinates(xgd);
3308
3309 xgd = T*xgd;
3310 //transform xgd to displacements, actually not necessary ...
3311 xgd -= e0;
3312 Matrix3D F;
3313 GraduD(local_position,xgd,F);
3314 F += Matrix3D(1);
3315 Matrix3D strain = 0.5*(F.GetTp()*F-Matrix3D(1));
3316 //F.GetATA2(strain);
3317 //strain-=Matrix3D(0.5);
3318 //F -= Matrix3D(1);
3319
3320 double la=Em * nu / ((1.+nu)*(1.-2.*nu));
3321 double mu=Em / 2. / (1.+nu);
3322 Matrix3D stress = (2*mu) * strain + Matrix3D(la * strain.Trace());
3323
3324 if(fvd.VariableType() == FieldVariableDescriptor::FVT_stress_mises)
3325 {
3326 return stress.Mises();
3327 }
3328 else if (fvd.VariableType() == FieldVariableDescriptor::FVT_stress)
3329 {
3330 return fvd.GetComponent(stress);
3331 }
3332 else if (fvd.VariableType() == FieldVariableDescriptor::FVT_total_strain)
3333 {
3334 return fvd.GetComponent(strain);
3335 }
3336 }
3337 else
3338 {
3339 //only for rectangular cross-section!:
3340 xgd.SetLen(SOS());
3341 GetDrawCoordinates(xgd);
3342
3343 xgd = T*xgd;
3344
3345 double x0 = local_position.X();
3346 double yb = 0.5*local_position.Y()*ly;
3347 double zb = 0.5*local_position.Z()*lz;
3348 //global_uo << "z=" << local_position.Z() << ", zb=" << zb << "\n";
3349
3350 double Gm = Em / (2.*(1+nu));
3351 double A = ly*lz;
3352
3353 double ky = 10.*(1+nu)/(12.+11.*nu); //shear correction factors (square): 10*(1+nu)/(12.+11.*nu)
3354 double kz = 10.*(1+nu)/(12.+11.*nu); //shear correction factors (square): 10*(1+nu)/(12.+11.*nu)
3355 double kx = 0.8436; //shear correction factor torsion : 0.8436 (square)
3356 //kx=ky=kz=1;
3357
3358 static Vector Sx; Sx.SetLen(NS()); //rx
3359 static Vector Sy; Sy.SetLen(NS()); //ry
3360 static Vector Sz; Sz.SetLen(NS()); //rz
3361 static Vector Sxx; Sxx.SetLen(NS()); //rxx
3362 static Vector Syx; Syx.SetLen(NS()); //ryx
3363 static Vector Szx; Szx.SetLen(NS()); //rzx
3364 Vector3D rx,ry,rz,rxx,ryx,rzx;
3365
3366 Vector3D rx1,ry1,rz1,ryx1,rzx1;
3367 Vector3D rx2,ry2,rz2,ryx2,rzx2;
3368
3369
3370 Matrix3D stress(0);
3371
3372 //shear stresses
3373 //need integration of coefficitents Wxz and Wxy:
3374 double Wxz1f = 0;
3375 double Wxz2f = 0;
3376 double Wxy1f = 0;
3377 double Wxy2f = 0;
3378
3379 static Vector x1d; //not allowed to take variables from computation!!!
3380 static Vector w1d;
3381
3382 GetIntegrationRuleLobatto(x1d,w1d,2);
3383 for (int i1=1; i1<=x1d.GetLen(); i1++)
3384 {
3385 double x = x1d(i1);
3386
3387 double x2 = x*x;
3388 //rx:
3389 //GetS0x(Sx,Vector3D(x,0,0));
3390 Sx(1) = (-1.5+1.5*x2)/lx;
3391 Sx(2) = (-0.25-0.5*x+0.75*x2);
3392 Sx(5) = -Sx(1);//(1.5-1.5*x2)/lx;
3393 Sx(6) = Sx(2)+x;//(-0.25+0.5*x+0.75*x2);
3394 rx.X() = Sx(1)*xgd(3*0+1);
3395 rx.Y() = Sx(1)*xgd(3*0+2);
3396 rx.Z() = Sx(1)*xgd(3*0+3);
3397 rx.X() += Sx(2)*xgd(3*1+1);
3398 rx.Y() += Sx(2)*xgd(3*1+2);
3399 rx.Z() += Sx(2)*xgd(3*1+3);
3400 rx.X() += Sx(5)*xgd(3*4+1);
3401 rx.Y() += Sx(5)*xgd(3*4+2);
3402 rx.Z() += Sx(5)*xgd(3*4+3);
3403 rx.X() += Sx(6)*xgd(3*5+1);
3404 rx.Y() += Sx(6)*xgd(3*5+2);
3405 rx.Z() += Sx(6)*xgd(3*5+3);
3406
3407 //GetS0y(Sy,Vector3D(x,0,0));
3408 Sy(3) = -0.5*(-1.0+x);
3409 Sy(7) = 0.5*(1.0+x);
3410 ry.X() = Sy(3)*xgd(3*2+1);
3411 ry.Y() = Sy(3)*xgd(3*2+2);
3412 ry.Z() = Sy(3)*xgd(3*2+3);
3413 ry.X() += Sy(7)*xgd(3*6+1);
3414 ry.Y() += Sy(7)*xgd(3*6+2);
3415 ry.Z() += Sy(7)*xgd(3*6+3);
3416
3417 //GetS0z(Sz,Vector3D(x,0,0));
3418 Sz(4) = -0.5*(-1.0+x);
3419 Sz(8) = 0.5*(1.0+x);
3420 rz.X() = Sz(4)*xgd(3*3+1);
3421 rz.Y() = Sz(4)*xgd(3*3+2);
3422 rz.Z() = Sz(4)*xgd(3*3+3);
3423 rz.X() += Sz(8)*xgd(3*7+1);
3424 rz.Y() += Sz(8)*xgd(3*7+2);
3425 rz.Z() += Sz(8)*xgd(3*7+3);
3426
3427 //integration factors:
3428 double fact = A * lx * 0.5 * w1d(i1);
3429
3430 double gamxy= fact*rx*ry;
3431 double gamxz= fact*rx*rz;
3432
3433 Wxy1f += 0.5*(1-x) * gamxy;
3434 Wxy2f += 0.5*(1+x) * gamxy;
3435 Wxz1f += 0.5*(1-x) * gamxz;
3436 Wxz2f += 0.5*(1+x) * gamxz;
3437
3438 GetS0yx(Syx,Vector3D(x,0,0));
3439 GetS0zx(Szx,Vector3D(x,0,0));
3440 ryx.Set(0,0,0);
3441 rzx.Set(0,0,0);
3442
3443 for (int i = 1; i <= NS(); i++)
3444 {
3445 ryx.X() += Syx(i)*xgd(3*(i-1)+1);
3446 ryx.Y() += Syx(i)*xgd(3*(i-1)+2);
3447 ryx.Z() += Syx(i)*xgd(3*(i-1)+3);
3448 rzx.X() += Szx(i)*xgd(3*(i-1)+1);
3449 rzx.Y() += Szx(i)*xgd(3*(i-1)+2);
3450 rzx.Z() += Szx(i)*xgd(3*(i-1)+3);
3451 }
3452
3453
3454 if (x == -1) {rx1 = rx; ry1 = ry; rz1 = rz; ryx1 = ryx; rzx1 = rzx;}
3455 if (x == 1) {rx2 = rx; ry2 = ry; rz2 = rz; ryx2 = ryx; rzx2 = rzx;}
3456 }
3457
3458 GetS0x(Sx,Vector3D(x0,0,0));
3459 GetS0y(Sy,Vector3D(x0,0,0));
3460 GetS0z(Sz,Vector3D(x0,0,0));
3461 GetS0xx(Sxx,Vector3D(x0,0,0));
3462 GetS0yx(Syx,Vector3D(x0,0,0));
3463 GetS0zx(Szx,Vector3D(x0,0,0));
3464
3465 rx.Set(0,0,0);
3466 ry.Set(0,0,0);
3467 rz.Set(0,0,0);
3468 rxx.Set(0,0,0);
3469 ryx.Set(0,0,0);
3470 rzx.Set(0,0,0);
3471
3472 for (int i = 1; i <= NS(); i++)
3473 {
3474 rx.X() += Sx(i)*xgd(3*(i-1)+1);
3475 rx.Y() += Sx(i)*xgd(3*(i-1)+2);
3476 rx.Z() += Sx(i)*xgd(3*(i-1)+3);
3477 ry.X() += Sy(i)*xgd(3*(i-1)+1);
3478 ry.Y() += Sy(i)*xgd(3*(i-1)+2);
3479 ry.Z() += Sy(i)*xgd(3*(i-1)+3);
3480 rz.X() += Sz(i)*xgd(3*(i-1)+1);
3481 rz.Y() += Sz(i)*xgd(3*(i-1)+2);
3482 rz.Z() += Sz(i)*xgd(3*(i-1)+3);
3483 rxx.X() += Sxx(i)*xgd(3*(i-1)+1);
3484 rxx.Y() += Sxx(i)*xgd(3*(i-1)+2);
3485 rxx.Z() += Sxx(i)*xgd(3*(i-1)+3);
3486 ryx.X() += Syx(i)*xgd(3*(i-1)+1);
3487 ryx.Y() += Syx(i)*xgd(3*(i-1)+2);
3488 ryx.Z() += Syx(i)*xgd(3*(i-1)+3);
3489 rzx.X() += Szx(i)*xgd(3*(i-1)+1);
3490 rzx.Y() += Szx(i)*xgd(3*(i-1)+2);
3491 rzx.Z() += Szx(i)*xgd(3*(i-1)+3);
3492 }
3493
3494 //bending curvatures:
3495 double kapy= (rz*rxx);
3496 double kapz=-(ry*rxx);
3497
3498 //(mid strain+curvatures*heigth)*Em:
3499 stress(1,1) = Em*(0.5*(rx*rx-1)+kapz*yb-kapy*zb); //checked sign by comparison!!!
3500
3501 //shear due to shear forces:
3502
3503 Vector2D Nf(0.5*(1-x0),0.5*(1+x0)); //shape function for shear ... linear!!!
3504
3505 double Sxym;
3506 double Sxzm;
3507
3508 //Sxym = Gm*ky/(A*lx)*(Vector2D(4.*Wxy1f-2.*Wxy2f,-2.*Wxy1f+4.*Wxy2f)*Nf);
3509 //Sxzm = Gm*kz/(A*lx)*(Vector2D(4.*Wxz1f-2.*Wxz2f,-2.*Wxz1f+4.*Wxz2f)*Nf);
3510
3511
3512 //alternatively for shear???
3513 //take shear directly, interpolate linearly ...
3514
3515 Sxym = Gm*ky*(Vector2D(rx1*ry1,rx2*ry2)*Nf);
3516 Sxzm = Gm*kz*(Vector2D(rx1*rz1,rx2*rz2)*Nf);
3517
3518 //linearly interpolate shear parameters computed in Hellinger Reissner?
3519 //Sxym = Gm*ky/(A*lx)*(Vector2D(2.*Wxy1f, 2.*Wxy2f)*Nf);
3520 //Sxzm = Gm*kz/(A*lx)*(Vector2D(2.*Wxz1f, 2.*Wxz2f)*Nf);
3521
3522
3523 stress(1,2) = Sxym*1.5*(Sqr(ly)-4.*Sqr(yb))/(lz*Cub(ly))*A; //put formula for shear for different cross-sections here!!!
3524 stress(1,3) =-Sxzm*1.5*(Sqr(lz)-4.*Sqr(zb))/(ly*Cub(lz))*A; //Vorzeichen nicht gleich mit General.cpp
3525
3526
3527 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
3528 //Torsion by St. Venant's theory:
3529
3530 double kapx1 = 0.5 * (rz1*ryx1 - ry1*rzx1);
3531 double kapx2 = 0.5 * (rz2*ryx2 - ry2*rzx2);
3532
3533 double Fxy, Fxz, theta;
3534 //linear interpolation of kapx:
3535 Fxy = Sxym;
3536 Fxz = Sxzm;
3537 theta = (Vector2D(kapx1,kapx2)*Nf); //twist
3538
3539 double sum12 = 0;
3540 double sum13 = 0;
3541 double a = 0.5*ly;
3542 double b = 0.5*lz;
3543
3544 double nn = 10; //terms to add from summation formula ... 2 gives 5 digits for maximum strain, but only 1 digit for edge strain
3545 //10 gives about 2.7% error in edge strain
3546
3547 for (double n=0; n <= nn; n++)
3548 {
3549 sum12 += 16.*pow(-1,n)*a/Sqr(2*n+1)/Sqr(MY_PI)/cosh(0.5*(2*n+1)*MY_PI/a*b)*cos(0.5*(2*n+1)*MY_PI/a*yb)*sinh(0.5*(2*n+1)*MY_PI/a*zb);
3550 sum13 += 16.*pow(-1,n)*a/Sqr(2*n+1)/Sqr(MY_PI)/cosh(0.5*(2*n+1)*MY_PI/a*b)*sin(0.5*(2*n+1)*MY_PI/a*yb)*cosh(0.5*(2*n+1)*MY_PI/a*zb);
3551 }
3552
3553 stress(1,2) +=-Gm*theta*sum12;
3554 stress(1,3) += Gm*theta*(2.*yb-sum13);
3555
3556 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
3557
3558 stress(2,1) = stress(1,2);
3559 stress(3,1) = stress(1,3);
3560
3561 if(fvd.VariableType() == FieldVariableDescriptor::FVT_stress_mises)
3562 {
3563 return stress.Mises();
3564 }
3565 else if (fvd.VariableType() == FieldVariableDescriptor::FVT_stress)
3566 {
3567 return fvd.GetComponent(stress);
3568 }
3569 }
3570
3571 return FIELD_VARIABLE_NO_VALUE;
3572 }
3573
ComputeCorotationalFrame(Vector3D & pref,Matrix3D & Aref)3574 void ANCFBeam3D::ComputeCorotationalFrame(Vector3D& pref, Matrix3D& Aref)
3575 {
3576
3577 xg.SetLen(SOS());
3578 GetCoordinates(xg);
3579 ApplyT(xg);
3580
3581 pref = 0.5*Vector3D(xg(13)+xg(1), xg(14)+xg(2), xg(15)+xg(3));
3582
3583 /*
3584 Vector3D x1(xg(13)-xg(1), xg(14)-xg(2), xg(15)-xg(3));
3585 Vector3D y1(xg(7), xg(8), xg(9));
3586 Vector3D z1(xg(10),xg(11),xg(12));
3587
3588 x1.Normalize();
3589 Vector3D y1a = y1;
3590 Vector3D z1a = z1;
3591 x1.GramSchmidt(y1);
3592 y1.Normalize();
3593 z1 = x1.Cross(y1);
3594
3595 Aref = Matrix3D(
3596 x1.X(),y1.X(),z1.X(),
3597 x1.Y(),y1.Y(),z1.Y(),
3598 x1.Z(),y1.Z(),z1.Z());
3599 */
3600
3601
3602 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
3603 double q1 = xg(1); double q2 = xg(2); double q3 = xg(3);
3604 double q7 = xg(7); double q8 = xg(8); double q9 = xg(9);
3605 double q13 = xg(13); double q14 = xg(14); double q15 = xg(15);
3606 double q19 = xg(19); double q20 = xg(20); double q21 = xg(21);
3607
3608 double t4 = ((double) q13 * (double) q13);
3609 double t7 = ((double) q1 * (double) q1);
3610 double t8 = ((double) q14 * (double) q14);
3611 double t11 = ((double) q2 * (double) q2);
3612 double t12 = ((double) q15 * (double) q15);
3613 double t15 = ((double) q3 * (double) q3);
3614 double t17 = sqrt((double) (t4 - 2 * q13 * q1 + t7 + t8 - 2 * q14 * q2 + t11 + t12 - 2 * q15 * q3 + t15));
3615 double t18 = 0.1e1 / t17;
3616 double t19 = q13 - q1;
3617 double t20 = t18 * (double) t19;
3618 double t23 = t19 * t19;
3619 double t24 = q14 - q2;
3620 double t25 = t24 * t24;
3621 double t26 = q15 - q3;
3622 double t27 = t26 * t26;
3623 double t37 = 0.1e1 / (double) (t23 + t25 + t27) * ((double) ((q7 + q19) * t19) / 0.2e1 + (double) ((q8 + q20) * t24) / 0.2e1 + (double) ((q9 + q21) * t26) / 0.2e1);
3624 double t39 = (double) q7 / 0.2e1 + (double) q19 / 0.2e1 - t37 * (double) t19;
3625 double t40 = t39 * t39;
3626 double t44 = (double) q8 / 0.2e1 + (double) q20 / 0.2e1 - t37 * (double) t24;
3627 double t45 = t44 * t44;
3628 double t49 = (double) q9 / 0.2e1 + (double) q21 / 0.2e1 - t37 * (double) t26;
3629 double t50 = t49 * t49;
3630 double t52 = sqrt(t40 + t45 + t50);
3631 double t53 = 0.1e1 / t52;
3632 double t54 = t53 * t39;
3633 double t55 = t18 * (double) t24;
3634 double t56 = t53 * t49;
3635 double t58 = t18 * (double) t26;
3636 double t59 = t53 * t44;
3637 Aref(1,1) = t20;
3638 Aref(1,2) = t54;
3639 Aref(1,3) = t55 * t56 - t58 * t59;
3640 Aref(2,1) = t55;
3641 Aref(2,2) = t59;
3642 Aref(2,3) = t58 * t54 - t20 * t56;
3643 Aref(3,1) = t58;
3644 Aref(3,2) = t56;
3645 Aref(3,3) = t20 * t59 - t55 * t54;
3646 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
3647
3648 }
3649
ComputeCorotationalFrameD(Vector3D & pref,Matrix3D & Aref)3650 void ANCFBeam3D::ComputeCorotationalFrameD(Vector3D& pref, Matrix3D& Aref)
3651 {
3652
3653 xgd.SetLen(SOS());
3654 GetDrawCoordinates(xgd);
3655 xgd = T*xgd;
3656
3657 pref = 0.5*Vector3D(xgd(13)+xgd(1), xgd(14)+xgd(2), xgd(15)+xgd(3));
3658
3659 /*
3660 Vector3D x1(xgd(13)-xgd(1), xgd(14)-xgd(2), xgd(15)-xgd(3));
3661 Vector3D y1(xgd(7), xgd(8), xgd(9));
3662 Vector3D z1(xgd(10),xgd(11),xgd(12));
3663
3664 x1.Normalize();
3665 Vector3D y1a = y1;
3666 Vector3D z1a = z1;
3667 x1.GramSchmidt(y1);
3668 y1.Normalize();
3669 z1 = x1.Cross(y1);
3670
3671 Aref = Matrix3D(
3672 x1.X(),y1.X(),z1.X(),
3673 x1.Y(),y1.Y(),z1.Y(),
3674 x1.Z(),y1.Z(),z1.Z());
3675 */
3676
3677
3678 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
3679 double q1 = xgd(1); double q2 = xgd(2); double q3 = xgd(3);
3680 double q7 = xgd(7); double q8 = xgd(8); double q9 = xgd(9);
3681 double q13 = xgd(13); double q14 = xgd(14); double q15 = xgd(15);
3682 double q19 = xgd(19); double q20 = xgd(20); double q21 = xgd(21);
3683
3684
3685 double t4 = ((double) q13 * (double) q13);
3686 double t7 = ((double) q1 * (double) q1);
3687 double t8 = ((double) q14 * (double) q14);
3688 double t11 = ((double) q2 * (double) q2);
3689 double t12 = ((double) q15 * (double) q15);
3690 double t15 = ((double) q3 * (double) q3);
3691 double t17 = sqrt((double) (t4 - 2 * q13 * q1 + t7 + t8 - 2 * q14 * q2 + t11 + t12 - 2 * q15 * q3 + t15));
3692 double t18 = 0.1e1 / t17;
3693 double t19 = q13 - q1;
3694 double t20 = t18 * (double) t19;
3695 double t23 = t19 * t19;
3696 double t24 = q14 - q2;
3697 double t25 = t24 * t24;
3698 double t26 = q15 - q3;
3699 double t27 = t26 * t26;
3700 double t37 = 0.1e1 / (double) (t23 + t25 + t27) * ((double) ((q7 + q19) * t19) / 0.2e1 + (double) ((q8 + q20) * t24) / 0.2e1 + (double) ((q9 + q21) * t26) / 0.2e1);
3701 double t39 = (double) q7 / 0.2e1 + (double) q19 / 0.2e1 - t37 * (double) t19;
3702 double t40 = t39 * t39;
3703 double t44 = (double) q8 / 0.2e1 + (double) q20 / 0.2e1 - t37 * (double) t24;
3704 double t45 = t44 * t44;
3705 double t49 = (double) q9 / 0.2e1 + (double) q21 / 0.2e1 - t37 * (double) t26;
3706 double t50 = t49 * t49;
3707 double t52 = sqrt(t40 + t45 + t50);
3708 double t53 = 0.1e1 / t52;
3709 double t54 = t53 * t39;
3710 double t55 = t18 * (double) t24;
3711 double t56 = t53 * t49;
3712 double t58 = t18 * (double) t26;
3713 double t59 = t53 * t44;
3714 Aref(1,1) = t20;
3715 Aref(1,2) = t54;
3716 Aref(1,3) = t55 * t56 - t58 * t59;
3717 Aref(2,1) = t55;
3718 Aref(2,2) = t59;
3719 Aref(2,3) = t58 * t54 - t20 * t56;
3720 Aref(3,1) = t58;
3721 Aref(3,2) = t56;
3722 Aref(3,3) = t20 * t59 - t55 * t54;
3723
3724
3725 }
3726
ComputeCorotationalFrameDAvdq(Vector3D & v,Matrix & dAvdq)3727 void ANCFBeam3D::ComputeCorotationalFrameDAvdq(Vector3D& v, Matrix& dAvdq)
3728 {
3729
3730 }
3731
3732
ComputeCorotationalFrameDATvdq(Vector3D & v,Matrix & dATvdq)3733 void ANCFBeam3D::ComputeCorotationalFrameDATvdq(Vector3D& v, Matrix& dATvdq)
3734 {
3735 xg.SetLen(SOS());
3736 GetCoordinates(xg);
3737 ApplyT(xg);
3738
3739 dATvdq.SetSize(3,24);
3740 double v1 = v.X();
3741 double v2 = v.Y();
3742 double v3 = v.Z();
3743 double q1 = xg(1); double q2 = xg(2); double q3 = xg(3);
3744 double q7 = xg(7); double q8 = xg(8); double q9 = xg(9);
3745 double q13 = xg(13); double q14 = xg(14); double q15 = xg(15);
3746 double q19 = xg(19); double q20 = xg(20); double q21 = xg(21);
3747
3748 double t4 = ( q13 * q13);
3749 double t7 = ( q1 * q1);
3750 double t8 = ( q14 * q14);
3751 double t11 = ( q2 * q2);
3752 double t12 = ( q15 * q15);
3753 double t15 = ( q3 * q3);
3754 double t16 = t4 - 2 * q13 * q1 + t7 + t8 - 2 * q14 * q2 + t11 + t12 - 2 * q15 * q3 + t15;
3755 double t17 = sqrt( t16);
3756 double t19 = 0.1e1 / t17 / t16;
3757 double t20 = q13 - q1;
3758 double t21 = t19 * t20;
3759 double t25 = 0.1e1 / t17;
3760 double t26 = t25 * v1;
3761 double t27 = q14 - q2;
3762 double t28 = t19 * t27;
3763 double t32 = q15 - q3;
3764 double t33 = t19 * t32;
3765 double t44 = t25 * v2;
3766 double t58 = t25 * v3;
3767 double t92 = t20 * t20;
3768 double t93 = t27 * t27;
3769 double t94 = t32 * t32;
3770 double t95 = t92 + t93 + t94;
3771 double t96 = 1 / t95;
3772 double t97 = (q7 + q19);
3773 double t99 = (q8 + q20);
3774 double t101 = (q9 + q21);
3775 double t103 = (t97 * t20) / 0.2e1 + (t99 * t27) / 0.2e1 + (t101 * t32) / 0.2e1;
3776 double t104 = t96 * t103;
3777 double t106 = q7 / 0.2e1 + q19 / 0.2e1 - t104 * t20;
3778 double t107 = t106 * t106;
3779 double t111 = q8 / 0.2e1 + q20 / 0.2e1 - t104 * t27;
3780 double t112 = t111 * t111;
3781 double t116 = q9 / 0.2e1 + q21 / 0.2e1 - t104 * t32;
3782 double t117 = t116 * t116;
3783 double t118 = t107 + t112 + t117;
3784 double t119 = sqrt(t118);
3785 double t121 = 0.1e1 / t119 / t118;
3786 double t122 = t121 * t106;
3787 double t123 = t95 * t95;
3788 double t125 = 0.1e1 / t123 * t103;
3789 double t128 = - (t96 * t97) / 0.2e1;
3790 double t130 = -0.2e1 * t125 * (t20 * t20) - t128 * t20 + t104;
3791 double t133 = -0.2e1 * t125 * t27 * t20;
3792 double t135 = t133 - t128 * t27;
3793 double t138 = -0.2e1 * t125 * t32 * t20;
3794 double t140 = t138 - t128 * t32;
3795 double t142 = t106 * t130 + t111 * t135 + t116 * t140;
3796 double t146 = 0.1e1 / t119;
3797 double t147 = t146 * t130;
3798 double t149 = t121 * t111;
3799 double t153 = t146 * t135;
3800 double t155 = t121 * t116;
3801 double t159 = t146 * t140;
3802 double t162 = - (t96 * t99) / 0.2e1;
3803 double t164 = t133 - t162 * t20;
3804 double t169 = -0.2e1 * t125 * (t27 * t27) - t162 * t27 + t104;
3805 double t172 = -0.2e1 * t125 * t32 * t27;
3806 double t174 = t172 - t162 * t32;
3807 double t176 = t106 * t164 + t111 * t169 + t116 * t174;
3808 double t180 = t146 * t164;
3809 double t185 = t146 * t169;
3810 double t190 = t146 * t174;
3811 double t193 = - (t96 * t101) / 0.2e1;
3812 double t195 = t138 - t193 * t20;
3813 double t198 = t172 - t193 * t27;
3814 double t203 = -0.2e1 * t125 * (t32 * t32) - t193 * t32 + t104;
3815 double t205 = t106 * t195 + t111 * t198 + t116 * t203;
3816 double t209 = t146 * t195;
3817 double t214 = t146 * t198;
3818 double t219 = t146 * t203;
3819 double t222 = (t96 * t20) / 0.2e1;
3820 double t224 = 0.1e1 / 0.2e1 - t222 * t20;
3821 double t226 = t111 * t96;
3822 double t227 = (t27 * t20) / 0.2e1;
3823 double t229 = t116 * t96;
3824 double t230 = (t32 * t20) / 0.2e1;
3825 double t232 = t106 * t224 - t226 * t227 - t229 * t230;
3826 double t236 = t146 * t224;
3827 double t241 = t146 * t96;
3828 double t249 = -t122 * v1 * t232 + t236 * v1 - t149 * v2 * t232 - t241 * t227 * v2 - t155 * v3 * t232 - t241 * t230 * v3;
3829 double t250 = t106 * t96;
3830 double t252 = (t96 * t27) / 0.2e1;
3831 double t254 = 0.1e1 / 0.2e1 - t252 * t27;
3832 double t256 = (t32 * t27) / 0.2e1;
3833 double t258 = -t250 * t227 + t111 * t254 - t229 * t256;
3834 double t267 = t146 * t254;
3835 double t274 = -t122 * v1 * t258 - t241 * t227 * v1 - t149 * v2 * t258 + t267 * v2 - t155 * v3 * t258 - t241 * t256 * v3;
3836 double t277 = (t96 * t32) / 0.2e1;
3837 double t279 = 0.1e1 / 0.2e1 - t277 * t32;
3838 double t281 = -t250 * t230 - t226 * t256 + t116 * t279;
3839 double t295 = t146 * t279;
3840 double t297 = -t122 * v1 * t281 - t241 * t230 * v1 - t149 * v2 * t281 - t241 * t256 * v2 - t155 * v3 * t281 + t295 * v3;
3841 double t300 = (t96 * t97) / 0.2e1;
3842 double t302 = 0.2e1 * t125 * (t20 * t20) - t300 * t20 - t104;
3843 double t305 = 0.2e1 * t125 * t27 * t20;
3844 double t307 = t305 - t300 * t27;
3845 double t310 = 0.2e1 * t125 * t32 * t20;
3846 double t312 = t310 - t300 * t32;
3847 double t314 = t106 * t302 + t111 * t307 + t116 * t312;
3848 double t318 = t146 * t302;
3849 double t323 = t146 * t307;
3850 double t328 = t146 * t312;
3851 double t331 = (t96 * t99) / 0.2e1;
3852 double t333 = t305 - t331 * t20;
3853 double t338 = 0.2e1 * t125 * (t27 * t27) - t331 * t27 - t104;
3854 double t341 = 0.2e1 * t125 * t32 * t27;
3855 double t343 = t341 - t331 * t32;
3856 double t345 = t106 * t333 + t111 * t338 + t116 * t343;
3857 double t349 = t146 * t333;
3858 double t354 = t146 * t338;
3859 double t359 = t146 * t343;
3860 double t362 = (t96 * t101) / 0.2e1;
3861 double t364 = t310 - t362 * t20;
3862 double t367 = t341 - t362 * t27;
3863 double t372 = 0.2e1 * t125 * (t32 * t32) - t362 * t32 - t104;
3864 double t374 = t106 * t364 + t111 * t367 + t116 * t372;
3865 double t378 = t146 * t364;
3866 double t383 = t146 * t367;
3867 double t388 = t146 * t372;
3868 double t391 = t146 * t116;
3869 double t392 = -0.2e1 * t391 * t20;
3870 double t395 = t25 * t27;
3871 double t396 = 0.2e1 * t155 * t142;
3872 double t400 = t146 * t111;
3873 double t401 = -0.2e1 * t400 * t20;
3874 double t404 = t25 * t32;
3875 double t405 = 0.2e1 * t149 * t142;
3876 double t411 = t146 * t106;
3877 double t412 = -0.2e1 * t411 * t20;
3878 double t415 = 0.2e1 * t122 * t142;
3879 double t421 = t25 * t146;
3880 double t422 = t421 * t116;
3881 double t423 = t25 * t20;
3882 double t431 = t421 * t111;
3883 double t443 = -0.2e1 * t391 * t27;
3884 double t446 = 0.2e1 * t155 * t176;
3885 double t450 = -0.2e1 * t400 * t27;
3886 double t453 = 0.2e1 * t149 * t176;
3887 double t459 = -0.2e1 * t411 * t27;
3888 double t462 = 0.2e1 * t122 * t176;
3889 double t480 = t421 * t106;
3890 double t487 = -0.2e1 * t391 * t32;
3891 double t490 = 0.2e1 * t155 * t205;
3892 double t494 = -0.2e1 * t400 * t32;
3893 double t497 = 0.2e1 * t149 * t205;
3894 double t503 = -0.2e1 * t411 * t32;
3895 double t506 = 0.2e1 * t122 * t205;
3896 double t530 = 0.2e1 * t155 * t232;
3897 double t532 = 0.2e1 * t149 * t232;
3898 double t536 = 0.2e1 * t122 * t232;
3899 double t542 = t423 * t146;
3900 double t556 = (-t395 * t530 + t404 * t532) * v1 / 0.2e1 + (-t404 * t536 / 0.2e1 + t404 * t236 + t423 * t530 / 0.2e1 + t542 * t222 * t32) * v2 + (-t423 * t532 / 0.2e1 - t542 * t222 * t27 + t395 * t536 / 0.2e1 - t395 * t236) * v3;
3901 double t557 = 0.2e1 * t155 * t258;
3902 double t560 = t395 * t146;
3903 double t563 = 0.2e1 * t149 * t258;
3904 double t569 = 0.2e1 * t122 * t258;
3905 double t583 = (-t395 * t557 / 0.2e1 - t560 * t252 * t32 + t404 * t563 / 0.2e1 - t404 * t267) * v1 + (-t404 * t569 + t423 * t557) * v2 / 0.2e1 + (-t423 * t563 / 0.2e1 + t423 * t267 + t395 * t569 / 0.2e1 + t560 * t252 * t20) * v3;
3906 double t584 = 0.2e1 * t155 * t281;
3907 double t588 = 0.2e1 * t149 * t281;
3908 double t591 = t404 * t146;
3909 double t596 = 0.2e1 * t122 * t281;
3910 double t610 = (-t395 * t584 / 0.2e1 + t395 * t295 + t404 * t588 / 0.2e1 + t591 * t277 * t27) * v1 + (-t404 * t596 / 0.2e1 - t591 * t277 * t20 + t423 * t584 / 0.2e1 - t423 * t295) * v2 + (-t423 * t588 + t395 * t596) * v3 / 0.2e1;
3911 double t611 = 0.2e1 * t391 * t20;
3912 double t614 = 0.2e1 * t155 * t314;
3913 double t618 = 0.2e1 * t400 * t20;
3914 double t621 = 0.2e1 * t149 * t314;
3915 double t627 = 0.2e1 * t411 * t20;
3916 double t630 = 0.2e1 * t122 * t314;
3917 double t654 = 0.2e1 * t391 * t27;
3918 double t657 = 0.2e1 * t155 * t345;
3919 double t661 = 0.2e1 * t400 * t27;
3920 double t664 = 0.2e1 * t149 * t345;
3921 double t670 = 0.2e1 * t411 * t27;
3922 double t673 = 0.2e1 * t122 * t345;
3923 double t697 = 0.2e1 * t391 * t32;
3924 double t700 = 0.2e1 * t155 * t374;
3925 double t704 = 0.2e1 * t400 * t32;
3926 double t707 = 0.2e1 * t149 * t374;
3927 double t713 = 0.2e1 * t411 * t32;
3928 double t716 = 0.2e1 * t122 * t374;
3929 dATvdq(0+1,1+0 ) = t21 * v1 * t20 - t26 + t28 * v2 * t20 + t33 * v3 * t20;
3930 dATvdq(0+1,1+1 ) = t21 * v1 * t27 + t28 * v2 * t27 - t44 + t33 * v3 * t27;
3931 dATvdq(0+1,1+2 ) = t21 * v1 * t32 + t28 * v2 * t32 + t33 * v3 * t32 - t58;
3932 dATvdq(0+1,1+3 ) = 0.0e0;
3933 dATvdq(0+1,1+4 ) = 0.0e0;
3934 dATvdq(0+1,1+5 ) = 0.0e0;
3935 dATvdq(0+1,1+6 ) = 0.0e0;
3936 dATvdq(0+1,1+7 ) = 0.0e0;
3937 dATvdq(0+1,1+8 ) = 0.0e0;
3938 dATvdq(0+1,1+9 ) = 0.0e0;
3939 dATvdq(0+1,1+10) = 0.0e0;
3940 dATvdq(0+1,1+11) = 0.0e0;
3941 dATvdq(0+1,1+12) = -t21 * v1 * t20 + t26 - t28 * v2 * t20 - t33 * v3 * t20;
3942 dATvdq(0+1,1+13) = -t21 * v1 * t27 - t28 * v2 * t27 + t44 - t33 * v3 * t27;
3943 dATvdq(0+1,1+14) = -t21 * v1 * t32 - t28 * v2 * t32 - t33 * v3 * t32 + t58;
3944 dATvdq(0+1,1+15) = 0.0e0;
3945 dATvdq(0+1,1+16) = 0.0e0;
3946 dATvdq(0+1,1+17) = 0.0e0;
3947 dATvdq(0+1,1+18) = 0.0e0;
3948 dATvdq(0+1,1+19) = 0.0e0;
3949 dATvdq(0+1,1+20) = 0.0e0;
3950 dATvdq(0+1,1+21) = 0.0e0;
3951 dATvdq(0+1,1+22) = 0.0e0;
3952 dATvdq(0+1,1+23) = 0.0e0;
3953 dATvdq(1+1,1+0 ) = -t122 * v1 * t142 + t147 * v1 - t149 * v2 * t142 + t153 * v2 - t155 * v3 * t142 + t159 * v3;
3954 dATvdq(1+1,1+1 ) = -t122 * v1 * t176 + t180 * v1 - t149 * v2 * t176 + t185 * v2 - t155 * v3 * t176 + t190 * v3;
3955 dATvdq(1+1,1+2 ) = -t122 * v1 * t205 + t209 * v1 - t149 * v2 * t205 + t214 * v2 - t155 * v3 * t205 + t219 * v3;
3956 dATvdq(1+1,1+3 ) = 0.0e0;
3957 dATvdq(1+1,1+4 ) = 0.0e0;
3958 dATvdq(1+1,1+5 ) = 0.0e0;
3959 dATvdq(1+1,1+6 ) = t249;
3960 dATvdq(1+1,1+7 ) = t274;
3961 dATvdq(1+1,1+8 ) = t297;
3962 dATvdq(1+1,1+9 ) = 0.0e0;
3963 dATvdq(1+1,1+10) = 0.0e0;
3964 dATvdq(1+1,1+11) = 0.0e0;
3965 dATvdq(1+1,1+12) = -t122 * v1 * t314 + t318 * v1 - t149 * v2 * t314 + t323 * v2 - t155 * v3 * t314 + t328 * v3;
3966 dATvdq(1+1,1+13) = -t122 * v1 * t345 + t349 * v1 - t149 * v2 * t345 + t354 * v2 - t155 * v3 * t345 + t359 * v3;
3967 dATvdq(1+1,1+14) = -t122 * v1 * t374 + t378 * v1 - t149 * v2 * t374 + t383 * v2 - t155 * v3 * t374 + t388 * v3;
3968 dATvdq(1+1,1+15) = 0.0e0;
3969 dATvdq(1+1,1+16) = 0.0e0;
3970 dATvdq(1+1,1+17) = 0.0e0;
3971 dATvdq(1+1,1+18) = t249;
3972 dATvdq(1+1,1+19) = t274;
3973 dATvdq(1+1,1+20) = t297;
3974 dATvdq(1+1,1+21) = 0.0e0;
3975 dATvdq(1+1,1+22) = 0.0e0;
3976 dATvdq(1+1,1+23) = 0.0e0;
3977 dATvdq(2+1,1+0 ) = (-t28 * t392 / 0.2e1 - t395 * t396 / 0.2e1 + t395 * t159 + t33 * t401 / 0.2e1 + t404 * t405 / 0.2e1 - t404 * t153) * v1 + (-t33 * t412 / 0.2e1 - t404 * t415 / 0.2e1 + t404 * t147 + t21 * t392 / 0.2e1 + t422 + t423 * t396 / 0.2e1 - t423 * t159) * v2 + (-t21 * t401 / 0.2e1 - t431 - t423 * t405 / 0.2e1 + t423 * t153 + t28 * t412 / 0.2e1 + t395 * t415 / 0.2e1 - t395 * t147) * v3;
3978 dATvdq(2+1,1+1 ) = (-t28 * t443 / 0.2e1 - t422 - t395 * t446 / 0.2e1 + t395 * t190 + t33 * t450 / 0.2e1 + t404 * t453 / 0.2e1 - t404 * t185) * v1 + (-t33 * t459 / 0.2e1 - t404 * t462 / 0.2e1 + t404 * t180 + t21 * t443 / 0.2e1 + t423 * t446 / 0.2e1 - t423 * t190) * v2 + (-t21 * t450 / 0.2e1 - t423 * t453 / 0.2e1 + t423 * t185 + t28 * t459 / 0.2e1 + t480 + t395 * t462 / 0.2e1 - t395 * t180) * v3;
3979 dATvdq(2+1,1+2 ) = (-t28 * t487 / 0.2e1 - t395 * t490 / 0.2e1 + t395 * t219 + t33 * t494 / 0.2e1 + t431 + t404 * t497 / 0.2e1 - t404 * t214) * v1 + (-t33 * t503 / 0.2e1 - t480 - t404 * t506 / 0.2e1 + t404 * t209 + t21 * t487 / 0.2e1 + t423 * t490 / 0.2e1 - t423 * t219) * v2 + (-t21 * t494 / 0.2e1 - t423 * t497 / 0.2e1 + t423 * t214 + t28 * t503 / 0.2e1 + t395 * t506 / 0.2e1 - t395 * t209) * v3;
3980 dATvdq(2+1,1+3 ) = 0.0e0;
3981 dATvdq(2+1,1+4 ) = 0.0e0;
3982 dATvdq(2+1,1+5 ) = 0.0e0;
3983 dATvdq(2+1,1+6 ) = t556;
3984 dATvdq(2+1,1+7 ) = t583;
3985 dATvdq(2+1,1+8 ) = t610;
3986 dATvdq(2+1,1+9 ) = 0.0e0;
3987 dATvdq(2+1,1+10) = 0.0e0;
3988 dATvdq(2+1,1+11) = 0.0e0;
3989 dATvdq(2+1,1+12) = (-t28 * t611 / 0.2e1 - t395 * t614 / 0.2e1 + t395 * t328 + t33 * t618 / 0.2e1 + t404 * t621 / 0.2e1 - t404 * t323) * v1 + (-t33 * t627 / 0.2e1 - t404 * t630 / 0.2e1 + t404 * t318 + t21 * t611 / 0.2e1 - t422 + t423 * t614 / 0.2e1 - t423 * t328) * v2 + (-t21 * t618 / 0.2e1 + t431 - t423 * t621 / 0.2e1 + t423 * t323 + t28 * t627 / 0.2e1 + t395 * t630 / 0.2e1 - t395 * t318) * v3;
3990 dATvdq(2+1,1+13) = (-t28 * t654 / 0.2e1 + t422 - t395 * t657 / 0.2e1 + t395 * t359 + t33 * t661 / 0.2e1 + t404 * t664 / 0.2e1 - t404 * t354) * v1 + (-t33 * t670 / 0.2e1 - t404 * t673 / 0.2e1 + t404 * t349 + t21 * t654 / 0.2e1 + t423 * t657 / 0.2e1 - t423 * t359) * v2 + (-t21 * t661 / 0.2e1 - t423 * t664 / 0.2e1 + t423 * t354 + t28 * t670 / 0.2e1 - t480 + t395 * t673 / 0.2e1 - t395 * t349) * v3;
3991 dATvdq(2+1,1+14) = (-t28 * t697 / 0.2e1 - t395 * t700 / 0.2e1 + t395 * t388 + t33 * t704 / 0.2e1 - t431 + t404 * t707 / 0.2e1 - t404 * t383) * v1 + (-t33 * t713 / 0.2e1 + t480 - t404 * t716 / 0.2e1 + t404 * t378 + t21 * t697 / 0.2e1 + t423 * t700 / 0.2e1 - t423 * t388) * v2 + (-t21 * t704 / 0.2e1 - t423 * t707 / 0.2e1 + t423 * t383 + t28 * t713 / 0.2e1 + t395 * t716 / 0.2e1 - t395 * t378) * v3;
3992 dATvdq(2+1,1+15) = 0.0e0;
3993 dATvdq(2+1,1+16) = 0.0e0;
3994 dATvdq(2+1,1+17) = 0.0e0;
3995 dATvdq(2+1,1+18) = t556;
3996 dATvdq(2+1,1+19) = t583;
3997 dATvdq(2+1,1+20) = t610;
3998 dATvdq(2+1,1+21) = 0.0e0;
3999 dATvdq(2+1,1+22) = 0.0e0;
4000 dATvdq(2+1,1+23) = 0.0e0;
4001 }
4002
4003
DrawElement()4004 void ANCFBeam3D::DrawElement()
4005 {
4006 mbs->SetColor(col);
4007
4008 //mbs->uout << "ANCFBeam3D: m=" << mass << ", I=" << Iphi << ", Size=" << size << "\n";
4009 //GetMBS()->UO() << "r_posD=" << GetPosD(Vector3D(1,1,1)) << "\n";
4010 //char str[100];
4011 //Vector3D p0 = GetPosD(Vector3D(-1,0,0));
4012 //sprintf(str," posx=%6.3g,posy=%6.3g,posz=%6.3g ", p0.X(), p0.Y(), p0.Z());
4013 //GetMBS()->GetRC()->PrintTextStruct(5,-1,str);
4014
4015 //double lx = -0.5*size.X(); double ly = 0.5*size.Y(); double lz = 0.5*size.Z();
4016 double lx1 = 1; double ly1 = 1*GetMBS()->GetMagnifyYZ(); double lz1 = 1*GetMBS()->GetMagnifyYZ();
4017
4018 double def_scale = GetMBS()->GetDOption(105); //deformation scaling
4019
4020
4021 if(0) //draw underlying rigid body configuration:
4022 {
4023 Vector3D pmid;
4024 Matrix3D Rot;
4025 ComputeCorotationalFrameD(pmid, Rot);
4026 Vector3D pmid0(x_init(13)+x_init(1),x_init(14)+x_init(2),x_init(15)+x_init(3));
4027 pmid0*=0.5;
4028
4029 //Matrix3D A;
4030 //A = GetRotMatrixD(Vector3D(0.,0.,0.));
4031
4032
4033 Vector3D p[10];
4034 double l1 = -0.5*lx;
4035 double l2 = 0.5*lx;
4036 double thickness = 1;
4037 double ly1 = 0.5*ly;
4038 double lz1 = 0.5*lz;
4039
4040 p[8] = Vector3D(l1,-ly1,-lz1);
4041 p[7] = Vector3D(l1,-ly1, lz1);
4042 p[4] = Vector3D(l1, ly1,-lz1);
4043 p[3] = Vector3D(l1, ly1, lz1);
4044 p[6] = Vector3D(l2,-ly1,-lz1);
4045 p[5] = Vector3D(l2,-ly1, lz1);
4046 p[2] = Vector3D(l2, ly1,-lz1);
4047 p[1] = Vector3D(l2, ly1, lz1);
4048
4049 for (int i=1; i <= 8; i++) p[i] = def_scale*(pmid-pmid0)+pmid0 + (def_scale*(Rot-Matrix3D(1.))+Matrix3D(1.))*p[i];
4050
4051 mbs->MyDrawLine(p[8],p[7],thickness);
4052 mbs->MyDrawLine(p[7],p[3],thickness);
4053 mbs->MyDrawLine(p[4],p[3],thickness);
4054 mbs->MyDrawLine(p[4],p[8],thickness);
4055 mbs->MyDrawLine(p[6],p[5],thickness);
4056 mbs->MyDrawLine(p[5],p[1],thickness);
4057 mbs->MyDrawLine(p[2],p[1],thickness);
4058 mbs->MyDrawLine(p[2],p[6],thickness);
4059 mbs->MyDrawLine(p[6],p[8],thickness);
4060 mbs->MyDrawLine(p[5],p[7],thickness);
4061 mbs->MyDrawLine(p[2],p[4],thickness);
4062 mbs->MyDrawLine(p[1],p[3],thickness);
4063
4064 }
4065
4066 int linemode = 1; //0=no lines, 1=outline+color, 2=outline, 3=elementline+color, 4=elementline
4067 if (GetMBS()->GetIOption(110) && !GetMBS()->GetIOption(111))
4068 {
4069 linemode = 2;
4070 }
4071 if (!GetMBS()->GetIOption(110) && GetMBS()->GetIOption(111))
4072 {
4073 linemode = 0;
4074 }
4075 if (!GetMBS()->GetIOption(110) && !GetMBS()->GetIOption(111))
4076 {
4077 linemode = 4;
4078 }
4079
4080 int colormode = 0;
4081 if (GetMBS()->GetActualPostProcessingFieldVariable() != NULL) colormode = 1;
4082 else
4083 {
4084 colormode = 0;
4085 }
4086
4087 if (colormode)
4088 {
4089
4090 //draw modes:
4091 //for (int ii = 1; ii <= 2; ii++)
4092 {
4093 double modeval = 0;
4094 int xgset = 0;
4095
4096 double tilex = GetMBS()->GetIOption(137);
4097 double tiley = GetMBS()->GetIOption(138);
4098 //if (!colormode) {tiley = 1; tilex = 20;}
4099 TArray<Vector3D> points((int)(tilex+1)*(int)(tiley+1));
4100 TArray<double> vals((int)(tilex+1)*(int)(tiley+1));
4101 double v=0;
4102
4103 for (int side=1; side <= 6; side++)
4104 {
4105 points.SetLen(0); vals.SetLen(0);
4106 Vector3D p0, vx, vy;
4107 int tileyn = (int)tiley;
4108 int tilexn = (int)tilex;
4109
4110 /*if (side >= 5)
4111 {
4112 tilexn = 2;
4113 tileyn = 2;
4114 }*/
4115
4116 if (side == 1)
4117 { //bottom
4118 p0 = Vector3D(-lx1,-ly1,-lz1);
4119 vx = Vector3D(2.*lx1/tilexn,0,0);
4120 vy = Vector3D(0,0,2.*lz1/tileyn);
4121 }
4122 else if (side == 2)
4123 { //top
4124 p0 = Vector3D(-lx1, ly1, lz1);
4125 vx = Vector3D(2.*lx1/tilexn,0,0);
4126 vy = Vector3D(0,0,-2.*lz1/tileyn);
4127 }
4128 else if (side == 3)
4129 { //front
4130 p0 = Vector3D(-lx1,-ly1, lz1);
4131 vx = Vector3D(2.*lx1/tilexn,0,0);
4132 vy = Vector3D(0,2.*ly1/tileyn,0);
4133 }
4134 else if (side == 4)
4135 { //back
4136 p0 = Vector3D(-lx1, ly1,-lz1);
4137 vx = Vector3D(2.*lx1/tilexn,0,0);
4138 vy = Vector3D(0,-2.*ly1/tileyn,0);
4139 }
4140 else if (side == 5)
4141 { //left
4142 p0 = Vector3D(-lx1, ly1,-lz1);
4143 vx = Vector3D(0,-2.*ly1/tilexn,0);
4144 vy = Vector3D(0,0,2.*lz1/tileyn);
4145 }
4146 else if (side == 6)
4147 { //right
4148 p0 = Vector3D( lx1,-ly1,-lz1);
4149 vx = Vector3D(0,2.*ly1/tilexn,0);
4150 vy = Vector3D(0,0,2.*lz1/tileyn);
4151 }
4152
4153 for (double iy = 0; iy <= tileyn+1e-10; iy++)
4154 {
4155 for (double ix = 0; ix <= tilexn+1e-10; ix++)
4156 {
4157 Vector3D ploc = (p0+ix*vx+iy*vy);
4158 Vector3D pg = GetPos0D(ploc, def_scale );
4159 Vector3D p(pg);
4160 points.Add(p);
4161 if (colormode)
4162 v = GetFieldVariableValue(*GetMBS()->GetActualPostProcessingFieldVariable(), ploc, true);
4163 vals.Add(v);
4164 }
4165 }
4166 mbs->DrawColorQuads(points,vals,(int)tilexn+1,(int)tileyn+1,colormode,linemode);
4167 }
4168
4169 /*
4170 if (ii == 2)
4171 {
4172 int nmode = GetMBS()->GetDrawResolution()+1;
4173 if (nmode >= 1 && nmode <= 12)
4174 {
4175 XGD((nmode)) -= modeval;
4176 }
4177 }*/
4178
4179 }
4180
4181 }
4182 else
4183 {
4184 int drawgrid = 0;
4185 double thickness = 1;
4186 //double tiling = 20;
4187 double tiling = GetMBS()->GetIOption(136);
4188 mbs->SetDrawlines(0);
4189 mbs->SetDrawlinesH(0);
4190 //lx1*=0.9;ly1*=0.9;lz1*=0.9;
4191 Vector3D p1,p2,p3,p4,p5,p6,p7,p8;
4192 for (double i = 0; i < tiling; i++)
4193 {
4194 double l1 = -lx1+2*lx1*i/tiling;
4195 double l2 = -lx1+2*lx1*(i+1)/tiling;
4196 if (i == 0)
4197 {
4198 p8 = Vector3D(GetPos0D(Vector3D(l1,-ly1,-lz1)));
4199 p7 = Vector3D(GetPos0D(Vector3D(l1,-ly1, lz1)));
4200 p4 = Vector3D(GetPos0D(Vector3D(l1, ly1,-lz1)));
4201 p3 = Vector3D(GetPos0D(Vector3D(l1, ly1, lz1)));
4202 if (drawgrid)
4203 {
4204 mbs->MyDrawLine(p8,p7,thickness);
4205 mbs->MyDrawLine(p7,p3,thickness);
4206 mbs->MyDrawLine(p4,p3,thickness);
4207 mbs->MyDrawLine(p4,p8,thickness);
4208 }
4209 }
4210 else
4211 {
4212 p8 = p6;
4213 p7 = p5;
4214 p4 = p2;
4215 p3 = p1;
4216 }
4217 p6 = Vector3D(GetPos0D(Vector3D(l2,-ly1,-lz1)));
4218 p5 = Vector3D(GetPos0D(Vector3D(l2,-ly1, lz1)));
4219 p2 = Vector3D(GetPos0D(Vector3D(l2, ly1,-lz1)));
4220 p1 = Vector3D(GetPos0D(Vector3D(l2, ly1, lz1)));
4221 int dout = 0;
4222 if (i == 0 || i == tiling-1) dout = 1;
4223 if (drawgrid)
4224 {
4225 mbs->MyDrawLine(p6,p5,thickness);
4226 mbs->MyDrawLine(p5,p1,thickness);
4227 mbs->MyDrawLine(p2,p1,thickness);
4228 mbs->MyDrawLine(p2,p6,thickness);
4229 mbs->MyDrawLine(p6,p8,thickness);
4230 mbs->MyDrawLine(p5,p7,thickness);
4231 mbs->MyDrawLine(p2,p4,thickness);
4232 mbs->MyDrawLine(p1,p3,thickness);
4233 }
4234 else
4235 {
4236 mbs->DrawHex(p1,p2,p3,p4,p5,p6,p7,p8,dout);
4237 }
4238 }
4239 mbs->SetDrawlines(0);
4240 }
4241
4242 if (concentratedmass1 != 0)
4243 {
4244 Vector3D mp1(GetPos0D(Vector3D(-lx1,0,0)));
4245 mbs->SetColor(colred);
4246 //double r = pow(3.*concentratedmass1/(4.*MY_PI*GetRho()),1./3.);
4247 double r = ly/10.; //$ DR 2013-02-04 deleted rho from class element, do not use it here!
4248 //double r = ly*GetMBS()->GetMagnifyYZ()*4.;
4249 mbs->DrawSphere(mp1,r,12);
4250 }
4251 if (concentratedmass2 != 0)
4252 {
4253 Vector3D mp1(GetPos0D(Vector3D(lx1,0,0)));
4254 mbs->SetColor(colred);
4255 //double r = pow(3.*concentratedmass2/(4.*MY_PI*GetRho()),1./3.);
4256 double r = ly/10.; //$ DR 2013-02-04 deleted rho from class element, do not use it here!
4257 //double r = ly*GetMBS()->GetMagnifyYZ()*4.;
4258 mbs->DrawSphere(mp1,r,12);
4259 }
4260 };
4261
4262
GetdPosdqT(const Vector3D & ploc,Matrix & d)4263 void ANCFBeam3D::GetdPosdqT(const Vector3D& ploc, Matrix& d)
4264 {
4265 //p = S(p.x,p.y,p.z)*q; d(p)/dq
4266 Vector3D p0=ploc;
4267 p0.Scale(0.5*lx,0.5*ly,0.5*lz);
4268 static Vector SV;
4269 GetS0(SV, p0);
4270 d.SetSize(NS()*Dim(),3);
4271 d.FillWithZeros();
4272 for (int i = 1; i <= NS(); i++)
4273 {
4274 d((i-1)*3+1,1)=SV(i);
4275 d((i-1)*3+2,2)=SV(i);
4276 d((i-1)*3+3,3)=SV(i);
4277 }
4278 ApplyTtp(d);
4279 }
4280
GetdRotvdqT(const Vector3D & vloc,const Vector3D & ploc,Matrix & d)4281 void ANCFBeam3D::GetdRotvdqT(const Vector3D& vloc, const Vector3D& ploc, Matrix& d)
4282 {
4283 d.SetSize(NS()*Dim(),3);
4284 /*
4285
4286 static Matrix d2;
4287 d2.SetSize(NS()*Dim(),3);
4288 double diffpar = 1e-8;
4289 GetdPosdqT(ploc+diffpar*vloc, d2);
4290 d = d2;
4291 GetdPosdqT(ploc,d2);
4292 d -= d2;
4293 d *= 1./diffpar;
4294 */
4295
4296 Vector3D p0(ploc);
4297 p0.Scale(0.5*lx,0.5*ly,0.5*lz);
4298
4299 static Vector xg;
4300 xg.SetLen(SOS());
4301 GetCoordinates(xg);
4302 ApplyT(xg);
4303
4304 if (usetangentframe) GetTangentFramedRotvdqT(vloc, p0.X(), xg, d);
4305 else GetCrosssectionFramedRotvdqT(vloc, p0.X(), xg, d);
4306
4307 ApplyTtp(d); //****????
4308 }
4309
GetdRotdqT(const Vector3D & ploc,Matrix & d)4310 void ANCFBeam3D::GetdRotdqT(const Vector3D& ploc, Matrix& d)
4311 {
4312 //UO() << "dRotdq\n";
4313
4314 Vector3D p0=ploc;
4315 p0.Scale(0.5*lx,0.5*ly,0.5*lz);
4316
4317 d.SetSize(NS()*Dim(),3);
4318 d.FillWithZeros(); //needed???
4319
4320 if (0) //old model: inconsistent with GetRotMatrix / TangentFrame !!!
4321 {
4322 static Vector Sy; Sy.SetLen(NS());
4323 static Vector Sz; Sz.SetLen(NS());
4324
4325 static Vector xg;
4326 xg.SetLen(SOS());
4327 GetCoordinates(xg);
4328 //ApplyT(xg);
4329
4330 Vector3D ry, rz;
4331
4332 GetS0y(Sy,p0);
4333 GetS0z(Sz,p0);
4334
4335 for (int i = 1; i <= NS(); i++)
4336 {
4337 ry.X() += Sy(i)*xg(3*(i-1)+1);
4338 ry.Y() += Sy(i)*xg(3*(i-1)+2);
4339 ry.Z() += Sy(i)*xg(3*(i-1)+3);
4340 rz.X() += Sz(i)*xg(3*(i-1)+1);
4341 rz.Y() += Sz(i)*xg(3*(i-1)+2);
4342 rz.Z() += Sz(i)*xg(3*(i-1)+3);
4343 }
4344
4345 double dyx = Sqr(ry.X())+Sqr(ry.Y());
4346 double dzx = Sqr(rz.X())+Sqr(rz.Z());
4347 double dyz = Sqr(ry.Y())+Sqr(ry.Z());
4348 double dzy = Sqr(rz.Y())+Sqr(rz.Z());
4349
4350 for (int i = 1; i <= NS(); i++)
4351 {
4352 //from delta gamma_x
4353 d((i-1)*3+1,1) = 0; //only delta r.X() terms!
4354 d((i-1)*3+2,1) =-(ry.Z()*Sy(i))/dyz - (rz.Z()*Sz(i))/dzy; //only delta r.Y() terms!
4355 d((i-1)*3+3,1) = (ry.Y()*Sy(i))/dyz + (rz.Y()*Sz(i))/dzy; //only delta r.Z() terms!
4356
4357 //from delta gamma_y
4358 d((i-1)*3+1,2) = (rz.Z()*Sz(i))/dzx; //only delta r.X() terms!
4359 d((i-1)*3+2,2) = 0; //only delta r.Y() terms!
4360 d((i-1)*3+3,2) =-(rz.X()*Sz(i))/dzx; //only delta r.Z() terms!
4361
4362 //from delta gamma_z
4363 d((i-1)*3+1,3) =-(ry.Y()*Sy(i))/dyx; //only delta r.X() terms!
4364 d((i-1)*3+2,3) = (ry.X()*Sy(i))/dyx; //only delta r.Y() terms!
4365 d((i-1)*3+3,3) = 0; //only delta r.Z() terms!
4366
4367 }
4368 //ApplyTtp(d);
4369 }
4370 else
4371 {
4372 //alternatively:
4373 ConstMatrix<72> dAvdq(24,3);
4374 double x = ploc.X();
4375
4376 Matrix3D AT = GetRotMatrix(ploc).GetTp();
4377
4378 Vector3D v;
4379 v = Vector3D(AT(1,1),AT(2,1),AT(3,1));
4380 GetdRotvdqT(v, ploc, dAvdq);
4381 for (int j=1; j <= SOS(); j++)
4382 {
4383 d(j,3) = dAvdq(j,2);
4384 d(j,2) =-dAvdq(j,3);
4385 }
4386 v = Vector3D(AT(1,2),AT(2,2),AT(3,2));
4387 GetdRotvdqT(v, ploc, dAvdq);
4388 for (int j=1; j <= SOS(); j++)
4389 {
4390 d(j,1) = dAvdq(j,3);
4391 }
4392 //UO() << "d_new=" << d << "\n";
4393 }
4394 }
4395
GetRotMatrix(const Vector3D & ploc) const4396 Matrix3D ANCFBeam3D::GetRotMatrix(const Vector3D& ploc) const
4397 {
4398 Vector3D p0(ploc);
4399 p0.Scale(0.5*lx,0.5*ly,0.5*lz);
4400 /*
4401 //Compute Gradient ...
4402 static Vector u;
4403 u.SetLen(SOS());
4404 GetCoordinates(u);
4405
4406 ApplyT(u);
4407 u -= e0;
4408 Matrix3D G;
4409 Gradu(p0, u, G);
4410 G += Matrix3D(1);
4411
4412 //use cross-section frame, use average of ry and rz for rotation
4413 Vector3D ry(G(1,2),G(2,2),G(3,2));
4414 Vector3D rz(G(1,3),G(2,3),G(3,3));
4415 ry.Normalize();
4416 rz.Normalize();
4417 Vector3D ry2=ry;
4418 Vector3D rz2=rz;
4419 ry.GramSchmidt(rz2);
4420 rz = 0.5*(rz+rz2);
4421 rz.GramSchmidt(ry);
4422 Vector3D rx = ry.Cross(rz);
4423
4424 Matrix3D A1(rx.X(),ry.X(),rz.X(),
4425 rx.Y(),ry.Y(),rz.Y(),
4426 rx.Z(),ry.Z(),rz.Z());
4427
4428 return A1;
4429 */
4430 Matrix3D A;
4431
4432 static Vector xg;
4433 xg.SetLen(SOS());
4434 GetCoordinates(xg);
4435 ApplyT(xg);
4436
4437 double x = p0.X();
4438
4439
4440 return ComputeTangentFrame(x, xg);
4441
4442 }
4443
GetRotMatrixP(const Vector3D & ploc) const4444 Matrix3D ANCFBeam3D::GetRotMatrixP(const Vector3D& ploc) const
4445 {
4446 /*
4447 //Compute Gradient ...
4448 static Vector u;
4449 u.SetLen(SOS());
4450 GetCoordinatesP(u);
4451
4452 ApplyT(u);
4453 Vector3D p0(ploc);
4454 p0.Scale(0.5*lx,0.5*ly,0.5*lz);
4455 Matrix3D G;
4456 Gradu(p0, u, G);
4457 return G;
4458 */
4459
4460 Vector3D p0(ploc);
4461 p0.Scale(0.5*lx,0.5*ly,0.5*lz);
4462
4463 static Vector xg;
4464 xg.SetLen(SOS());
4465 GetCoordinates(xg);
4466 ApplyT(xg);
4467
4468 static Vector xgp;
4469 xgp.SetLen(SOS());
4470 GetCoordinatesP(xgp);
4471 ApplyT(xgp);
4472
4473 double x = p0.X();
4474
4475 return ComputeTangentFrameP(x, xg, xgp);//JG: 2012 falsch, hier sollte cross section frame
4476 }
4477
GetRotMatrixD(const Vector3D & ploc) const4478 Matrix3D ANCFBeam3D::GetRotMatrixD(const Vector3D& ploc) const
4479 {
4480 Vector3D p0(ploc);
4481 p0.Scale(0.5*lx,0.5*ly,0.5*lz);
4482 /*
4483 //Compute Gradient ...
4484 static Vector u;
4485 u.SetLen(SOS());
4486 GetDrawCoordinates(u);
4487
4488 ApplyTD(u);
4489 u -= e0;
4490 Matrix3D G;
4491 GraduD(p0, u, G);
4492 G += Matrix3D(1);
4493
4494 //use cross-section frame, use average of ry and rz for rotation
4495 Vector3D ry(G(1,2),G(2,2),G(3,2));
4496 Vector3D rz(G(1,3),G(2,3),G(3,3));
4497 ry.Normalize();
4498 rz.Normalize();
4499
4500 Vector3D ry2=ry;
4501 Vector3D rz2=rz;
4502 ry.GramSchmidt(rz2);
4503 rz = 0.5*(rz+rz2);
4504 rz.GramSchmidt(ry);
4505 Vector3D rx = ry.Cross(rz);
4506
4507 Matrix3D A1(rx.X(),ry.X(),rz.X(),
4508 rx.Y(),ry.Y(),rz.Y(),
4509 rx.Z(),ry.Z(),rz.Z());
4510 */
4511
4512 //the following is the tangent frame
4513 //Reason: The rotation of the cross-section is only interpolated linearly
4514 Matrix3D A;
4515
4516 static Vector xgd;
4517 xgd.SetLen(SOS());
4518 GetCoordinates(xgd);
4519 ApplyT(xgd);
4520
4521 double q1 = xgd(1); double q2 = xgd(2); double q3 = xgd(3);
4522 double q4 = xgd(4); double q5 = xgd(5); double q6 = xgd(6);
4523 double q7 = xgd(7); double q8 = xgd(8); double q9 = xgd(9);
4524 double q10 = xgd(10); double q11 = xgd(11); double q12 = xgd(12);
4525 double q13 = xgd(13); double q14 = xgd(14); double q15 = xgd(15);
4526 double q16 = xgd(16); double q17 = xgd(17); double q18 = xgd(18);
4527 double q19 = xgd(19); double q20 = xgd(20); double q21 = xgd(21);
4528 double q22 = xgd(22); double q23 = xgd(23); double q24 = xgd(24);
4529
4530 double x = p0.X();
4531
4532 double t4 = 1 / lx;
4533 double t5 = (x * x);
4534 double t6 = -1 + t5;
4535 double t7 = 0.3e1 / 0.4e1 * (double) t4 * (double) t6;
4536 double t12 = -0.1e1 / 0.4e1 - x / 0.2e1 + 0.3e1 / 0.4e1 * (double) t5;
4537 double t14 = -0.3e1 / 0.4e1 * (double) t4 * (double) t6;
4538 double t17 = 0.1e1 + x;
4539 double t19 = -0.1e1 + x;
4540 double t22 = t17 * t17;
4541 double t26 = (double) t4 * (lx * t17 * t19 / 0.4e1 + lx * t22 / 0.8e1);
4542 double t29 = 0.2e1 * t7 * q1 + t12 * q4 + 0.2e1 * t14 * q13 + 0.2e1 * t26 * q16;
4543 double t30 = t29 * t29;
4544 double t38 = 0.2e1 * t7 * q2 + t12 * q5 + 0.2e1 * t14 * q14 + 0.2e1 * t26 * q17;
4545 double t39 = t38 * t38;
4546 double t47 = 0.2e1 * t7 * q3 + t12 * q6 + 0.2e1 * t14 * q15 + 0.2e1 * t26 * q18;
4547 double t48 = t47 * t47;
4548 double t49 = t30 + t39 + t48;
4549 double t50 = sqrt(t49);
4550 double t51 = 0.1e1 / t50;
4551 double t52 = t51 * t29;
4552 double t53 = -t19 * q7 / 0.2e1;
4553 double t54 = t17 * q19 / 0.2e1;
4554 double t55 = 0.1e1 / t49;
4555 double t58 = -t19 * q8 / 0.2e1;
4556 double t59 = t17 * q20 / 0.2e1;
4557 double t62 = -t19 * q9 / 0.2e1;
4558 double t63 = t17 * q21 / 0.2e1;
4559 double t67 = t55 * ((t53 + t54) * t29 + (t58 + t59) * t38 + (t62 + t63) * t47);
4560 double t69 = t53 + t54 - t67 * t29;
4561 double t70 = t69 * t69;
4562 double t72 = t58 + t59 - t67 * t38;
4563 double t73 = t72 * t72;
4564 double t75 = t62 + t63 - t67 * t47;
4565 double t76 = t75 * t75;
4566 double t78 = sqrt(t70 + t73 + t76);
4567 double t79 = 0.1e1 / t78;
4568 double t81 = t51 * t38;
4569 double t82 = -t19 * q10 / 0.2e1;
4570 double t83 = t17 * q22 / 0.2e1;
4571 double t86 = -t19 * q11 / 0.2e1;
4572 double t87 = t17 * q23 / 0.2e1;
4573 double t90 = -t19 * q12 / 0.2e1;
4574 double t91 = t17 * q24 / 0.2e1;
4575 double t95 = t55 * ((t82 + t83) * t29 + (t86 + t87) * t38 + (t90 + t91) * t47);
4576 double t97 = t82 + t83 - t95 * t29;
4577 double t98 = t97 * t97;
4578 double t100 = t86 + t87 - t95 * t38;
4579 double t101 = t100 * t100;
4580 double t103 = t90 + t91 - t95 * t47;
4581 double t104 = t103 * t103;
4582 double t106 = sqrt(t98 + t101 + t104);
4583 double t107 = 0.1e1 / t106;
4584 double t108 = t107 * t103;
4585 double t110 = t51 * t47;
4586 double t111 = t107 * t100;
4587 double t113 = t79 * t69 - t81 * t108 + t110 * t111;
4588 double t116 = t107 * t97;
4589 double t118 = t79 * t75 - t52 * t111 + t81 * t116;
4590 double t123 = t79 * t72 - t110 * t116 + t52 * t108;
4591 A(0+1,1+0) = t52;
4592 A(0+1,1+1) = t113 / 0.2e1;
4593 A(0+1,1+2) = t81 * t118 / 0.2e1 - t110 * t123 / 0.2e1;
4594 A(1+1,1+0) = t81;
4595 A(1+1,1+1) = t123 / 0.2e1;
4596 A(1+1,1+2) = t110 * t113 / 0.2e1 - t52 * t118 / 0.2e1;
4597 A(2+1,1+0) = t110;
4598 A(2+1,1+1) = t118 / 0.2e1;
4599 A(2+1,1+2) = t52 * t123 / 0.2e1 - t81 * t113 / 0.2e1;
4600
4601 //if (GetMBS()->GetTime() > 0.05)
4602 // global_uo << "A=" << A << ", A1=" << A1 << "\n";
4603
4604 return A;
4605
4606 }
4607
4608