Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Cylinder Line Minimum Distance plot
from mpl_toolkits.mplot3d import axes3d
import matplotlib.pyplot as plt
import numpy as np
import scipy as sp
import scipy.optimize
def cylinderLineFirstInterval(cylinderLength, lineP0, lineP1):
l = cylinderLength
p0 = lineP0
p1 = lineP1
b0 = p0+0
b1 = p1+0
if min(b0[2], b1[2]) < 0:
if max(b0[2], b1[2]) > 0:
bd = b1-b0
if bd[2] > 0:
ty = b1[2]/bd[2]
b1 = b1 - ty*bd
else:
ty = b0[2]/bd[2]
b0 = b0 - ty*bd
return b0, b1
return None, None
def cylinderLineSecondInterval(cylinderLength, lineP0, lineP1):
l = cylinderLength
p0 = lineP0
p1 = lineP1
b0 = p0+0
b1 = p1+0
if min(b0[2],b1[2]) <= l and max(b0[2],b1[2]) >= 0:
bd = b1-b0
if b0[2] < 0:
ty = (-b0[2])/(bd[2])
b0 = b0 + ty*bd
if b1[2] > l:
ty = (b1[2]-l)/(bd[2])
b1 = b1 - ty*bd
return b0, b1
return None, None
def cylinderLineThirdInterval(cylinderLength, lineP0, lineP1):
l = cylinderLength
p0 = lineP0
p1 = lineP1
b0 = p0+0
b1 = p1+0
if max(b0[2], b1[2]) > l:
if min(b0[2], b1[2]) < l:
bd = b1-b0
if bd[2] > 0:
ty = (l-b0[2])/bd[2]
b0 = b0 + ty*bd
else:
ty = (l-b1[2])/bd[2]
b1 = b1 - ty*bd
return b0, b1
return None, None
def origin2DLineClosestPoint(lineP0, lineP1):
p0 = lineP0
p1 = lineP1
o = np.array([0.0, 0.0])
d = o-p0
e = p1 - p0
e2 = e.dot(e)
ed = e.dot(d)
t = ed/e2
if t < 0.0:
t = 0.0
elif t > 1.0:
t = 1.0
h = p0 + e*t
return o, h
# based on: https://github.com/timprepscius/GeometricTools/blob/master/WildMagic5/LibMathematics/Distance/Wm5DistPoint3Circle3.cpp
def circlePointClosestPoint(circleRadius, circleHeight, p):
r = circleRadius
h = circleHeight
circleOrigin = np.array([0.0, 0.0, h])
circleNormal = np.array([0.0, 0.0, 1.0])
diff0 = p - circleOrigin
dist = diff0.dot(circleNormal)
diff1 = diff0 - dist*circleNormal
sqrLen = np.linalg.norm(diff1)**2
sqrDistance = 0.0
closestPoint = circleOrigin+0
if sqrLen > 0:
closestPoint = circleOrigin + (r/(sqrLen)**0.5)*diff1
return p, closestPoint
def diskPointClosestPoint(diskRadius, diskHeight, p):
r = diskRadius
h = diskHeight
pz = p+0
pz[2] = h
diskOrigin = np.array([0.0, 0.0, h])
pd = np.linalg.norm(pz-diskOrigin)
if pd < r:
return p, pz
return circlePointClosestPoint(r, h, p)
def diskPointDistance(diskRadius, diskHeight, p):
a, b = diskPointClosestPoint(diskRadius, diskHeight, p)
return np.linalg.norm(a-b)
def diskLineClosestPoint(diskRadius, diskHeight, lineP0, lineP1):
r = diskRadius
h = diskHeight
p0 = lineP0
p1 = lineP1
pd = p1-p0
f = lambda t: diskPointDistance(r, h, p0 + t*pd)
res = sp.optimize.minimize(f, 0.5, bounds=[[0.0,1.0]])
t = res.x
p = p0+t*pd
closestPoint0, closestPoint1 = diskPointClosestPoint(diskRadius, diskHeight, p)
return closestPoint0, closestPoint1
def cylinderZAxisLineClosestPoint(cylinderRadius, cylinderLength, lineP0, lineP1):
r = cylinderRadius
l = cylinderLength
p0 = lineP0
p1 = lineP1
b0 = p0+0
b1 = p1+0
x0 = b1-b0
if np.all(b0 == b1):
b0[0] += 0.001
o = np.array([0.0, 0.0])
b02d = b0[0:2]
b12d = b1[0:2]
ha2d, hb2d = origin2DLineClosestPoint(b02d, b12d)
hz = b0[2] + x0[2]*((hb2d[0] - b0[0])/x0[0])
hb = np.array([hb2d[0], hb2d[1], hz])
ha = np.array([ha2d[0], ha2d[1], hz])
hd = hb-ha
hdd = np.linalg.norm(hd)
if hdd > 0.001:
hd = hd/np.linalg.norm(hd)
ha = ha + r*hd
else:
ha = ha + np.array([1.0, 0, 0]) * r
return ha, hb
def cylinderZAxisPointClosestPoint(cylinderRadius, cylinderLength, lineP0, lineP1, p):
pd = lineP1-lineP0
p0 = p + pd*0.001
a, b = cylinderZAxisLineClosestPoint(cylinderRadius, cylinderLength, p, p0)
if a != None:
return a, b
def plotLine(axes, p0, p1):
axes.plot([p0[0],p1[0]], [p0[1],p1[1]], [p0[2], p1[2]])
def plotPoint(axes, p):
axes.plot([p[0]], [p[1]], [p[2]], marker='o', ms=10)
def plotDistance(axes, p0, p1):
axes.plot([p0[0],p1[0]], [p0[1],p1[1]], [p0[2],p1[2]], marker='o', ms=7)
def plotDistanceFunction(axeslocal, f, p0, p1, i0, i1):
ind = i1-i0
pd = p1-p0
dp = np.linalg.norm(p1-p0)
di0p0 = np.linalg.norm(i0-p0)
di1p0 = np.linalg.norm(i1-p0)
tmin = di0p0/dp
tmax = di1p0/dp
x = []
y = []
for t in np.linspace(tmin, tmax, 10):
x.append(t)
a, b = f(p0 + t*pd)
# show debug distance of below plot
#plotDistance(axes, a, b)
y.append(np.linalg.norm(a-b))
axeslocal.plot(x, y)
fig = plt.figure()
axes = fig.add_subplot(1, 2, 1, projection='3d')
axes_dist = fig.add_subplot(1, 2, 2)
#axes = axes3d.Axes3D(fig,azim=30,elev=30)
r = 3
l = 20
x = np.linspace(-r,r,10)
z = np.linspace(0,l,10)
X, Z = np.meshgrid(x, z)
Y = np.sqrt(r*r-X**2)
axes.plot_wireframe(X, Y, Z, color="#993333")
axes.plot_wireframe(X, -Y, Z, color="#993333")
p0 = np.array([-10, -10, -20.0])
p1 = np.array([5, 10, 30.0])
p0 = np.array([-10, -10, -10.0])
p1 = np.array([10, -10, 30.0])
#p0 = np.array([10, -50, -20.0])
#p1 = np.array([5, 10, 30.0])
#p0 = np.array([10, -10, 10.0])
#p1 = np.array([-10, -10, 10.0])
#p0 = np.array([0.0, 0.0, 25])
#p1 = np.array([0.0, 0.0, 35])
#p0 = np.array([0.0, 0.0, 10.0])
#p1 = np.array([10.0, 10.0, 10.0])
#p0 = np.array([10.0, 10.0, 10.0])
#p1 = np.array([-10.0, -10.0, 10.0])
#p0 = np.array([2.0, -10.0, 10.0])
#p1 = np.array([2.0, 10.0, 10.0])
#p0 = np.array([10.0, -10.0, 30.0])
#p1 = np.array([-2.0, 10.0, 24.0])
#p0 = np.array([0.0, -10.0, 30.0])
#p1 = np.array([0.0, 10.0, 24.0])
i10, i11 = cylinderLineFirstInterval(l, p0, p1)
if i10 != None:
plotLine(axes, i10, i11)
plotDistanceFunction(axes_dist, lambda p: diskPointClosestPoint(r, 0, p), p0, p1, i10, i11)
''
i20, i21 = cylinderLineSecondInterval(l, p0, p1)
if i20 != None:
plotLine(axes, i20, i21)
plotDistanceFunction(axes_dist, lambda p: cylinderZAxisPointClosestPoint(r, l, p0, p1, p), p0, p1, i20, i21)
''
i30, i31 = cylinderLineThirdInterval(l, p0, p1)
if i30 != None:
plotLine(axes, i30, i31)
plotDistanceFunction(axes_dist, lambda p: diskPointClosestPoint(r, l, p), p0, p1, i30, i31)
if i10 != None:
h0a, h0b = diskLineClosestPoint(r, 0.0, i10, i11)
if h0a != None:
plotDistance(axes, h0a, h0b)
if i20 != None:
h1a, h1b = cylinderZAxisLineClosestPoint(r, l, i20, i21)
if h1a != None:
plotDistance(axes, h1a,h1b)
if i30 != None:
h2a, h2b = diskLineClosestPoint(r, l, i30, i31)
if h2a != None:
plotDistance(axes, h2a, h2b)
'''
xmin,xmax = axes.get_xlim()
ymin,ymax = axes.get_ylim()
zmin,zmax = axes.get_zlim()
xmin = min(xmin,ymin,zmin)
xmax = max(xmax,ymax,zmax)
axes.set_xlim(xmin,xmax)
axes.set_ylim(xmin,xmax)
axes.set_zlim(xmin,xmax)
'''
axes.set_xlabel('x')
axes.set_ylabel('y')
axes.set_zlabel('z')
axes_dist.set_xlabel('t')
axes_dist.set_ylabel('distance')
plt.show()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.