Skip to content

Commit d2c6d37

Browse files
liminchenliminchen
committed
point-edge distance functions
1 parent 1034605 commit d2c6d37

3 files changed

Lines changed: 177 additions & 0 deletions

File tree

‎self_contact/distance/PointEdgeDistance.py‎

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
import numpy as np
2+
3+
import PointPointDistance as PP
4+
import PointLineDistance as PL
5+
6+
def val(p, e0, e1):
7+
e = e1 - e0
8+
ratio = np.dot(e, p - e0) / np.dot(e, e)
9+
if ratio < 0: # point(p)-point(e0) expression
10+
return PP.val(p, e0)
11+
elif ratio > 1: # point(p)-point(e1) expression
12+
return PP.val(p, e1)
13+
else: # point(p)-line(e0e1) expression
14+
return PL.val(p, e0, e1)
15+
16+
def grad(p, e0, e1):
17+
e = e1 - e0
18+
ratio = np.dot(e, p - e0) / np.dot(e, e)
19+
if ratio < 0: # point(p)-point(e0) expression
20+
g_PP = PP.grad(p, e0)
21+
return np.reshape([g_PP[0:2], g_PP[2:4], np.array([0.0, 0.0])], (1, 6))[0]
22+
elif ratio > 1: # point(p)-point(e1) expression
23+
g_PP = PP.grad(p, e1)
24+
return np.reshape([g_PP[0:2], np.array([0.0, 0.0]), g_PP[2:4]], (1, 6))[0]
25+
else: # point(p)-line(e0e1) expression
26+
return PL.grad(p, e0, e1)
27+
28+
def hess(p, e0, e1):
29+
e = e1 - e0
30+
ratio = np.dot(e, p - e0) / np.dot(e, e)
31+
if ratio < 0: # point(p)-point(e0) expression
32+
H_PP = PP.hess(p, e0)
33+
return np.array([np.reshape([H_PP[0, 0:2], H_PP[0, 2:4], np.array([0.0, 0.0])], (1, 6))[0], \
34+
np.reshape([H_PP[1, 0:2], H_PP[1, 2:4], np.array([0.0, 0.0])], (1, 6))[0], \
35+
np.reshape([H_PP[2, 0:2], H_PP[2, 2:4], np.array([0.0, 0.0])], (1, 6))[0], \
36+
np.reshape([H_PP[3, 0:2], H_PP[3, 2:4], np.array([0.0, 0.0])], (1, 6))[0], \
37+
np.array([0.0] * 6), \
38+
np.array([0.0] * 6)])
39+
elif ratio > 1: # point(p)-point(e1) expression
40+
H_PP = PP.hess(p, e1)
41+
return np.array([np.reshape([H_PP[0, 0:2], np.array([0.0, 0.0]), H_PP[0, 2:4]], (1, 6))[0], \
42+
np.reshape([H_PP[1, 0:2], np.array([0.0, 0.0]), H_PP[1, 2:4]], (1, 6))[0], \
43+
np.array([0.0] * 6), \
44+
np.array([0.0] * 6), \
45+
np.reshape([H_PP[2, 0:2], np.array([0.0, 0.0]), H_PP[2, 2:4]], (1, 6))[0], \
46+
np.reshape([H_PP[3, 0:2], np.array([0.0, 0.0]), H_PP[3, 2:4]], (1, 6))[0]])
47+
else: # point(p)-line(e0e1) expression
48+
return PL.hess(p, e0, e1)

‎self_contact/distance/PointLineDistance.py‎

