Cylinder Line Minimum Distance plot

gistfile1.py

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()