1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2016 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: Hammad Mazhar
13 // =============================================================================
14 // Constraint utility functions
15 // =============================================================================
16
17 #include "chrono_multicore/constraints/ChConstraintUtils.h"
18
19 namespace chrono {
20
21 using namespace collision;
22
23 CH_MULTICORE_API
Orthogonalize(const real3 & Vx,real3 & Vy,real3 & Vz)24 void Orthogonalize(const real3& Vx, real3& Vy, real3& Vz) {
25 real3 mVsingular = real3(0, 1, 0);
26 Vz = Cross(Vx, mVsingular);
27 real mzlen = Length(Vz);
28 // was near singularity? change singularity reference vector!
29 if (mzlen < real(0.0001)) {
30 mVsingular = real3(1, 0, 0);
31 Vz = Cross(Vx, mVsingular);
32 mzlen = Length(Vz);
33 }
34 Vz = Vz / mzlen;
35 Vy = Cross(Vz, Vx);
36 }
37
38 CH_MULTICORE_API
Compute_Jacobian(const quaternion & quat,const real3 & U,const real3 & V,const real3 & W,const real3 & point,real3 & T1,real3 & T2,real3 & T3)39 void Compute_Jacobian(const quaternion& quat,
40 const real3& U,
41 const real3& V,
42 const real3& W,
43 const real3& point,
44 real3& T1,
45 real3& T2,
46 real3& T3) {
47 quaternion quaternion_conjugate = ~quat;
48 real3 sbar = Rotate(point, quaternion_conjugate);
49
50 T1 = Cross(Rotate(U, quaternion_conjugate), sbar);
51 T2 = Cross(Rotate(V, quaternion_conjugate), sbar);
52 T3 = Cross(Rotate(W, quaternion_conjugate), sbar);
53 }
54
55 CH_MULTICORE_API
Compute_Jacobian_Rolling(const quaternion & quat,const real3 & U,const real3 & V,const real3 & W,real3 & T1,real3 & T2,real3 & T3)56 void Compute_Jacobian_Rolling(const quaternion& quat,
57 const real3& U,
58 const real3& V,
59 const real3& W,
60 real3& T1,
61 real3& T2,
62 real3& T3) {
63 quaternion quaternion_conjugate = ~quat;
64
65 T1 = Rotate(U, quaternion_conjugate);
66 T2 = Rotate(V, quaternion_conjugate);
67 T3 = Rotate(W, quaternion_conjugate);
68 }
69
70 CH_MULTICORE_API
Cone_generalized_rigid(real & gamma_n,real & gamma_u,real & gamma_v,real mu)71 bool Cone_generalized_rigid(real& gamma_n, real& gamma_u, real& gamma_v, real mu) {
72 real f_tang = sqrt(gamma_u * gamma_u + gamma_v * gamma_v);
73
74 // inside upper cone? keep untouched!
75 if (f_tang < (mu * gamma_n)) {
76 return false;
77 }
78
79 // inside lower cone? reset normal,u,v to zero!
80 if ((f_tang) < -(1 / mu) * gamma_n || (fabs(gamma_n) < 10e-15)) {
81 gamma_n = 0;
82 gamma_u = 0;
83 gamma_v = 0;
84 return false;
85 }
86
87 // remaining case: project orthogonally to generator segment of upper cone
88 gamma_n = (f_tang * mu + gamma_n) / (mu * mu + 1);
89 real tproj_div_t = (gamma_n * mu) / f_tang;
90 gamma_u *= tproj_div_t;
91 gamma_v *= tproj_div_t;
92
93 return true;
94 }
95
96 CH_MULTICORE_API
Cone_single_rigid(real & gamma_n,real & gamma_s,real mu)97 bool Cone_single_rigid(real& gamma_n, real& gamma_s, real mu) {
98 real f_tang = fabs(gamma_s);
99
100 // inside upper cone? keep untouched!
101 if (f_tang < (mu * gamma_n)) {
102 return false;
103 }
104
105 // inside lower cone? reset normal,u,v to zero!
106 if ((f_tang) < -(1 / mu) * gamma_n || (fabs(gamma_n) < 10e-15)) {
107 gamma_n = 0;
108 gamma_s = 0;
109 return false;
110 }
111
112 // remaining case: project orthogonally to generator segment of upper cone
113 gamma_n = (f_tang * mu + gamma_n) / (mu * mu + 1);
114 real tproj_div_t = (gamma_n * mu) / f_tang;
115 gamma_s *= tproj_div_t;
116
117 return true;
118 }
119
120 CH_MULTICORE_API
AppendRigidFluidBoundary(const real contact_mu,const uint num_fluid_bodies,const uint body_offset,const uint start_boundary,ChMulticoreDataManager * data_manager)121 void AppendRigidFluidBoundary(const real contact_mu,
122 const uint num_fluid_bodies,
123 const uint body_offset,
124 const uint start_boundary,
125 ChMulticoreDataManager* data_manager) {
126 CompressedMatrix<real>& D_T = data_manager->host_data.D_T;
127 uint num_rigid_fluid_contacts = data_manager->cd_data->num_rigid_fluid_contacts;
128 if (num_rigid_fluid_contacts > 0) {
129 custom_vector<int>& neighbor_rigid_fluid = data_manager->cd_data->neighbor_rigid_fluid;
130 custom_vector<int>& contact_counts = data_manager->cd_data->c_counts_rigid_fluid;
131
132 Loop_Over_Rigid_Neighbors( //
133 int rigid = neighbor_rigid_fluid[p * ChNarrowphase::max_rigid_neighbors + i]; //
134 AppendRow6(D_T, start_boundary + index + 0, rigid * 6, 0); //
135 AppendRow3(D_T, start_boundary + index + 0, body_offset + p * 3, 0); //
136 D_T.finalize(start_boundary + index + 0); //
137 );
138 if (contact_mu != 0) {
139 Loop_Over_Rigid_Neighbors( //
140 int rigid = neighbor_rigid_fluid[p * ChNarrowphase::max_rigid_neighbors + i]; //
141 AppendRow6(D_T, start_boundary + num_rigid_fluid_contacts + index * 2 + 0, rigid * 6, 0); //
142 AppendRow3(D_T, start_boundary + num_rigid_fluid_contacts + index * 2 + 0, body_offset + p * 3, 0); //
143 D_T.finalize(start_boundary + num_rigid_fluid_contacts + index * 2 + 0); //
144 AppendRow6(D_T, start_boundary + num_rigid_fluid_contacts + index * 2 + 1, rigid * 6, 0); //
145 AppendRow3(D_T, start_boundary + num_rigid_fluid_contacts + index * 2 + 1, body_offset + p * 3, 0); //
146 D_T.finalize(start_boundary + num_rigid_fluid_contacts + index * 2 + 1); //
147 );
148 }
149 }
150 }
151
152 CH_MULTICORE_API
ProjectRigidFluidBoundary(const real contact_mu,const real contact_cohesion,const uint num_fluid_bodies,const uint start_boundary,real * gamma,ChMulticoreDataManager * data_manager)153 void ProjectRigidFluidBoundary(const real contact_mu,
154 const real contact_cohesion,
155 const uint num_fluid_bodies,
156 const uint start_boundary,
157 real* gamma,
158 ChMulticoreDataManager* data_manager) {
159 custom_vector<int>& neighbor_rigid_fluid = data_manager->cd_data->neighbor_rigid_fluid;
160 custom_vector<int>& contact_counts = data_manager->cd_data->c_counts_rigid_fluid;
161 uint num_rigid_fluid_contacts = data_manager->cd_data->num_rigid_fluid_contacts;
162
163 if (contact_mu == 0) {
164 #pragma omp parallel for
165 Loop_Over_Rigid_Neighbors(
166 // rigid stored in first index
167 int rigid = neighbor_rigid_fluid[p * ChNarrowphase::max_rigid_neighbors + i]; //
168 float rigid_coh = data_manager->host_data.cohesion[rigid]; //
169 real cohesion = data_manager->composition_strategy->CombineCohesion(rigid_coh, (float)contact_cohesion); //
170 real3 gam; //
171 gam.x = gamma[start_boundary + index]; //
172 gam.x += cohesion; //
173 gam.x = gam.x < 0 ? 0 : gam.x - cohesion; //
174 gamma[start_boundary + index] = gam.x; //
175 );
176 } else {
177 #pragma omp parallel for
178 Loop_Over_Rigid_Neighbors(
179 // rigid stored in first index
180 int rigid = neighbor_rigid_fluid[p * ChNarrowphase::max_rigid_neighbors + i]; //
181 float rigid_coh = data_manager->host_data.cohesion[rigid]; //
182 float rigid_mu = data_manager->host_data.sliding_friction[rigid]; //
183 real cohesion = data_manager->composition_strategy->CombineCohesion(rigid_coh, (float)contact_cohesion); //
184 real friction = data_manager->composition_strategy->CombineFriction(rigid_mu, (float)contact_mu); //
185
186 real3 gam; //
187 gam.x = gamma[start_boundary + index]; //
188 gam.y = gamma[start_boundary + num_rigid_fluid_contacts + index * 2 + 0]; //
189 gam.z = gamma[start_boundary + num_rigid_fluid_contacts + index * 2 + 1]; //
190
191 gam.x += cohesion;
192
193 if (friction == 0) {
194 gam.x = gam.x < 0 ? 0 : gam.x - cohesion;
195 gam.y = gam.z = 0;
196
197 gamma[start_boundary + index] = gam.x;
198 gamma[start_boundary + num_rigid_fluid_contacts + index * 2 + 0] = gam.y;
199 gamma[start_boundary + num_rigid_fluid_contacts + index * 2 + 1] = gam.z;
200 continue;
201 }
202
203 Cone_generalized_rigid(gam.x, gam.y, gam.z, friction);
204
205 gamma[start_boundary + index] = gam.x - cohesion;
206 gamma[start_boundary + num_rigid_fluid_contacts + index * 2 + 0] = gam.y;
207 gamma[start_boundary + num_rigid_fluid_contacts + index * 2 + 1] = gam.z;);
208 }
209 }
210
211 //// TODO: This uses the same compliance value, for all interactions.
212 //// Consider using a combination law.
213 CH_MULTICORE_API
ComplianceRigidFluidBoundary(const real contact_mu,const real contact_compliance,const real alpha,const uint start_boundary,ChMulticoreDataManager * data_manager)214 void ComplianceRigidFluidBoundary(const real contact_mu,
215 const real contact_compliance,
216 const real alpha,
217 const uint start_boundary,
218 ChMulticoreDataManager* data_manager) {
219 DynamicVector<real>& E = data_manager->host_data.E;
220 uint num_rigid_fluid_contacts = data_manager->cd_data->num_rigid_fluid_contacts;
221 real inv_h = 1 / data_manager->settings.step_size;
222 real inv_hpa = 1 / (data_manager->settings.step_size + alpha);
223 real inv_hhpa = inv_h * inv_hpa;
224 real com = 0;
225 if (alpha) {
226 com = inv_hhpa * contact_compliance;
227 }
228 if (num_rigid_fluid_contacts > 0) {
229 if (contact_mu == 0) {
230 #pragma omp parallel for
231 for (int index = 0; index < (signed)num_rigid_fluid_contacts; index++) {
232 E[start_boundary + index + 0] = com;
233 }
234 } else {
235 #pragma omp parallel for
236 for (int index = 0; index < (signed)num_rigid_fluid_contacts; index++) {
237 E[start_boundary + index + 0] = com;
238 E[start_boundary + num_rigid_fluid_contacts + index * 2 + 0] = 0;
239 E[start_boundary + num_rigid_fluid_contacts + index * 2 + 1] = 0;
240 }
241 }
242 }
243 }
244
245 CH_MULTICORE_API
CorrectionRigidFluidBoundary(const real contact_mu,const real contact_cohesion,const real alpha,const real contact_recovery_speed,const uint num_fluid_bodies,const uint start_boundary,ChMulticoreDataManager * data_manager)246 void CorrectionRigidFluidBoundary(const real contact_mu,
247 const real contact_cohesion,
248 const real alpha,
249 const real contact_recovery_speed,
250 const uint num_fluid_bodies,
251 const uint start_boundary,
252 ChMulticoreDataManager* data_manager) {
253 real inv_hpa = 1 / (data_manager->settings.step_size + alpha);
254
255 DynamicVector<real>& b = data_manager->host_data.b;
256 custom_vector<real>& dpth_rigid_fluid = data_manager->cd_data->dpth_rigid_fluid;
257 uint num_rigid_fluid_contacts = data_manager->cd_data->num_rigid_fluid_contacts;
258
259 if (num_rigid_fluid_contacts > 0) {
260 custom_vector<int>& contact_counts = data_manager->cd_data->c_counts_rigid_fluid;
261
262 if (contact_mu == 0) {
263 #pragma omp parallel for
264 Loop_Over_Rigid_Neighbors(real depth = dpth_rigid_fluid[p * ChNarrowphase::max_rigid_neighbors + i]; //
265 real bi = 0; //
266 if (contact_cohesion != 0) { depth = Min(depth, 0); } //
267 bi = std::max(inv_hpa * depth, -contact_recovery_speed); //
268 b[start_boundary + index + 0] = bi; //
269 );
270 } else {
271 #pragma omp parallel for
272 Loop_Over_Rigid_Neighbors(real depth = dpth_rigid_fluid[p * ChNarrowphase::max_rigid_neighbors + i]; //
273 real bi = 0; //
274 if (contact_cohesion != 0) { depth = Min(depth, 0); } //
275 bi = std::max(inv_hpa * depth, -contact_recovery_speed); //
276 b[start_boundary + index + 0] = bi; //
277 b[start_boundary + num_rigid_fluid_contacts + index * 2 + 0] = 0; //
278 b[start_boundary + num_rigid_fluid_contacts + index * 2 + 1] = 0; //
279 );
280 }
281 }
282 }
283
284 CH_MULTICORE_API
BuildRigidFluidBoundary(const real contact_mu,const uint num_fluid_bodies,const uint body_offset,const uint start_boundary,ChMulticoreDataManager * data_manager)285 void BuildRigidFluidBoundary(const real contact_mu,
286 const uint num_fluid_bodies,
287 const uint body_offset,
288 const uint start_boundary,
289 ChMulticoreDataManager* data_manager) {
290 uint num_rigid_fluid_contacts = data_manager->cd_data->num_rigid_fluid_contacts;
291 if (num_rigid_fluid_contacts > 0) {
292 CompressedMatrix<real>& D_T = data_manager->host_data.D_T;
293 custom_vector<real3>& pos_rigid = data_manager->host_data.pos_rigid;
294 custom_vector<quaternion>& rot_rigid = data_manager->host_data.rot_rigid;
295
296 custom_vector<real3>& cpta = data_manager->cd_data->cpta_rigid_fluid;
297 custom_vector<real3>& norm = data_manager->cd_data->norm_rigid_fluid;
298 custom_vector<int>& neighbor_rigid_fluid = data_manager->cd_data->neighbor_rigid_fluid;
299 custom_vector<int>& contact_counts = data_manager->cd_data->c_counts_rigid_fluid;
300
301 if (contact_mu == 0) {
302 #pragma omp parallel for
303 Loop_Over_Rigid_Neighbors(
304 int rigid = neighbor_rigid_fluid[p * ChNarrowphase::max_rigid_neighbors + i]; //
305 const real3& U = norm[p * ChNarrowphase::max_rigid_neighbors + i]; //
306 real3 V; //
307 real3 W; //
308 Orthogonalize(U, V, W); //
309 real3 T1; real3 T2; real3 T3; //
310 Compute_Jacobian(rot_rigid[rigid], U, V, W,
311 cpta[p * ChNarrowphase::max_rigid_neighbors + i] - pos_rigid[rigid], T1, T2, T3);
312
313 SetRow6Check(D_T, start_boundary + index + 0, rigid * 6, -U, T1);
314 SetRow3Check(D_T, start_boundary + index + 0, body_offset + p * 3, U););
315 } else {
316 #pragma omp parallel for
317 Loop_Over_Rigid_Neighbors(
318 int rigid = neighbor_rigid_fluid[p * ChNarrowphase::max_rigid_neighbors + i]; //
319 const real3& U = norm[p * ChNarrowphase::max_rigid_neighbors + i]; //
320 real3 V; //
321 real3 W; //
322 Orthogonalize(U, V, W); //
323 real3 T1; real3 T2; real3 T3; //
324 Compute_Jacobian(rot_rigid[rigid], U, V, W,
325 cpta[p * ChNarrowphase::max_rigid_neighbors + i] - pos_rigid[rigid], T1, T2, T3);
326
327 SetRow6Check(D_T, start_boundary + index + 0, rigid * 6, -U, T1);
328 SetRow6Check(D_T, start_boundary + num_rigid_fluid_contacts + index * 2 + 0, rigid * 6, -V, T2);
329 SetRow6Check(D_T, start_boundary + num_rigid_fluid_contacts + index * 2 + 1, rigid * 6, -W, T3);
330
331 SetRow3Check(D_T, start_boundary + index + 0, body_offset + p * 3, U);
332 SetRow3Check(D_T, start_boundary + num_rigid_fluid_contacts + index * 2 + 0, body_offset + p * 3, V);
333 SetRow3Check(D_T, start_boundary + num_rigid_fluid_contacts + index * 2 + 1, body_offset + p * 3, W););
334 }
335 }
336 }
337
338 } // end namespace chrono
339