1#include <Eigen/Geometry>
3namespace muda::distance
5template <
class T,
int dim>
6MUDA_GENERIC
void point_edge_distance(
const Eigen::Vector<T, dim>& p,
7 const Eigen::Vector<T, dim>& e0,
8 const Eigen::Vector<T, dim>& e1,
11 if constexpr(dim == 2)
13 const Eigen::Matrix<T, 2, 1> e = e1 - e0;
14 T numerator = (e[1] * p[0] - e[0] * p[1] + e1[0] * e0[1] - e1[1] * e0[0]);
15 dist2 = numerator * numerator / e.squaredNorm();
19 dist2 = (e0 - p).cross(e1 - p).squaredNorm() / (e1 - e0).squaredNorm();
26 MUDA_GENERIC
void g_PE2D(T v01, T v02, T v11, T v12, T v21, T v22, T g[6])
42 t23 = 1.0 / (t13 * t13 + t14 * t14);
43 t25 = ((v11 * v22 + -(v12 * v21)) + t14 * v01) + -(t13 * v02);
46 t27 = (v11 * 2.0 + -(v21 * 2.0)) * t24 * t26;
47 t26 *= (v12 * 2.0 + -(v22 * 2.0)) * t24;
48 g[0] = t14 * t23 * t25 * 2.0;
49 g[1] = t13 * t23 * t25 * -2.0;
51 g[2] = -t27 - t24 * (-v22 + v02) * 2.0;
52 g[3] = -t26 + t24 * (-v21 + v01) * 2.0;
53 g[4] = t27 + t24 * (v02 - v12) * 2.0;
54 g[5] = t26 - t24 * (v01 - v11) * 2.0;
58 MUDA_GENERIC
void g_PE3D(T v01, T v02, T v03, T v11, T v12, T v13, T v21, T v22, T v23, T g[9])
91 t42 = 1.0 / ((t23 * t23 + t24 * t24) + t25 * t25);
92 t44 = t17 * t21 + -(t18 * t20);
93 t45 = t17 * t22 + -(t19 * t20);
94 t46 = t18 * t22 + -(t19 * t21);
96 t50 = (t44 * t44 + t45 * t45) + t46 * t46;
97 t51 = (v11 * 2.0 + -(v21 * 2.0)) * t43 * t50;
98 t52 = (v12 * 2.0 + -(v22 * 2.0)) * t43 * t50;
99 t43 = (v13 * 2.0 + -(v23 * 2.0)) * t43 * t50;
100 g[0] = t42 * (t24 * t44 * 2.0 + t25 * t45 * 2.0);
101 g[1] = -t42 * (t23 * t44 * 2.0 - t25 * t46 * 2.0);
102 g[2] = -t42 * (t23 * t45 * 2.0 + t24 * t46 * 2.0);
103 g[3] = -t51 - t42 * (t21 * t44 * 2.0 + t22 * t45 * 2.0);
104 g[4] = -t52 + t42 * (t20 * t44 * 2.0 - t22 * t46 * 2.0);
105 g[5] = -t43 + t42 * (t20 * t45 * 2.0 + t21 * t46 * 2.0);
106 g[6] = t51 + t42 * (t18 * t44 * 2.0 + t19 * t45 * 2.0);
107 g[7] = t52 - t42 * (t17 * t44 * 2.0 - t19 * t46 * 2.0);
108 g[8] = t43 - t42 * (t17 * t45 * 2.0 + t18 * t46 * 2.0);
112 MUDA_GENERIC
void H_PE2D(T v01, T v02, T v11, T v12, T v21, T v22, T H[36])
163 t21 = v11 * 2.0 + -(v21 * 2.0);
164 t22 = v12 * 2.0 + -(v22 * 2.0);
167 t31 = 1.0 / (t23 + t24);
168 t34 = ((v11 * v22 + -(v12 * v21)) + t20 * v01) + -(t19 * v02);
172 t60 = t31 * t34 * 2.0;
173 t59 = -(t19 * t20 * t31 * 2.0);
174 t62 = t32 * t35 * 2.0;
175 t64 = t21 * t21 * t33 * t35 * 2.0;
176 t65 = t22 * t22 * t33 * t35 * 2.0;
177 t68 = t15 * t21 * t32 * t34 * 2.0;
178 t71 = t16 * t22 * t32 * t34 * 2.0;
179 t72 = t17 * t21 * t32 * t34 * 2.0;
180 t75 = t18 * t22 * t32 * t34 * 2.0;
181 t76 = t19 * t21 * t32 * t34 * 2.0;
182 t77 = t20 * t21 * t32 * t34 * 2.0;
183 t78 = t19 * t22 * t32 * t34 * 2.0;
184 t79 = t20 * t22 * t32 * t34 * 2.0;
185 t90 = t21 * t22 * t33 * t35 * 2.0;
186 t92 = t16 * t20 * t31 * 2.0 + t77;
187 t94 = -(t17 * t19 * t31 * 2.0) + t78;
188 t96 = (t18 * t19 * t31 * 2.0 + -t60) + t76;
189 t99 = (-(t15 * t20 * t31 * 2.0) + -t60) + t79;
190 t93 = t15 * t19 * t31 * 2.0 + -t78;
191 t35 = -(t18 * t20 * t31 * 2.0) + -t77;
192 t97 = (t17 * t20 * t31 * 2.0 + t60) + -t79;
193 t98 = (-(t16 * t19 * t31 * 2.0) + t60) + -t76;
194 t100 = ((-(t15 * t16 * t31 * 2.0) + t71) + -t68) + t90;
195 t19 = ((-(t17 * t18 * t31 * 2.0) + t75) + -t72) + t90;
196 t102_tmp = t17 * t22 * t32 * t34;
197 t76 = t15 * t22 * t32 * t34;
198 t22 = (((-(t15 * t17 * t31 * 2.0) + t62) + -t65) + t76 * 2.0) + t102_tmp * 2.0;
199 t33 = t18 * t21 * t32 * t34;
200 t20 = t16 * t21 * t32 * t34;
201 t79 = (((-(t16 * t18 * t31 * 2.0) + t62) + -t64) + -(t20 * 2.0)) + -(t33 * 2.0);
202 t77 = (((t15 * t18 * t31 * 2.0 + t60) + t68) + -t75) + -t90;
203 t78 = (((t16 * t17 * t31 * 2.0 + -t60) + t72) + -t71) + -t90;
204 H[0] = t24 * t31 * 2.0;
211 H[7] = t23 * t31 * 2.0;
219 H[14] = (t35 + t18 * t18 * t31 * 2.0) + t33 * 4.0;
227 H[21] = (t33 + t17 * t17 * t31 * 2.0) - t102_tmp * 4.0;
234 H[28] = (t35 + t16 * t16 * t31 * 2.0) + t20 * 4.0;
241 H[35] = (t33 + t15 * t15 * t31 * 2.0) - t76 * 4.0;
245 MUDA_GENERIC
void H_PE3D(T v01, T v02, T v03, T v11, T v12, T v13, T v21, T v22, T v23, T H[81])
358 t26 = v11 * 2.0 + -(v21 * 2.0);
359 t27 = v12 * 2.0 + -(v22 * 2.0);
360 t28 = v13 * 2.0 + -(v23 * 2.0);
370 t56 = t17 * t20 * 2.0;
371 t62 = t18 * t21 * 2.0;
372 t70 = t19 * t22 * 2.0;
373 t71 = t17 * t23 * 2.0;
374 t75 = t18 * t24 * 2.0;
375 t79 = t19 * t25 * 2.0;
376 t80 = t20 * t23 * 2.0;
377 t84 = t21 * t24 * 2.0;
378 t88 = t22 * t25 * 2.0;
379 t38 = t17 * t17 * 2.0;
380 t39 = t18 * t18 * 2.0;
381 t40 = t19 * t19 * 2.0;
382 t41 = t20 * t20 * 2.0;
383 t42 = t21 * t21 * 2.0;
384 t43 = t22 * t22 * 2.0;
394 t102 = 1.0 / ((t35 + t36) + t37);
399 t104 = pow(t102, 3.0);
400 t162 = -(t23 * t24 * t102 * 2.0);
401 t163 = -(t23 * t25 * t102 * 2.0);
402 t164 = -(t24 * t25 * t102 * 2.0);
403 t213 = t18 * t36 * 2.0 + t19 * t35 * 2.0;
404 t214 = t17 * t35 * 2.0 + t18 * t37 * 2.0;
405 t215 = t21 * t36 * 2.0 + t22 * t35 * 2.0;
406 t216 = t20 * t35 * 2.0 + t21 * t37 * 2.0;
407 t217 = t24 * t36 * 2.0 + t25 * t35 * 2.0;
408 t218 = t23 * t35 * 2.0 + t24 * t37 * 2.0;
409 t35 = (t36 * t36 + t35 * t35) + t37 * t37;
410 t225 = t17 * t36 * 2.0 + -(t19 * t37 * 2.0);
411 t226 = t20 * t36 * 2.0 + -(t22 * t37 * 2.0);
412 t227 = t23 * t36 * 2.0 + -(t25 * t37 * 2.0);
434 t279 = t103 * t35 * 2.0;
435 t281 = t26 * t26 * t104 * t35 * 2.0;
436 t282 = t27 * t27 * t104 * t35 * 2.0;
437 t283 = t28 * t28 * t104 * t35 * 2.0;
438 t287 = t26 * t27 * t104 * t35 * 2.0;
439 t218 = t26 * t28 * t104 * t35 * 2.0;
440 t289 = t27 * t28 * t104 * t35 * 2.0;
450 t293 = t102 * (t75 + t79) + t214;
451 t295 = -(t102 * (t80 + t84)) + t213;
452 t299 = t102 * ((t63 + t22 * t23 * 2.0) + -t60) + t217;
453 t300 = t102 * ((t67 + t22 * t24 * 2.0) + -t65) + t245;
454 t303 = -(t102 * ((t57 + t17 * t24 * 2.0) + -t58)) + t215;
455 t304 = -(t102 * ((t60 + t17 * t25 * 2.0) + -t63)) + t216;
456 t294 = t102 * (t71 + t75) + -t213;
457 t297 = -(t102 * (t80 + t88)) + t35;
458 t88 = -(t102 * (t84 + t88)) + -t214;
459 t301 = t102 * ((t58 + t21 * t23 * 2.0) + -t57) + t253;
460 t302 = t102 * ((t65 + t21 * t25 * 2.0) + -t67) + t36;
461 t84 = t102 * ((t57 + t20 * t24 * 2.0) + -t58) + -t215;
462 t80 = t102 * ((t60 + t20 * t25 * 2.0) + -t63) + -t216;
463 t75 = -(t102 * ((t63 + t19 * t23 * 2.0) + -t60)) + -t217;
464 t227 = -(t102 * ((t67 + t19 * t24 * 2.0) + -t65)) + -t245;
465 t311 = ((-(t17 * t19 * t102 * 2.0) + t231) + -t232) + t218;
466 t245 = ((-(t20 * t22 * t102 * 2.0) + t237) + -t238) + t218;
467 t226 = ((-t102 * (t67 - t54 * 4.0) + t233) + t252) + -t289;
468 t28 = ((-t102 * (t63 - t52 * 4.0) + t232) + -t237) + -t218;
469 t27 = ((-t102 * (t58 - t50 * 4.0) + t247) + -t236) + -t287;
470 t225 = ((-(t102 * (t65 + -(t55 * 4.0))) + t239) + t249) + -t289;
471 t26 = ((-(t102 * (t60 + -(t53 * 4.0))) + t238) + -t231) + -t218;
472 t103 = ((-(t102 * (t57 + -(t51 * 4.0))) + t250) + -t230) + -t287;
473 t104 = (((-(t102 * (t56 + t62)) + t234) + t240) + t279) + -t283;
474 t218 = (((-(t102 * (t56 + t70)) + t248) + t251) + t279) + -t282;
475 t217 = (((-(t102 * (t62 + t70)) + -t229) + -t235) + t279) + -t281;
476 t216 = t102 * (t71 + t79) + -t35;
477 t215 = -(t102 * ((t58 + t18 * t23 * 2.0) + -t57)) + -t253;
478 t214 = -(t102 * ((t65 + t18 * t25 * 2.0) + -t67)) + -t36;
479 t213 = ((-(t17 * t18 * t102 * 2.0) + t230) + -t247) + t287;
480 t37 = ((-(t20 * t21 * t102 * 2.0) + t236) + -t250) + t287;
481 t36 = ((-(t18 * t19 * t102 * 2.0) + -t233) + -t249) + t289;
482 t35 = ((-(t21 * t22 * t102 * 2.0) + -t239) + -t252) + t289;
483 H[0] = t102 * (t46 + t48);
493 H[10] = t102 * (t44 + t48);
503 H[20] = t102 * (t44 + t46);
513 H[30] = ((t235 * 2.0 + -t279) + t281) + t102 * (t42 + t43);
523 H[40] = ((t251 * -2.0 + -t279) + t282) + t102 * (t41 + t43);
533 H[50] = ((t240 * -2.0 + -t279) + t283) + t102 * (t41 + t42);
543 H[60] = ((t229 * 2.0 + -t279) + t281) + t102 * (t39 + t40);
553 H[70] = ((t248 * -2.0 + -t279) + t282) + t102 * (t38 + t40);
563 H[80] = ((t234 * -2.0 + -t279) + t283) + t102 * (t38 + t39);
568template <
class T,
int dim>
569MUDA_GENERIC
void point_edge_distance_gradient(
const Eigen::Vector<T, dim>& p,
570 const Eigen::Vector<T, dim>& e0,
571 const Eigen::Vector<T, dim>& e1,
572 Eigen::Vector<T, dim * 3>& grad)
574 if constexpr(dim == 2)
576 details::g_PE2D(p[0], p[1], e0[0], e0[1], e1[0], e1[1], grad.data());
580 details::g_PE3D(p[0], p[1], p[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2], grad.data());
584template <
class T,
int dim>
585MUDA_GENERIC
void point_edge_distance_hessian(
const Eigen::Vector<T, dim>& p,
586 const Eigen::Vector<T, dim>& e0,
587 const Eigen::Vector<T, dim>& e1,
588 Eigen::Matrix<T, dim * 3, dim * 3>& Hessian)
590 if constexpr(dim == 2)
592 details::H_PE2D(p[0], p[1], e0[0], e0[1], e1[0], e1[1], Hessian.data());
597 p[0], p[1], p[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2], Hessian.data());