1#include <Eigen/Geometry>
3namespace muda::distance
8 MUDA_GENERIC
void g_PT(
9 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])
47 t32 = t14 * t18 + -(t15 * t17);
48 t33 = t14 * t19 + -(t16 * t17);
49 t34 = t15 * t19 + -(t16 * t18);
50 t43 = 1.0 / ((t32 * t32 + t33 * t33) + t34 * t34);
51 t45 = (t13 * t32 + t11 * t34) + -(t12 * t33);
54 g[0] = t34 * t43 * t45 * 2.0;
55 g[1] = t33 * t43 * t45 * -2.0;
56 g[2] = t32 * t43 * t45 * 2.0;
58 g[3] = -t44 * t46 * (t21 * t32 * 2.0 + t22 * t33 * 2.0)
59 - t45 * ((t34 + t12 * t22) - t13 * t21) * 2.0;
61 g[4] = t43 * (t20 * t32 * 2.0 - t22 * t34 * 2.0)
62 + t45 * ((t33 + t11 * t22) - t13 * t20) * 2.0;
63 g[5] = t43 * (t20 * t33 * 2.0 + t21 * t34 * 2.0)
64 - t45 * ((t32 + t11 * t21) - t12 * t20) * 2.0;
65 g[6] = t45 * (t12 * t19 - t13 * t18) * 2.0
66 + t43 * (t18 * t32 * 2.0 + t19 * t33 * 2.0);
67 g[7] = t45 * (t11 * t19 - t13 * t17) * -2.0
68 - t43 * (t17 * t32 * 2.0 - t19 * t34 * 2.0);
69 g[8] = t45 * (t11 * t18 - t12 * t17) * 2.0
70 - t43 * (t17 * t33 * 2.0 + t18 * t34 * 2.0);
71 g[9] = t45 * (t12 * t16 - t13 * t15) * -2.0
72 - t43 * (t15 * t32 * 2.0 + t16 * t33 * 2.0);
73 g[10] = t45 * (t11 * t16 - t13 * t14) * 2.0
74 + t43 * (t14 * t32 * 2.0 - t16 * t34 * 2.0);
75 g[11] = t45 * (t11 * t15 - t12 * t14) * -2.0
76 + t43 * (t14 * t33 * 2.0 + t15 * t34 * 2.0);
80 MUDA_GENERIC
void H_PT(
81 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])
238 t71 = t18 * t23 * 2.0;
239 t77 = t20 * t24 * 2.0;
240 t85 = t22 * t25 * 2.0;
241 t86 = t18 * t26 * 2.0;
242 t90 = t20 * t27 * 2.0;
243 t94 = t22 * t28 * 2.0;
244 t95 = t23 * t26 * 2.0;
245 t99 = t24 * t27 * 2.0;
246 t103 = t25 * t28 * 2.0;
247 t38 = t18 * t18 * 2.0;
248 t39 = t20 * t20 * 2.0;
249 t40 = t22 * t22 * 2.0;
250 t41 = t23 * t23 * 2.0;
251 t42 = t24 * t24 * 2.0;
252 t43 = t25 * t25 * 2.0;
253 t44 = t26 * t26 * 2.0;
254 t45 = t27 * t27 * 2.0;
255 t46 = t28 * t28 * 2.0;
262 t125 = t11 * t20 + -(t12 * t18);
263 t126 = t11 * t22 + -(t13 * t18);
264 t127 = t12 * t22 + -(t13 * t20);
265 t128 = t11 * t24 + -(t12 * t23);
266 t129 = t11 * t25 + -(t13 * t23);
267 t130 = t12 * t25 + -(t13 * t24);
274 t189 = (t11 * t27 + -(t12 * t26)) + t131;
275 t190 = (t11 * t28 + -(t13 * t26)) + t133;
276 t191 = (t12 * t28 + -(t13 * t27)) + t135;
277 t192 = t20 * t131 * 2.0 + t22 * t133 * 2.0;
278 t193 = t18 * t133 * 2.0 + t20 * t135 * 2.0;
279 t194 = t24 * t131 * 2.0 + t25 * t133 * 2.0;
280 t195 = t23 * t133 * 2.0 + t24 * t135 * 2.0;
281 t196 = t27 * t131 * 2.0 + t28 * t133 * 2.0;
282 t197 = t26 * t133 * 2.0 + t27 * t135 * 2.0;
283 t198 = t18 * t131 * 2.0 + -(t22 * t135 * 2.0);
284 t199 = t23 * t131 * 2.0 + -(t25 * t135 * 2.0);
285 t200 = t26 * t131 * 2.0 + -(t28 * t135 * 2.0);
286 t202 = 1.0 / ((t149 + t150) + t151);
287 t205 = (t13 * t131 + t11 * t135) + -(t12 * t133);
289 t204 = pow(t202, 3.0);
291 t241 = t131 * t135 * t202 * 2.0;
292 t309 = t11 * t202 * t205 * 2.0;
293 t310 = t12 * t202 * t205 * 2.0;
294 t13 = t13 * t202 * t205 * 2.0;
295 t312 = (-v21 + v01) * t202 * t205 * 2.0;
296 t313 = (-v22 + v02) * t202 * t205 * 2.0;
297 t314 = (-v23 + v03) * t202 * t205 * 2.0;
298 t315 = (-v31 + v01) * t202 * t205 * 2.0;
299 t316 = t18 * t202 * t205 * 2.0;
300 t317 = (-v32 + v02) * t202 * t205 * 2.0;
301 t318 = t20 * t202 * t205 * 2.0;
302 t319 = (-v33 + v03) * t202 * t205 * 2.0;
303 t11 = t22 * t202 * t205 * 2.0;
304 t321 = t23 * t202 * t205 * 2.0;
305 t322 = t24 * t202 * t205 * 2.0;
306 t323 = t25 * t202 * t205 * 2.0;
307 t324 = t26 * t202 * t205 * 2.0;
308 t325 = t27 * t202 * t205 * 2.0;
309 t12 = t28 * t202 * t205 * 2.0;
310 t261 = -(t131 * t133 * t202 * 2.0);
311 t262 = -(t133 * t135 * t202 * 2.0);
312 t599 = t130 * t135 * t202 * 2.0 + t135 * t194 * t203 * t205 * 2.0;
313 t600 = -(t125 * t131 * t202 * 2.0) + t131 * t193 * t203 * t205 * 2.0;
314 t602 = t129 * t133 * t202 * 2.0 + t133 * t199 * t203 * t205 * 2.0;
315 t605 = -(t131 * t189 * t202 * 2.0) + t131 * t197 * t203 * t205 * 2.0;
316 t609 = (t127 * t133 * t202 * 2.0 + -t11) + t133 * t192 * t203 * t205 * 2.0;
317 t610 = (t126 * t135 * t202 * 2.0 + t11) + t135 * t198 * t203 * t205 * 2.0;
318 t611 = (t130 * t131 * t202 * 2.0 + -t322) + t131 * t194 * t203 * t205 * 2.0;
319 t613 = (t126 * t131 * t202 * 2.0 + -t316) + t131 * t198 * t203 * t205 * 2.0;
320 t615 = (-(t125 * t135 * t202 * 2.0) + -t318) + t135 * t193 * t203 * t205 * 2.0;
321 t616 = (-(t128 * t133 * t202 * 2.0) + -t321) + t133 * t195 * t203 * t205 * 2.0;
322 t621 = (t133 * t191 * t202 * 2.0 + -t12) + t133 * t196 * t203 * t205 * 2.0;
323 t622 = (t135 * t190 * t202 * 2.0 + t12) + t135 * t200 * t203 * t205 * 2.0;
324 t623 = (t131 * t190 * t202 * 2.0 + -t324) + t131 * t200 * t203 * t205 * 2.0;
325 t625 = (-(t135 * t189 * t202 * 2.0) + -t325) + t135 * t197 * t203 * t205 * 2.0;
326 t645 = ((((t127 * t129 * t202 * 2.0 + -t13) + (t72 + -(t66 * 4.0)) * t203 * t206)
327 + t129 * t192 * t203 * t205 * 2.0)
328 + t127 * t199 * t203 * t205 * 2.0)
329 + t192 * t199 * t204 * t206 * 2.0;
330 t646_tmp = t203 * t206;
331 t646 = ((((t126 * t130 * t202 * 2.0 + t13) + t646_tmp * (t73 - t65 * 4.0))
332 + t126 * t194 * t203 * t205 * 2.0)
333 + t130 * t198 * t203 * t205 * 2.0)
334 + t194 * t198 * t204 * t206 * 2.0;
335 t601 = t128 * t131 * t202 * 2.0 + -(t131 * t195 * t203 * t205 * 2.0);
336 t603 = -(t127 * t135 * t202 * 2.0) + -(t135 * t192 * t203 * t205 * 2.0);
337 t604 = -(t126 * t133 * t202 * 2.0) + -(t133 * t198 * t203 * t205 * 2.0);
338 t606 = -(t135 * t191 * t202 * 2.0) + -(t135 * t196 * t203 * t205 * 2.0);
339 t607 = -(t133 * t190 * t202 * 2.0) + -(t133 * t200 * t203 * t205 * 2.0);
340 t608 = (t125 * t133 * t202 * 2.0 + t316) + -(t133 * t193 * t203 * t205 * 2.0);
341 t612 = (t128 * t135 * t202 * 2.0 + t322) + -(t135 * t195 * t203 * t205 * 2.0);
342 t614 = (-(t127 * t131 * t202 * 2.0) + t318) + -(t131 * t192 * t203 * t205 * 2.0);
343 t617 = (-(t130 * t133 * t202 * 2.0) + t323) + -(t133 * t194 * t203 * t205 * 2.0);
344 t618 = (-(t129 * t131 * t202 * 2.0) + t321) + -(t131 * t199 * t203 * t205 * 2.0);
345 t619 = (-(t129 * t135 * t202 * 2.0) + -t323) + -(t135 * t199 * t203 * t205 * 2.0);
346 t620 = (t133 * t189 * t202 * 2.0 + t324) + -(t133 * t197 * t203 * t205 * 2.0);
347 t624 = (-(t131 * t191 * t202 * 2.0) + t325) + -(t131 * t196 * t203 * t205 * 2.0);
348 t626 = (((t125 * t127 * t202 * 2.0 + t18 * t22 * t203 * t206 * 2.0)
349 + t125 * t192 * t203 * t205 * 2.0)
350 + -(t127 * t193 * t203 * t205 * 2.0))
351 + -(t192 * t193 * t204 * t206 * 2.0);
352 t627 = (((t128 * t130 * t202 * 2.0 + t23 * t25 * t203 * t206 * 2.0)
353 + t128 * t194 * t203 * t205 * 2.0)
354 + -(t130 * t195 * t203 * t205 * 2.0))
355 + -(t194 * t195 * t204 * t206 * 2.0);
356 t628 = (((-(t125 * t126 * t202 * 2.0) + t20 * t22 * t203 * t206 * 2.0)
357 + t126 * t193 * t203 * t205 * 2.0)
358 + -(t125 * t198 * t203 * t205 * 2.0))
359 + t193 * t198 * t204 * t206 * 2.0;
360 t629 = (((-(t128 * t129 * t202 * 2.0) + t24 * t25 * t203 * t206 * 2.0)
361 + t129 * t195 * t203 * t205 * 2.0)
362 + -(t128 * t199 * t203 * t205 * 2.0))
363 + t195 * t199 * t204 * t206 * 2.0;
364 t630 = (((-(t126 * t127 * t202 * 2.0) + t18 * t20 * t203 * t206 * 2.0)
365 + -(t126 * t192 * t203 * t205 * 2.0))
366 + -(t127 * t198 * t203 * t205 * 2.0))
367 + -(t192 * t198 * t204 * t206 * 2.0);
368 t631 = (((-(t129 * t130 * t202 * 2.0) + t23 * t24 * t203 * t206 * 2.0)
369 + -(t129 * t194 * t203 * t205 * 2.0))
370 + -(t130 * t199 * t203 * t205 * 2.0))
371 + -(t194 * t199 * t204 * t206 * 2.0);
372 t632 = (((-(t125 * t128 * t202 * 2.0) + (t71 + t77) * t203 * t206)
373 + t128 * t193 * t203 * t205 * 2.0)
374 + t125 * t195 * t203 * t205 * 2.0)
375 + -(t193 * t195 * t204 * t206 * 2.0);
376 t633 = (((-(t127 * t130 * t202 * 2.0) + (t77 + t85) * t203 * t206)
377 + -(t130 * t192 * t203 * t205 * 2.0))
378 + -(t127 * t194 * t203 * t205 * 2.0))
379 + -(t192 * t194 * t204 * t206 * 2.0);
380 t634 = (((-(t126 * t129 * t202 * 2.0) + (t71 + t85) * t203 * t206)
381 + -(t129 * t198 * t203 * t205 * 2.0))
382 + -(t126 * t199 * t203 * t205 * 2.0))
383 + -(t198 * t199 * t204 * t206 * 2.0);
384 t635 = (((t127 * t191 * t202 * 2.0 + -((t90 + t94) * t203 * t206))
385 + t127 * t196 * t203 * t205 * 2.0)
386 + t191 * t192 * t203 * t205 * 2.0)
387 + t192 * t196 * t204 * t206 * 2.0;
388 t636 = (((-(t128 * t189 * t202 * 2.0) + (t95 + t99) * t203 * t206)
389 + t128 * t197 * t203 * t205 * 2.0)
390 + t189 * t195 * t203 * t205 * 2.0)
391 + -(t195 * t197 * t204 * t206 * 2.0);
392 t637 = (((t125 * t189 * t202 * 2.0 + -((t86 + t90) * t203 * t206))
393 + -(t125 * t197 * t203 * t205 * 2.0))
394 + -(t189 * t193 * t203 * t205 * 2.0))
395 + t193 * t197 * t204 * t206 * 2.0;
396 t638 = (((-(t130 * t191 * t202 * 2.0) + (t99 + t103) * t203 * t206)
397 + -(t130 * t196 * t203 * t205 * 2.0))
398 + -(t191 * t194 * t203 * t205 * 2.0))
399 + -(t194 * t196 * t204 * t206 * 2.0);
400 t86 = (((t126 * t190 * t202 * 2.0 + -((t86 + t94) * t203 * t206))
401 + t126 * t200 * t203 * t205 * 2.0)
402 + t190 * t198 * t203 * t205 * 2.0)
403 + t198 * t200 * t204 * t206 * 2.0;
404 t71 = (((-(t129 * t190 * t202 * 2.0) + (t95 + t103) * t203 * t206)
405 + -(t129 * t200 * t203 * t205 * 2.0))
406 + -(t190 * t199 * t203 * t205 * 2.0))
407 + -(t199 * t200 * t204 * t206 * 2.0);
408 t85 = (((t189 * t191 * t202 * 2.0 + t26 * t28 * t203 * t206 * 2.0)
409 + t189 * t196 * t203 * t205 * 2.0)
410 + -(t191 * t197 * t203 * t205 * 2.0))
411 + -(t196 * t197 * t204 * t206 * 2.0);
412 t90 = (((-(t189 * t190 * t202 * 2.0) + t27 * t28 * t203 * t206 * 2.0)
413 + t190 * t197 * t203 * t205 * 2.0)
414 + -(t189 * t200 * t203 * t205 * 2.0))
415 + t197 * t200 * t204 * t206 * 2.0;
416 t99 = (((-(t190 * t191 * t202 * 2.0) + t26 * t27 * t203 * t206 * 2.0)
417 + -(t190 * t196 * t203 * t205 * 2.0))
418 + -(t191 * t200 * t203 * t205 * 2.0))
419 + -(t196 * t200 * t204 * t206 * 2.0);
420 t77 = ((((-(t127 * t128 * t202 * 2.0) + t310) + (t75 + -(t68 * 4.0)) * t203 * t206)
421 + t127 * t195 * t203 * t205 * 2.0)
422 + -(t128 * t192 * t203 * t205 * 2.0))
423 + t192 * t195 * t204 * t206 * 2.0;
424 t131 = ((((t126 * t128 * t202 * 2.0 + -t309) + (t80 + -(t70 * 4.0)) * t203 * t206)
425 + t128 * t198 * t203 * t205 * 2.0)
426 + -(t126 * t195 * t203 * t205 * 2.0))
427 + -(t195 * t198 * t204 * t206 * 2.0);
428 t133 = ((((-(t125 * t130 * t202 * 2.0) + -t310) + t646_tmp * (t78 - t67 * 4.0))
429 + t130 * t193 * t203 * t205 * 2.0)
430 + -(t125 * t194 * t203 * t205 * 2.0))
431 + t193 * t194 * t204 * t206 * 2.0;
432 t325 = ((((t125 * t129 * t202 * 2.0 + t309) + t646_tmp * (t82 - t69 * 4.0))
433 + t125 * t199 * t203 * t205 * 2.0)
434 + -(t129 * t193 * t203 * t205 * 2.0))
435 + -(t193 * t199 * t204 * t206 * 2.0);
436 t135 = ((((t125 * t191 * t202 * 2.0 + t313) + ((t75 + t18 * t28 * 2.0) + -t78) * t203 * t206)
437 + t125 * t196 * t203 * t205 * 2.0)
438 + -(t191 * t193 * t203 * t205 * 2.0))
439 + -(t193 * t196 * t204 * t206 * 2.0);
440 t324 = ((((t127 * t189 * t202 * 2.0 + -t313) + ((t78 + t22 * t26 * 2.0) + -t75) * t203 * t206)
441 + -(t127 * t197 * t203 * t205 * 2.0))
442 + t189 * t192 * t203 * t205 * 2.0)
443 + -(t192 * t197 * t204 * t206 * 2.0);
444 t318 = ((((-(t126 * t189 * t202 * 2.0) + t312)
445 + ((t82 + t22 * t27 * 2.0) + -t80) * t203 * t206)
446 + t126 * t197 * t203 * t205 * 2.0)
447 + -(t189 * t198 * t203 * t205 * 2.0))
448 + t197 * t198 * t204 * t206 * 2.0;
449 t321 = ((((-(t130 * t189 * t202 * 2.0) + t317)
450 + -(((t78 + t25 * t26 * 2.0) + -t75) * t203 * t206))
451 + t130 * t197 * t203 * t205 * 2.0)
452 + -(t189 * t194 * t203 * t205 * 2.0))
453 + t194 * t197 * t204 * t206 * 2.0;
454 t323 = ((((t129 * t191 * t202 * 2.0 + t319)
455 + -(((t72 + t23 * t27 * 2.0) + -t73) * t203 * t206))
456 + t129 * t196 * t203 * t205 * 2.0)
457 + t191 * t199 * t203 * t205 * 2.0)
458 + t196 * t199 * t204 * t206 * 2.0;
459 t322 = ((((-(t125 * t190 * t202 * 2.0) + -t312)
460 + ((t80 + t20 * t28 * 2.0) + -t82) * t203 * t206)
461 + -(t125 * t200 * t203 * t205 * 2.0))
462 + t190 * t193 * t203 * t205 * 2.0)
463 + t193 * t200 * t204 * t206 * 2.0;
464 t316 = ((((t130 * t190 * t202 * 2.0 + -t319)
465 + -(((t73 + t24 * t26 * 2.0) + -t72) * t203 * t206))
466 + t130 * t200 * t203 * t205 * 2.0)
467 + t190 * t194 * t203 * t205 * 2.0)
468 + t194 * t200 * t204 * t206 * 2.0;
469 t65 = ((((-(t128 * t191 * t202 * 2.0) + -t317)
470 + -(((t75 + t23 * t28 * 2.0) + -t78) * t203 * t206))
471 + -(t128 * t196 * t203 * t205 * 2.0))
472 + t191 * t195 * t203 * t205 * 2.0)
473 + t195 * t196 * t204 * t206 * 2.0;
474 t66 = ((((-(t127 * t190 * t202 * 2.0) + t314) + ((t73 + t20 * t26 * 2.0) + -t72) * t203 * t206)
475 + -(t127 * t200 * t203 * t205 * 2.0))
476 + -(t190 * t192 * t203 * t205 * 2.0))
477 + -(t192 * t200 * t204 * t206 * 2.0);
478 t13 = ((((t128 * t190 * t202 * 2.0 + t315)
479 + -(((t80 + t24 * t28 * 2.0) + -t82) * t203 * t206))
480 + t128 * t200 * t203 * t205 * 2.0)
481 + -(t190 * t195 * t203 * t205 * 2.0))
482 + -(t195 * t200 * t204 * t206 * 2.0);
483 t12 = ((((-(t126 * t191 * t202 * 2.0) + -t314)
484 + ((t72 + t18 * t27 * 2.0) + -t73) * t203 * t206)
485 + -(t126 * t196 * t203 * t205 * 2.0))
486 + -(t191 * t198 * t203 * t205 * 2.0))
487 + -(t196 * t198 * t204 * t206 * 2.0);
488 t11 = ((((t129 * t189 * t202 * 2.0 + -t315)
489 + -(((t82 + t25 * t27 * 2.0) + -t80) * t203 * t206))
490 + -(t129 * t197 * t203 * t205 * 2.0))
491 + t189 * t199 * t203 * t205 * 2.0)
492 + -(t197 * t199 * t204 * t206 * 2.0);
493 H[0] = t151 * t202 * 2.0;
506 H[13] = t150 * t202 * 2.0;
519 H[26] = t149 * t202 * 2.0;
532 H[39] = ((t191 * t191 * t202 * 2.0 + t196 * t196 * t204 * t206 * 2.0)
533 - t646_tmp * (t45 + t46))
534 + t191 * t196 * t203 * t205 * 4.0;
547 H[52] = ((t190 * t190 * t202 * 2.0 + t200 * t200 * t204 * t206 * 2.0)
548 - t646_tmp * (t44 + t46))
549 + t190 * t200 * t203 * t205 * 4.0;
562 H[65] = ((t189 * t189 * t202 * 2.0 + t197 * t197 * t204 * t206 * 2.0)
563 - t646_tmp * (t44 + t45))
564 - t189 * t197 * t203 * t205 * 4.0;
577 H[78] = ((t130 * t130 * t202 * 2.0 + t194 * t194 * t204 * t206 * 2.0)
578 - t646_tmp * (t42 + t43))
579 + t130 * t194 * t203 * t205 * 4.0;
592 H[91] = ((t129 * t129 * t202 * 2.0 + t199 * t199 * t204 * t206 * 2.0)
593 - t646_tmp * (t41 + t43))
594 + t129 * t199 * t203 * t205 * 4.0;
607 H[104] = ((t128 * t128 * t202 * 2.0 + t195 * t195 * t204 * t206 * 2.0)
608 - t646_tmp * (t41 + t42))
609 - t128 * t195 * t203 * t205 * 4.0;
622 H[117] = ((t127 * t127 * t202 * 2.0 + t192 * t192 * t204 * t206 * 2.0)
623 - t646_tmp * (t39 + t40))
624 + t127 * t192 * t203 * t205 * 4.0;
637 H[130] = ((t126 * t126 * t202 * 2.0 + t198 * t198 * t204 * t206 * 2.0)
638 - t646_tmp * (t38 + t40))
639 + t126 * t198 * t203 * t205 * 4.0;
652 H[143] = ((t125 * t125 * t202 * 2.0 + t193 * t193 * t204 * t206 * 2.0)
653 - t646_tmp * (t38 + t39))
654 - t125 * t193 * t203 * t205 * 4.0;
659MUDA_GENERIC
void point_triangle_distance(
const Eigen::Vector<T, 3>& p,
660 const Eigen::Vector<T, 3>& t0,
661 const Eigen::Vector<T, 3>& t1,
662 const Eigen::Vector<T, 3>& t2,
665 const Eigen::Vector<T, 3> b = (t1 - t0).cross(t2 - t0);
666 T aTb = (p - t0).dot(b);
667 dist2 = aTb * aTb / b.squaredNorm();
671MUDA_GENERIC
void point_triangle_distance_gradient(
const Eigen::Vector<T, 3>& p,
672 const Eigen::Vector<T, 3>& t0,
673 const Eigen::Vector<T, 3>& t1,
674 const Eigen::Vector<T, 3>& t2,
675 Eigen::Vector<T, 12>& grad)
678 p[0], p[1], p[2], t0[0], t0[1], t0[2], t1[0], t1[1], t1[2], t2[0], t2[1], t2[2], grad.data());
682MUDA_GENERIC
void point_triangle_distance_hessian(
const Eigen::Vector<T, 3>& p,
683 const Eigen::Vector<T, 3>& t0,
684 const Eigen::Vector<T, 3>& t1,
685 const Eigen::Vector<T, 3>& t2,
686 Eigen::Matrix<T, 12, 12>& Hessian)