1namespace muda::distance::details
5 T v01, T v02, T v03, T v11, T v12, T v13, T v21, T v22, T v23, T v31, T v32, T v33, T g[12])
53 t44 = t11 * t18 + -(t12 * t17);
54 t45 = t11 * t19 + -(t13 * t17);
55 t46 = t12 * t19 + -(t13 * t18);
56 t75 = 1.0 / ((t44 * t44 + t45 * t45) + t46 * t46);
57 t77 = (t16 * t44 + t14 * t46) + -(t15 * t45);
60 t79 = (t12 * t44 * 2.0 + t13 * t45 * 2.0) * t76 * t78;
61 t80 = (t11 * t45 * 2.0 + t12 * t46 * 2.0) * t76 * t78;
62 t81 = (t18 * t44 * 2.0 + t19 * t45 * 2.0) * t76 * t78;
63 t18 = (t17 * t45 * 2.0 + t18 * t46 * 2.0) * t76 * t78;
64 t83 = (t11 * t44 * 2.0 + -(t13 * t46 * 2.0)) * t76 * t78;
65 t19 = (t17 * t44 * 2.0 + -(t19 * t46 * 2.0)) * t76 * t78;
67 g[0] = -t81 + t76 * ((-t36 + t37) + t46) * 2.0;
68 g[1] = t19 - t76 * ((-t34 + t35) + t45) * 2.0;
69 g[2] = t18 + t76 * ((-t32 + t33) + t44) * 2.0;
70 g[3] = t81 + t76 * (t36 - t37) * 2.0;
71 g[4] = -t19 - t76 * (t34 - t35) * 2.0;
72 g[5] = -t18 + t76 * (t32 - t33) * 2.0;
73 t17 = t12 * t16 + -(t13 * t15);
74 g[6] = t79 - t76 * (t17 + t46) * 2.0;
75 t18 = t11 * t16 + -(t13 * t14);
76 g[7] = -t83 + t76 * (t18 + t45) * 2.0;
77 t19 = t11 * t15 + -(t12 * t14);
78 g[8] = -t80 - t76 * (t19 + t44) * 2.0;
79 g[9] = -t79 + t76 * t17 * 2.0;
80 g[10] = t83 - t76 * t18 * 2.0;
81 g[11] = t80 + t76 * t19 * 2.0;
85MUDA_GENERIC
void H_EE(
86 T v01, T v02, T v03, T v11, T v12, T v13, T v21, T v22, T v23, T v31, T v32, T v33, T H[144])
303 t59 = t11 * t26 * 2.0;
304 t65 = t12 * t27 * 2.0;
305 t73 = t13 * t28 * 2.0;
306 t35 = t11 * t11 * 2.0;
307 t36 = t12 * t12 * 2.0;
308 t37 = t13 * t13 * 2.0;
309 t38 = t26 * t26 * 2.0;
310 t39 = t27 * t27 * 2.0;
311 t40 = t28 * t28 * 2.0;
312 t98 = t11 * t15 + -(t12 * t14);
313 t99 = t11 * t16 + -(t13 * t14);
314 t100 = t12 * t16 + -(t13 * t15);
324 t140 = (t54 + -t53) + t101;
325 t141 = (t56 + -t55) + t103;
326 t142 = (t58 + -t57) + t105;
327 t143 = t12 * t101 * 2.0 + t13 * t103 * 2.0;
328 t144 = t11 * t103 * 2.0 + t12 * t105 * 2.0;
329 t145 = t27 * t101 * 2.0 + t28 * t103 * 2.0;
330 t146 = t26 * t103 * 2.0 + t27 * t105 * 2.0;
331 t147 = t11 * t101 * 2.0 + -(t13 * t105 * 2.0);
332 t148 = t26 * t101 * 2.0 + -(t28 * t105 * 2.0);
333 t156 = 1.0 / ((t101 * t101 + t103 * t103) + t105 * t105);
334 t159 = (t16 * t101 + t14 * t105) + -(t15 * t103);
336 t57 = pow(t156, 3.0);
338 t262 = t11 * t156 * t159 * 2.0;
339 t263 = t12 * t156 * t159 * 2.0;
340 t264 = t13 * t156 * t159 * 2.0;
341 t265 = t14 * t156 * t159 * 2.0;
342 t266 = t15 * t156 * t159 * 2.0;
343 t267 = t16 * t156 * t159 * 2.0;
344 t268 = (-v31 + v01) * t156 * t159 * 2.0;
345 t269 = (-v21 + v11) * t156 * t159 * 2.0;
346 t270 = (-v32 + v02) * t156 * t159 * 2.0;
347 t271 = (-v22 + v12) * t156 * t159 * 2.0;
348 t272 = (-v33 + v03) * t156 * t159 * 2.0;
349 t273 = (-v23 + v13) * t156 * t159 * 2.0;
350 t274 = (-v31 + v11) * t156 * t159 * 2.0;
351 t275 = (-v32 + v12) * t156 * t159 * 2.0;
352 t276 = (-v33 + v13) * t156 * t159 * 2.0;
353 t277 = t26 * t156 * t159 * 2.0;
354 t278 = t27 * t156 * t159 * 2.0;
355 t279 = t28 * t156 * t159 * 2.0;
356 t298 = t11 * t12 * t157 * t58 * 2.0;
357 t299 = t11 * t13 * t157 * t58 * 2.0;
358 t300 = t12 * t13 * t157 * t58 * 2.0;
359 t301 = t26 * t27 * t157 * t58 * 2.0;
360 t302 = t26 * t28 * t157 * t58 * 2.0;
361 t303 = t27 * t28 * t157 * t58 * 2.0;
362 t310 = (t35 + t36) * t157 * t58;
363 t311 = (t35 + t37) * t157 * t58;
364 t312 = (t36 + t37) * t157 * t58;
365 t313 = (t38 + t39) * t157 * t58;
366 t314 = (t38 + t40) * t157 * t58;
367 t315 = (t39 + t40) * t157 * t58;
368 t322 = (t59 + t65) * t157 * t58;
369 t323 = (t59 + t73) * t157 * t58;
370 t59 = (t65 + t73) * t157 * t58;
371 t325 = (t47 * 2.0 + -(t48 * 4.0)) * t157 * t58;
373 t56 = t48 * 2.0 - t47 * 4.0;
375 t327 = (t49 * 2.0 + -(t50 * 4.0)) * t157 * t58;
376 t55 = t50 * 2.0 - t49 * 4.0;
378 t329 = (t51 * 2.0 + -(t52 * 4.0)) * t157 * t58;
379 t54 = t52 * 2.0 - t51 * 4.0;
385 t340 = t143 * t143 * t57 * t58 * 2.0;
386 t341 = t144 * t144 * t57 * t58 * 2.0;
387 t342 = t145 * t145 * t57 * t58 * 2.0;
388 t343 = t146 * t146 * t57 * t58 * 2.0;
389 t345 = t147 * t147 * t57 * t58 * 2.0;
390 t348 = t148 * t148 * t57 * t58 * 2.0;
391 t36 = t98 * t143 * t157 * t159 * 2.0;
392 t353 = t99 * t143 * t157 * t159 * 2.0;
393 t356 = t99 * t144 * t157 * t159 * 2.0;
394 t65 = t100 * t144 * t157 * t159 * 2.0;
395 t358 = t107 * t143 * t157 * t159 * 2.0;
396 t359 = t98 * t145 * t157 * t159 * 2.0;
397 t360 = t108 * t143 * t157 * t159 * 2.0;
398 t54 = t107 * t144 * t157 * t159 * 2.0;
399 t362 = t99 * t145 * t157 * t159 * 2.0;
400 t53 = t98 * t146 * t157 * t159 * 2.0;
401 t56 = t109 * t143 * t157 * t159 * 2.0;
402 t27 = t108 * t144 * t157 * t159 * 2.0;
403 t55 = t100 * t145 * t157 * t159 * 2.0;
404 t367 = t99 * t146 * t157 * t159 * 2.0;
405 t368 = t109 * t144 * t157 * t159 * 2.0;
406 t369 = t100 * t146 * t157 * t159 * 2.0;
407 t38 = t107 * t145 * t157 * t159 * 2.0;
408 t371 = t108 * t145 * t157 * t159 * 2.0;
409 t374 = t108 * t146 * t157 * t159 * 2.0;
410 t28 = t109 * t146 * t157 * t159 * 2.0;
411 t377 = t98 * t147 * t157 * t159 * 2.0;
412 t382 = t100 * t147 * t157 * t159 * 2.0;
413 t386 = t107 * t147 * t157 * t159 * 2.0;
414 t387 = t98 * t148 * t157 * t159 * 2.0;
415 t103 = t108 * t147 * t157 * t159 * 2.0;
416 t101 = t99 * t148 * t157 * t159 * 2.0;
417 t398 = t109 * t147 * t157 * t159 * 2.0;
418 t399 = t100 * t148 * t157 * t159 * 2.0;
419 t403 = t107 * t148 * t157 * t159 * 2.0;
420 t408 = t109 * t148 * t157 * t159 * 2.0;
421 t73 = t137 * t143 * t157 * t159 * 2.0;
422 t423 = t138 * t143 * t157 * t159 * 2.0;
423 t424 = t138 * t144 * t157 * t159 * 2.0;
424 t37 = t139 * t144 * t157 * t159 * 2.0;
425 t427 = t140 * t143 * t157 * t159 * 2.0;
426 t428 = t137 * t145 * t157 * t159 * 2.0;
427 t16 = t140 * t144 * t157 * t159 * 2.0;
428 t11 = t137 * t146 * t157 * t159 * 2.0;
429 t431 = t141 * t143 * t157 * t159 * 2.0;
430 t432 = t138 * t145 * t157 * t159 * 2.0;
431 t433 = t141 * t144 * t157 * t159 * 2.0;
432 t434 = t138 * t146 * t157 * t159 * 2.0;
433 t105 = t142 * t143 * t157 * t159 * 2.0;
434 t14 = t139 * t145 * t157 * t159 * 2.0;
435 t437 = t142 * t144 * t157 * t159 * 2.0;
436 t438 = t139 * t146 * t157 * t159 * 2.0;
437 t35 = t140 * t145 * t157 * t159 * 2.0;
438 t441 = t141 * t145 * t157 * t159 * 2.0;
439 t442 = t141 * t146 * t157 * t159 * 2.0;
440 t39 = t142 * t146 * t157 * t159 * 2.0;
441 t446 = t137 * t147 * t157 * t159 * 2.0;
442 t451 = t139 * t147 * t157 * t159 * 2.0;
443 t455 = t140 * t147 * t157 * t159 * 2.0;
444 t456 = t137 * t148 * t157 * t159 * 2.0;
445 t13 = t141 * t147 * t157 * t159 * 2.0;
446 t26 = t138 * t148 * t157 * t159 * 2.0;
447 t467 = t142 * t147 * t157 * t159 * 2.0;
448 t468 = t139 * t148 * t157 * t159 * 2.0;
449 t472 = t140 * t148 * t157 * t159 * 2.0;
450 t477 = t142 * t148 * t157 * t159 * 2.0;
451 t47 = t143 * t144 * t57 * t58 * 2.0;
452 t15 = t143 * t145 * t57 * t58 * 2.0;
453 t491 = t143 * t146 * t57 * t58 * 2.0;
454 t492 = t144 * t145 * t57 * t58 * 2.0;
455 t12 = t144 * t146 * t57 * t58 * 2.0;
456 t40 = t145 * t146 * t57 * t58 * 2.0;
457 t495 = t143 * t147 * t57 * t58 * 2.0;
458 t497 = t144 * t147 * t57 * t58 * 2.0;
459 t499 = t143 * t148 * t57 * t58 * 2.0;
460 t500 = t145 * t147 * t57 * t58 * 2.0;
461 t503 = t146 * t147 * t57 * t58 * 2.0;
462 t504 = t144 * t148 * t57 * t58 * 2.0;
463 t506 = t145 * t148 * t57 * t58 * 2.0;
464 t508 = t146 * t148 * t57 * t58 * 2.0;
465 t57 = t147 * t148 * t57 * t58 * 2.0;
466 t550 = ((((t98 * t109 * t156 * 2.0 + -t266) + t337) + t359) + t368) + t492;
467 t568 = ((((t108 * t137 * t156 * 2.0 + -t268) + t330) + t27) + t456) + t504;
468 t519_tmp = t139 * t143 * t157 * t159;
469 b_t519_tmp = t100 * t143 * t157 * t159;
470 t519 = (((-(t100 * t139 * t156 * 2.0) + t312) + -t340) + b_t519_tmp * 2.0)
472 t520_tmp = t140 * t146 * t157 * t159;
473 b_t520_tmp = t107 * t146 * t157 * t159;
474 t520 = (((t107 * t140 * t156 * 2.0 + t313) + -t343) + b_t520_tmp * 2.0)
476 t521_tmp = t142 * t145 * t157 * t159;
477 b_t521_tmp = t109 * t145 * t157 * t159;
478 t521 = (((t109 * t142 * t156 * 2.0 + t315) + -t342) + -(b_t521_tmp * 2.0))
480 t522_tmp = t137 * t144 * t157 * t159;
481 b_t522_tmp = t98 * t144 * t157 * t159;
482 t522 = (((-(t98 * t137 * t156 * 2.0) + t310) + -t341) + -(b_t522_tmp * 2.0))
484 t523_tmp = t138 * t147 * t157 * t159;
485 b_t523_tmp = t99 * t147 * t157 * t159;
486 t523 = (((-(t99 * t138 * t156 * 2.0) + t311) + -t345) + b_t523_tmp * 2.0) + t523_tmp * 2.0;
487 t524_tmp = t141 * t148 * t157 * t159;
488 b_t524_tmp = t108 * t148 * t157 * t159;
489 t524 = (((t108 * t141 * t156 * 2.0 + t314) + -t348) + -(b_t524_tmp * 2.0))
491 t525 = (((t98 * t100 * t156 * 2.0 + t299) + t65) + -t36) + -t47;
492 t526 = (((t107 * t109 * t156 * 2.0 + t302) + t38) + -t28) + -t40;
493 t527 = (((-(t98 * t99 * t156 * 2.0) + t300) + t377) + -t356) + t497;
494 t528 = (((-(t99 * t100 * t156 * 2.0) + t298) + t353) + t382) + -t495;
495 t529 = (((-(t107 * t108 * t156 * 2.0) + t303) + t374) + -t403) + t508;
496 t530 = (((-(t108 * t109 * t156 * 2.0) + t301) + -t371) + -t408) + -t506;
497 t531 = (((t98 * t107 * t156 * 2.0 + t322) + t54) + -t53) + -t12;
498 t532 = (((t100 * t109 * t156 * 2.0 + t59) + t55) + -t56) + -t15;
499 t533 = (((t99 * t108 * t156 * 2.0 + t323) + t101) + -t103) + -t57;
500 t534 = (((t98 * t140 * t156 * 2.0 + -t322) + t53) + t16) + t12;
501 t535 = (((-(t107 * t137 * t156 * 2.0) + -t322) + -t54) + t11) + t12;
502 t536 = (((t100 * t142 * t156 * 2.0 + -t59) + -t55) + -t105) + t15;
503 t537 = (((-(t109 * t139 * t156 * 2.0) + -t59) + t56) + -t14) + t15;
504 t538 = (((t99 * t141 * t156 * 2.0 + -t323) + -t101) + -t13) + t57;
505 t539 = (((-(t108 * t138 * t156 * 2.0) + -t323) + t103) + -t26) + t57;
506 t540 = (((t137 * t139 * t156 * 2.0 + t299) + t37) + -t73) + -t47;
507 t148 = (((t140 * t142 * t156 * 2.0 + t302) + t39) + -t35) + -t40;
508 t542 = (((-(t137 * t138 * t156 * 2.0) + t300) + t446) + -t424) + t497;
509 t543 = (((-(t138 * t139 * t156 * 2.0) + t298) + t423) + t451) + -t495;
510 t544 = (((-(t140 * t141 * t156 * 2.0) + t303) + t472) + -t442) + t508;
511 t53 = (((-(t141 * t142 * t156 * 2.0) + t301) + t441) + t477) + -t506;
512 t157 = (((-(t139 * t142 * t156 * 2.0) + t59) + t105) + t14) + -t15;
513 t159 = (((-(t137 * t140 * t156 * 2.0) + t322) + -t16) + -t11) + -t12;
514 t147 = (((-(t138 * t141 * t156 * 2.0) + t323) + t13) + t26) + -t57;
515 t146 = ((((t100 * t107 * t156 * 2.0 + t266) + t327) + -t358) + -t369) + t491;
516 t145 = ((((-(t99 * t107 * t156 * 2.0) + -t265) + t329) + t367) + t386) + -t503;
517 t144 = ((((-(t100 * t108 * t156 * 2.0) + -t267) + t325) + t360) + -t399) + t499;
518 t143 = ((((-(t99 * t109 * t156 * 2.0) + t267) + t335) + -t362) + t398) + t500;
519 t52 = ((((-(t98 * t108 * t156 * 2.0) + t265) + t339) + -t27) + -t387) + -t504;
520 t51 = ((((t109 * t140 * t156 * 2.0 + -t278) + -t302) + t28) + t35) + t40;
521 t50 = ((((-(t98 * t139 * t156 * 2.0) + t263) + -t299) + t36) + -t37) + t47;
522 t49 = ((((t107 * t142 * t156 * 2.0 + t278) + -t302) + -t38) + -t39) + t40;
523 t48 = ((((-(t100 * t137 * t156 * 2.0) + -t263) + -t299) + -t65) + t73) + t47;
524 t47 = ((((t99 * t137 * t156 * 2.0 + t262) + -t300) + t356) + -t446) + -t497;
525 t73 = ((((t100 * t138 * t156 * 2.0 + t264) + -t298) + -t382) + -t423) + t495;
526 t65 = ((((-(t109 * t141 * t156 * 2.0) + t279) + -t301) + t408) + -t441) + t506;
527 t59 = ((((t98 * t138 * t156 * 2.0 + -t262) + -t300) + -t377) + t424) + -t497;
528 t40 = ((((t99 * t139 * t156 * 2.0 + -t264) + -t298) + -t353) + -t451) + t495;
529 t39 = ((((-(t107 * t141 * t156 * 2.0) + -t277) + -t303) + t403) + t442) + -t508;
530 t38 = ((((-(t108 * t142 * t156 * 2.0) + -t279) + -t301) + t371) + -t477) + t506;
531 t37 = ((((-(t108 * t140 * t156 * 2.0) + t277) + -t303) + -t374) + -t472) + -t508;
532 t36 = ((((t98 * t142 * t156 * 2.0 + t271) + t328) + -t359) + t437) + -t492;
533 t35 = ((((-(t109 * t137 * t156 * 2.0) + t270) + t328) + -t368) + -t428) + -t492;
534 t28 = ((((t100 * t140 * t156 * 2.0 + -t271) + -t327) + t369) + -t427) + -t491;
535 t27 = ((((-(t98 * t141 * t156 * 2.0) + -t269) + t330) + t387) + -t433) + t504;
536 t26 = ((((t109 * t138 * t156 * 2.0 + -t272) + t326) + -t398) + t432) + -t500;
537 t13 = ((((-(t107 * t139 * t156 * 2.0) + -t270) + -t327) + t358) + t438) + -t491;
538 t12 = ((((-(t99 * t142 * t156 * 2.0) + -t273) + t326) + t362) + t467) + -t500;
539 t11 = ((((-(t99 * t140 * t156 * 2.0) + t269) + -t329) + -t367) + t455) + t503;
540 t16 = ((((t107 * t138 * t156 * 2.0 + t268) + -t329) + -t386) + -t434) + t503;
541 t15 = ((((-(t100 * t141 * t156 * 2.0) + t273) + -t325) + t399) + t431) + -t499;
542 t14 = ((((t108 * t139 * t156 * 2.0 + t272) + -t325) + -t360) + t468) + -t499;
543 t105 = ((((-(t139 * t140 * t156 * 2.0) + t275) + t327) + t427) + -t438) + t491;
544 t103 = ((((t138 * t140 * t156 * 2.0 + -t274) + t329) + t434) + -t455) + -t503;
545 t101 = ((((-(t137 * t142 * t156 * 2.0) + -t275) + t337) + t428) + -t437) + t492;
546 t58 = ((((t139 * t141 * t156 * 2.0 + -t276) + t325) + -t431) + -t468) + t499;
547 t57 = ((((t137 * t141 * t156 * 2.0 + t274) + t339) + t433) + -t456) + -t504;
548 t56 = ((((t138 * t142 * t156 * 2.0 + t276) + t335) + -t432) + -t467) + t500;
550 H[0] = (t55 + t142 * t142 * t156 * 2.0) - t521_tmp * 4.0;
564 H[13] = (t54 + t141 * t141 * t156 * 2.0) - t524_tmp * 4.0;
578 H[26] = (t53 + t140 * t140 * t156 * 2.0) + t520_tmp * 4.0;
591 H[39] = (t55 + t109 * t109 * t156 * 2.0) + b_t521_tmp * 4.0;
604 H[52] = (t54 + t108 * t108 * t156 * 2.0) + b_t524_tmp * 4.0;
617 H[65] = (t53 + t107 * t107 * t156 * 2.0) - b_t520_tmp * 4.0;
631 H[78] = (t55 + t139 * t139 * t156 * 2.0) - t519_tmp * 4.0;
645 H[91] = (t54 + t138 * t138 * t156 * 2.0) - t523_tmp * 4.0;
659 H[104] = (t53 + t137 * t137 * t156 * 2.0) + t522_tmp * 4.0;
672 H[117] = (t55 + t100 * t100 * t156 * 2.0) - b_t519_tmp * 4.0;
685 H[130] = (t54 + t99 * t99 * t156 * 2.0) - b_t523_tmp * 4.0;
698 H[143] = (t53 + t98 * t98 * t156 * 2.0) + b_t522_tmp * 4.0;
703namespace muda::distance
706MUDA_GENERIC
void edge_edge_distance(
const Eigen::Matrix<T, 3, 1>& ea0,
707 const Eigen::Matrix<T, 3, 1>& ea1,
708 const Eigen::Matrix<T, 3, 1>& eb0,
709 const Eigen::Matrix<T, 3, 1>& eb1,
712 Eigen::Matrix<T, 3, 1> b = Eigen::Matrix<T, 3, 1>(ea1 - ea0).cross(eb1 - eb0);
713 T aTb = Eigen::Matrix<T, 3, 1>(eb0 - ea0).dot(b);
714 dist2 = aTb * aTb / b.squaredNorm();
718MUDA_GENERIC
void edge_edge_distance_gradient(
const Eigen::Matrix<T, 3, 1>& ea0,
719 const Eigen::Matrix<T, 3, 1>& ea1,
720 const Eigen::Matrix<T, 3, 1>& eb0,
721 const Eigen::Matrix<T, 3, 1>& eb1,
722 Eigen::Matrix<T, 12, 1>& grad)
724 details::g_EE(ea0[0],
740MUDA_GENERIC
void edge_edge_distance_hessian(
const Eigen::Matrix<T, 3, 1>& ea0,
741 const Eigen::Matrix<T, 3, 1>& ea1,
742 const Eigen::Matrix<T, 3, 1>& eb0,
743 const Eigen::Matrix<T, 3, 1>& eb1,
744 Eigen::Matrix<T, 12, 12>& Hessian)
746 details::H_EE(ea0[0],