1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2014 projectchrono.org
5 // All rights reserved.
6 //
7 // Use of this source code is governed by a BSD-style license that can be found
8 // in the LICENSE file at the top level of the distribution and at
9 // http://projectchrono.org/license-chrono.txt.
10 //
11 // =============================================================================
12 // Authors: Alessandro Tasora, Radu Serban
13 // =============================================================================
14
15 #include "chrono/core/ChMath.h"
16 #include "chrono/timestepper/ChAssemblyAnalysis.h"
17
18 namespace chrono {
19
ChAssemblyAnalysis(ChIntegrableIIorder & mintegrable)20 ChAssemblyAnalysis::ChAssemblyAnalysis(ChIntegrableIIorder& mintegrable) {
21 integrable = &mintegrable;
22 X.setZero(1, &mintegrable);
23 V.setZero(1, &mintegrable);
24 A.setZero(1, &mintegrable);
25 max_assembly_iters = 4;
26 }
27
AssemblyAnalysis(int action,double dt)28 void ChAssemblyAnalysis::AssemblyAnalysis(int action, double dt) {
29 ChVectorDynamic<> R;
30 ChVectorDynamic<> Qc;
31 double T;
32
33 // Set up main vectors
34 integrable->StateSetup(X, V, A);
35
36 if (action & AssemblyLevel::POSITION) {
37 ChStateDelta Dx;
38
39 for (int m_iter = 0; m_iter < max_assembly_iters; m_iter++) {
40 // Set up auxiliary vectors
41 Dx.setZero(integrable->GetNcoords_v(), GetIntegrable());
42 R.setZero(integrable->GetNcoords_v());
43 Qc.setZero(integrable->GetNconstr());
44 L.setZero(integrable->GetNconstr());
45
46 integrable->StateGather(X, V, T); // state <- system
47
48 // Solve:
49 //
50 // [M Cq' ] [ dx ] = [ 0]
51 // [ Cq 0 ] [ l ] = [ C]
52
53 integrable->LoadConstraint_C(Qc, 1.0);
54
55 integrable->StateSolveCorrection(
56 Dx, L, R, Qc,
57 1.0, // factor for M
58 0, // factor for dF/dv
59 0, // factor for dF/dx (the stiffness matrix)
60 X, V, T, // not needed
61 false, // do not scatter Xnew Vnew T+dt before computing correction
62 false, // full update? (not used, since no scatter)
63 true // force a call to the solver's Setup function
64 );
65
66 X += Dx;
67
68 integrable->StateScatter(X, V, T, true); // state -> system
69 }
70 }
71
72 if ((action & AssemblyLevel::VELOCITY) || (action & AssemblyLevel::ACCELERATION)) {
73 ChStateDelta Vold;
74
75 // setup auxiliary vectors
76 Vold.setZero(integrable->GetNcoords_v(), GetIntegrable());
77 R.setZero(integrable->GetNcoords_v());
78 Qc.setZero(integrable->GetNconstr());
79 L.setZero(integrable->GetNconstr());
80
81 integrable->StateGather(X, V, T); // state <- system
82
83 Vold = V;
84
85 // Perform a linearized semi-implicit Euler integration step
86 //
87 // [ M - dt*dF/dv - dt^2*dF/dx Cq' ] [ v_new ] = [ M*(v_old) + dt*f]
88 // [ Cq 0 ] [ -dt*l ] = [ C/dt + Ct ]
89
90 integrable->LoadResidual_F(R, dt);
91 integrable->LoadResidual_Mv(R, V, 1.0);
92 integrable->LoadConstraint_C(Qc, 1.0 / dt, false);
93 integrable->LoadConstraint_Ct(Qc, 1.0);
94
95 integrable->StateSolveCorrection(
96 V, L, R, Qc,
97 1.0, // factor for M
98 -dt, // factor for dF/dv
99 -dt * dt, // factor for dF/dx
100 X, V, T + dt, // not needed
101 false, // do not scatter Xnew Vnew T+dt before computing correction
102 false, // full update? (not used, since no scatter)
103 true // force a call to the solver's Setup() function
104 );
105
106 integrable->StateScatter(X, V, T, true); // state -> system
107
108 L *= (1.0 / dt); // Note it is not -(1.0/dt) because we assume StateSolveCorrection already flips sign of L
109
110 if (action & AssemblyLevel::ACCELERATION) {
111 integrable->StateScatterAcceleration(
112 (V - Vold) * (1 / dt)); // -> system auxiliary data (i.e acceleration as measure, fits DVI/MDI)
113
114 integrable->StateScatterReactions(L); // -> system auxiliary data
115 }
116 }
117 }
118
119 } // end namespace chrono