Lines changed: 114 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,114 @@
1+
import numpy as np
2+
3+
def val(p, e0, e1):
4+
e = e1 - e0
5+
numerator = e[1] * p[0] - e[0] * p[1] + e1[0] * e0[1] - e1[1] * e0[0]
6+
return numerator * numerator / np.dot(e, e)
7+
8+
def grad(p, e0, e1):
9+
g = np.array([0.0] * 6)
10+
t13 = -e1[0] + e0[0]
11+
t14 = -e1[1] + e0[1]
12+
t23 = 1.0 / (t13 * t13 + t14 * t14)
13+
t25 = ((e0[0] * e1[1] + -(e0[1] * e1[0])) + t14 * p[0]) + -(t13 * p[1])
14+
t24 = t23 * t23
15+
t26 = t25 * t25
16+
t27 = (e0[0] * 2.0 + -(e1[0] * 2.0)) * t24 * t26
17+
t26 *= (e0[1] * 2.0 + -(e1[1] * 2.0)) * t24
18+
g[0] = t14 * t23 * t25 * 2.0
19+
g[1] = t13 * t23 * t25 * -2.0
20+
t24 = t23 * t25
21+
g[2] = -t27 - t24 * (-e1[1] + p[1]) * 2.0
22+
g[3] = -t26 + t24 * (-e1[0] + p[0]) * 2.0
23+
g[4] = t27 + t24 * (p[1] - e0[1]) * 2.0
24+
g[5] = t26 - t24 * (p[0] - e0[0]) * 2.0
25+
return g
26+
27+
def hess(p, e0, e1):
28+
H = np.array([0.0] * 36)
29+
t15 = -e0[0] + p[0]
30+
t16 = -e0[1] + p[1]
31+
t17 = -e1[0] + p[0]
32+
t18 = -e1[1] + p[1]
33+
t19 = -e1[0] + e0[0]
34+
t20 = -e1[1] + e0[1]
35+
t21 = e0[0] * 2.0 + -(e1[0] * 2.0)
36+
t22 = e0[1] * 2.0 + -(e1[1] * 2.0)
37+
t23 = t19 * t19
38+
t24 = t20 * t20
39+
t31 = 1.0 / (t23 + t24)
40+
t34 = ((e0[0] * e1[1] + -(e0[1] * e1[0])) + t20 * p[0]) + -(t19 * p[1])
41+
t32 = t31 * t31
42+
t33 = t32 * t31
43+
t35 = t34 * t34
44+
t60 = t31 * t34 * 2.0
45+
t59 = -(t19 * t20 * t31 * 2.0)
46+
t62 = t32 * t35 * 2.0
47+
t64 = t21 * t21 * t33 * t35 * 2.0
48+
t65 = t22 * t22 * t33 * t35 * 2.0
49+
t68 = t15 * t21 * t32 * t34 * 2.0
50+
t71 = t16 * t22 * t32 * t34 * 2.0
51+
t72 = t17 * t21 * t32 * t34 * 2.0
52+
t75 = t18 * t22 * t32 * t34 * 2.0
53+
t76 = t19 * t21 * t32 * t34 * 2.0
54+
t77 = t20 * t21 * t32 * t34 * 2.0
55+
t78 = t19 * t22 * t32 * t34 * 2.0
56+
t79 = t20 * t22 * t32 * t34 * 2.0
57+
t90 = t21 * t22 * t33 * t35 * 2.0
58+
t92 = t16 * t20 * t31 * 2.0 + t77
59+
t94 = -(t17 * t19 * t31 * 2.0) + t78
60+
t96 = (t18 * t19 * t31 * 2.0 + -t60) + t76
61+
t99 = (-(t15 * t20 * t31 * 2.0) + -t60) + t79
62+
t93 = t15 * t19 * t31 * 2.0 + -t78
63+
t35 = -(t18 * t20 * t31 * 2.0) + -t77
64+
t97 = (t17 * t20 * t31 * 2.0 + t60) + -t79
65+
t98 = (-(t16 * t19 * t31 * 2.0) + t60) + -t76
66+
t100 = ((-(t15 * t16 * t31 * 2.0) + t71) + -t68) + t90
67+
t19 = ((-(t17 * t18 * t31 * 2.0) + t75) + -t72) + t90
68+
t102_tmp = t17 * t22 * t32 * t34
69+
t76 = t15 * t22 * t32 * t34
70+
t22 = (((-(t15 * t17 * t31 * 2.0) + t62) + -t65) + t76 * 2.0) + t102_tmp * 2.0
71+
t33 = t18 * t21 * t32 * t34
72+
t20 = t16 * t21 * t32 * t34
73+
t79 = (((-(t16 * t18 * t31 * 2.0) + t62) + -t64) + -(t20 * 2.0)) + -(t33 * 2.0)
74+
t77 = (((t15 * t18 * t31 * 2.0 + t60) + t68) + -t75) + -t90
75+
t78 = (((t16 * t17 * t31 * 2.0 + -t60) + t72) + -t71) + -t90
76+
H[0] = t24 * t31 * 2.0
77+
H[1] = t59
78+
H[2] = t35
79+
H[3] = t97
80+
H[4] = t92
81+
H[5] = t99
82+
H[6] = t59
83+
H[7] = t23 * t31 * 2.0
84+
H[8] = t96
85+
H[9] = t94
86+
H[10] = t98
87+
H[11] = t93
88+
H[12] = t35
89+
H[13] = t96
90+
t35 = -t62 + t64
91+
H[14] = (t35 + t18 * t18 * t31 * 2.0) + t33 * 4.0
92+
H[15] = t19
93+
H[16] = t79
94+
H[17] = t77
95+
H[18] = t97
96+
H[19] = t94
97+
H[20] = t19
98+
t33 = -t62 + t65
99+
H[21] = (t33 + t17 * t17 * t31 * 2.0) - t102_tmp * 4.0
100+
H[22] = t78
101+
H[23] = t22
102+
H[24] = t92
103+
H[25] = t98
104+
H[26] = t79
105+
H[27] = t78
106+
H[28] = (t35 + t16 * t16 * t31 * 2.0) + t20 * 4.0
107+
H[29] = t100
108+
H[30] = t99
109+
H[31] = t93
110+
H[32] = t77
111+
H[33] = t22
112+
H[34] = t100
113+
H[35] = (t33 + t15 * t15 * t31 * 2.0) - t76 * 4.0
114+
return np.reshape(H, (6, 6))

‎self_contact/distance/PointPointDistance.py‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
import numpy as np
2+
3+
def val(p0, p1):
4+
e = p0 - p1
5+
return np.dot(e, e)
6+
7+
def grad(p0, p1):
8+
e = p0 - p1
9+
return np.reshape([2 * e, -2 * e], (1, 4))[0]
10+
11+
def hess(p0, p1):
12+
H = np.array([[0.0] * 4] * 4)
13+
H[0, 0] = H[1, 1] = H[2, 2] = H[3, 3] = 2
14+
H[0, 2] = H[1, 3] = H[2, 0] = H[3, 1] = -2
15+
return H

0 commit comments

Comments
 (0)