PeriDyno 1.0.0
Loading...
Searching...
No Matches
ContactRule.cu
Go to the documentation of this file.
1#include "ContactRule.h"
2
3#include "Primitive/Primitive3D.h"
4#include "Topology/SparseOctree.h"
5
6#include "CCD/AdditiveCCD.h"
7#include "CCD/TightCCD.h"
8#include "Vector.h"
9#define eps (2e-1)
10namespace dyno
11{
12 IMPLEMENT_TCLASS(ContactRule, TDataType)
13
14 template<typename TDataType>
15 ContactRule<TDataType>::ContactRule()
16 : ConstraintModule()
17 {
18 mBroadPhaseCD = std::make_shared<CollisionDetectionBroadPhase<TDataType>>();
19 }
20
21 template<typename TDataType>
22 ContactRule<TDataType>::~ContactRule()
23 {
24 firstTri.clear();
25 secondTri.clear();
26 trueContact.clear();
27 trueContactCnt.clear();
28 Weight.clear();
29 }
30
31 template<typename Real, typename Coord, typename Triangle>
32 __global__ void CR_SetupAABB(
33 DArray<AABB> boundingBox,
34 DArray<Coord> oldVertices,
35 DArray<Coord> newVertices,
36 DArray<Triangle> triangles,
37 Real epsilon) //This will expend the AABB epsilon time of its length. For the shrinking, please use a negative parameter.
38 {
39 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
40 if (tId >= boundingBox.size()) return;
41
42 Triangle t = triangles[tId];
43 Coord corner1;
44 corner1[0] = epsilon; corner1[1] = epsilon; corner1[2] = epsilon;
45
46 Coord v0 = newVertices[t[0]];
47 Coord v1 = newVertices[t[1]];
48 Coord v2 = newVertices[t[2]];
49
50 Coord s0 = oldVertices[t[0]];
51 Coord s1 = oldVertices[t[1]];
52 Coord s2 = oldVertices[t[2]];
53
54 AABB box;
55 box.v0 = v0;
56 box.v1 = v0;
57
58 box.v0 = minimum(box.v0, v1);
59 box.v1 = maximum(box.v1, v1);
60
61 box.v0 = minimum(box.v0, v2);
62 box.v1 = maximum(box.v1, v2);
63
64 box.v0 = minimum(box.v0, s0);
65 box.v1 = maximum(box.v1, s0);
66
67 box.v0 = minimum(box.v0, s1);
68 box.v1 = maximum(box.v1, s1);
69
70 box.v0 = minimum(box.v0, s2);
71 box.v1 = maximum(box.v1, s2);
72
73 Real len = (box.v0 - box.v1).norm();
74 //expend epsilon
75 box.v0 += corner1 * len;
76 box.v1 += corner1 * len;
77
78 boundingBox[tId] = box;
79 }
80
81 template<typename Triangle>
82 __device__ bool isTwoTriangleNeighoring(Triangle t0, Triangle t1)
83 {
84 for (int i = 0; i < 3; i++)
85 {
86 for (int j = 0; j < 3; j++)
87 {
88 if (t0[i] == t1[j])
89 return true;
90 }
91 }
92
93 return false;
94 }
95
96
97 template<typename Coord, typename Triangle, typename Real>
98 __global__ void CR_Calculate_Timestep(
99 DArray<Real> timestep,
100 DArray<Coord> vertexOld,
101 DArray<Coord> vertexNew,
102 DArray<Triangle> triangles,
103 DArrayList<int> contactList,
104 Real thickness,
105 Real collisionRefactor,
106 DArrayList<int> trueContact,
107 DArray<uint> trueContactCnt)
108 {
109
110 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
111 if (tId >= timestep.size()) return;
112
113 Triangle t_i = triangles[tId];
114 List<int>& list_i = contactList[tId]; // vertex's in triangle list
115 int nbSize = list_i.size();
116 Real time = Real(1.0);
117
118 trueContactCnt[tId] = 0;
119
120
121 TTriangle3D<Real> tri_old_i(vertexOld[t_i[0]], vertexOld[t_i[1]], vertexOld[t_i[2]]);
122 TTriangle3D<Real> tri_new_i(vertexNew[t_i[0]], vertexNew[t_i[1]], vertexNew[t_i[2]]);
123
124 List<int>& tContact = trueContact[tId];
125
126 for (int n = 0; n < nbSize; n++)
127 {
128 int j = list_i[n];
129 Triangle t_j = triangles[j];
130
131 if (!isTwoTriangleNeighoring(t_i, t_j))
132 {
133 TTriangle3D<Real> tri_old_j(vertexOld[t_j[0]], vertexOld[t_j[1]], vertexOld[t_j[2]]);
134 TTriangle3D<Real> tri_new_j(vertexNew[t_j[0]], vertexNew[t_j[1]], vertexNew[t_j[2]]);
135
136 Real toi = Real(1.0);
137
138 auto ccdPhase = AdditiveCCD<Real>(thickness, collisionRefactor, 0.95);
139 bool collided = ccdPhase.TriangleCCD(tri_old_i, tri_new_i, tri_old_j, tri_new_j, toi);
140
141 time = collided ? minimum(time, toi) : time;
142 if (collided) {
143 for (int index = 0; index < 3; ++index) {
144 tContact.insert(j);
145 trueContactCnt[tId] += 1;
146 }
147 }
148 }
149 }
150
151 timestep[tId] = time;
152 //if(time<1.0 && time>EPSILON)printf("contact %f\n",time);
153
154 }
155
156
157 template<typename Real>
158 __global__ void CR_Calculate_Timestep(
159 DArray<Real> timestepOfVertex, //timestep for each vertex
160 DArray<Real> timestepOfTriangle,
161 DArrayList<int> ver2tri)
162 {
163 int vId = threadIdx.x + (blockIdx.x * blockDim.x);
164 if (vId >= timestepOfVertex.size()) return;
165
166 List<int>& list_i = ver2tri[vId];
167 int nbSize = list_i.size();
168
169 Real time = Real(1);
170 for (int ne = 0; ne < nbSize; ne++)
171 {
172 int j = list_i[ne];
173
174 time = minimum(time, timestepOfTriangle[j]);
175 }
176
177 timestepOfVertex[vId] = time;
178
179 }
180
181
182
183 template<typename Real, typename Coord>
184 __global__ void CR_Update_Vertex(
185 DArray<Coord> vertexNew,
186 DArray<Coord> vertexOld,
187 DArray<Real> timestep) //timestep for each triangle
188 {
189 int vId = threadIdx.x + (blockIdx.x * blockDim.x);
190 if (vId >= timestep.size()) return;
191
192 Coord vOld = vertexOld[vId];
193 Coord vNew = vertexNew[vId];
194
195 Real timestep_i = timestep[vId];
196
197 vertexNew[vId] = vOld + (vNew - vOld) * timestep_i;
198
199 }
200
201
202template<typename Coord, typename Real>
203__global__ void CR_Init_Force_Weight(
204 DArray<Coord> force,
205 DArray<Real> Weight) {
206 int vId = threadIdx.x + (blockIdx.x * blockDim.x);
207 if (vId >= force.size()) return;
208
209 force[vId] = Coord(0);
210 Weight[vId] = (Real)0.0;
211
212}
213
214
215template<typename Coord, typename Triangle>
216__global__ void CR_Update_Distance(
217 DArrayList<int> ContactList,
218 DArrayList<Coord> firstTri,
219 DArrayList<Coord> secondTri,
220 DArray<Triangle> triangles,
221 DArray<Coord> vertexNew) {
222 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
223 if (tId >= ContactList.size()) return;
224
225 List<int>& list_i = ContactList[tId];
226 Triangle t_i = triangles[tId];
227 TTriangle3D<Real> tri_i(vertexNew[t_i[0]], vertexNew[t_i[1]], vertexNew[t_i[2]]);
228 int nbSize = list_i.size();
229 auto ccdPhase = AdditiveCCD<Real>();
230 List<Coord>& fi = firstTri[tId];
231 List<Coord>& se = secondTri[tId];
232 for (int ne = 0; ne < nbSize; ++ne) {
233 int j = list_i[ne];
234 Triangle t_j = triangles[j];
235 TTriangle3D<Real> tri_j(vertexNew[t_j[0]], vertexNew[t_j[1]], vertexNew[t_j[2]]);
236 Vector<Real, 3> firstT, secondT;
237 ccdPhase.projectClosePoint(tri_i, tri_j, firstT, secondT);
238 Coord f, s;
239 f[0] = firstT[0], f[1] = firstT[1], f[2] = firstT[2];
240 s[0] = secondT[0], s[1] = secondT[1], s[2] = secondT[2];
241
242 fi.insert(f);
243 se.insert(s);
244 }
245}
246
247template<typename Coord, typename Real, typename Triangle>
248__global__ void CR_UpDate_Contact_Force(
249 DArrayList<int> ContactList,
250 DArrayList<Coord> firstT,
251 DArrayList<Coord> secondT,
252 DArray<Coord> contactForce,
253 Real d,
254 Real xi,
255 DArray<Triangle> triangles,
256 DArray<Coord> vertexNew,
257 DArray<Real> Weight) {
258 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
259 if (tId >= firstT.size()) return;
260
261 auto myNorm = [](Coord v0, Coord v1) {
262 return sqrt(pow(v0[0] - v1[0], 2) + pow(v0[1] - v1[1], 2) + pow(v0[2] - v1[2], 2));
263 };
264 auto calcN = [&](Coord v, const Triangle& t_i, const Triangle& t_j) {
265 Coord e1 = vertexNew[t_i[0]] / d - vertexNew[t_i[1]] / d;
266 Coord e2 = vertexNew[t_i[0]] / d - vertexNew[t_i[2]] / d;
267 Coord n = e1.cross(e2);
268 Real nL = sqrt(pow(n[0], 2) + pow(n[1], 2) + pow(n[2], 2));
269 n /= nL;
270 Coord vj_b = v - 1.0 / 3.0 * (vertexNew[t_j[0]] + vertexNew[t_j[1]] + vertexNew[t_j[2]]);
271 Real Dot = n[0] * vj_b[0] + n[1] * vj_b[1] + n[2] * vj_b[2];
272 n *= Dot <= 0.0 ? -1.0 : 1.0;
273 return n;
274 };
275
276 auto refactor_n = [&](Real n) {
277 if (n >= (Real)((1.0 + 2.0 * eps) * xi))
278 return n;
279 if (n <= xi)
280 {
281 Real n_re = Real((1.0 + 1.0 * eps) * xi);
282 return n_re;
283 }
284 Real a = Real(1.0 / (4.0 * eps * xi));
285 Real n_re =Real( a * pow(n - xi, 2) + (1.0 + 1.0 * eps) * xi);
286 return n_re;
287 };
288
289 auto contactforce = [&](Coord v1, Coord v2, const Triangle& t_i, const Triangle& t_j, Coord vi_c, Coord vj_c) {
290 Coord force = v1 * 0.0;
291 Real n = myNorm(v1, v2);
292 if (n <= (Real)(1.0+2*eps) * xi) {
293 for (int index_tri = 0; index_tri < 3; ++index_tri) {
294 atomicAdd(&Weight[t_i[index_tri]], vi_c[index_tri]);
295 }
296 }
297 n = refactor_n(n);
298 Coord N = (v1 - v2) / myNorm(v1, v2);
299 force = (pow((n - xi) - d, 2) / (n - xi) + log((n - xi) / d) * 2 * (n - xi - d)) * N;
300 return force;
301 };
302
303 auto contactforceN = [&](Coord v1, Coord v2, Coord N, const Triangle& t_i, const Triangle& t_j, Coord vi_c, Coord vj_c) {
304 Coord force = v1 * 0.0;
305 Real n = myNorm(v1, v2);
306 if (n <= (Real)(1.0 + 2*eps) * xi) {
307 for (int index_tri = 0; index_tri < 3; ++index_tri) {
308 atomicAdd(&Weight[t_i[index_tri]], vi_c[index_tri]);
309 }
310 }
311 n = refactor_n(n);
312 force = (pow(n - xi - d, 2) / (n - xi) + log((n - xi) / d) * 2.0 * (n - xi - d)) * N;
313 return force;
314 };
315
316 List<int>& list_i = ContactList[tId];
317 List<Coord>& tri_coordinate_i = firstT[tId];
318 List<Coord>& tri_coordinate_j = secondT[tId];
319 Triangle t_i = triangles[tId];
320 int nbSize = list_i.size();
321
322 for (int ne = 0; ne < nbSize; ne++) {
323
324 int j = list_i[ne];
325 Triangle t_j = triangles[j];
326 Coord vi_N = tri_coordinate_i[ne][0] * vertexNew[t_i[0]] + tri_coordinate_i[ne][1] * vertexNew[t_i[1]] + tri_coordinate_i[ne][2] * vertexNew[t_i[2]];
327 Coord vj_N = tri_coordinate_j[ne][0] * vertexNew[t_j[0]] + tri_coordinate_j[ne][1] * vertexNew[t_j[1]] + tri_coordinate_j[ne][2] * vertexNew[t_j[2]];
328 Real disN = myNorm(vi_N, vj_N);
329
330 if (disN > d)
331 break;
332
333 vi_N = tri_coordinate_i[ne][0] * vertexNew[t_i[0]] + tri_coordinate_i[ne][1] * vertexNew[t_i[1]] + tri_coordinate_i[ne][2] * vertexNew[t_i[2]];
334 vj_N = tri_coordinate_j[ne][0] * vertexNew[t_j[0]] + tri_coordinate_j[ne][1] * vertexNew[t_j[1]] + tri_coordinate_j[ne][2] * vertexNew[t_j[2]];
335 Coord force;
336 if (myNorm(vi_N, vj_N) >= EPSILON)
337 force = contactforce(vi_N, vj_N,t_i,t_j, tri_coordinate_i[ne], tri_coordinate_j[ne]);
338 else
339 force = contactforceN(vi_N, vj_N, calcN(vi_N,t_i,t_j), t_i, t_j, tri_coordinate_i[ne], tri_coordinate_j[ne]);
340
341 for (int index_tri = 0; index_tri < 3; ++index_tri) {
342 for (int index_Coord = 0; index_Coord < 3; ++index_Coord) {
343 atomicAdd(&contactForce[t_i[index_tri]][index_Coord], force[index_Coord] * (tri_coordinate_i[ne][index_tri]));
344 }
345 }
346 }
347
348 }
349
350 template<typename Coord, typename Real>
351 __global__ void CR_Weighted_Force(
352 DArray<Real> Weight,
353 DArray<Coord> force,
354 DArray<Coord> Position,
355 DArray<Coord> OldPosition,
356 Real timestep) {
357 Real t = timestep;
358 int vId = threadIdx.x + (blockIdx.x * blockDim.x);
359 if (vId >= Weight.size()) return;
360 auto myNorm = [](Coord v0) {
361 return sqrt(pow(v0[0], 2) + pow(v0[1], 2) + pow(v0[2], 2));
362 };
363
364 if (myNorm(force[vId]) <= EPSILON)
365 return;
366 if (myNorm(force[vId]) > 1e16)
367 force[vId] *= 0.0;
368
369 Position[vId] += (force[vId] + OldPosition[vId]-Position[vId])* t;
370 }
371
372 __global__ void CR_Cnt(
373 DArrayList<int>cList,
374 DArray<uint> cnt ) {
375 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
376 if (tId >= cnt.size()) return;
377 cnt[tId] = cList[tId].size();
378 }
379
380 template<typename Coord, typename Real>
381 __global__ void CR_Force_Magnitude(
382 DArray<Real> force_m,
383 DArray<Coord> force,
384 DArray<Real> weight,
385 DArray<Coord> oldPosition,
386 DArray<Coord> newPosition) {
387 int vId = threadIdx.x + (blockIdx.x * blockDim.x);
388 if (vId >= force.size()) return;
389 auto myNorm = [](Coord v0) {
390 return sqrt(pow(v0[0], 2) + pow(v0[1], 2) + pow(v0[2], 2));
391 };
392 force_m[vId] = myNorm(force[vId]);
393 }
394
395
396 template<typename Coord, typename Real>
397 __global__ void CR_Cancle_Position(
398 DArray<Real> Weight,
399 DArray<Coord> Position,
400 DArray<Coord> OldPosition) {
401 int vId = threadIdx.x + (blockIdx.x * blockDim.x);
402 if (vId >= Weight.size()) return;
403 if(Weight[vId] - EPSILON>=0.0)
404 Position[vId] = OldPosition[vId];
405 }
406
407 template<typename TDataType>
408 void ContactRule<TDataType>::initCCDBroadPhase()
409 {
410 auto topo = this->inTriangularMesh()->getDataPtr();
411 int vNum = this->inOldPosition()->size();
412 auto& indices = topo->getTriangles();
413 auto& ver2tri = topo->getVertex2Triangles();
414
415 int tNum = indices.size();
416
417 DArray<AABB> aabb(tNum);
418 cuExecute(tNum,
419 CR_SetupAABB,
420 aabb,
421 this->inOldPosition()->getData(),
422 this->inNewPosition()->getData(),
423 indices,
424 this->varXi()->getData() * this->inUnit()->getData());
425
426 mBroadPhaseCD->varGridSizeLimit()->setValue(this->inUnit()->getData());
427 mBroadPhaseCD->inSource()->assign(aabb);
428 mBroadPhaseCD->inTarget()->assign(aabb);
429 mBroadPhaseCD->update();
430 auto& cList = mBroadPhaseCD->outContactList()->getData();
431
432 if (this->outContactForce()->isEmpty())
433 this->outContactForce()->allocate();
434 this->outContactForce()->getData().resize(vNum);
435
436 if (this->outWeight()->isEmpty())
437 this->outWeight()->allocate();
438 this->outWeight()->getData().resize(vNum);
439
440 cuExecute(vNum,
441 CR_Init_Force_Weight,
442 this->outContactForce()->getData(),
443 this->outWeight()->getData());
444
445 this->trueContactCnt.resize(tNum);
446
447 aabb.clear();
448 }
449
450
451
452 template<typename TDataType>
453 void ContactRule<TDataType>::constrain()
454 {
455
456 int vNum = this->inOldPosition()->size();
457 auto topo = this->inTriangularMesh()->getDataPtr();
458 auto& indices = topo->getTriangles();
459 int tNum = indices.size();
460 auto& ver2tri = topo->getVertex2Triangles();
461
462 auto& contactForce = this->outContactForce()->getData();
463 if (contactForce.size() != vNum) {
464 contactForce.resize(vNum);
465 if (this->outWeight()->isEmpty())
466 this->outWeight()->allocate();
467 this->outWeight()->getData().resize(vNum);
468 cuExecute(vNum,
469 CR_Init_Force_Weight,
470 this->outContactForce()->getData(),
471 this->outWeight()->getData());
472 }
473
474 auto& cList = mBroadPhaseCD->outContactList()->getData();
475
476 cuExecute(tNum,
477 CR_Cnt,
478 cList,
479 this->trueContactCnt);
480 cuSynchronize();
481
482 this->trueContact.resize(this->trueContactCnt);
483
484 DArray<Real> steplengthOfVertex(vNum);
485 DArray<Real> steplengthOfTriangle(tNum);
486 Real d_hat = Real((1.0+15*eps) * this->varXi()->getData() * this->inUnit()->getData());
487
488 cuExecute(tNum,
489 CR_Calculate_Timestep,
490 steplengthOfTriangle,
491 this->inOldPosition()->getData(),
492 this->inNewPosition()->getData(),
493 indices,
494 cList,
495 Real(this->varXi()->getValue() * this->inUnit()->getData()),
496 this->varS()->getData(),
497 this->trueContact,
498 this->trueContactCnt
499 );
500
501 cuExecute(vNum,
502 CR_Calculate_Timestep,
503 steplengthOfVertex,
504 steplengthOfTriangle,
505 ver2tri);
506 cuSynchronize();
507
508 cuExecute(vNum,
509 CR_Update_Vertex,
510 this->inNewPosition()->getData(),
511 this->inOldPosition()->getData(),
512 steplengthOfVertex);
513
514 cuSynchronize();
515 DArray<Coord> msBof;
516 msBof.resize(vNum);
517 msBof.assign(this->inNewPosition()->getData());
518 int ite = 0;
519 Real maxWeight = 0.0;
520 Real maxforce = 0.1;
521 Reduction<Real> reduce;
522 this->weight = -1;
523 bool status = true;
524 while(ite < this->ContactMaxIte && maxforce >= 1e-2){
525
526
527 this->secondTri.resize(this->trueContactCnt);
528 this->firstTri.resize(this->trueContactCnt);
529
530 cuExecute(tNum,
531 CR_Update_Distance,
532 this->trueContact,
533 this->firstTri,
534 this->secondTri,
535 indices,
536 this->inNewPosition()->getData());
537
538 cuExecute(vNum,
539 CR_Init_Force_Weight,
540 this->outContactForce()->getData(),
541 this->outWeight()->getData());
542
543 cuExecute(tNum,
544 CR_UpDate_Contact_Force,
545 this->trueContact,
546 this->firstTri,
547 this->secondTri,
548 this->outContactForce()->getData(),
549 d_hat,
550 (Real)(this->varXi()->getData() * this->inUnit()->getData()),
551 indices,
552 this->inNewPosition()->getData(),
553 this->outWeight()->getData());
554
555 cuSynchronize();
556 DArray<Real> force_m;
557 force_m.resize(vNum);
558 cuExecute(vNum,
559 CR_Force_Magnitude,
560 force_m,
561 this->outContactForce()->getData(),
562 this->outWeight()->getData(),
563 msBof,
564 this->inNewPosition()->getData());
565 cuSynchronize();
566
567 Real m = reduce.maximum(this->outWeight()->getData().begin(), this->outWeight()->getData().size());
568 maxWeight = maximum(m, maxWeight);
569 maxforce = reduce.maximum(force_m.begin(), force_m.size());
570 if (m <= EPSILON || maxforce <=1e-2)
571 {
572 printf("============== contact Rule: ite = %d ================\n", ite);
573 status = false;
574 break;
575 }
576
577 if (maxforce > 1e16)
578 maxforce = 1e16;
579
580 Real u_xi = Real(this->inUnit()->getData()* this->varXi()->getData());
581 Real max_f = pow(eps * u_xi - d_hat, 2) / (eps * u_xi) + log((eps * u_xi) / d_hat) * 2.0 * (eps * u_xi - d_hat);
582 this->weight = max_f;
583 Real timestep = minimum(Real( eps * u_xi/((maxWeight*max_f)+0.01)), Real(eps* u_xi / (maxforce)));
584 //printf("maxWeight ,maxforce,timestep %f %f %f\n", maxWeight, maxforce,timestep);
585 cuExecute(vNum,
586 CR_Weighted_Force,
587 this->outWeight()->getData(),
588 this->outContactForce()->getData(),
589 this->inNewPosition()->getData(),
590 msBof,
591 timestep);
592
593 ++ite;
594 cuSynchronize();
595 force_m.clear();
596 }
597
598 msBof.clear();
599 steplengthOfVertex.clear();
600 steplengthOfTriangle.clear();
601 if(status)
602 printf("========= contact Rule clamped with ite = %d =========\n", ite);
603 }
604
605 DEFINE_CLASS(ContactRule);
606}
Generated by doxygen 1.13.2