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/physics/ChLinkMask.h"
16
17 namespace chrono {
18
19 // Register into the object factory, to enable run-time dynamic creation and persistence
CH_FACTORY_REGISTER(ChLinkMask)20 CH_FACTORY_REGISTER(ChLinkMask)
21
22 ChLinkMask::ChLinkMask() : nconstr(0) {}
23
ChLinkMask(int mnconstr)24 ChLinkMask::ChLinkMask(int mnconstr) {
25 nconstr = mnconstr;
26 constraints.resize(nconstr);
27 for (int i = 0; i < nconstr; i++) {
28 constraints[i] = new ChConstraintTwoBodies;
29 }
30 }
31
ChLinkMask(const ChLinkMask & other)32 ChLinkMask::ChLinkMask(const ChLinkMask& other) {
33 nconstr = other.nconstr;
34 constraints.resize(other.nconstr);
35 for (int i = 0; i < nconstr; i++) {
36 constraints[i] = other.constraints[i]->Clone();
37 }
38 }
39
~ChLinkMask()40 ChLinkMask::~ChLinkMask() {
41 for (int i = 0; i < nconstr; i++) {
42 if (constraints[i]) {
43 delete constraints[i];
44 }
45 }
46 }
47
operator =(const ChLinkMask & other)48 ChLinkMask& ChLinkMask::operator=(const ChLinkMask& other) {
49 if (this == &other)
50 return *this;
51
52 nconstr = other.nconstr;
53 constraints.resize(nconstr);
54 for (int i = 0; i < nconstr; i++) {
55 constraints[i] = other.constraints[i]->Clone();
56 }
57
58 return *this;
59 }
60
ResetNconstr(int newnconstr)61 void ChLinkMask::ResetNconstr(int newnconstr) {
62 if (nconstr != newnconstr) {
63 for (int i = 0; i < nconstr; i++)
64 if (constraints[i]) {
65 delete constraints[i];
66 constraints[i] = nullptr;
67 }
68
69 nconstr = newnconstr;
70
71 constraints.resize(nconstr);
72
73 for (int i = 0; i < nconstr; i++) {
74 constraints[i] = new ChConstraintTwoBodies;
75 }
76 }
77 }
78
AddConstraint(ChConstraintTwoBodies * aconstr)79 void ChLinkMask::AddConstraint(ChConstraintTwoBodies* aconstr) {
80 nconstr++;
81 constraints.push_back(aconstr);
82 }
83
SetTwoBodiesVariables(ChVariables * var1,ChVariables * var2)84 void ChLinkMask::SetTwoBodiesVariables(ChVariables* var1, ChVariables* var2) {
85 for (int i = 0; i < nconstr; i++)
86 constraints[i]->SetVariables(var1, var2);
87 }
88
IsEqual(ChLinkMask & mask2)89 bool ChLinkMask::IsEqual(ChLinkMask& mask2) {
90 if (nconstr != mask2.nconstr)
91 return false;
92 for (int j = 0; j < nconstr; j++) {
93 if (!(Constr_N(j) == mask2.Constr_N(j)))
94 return false;
95 }
96 return true;
97 }
98
GetMaskDoc()99 int ChLinkMask::GetMaskDoc() {
100 int tot = 0;
101 for (int j = 0; j < nconstr; j++) {
102 if (Constr_N(j).IsActive())
103 tot++;
104 }
105 return tot;
106 }
107
GetMaskDoc_d()108 int ChLinkMask::GetMaskDoc_d() {
109 int cnt = 0;
110 for (int i = 0; i < nconstr; i++) {
111 if (Constr_N(i).IsActive() && Constr_N(i).IsUnilateral())
112 cnt++;
113 }
114 return cnt;
115 }
116
GetMaskDoc_c()117 int ChLinkMask::GetMaskDoc_c() {
118 return (GetMaskDoc() - GetMaskDoc_d());
119 }
120
GetActiveConstrByNum(int mnum)121 ChConstraintTwoBodies* ChLinkMask::GetActiveConstrByNum(int mnum) {
122 int cnt = 0;
123 for (int i = 0; i < nconstr; i++) {
124 if (Constr_N(i).IsActive()) {
125 if (cnt == mnum)
126 return &Constr_N(i);
127 cnt++;
128 }
129 }
130 return NULL;
131 }
132
SetActiveRedundantByArray(int * mvector,int mcount)133 int ChLinkMask::SetActiveRedundantByArray(int* mvector, int mcount) {
134 int cnt;
135
136 ChLinkMask newmask = *this;
137 for (int elem = 0; elem < mcount; elem++) {
138 cnt = 0;
139 for (int i = 0; i < nconstr; i++) {
140 if (constraints[i]->IsActive()) {
141 if (cnt == mvector[elem])
142 newmask.constraints[i]->SetRedundant(true);
143 cnt++;
144 }
145 }
146 }
147
148 // Replace the mask with updated one.
149 for (int i = 0; i < nconstr; i++) {
150 delete constraints[i];
151 constraints[i] = newmask.constraints[i]->Clone();
152 }
153
154 return mcount;
155 }
156
157 // set lock =ON for constraints which were disabled because redundant
RestoreRedundant()158 int ChLinkMask::RestoreRedundant() {
159 int tot = 0;
160 for (int j = 0; j < nconstr; j++) {
161 if (Constr_N(j).IsRedundant()) {
162 Constr_N(j).SetRedundant(false);
163 tot++;
164 }
165 }
166 return tot;
167 }
168
SetAllDisabled(bool mdis)169 int ChLinkMask::SetAllDisabled(bool mdis) {
170 int cnt = 0;
171
172 for (int i = 0; i < nconstr; i++) {
173 if (Constr_N(i).IsDisabled() != mdis) {
174 Constr_N(i).SetDisabled(mdis);
175 cnt++;
176 }
177 }
178
179 return cnt;
180 }
181
SetAllBroken(bool mdis)182 int ChLinkMask::SetAllBroken(bool mdis) {
183 int cnt = 0;
184
185 for (int i = 0; i < nconstr; i++) {
186 if (Constr_N(i).IsBroken() != mdis) {
187 Constr_N(i).SetBroken(mdis);
188 cnt++;
189 }
190 }
191
192 return cnt;
193 }
194
ArchiveOUT(ChArchiveOut & marchive)195 void ChLinkMask::ArchiveOUT(ChArchiveOut& marchive) {
196 //// TODO
197 }
198
ArchiveIN(ChArchiveIn & marchive)199 void ChLinkMask::ArchiveIN(ChArchiveIn& marchive) {
200 //// TODO
201 }
202
203 // -----------------------------------------------------------------------------
204 // Lock formulation LF link mask:
205 // -----------------------------------------------------------------------------
206
207 // Register into the object factory, to enable run-time dynamic creation and persistence
CH_FACTORY_REGISTER(ChLinkMaskLF)208 CH_FACTORY_REGISTER(ChLinkMaskLF)
209
210 ChLinkMaskLF::ChLinkMaskLF() {
211 ResetNconstr(7); // the LF formulation uses 7 constraint flags
212 }
213
operator =(const ChLinkMaskLF & other)214 ChLinkMaskLF& ChLinkMaskLF::operator=(const ChLinkMaskLF& other) {
215 ChLinkMask::operator=(other);
216 return *this;
217 }
218
SetLockMask(bool x,bool y,bool z,bool e0,bool e1,bool e2,bool e3)219 void ChLinkMaskLF::SetLockMask(bool x, bool y, bool z, bool e0, bool e1, bool e2, bool e3) {
220 if (x)
221 Constr_X().SetMode(CONSTRAINT_LOCK);
222 else
223 Constr_X().SetMode(CONSTRAINT_FREE);
224
225 if (y)
226 Constr_Y().SetMode(CONSTRAINT_LOCK);
227 else
228 Constr_Y().SetMode(CONSTRAINT_FREE);
229
230 if (z)
231 Constr_Z().SetMode(CONSTRAINT_LOCK);
232 else
233 Constr_Z().SetMode(CONSTRAINT_FREE);
234
235 if (e0)
236 Constr_E0().SetMode(CONSTRAINT_LOCK);
237 else
238 Constr_E0().SetMode(CONSTRAINT_FREE);
239
240 if (e1)
241 Constr_E1().SetMode(CONSTRAINT_LOCK);
242 else
243 Constr_E1().SetMode(CONSTRAINT_FREE);
244
245 if (e2)
246 Constr_E2().SetMode(CONSTRAINT_LOCK);
247 else
248 Constr_E2().SetMode(CONSTRAINT_FREE);
249
250 if (e3)
251 Constr_E3().SetMode(CONSTRAINT_LOCK);
252 else
253 Constr_E3().SetMode(CONSTRAINT_FREE);
254 }
255
ArchiveOUT(ChArchiveOut & marchive)256 void ChLinkMaskLF::ArchiveOUT(ChArchiveOut& marchive) {
257 //// TODO
258 }
259
ArchiveIN(ChArchiveIn & marchive)260 void ChLinkMaskLF::ArchiveIN(ChArchiveIn& marchive) {
261 //// TODO
262 }
263
264 } // end namespace chrono
265