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, Radu Serban
13 // =============================================================================
14 //
15 // Handling of rigid contact and calculation of corrections and Jacobians
16 //
17 // =============================================================================
18
19 #include <algorithm>
20 #include <limits>
21
22 #include "chrono_multicore/ChConfigMulticore.h"
23 #include "chrono_multicore/constraints/ChConstraintRigidRigid.h"
24 #include "chrono_multicore/constraints/ChConstraintUtils.h"
25
26 #include <thrust/iterator/constant_iterator.h>
27
28 using namespace chrono;
29
30 // -----------------------------------------------------------------------------
31
ChConstraintRigidRigid()32 ChConstraintRigidRigid::ChConstraintRigidRigid()
33 : data_manager(nullptr), offset(3), inv_h(0), inv_hpa(0), inv_hhpa(0) {}
34
func_Project_normal(int index,const vec2 * ids,const real * cohesion,real * gamma)35 void ChConstraintRigidRigid::func_Project_normal(int index, const vec2* ids, const real* cohesion, real* gamma) {
36 const auto num_rigid_contacts = data_manager->cd_data->num_rigid_contacts;
37
38 real gamma_x = gamma[index * 1 + 0];
39 real coh = cohesion[index];
40
41 gamma[index * 1 + 0] = (gamma_x < -coh) ? -coh : gamma_x;
42 switch (data_manager->settings.solver.solver_mode) {
43 case SolverMode::SLIDING:
44 gamma[num_rigid_contacts + index * 2 + 0] = 0;
45 gamma[num_rigid_contacts + index * 2 + 1] = 0;
46 break;
47 case SolverMode::SPINNING:
48 gamma[num_rigid_contacts + index * 2 + 0] = 0;
49 gamma[num_rigid_contacts + index * 2 + 1] = 0;
50 gamma[3 * num_rigid_contacts + index * 3 + 0] = 0;
51 gamma[3 * num_rigid_contacts + index * 3 + 1] = 0;
52 gamma[3 * num_rigid_contacts + index * 3 + 2] = 0;
53 break;
54 default:
55 break;
56 }
57 }
58
func_Project_sliding(int index,const vec2 * ids,const real3 * fric,const real * cohesion,real * gam)59 void ChConstraintRigidRigid::func_Project_sliding(int index,
60 const vec2* ids,
61 const real3* fric,
62 const real* cohesion,
63 real* gam) {
64 const auto num_rigid_contacts = data_manager->cd_data->num_rigid_contacts;
65
66 real3 gamma;
67 gamma.x = gam[index * 1 + 0];
68 gamma.y = gam[num_rigid_contacts + index * 2 + 0];
69 gamma.z = gam[num_rigid_contacts + index * 2 + 1];
70
71 real coh = cohesion[index];
72 real mu = fric[index].x;
73
74 if (mu == 0) {
75 gam[index * 1 + 0] = (gamma.x < -coh) ? -coh : gamma.x;
76 gam[num_rigid_contacts + index * 2 + 0] = 0;
77 gam[num_rigid_contacts + index * 2 + 1] = 0;
78 return;
79 }
80
81 gamma.x += coh;
82 Cone_generalized_rigid(gamma.x, gamma.y, gamma.z, mu);
83 gam[index * 1 + 0] = gamma.x - coh;
84 gam[num_rigid_contacts + index * 2 + 0] = gamma.y;
85 gam[num_rigid_contacts + index * 2 + 1] = gamma.z;
86 }
87
func_Project_spinning(int index,const vec2 * ids,const real3 * fric,real * gam)88 void ChConstraintRigidRigid::func_Project_spinning(int index, const vec2* ids, const real3* fric, real* gam) {
89 const auto num_rigid_contacts = data_manager->cd_data->num_rigid_contacts;
90
91 real rollingfriction = fric[index].y;
92 real spinningfriction = fric[index].z;
93
94 real f_n = gam[index * 1 + 0];
95 real t_n = gam[3 * num_rigid_contacts + index * 3 + 0];
96 real t_u = gam[3 * num_rigid_contacts + index * 3 + 1];
97 real t_v = gam[3 * num_rigid_contacts + index * 3 + 2];
98
99 real t_tang = sqrt(t_v * t_v + t_u * t_u);
100 real t_sptang = fabs(t_n); // = sqrt(t_n*t_n);
101
102 if (spinningfriction) {
103 if (t_sptang < spinningfriction * f_n) {
104 // inside upper cone? keep untouched!
105 } else {
106 // inside lower cone? reset normal,u,v to zero!
107 if ((t_sptang < -(1 / spinningfriction) * f_n) || (fabs(f_n) < 10e-15)) {
108 gam[index * 1 + 0] = 0;
109 gam[3 * num_rigid_contacts + index * 3 + 0] = 0;
110 } else {
111 // remaining case: project orthogonally to generator segment of upper cone (CAN BE simplified)
112 real f_n_proj = (t_sptang * spinningfriction + f_n) / (spinningfriction * spinningfriction + 1);
113 real t_tang_proj = f_n_proj * spinningfriction;
114 real tproj_div_t = t_tang_proj / t_sptang;
115 real t_n_proj = tproj_div_t * t_n;
116
117 gam[index * 1 + 0] = f_n_proj;
118 gam[3 * num_rigid_contacts + index * 3 + 0] = t_n_proj;
119 }
120 }
121 }
122
123 if (!rollingfriction) {
124 gam[3 * num_rigid_contacts + index * 3 + 1] = 0;
125 gam[3 * num_rigid_contacts + index * 3 + 2] = 0;
126
127 if (f_n < 0)
128 gam[index * 1 + 0] = 0;
129 return;
130 }
131 if (t_tang < rollingfriction * f_n)
132 return;
133
134 if ((t_tang < -(1 / rollingfriction) * f_n) || (fabs(f_n) < 10e-15)) {
135 real f_n_proj = 0;
136 real t_u_proj = 0;
137 real t_v_proj = 0;
138
139 gam[index * 1 + 0] = f_n_proj;
140 gam[3 * num_rigid_contacts + index * 3 + 1] = t_u_proj;
141 gam[3 * num_rigid_contacts + index * 3 + 2] = t_v_proj;
142
143 return;
144 }
145 real f_n_proj = (t_tang * rollingfriction + f_n) / (rollingfriction * rollingfriction + 1);
146 real t_tang_proj = f_n_proj * rollingfriction;
147 real tproj_div_t = t_tang_proj / t_tang;
148 real t_u_proj = tproj_div_t * t_u;
149 real t_v_proj = tproj_div_t * t_v;
150
151 gam[index * 1 + 0] = f_n_proj;
152 gam[3 * num_rigid_contacts + index * 3 + 1] = t_u_proj;
153 gam[3 * num_rigid_contacts + index * 3 + 2] = t_v_proj;
154 }
155
156 // -----------------------------------------------------------------------------
157
host_Project_single(int index,vec2 * ids,real3 * friction,real * cohesion,real * gamma)158 void ChConstraintRigidRigid::host_Project_single(int index, vec2* ids, real3* friction, real* cohesion, real* gamma) {
159 // always project normal
160 switch (data_manager->settings.solver.local_solver_mode) {
161 case SolverMode::NORMAL: {
162 func_Project_normal(index, ids, cohesion, gamma);
163 } break;
164
165 case SolverMode::SLIDING: {
166 func_Project_sliding(index, ids, friction, cohesion, gamma);
167 } break;
168
169 case SolverMode::SPINNING: {
170 func_Project_sliding(index, ids, friction, cohesion, gamma);
171 func_Project_spinning(index, ids, friction, gamma);
172 } break;
173 default:
174 break;
175 }
176 }
177
Setup(ChMulticoreDataManager * dm)178 void ChConstraintRigidRigid::Setup(ChMulticoreDataManager* dm) {
179 data_manager = dm;
180 const auto num_rigid_contacts = data_manager->cd_data->num_rigid_contacts;
181
182 inv_h = 1 / data_manager->settings.step_size;
183 inv_hpa = 1 / (data_manager->settings.step_size + data_manager->settings.solver.alpha);
184 inv_hhpa = inv_h * inv_hpa;
185
186 if (num_rigid_contacts <= 0) {
187 return;
188 }
189
190 contact_active_pairs.resize(int(num_rigid_contacts));
191 rotated_point_a.resize(num_rigid_contacts);
192 rotated_point_b.resize(num_rigid_contacts);
193 quat_a.resize(num_rigid_contacts);
194 quat_b.resize(num_rigid_contacts);
195
196 // Readability replacements
197 auto& bids = data_manager->cd_data->bids_rigid_rigid; // global IDs of bodies in contact
198 auto& abody = data_manager->host_data.active_rigid; // flags for active bodies
199
200 #pragma omp parallel for
201 for (int i = 0; i < (signed)num_rigid_contacts; i++) {
202 auto b1 = bids[i].x; // global IDs of bodies in contact
203 auto b2 = bids[i].y; //
204
205 contact_active_pairs[i] = bool2(abody[b1] != 0, abody[b2] != 0);
206
207 {
208 quaternion quaternion_conjugate = ~data_manager->host_data.rot_rigid[b1];
209 real3 sbar = Rotate(data_manager->cd_data->cpta_rigid_rigid[i] - data_manager->host_data.pos_rigid[b1],
210 quaternion_conjugate);
211
212 rotated_point_a[i] = real3_int(sbar, b1);
213 quat_a[i] = quaternion_conjugate;
214 }
215 {
216 quaternion quaternion_conjugate = ~data_manager->host_data.rot_rigid[b2];
217 real3 sbar = Rotate(data_manager->cd_data->cptb_rigid_rigid[i] - data_manager->host_data.pos_rigid[b2],
218 quaternion_conjugate);
219
220 rotated_point_b[i] = real3_int(sbar, b2);
221 quat_b[i] = quaternion_conjugate;
222 }
223 }
224 }
225
Project(real * gamma)226 void ChConstraintRigidRigid::Project(real* gamma) {
227 const auto num_rigid_contacts = data_manager->cd_data->num_rigid_contacts;
228 const custom_vector<vec2>& bids = data_manager->cd_data->bids_rigid_rigid;
229 const custom_vector<real3>& friction = data_manager->host_data.fric_rigid_rigid;
230 const custom_vector<real>& cohesion = data_manager->host_data.coh_rigid_rigid;
231
232 switch (data_manager->settings.solver.local_solver_mode) {
233 case SolverMode::NORMAL: {
234 #pragma omp parallel for
235 for (int index = 0; index < (signed)num_rigid_contacts; index++) {
236 func_Project_normal(index, bids.data(), cohesion.data(), gamma);
237 }
238 } break;
239
240 case SolverMode::SLIDING: {
241 #pragma omp parallel for
242 for (int index = 0; index < (signed)num_rigid_contacts; index++) {
243 func_Project_sliding(index, bids.data(), friction.data(), cohesion.data(), gamma);
244 }
245 } break;
246
247 case SolverMode::SPINNING: {
248 #pragma omp parallel for
249 for (int index = 0; index < (signed)num_rigid_contacts; index++) {
250 func_Project_sliding(index, bids.data(), friction.data(), cohesion.data(), gamma);
251 func_Project_spinning(index, bids.data(), friction.data(), gamma);
252 }
253 } break;
254 default:
255 break;
256 }
257 }
258
Project_Single(int index,real * gamma)259 void ChConstraintRigidRigid::Project_Single(int index, real* gamma) {
260 custom_vector<vec2>& bids = data_manager->cd_data->bids_rigid_rigid;
261 custom_vector<real3>& friction = data_manager->host_data.fric_rigid_rigid;
262 custom_vector<real>& cohesion = data_manager->host_data.coh_rigid_rigid;
263
264 // host_Project_single(index, bids.data(), friction.data(), cohesion.data(), gamma);
265
266 switch (data_manager->settings.solver.local_solver_mode) {
267 case SolverMode::NORMAL: {
268 func_Project_normal(index, bids.data(), cohesion.data(), gamma);
269 } break;
270 case SolverMode::SLIDING: {
271 func_Project_sliding(index, bids.data(), friction.data(), cohesion.data(), gamma);
272 } break;
273
274 case SolverMode::SPINNING: {
275 func_Project_sliding(index, bids.data(), friction.data(), cohesion.data(), gamma);
276 func_Project_spinning(index, bids.data(), friction.data(), gamma);
277 } break;
278 default:
279 break;
280 }
281 }
282
Build_b()283 void ChConstraintRigidRigid::Build_b() {
284 const auto num_rigid_contacts = data_manager->cd_data->num_rigid_contacts;
285
286 if (num_rigid_contacts <= 0) {
287 return;
288 }
289
290 #pragma omp parallel for
291 for (int index = 0; index < (signed)num_rigid_contacts; index++) {
292 real bi = 0;
293 real depth = data_manager->cd_data->dpth_rigid_rigid[index];
294
295 if (data_manager->settings.solver.alpha > 0) {
296 bi = inv_hpa * depth;
297 } else if (data_manager->settings.solver.contact_recovery_speed < 0) {
298 bi = inv_h * depth;
299 } else {
300 bi = std::max(inv_h * depth, -data_manager->settings.solver.contact_recovery_speed);
301 // bi = std::min(bi, inv_h * data_manager->settings.solver.cohesion_epsilon);
302 }
303
304 data_manager->host_data.b[index * 1 + 0] = bi;
305 }
306 }
307
Build_s()308 void ChConstraintRigidRigid::Build_s() {
309 const auto num_rigid_contacts = data_manager->cd_data->num_rigid_contacts;
310
311 if (num_rigid_contacts <= 0) {
312 return;
313 }
314
315 if (data_manager->settings.solver.solver_mode == SolverMode::NORMAL) {
316 return;
317 }
318
319 vec2* ids = data_manager->cd_data->bids_rigid_rigid.data();
320 const SubMatrixType& D_t_T = _DTT_;
321 DynamicVector<real> v_new;
322
323 const DynamicVector<real>& M_invk = data_manager->host_data.M_invk;
324 const DynamicVector<real>& gamma = data_manager->host_data.gamma;
325
326 const CompressedMatrix<real, blaze::columnMajor>& M_invD = data_manager->host_data.M_invD;
327
328 v_new = M_invk + M_invD * gamma;
329
330 #pragma omp parallel for
331 for (int index = 0; index < (signed)num_rigid_contacts; index++) {
332 real fric = data_manager->host_data.fric_rigid_rigid[index].x;
333 vec2 body_id = ids[index];
334
335 real s_v = D_t_T(index * 2 + 0, body_id.x * 6 + 0) * +v_new[body_id.x * 6 + 0] +
336 D_t_T(index * 2 + 0, body_id.x * 6 + 1) * +v_new[body_id.x * 6 + 1] +
337 D_t_T(index * 2 + 0, body_id.x * 6 + 2) * +v_new[body_id.x * 6 + 2] +
338 D_t_T(index * 2 + 0, body_id.x * 6 + 3) * +v_new[body_id.x * 6 + 3] +
339 D_t_T(index * 2 + 0, body_id.x * 6 + 4) * +v_new[body_id.x * 6 + 4] +
340 D_t_T(index * 2 + 0, body_id.x * 6 + 5) * +v_new[body_id.x * 6 + 5] +
341
342 D_t_T(index * 2 + 0, body_id.y * 6 + 0) * +v_new[body_id.y * 6 + 0] +
343 D_t_T(index * 2 + 0, body_id.y * 6 + 1) * +v_new[body_id.y * 6 + 1] +
344 D_t_T(index * 2 + 0, body_id.y * 6 + 2) * +v_new[body_id.y * 6 + 2] +
345 D_t_T(index * 2 + 0, body_id.y * 6 + 3) * +v_new[body_id.y * 6 + 3] +
346 D_t_T(index * 2 + 0, body_id.y * 6 + 4) * +v_new[body_id.y * 6 + 4] +
347 D_t_T(index * 2 + 0, body_id.y * 6 + 5) * +v_new[body_id.y * 6 + 5];
348
349 real s_w = D_t_T(index * 2 + 1, body_id.x * 6 + 0) * +v_new[body_id.x * 6 + 0] +
350 D_t_T(index * 2 + 1, body_id.x * 6 + 1) * +v_new[body_id.x * 6 + 1] +
351 D_t_T(index * 2 + 1, body_id.x * 6 + 2) * +v_new[body_id.x * 6 + 2] +
352 D_t_T(index * 2 + 1, body_id.x * 6 + 3) * +v_new[body_id.x * 6 + 3] +
353 D_t_T(index * 2 + 1, body_id.x * 6 + 4) * +v_new[body_id.x * 6 + 4] +
354 D_t_T(index * 2 + 1, body_id.x * 6 + 5) * +v_new[body_id.x * 6 + 5] +
355
356 D_t_T(index * 2 + 1, body_id.y * 6 + 0) * +v_new[body_id.y * 6 + 0] +
357 D_t_T(index * 2 + 1, body_id.y * 6 + 1) * +v_new[body_id.y * 6 + 1] +
358 D_t_T(index * 2 + 1, body_id.y * 6 + 2) * +v_new[body_id.y * 6 + 2] +
359 D_t_T(index * 2 + 1, body_id.y * 6 + 3) * +v_new[body_id.y * 6 + 3] +
360 D_t_T(index * 2 + 1, body_id.y * 6 + 4) * +v_new[body_id.y * 6 + 4] +
361 D_t_T(index * 2 + 1, body_id.y * 6 + 5) * +v_new[body_id.y * 6 + 5];
362
363 data_manager->host_data.s[index * 1 + 0] = sqrt(s_v * s_v + s_w * s_w) * fric;
364 }
365 }
366
Build_E()367 void ChConstraintRigidRigid::Build_E() {
368 const auto num_rigid_contacts = data_manager->cd_data->num_rigid_contacts;
369
370 if (num_rigid_contacts <= 0) {
371 return;
372 }
373 SolverMode solver_mode = data_manager->settings.solver.solver_mode;
374 DynamicVector<real>& E = data_manager->host_data.E;
375 uint num_contacts = num_rigid_contacts;
376 const custom_vector<real4>& compliance = data_manager->host_data.compliance_rigid_rigid;
377
378 #pragma omp parallel for
379 for (int index = 0; index < (signed)num_rigid_contacts; index++) {
380 ////vec2 body = data_manager->cd_data->bids_rigid_rigid[index];
381
382 real compliance_normal = compliance[index].x;
383 real compliance_sliding = compliance[index].y;
384 real compliance_rolling = compliance[index].z;
385 real compliance_spinning = compliance[index].w;
386
387 E[index * 1 + 0] = inv_hhpa * compliance_normal;
388 if (solver_mode == SolverMode::SLIDING) {
389 E[num_contacts + index * 2 + 0] = inv_hhpa * compliance_sliding;
390 E[num_contacts + index * 2 + 1] = inv_hhpa * compliance_sliding;
391 } else if (solver_mode == SolverMode::SPINNING) {
392 E[3 * num_contacts + index * 3 + 0] = inv_hhpa * compliance_spinning;
393 E[3 * num_contacts + index * 3 + 1] = inv_hhpa * compliance_rolling;
394 E[3 * num_contacts + index * 3 + 2] = inv_hhpa * compliance_rolling;
395 }
396 }
397 }
398
Build_D()399 void ChConstraintRigidRigid::Build_D() {
400 LOG(INFO) << "ChConstraintRigidRigid::Build_D";
401 const auto num_rigid_contacts = data_manager->cd_data->num_rigid_contacts;
402 real3* norm = data_manager->cd_data->norm_rigid_rigid.data();
403 vec2* ids = data_manager->cd_data->bids_rigid_rigid.data();
404
405 CompressedMatrix<real>& D_T = data_manager->host_data.D_T;
406
407 SolverMode solver_mode = data_manager->settings.solver.solver_mode;
408
409 #pragma omp parallel for
410 for (int index = 0; index < (signed)num_rigid_contacts; index++) {
411 const real3& U = norm[index];
412 real3 V, W;
413 Orthogonalize(U, V, W);
414 vec2 body_id = ids[index];
415 int off = 0;
416 int row = index;
417 // The position is subtracted here now instead of performing it in the narrowphase
418
419 real3 T3, T4, T5, T6, T7, T8;
420 real3 TA, TB, TC;
421 real3 TD, TE, TF;
422
423 // Normal jacobian entries
424 const real3_int& sbar_a = rotated_point_a[index];
425 const real3_int& sbar_b = rotated_point_b[index];
426 body_id.x = sbar_a.i;
427 body_id.y = sbar_b.i;
428 const quaternion& q_a = quat_a[index];
429 const quaternion& q_b = quat_b[index];
430
431 real3 U_A = Rotate(U, q_a);
432 T3 = Cross(U_A, sbar_a.v);
433 real3 U_B = Rotate(U, q_b);
434 T6 = Cross(U_B, sbar_b.v);
435
436 SetRow6Check(D_T, off + row * 1 + 0, body_id.x * 6, -U, T3);
437 SetRow6Check(D_T, off + row * 1 + 0, body_id.y * 6, U, -T6);
438
439 if (solver_mode == SolverMode::SLIDING || solver_mode == SolverMode::SPINNING) {
440 off = num_rigid_contacts;
441
442 real3 V_A = Rotate(V, q_a);
443 real3 W_A = Rotate(W, q_a);
444 T4 = Cross(V_A, sbar_a.v);
445 T5 = Cross(W_A, sbar_a.v);
446
447 real3 V_B = Rotate(V, q_b);
448 real3 W_B = Rotate(W, q_b);
449 T7 = Cross(V_B, sbar_b.v);
450 T8 = Cross(W_B, sbar_b.v);
451
452 SetRow6Check(D_T, off + row * 2 + 0, body_id.x * 6, -V, T4);
453 SetRow6Check(D_T, off + row * 2 + 1, body_id.x * 6, -W, T5);
454
455 SetRow6Check(D_T, off + row * 2 + 0, body_id.y * 6, V, -T7);
456 SetRow6Check(D_T, off + row * 2 + 1, body_id.y * 6, W, -T8);
457
458 if (solver_mode == SolverMode::SPINNING) {
459 off = 3 * num_rigid_contacts;
460
461 SetRow3Check(D_T, off + row * 3 + 0, body_id.x * 6 + 3, -U_A);
462 SetRow3Check(D_T, off + row * 3 + 1, body_id.x * 6 + 3, -V_A);
463 SetRow3Check(D_T, off + row * 3 + 2, body_id.x * 6 + 3, -W_A);
464
465 SetRow3Check(D_T, off + row * 3 + 0, body_id.y * 6 + 3, U_B);
466 SetRow3Check(D_T, off + row * 3 + 1, body_id.y * 6 + 3, V_B);
467 SetRow3Check(D_T, off + row * 3 + 2, body_id.y * 6 + 3, W_B);
468 }
469 }
470 }
471 }
472
GenerateSparsity()473 void ChConstraintRigidRigid::GenerateSparsity() {
474 LOG(INFO) << "ChConstraintRigidRigid::GenerateSparsity";
475 const auto num_rigid_contacts = data_manager->cd_data->num_rigid_contacts;
476 SolverMode solver_mode = data_manager->settings.solver.solver_mode;
477
478 CompressedMatrix<real>& D_T = data_manager->host_data.D_T;
479
480 const vec2* ids = data_manager->cd_data->bids_rigid_rigid.data();
481
482 for (int index = 0; index < (signed)num_rigid_contacts; index++) {
483 const vec2& body_id = ids[index];
484 int row = index;
485 int off = 0;
486
487 AppendRow6(D_T, off + row * 1, body_id.x * 6, 0);
488 AppendRow6(D_T, off + row * 1, body_id.y * 6, 0);
489
490 D_T.finalize(off + row * 1 + 0);
491 }
492
493 if (solver_mode == SolverMode::SLIDING || solver_mode == SolverMode::SPINNING) {
494 for (int index = 0; index < (signed)num_rigid_contacts; index++) {
495 const vec2& body_id = ids[index];
496 int row = index;
497 int off = num_rigid_contacts;
498
499 AppendRow6(D_T, off + row * 2 + 0, body_id.x * 6, 0);
500 AppendRow6(D_T, off + row * 2 + 0, body_id.y * 6, 0);
501
502 D_T.finalize(off + row * 2 + 0);
503
504 AppendRow6(D_T, off + row * 2 + 1, body_id.x * 6, 0);
505 AppendRow6(D_T, off + row * 2 + 1, body_id.y * 6, 0);
506
507 D_T.finalize(off + row * 2 + 1);
508 }
509 }
510
511 if (solver_mode == SolverMode::SPINNING) {
512 for (int index = 0; index < (signed)num_rigid_contacts; index++) {
513 const vec2& body_id = ids[index];
514 int row = index;
515 int off = 3 * num_rigid_contacts;
516 D_T.append(off + row * 3 + 0, body_id.x * 6 + 3, 0);
517 D_T.append(off + row * 3 + 0, body_id.x * 6 + 4, 0);
518 D_T.append(off + row * 3 + 0, body_id.x * 6 + 5, 0);
519
520 D_T.append(off + row * 3 + 0, body_id.y * 6 + 3, 0);
521 D_T.append(off + row * 3 + 0, body_id.y * 6 + 4, 0);
522 D_T.append(off + row * 3 + 0, body_id.y * 6 + 5, 0);
523
524 D_T.finalize(off + row * 3 + 0);
525
526 D_T.append(off + row * 3 + 1, body_id.x * 6 + 3, 0);
527 D_T.append(off + row * 3 + 1, body_id.x * 6 + 4, 0);
528 D_T.append(off + row * 3 + 1, body_id.x * 6 + 5, 0);
529
530 D_T.append(off + row * 3 + 1, body_id.y * 6 + 3, 0);
531 D_T.append(off + row * 3 + 1, body_id.y * 6 + 4, 0);
532 D_T.append(off + row * 3 + 1, body_id.y * 6 + 5, 0);
533
534 D_T.finalize(off + row * 3 + 1);
535
536 D_T.append(off + row * 3 + 2, body_id.x * 6 + 3, 0);
537 D_T.append(off + row * 3 + 2, body_id.x * 6 + 4, 0);
538 D_T.append(off + row * 3 + 2, body_id.x * 6 + 5, 0);
539
540 D_T.append(off + row * 3 + 2, body_id.y * 6 + 3, 0);
541 D_T.append(off + row * 3 + 2, body_id.y * 6 + 4, 0);
542 D_T.append(off + row * 3 + 2, body_id.y * 6 + 5, 0);
543
544 D_T.finalize(off + row * 3 + 2);
545 }
546 }
547 }
548
Dx(const DynamicVector<real> & gam,DynamicVector<real> & XYZUVW)549 void ChConstraintRigidRigid::Dx(const DynamicVector<real>& gam, DynamicVector<real>& XYZUVW) {
550 const auto num_rigid_contacts = data_manager->cd_data->num_rigid_contacts;
551 real3* norm = data_manager->cd_data->norm_rigid_rigid.data();
552 ////custom_vector<char>& active_rigid = data_manager->host_data.active_rigid;
553 ////vec2* bids_rigid_rigid = data_manager->host_data.bids_rigid_rigid.data();
554
555 #pragma omp parallel for
556 for (int i = 0; i < (signed)num_rigid_contacts; i++) {
557 const real3& U = real3(norm[i]);
558 real3 V, W;
559 Orthogonalize(U, V, W);
560 ////int id_a = bids_rigid_rigid[i].x;
561 ////int id_b = bids_rigid_rigid[i].y;
562
563 real3 T3, T4, T5, T6, T7, T8;
564 real3 g = real3(gam[i], gam[num_rigid_contacts + i * 2 + 0], gam[num_rigid_contacts + i * 2 + 1]);
565 {
566 const real3_int& sbar = rotated_point_a[i];
567 const quaternion& q_a = quat_a[i];
568 T3 = Cross(Rotate(U, q_a), sbar.v);
569 T4 = Cross(Rotate(V, q_a), sbar.v);
570 T5 = Cross(Rotate(W, q_a), sbar.v);
571
572 ////if (active_rigid[id_a] != 0)
573
574 real3 res = -U * g.x - V * g.y - W * g.z;
575
576 #pragma omp atomic
577 XYZUVW[sbar.i * 6 + 0] += res.x;
578 #pragma omp atomic
579 XYZUVW[sbar.i * 6 + 1] += res.y;
580 #pragma omp atomic
581 XYZUVW[sbar.i * 6 + 2] += res.z;
582
583 res = T3 * g.x + T4 * g.y + T5 * g.z;
584
585 #pragma omp atomic
586 XYZUVW[sbar.i * 6 + 3] += res.x;
587 #pragma omp atomic
588 XYZUVW[sbar.i * 6 + 4] += res.y;
589 #pragma omp atomic
590 XYZUVW[sbar.i * 6 + 5] += res.z;
591 }
592
593 {
594 const real3_int& sbar = rotated_point_b[i];
595 const quaternion& q_b = quat_b[i];
596 T6 = Cross(Rotate(U, q_b), sbar.v);
597 T7 = Cross(Rotate(V, q_b), sbar.v);
598 T8 = Cross(Rotate(W, q_b), sbar.v);
599
600 // if (active_rigid[id_b] != 0)
601
602 real3 res = U * g.x + V * g.y + W * g.z;
603
604 #pragma omp atomic
605 XYZUVW[sbar.i * 6 + 0] += res.x;
606 #pragma omp atomic
607 XYZUVW[sbar.i * 6 + 1] += res.y;
608 #pragma omp atomic
609 XYZUVW[sbar.i * 6 + 2] += res.z;
610
611 res = -T6 * g.x - T7 * g.y - T8 * g.z;
612
613 #pragma omp atomic
614 XYZUVW[sbar.i * 6 + 3] += res.x;
615 #pragma omp atomic
616 XYZUVW[sbar.i * 6 + 4] += res.y;
617 #pragma omp atomic
618 XYZUVW[sbar.i * 6 + 5] += res.z;
619 }
620 }
621 }
622
D_Tx(const DynamicVector<real> & XYZUVW,DynamicVector<real> & out_vector)623 void ChConstraintRigidRigid::D_Tx(const DynamicVector<real>& XYZUVW, DynamicVector<real>& out_vector) {
624 const auto num_rigid_contacts = data_manager->cd_data->num_rigid_contacts;
625 real3* norm = data_manager->cd_data->norm_rigid_rigid.data();
626 ////custom_vector<char>& active_rigid = data_manager->host_data.active_rigid;
627 ////real3* ptA = data_manager->cd_data->cpta_rigid_rigid.data();
628 ////real3* ptB = data_manager->cd_data->cptb_rigid_rigid.data();
629 ////vec2* bids_rigid_rigid = data_manager->cd_data->bids_rigid_rigid.data();
630 ////real3* pos = data_manager->host_data.pos_rigid.data();
631 ////quaternion* rot = data_manager->host_data.rot_rigid.data();
632
633 #pragma omp parallel for
634 for (int i = 0; i < (signed)num_rigid_contacts; i++) {
635 const real3& U = real3(norm[i]);
636 real3 V, W;
637 Orthogonalize(U, V, W);
638 real temp[3] = {0, 0, 0};
639
640 ////real3 pA = ptA[i];
641 ////int id_a = bids_rigid_rigid[i].x;
642 ////if (active_rigid[id_a] != 0)
643 {
644 const real3_int& sbar = rotated_point_a[i];
645 const quaternion& quaternion_conjugate = quat_a[i];
646
647 real3 XYZ(XYZUVW[sbar.i * 6 + 0], XYZUVW[sbar.i * 6 + 1], XYZUVW[sbar.i * 6 + 2]);
648 real3 UVW(XYZUVW[sbar.i * 6 + 3], XYZUVW[sbar.i * 6 + 4], XYZUVW[sbar.i * 6 + 5]);
649
650 real3 T1 = Cross(Rotate(U, quaternion_conjugate), sbar.v);
651 real3 T2 = Cross(Rotate(V, quaternion_conjugate), sbar.v);
652 real3 T3 = Cross(Rotate(W, quaternion_conjugate), sbar.v);
653
654 temp[0] = Dot(XYZ, -U) + Dot(UVW, T1);
655 temp[1] = Dot(XYZ, -V) + Dot(UVW, T2);
656 temp[2] = Dot(XYZ, -W) + Dot(UVW, T3);
657
658 ////Jacobian<false>(rot[id_a], U, V, W, ptA[i] - pos[id_a], &XYZUVW[id_a * 6 + 0], temp);
659 }
660
661 ////real3 pB = ptB[i];
662 ////int id_b = bids_rigid_rigid[i].y;
663 ////if (active_rigid[id_b] != 0)
664 {
665 const real3_int& sbar = rotated_point_b[i];
666 const quaternion& quaternion_conjugate = quat_b[i];
667
668 real3 XYZ(XYZUVW[sbar.i * 6 + 0], XYZUVW[sbar.i * 6 + 1], XYZUVW[sbar.i * 6 + 2]);
669 real3 UVW(XYZUVW[sbar.i * 6 + 3], XYZUVW[sbar.i * 6 + 4], XYZUVW[sbar.i * 6 + 5]);
670
671 real3 T1 = Cross(Rotate(U, quaternion_conjugate), sbar.v);
672 real3 T2 = Cross(Rotate(V, quaternion_conjugate), sbar.v);
673 real3 T3 = Cross(Rotate(W, quaternion_conjugate), sbar.v);
674
675 temp[0] += Dot(XYZ, U) + Dot(UVW, -T1);
676 temp[1] += Dot(XYZ, V) + Dot(UVW, -T2);
677 temp[2] += Dot(XYZ, W) + Dot(UVW, -T3);
678 }
679
680 out_vector[i] = temp[0];
681 out_vector[num_rigid_contacts + i * 2 + 0] = temp[1];
682 out_vector[num_rigid_contacts + i * 2 + 1] = temp[2];
683
684 // out_vector[CONTACT + 3] = temp[3];
685 // out_vector[CONTACT + 4] = temp[4];
686 // out_vector[CONTACT + 5] = temp[5];
687 }
688
689 // // data_manager->PrintMatrix(data_manager->host_data.D);
690
691 // DynamicVector<real> compare =
692 // data_manager->host_data.D_T * data_manager->host_data.D * data_manager->host_data.gamma;
693 // std::cout << "nconstr " << compare.size() << std::endl;
694 // for (int i = 0; i < compare.size(); i++) {
695 // std::cout << compare[i] << " " << out_vector[i] << std::endl;
696 // }
697 }
698