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/collision/ChCollisionModelBullet.h"
16 #include "chrono/core/ChMath.h"
17 #include "chrono/physics/ChSystem.h"
18
19 #include "chrono/fea/ChContactSurfaceMesh.h"
20 #include "chrono/fea/ChElementShellANCF_3423.h"
21 #include "chrono/fea/ChElementShellANCF_3443.h"
22 #include "chrono/fea/ChElementShellANCF_3833.h"
23 #include "chrono/fea/ChElementShellReissner4.h"
24 #include "chrono/fea/ChElementCableANCF.h"
25 #include "chrono/fea/ChElementBeamANCF_3243.h"
26 #include "chrono/fea/ChElementBeamANCF_3333.h"
27 #include "chrono/fea/ChElementBeamEuler.h"
28 #include "chrono/fea/ChHexahedronFace.h"
29 #include "chrono/fea/ChTetrahedronFace.h"
30 #include "chrono/fea/ChMesh.h"
31
32 #include <unordered_map>
33 #include <map>
34 #include <set>
35 #include <array>
36 #include <algorithm>
37
38 namespace chrono {
39 namespace fea {
40
41 //////////////////////////////////////////////////////////////////////////////
42 //// ChContactTriangleXYZ
43
ChContactTriangleXYZ()44 ChContactTriangleXYZ::ChContactTriangleXYZ() {
45 collision_model = new collision::ChCollisionModelBullet;
46 collision_model->SetContactable(this);
47 }
48
ChContactTriangleXYZ(std::shared_ptr<ChNodeFEAxyz> n1,std::shared_ptr<ChNodeFEAxyz> n2,std::shared_ptr<ChNodeFEAxyz> n3,ChContactSurface * acontainer)49 ChContactTriangleXYZ::ChContactTriangleXYZ(std::shared_ptr<ChNodeFEAxyz> n1,
50 std::shared_ptr<ChNodeFEAxyz> n2,
51 std::shared_ptr<ChNodeFEAxyz> n3,
52 ChContactSurface* acontainer) {
53 mnode1 = n1;
54 mnode1 = n2;
55 mnode1 = n3;
56 container = acontainer;
57
58 collision_model = new collision::ChCollisionModelBullet;
59 collision_model->SetContactable(this);
60 }
61
GetPhysicsItem()62 ChPhysicsItem* ChContactTriangleXYZ::GetPhysicsItem() {
63 return (ChPhysicsItem*)container->GetMesh();
64 }
65
66 // interface to ChLoadableUV
67
68 // Gets all the DOFs packed in a single vector (position part).
LoadableGetStateBlock_x(int block_offset,ChState & mD)69 void ChContactTriangleXYZ::LoadableGetStateBlock_x(int block_offset, ChState& mD) {
70 mD.segment(block_offset + 0, 3) = mnode1->GetPos().eigen();
71 mD.segment(block_offset + 3, 3) = mnode2->GetPos().eigen();
72 mD.segment(block_offset + 6, 3) = mnode3->GetPos().eigen();
73 }
74
75 // Gets all the DOFs packed in a single vector (velocity part).
LoadableGetStateBlock_w(int block_offset,ChStateDelta & mD)76 void ChContactTriangleXYZ::LoadableGetStateBlock_w(int block_offset, ChStateDelta& mD) {
77 mD.segment(block_offset + 0, 3) = mnode1->GetPos_dt().eigen();
78 mD.segment(block_offset + 3, 3) = mnode2->GetPos_dt().eigen();
79 mD.segment(block_offset + 6, 3) = mnode3->GetPos_dt().eigen();
80 }
81
82 /// Increment all DOFs using a delta.
LoadableStateIncrement(const unsigned int off_x,ChState & x_new,const ChState & x,const unsigned int off_v,const ChStateDelta & Dv)83 void ChContactTriangleXYZ::LoadableStateIncrement(const unsigned int off_x,
84 ChState& x_new,
85 const ChState& x,
86 const unsigned int off_v,
87 const ChStateDelta& Dv) {
88 mnode1->NodeIntStateIncrement(off_x, x_new, x, off_v, Dv);
89 mnode2->NodeIntStateIncrement(off_x + 3, x_new, x, off_v + 3, Dv);
90 mnode3->NodeIntStateIncrement(off_x + 6, x_new, x, off_v + 6, Dv);
91 }
92 // Get the pointers to the contained ChVariables, appending to the mvars vector.
LoadableGetVariables(std::vector<ChVariables * > & mvars)93 void ChContactTriangleXYZ::LoadableGetVariables(std::vector<ChVariables*>& mvars) {
94 mvars.push_back(&mnode1->Variables());
95 mvars.push_back(&mnode2->Variables());
96 mvars.push_back(&mnode3->Variables());
97 }
98
ContactableGetStateBlock_x(ChState & x)99 void ChContactTriangleXYZ::ContactableGetStateBlock_x(ChState& x) {
100 x.segment(0, 3) = mnode1->pos.eigen();
101 x.segment(3, 3) = mnode2->pos.eigen();
102 x.segment(6, 3) = mnode3->pos.eigen();
103 }
104
ContactableGetStateBlock_w(ChStateDelta & w)105 void ChContactTriangleXYZ::ContactableGetStateBlock_w(ChStateDelta& w) {
106 w.segment(0, 3) = mnode1->pos_dt.eigen();
107 w.segment(3, 3) = mnode2->pos_dt.eigen();
108 w.segment(6, 3) = mnode3->pos_dt.eigen();
109 }
110
ContactableIncrementState(const ChState & x,const ChStateDelta & dw,ChState & x_new)111 void ChContactTriangleXYZ::ContactableIncrementState(const ChState& x, const ChStateDelta& dw, ChState& x_new) {
112 mnode1->NodeIntStateIncrement(0, x_new, x, 0, dw);
113 mnode2->NodeIntStateIncrement(3, x_new, x, 3, dw);
114 mnode3->NodeIntStateIncrement(6, x_new, x, 6, dw);
115 }
116
GetContactPoint(const ChVector<> & loc_point,const ChState & state_x)117 ChVector<> ChContactTriangleXYZ::GetContactPoint(const ChVector<>& loc_point, const ChState& state_x) {
118 // Note: because the reference coordinate system for a ChcontactTriangleXYZ is the identity,
119 // the given point loc_point is actually expressed in the global frame. In this case, we
120 // calculate the output point here by assuming that its barycentric coordinates do not change
121 // with a change in the states of this object.
122 double s2, s3;
123 ComputeUVfromP(loc_point, s2, s3);
124 double s1 = 1 - s2 - s3;
125
126 ChVector<> A1(state_x.segment(0, 3));
127 ChVector<> A2(state_x.segment(3, 3));
128 ChVector<> A3(state_x.segment(6, 3));
129
130 return s1 * A1 + s2 * A2 + s3 * A3;
131 }
132
GetContactPointSpeed(const ChVector<> & loc_point,const ChState & state_x,const ChStateDelta & state_w)133 ChVector<> ChContactTriangleXYZ::GetContactPointSpeed(const ChVector<>& loc_point,
134 const ChState& state_x,
135 const ChStateDelta& state_w) {
136 // Note: because the reference coordinate system for a ChcontactTriangleXYZ is the identity,
137 // the given point loc_point is actually expressed in the global frame. In this case, we
138 // calculate the output point here by assuming that its barycentric coordinates do not change
139 // with a change in the states of this object.
140 double s2, s3;
141 ComputeUVfromP(loc_point, s2, s3);
142 double s1 = 1 - s2 - s3;
143
144 ChVector<> A1_dt(state_w.segment(0, 3));
145 ChVector<> A2_dt(state_w.segment(3, 3));
146 ChVector<> A3_dt(state_w.segment(6, 3));
147
148 return s1 * A1_dt + s2 * A2_dt + s3 * A3_dt;
149 }
150
GetContactPointSpeed(const ChVector<> & abs_point)151 ChVector<> ChContactTriangleXYZ::GetContactPointSpeed(const ChVector<>& abs_point) {
152 double s2, s3;
153 ComputeUVfromP(abs_point, s2, s3);
154 double s1 = 1 - s2 - s3;
155 return (s1 * mnode1->pos_dt + s2 * mnode2->pos_dt + s3 * mnode3->pos_dt);
156 }
157
ContactForceLoadResidual_F(const ChVector<> & F,const ChVector<> & abs_point,ChVectorDynamic<> & R)158 void ChContactTriangleXYZ::ContactForceLoadResidual_F(const ChVector<>& F,
159 const ChVector<>& abs_point,
160 ChVectorDynamic<>& R) {
161 double s2, s3;
162 ComputeUVfromP(abs_point, s2, s3);
163 double s1 = 1 - s2 - s3;
164 R.segment(mnode1->NodeGetOffset_w(), 3) += F.eigen() * s1;
165 R.segment(mnode2->NodeGetOffset_w(), 3) += F.eigen() * s2;
166 R.segment(mnode3->NodeGetOffset_w(), 3) += F.eigen() * s3;
167 }
168
ContactForceLoadQ(const ChVector<> & F,const ChVector<> & point,const ChState & state_x,ChVectorDynamic<> & Q,int offset)169 void ChContactTriangleXYZ::ContactForceLoadQ(const ChVector<>& F,
170 const ChVector<>& point,
171 const ChState& state_x,
172 ChVectorDynamic<>& Q,
173 int offset) {
174 // Calculate barycentric coordinates
175 ChVector<> A1(state_x.segment(0, 3));
176 ChVector<> A2(state_x.segment(3, 3));
177 ChVector<> A3(state_x.segment(6, 3));
178
179 double s2, s3;
180 bool is_into;
181 ChVector<> p_projected;
182 /*double dist =*/collision::utils::PointTriangleDistance(point, A1, A2, A3, s2, s3, is_into, p_projected);
183 double s1 = 1 - s2 - s3;
184 Q.segment(offset + 0, 3) = F.eigen() * s1;
185 Q.segment(offset + 3, 3) = F.eigen() * s2;
186 Q.segment(offset + 6, 3) = F.eigen() * s3;
187 }
188
ComputeJacobianForContactPart(const ChVector<> & abs_point,ChMatrix33<> & contact_plane,type_constraint_tuple & jacobian_tuple_N,type_constraint_tuple & jacobian_tuple_U,type_constraint_tuple & jacobian_tuple_V,bool second)189 void ChContactTriangleXYZ::ComputeJacobianForContactPart(const ChVector<>& abs_point,
190 ChMatrix33<>& contact_plane,
191 type_constraint_tuple& jacobian_tuple_N,
192 type_constraint_tuple& jacobian_tuple_U,
193 type_constraint_tuple& jacobian_tuple_V,
194 bool second) {
195 // compute the triangular area-parameters s1 s2 s3:
196 double s2, s3;
197 bool is_into;
198 ChVector<> p_projected;
199 /*double dist =*/collision::utils::PointTriangleDistance(abs_point, GetNode1()->pos, GetNode2()->pos,
200 GetNode3()->pos, s2, s3, is_into, p_projected);
201 double s1 = 1 - s2 - s3;
202
203 ChMatrix33<> Jx1 = contact_plane.transpose();
204 if (!second)
205 Jx1 *= -1;
206
207 jacobian_tuple_N.Get_Cq_1().segment(0, 3) = Jx1.row(0);
208 jacobian_tuple_U.Get_Cq_1().segment(0, 3) = Jx1.row(1);
209 jacobian_tuple_V.Get_Cq_1().segment(0, 3) = Jx1.row(2);
210 jacobian_tuple_N.Get_Cq_1() *= s1;
211 jacobian_tuple_U.Get_Cq_1() *= s1;
212 jacobian_tuple_V.Get_Cq_1() *= s1;
213 jacobian_tuple_N.Get_Cq_2().segment(0, 3) = Jx1.row(0);
214 jacobian_tuple_U.Get_Cq_2().segment(0, 3) = Jx1.row(1);
215 jacobian_tuple_V.Get_Cq_2().segment(0, 3) = Jx1.row(2);
216 jacobian_tuple_N.Get_Cq_2() *= s2;
217 jacobian_tuple_U.Get_Cq_2() *= s2;
218 jacobian_tuple_V.Get_Cq_2() *= s2;
219 jacobian_tuple_N.Get_Cq_3().segment(0, 3) = Jx1.row(0);
220 jacobian_tuple_U.Get_Cq_3().segment(0, 3) = Jx1.row(1);
221 jacobian_tuple_V.Get_Cq_3().segment(0, 3) = Jx1.row(2);
222 jacobian_tuple_N.Get_Cq_3() *= s3;
223 jacobian_tuple_U.Get_Cq_3() *= s3;
224 jacobian_tuple_V.Get_Cq_3() *= s3;
225 }
226
GetSubBlockOffset(int nblock)227 unsigned int ChContactTriangleXYZ::GetSubBlockOffset(int nblock) {
228 if (nblock == 0)
229 return GetNode1()->NodeGetOffset_w();
230 if (nblock == 1)
231 return GetNode2()->NodeGetOffset_w();
232 if (nblock == 2)
233 return GetNode3()->NodeGetOffset_w();
234 return 0;
235 }
236
IsSubBlockActive(int nblock) const237 bool ChContactTriangleXYZ::IsSubBlockActive(int nblock) const {
238 if (nblock == 0)
239 return !GetNode1()->GetFixed();
240 if (nblock == 1)
241 return !GetNode2()->GetFixed();
242 if (nblock == 2)
243 return !GetNode3()->GetFixed();
244
245 return false;
246 }
247
248 // Evaluate N'*F , where N is the shape function evaluated at (U,V) coordinates of the surface.
ComputeNF(const double U,const double V,ChVectorDynamic<> & Qi,double & detJ,const ChVectorDynamic<> & F,ChVectorDynamic<> * state_x,ChVectorDynamic<> * state_w)249 void ChContactTriangleXYZ::ComputeNF(
250 const double U, // parametric coordinate in surface
251 const double V, // parametric coordinate in surface
252 ChVectorDynamic<>& Qi, // Return result of Q = N'*F here
253 double& detJ, // Return det[J] here
254 const ChVectorDynamic<>& F, // Input F vector, size is =n. field coords.
255 ChVectorDynamic<>* state_x, // if != 0, update state (pos. part) to this, then evaluate Q
256 ChVectorDynamic<>* state_w // if != 0, update state (speed part) to this, then evaluate Q
257 ) {
258 ChMatrixNM<double, 1, 3> N;
259 // shape functions (U and V in 0..1 as triangle integration)
260 N(0) = 1 - U - V;
261 N(1) = U;
262 N(2) = V;
263
264 // determinant of jacobian is also =2*areaoftriangle, also length of cross product of sides
265 ChVector<> p0 = GetNode1()->GetPos();
266 ChVector<> p1 = GetNode2()->GetPos();
267 ChVector<> p2 = GetNode3()->GetPos();
268 detJ = (Vcross(p2 - p0, p1 - p0)).Length();
269
270 Qi.segment(0, 3) = N(0) * F.segment(0, 3);
271 Qi.segment(3, 3) = N(1) * F.segment(0, 3);
272 Qi.segment(6, 3) = N(2) * F.segment(0, 3);
273 }
274
ComputeNormal(const double U,const double V)275 ChVector<> ChContactTriangleXYZ::ComputeNormal(const double U, const double V) {
276 ChVector<> p0 = GetNode1()->GetPos();
277 ChVector<> p1 = GetNode2()->GetPos();
278 ChVector<> p2 = GetNode3()->GetPos();
279 return Vcross(p1 - p0, p2 - p0).GetNormalized();
280 }
281
ComputeUVfromP(const ChVector<> P,double & u,double & v)282 void ChContactTriangleXYZ::ComputeUVfromP(const ChVector<> P, double& u, double& v) {
283 bool is_into;
284 ChVector<> p_projected;
285 /*double dist =*/collision::utils::PointTriangleDistance(P, mnode1->pos, mnode2->pos, mnode3->pos, u, v, is_into,
286 p_projected);
287 }
288
289 //////////////////////////////////////////////////////////////////////////////
290 //// ChContactTriangleXYZROT
291
ChContactTriangleXYZROT()292 ChContactTriangleXYZROT::ChContactTriangleXYZROT() {
293 collision_model = new collision::ChCollisionModelBullet;
294 collision_model->SetContactable(this);
295 }
296
ChContactTriangleXYZROT(std::shared_ptr<ChNodeFEAxyzrot> n1,std::shared_ptr<ChNodeFEAxyzrot> n2,std::shared_ptr<ChNodeFEAxyzrot> n3,ChContactSurface * acontainer)297 ChContactTriangleXYZROT::ChContactTriangleXYZROT(std::shared_ptr<ChNodeFEAxyzrot> n1,
298 std::shared_ptr<ChNodeFEAxyzrot> n2,
299 std::shared_ptr<ChNodeFEAxyzrot> n3,
300 ChContactSurface* acontainer) {
301 mnode1 = n1;
302 mnode1 = n2;
303 mnode1 = n3;
304 container = acontainer;
305
306 collision_model = new collision::ChCollisionModelBullet;
307 collision_model->SetContactable(this);
308 }
309
GetPhysicsItem()310 ChPhysicsItem* ChContactTriangleXYZROT::GetPhysicsItem() {
311 return (ChPhysicsItem*)container->GetMesh();
312 }
313
314 // interface to ChLoadableUV
315
316 // Gets all the DOFs packed in a single vector (position part).
LoadableGetStateBlock_x(int block_offset,ChState & mD)317 void ChContactTriangleXYZROT::LoadableGetStateBlock_x(int block_offset, ChState& mD) {
318 mD.segment(block_offset + 0, 3) = mnode1->GetPos().eigen();
319 mD.segment(block_offset + 3, 4) = mnode1->GetRot().eigen();
320
321 mD.segment(block_offset + 7, 3) = mnode2->GetPos().eigen();
322 mD.segment(block_offset + 10, 4) = mnode2->GetRot().eigen();
323
324 mD.segment(block_offset + 14, 3) = mnode3->GetPos().eigen();
325 mD.segment(block_offset + 17, 4) = mnode3->GetRot().eigen();
326 }
327
328 // Gets all the DOFs packed in a single vector (velocity part).
LoadableGetStateBlock_w(int block_offset,ChStateDelta & mD)329 void ChContactTriangleXYZROT::LoadableGetStateBlock_w(int block_offset, ChStateDelta& mD) {
330 mD.segment(block_offset + 0, 3) = mnode1->GetPos_dt().eigen();
331 mD.segment(block_offset + 3, 3) = mnode1->GetWvel_loc().eigen();
332
333 mD.segment(block_offset + 6, 3) = mnode2->GetPos_dt().eigen();
334 mD.segment(block_offset + 9, 3) = mnode2->GetWvel_loc().eigen();
335
336 mD.segment(block_offset + 12, 3) = mnode3->GetPos_dt().eigen();
337 mD.segment(block_offset + 15, 3) = mnode3->GetWvel_loc().eigen();
338 }
339
340 // Increment all DOFs using a delta.
LoadableStateIncrement(const unsigned int off_x,ChState & x_new,const ChState & x,const unsigned int off_v,const ChStateDelta & Dv)341 void ChContactTriangleXYZROT::LoadableStateIncrement(const unsigned int off_x,
342 ChState& x_new,
343 const ChState& x,
344 const unsigned int off_v,
345 const ChStateDelta& Dv) {
346 mnode1->NodeIntStateIncrement(off_x, x_new, x, off_v, Dv);
347 mnode2->NodeIntStateIncrement(off_x + 7, x_new, x, off_v + 6, Dv);
348 mnode3->NodeIntStateIncrement(off_x + 14, x_new, x, off_v + 12, Dv);
349 }
350
351 // Get the pointers to the contained ChVariables, appending to the mvars vector.
LoadableGetVariables(std::vector<ChVariables * > & mvars)352 void ChContactTriangleXYZROT::LoadableGetVariables(std::vector<ChVariables*>& mvars) {
353 mvars.push_back(&mnode1->Variables());
354 mvars.push_back(&mnode2->Variables());
355 mvars.push_back(&mnode3->Variables());
356 }
357
ContactableGetStateBlock_x(ChState & x)358 void ChContactTriangleXYZROT::ContactableGetStateBlock_x(ChState& x) {
359 x.segment(0, 3) = mnode1->GetPos().eigen();
360 x.segment(3, 4) = mnode1->GetRot().eigen();
361
362 x.segment(7, 3) = mnode2->GetPos().eigen();
363 x.segment(10, 4) = mnode2->GetRot().eigen();
364
365 x.segment(14, 3) = mnode3->GetPos().eigen();
366 x.segment(17, 4) = mnode3->GetRot().eigen();
367 }
368
ContactableGetStateBlock_w(ChStateDelta & w)369 void ChContactTriangleXYZROT::ContactableGetStateBlock_w(ChStateDelta& w) {
370 w.segment(0, 3) = mnode1->GetPos_dt().eigen();
371 w.segment(3, 3) = mnode1->GetWvel_loc().eigen();
372
373 w.segment(6, 3) = mnode2->GetPos_dt().eigen();
374 w.segment(9, 3) = mnode2->GetWvel_loc().eigen();
375
376 w.segment(12, 3) = mnode3->GetPos_dt().eigen();
377 w.segment(15, 3) = mnode3->GetWvel_loc().eigen();
378 }
379
ContactableIncrementState(const ChState & x,const ChStateDelta & dw,ChState & x_new)380 void ChContactTriangleXYZROT::ContactableIncrementState(const ChState& x, const ChStateDelta& dw, ChState& x_new) {
381 mnode1->NodeIntStateIncrement(0, x_new, x, 0, dw);
382 mnode2->NodeIntStateIncrement(7, x_new, x, 6, dw);
383 mnode3->NodeIntStateIncrement(14, x_new, x, 12, dw);
384 }
385
GetContactPoint(const ChVector<> & loc_point,const ChState & state_x)386 ChVector<> ChContactTriangleXYZROT::GetContactPoint(const ChVector<>& loc_point, const ChState& state_x) {
387 // Note: because the reference coordinate system for a ChContactTriangleXYZROT is the identity,
388 // the given point loc_point is actually expressed in the global frame. In this case, we
389 // calculate the output point here by assuming that its barycentric coordinates do not change
390 // with a change in the states of this object.
391 double s2, s3;
392 ComputeUVfromP(loc_point, s2, s3);
393 double s1 = 1 - s2 - s3;
394
395 ChVector<> A1(state_x.segment(0, 3));
396 ChVector<> A2(state_x.segment(7, 3));
397 ChVector<> A3(state_x.segment(14, 3));
398
399 return s1 * A1 + s2 * A2 + s3 * A3;
400 }
401
GetContactPointSpeed(const ChVector<> & loc_point,const ChState & state_x,const ChStateDelta & state_w)402 ChVector<> ChContactTriangleXYZROT::GetContactPointSpeed(const ChVector<>& loc_point,
403 const ChState& state_x,
404 const ChStateDelta& state_w) {
405 // Note: because the reference coordinate system for a ChContactTriangleXYZROT is the identity,
406 // the given point loc_point is actually expressed in the global frame. In this case, we
407 // calculate the output point here by assuming that its barycentric coordinates do not change
408 // with a change in the states of this object.
409 double s2, s3;
410 ComputeUVfromP(loc_point, s2, s3);
411 double s1 = 1 - s2 - s3;
412
413 ChVector<> A1_dt(state_w.segment(0, 3));
414 ChVector<> A2_dt(state_w.segment(6, 3));
415 ChVector<> A3_dt(state_w.segment(12, 3));
416
417 return s1 * A1_dt + s2 * A2_dt + s3 * A3_dt;
418 }
419
GetContactPointSpeed(const ChVector<> & abs_point)420 ChVector<> ChContactTriangleXYZROT::GetContactPointSpeed(const ChVector<>& abs_point) {
421 double s2, s3;
422 ComputeUVfromP(abs_point, s2, s3);
423 double s1 = 1 - s2 - s3;
424 return (s1 * mnode1->GetPos_dt() + s2 * mnode2->GetPos_dt() + s3 * mnode3->GetPos_dt());
425 }
426
ContactForceLoadResidual_F(const ChVector<> & F,const ChVector<> & abs_point,ChVectorDynamic<> & R)427 void ChContactTriangleXYZROT::ContactForceLoadResidual_F(const ChVector<>& F,
428 const ChVector<>& abs_point,
429 ChVectorDynamic<>& R) {
430 double s2, s3;
431 ComputeUVfromP(abs_point, s2, s3);
432 double s1 = 1 - s2 - s3;
433 R.segment(mnode1->NodeGetOffset_w(), 3) += F.eigen() * s1;
434 R.segment(mnode2->NodeGetOffset_w(), 3) += F.eigen() * s2;
435 R.segment(mnode3->NodeGetOffset_w(), 3) += F.eigen() * s3;
436 }
437
ContactForceLoadQ(const ChVector<> & F,const ChVector<> & point,const ChState & state_x,ChVectorDynamic<> & Q,int offset)438 void ChContactTriangleXYZROT::ContactForceLoadQ(const ChVector<>& F,
439 const ChVector<>& point,
440 const ChState& state_x,
441 ChVectorDynamic<>& Q,
442 int offset) {
443 // Calculate barycentric coordinates
444 ChVector<> A1(state_x.segment(0, 3));
445 ChVector<> A2(state_x.segment(7, 3));
446 ChVector<> A3(state_x.segment(14, 3));
447
448 double s2, s3;
449 bool is_into;
450 ChVector<> p_projected;
451 /*double dist =*/collision::utils::PointTriangleDistance(point, A1, A2, A3, s2, s3, is_into, p_projected);
452 double s1 = 1 - s2 - s3;
453 Q.segment(offset + 0, 3) = F.eigen() * s1;
454 Q.segment(offset + 6, 3) = F.eigen() * s2;
455 Q.segment(offset + 12, 3) = F.eigen() * s3;
456 }
457
ComputeJacobianForContactPart(const ChVector<> & abs_point,ChMatrix33<> & contact_plane,type_constraint_tuple & jacobian_tuple_N,type_constraint_tuple & jacobian_tuple_U,type_constraint_tuple & jacobian_tuple_V,bool second)458 void ChContactTriangleXYZROT::ComputeJacobianForContactPart(const ChVector<>& abs_point,
459 ChMatrix33<>& contact_plane,
460 type_constraint_tuple& jacobian_tuple_N,
461 type_constraint_tuple& jacobian_tuple_U,
462 type_constraint_tuple& jacobian_tuple_V,
463 bool second) {
464 // compute the triangular area-parameters s1 s2 s3:
465 double s2, s3;
466 bool is_into;
467 ChVector<> p_projected;
468 /*double dist =*/collision::utils::PointTriangleDistance(abs_point, GetNode1()->coord.pos, GetNode2()->coord.pos,
469 GetNode3()->coord.pos, s2, s3, is_into, p_projected);
470 double s1 = 1 - s2 - s3;
471
472 ChMatrix33<> Jx1 = contact_plane.transpose();
473 if (!second)
474 Jx1 *= -1;
475
476 jacobian_tuple_N.Get_Cq_1().segment(0, 3) = Jx1.row(0);
477 jacobian_tuple_U.Get_Cq_1().segment(0, 3) = Jx1.row(1);
478 jacobian_tuple_V.Get_Cq_1().segment(0, 3) = Jx1.row(2);
479 jacobian_tuple_N.Get_Cq_1() *= s1;
480 jacobian_tuple_U.Get_Cq_1() *= s1;
481 jacobian_tuple_V.Get_Cq_1() *= s1;
482 jacobian_tuple_N.Get_Cq_2().segment(0, 3) = Jx1.row(0);
483 jacobian_tuple_U.Get_Cq_2().segment(0, 3) = Jx1.row(1);
484 jacobian_tuple_V.Get_Cq_2().segment(0, 3) = Jx1.row(2);
485 jacobian_tuple_N.Get_Cq_2() *= s2;
486 jacobian_tuple_U.Get_Cq_2() *= s2;
487 jacobian_tuple_V.Get_Cq_2() *= s2;
488 jacobian_tuple_N.Get_Cq_3().segment(0, 3) = Jx1.row(0);
489 jacobian_tuple_U.Get_Cq_3().segment(0, 3) = Jx1.row(1);
490 jacobian_tuple_V.Get_Cq_3().segment(0, 3) = Jx1.row(2);
491 jacobian_tuple_N.Get_Cq_3() *= s3;
492 jacobian_tuple_U.Get_Cq_3() *= s3;
493 jacobian_tuple_V.Get_Cq_3() *= s3;
494 }
495
GetSubBlockOffset(int nblock)496 unsigned int ChContactTriangleXYZROT::GetSubBlockOffset(int nblock) {
497 if (nblock == 0)
498 return GetNode1()->NodeGetOffset_w();
499 if (nblock == 1)
500 return GetNode2()->NodeGetOffset_w();
501 if (nblock == 2)
502 return GetNode3()->NodeGetOffset_w();
503 return 0;
504 }
505
IsSubBlockActive(int nblock) const506 bool ChContactTriangleXYZROT::IsSubBlockActive(int nblock) const {
507 if (nblock == 0)
508 return !GetNode1()->GetFixed();
509 if (nblock == 1)
510 return !GetNode2()->GetFixed();
511 if (nblock == 2)
512 return !GetNode3()->GetFixed();
513
514 return false;
515 }
516
517 // Evaluate N'*F , where N is the shape function evaluated at (U,V) coordinates of the surface.
ComputeNF(const double U,const double V,ChVectorDynamic<> & Qi,double & detJ,const ChVectorDynamic<> & F,ChVectorDynamic<> * state_x,ChVectorDynamic<> * state_w)518 void ChContactTriangleXYZROT::ComputeNF(
519 const double U, // parametric coordinate in surface
520 const double V, // parametric coordinate in surface
521 ChVectorDynamic<>& Qi, // Return result of Q = N'*F here
522 double& detJ, // Return det[J] here
523 const ChVectorDynamic<>& F, // Input F vector, size is =n. field coords.
524 ChVectorDynamic<>* state_x, // if != 0, update state (pos. part) to this, then evaluate Q
525 ChVectorDynamic<>* state_w // if != 0, update state (speed part) to this, then evaluate Q
526 ) {
527 ChMatrixNM<double, 1, 3> N;
528 // shape functions (U and V in 0..1 as triangle integration)
529 N(0) = 1 - U - V;
530 N(1) = U;
531 N(2) = V;
532
533 // determinant of jacobian is also =2*areaoftriangle, also length of cross product of sides
534 ChVector<> p0 = GetNode1()->GetPos();
535 ChVector<> p1 = GetNode2()->GetPos();
536 ChVector<> p2 = GetNode3()->GetPos();
537 detJ = (Vcross(p2 - p0, p1 - p0)).Length();
538
539 Qi.segment(0, 3) = N(0) * F.segment(0, 3);
540 Qi.segment(3, 3) = N(0) * F.segment(3, 3);
541
542 Qi.segment(6, 3) = N(1) * F.segment(0, 3);
543 Qi.segment(9, 3) = N(1) * F.segment(3, 3);
544
545 Qi.segment(12, 3) = N(2) * F.segment(0, 3);
546 Qi.segment(15, 3) = N(2) * F.segment(3, 3);
547 }
548
ComputeNormal(const double U,const double V)549 ChVector<> ChContactTriangleXYZROT::ComputeNormal(const double U, const double V) {
550 ChVector<> p0 = GetNode1()->GetPos();
551 ChVector<> p1 = GetNode2()->GetPos();
552 ChVector<> p2 = GetNode3()->GetPos();
553 return Vcross(p1 - p0, p2 - p0).GetNormalized();
554 }
555
ComputeUVfromP(const ChVector<> P,double & u,double & v)556 void ChContactTriangleXYZROT::ComputeUVfromP(const ChVector<> P, double& u, double& v) {
557 bool is_into;
558 ChVector<> p_projected;
559 /*double dist =*/collision::utils::PointTriangleDistance(P, mnode1->GetPos(), mnode2->GetPos(), mnode3->GetPos(), u,
560 v, is_into, p_projected);
561 }
562
563 // -----------------------------------------------------------------------------
564 // ChContactSurfaceMesh
565
ChContactSurfaceMesh(std::shared_ptr<ChMaterialSurface> material,ChMesh * mesh)566 ChContactSurfaceMesh::ChContactSurfaceMesh(std::shared_ptr<ChMaterialSurface> material, ChMesh* mesh)
567 : ChContactSurface(material, mesh) {}
568
AddFacesFromBoundary(double sphere_swept,bool ccw)569 void ChContactSurfaceMesh::AddFacesFromBoundary(double sphere_swept, bool ccw) {
570 std::vector<std::array<ChNodeFEAxyz*, 3>> triangles;
571 std::vector<std::array<std::shared_ptr<ChNodeFEAxyz>, 3>> triangles_ptrs;
572
573 std::vector<std::array<ChNodeFEAxyzrot*, 3>> triangles_rot;
574 std::vector<std::array<std::shared_ptr<ChNodeFEAxyzrot>, 3>> triangles_rot_ptrs;
575
576 // Boundary faces of TETRAHEDRONS
577 std::multimap<std::array<ChNodeFEAxyz*, 3>, ChTetrahedronFace> face_map;
578
579 for (unsigned int ie = 0; ie < m_mesh->GetNelements(); ++ie) {
580 if (auto mtetra = std::dynamic_pointer_cast<ChElementTetrahedron>(m_mesh->GetElement(ie))) {
581 for (int nface = 0; nface < 4; ++nface) {
582 ChTetrahedronFace mface(mtetra, nface);
583 std::array<ChNodeFEAxyz*, 3> mface_key = {mface.GetNodeN(0).get(), mface.GetNodeN(1).get(),
584 mface.GetNodeN(2).get()};
585 std::sort(mface_key.begin(), mface_key.end());
586 face_map.insert({mface_key, mface});
587 }
588 }
589 }
590 for (unsigned int ie = 0; ie < m_mesh->GetNelements(); ++ie) {
591 if (auto mtetra = std::dynamic_pointer_cast<ChElementTetrahedron>(m_mesh->GetElement(ie))) {
592 for (int nface = 0; nface < 4; ++nface) {
593 ChTetrahedronFace mface(mtetra, nface);
594 std::array<ChNodeFEAxyz*, 3> mface_key = {mface.GetNodeN(0).get(), mface.GetNodeN(1).get(),
595 mface.GetNodeN(2).get()};
596 std::sort(mface_key.begin(), mface_key.end());
597 if (face_map.count(mface_key) == 1) {
598 // Found a face that is not shared.. so it is a boundary face.
599 triangles.push_back({{mface.GetNodeN(0).get(), mface.GetNodeN(1).get(), mface.GetNodeN(2).get()}});
600 triangles_ptrs.push_back({{mface.GetNodeN(0), mface.GetNodeN(1), mface.GetNodeN(2)}});
601 }
602 }
603 }
604 }
605
606 // Boundary faces of HEXAHEDRONS
607 std::multimap<std::array<ChNodeFEAxyz*, 4>, ChHexahedronFace> face_map_brick;
608
609 for (unsigned int ie = 0; ie < m_mesh->GetNelements(); ++ie) {
610 if (auto mbrick = std::dynamic_pointer_cast<ChElementHexahedron>(m_mesh->GetElement(ie))) {
611 for (int nface = 0; nface < 6; ++nface) {
612 ChHexahedronFace mface(mbrick, nface);
613 std::array<ChNodeFEAxyz*, 4> mface_key = {mface.GetNodeN(0).get(), mface.GetNodeN(1).get(),
614 mface.GetNodeN(2).get(), mface.GetNodeN(3).get()};
615 std::sort(mface_key.begin(), mface_key.end());
616 face_map_brick.insert({mface_key, mface});
617 }
618 }
619 }
620 for (unsigned int ie = 0; ie < m_mesh->GetNelements(); ++ie) {
621 if (auto mbrick = std::dynamic_pointer_cast<ChElementHexahedron>(m_mesh->GetElement(ie))) {
622 for (int nface = 0; nface < 6; ++nface) { // Each of the 6 faces of a brick
623 ChHexahedronFace mface(mbrick, nface); // Create a face of the element
624 std::array<ChNodeFEAxyz*, 4> mface_key = {mface.GetNodeN(0).get(), mface.GetNodeN(1).get(),
625 mface.GetNodeN(2).get(), mface.GetNodeN(3).get()};
626 std::sort(mface_key.begin(), mface_key.end());
627 if (face_map_brick.count(mface_key) == 1) {
628 // Found a face that is not shared.. so it is a boundary face: Make two triangles out of that face
629 triangles.push_back({{mface.GetNodeN(0).get(), mface.GetNodeN(1).get(), mface.GetNodeN(2).get()}});
630 triangles.push_back({{mface.GetNodeN(0).get(), mface.GetNodeN(2).get(), mface.GetNodeN(3).get()}});
631 triangles_ptrs.push_back({{mface.GetNodeN(0), mface.GetNodeN(1), mface.GetNodeN(2)}});
632 triangles_ptrs.push_back({{mface.GetNodeN(0), mface.GetNodeN(2), mface.GetNodeN(3)}});
633 }
634 }
635 }
636 }
637
638 // Skin of ANCF SHELLS:
639 for (unsigned int ie = 0; ie < m_mesh->GetNelements(); ++ie) {
640 if (auto mshell = std::dynamic_pointer_cast<ChElementShellANCF_3423>(m_mesh->GetElement(ie))) {
641 std::shared_ptr<ChNodeFEAxyz> nA = mshell->GetNodeA();
642 std::shared_ptr<ChNodeFEAxyz> nB = mshell->GetNodeB();
643 std::shared_ptr<ChNodeFEAxyz> nC = mshell->GetNodeC();
644 std::shared_ptr<ChNodeFEAxyz> nD = mshell->GetNodeD();
645 if (ccw) {
646 triangles.push_back({{nA.get(), nD.get(), nB.get()}});
647 triangles.push_back({{nB.get(), nD.get(), nC.get()}});
648 triangles_ptrs.push_back({{nA, nD, nB}});
649 triangles_ptrs.push_back({{nB, nD, nC}});
650 } else {
651 triangles.push_back({{nA.get(), nB.get(), nD.get()}});
652 triangles.push_back({{nB.get(), nC.get(), nD.get()}});
653 triangles_ptrs.push_back({{nA, nB, nD}});
654 triangles_ptrs.push_back({{nB, nC, nD}});
655 }
656 }
657 }
658
659 for (unsigned int ie = 0; ie < m_mesh->GetNelements(); ++ie) {
660 if (auto mshell = std::dynamic_pointer_cast<ChElementShellANCF_3443>(m_mesh->GetElement(ie))) {
661 std::shared_ptr<ChNodeFEAxyz> nA = mshell->GetNodeA();
662 std::shared_ptr<ChNodeFEAxyz> nB = mshell->GetNodeB();
663 std::shared_ptr<ChNodeFEAxyz> nC = mshell->GetNodeC();
664 std::shared_ptr<ChNodeFEAxyz> nD = mshell->GetNodeD();
665 if (ccw) {
666 triangles.push_back({{nA.get(), nD.get(), nB.get()}});
667 triangles.push_back({{nB.get(), nD.get(), nC.get()}});
668 triangles_ptrs.push_back({{nA, nD, nB}});
669 triangles_ptrs.push_back({{nB, nD, nC}});
670 } else {
671 triangles.push_back({{nA.get(), nB.get(), nD.get()}});
672 triangles.push_back({{nB.get(), nC.get(), nD.get()}});
673 triangles_ptrs.push_back({{nA, nB, nD}});
674 triangles_ptrs.push_back({{nB, nC, nD}});
675 }
676 }
677 }
678
679 for (unsigned int ie = 0; ie < m_mesh->GetNelements(); ++ie) {
680 if (auto mshell = std::dynamic_pointer_cast<ChElementShellANCF_3833>(m_mesh->GetElement(ie))) {
681 auto nA = mshell->GetNodeA();
682 auto nB = mshell->GetNodeB();
683 auto nC = mshell->GetNodeC();
684 auto nD = mshell->GetNodeD();
685 auto nE = mshell->GetNodeE();
686 auto nF = mshell->GetNodeF();
687 auto nG = mshell->GetNodeG();
688 auto nH = mshell->GetNodeH();
689 if (ccw) {
690 triangles.push_back({{nA.get(), nH.get(), nE.get()}});
691 triangles.push_back({{nB.get(), nE.get(), nF.get()}});
692 triangles.push_back({{nC.get(), nF.get(), nG.get()}});
693 triangles.push_back({{nD.get(), nG.get(), nH.get()}});
694 triangles.push_back({{nH.get(), nG.get(), nE.get()}});
695 triangles.push_back({{nF.get(), nE.get(), nG.get()}});
696 triangles_ptrs.push_back({{nA, nH, nE}});
697 triangles_ptrs.push_back({{nB, nE, nF}});
698 triangles_ptrs.push_back({{nC, nF, nG}});
699 triangles_ptrs.push_back({{nD, nG, nH}});
700 triangles_ptrs.push_back({{nH, nG, nE}});
701 triangles_ptrs.push_back({{nF, nE, nG}});
702 } else {
703 triangles.push_back({{nA.get(), nE.get(), nH.get()}});
704 triangles.push_back({{nB.get(), nF.get(), nE.get()}});
705 triangles.push_back({{nC.get(), nG.get(), nF.get()}});
706 triangles.push_back({{nD.get(), nH.get(), nG.get()}});
707 triangles.push_back({{nH.get(), nE.get(), nG.get()}});
708 triangles.push_back({{nF.get(), nG.get(), nE.get()}});
709 triangles_ptrs.push_back({{nA, nE, nH}});
710 triangles_ptrs.push_back({{nB, nF, nE}});
711 triangles_ptrs.push_back({{nC, nG, nF}});
712 triangles_ptrs.push_back({{nD, nH, nG}});
713 triangles_ptrs.push_back({{nH, nE, nG}});
714 triangles_ptrs.push_back({{nF, nG, nE}});
715 }
716 }
717 }
718
719 // Skin of REISSNER SHELLS:
720 for (unsigned int ie = 0; ie < m_mesh->GetNelements(); ++ie) {
721 if (auto mshell = std::dynamic_pointer_cast<ChElementShellReissner4>(m_mesh->GetElement(ie))) {
722 std::shared_ptr<ChNodeFEAxyzrot> nA = mshell->GetNodeA();
723 std::shared_ptr<ChNodeFEAxyzrot> nB = mshell->GetNodeB();
724 std::shared_ptr<ChNodeFEAxyzrot> nC = mshell->GetNodeC();
725 std::shared_ptr<ChNodeFEAxyzrot> nD = mshell->GetNodeD();
726 if (ccw) {
727 triangles_rot.push_back({{nA.get(), nD.get(), nB.get()}});
728 triangles_rot.push_back({{nB.get(), nD.get(), nC.get()}});
729 triangles_rot_ptrs.push_back({{nA, nD, nB}});
730 triangles_rot_ptrs.push_back({{nB, nD, nC}});
731 } else {
732 triangles_rot.push_back({{nA.get(), nB.get(), nD.get()}});
733 triangles_rot.push_back({{nB.get(), nC.get(), nD.get()}});
734 triangles_rot_ptrs.push_back({{nA, nB, nD}});
735 triangles_rot_ptrs.push_back({{nB, nC, nD}});
736 }
737 }
738 }
739
740 // EULER BEAMS (handles as a skinny triangle, with sphere swept radii, i.e. a capsule):
741 for (unsigned int ie = 0; ie < m_mesh->GetNelements(); ++ie) {
742 if (auto mbeam = std::dynamic_pointer_cast<ChElementBeamEuler>(m_mesh->GetElement(ie))) {
743 std::shared_ptr<ChNodeFEAxyzrot> nA = mbeam->GetNodeA();
744 std::shared_ptr<ChNodeFEAxyzrot> nB = mbeam->GetNodeB();
745
746 auto contact_triangle = chrono_types::make_shared<ChContactTriangleXYZROT>();
747 contact_triangle->SetNode1(nA);
748 contact_triangle->SetNode2(nB);
749 contact_triangle->SetNode3(nB);
750 vfaces_rot.push_back(contact_triangle);
751 contact_triangle->SetContactSurface(this);
752
753 double capsule_radius =
754 collision::ChCollisionModel::GetDefaultSuggestedMargin(); // fallback for no draw profile
755 if (auto mdrawshape = mbeam->GetSection()->GetDrawShape()) {
756 double ymin, ymax, zmin, zmax;
757 mdrawshape->GetAABB(ymin, ymax, zmin, zmax);
758 capsule_radius = 0.5 * sqrt(pow(ymax - ymin, 2) + pow(zmax - zmin, 2));
759 }
760
761 contact_triangle->GetCollisionModel()->ClearModel();
762 ((collision::ChCollisionModelBullet*)contact_triangle->GetCollisionModel())
763 ->AddTriangleProxy(m_material, // contact material
764 &nA->coord.pos, &nB->coord.pos, &nB->coord.pos, // vertices
765 0, 0, 0, // no wing vertexes
766 false, false, false, // are vertexes owned by this triangle?
767 true, false, true, // are edges owned by this triangle?
768 capsule_radius);
769 contact_triangle->GetCollisionModel()->BuildModel();
770 }
771 }
772
773 //
774 // ANCF BEAMS (handles as a skinny triangle, with sphere swept radii, i.e. a capsule):
775 //
776 for (unsigned int ie = 0; ie < m_mesh->GetNelements(); ++ie) {
777 if (auto mbeam = std::dynamic_pointer_cast<ChElementCableANCF>(m_mesh->GetElement(ie))) {
778 std::shared_ptr<ChNodeFEAxyzD> nA = mbeam->GetNodeA();
779 std::shared_ptr<ChNodeFEAxyzD> nB = mbeam->GetNodeB();
780
781 auto contact_triangle = chrono_types::make_shared<ChContactTriangleXYZ>();
782 contact_triangle->SetNode1(nA);
783 contact_triangle->SetNode2(nB);
784 contact_triangle->SetNode3(nB);
785 vfaces.push_back(contact_triangle);
786 contact_triangle->SetContactSurface(this);
787
788 double capsule_radius =
789 collision::ChCollisionModel::GetDefaultSuggestedMargin(); // fallback for no draw profile
790 if (auto mdrawshape = mbeam->GetSection()->GetDrawShape()) {
791 double ymin, ymax, zmin, zmax;
792 mdrawshape->GetAABB(ymin, ymax, zmin, zmax);
793 capsule_radius = 0.5 * sqrt(pow(ymax - ymin, 2) + pow(zmax - zmin, 2));
794 }
795
796 contact_triangle->GetCollisionModel()->ClearModel();
797 ((collision::ChCollisionModelBullet*)contact_triangle->GetCollisionModel())
798 ->AddTriangleProxy(m_material, // contact materials
799 &nA->pos, &nB->pos, &nB->pos, // vertices
800 0, 0, 0, // no wing vertexes
801 false, false, false, // are vertexes owned by this triangle?
802 true, false, true, // are edges owned by this triangle?
803 capsule_radius);
804 contact_triangle->GetCollisionModel()->BuildModel();
805 } else if (auto mbeam = std::dynamic_pointer_cast<ChElementBeamANCF_3243>(m_mesh->GetElement(ie))) {
806 std::shared_ptr<ChNodeFEAxyzD> nA = mbeam->GetNodeA();
807 std::shared_ptr<ChNodeFEAxyzD> nB = mbeam->GetNodeB();
808
809 auto contact_triangle = chrono_types::make_shared<ChContactTriangleXYZ>();
810 contact_triangle->SetNode1(nA);
811 contact_triangle->SetNode2(nB);
812 contact_triangle->SetNode3(nB);
813 vfaces.push_back(contact_triangle);
814 contact_triangle->SetContactSurface(this);
815
816 double capsule_radius = 0.5 * sqrt(pow(mbeam->GetThicknessY(), 2) + pow(mbeam->GetThicknessZ(), 2));
817
818 contact_triangle->GetCollisionModel()->ClearModel();
819 ((collision::ChCollisionModelBullet*)contact_triangle->GetCollisionModel())
820 ->AddTriangleProxy(m_material, // contact materials
821 &nA->pos, &nB->pos, &nB->pos, // vertices
822 0, 0, 0, // no wing vertexes
823 false, false, false, // are vertexes owned by this triangle?
824 true, false, true, // are edges owned by this triangle?
825 capsule_radius);
826 contact_triangle->GetCollisionModel()->BuildModel();
827 } else if (auto mbeam = std::dynamic_pointer_cast<ChElementBeamANCF_3333>(m_mesh->GetElement(ie))) {
828 std::shared_ptr<ChNodeFEAxyzD> nA = mbeam->GetNodeA();
829 std::shared_ptr<ChNodeFEAxyzD> nB = mbeam->GetNodeB();
830
831 auto contact_triangle = chrono_types::make_shared<ChContactTriangleXYZ>();
832 contact_triangle->SetNode1(nA);
833 contact_triangle->SetNode2(nB);
834 contact_triangle->SetNode3(nB);
835 vfaces.push_back(contact_triangle);
836 contact_triangle->SetContactSurface(this);
837
838 double capsule_radius = 0.5 * sqrt(pow(mbeam->GetThicknessY(), 2) + pow(mbeam->GetThicknessZ(), 2));
839
840 contact_triangle->GetCollisionModel()->ClearModel();
841 ((collision::ChCollisionModelBullet*)contact_triangle->GetCollisionModel())
842 ->AddTriangleProxy(m_material, // contact materials
843 &nA->pos, &nB->pos, &nB->pos, // vertices
844 0, 0, 0, // no wing vertexes
845 false, false, false, // are vertexes owned by this triangle?
846 true, false, true, // are edges owned by this triangle?
847 capsule_radius);
848 contact_triangle->GetCollisionModel()->BuildModel();
849 }
850 }
851
852 // Compute triangles connectivity
853 std::multimap<std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*>, int> edge_map;
854
855 for (int it = 0; it < triangles.size(); ++it) {
856 // edges = pairs of vertexes indexes
857 std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*> medgeA(triangles[it][0], triangles[it][1]);
858 std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*> medgeB(triangles[it][1], triangles[it][2]);
859 std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*> medgeC(triangles[it][2], triangles[it][0]);
860 // vertex indexes in edges: always in increasing order to avoid ambiguous duplicated edges
861 if (medgeA.first > medgeA.second)
862 medgeA = std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*>(medgeA.second, medgeA.first);
863 if (medgeB.first > medgeB.second)
864 medgeB = std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*>(medgeB.second, medgeB.first);
865 if (medgeC.first > medgeC.second)
866 medgeC = std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*>(medgeC.second, medgeC.first);
867 edge_map.insert({medgeA, it});
868 edge_map.insert({medgeB, it});
869 edge_map.insert({medgeC, it});
870 }
871
872 // Create a map of neighboring triangles, vector of:
873 // [Ti TieA TieB TieC]
874 std::vector<std::array<int, 4>> tri_map;
875 tri_map.resize(triangles.size());
876
877 for (int it = 0; it < triangles.size(); ++it) {
878 tri_map[it][0] = it;
879 tri_map[it][1] = -1; // default no neighbor
880 tri_map[it][2] = -1; // default no neighbor
881 tri_map[it][3] = -1; // default no neighbor
882 // edges = pairs of vertexes indexes
883 std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*> medgeA(triangles[it][0], triangles[it][1]);
884 std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*> medgeB(triangles[it][1], triangles[it][2]);
885 std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*> medgeC(triangles[it][2], triangles[it][0]);
886 // vertex indexes in edges: always in increasing order to avoid ambiguous duplicated edges
887 if (medgeA.first > medgeA.second)
888 medgeA = std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*>(medgeA.second, medgeA.first);
889 if (medgeB.first > medgeB.second)
890 medgeB = std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*>(medgeB.second, medgeB.first);
891 if (medgeC.first > medgeC.second)
892 medgeC = std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*>(medgeC.second, medgeC.first);
893 auto retA = edge_map.equal_range(medgeA);
894 for (auto fedge = retA.first; fedge != retA.second; ++fedge) {
895 if (fedge->second != it) {
896 tri_map[it][1] = fedge->second;
897 break;
898 }
899 }
900 auto retB = edge_map.equal_range(medgeB);
901 for (auto fedge = retB.first; fedge != retB.second; ++fedge) {
902 if (fedge->second != it) {
903 tri_map[it][2] = fedge->second;
904 break;
905 }
906 }
907 auto retC = edge_map.equal_range(medgeC);
908 for (auto fedge = retC.first; fedge != retC.second; ++fedge) {
909 if (fedge->second != it) {
910 tri_map[it][3] = fedge->second;
911 break;
912 }
913 }
914 }
915
916 std::map<std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*>, std::pair<int, int>> winged_edges;
917 bool allow_single_wing = true;
918
919 for (auto aedge = edge_map.begin(); aedge != edge_map.end(); ++aedge) {
920 auto ret = edge_map.equal_range(aedge->first);
921 int nt = 0;
922 std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*> wingedge;
923 std::pair<int, int> wingtri;
924 wingtri.first = -1;
925 wingtri.second = -1;
926 for (auto fedge = ret.first; fedge != ret.second; ++fedge) {
927 if (fedge->second == -1)
928 break;
929 wingedge.first = fedge->first.first;
930 wingedge.second = fedge->first.second;
931 if (nt == 0)
932 wingtri.first = fedge->second;
933 if (nt == 1)
934 wingtri.second = fedge->second;
935 ++nt;
936 if (nt == 2)
937 break;
938 }
939 if ((nt == 2) || ((nt == 1) && allow_single_wing)) {
940 winged_edges.insert(std::pair<std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*>, std::pair<int, int>>(
941 wingedge, wingtri)); // ok found winged edge!
942 aedge->second = -1; // deactivate this way otherwise found again by sister
943 }
944 }
945
946 // ....repeat: compute connectivity also for triangles with rotational dofs:
947
948 std::multimap<std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*>, int> edge_map_rot;
949
950 for (int it = 0; it < triangles_rot.size(); ++it) {
951 // edges = pairs of vertexes indexes
952 std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*> medgeA(triangles_rot[it][0], triangles_rot[it][1]);
953 std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*> medgeB(triangles_rot[it][1], triangles_rot[it][2]);
954 std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*> medgeC(triangles_rot[it][2], triangles_rot[it][0]);
955 // vertex indexes in edges: always in increasing order to avoid ambiguous duplicated edges
956 if (medgeA.first > medgeA.second)
957 medgeA = std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*>(medgeA.second, medgeA.first);
958 if (medgeB.first > medgeB.second)
959 medgeB = std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*>(medgeB.second, medgeB.first);
960 if (medgeC.first > medgeC.second)
961 medgeC = std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*>(medgeC.second, medgeC.first);
962 edge_map_rot.insert({medgeA, it});
963 edge_map_rot.insert({medgeB, it});
964 edge_map_rot.insert({medgeC, it});
965 }
966
967 // Create a map of neighboring triangles, vector of:
968 // [Ti TieA TieB TieC]
969 std::vector<std::array<int, 4>> tri_map_rot;
970 tri_map_rot.resize(triangles_rot.size());
971
972 for (int it = 0; it < triangles_rot.size(); ++it) {
973 tri_map_rot[it][0] = it;
974 tri_map_rot[it][1] = -1; // default no neighbor
975 tri_map_rot[it][2] = -1; // default no neighbor
976 tri_map_rot[it][3] = -1; // default no neighbor
977 // edges = pairs of vertexes indexes
978 std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*> medgeA(triangles_rot[it][0], triangles_rot[it][1]);
979 std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*> medgeB(triangles_rot[it][1], triangles_rot[it][2]);
980 std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*> medgeC(triangles_rot[it][2], triangles_rot[it][0]);
981 // vertex indexes in edges: always in increasing order to avoid ambiguous duplicated edges
982 if (medgeA.first > medgeA.second)
983 medgeA = std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*>(medgeA.second, medgeA.first);
984 if (medgeB.first > medgeB.second)
985 medgeB = std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*>(medgeB.second, medgeB.first);
986 if (medgeC.first > medgeC.second)
987 medgeC = std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*>(medgeC.second, medgeC.first);
988 auto retA = edge_map_rot.equal_range(medgeA);
989 for (auto fedge = retA.first; fedge != retA.second; ++fedge) {
990 if (fedge->second != it) {
991 tri_map_rot[it][1] = fedge->second;
992 break;
993 }
994 }
995 auto retB = edge_map_rot.equal_range(medgeB);
996 for (auto fedge = retB.first; fedge != retB.second; ++fedge) {
997 if (fedge->second != it) {
998 tri_map_rot[it][2] = fedge->second;
999 break;
1000 }
1001 }
1002 auto retC = edge_map_rot.equal_range(medgeC);
1003 for (auto fedge = retC.first; fedge != retC.second; ++fedge) {
1004 if (fedge->second != it) {
1005 tri_map_rot[it][3] = fedge->second;
1006 break;
1007 }
1008 }
1009 }
1010
1011 std::map<std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*>, std::pair<int, int>> winged_edges_rot;
1012 bool allow_single_wing_rot = true;
1013
1014 for (auto aedge = edge_map_rot.begin(); aedge != edge_map_rot.end(); ++aedge) {
1015 auto ret = edge_map_rot.equal_range(aedge->first);
1016 int nt = 0;
1017 std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*> wingedge;
1018 std::pair<int, int> wingtri;
1019 wingtri.first = -1;
1020 wingtri.second = -1;
1021 for (auto fedge = ret.first; fedge != ret.second; ++fedge) {
1022 if (fedge->second == -1)
1023 break;
1024 wingedge.first = fedge->first.first;
1025 wingedge.second = fedge->first.second;
1026 if (nt == 0)
1027 wingtri.first = fedge->second;
1028 if (nt == 1)
1029 wingtri.second = fedge->second;
1030 ++nt;
1031 if (nt == 2)
1032 break;
1033 }
1034 if ((nt == 2) || ((nt == 1) && allow_single_wing_rot)) {
1035 winged_edges_rot.insert(std::pair<std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*>, std::pair<int, int>>(
1036 wingedge, wingtri)); // ok found winged edge!
1037 aedge->second = -1; // deactivate this way otherwise found again by sister
1038 }
1039 }
1040
1041 // Create triangles with collision models
1042 std::set<ChNodeFEAxyz*> added_vertexes;
1043
1044 // iterate on triangles
1045 for (int it = 0; it < triangles.size(); ++it) {
1046 // edges = pairs of vertexes indexes
1047 std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*> medgeA(triangles[it][0], triangles[it][1]);
1048 std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*> medgeB(triangles[it][1], triangles[it][2]);
1049 std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*> medgeC(triangles[it][2], triangles[it][0]);
1050 // vertex indexes in edges: always in increasing order to avoid ambiguous duplicated edges
1051 if (medgeA.first > medgeA.second)
1052 medgeA = std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*>(medgeA.second, medgeA.first);
1053 if (medgeB.first > medgeB.second)
1054 medgeB = std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*>(medgeB.second, medgeB.first);
1055 if (medgeC.first > medgeC.second)
1056 medgeC = std::pair<ChNodeFEAxyz*, ChNodeFEAxyz*>(medgeC.second, medgeC.first);
1057 auto wingedgeA = winged_edges.find(medgeA);
1058 auto wingedgeB = winged_edges.find(medgeB);
1059 auto wingedgeC = winged_edges.find(medgeC);
1060
1061 ChNodeFEAxyz* i_wingvertex_A = 0;
1062 ChNodeFEAxyz* i_wingvertex_B = 0;
1063 ChNodeFEAxyz* i_wingvertex_C = 0;
1064
1065 if (tri_map[it][1] != -1) {
1066 i_wingvertex_A = triangles[tri_map[it][1]][0];
1067 if (triangles[tri_map[it][1]][1] != wingedgeA->first.first &&
1068 triangles[tri_map[it][1]][1] != wingedgeA->first.second)
1069 i_wingvertex_A = triangles[tri_map[it][1]][1];
1070 if (triangles[tri_map[it][1]][2] != wingedgeA->first.first &&
1071 triangles[tri_map[it][1]][2] != wingedgeA->first.second)
1072 i_wingvertex_A = triangles[tri_map[it][1]][2];
1073 }
1074
1075 if (tri_map[it][2] != -1) {
1076 i_wingvertex_B = triangles[tri_map[it][2]][0];
1077 if (triangles[tri_map[it][2]][1] != wingedgeB->first.first &&
1078 triangles[tri_map[it][2]][1] != wingedgeB->first.second)
1079 i_wingvertex_B = triangles[tri_map[it][2]][1];
1080 if (triangles[tri_map[it][2]][2] != wingedgeB->first.first &&
1081 triangles[tri_map[it][2]][2] != wingedgeB->first.second)
1082 i_wingvertex_B = triangles[tri_map[it][2]][2];
1083 }
1084
1085 if (tri_map[it][3] != -1) {
1086 i_wingvertex_C = triangles[tri_map[it][3]][0];
1087 if (triangles[tri_map[it][3]][1] != wingedgeC->first.first &&
1088 triangles[tri_map[it][3]][1] != wingedgeC->first.second)
1089 i_wingvertex_C = triangles[tri_map[it][3]][1];
1090 if (triangles[tri_map[it][3]][2] != wingedgeC->first.first &&
1091 triangles[tri_map[it][3]][2] != wingedgeC->first.second)
1092 i_wingvertex_C = triangles[tri_map[it][3]][2];
1093 }
1094
1095 auto contact_triangle = chrono_types::make_shared<ChContactTriangleXYZ>();
1096 contact_triangle->SetNode1(triangles_ptrs[it][0]);
1097 contact_triangle->SetNode2(triangles_ptrs[it][1]);
1098 contact_triangle->SetNode3(triangles_ptrs[it][2]);
1099 vfaces.push_back(contact_triangle);
1100 contact_triangle->SetContactSurface(this);
1101
1102 contact_triangle->GetCollisionModel()->ClearModel();
1103 ((collision::ChCollisionModelBullet*)contact_triangle->GetCollisionModel())
1104 ->AddTriangleProxy(m_material, // contact material
1105 &triangles[it][0]->pos, &triangles[it][1]->pos, &triangles[it][2]->pos,
1106 // if no wing vertex (ie. 'free' edge), point to opposite vertex, ie vertex in triangle
1107 // not belonging to edge
1108 wingedgeA->second.second != -1 ? &i_wingvertex_A->pos : &triangles[it][2]->pos,
1109 wingedgeB->second.second != -1 ? &i_wingvertex_B->pos : &triangles[it][0]->pos,
1110 wingedgeC->second.second != -1 ? &i_wingvertex_C->pos : &triangles[it][1]->pos,
1111 (added_vertexes.find(triangles[it][0]) == added_vertexes.end()),
1112 (added_vertexes.find(triangles[it][1]) == added_vertexes.end()),
1113 (added_vertexes.find(triangles[it][2]) == added_vertexes.end()),
1114 // are edges owned by this triangle? (if not, they belong to a neighboring triangle)
1115 wingedgeA->second.first != -1, wingedgeB->second.first != -1,
1116 wingedgeC->second.first != -1, sphere_swept);
1117 contact_triangle->GetCollisionModel()->BuildModel();
1118
1119 // Mark added vertexes
1120 added_vertexes.insert(triangles[it][0]);
1121 added_vertexes.insert(triangles[it][1]);
1122 added_vertexes.insert(triangles[it][2]);
1123 // Mark added edges, setting to -1 the 'ti' id of 1st triangle in winged edge {{vi,vj}{ti,tj}}
1124 wingedgeA->second.first = -1;
1125 wingedgeB->second.first = -1;
1126 wingedgeC->second.first = -1;
1127 }
1128
1129 // ....repeat: create triangles with collision models for nodes with rotationaldofs too:
1130
1131 std::set<ChNodeFEAxyzrot*> added_vertexes_rot;
1132
1133 // iterate on triangles
1134 for (int it = 0; it < triangles_rot.size(); ++it) {
1135 // edges = pairs of vertexes indexes
1136 std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*> medgeA(triangles_rot[it][0], triangles_rot[it][1]);
1137 std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*> medgeB(triangles_rot[it][1], triangles_rot[it][2]);
1138 std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*> medgeC(triangles_rot[it][2], triangles_rot[it][0]);
1139 // vertex indexes in edges: always in increasing order to avoid ambiguous duplicated edges
1140 if (medgeA.first > medgeA.second)
1141 medgeA = std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*>(medgeA.second, medgeA.first);
1142 if (medgeB.first > medgeB.second)
1143 medgeB = std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*>(medgeB.second, medgeB.first);
1144 if (medgeC.first > medgeC.second)
1145 medgeC = std::pair<ChNodeFEAxyzrot*, ChNodeFEAxyzrot*>(medgeC.second, medgeC.first);
1146 auto wingedgeA = winged_edges_rot.find(medgeA);
1147 auto wingedgeB = winged_edges_rot.find(medgeB);
1148 auto wingedgeC = winged_edges_rot.find(medgeC);
1149
1150 ChNodeFEAxyzrot* i_wingvertex_A = 0;
1151 ChNodeFEAxyzrot* i_wingvertex_B = 0;
1152 ChNodeFEAxyzrot* i_wingvertex_C = 0;
1153
1154 if (tri_map_rot[it][1] != -1) {
1155 i_wingvertex_A = triangles_rot[tri_map_rot[it][1]][0];
1156 if (triangles_rot[tri_map_rot[it][1]][1] != wingedgeA->first.first &&
1157 triangles_rot[tri_map_rot[it][1]][1] != wingedgeA->first.second)
1158 i_wingvertex_A = triangles_rot[tri_map_rot[it][1]][1];
1159 if (triangles_rot[tri_map_rot[it][1]][2] != wingedgeA->first.first &&
1160 triangles_rot[tri_map_rot[it][1]][2] != wingedgeA->first.second)
1161 i_wingvertex_A = triangles_rot[tri_map_rot[it][1]][2];
1162 }
1163
1164 if (tri_map_rot[it][2] != -1) {
1165 i_wingvertex_B = triangles_rot[tri_map_rot[it][2]][0];
1166 if (triangles_rot[tri_map_rot[it][2]][1] != wingedgeB->first.first &&
1167 triangles_rot[tri_map_rot[it][2]][1] != wingedgeB->first.second)
1168 i_wingvertex_B = triangles_rot[tri_map_rot[it][2]][1];
1169 if (triangles_rot[tri_map_rot[it][2]][2] != wingedgeB->first.first &&
1170 triangles_rot[tri_map_rot[it][2]][2] != wingedgeB->first.second)
1171 i_wingvertex_B = triangles_rot[tri_map_rot[it][2]][2];
1172 }
1173
1174 if (tri_map_rot[it][3] != -1) {
1175 i_wingvertex_C = triangles_rot[tri_map_rot[it][3]][0];
1176 if (triangles_rot[tri_map_rot[it][3]][1] != wingedgeC->first.first &&
1177 triangles_rot[tri_map_rot[it][3]][1] != wingedgeC->first.second)
1178 i_wingvertex_C = triangles_rot[tri_map_rot[it][3]][1];
1179 if (triangles_rot[tri_map_rot[it][3]][2] != wingedgeC->first.first &&
1180 triangles_rot[tri_map_rot[it][3]][2] != wingedgeC->first.second)
1181 i_wingvertex_C = triangles_rot[tri_map_rot[it][3]][2];
1182 }
1183
1184 auto contact_triangle_rot = chrono_types::make_shared<ChContactTriangleXYZROT>();
1185 contact_triangle_rot->SetNode1(triangles_rot_ptrs[it][0]);
1186 contact_triangle_rot->SetNode2(triangles_rot_ptrs[it][1]);
1187 contact_triangle_rot->SetNode3(triangles_rot_ptrs[it][2]);
1188 vfaces_rot.push_back(contact_triangle_rot);
1189 contact_triangle_rot->SetContactSurface(this);
1190
1191 contact_triangle_rot->GetCollisionModel()->ClearModel();
1192 ((collision::ChCollisionModelBullet*)contact_triangle_rot->GetCollisionModel())
1193 ->AddTriangleProxy(
1194 m_material, // contact material
1195 &triangles_rot[it][0]->coord.pos, &triangles_rot[it][1]->coord.pos, &triangles_rot[it][2]->coord.pos,
1196 // if no wing vertex (ie. 'free' edge), point to opposite vertex, ie vertex in triangle not belonging to
1197 // edge
1198 wingedgeA->second.second != -1 ? &i_wingvertex_A->coord.pos : &triangles_rot[it][2]->coord.pos,
1199 wingedgeB->second.second != -1 ? &i_wingvertex_B->coord.pos : &triangles_rot[it][0]->coord.pos,
1200 wingedgeC->second.second != -1 ? &i_wingvertex_C->coord.pos : &triangles_rot[it][1]->coord.pos,
1201 (added_vertexes_rot.find(triangles_rot[it][0]) == added_vertexes_rot.end()),
1202 (added_vertexes_rot.find(triangles_rot[it][1]) == added_vertexes_rot.end()),
1203 (added_vertexes_rot.find(triangles_rot[it][2]) == added_vertexes_rot.end()),
1204 // are edges owned by this triangle? (if not, they belong to a neighboring triangle)
1205 wingedgeA->second.first != -1, wingedgeB->second.first != -1, wingedgeC->second.first != -1,
1206 sphere_swept);
1207 contact_triangle_rot->GetCollisionModel()->BuildModel();
1208
1209 // Mark added vertexes
1210 added_vertexes_rot.insert(triangles_rot[it][0]);
1211 added_vertexes_rot.insert(triangles_rot[it][1]);
1212 added_vertexes_rot.insert(triangles_rot[it][2]);
1213 // Mark added edges, setting to -1 the 'ti' id of 1st triangle in winged edge {{vi,vj}{ti,tj}}
1214 wingedgeA->second.first = -1;
1215 wingedgeB->second.first = -1;
1216 wingedgeC->second.first = -1;
1217 }
1218 }
1219
GetNumVertices() const1220 unsigned int ChContactSurfaceMesh::GetNumVertices() const {
1221 std::map<ChNodeFEAxyz*, size_t> ptr_ind_map;
1222 size_t count = 0;
1223 for (size_t i = 0; i < vfaces.size(); ++i) {
1224 if (!ptr_ind_map.count(vfaces[i]->GetNode1().get())) {
1225 ptr_ind_map.insert({vfaces[i]->GetNode1().get(), count});
1226 count++;
1227 }
1228 if (!ptr_ind_map.count(vfaces[i]->GetNode2().get())) {
1229 ptr_ind_map.insert({vfaces[i]->GetNode2().get(), count});
1230 count++;
1231 }
1232 if (!ptr_ind_map.count(vfaces[i]->GetNode3().get())) {
1233 ptr_ind_map.insert({vfaces[i]->GetNode3().get(), count});
1234 count++;
1235 }
1236 }
1237
1238 std::map<ChNodeFEAxyzrot*, size_t> ptr_ind_map_rot;
1239 size_t count_rot = 0;
1240 for (size_t i = 0; i < vfaces_rot.size(); ++i) {
1241 if (!ptr_ind_map_rot.count(vfaces_rot[i]->GetNode1().get())) {
1242 ptr_ind_map_rot.insert({vfaces_rot[i]->GetNode1().get(), count_rot});
1243 count_rot++;
1244 }
1245 if (!ptr_ind_map_rot.count(vfaces_rot[i]->GetNode2().get())) {
1246 ptr_ind_map_rot.insert({vfaces_rot[i]->GetNode2().get(), count_rot});
1247 count_rot++;
1248 }
1249 if (!ptr_ind_map_rot.count(vfaces_rot[i]->GetNode3().get())) {
1250 ptr_ind_map_rot.insert({vfaces_rot[i]->GetNode3().get(), count_rot});
1251 count_rot++;
1252 }
1253 }
1254
1255 return (unsigned int)(count + count_rot);
1256 }
1257
SurfaceSyncCollisionModels()1258 void ChContactSurfaceMesh::SurfaceSyncCollisionModels() {
1259 for (unsigned int j = 0; j < vfaces.size(); j++) {
1260 vfaces[j]->GetCollisionModel()->SyncPosition();
1261 }
1262 for (unsigned int j = 0; j < vfaces_rot.size(); j++) {
1263 vfaces_rot[j]->GetCollisionModel()->SyncPosition();
1264 }
1265 }
1266
SurfaceAddCollisionModelsToSystem(ChSystem * msys)1267 void ChContactSurfaceMesh::SurfaceAddCollisionModelsToSystem(ChSystem* msys) {
1268 assert(msys);
1269 SurfaceSyncCollisionModels();
1270 for (unsigned int j = 0; j < vfaces.size(); j++) {
1271 msys->GetCollisionSystem()->Add(vfaces[j]->GetCollisionModel());
1272 }
1273 for (unsigned int j = 0; j < vfaces_rot.size(); j++) {
1274 msys->GetCollisionSystem()->Add(vfaces_rot[j]->GetCollisionModel());
1275 }
1276 }
1277
SurfaceRemoveCollisionModelsFromSystem(ChSystem * msys)1278 void ChContactSurfaceMesh::SurfaceRemoveCollisionModelsFromSystem(ChSystem* msys) {
1279 assert(msys);
1280 for (unsigned int j = 0; j < vfaces.size(); j++) {
1281 msys->GetCollisionSystem()->Remove(vfaces[j]->GetCollisionModel());
1282 }
1283 for (unsigned int j = 0; j < vfaces_rot.size(); j++) {
1284 msys->GetCollisionSystem()->Remove(vfaces_rot[j]->GetCollisionModel());
1285 }
1286 }
1287
1288 } // end namespace fea
1289 } // end namespace chrono
1290