MUDA
Loading...
Searching...
No Matches
ccd.inl
1//ref: https://github.com/ipc-sim/Codim-IPC/tree/main/Library/Math/Distance
2#include <thrust/extrema.h>
3#include <thrust/swap.h>
4namespace muda::distance
5{
6template <typename T, int dim>
7MUDA_GENERIC bool point_edge_cd_broadphase(const Eigen::Matrix<T, dim, 1>& x0,
8 const Eigen::Matrix<T, dim, 1>& x1,
9 const Eigen::Matrix<T, dim, 1>& x2,
10 T dist)
11{
12 const Eigen::Array<T, dim, 1> max_e = x1.array().max(x2.array());
13 const Eigen::Array<T, dim, 1> min_e = x1.array().min(x2.array());
14 if((x0.array() - max_e > dist).any() || (min_e - x0.array() > dist).any())
15 {
16 return false;
17 }
18 else
19 {
20 return true;
21 }
22}
23
24template <typename T>
25MUDA_GENERIC bool point_edge_ccd_broadphase(const Eigen::Matrix<T, 2, 1>& p,
26 const Eigen::Matrix<T, 2, 1>& e0,
27 const Eigen::Matrix<T, 2, 1>& e1,
28 const Eigen::Matrix<T, 2, 1>& dp,
29 const Eigen::Matrix<T, 2, 1>& de0,
30 const Eigen::Matrix<T, 2, 1>& de1,
31 T dist)
32{
33 const Eigen::Array<T, 2, 1> max_p = p.array().max((p + dp).array());
34 const Eigen::Array<T, 2, 1> min_p = p.array().min((p + dp).array());
35 const Eigen::Array<T, 2, 1> max_e =
36 e0.array().max(e1.array()).max((e0 + de0).array()).max((e1 + de1).array());
37 const Eigen::Array<T, 2, 1> min_e =
38 e0.array().min(e1.array()).min((e0 + de0).array()).min((e1 + de1).array());
39 if((min_p - max_e > dist).any() || (min_e - max_p > dist).any())
40 {
41 return false;
42 }
43 else
44 {
45 return true;
46 }
47}
48
49template <typename T>
50MUDA_GENERIC bool point_triangle_cd_broadphase(const Eigen::Matrix<T, 3, 1>& p,
51 const Eigen::Matrix<T, 3, 1>& t0,
52 const Eigen::Matrix<T, 3, 1>& t1,
53 const Eigen::Matrix<T, 3, 1>& t2,
54 T dist)
55{
56 const Eigen::Array<T, 3, 1> max_tri = t0.array().max(t1.array()).max(t2.array());
57 const Eigen::Array<T, 3, 1> min_tri = t0.array().min(t1.array()).min(t2.array());
58 if((p.array() - max_tri > dist).any() || (min_tri - p.array() > dist).any())
59 {
60 return false;
61 }
62 else
63 {
64 return true;
65 }
66}
67
68template <typename T>
69MUDA_GENERIC bool edge_edge_cd_broadphase(const Eigen::Matrix<T, 3, 1>& ea0,
70 const Eigen::Matrix<T, 3, 1>& ea1,
71 const Eigen::Matrix<T, 3, 1>& eb0,
72 const Eigen::Matrix<T, 3, 1>& eb1,
73 T dist)
74{
75 const Eigen::Array<T, 3, 1> max_a = ea0.array().max(ea1.array());
76 const Eigen::Array<T, 3, 1> min_a = ea0.array().min(ea1.array());
77 const Eigen::Array<T, 3, 1> max_b = eb0.array().max(eb1.array());
78 const Eigen::Array<T, 3, 1> min_b = eb0.array().min(eb1.array());
79 if((min_a - max_b > dist).any() || (min_b - max_a > dist).any())
80 {
81 return false;
82 }
83 else
84 {
85 return true;
86 }
87}
88
89template <typename T>
90MUDA_GENERIC bool point_triangle_ccd_broadphase(const Eigen::Matrix<T, 3, 1>& p,
91 const Eigen::Matrix<T, 3, 1>& t0,
92 const Eigen::Matrix<T, 3, 1>& t1,
93 const Eigen::Matrix<T, 3, 1>& t2,
94 const Eigen::Matrix<T, 3, 1>& dp,
95 const Eigen::Matrix<T, 3, 1>& dt0,
96 const Eigen::Matrix<T, 3, 1>& dt1,
97 const Eigen::Matrix<T, 3, 1>& dt2,
98 T dist)
99{
100 const Eigen::Array<T, 3, 1> max_p = p.array().max((p + dp).array());
101 const Eigen::Array<T, 3, 1> min_p = p.array().min((p + dp).array());
102 const Eigen::Array<T, 3, 1> max_tri = t0.array()
103 .max(t1.array())
104 .max(t2.array())
105 .max((t0 + dt0).array())
106 .max((t1 + dt1).array())
107 .max((t2 + dt2).array());
108 const Eigen::Array<T, 3, 1> min_tri = t0.array()
109 .min(t1.array())
110 .min(t2.array())
111 .min((t0 + dt0).array())
112 .min((t1 + dt1).array())
113 .min((t2 + dt2).array());
114 if((min_p - max_tri > dist).any() || (min_tri - max_p > dist).any())
115 {
116 return false;
117 }
118 else
119 {
120 return true;
121 }
122}
123
124template <typename T>
125MUDA_GENERIC bool edge_edge_ccd_broadphase(const Eigen::Matrix<T, 3, 1>& ea0,
126 const Eigen::Matrix<T, 3, 1>& ea1,
127 const Eigen::Matrix<T, 3, 1>& eb0,
128 const Eigen::Matrix<T, 3, 1>& eb1,
129 const Eigen::Matrix<T, 3, 1>& dea0,
130 const Eigen::Matrix<T, 3, 1>& dea1,
131 const Eigen::Matrix<T, 3, 1>& deb0,
132 const Eigen::Matrix<T, 3, 1>& deb1,
133 T dist)
134{
135 const Eigen::Array<T, 3, 1> max_a =
136 ea0.array().max(ea1.array()).max((ea0 + dea0).array()).max((ea1 + dea1).array());
137 const Eigen::Array<T, 3, 1> min_a =
138 ea0.array().min(ea1.array()).min((ea0 + dea0).array()).min((ea1 + dea1).array());
139 const Eigen::Array<T, 3, 1> max_b =
140 eb0.array().max(eb1.array()).max((eb0 + deb0).array()).max((eb1 + deb1).array());
141 const Eigen::Array<T, 3, 1> min_b =
142 eb0.array().min(eb1.array()).min((eb0 + deb0).array()).min((eb1 + deb1).array());
143 if((min_a - max_b > dist).any() || (min_b - max_a > dist).any())
144 {
145 return false;
146 }
147 else
148 {
149 return true;
150 }
151}
152
153template <typename T>
154MUDA_GENERIC bool point_edge_ccd_broadphase(const Eigen::Matrix<T, 3, 1>& p,
155 const Eigen::Matrix<T, 3, 1>& e0,
156 const Eigen::Matrix<T, 3, 1>& e1,
157 const Eigen::Matrix<T, 3, 1>& dp,
158 const Eigen::Matrix<T, 3, 1>& de0,
159 const Eigen::Matrix<T, 3, 1>& de1,
160 T dist)
161{
162 const Eigen::Array<T, 3, 1> max_p = p.array().max((p + dp).array());
163 const Eigen::Array<T, 3, 1> min_p = p.array().min((p + dp).array());
164 const Eigen::Array<T, 3, 1> max_e =
165 e0.array().max(e1.array()).max((e0 + de0).array()).max((e1 + de1).array());
166 const Eigen::Array<T, 3, 1> min_e =
167 e0.array().min(e1.array()).min((e0 + de0).array()).min((e1 + de1).array());
168 if((min_p - max_e > dist).any() || (min_e - max_p > dist).any())
169 {
170 return false;
171 }
172 else
173 {
174 return true;
175 }
176}
177
178template <typename T>
179MUDA_GENERIC bool point_point_ccd_broadphase(const Eigen::Matrix<T, 3, 1>& p0,
180 const Eigen::Matrix<T, 3, 1>& p1,
181 const Eigen::Matrix<T, 3, 1>& dp0,
182 const Eigen::Matrix<T, 3, 1>& dp1,
183 T dist)
184{
185 const Eigen::Array<T, 3, 1> max_p0 = p0.array().max((p0 + dp0).array());
186 const Eigen::Array<T, 3, 1> min_p0 = p0.array().min((p0 + dp0).array());
187 const Eigen::Array<T, 3, 1> max_p1 = p1.array().max((p1 + dp1).array());
188 const Eigen::Array<T, 3, 1> min_p1 = p1.array().min((p1 + dp1).array());
189 if((min_p0 - max_p1 > dist).any() || (min_p1 - max_p0 > dist).any())
190 {
191 return false;
192 }
193 else
194 {
195 return true;
196 }
197}
198
199template <typename T>
200MUDA_GENERIC bool point_triangle_ccd(Eigen::Matrix<T, 3, 1> p,
201 Eigen::Matrix<T, 3, 1> t0,
202 Eigen::Matrix<T, 3, 1> t1,
203 Eigen::Matrix<T, 3, 1> t2,
204 Eigen::Matrix<T, 3, 1> dp,
205 Eigen::Matrix<T, 3, 1> dt0,
206 Eigen::Matrix<T, 3, 1> dt1,
207 Eigen::Matrix<T, 3, 1> dt2,
208 T eta,
209 T thickness,
210 int max_iter,
211 T& toc)
212{
213 Eigen::Matrix<T, 3, 1> mov = (dt0 + dt1 + dt2 + dp) / 4;
214 dt0 -= mov;
215 dt1 -= mov;
216 dt2 -= mov;
217 dp -= mov;
218 Eigen::Array3<T> dispMag2Vec{dt0.squaredNorm(), dt1.squaredNorm(), dt2.squaredNorm()};
219 T maxDispMag = dp.norm() + sqrt(dispMag2Vec.maxCoeff());
220
221 if(maxDispMag <= T(0))
222 {
223 return false;
224 }
225
226 T dist2_cur;
227 point_triangle_distance_unclassified(p, t0, t1, t2, dist2_cur);
228 T dist_cur = sqrt(dist2_cur);
229 T gap = eta * (dist2_cur - thickness * thickness) / (dist_cur + thickness);
230 T toc_prev = toc;
231 toc = 0;
232 while(true)
233 {
234 if(max_iter >= 0)
235 {
236 if(--max_iter < 0)
237 return true;
238 }
239
240 T tocLowerBound = (1 - eta) * (dist2_cur - thickness * thickness)
241 / ((dist_cur + thickness) * maxDispMag);
242
243 p += tocLowerBound * dp;
244 t0 += tocLowerBound * dt0;
245 t1 += tocLowerBound * dt1;
246 t2 += tocLowerBound * dt2;
247 point_triangle_distance_unclassified(p, t0, t1, t2, dist2_cur);
248 dist_cur = sqrt(dist2_cur);
249 if(toc && ((dist2_cur - thickness * thickness) / (dist_cur + thickness) < gap))
250 {
251 break;
252 }
253
254 toc += tocLowerBound;
255 if(toc > toc_prev)
256 {
257 return false;
258 }
259 }
260
261 return true;
262}
263
264template <typename T>
265MUDA_GENERIC bool edge_edge_ccd(Eigen::Matrix<T, 3, 1> ea0,
266 Eigen::Matrix<T, 3, 1> ea1,
267 Eigen::Matrix<T, 3, 1> eb0,
268 Eigen::Matrix<T, 3, 1> eb1,
269 Eigen::Matrix<T, 3, 1> dea0,
270 Eigen::Matrix<T, 3, 1> dea1,
271 Eigen::Matrix<T, 3, 1> deb0,
272 Eigen::Matrix<T, 3, 1> deb1,
273 T eta,
274 T thickness,
275 int max_iter,
276 T& toc)
277{
278 Eigen::Matrix<T, 3, 1> mov = (dea0 + dea1 + deb0 + deb1) / 4;
279 dea0 -= mov;
280 dea1 -= mov;
281 deb0 -= mov;
282 deb1 -= mov;
283 T maxDispMag = sqrt(std::max(dea0.squaredNorm(), dea1.squaredNorm()))
284 + sqrt(std::max(deb0.squaredNorm(), deb1.squaredNorm()));
285 if(maxDispMag == 0)
286 {
287 return false;
288 }
289
290 T dist2_cur;
291 edge_edge_distance_unclassified(ea0, ea1, eb0, eb1, dist2_cur);
292 T dFunc = dist2_cur - thickness * thickness;
293 if(dFunc <= 0)
294 {
295 // since we ensured other place that all dist smaller than dHat are positive,
296 // this must be some far away nearly parallel edges
297 Eigen::Array4<T> dists{(ea0 - eb0).squaredNorm(),
298 (ea0 - eb1).squaredNorm(),
299 (ea1 - eb0).squaredNorm(),
300 (ea1 - eb1).squaredNorm()};
301 dist2_cur = dists.minCoeff();
302 dFunc = dist2_cur - thickness * thickness;
303 }
304 T dist_cur = sqrt(dist2_cur);
305 T gap = eta * dFunc / (dist_cur + thickness);
306 T toc_prev = toc;
307 toc = 0;
308 while(true)
309 {
310 if(max_iter >= 0)
311 {
312 if(--max_iter < 0)
313 return true;
314 }
315
316 T tocLowerBound = (1 - eta) * dFunc / ((dist_cur + thickness) * maxDispMag);
317
318 ea0 += tocLowerBound * dea0;
319 ea1 += tocLowerBound * dea1;
320 eb0 += tocLowerBound * deb0;
321 eb1 += tocLowerBound * deb1;
322 edge_edge_distance_unclassified(ea0, ea1, eb0, eb1, dist2_cur);
323 dFunc = dist2_cur - thickness * thickness;
324 if(dFunc <= 0)
325 {
326 // since we ensured other place that all dist smaller than dHat are positive,
327 // this must be some far away nearly parallel edges
328 Eigen::Array4<T> dists{(ea0 - eb0).squaredNorm(),
329 (ea0 - eb1).squaredNorm(),
330 (ea1 - eb0).squaredNorm(),
331 (ea1 - eb1).squaredNorm()};
332 dist2_cur = dists.minCoeff();
333 dFunc = dist2_cur - thickness * thickness;
334 }
335 dist_cur = sqrt(dist2_cur);
336 if(toc && (dFunc / (dist_cur + thickness) < gap))
337 {
338 break;
339 }
340
341 toc += tocLowerBound;
342 if(toc > toc_prev)
343 {
344 return false;
345 }
346 }
347
348 return true;
349}
350
351template <typename T>
352MUDA_GENERIC bool point_edge_ccd(const Eigen::Matrix<T, 2, 1>& x0,
353 const Eigen::Matrix<T, 2, 1>& x1,
354 const Eigen::Matrix<T, 2, 1>& x2,
355 const Eigen::Matrix<T, 2, 1>& d0,
356 const Eigen::Matrix<T, 2, 1>& d1,
357 const Eigen::Matrix<T, 2, 1>& d2,
358 T eta,
359 T& toc)
360{
361 T a = d0[0] * (d2[1] - d1[1]) + d0[1] * (d1[0] - d2[0]) + d2[0] * d1[1]
362 - d2[1] * d1[0];
363 T b = x0[0] * (d2[1] - d1[1]) + d0[0] * (x2[1] - x1[1])
364 + d0[1] * (x1[0] - x2[0]) + x0[1] * (d1[0] - d2[0]) + d1[1] * x2[0]
365 + d2[0] * x1[1] - d1[0] * x2[1] - d2[1] * x1[0];
366 T c = x0[0] * (x2[1] - x1[1]) + x0[1] * (x1[0] - x2[0]) + x2[0] * x1[1]
367 - x2[1] * x1[0];
368
369 T roots[2];
370 int rootAmt = 0;
371 if(a == 0)
372 {
373 if(b == 0)
374 {
375 // parallel motion, only need to handle colinear case
376 if(c == 0)
377 {
378 // colinear PP CCD
379 if((x0 - x1).dot(d0 - d1) < 0)
380 {
381 roots[rootAmt] =
382 sqrt((x0 - x1).squaredNorm() / (d0 - d1).squaredNorm());
383 if(roots[rootAmt] > 0 && roots[rootAmt] <= 1)
384 {
385 ++rootAmt;
386 }
387 }
388 if((x0 - x2).dot(d0 - d2) < 0)
389 {
390 roots[rootAmt] =
391 sqrt((x0 - x2).squaredNorm() / (d0 - d2).squaredNorm());
392 if(roots[rootAmt] > 0 && roots[rootAmt] <= 1)
393 {
394 ++rootAmt;
395 }
396 }
397
398 if(rootAmt == 2)
399 {
400 toc = std::min(roots[0], roots[1]) * (1 - eta);
401 return true;
402 }
403 else if(rootAmt == 1)
404 {
405 toc = roots[0] * (1 - eta);
406 return true;
407 }
408 else
409 {
410 return false;
411 }
412 }
413 }
414 else
415 {
416 rootAmt = 1;
417 roots[0] = -c / b;
418 }
419 }
420 else
421 {
422 T delta = b * b - 4 * a * c;
423 if(delta == 0)
424 {
425 rootAmt = 1;
426 roots[0] = -b / (2 * a);
427 }
428 else if(delta > 0)
429 {
430 rootAmt = 2;
431 // accurate expression differs in b's sign
432 if(b > 0)
433 {
434 roots[0] = (-b - sqrt(delta)) / (2 * a);
435 roots[1] = 2 * c / (-b - sqrt(delta));
436 }
437 else
438 {
439 roots[0] = 2 * c / (-b + sqrt(delta));
440 roots[1] = (-b + sqrt(delta)) / (2 * a);
441 }
442
443 if(roots[0] > roots[1])
444 {
445 thrust::swap(roots[0], roots[1]);
446 }
447 }
448 }
449
450 for(int i = 0; i < rootAmt; ++i)
451 {
452 if(roots[i] > 0 && roots[i] <= 1)
453 {
454 // check overlap
455 T ratio;
456 if(point_edge_distance_type(Eigen::Matrix<T, 2, 1>(x0 + roots[i] * d0),
457 Eigen::Matrix<T, 2, 1>(x1 + roots[i] * d1),
458 Eigen::Matrix<T, 2, 1>(x2 + roots[i] * d2),
459 ratio)
460 == PointEdgeDistanceType::PE)
461 {
462 toc = roots[i] * (1 - eta); //TODO: distance eta
463 return true;
464 }
465 }
466 }
467
468 return false;
469}
470
471template <typename T>
472MUDA_GENERIC bool point_edge_ccd(Eigen::Matrix<T, 3, 1> p,
473 Eigen::Matrix<T, 3, 1> e0,
474 Eigen::Matrix<T, 3, 1> e1,
475 Eigen::Matrix<T, 3, 1> dp,
476 Eigen::Matrix<T, 3, 1> de0,
477 Eigen::Matrix<T, 3, 1> de1,
478 T eta,
479 T thickness,
480 int max_iter,
481 T& toc)
482{
483 Eigen::Matrix<T, 3, 1> mov = (dp + de0 + de1) / 3;
484 de0 -= mov;
485 de1 -= mov;
486 dp -= mov;
487 T maxDispMag = dp.norm() + sqrt(std::max(de0.squaredNorm(), de1.squaredNorm()));
488 if(maxDispMag == 0)
489 {
490 return false;
491 }
492
493 T dist2_cur;
494 point_edge_distance_unclassified(p, e0, e1, dist2_cur);
495 T dist_cur = sqrt(dist2_cur);
496 T gap = eta * (dist2_cur - thickness * thickness) / (dist_cur + thickness);
497 T toc_prev = toc;
498 toc = 0;
499 while(true)
500 {
501 if(max_iter >= 0)
502 {
503 if(--max_iter < 0)
504 return true;
505 }
506
507 T tocLowerBound = (1 - eta) * (dist2_cur - thickness * thickness)
508 / ((dist_cur + thickness) * maxDispMag);
509
510 p += tocLowerBound * dp;
511 e0 += tocLowerBound * de0;
512 e1 += tocLowerBound * de1;
513 point_edge_distance_unclassified(p, e0, e1, dist2_cur);
514 dist_cur = sqrt(dist2_cur);
515 if(toc && (dist2_cur - thickness * thickness) / (dist_cur + thickness) < gap)
516 {
517 break;
518 }
519
520 toc += tocLowerBound;
521 if(toc > toc_prev)
522 {
523 return false;
524 }
525 }
526
527 return true;
528}
529
530template <typename T>
531MUDA_GENERIC bool point_point_ccd(Eigen::Matrix<T, 3, 1> p0,
532 Eigen::Matrix<T, 3, 1> p1,
533 Eigen::Matrix<T, 3, 1> dp0,
534 Eigen::Matrix<T, 3, 1> dp1,
535 T eta,
536 T thickness,
537 int max_iter,
538 T& toc)
539{
540 Eigen::Matrix<T, 3, 1> mov = (dp0 + dp1) / 2;
541 dp1 -= mov;
542 dp0 -= mov;
543 T maxDispMag = dp0.norm() + dp1.norm();
544 if(maxDispMag == 0)
545 {
546 return false;
547 }
548
549 T dist2_cur;
550 point_point_distance(p0, p1, dist2_cur);
551 T dist_cur = sqrt(dist2_cur);
552 T gap = eta * (dist2_cur - thickness * thickness) / (dist_cur + thickness);
553 T toc_prev = toc;
554 toc = 0;
555 while(true)
556 {
557 if(max_iter >= 0)
558 {
559 if(--max_iter < 0)
560 return true;
561 }
562
563 T tocLowerBound = (1 - eta) * (dist2_cur - thickness * thickness)
564 / ((dist_cur + thickness) * maxDispMag);
565
566 p0 += tocLowerBound * dp0;
567 p1 += tocLowerBound * dp1;
568 point_point_distance(p0, p1, dist2_cur);
569 dist_cur = sqrt(dist2_cur);
570 if(toc && (dist2_cur - thickness * thickness) / (dist_cur + thickness) < gap)
571 {
572 break;
573 }
574
575 toc += tocLowerBound;
576 if(toc > toc_prev)
577 {
578 return false;
579 }
580 }
581
582 return true;
583}
584} // namespace muda::distance