python - Speeding up a closest point on a hyperbolic paraboloid algorithm

Question

I wrote a python script which finds the UV coords of the closest point on surface from a query point (p). The surface is defined by four linear edges made from four known points (p0,p1,p2,p3) listed counter clockwise.

(Please ignore the little red ball)

The problem with my approach is that it is very slow (~10 seconds to do 5000 queries with a low precision threshold.

I'm looking for a better approach to achieve what i want, or suggestions to make my code more efficient. My only constraint is that it must be written in python.

import numpy as np

# Define constants
LARGE_VALUE=99999999.0
SMALL_VALUE=0.00000001
SUBSAMPLES=10.0

def closestPointOnLineSegment(a,b,c):
    ''' Given two points (a,b) defining a line segment and a query point (c)
        return the closest point on that segment, the distance between
        query and closest points, and a u value derived from the results
    '''

    # Check if c is same as a or b
    ac=c-a
    AC=np.linalg.norm(ac)
    if AC==0.:
        return c,0.,0.

    bc=c-b
    BC=np.linalg.norm(bc)
    if BC==0.:
        return c,0.,1.


    # See if segment length is 0
    ab=b-a
    AB=np.linalg.norm(ab)
    if AB == 0.:
        return a,0.,0.

    # Normalize segment and do edge tests
    ab=ab/AB
    test=np.dot(ac,ab)
    if test < 0.:
        return a,AC,0.
    elif test > AB:
        return b,BC,1.

    # Return closest xyz on segment, distance, and u value
    p=(test*ab)+a
    return p,np.linalg.norm(c-p),(test/AB)




def surfaceWalk(e0,e1,p,v0=0.,v1=1.):
    ''' Walk on the surface along 2 edges, for each sample segment
        look for closest point, recurse until the both sampled edges
        are smaller than SMALL_VALUE
    '''

    edge0=(e1[0]-e0[0])
    edge1=(e1[1]-e0[1])
    len0=np.linalg.norm(edge0*(v1-v0))
    len1=np.linalg.norm(edge1*(v1-v0))

    vMin=v0
    vMax=v1
    closest_d=0.
    closest_u=0.
    closest_v=0.
    ii=0.
    dist=LARGE_VALUE

    for i in range(int(SUBSAMPLES)+1):
        v=v0+((v1-v0)*(i/SUBSAMPLES))
        a=(edge0*v)+e0[0]
        b=(edge1*v)+e0[1]

        closest_p,closest_d,closest_u=closestPointOnLineSegment(a,b,p)

        if closest_d < dist:
            dist=closest_d
            closest_v=v
            ii=i

    # If both edge lengths <= SMALL_VALUE, we're within our precision treshold so return results
    if len0 <= SMALL_VALUE and len1 <= SMALL_VALUE:
        return closest_p,closest_d,closest_u,closest_v

    # Threshold hasn't been met, set v0 anf v1 limits to either side of closest_v and keep recursing
    vMin=v0+((v1-v0)*(np.clip((ii-1),0.,SUBSAMPLES)/SUBSAMPLES))
    vMax=v0+((v1-v0)*(np.clip((ii+1),0.,SUBSAMPLES)/SUBSAMPLES))
    return surfaceWalk(e0,e1,p,vMin,vMax)




def closestPointToPlane(p0,p1,p2,p3,p,debug=True):
    ''' Given four points defining a quad surface (p0,p1,p2,3) and
        a query point p. Find the closest edge and begin walking
        across one end to the next until we find the closest point 
    '''

    # Find the closest edge, we'll use that edge to start our walk
    c,d,u,v=surfaceWalk([p0,p1],[p3,p2],p)
    if debug:
        print 'Closest Point:     %s'%c
        print 'Distance to Point: %s'%d
        print 'U Coord:           %s'%u
        print 'V Coord:           %s'%v

    return c,d,u,v



p0 = np.array([1.15, 0.62, -1.01])
p1 = np.array([1.74, 0.86, -0.88])
p2 = np.array([1.79, 0.40, -1.46])
p3 = np.array([0.91, 0.79, -1.84])
p =  np.array([1.17, 0.94, -1.52])
closestPointToPlane(p0,p1,p2,p3,p)


Closest Point:     [ 1.11588876  0.70474519 -1.52660706]
Distance to Point: 0.241488104197
U Coord:           0.164463481066
V Coord:           0.681959858995

Answer 1

If your surface is, as it seems, a hyperbolic paraboloid, you can parametrize a point s on it as:

s = p0 + u * (p1 - p0) + v * (p3 - p0) + u * v * (p2 - p3 - p1 + p0)

Doing things this way, the line p0p3 has equation u = 0, p1p2 is u = 1, p0p1 is v = 0 and p2p3 is v = 1. I haven't been able to figure out a way of coming up with an analytical expression for the closest point to a point p, but scipy.optimize can do the job numerically for you:

import numpy as np
from scipy.optimize import minimize

p0 = np.array([1.15, 0.62, -1.01])
p1 = np.array([1.74, 0.86, -0.88])
p2 = np.array([1.79, 0.40, -1.46])
p3 = np.array([0.91, 0.79, -1.84])
p =  np.array([1.17, 0.94, -1.52])

def fun(x, p0, p1, p2, p3, p):
    u, v = x
    s = u*(p1-p0) + v*(p3-p0) + u*v*(p2-p3-p1+p0) + p0
    return np.linalg.norm(p - s)

>>> minimize(fun, (0.5, 0.5), (p0, p1, p2, p3, p))
  status: 0
 success: True
    njev: 9
    nfev: 36
     fun: 0.24148810420527048
       x: array([ 0.16446403,  0.68196253])
 message: 'Optimization terminated successfully.'
    hess: array([[ 0.38032445,  0.15919791],
       [ 0.15919791,  0.44908365]])
     jac: array([ -1.27032399e-06,   6.74091280e-06])

The return of minimize is an object, you can access the values through its attributes:

>>> res = minimize(fun, (0.5, 0.5), (p0, p1, p2, p3, p))
>>> res.x # u and v coordinates of the nearest point
array([ 0.16446403,  0.68196253])
>>> res.fun # minimum distance
0.24148810420527048

---

Some pointers on how to go about finding a solution without scipy... The vector joining the point s parametrized as above with a generic point p is p-s. TO find out the closest point you can go two different ways that give the same result:

1. Compute the length of that vector, (p-s)**2, take its derivatives w.r.t. u and v and equate them to zero.
2. Compute two vectors tangent to the hypar at s, which can be found as ds/du and ds/dv, and impose that their inner product with p-s be zero.

If you work these out, you'll end up with two equations that would require a lot of manipulation to arrive at something like a third or fourth degree equation for either u or v, so there is no exact analytical solution, although you could solve that numerically with numpy only. An easier option is to work out those equations until you get these two equations, where a = p1-p0, b = p3-p0, c = p2-p3-p1+p0, s_ = s-p0, p_ = p-p0:

u = (p_ - v*b)*(a + v*c) / (a + v*c)**2
v = (p_ - u*a)*(b + u*c) / (b + u*c)**2

You can't come up with an analytical solution for this easily, but you can hope that if you use those two relations to iterate a trial solution, it will converge. For your test case it does work:

def solve_uv(x0, p0, p1, p2, p3, p, tol=1e-6, niter=100):
    a = p1 - p0
    b = p3 - p0
    c = p2 - p3 - p1 + p0
    p_ = p - p0
    u, v = x0
    error = None
    while niter and (error is None or error > tol):
        niter -= 1
        u_ = np.dot(p_ - v*b, a + v*c) / np.dot(a + v*c, a + v*c)
        v_ = np.dot(p_ - u*a, b + u*c) / np.dot(b + u*c, b + u*c)
        error = np.linalg.norm([u - u_, v - v_])
        u, v = u_, v_
    return np.array([u, v]), np.linalg.norm(u*a + v*b +u*v*c + p0 - p)

>>> solve_uv([0.5, 0.5], p0, p1, p2, p3, p)
(array([ 0.16446338,  0.68195998]), 0.2414881041967159)

I don't think this is guaranteed to converge, but for this particular case it seems to work pretty fine, and only needs 15 iterations to get to the requested tolerance.