1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
| def getFootPoint(point, line_p1, line_p2):
x0 = point[0]
y0 = point[1]
z0 = point[2]
x1 = line_p1[0]
y1 = line_p1[1]
z1 = line_p1[2]
x2 = line_p2[0]
y2 = line_p2[1]
z2 = line_p2[2]
k = -((x1 - x0) * (x2 - x1) + (y1 - y0) * (y2 - y1) + (z1 - z0) * (z2 - z1)) / \
((x2 - x1) ** 2 + (y2 - y1) ** 2 + (z2 - z1) ** 2)*1.0
xn = k * (x2 - x1) + x1
yn = k * (y2 - y1) + y1
zn = k * (z2 - z1) + z1
return (xn, yn, zn)
print(getFootPoint([1,1,1], [0,0,0], [2,2,2]))
print(getFootPoint([1,1,1], [0,0,0], [2,3,2]))
|