MUDA
Loading...
Searching...
No Matches
point_edge.inl
1#include <Eigen/Geometry>
2
3namespace muda::distance
4{
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,
9 T& dist2)
10{
11 if constexpr(dim == 2)
12 {
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();
16 }
17 else
18 {
19 dist2 = (e0 - p).cross(e1 - p).squaredNorm() / (e1 - e0).squaredNorm();
20 }
21}
22
23namespace details
24{
25 template <class T>
26 MUDA_GENERIC void g_PE2D(T v01, T v02, T v11, T v12, T v21, T v22, T g[6])
27 {
28 T t13;
29 T t14;
30 T t23;
31 T t25;
32 T t24;
33 T t26;
34 T t27;
35
36 /* G_PE2D */
37 /* G = G_PE2D(V01,V02,V11,V12,V21,V22) */
38 /* This function was generated by the Symbolic Math Toolbox version 8.3. */
39 /* 16-Mar-2020 15:56:29 */
40 t13 = -v21 + v11;
41 t14 = -v22 + v12;
42 t23 = 1.0 / (t13 * t13 + t14 * t14);
43 t25 = ((v11 * v22 + -(v12 * v21)) + t14 * v01) + -(t13 * v02);
44 t24 = t23 * t23;
45 t26 = t25 * t25;
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;
50 t24 = t23 * t25;
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;
55 }
56
57 template <class T>
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])
59 {
60 T t17;
61 T t18;
62 T t19;
63 T t20;
64 T t21;
65 T t22;
66 T t23;
67 T t24;
68 T t25;
69 T t42;
70 T t44;
71 T t45;
72 T t46;
73 T t43;
74 T t50;
75 T t51;
76 T t52;
77
78 /* G_PE */
79 /* G = G_PE(V01,V02,V03,V11,V12,V13,V21,V22,V23) */
80 /* This function was generated by the Symbolic Math Toolbox version 8.3. */
81 /* 10-Jun-2019 18:02:37 */
82 t17 = -v11 + v01;
83 t18 = -v12 + v02;
84 t19 = -v13 + v03;
85 t20 = -v21 + v01;
86 t21 = -v22 + v02;
87 t22 = -v23 + v03;
88 t23 = -v21 + v11;
89 t24 = -v22 + v12;
90 t25 = -v23 + v13;
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);
95 t43 = t42 * t42;
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);
109 }
110
111 template <class T>
112 MUDA_GENERIC void H_PE2D(T v01, T v02, T v11, T v12, T v21, T v22, T H[36])
113 {
114 T t15;
115 T t16;
116 T t17;
117 T t18;
118 T t19;
119 T t20;
120 T t21;
121 T t22;
122 T t23;
123 T t24;
124 T t31;
125 T t34;
126 T t32;
127 T t33;
128 T t35;
129 T t60;
130 T t59;
131 T t62;
132 T t64;
133 T t65;
134 T t68;
135 T t71;
136 T t72;
137 T t75;
138 T t76;
139 T t77;
140 T t78;
141 T t79;
142 T t90;
143 T t92;
144 T t94;
145 T t96;
146 T t99;
147 T t93;
148 T t97;
149 T t98;
150 T t100;
151 T t102_tmp;
152
153 /* H_PE2D */
154 /* H = H_PE2D(V01,V02,V11,V12,V21,V22) */
155 /* This function was generated by the Symbolic Math Toolbox version 8.3. */
156 /* 16-Mar-2020 15:56:29 */
157 t15 = -v11 + v01;
158 t16 = -v12 + v02;
159 t17 = -v21 + v01;
160 t18 = -v22 + v02;
161 t19 = -v21 + v11;
162 t20 = -v22 + v12;
163 t21 = v11 * 2.0 + -(v21 * 2.0);
164 t22 = v12 * 2.0 + -(v22 * 2.0);
165 t23 = t19 * t19;
166 t24 = t20 * t20;
167 t31 = 1.0 / (t23 + t24);
168 t34 = ((v11 * v22 + -(v12 * v21)) + t20 * v01) + -(t19 * v02);
169 t32 = t31 * t31;
170 t33 = pow(t31, 3.0);
171 t35 = t34 * t34;
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;
205 H[1] = t59;
206 H[2] = t35;
207 H[3] = t97;
208 H[4] = t92;
209 H[5] = t99;
210 H[6] = t59;
211 H[7] = t23 * t31 * 2.0;
212 H[8] = t96;
213 H[9] = t94;
214 H[10] = t98;
215 H[11] = t93;
216 H[12] = t35;
217 H[13] = t96;
218 t35 = -t62 + t64;
219 H[14] = (t35 + t18 * t18 * t31 * 2.0) + t33 * 4.0;
220 H[15] = t19;
221 H[16] = t79;
222 H[17] = t77;
223 H[18] = t97;
224 H[19] = t94;
225 H[20] = t19;
226 t33 = -t62 + t65;
227 H[21] = (t33 + t17 * t17 * t31 * 2.0) - t102_tmp * 4.0;
228 H[22] = t78;
229 H[23] = t22;
230 H[24] = t92;
231 H[25] = t98;
232 H[26] = t79;
233 H[27] = t78;
234 H[28] = (t35 + t16 * t16 * t31 * 2.0) + t20 * 4.0;
235 H[29] = t100;
236 H[30] = t99;
237 H[31] = t93;
238 H[32] = t77;
239 H[33] = t22;
240 H[34] = t100;
241 H[35] = (t33 + t15 * t15 * t31 * 2.0) - t76 * 4.0;
242 }
243
244 template <class T>
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])
246 {
247 T t17;
248 T t18;
249 T t19;
250 T t20;
251 T t21;
252 T t22;
253 T t23;
254 T t24;
255 T t25;
256 T t26;
257 T t27;
258 T t28;
259 T t35;
260 T t36;
261 T t37;
262 T t50;
263 T t51;
264 T t52;
265 T t53;
266 T t54;
267 T t55;
268 T t56;
269 T t62;
270 T t70;
271 T t71;
272 T t75;
273 T t79;
274 T t80;
275 T t84;
276 T t88;
277 T t38;
278 T t39;
279 T t40;
280 T t41;
281 T t42;
282 T t43;
283 T t44;
284 T t46;
285 T t48;
286 T t57;
287 T t58;
288 T t60;
289 T t63;
290 T t65;
291 T t67;
292 T t102;
293 T t103;
294 T t104;
295 T t162;
296 T t163;
297 T t164;
298 T t213;
299 T t214;
300 T t215;
301 T t216;
302 T t217;
303 T t218;
304 T t225;
305 T t226;
306 T t227;
307 T t229;
308 T t230;
309 T t311;
310 T t231;
311 T t232;
312 T t233;
313 T t234;
314 T t235;
315 T t236;
316 T t237;
317 T t238;
318 T t239;
319 T t240;
320 T t245;
321 T t279;
322 T t281;
323 T t282;
324 T t283;
325 T t287;
326 T t289;
327 T t247;
328 T t248;
329 T t249;
330 T t250;
331 T t251;
332 T t252;
333 T t253;
334 T t293;
335 T t295;
336 T t299;
337 T t300;
338 T t303;
339 T t304;
340 T t294;
341 T t297;
342 T t301;
343 T t302;
344
345 /* H_PE */
346 /* H = H_PE(V01,V02,V03,V11,V12,V13,V21,V22,V23) */
347 /* This function was generated by the Symbolic Math Toolbox version 8.3. */
348 /* 10-Jun-2019 18:02:39 */
349 t17 = -v11 + v01;
350 t18 = -v12 + v02;
351 t19 = -v13 + v03;
352 t20 = -v21 + v01;
353 t21 = -v22 + v02;
354 t22 = -v23 + v03;
355 t23 = -v21 + v11;
356 t24 = -v22 + v12;
357 t25 = -v23 + v13;
358 t26 = v11 * 2.0 + -(v21 * 2.0);
359 t27 = v12 * 2.0 + -(v22 * 2.0);
360 t28 = v13 * 2.0 + -(v23 * 2.0);
361 t35 = t23 * t23;
362 t36 = t24 * t24;
363 t37 = t25 * t25;
364 t50 = t17 * t21;
365 t51 = t18 * t20;
366 t52 = t17 * t22;
367 t53 = t19 * t20;
368 t54 = t18 * t22;
369 t55 = t19 * t21;
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;
385 t44 = t35 * 2.0;
386 t46 = t36 * 2.0;
387 t48 = t37 * 2.0;
388 t57 = t50 * 2.0;
389 t58 = t51 * 2.0;
390 t60 = t52 * 2.0;
391 t63 = t53 * 2.0;
392 t65 = t54 * 2.0;
393 t67 = t55 * 2.0;
394 t102 = 1.0 / ((t35 + t36) + t37);
395 t36 = t50 + -t51;
396 t35 = t52 + -t53;
397 t37 = t54 + -t55;
398 t103 = t102 * t102;
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);
413 t36 = t26 * t103;
414 t229 = t36 * t213;
415 t37 = t27 * t103;
416 t230 = t37 * t213;
417 t311 = t28 * t103;
418 t231 = t311 * t213;
419 t232 = t36 * t214;
420 t233 = t37 * t214;
421 t234 = t311 * t214;
422 t235 = t36 * t215;
423 t236 = t37 * t215;
424 t237 = t311 * t215;
425 t238 = t36 * t216;
426 t239 = t37 * t216;
427 t240 = t311 * t216;
428 t214 = t36 * t217;
429 t215 = t37 * t217;
430 t216 = t311 * t217;
431 t217 = t36 * t218;
432 t245 = t37 * t218;
433 t213 = t311 * t218;
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;
441 t247 = t36 * t225;
442 t248 = t37 * t225;
443 t249 = t311 * t225;
444 t250 = t36 * t226;
445 t251 = t37 * t226;
446 t252 = t311 * t226;
447 t253 = t36 * t227;
448 t35 = t37 * t227;
449 t36 = t311 * t227;
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);
484 H[1] = t162;
485 H[2] = t163;
486 H[3] = t88;
487 H[4] = t84;
488 H[5] = t80;
489 H[6] = t293;
490 H[7] = t303;
491 H[8] = t304;
492 H[9] = t162;
493 H[10] = t102 * (t44 + t48);
494 H[11] = t164;
495 H[12] = t301;
496 H[13] = t297;
497 H[14] = t302;
498 H[15] = t215;
499 H[16] = t216;
500 H[17] = t214;
501 H[18] = t163;
502 H[19] = t164;
503 H[20] = t102 * (t44 + t46);
504 H[21] = t299;
505 H[22] = t300;
506 H[23] = t295;
507 H[24] = t75;
508 H[25] = t227;
509 H[26] = t294;
510 H[27] = t88;
511 H[28] = t301;
512 H[29] = t299;
513 H[30] = ((t235 * 2.0 + -t279) + t281) + t102 * (t42 + t43);
514 H[31] = t37;
515 H[32] = t245;
516 H[33] = t217;
517 H[34] = t27;
518 H[35] = t28;
519 H[36] = t84;
520 H[37] = t297;
521 H[38] = t300;
522 H[39] = t37;
523 H[40] = ((t251 * -2.0 + -t279) + t282) + t102 * (t41 + t43);
524 H[41] = t35;
525 H[42] = t103;
526 H[43] = t218;
527 H[44] = t226;
528 H[45] = t80;
529 H[46] = t302;
530 H[47] = t295;
531 H[48] = t245;
532 H[49] = t35;
533 H[50] = ((t240 * -2.0 + -t279) + t283) + t102 * (t41 + t42);
534 H[51] = t26;
535 H[52] = t225;
536 H[53] = t104;
537 H[54] = t293;
538 H[55] = t215;
539 H[56] = t75;
540 H[57] = t217;
541 H[58] = t103;
542 H[59] = t26;
543 H[60] = ((t229 * 2.0 + -t279) + t281) + t102 * (t39 + t40);
544 H[61] = t213;
545 H[62] = t311;
546 H[63] = t303;
547 H[64] = t216;
548 H[65] = t227;
549 H[66] = t27;
550 H[67] = t218;
551 H[68] = t225;
552 H[69] = t213;
553 H[70] = ((t248 * -2.0 + -t279) + t282) + t102 * (t38 + t40);
554 H[71] = t36;
555 H[72] = t304;
556 H[73] = t214;
557 H[74] = t294;
558 H[75] = t28;
559 H[76] = t226;
560 H[77] = t104;
561 H[78] = t311;
562 H[79] = t36;
563 H[80] = ((t234 * -2.0 + -t279) + t283) + t102 * (t38 + t39);
564 }
565} // namespace details
566
567
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)
573{
574 if constexpr(dim == 2)
575 {
576 details::g_PE2D(p[0], p[1], e0[0], e0[1], e1[0], e1[1], grad.data());
577 }
578 else
579 {
580 details::g_PE3D(p[0], p[1], p[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2], grad.data());
581 }
582}
583
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)
589{
590 if constexpr(dim == 2)
591 {
592 details::H_PE2D(p[0], p[1], e0[0], e0[1], e1[0], e1[1], Hessian.data());
593 }
594 else
595 {
596 details::H_PE3D(
597 p[0], p[1], p[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2], Hessian.data());
598 }
599}
600} // namespace muda::distance