How do you check for intersection between a line segment and a line ray emanating from a point at an angle from horizontal? - graphics

Given a line segment, that is two points (x1,y1) and (x2,y2), one point P(x,y) and an angle theta. How do we find if this line segment and the line ray that emanates from P at an angle theta from horizontal intersects or not? If they do intersect, how to find the point of intersection?

Let's label the points q = (x1, y1) and q + s = (x2, y2). Hence s = (x2 − x1, y2 − y1). Then the problem looks like this:
Let r = (cos θ, sin θ). Then any point on the ray through p is representable as p + t r (for a scalar parameter 0 ≤ t) and any point on the line segment is representable as q + u s (for a scalar parameter 0 ≤ u ≤ 1).
The two lines intersect if we can find t and u such that p + t r = q + u s:
See this answer for how to find this point (or determine that there is no such point).
Then your line segment intersects the ray if 0 ≤ t and 0 ≤ u ≤ 1.

Here is a C# code for the algorithm given in other answers:
/// <summary>
/// Returns the distance from the ray origin to the intersection point or null if there is no intersection.
/// </summary>
public double? GetRayToLineSegmentIntersection(Point rayOrigin, Vector rayDirection, Point point1, Point point2)
{
var v1 = rayOrigin - point1;
var v2 = point2 - point1;
var v3 = new Vector(-rayDirection.Y, rayDirection.X);
var dot = v2 * v3;
if (Math.Abs(dot) < 0.000001)
return null;
var t1 = Vector.CrossProduct(v2, v1) / dot;
var t2 = (v1 * v3) / dot;
if (t1 >= 0.0 && (t2 >= 0.0 && t2 <= 1.0))
return t1;
return null;
}

Thanks Gareth for a great answer. Here is the solution implemented in Python. Feel free to remove the tests and just copy paste the actual function. I have followed the write-up of the methods that appeared here, https://rootllama.wordpress.com/2014/06/20/ray-line-segment-intersection-test-in-2d/.
import numpy as np
def magnitude(vector):
return np.sqrt(np.dot(np.array(vector),np.array(vector)))
def norm(vector):
return np.array(vector)/magnitude(np.array(vector))
def lineRayIntersectionPoint(rayOrigin, rayDirection, point1, point2):
"""
>>> # Line segment
>>> z1 = (0,0)
>>> z2 = (10, 10)
>>>
>>> # Test ray 1 -- intersecting ray
>>> r = (0, 5)
>>> d = norm((1,0))
>>> len(lineRayIntersectionPoint(r,d,z1,z2)) == 1
True
>>> # Test ray 2 -- intersecting ray
>>> r = (5, 0)
>>> d = norm((0,1))
>>> len(lineRayIntersectionPoint(r,d,z1,z2)) == 1
True
>>> # Test ray 3 -- intersecting perpendicular ray
>>> r0 = (0,10)
>>> r1 = (10,0)
>>> d = norm(np.array(r1)-np.array(r0))
>>> len(lineRayIntersectionPoint(r0,d,z1,z2)) == 1
True
>>> # Test ray 4 -- intersecting perpendicular ray
>>> r0 = (0, 10)
>>> r1 = (10, 0)
>>> d = norm(np.array(r0)-np.array(r1))
>>> len(lineRayIntersectionPoint(r1,d,z1,z2)) == 1
True
>>> # Test ray 5 -- non intersecting anti-parallel ray
>>> r = (-2, 0)
>>> d = norm(np.array(z1)-np.array(z2))
>>> len(lineRayIntersectionPoint(r,d,z1,z2)) == 0
True
>>> # Test ray 6 --intersecting perpendicular ray
>>> r = (-2, 0)
>>> d = norm(np.array(z1)-np.array(z2))
>>> len(lineRayIntersectionPoint(r,d,z1,z2)) == 0
True
"""
# Convert to numpy arrays
rayOrigin = np.array(rayOrigin, dtype=np.float)
rayDirection = np.array(norm(rayDirection), dtype=np.float)
point1 = np.array(point1, dtype=np.float)
point2 = np.array(point2, dtype=np.float)
# Ray-Line Segment Intersection Test in 2D
# http://bit.ly/1CoxdrG
v1 = rayOrigin - point1
v2 = point2 - point1
v3 = np.array([-rayDirection[1], rayDirection[0]])
t1 = np.cross(v2, v1) / np.dot(v2, v3)
t2 = np.dot(v1, v3) / np.dot(v2, v3)
if t1 >= 0.0 and t2 >= 0.0 and t2 <= 1.0:
return [rayOrigin + t1 * rayDirection]
return []
if __name__ == "__main__":
import doctest
doctest.testmod()

Note: this solution works without making vector classes or defining vector multiplication/division, but is longer to implement. It also avoids division by zero errors. If you just want a block of code and don’t care about the derivation, scroll to the bottom of the post.
Let’s say we have a ray defined by x, y, and theta, and a line defined by x1, y1, x2, and y2.
First, let’s draw two rays that point from the ray’s origin to the ends of the line segment. In pseudocode, that’s
theta1 = atan2(y1-y, x1-x);
theta2 = atan2(y2-y, x2-x);
Next we check whether the ray is inside these two new rays. They all have the same origin, so we only have to check the angles.
To make this easier, let’s shift all the angles so theta1 is on the x axis, then put everything back into a range of -pi to pi. In pseudocode that’s
dtheta = theta2-theta1; //this is where theta2 ends up after shifting
ntheta = theta-theta1; //this is where the ray ends up after shifting
dtheta = atan2(sin(dtheta), cos(dtheta))
ntheta = atan2(sin(ntheta), cos(ntheta))
(Note: Taking the atan2 of the sin and cos of the angle just resets the range of the angle to within -pi and pi without changing the angle.)
Now imagine drawing a line from theta2’s new location (dtheta) to theta1’s new location (0 radians). That’s where the line segment ended up.
The only time where the ray intersects the line segment is when theta is between theta1 and theta2, which is the same as when ntheta is between dtheta and 0 radians. Here is the corresponding pseudocode:
sign(ntheta)==sign(dtheta)&&
abs(ntheta)<=abs(dtheta)
This will tell you if the two lines intersect. Here it is in one pseudocode block:
theta1=atan2(y1-y, x1-x);
theta2=atan2(y2-y, x2-x);
dtheta=theta2-theta1;
ntheta=theta-theta1;
dtheta=atan2(sin(dtheta), cos(dtheta))
ntheta=atan2(sin(ntheta), cos(ntheta))
return (sign(ntheta)==sign(dtheta)&&
abs(ntheta)<=abs(dtheta));
Now that we know the points intersect, we need to find the point of intersection. We’ll be working from a completely clean slate here, so we can ignore any code up to this part. To find the point of intersection, you can use the following system of equations and solve for xp and yp, where lb and rb are the y-intercepts of the line segment and the ray, respectively.
y1=(y2-y1)/(x2-x1)*x1+lb
yp=(y2-y1)/(x2-x1)*xp+lb
y=sin(theta)/cos(theta)*x+rb
yp=sin(theta)/cos(theta)*x+rb
This yields the following formulas for xp and yp:
xp=(y1-(y2-y1)/(x2-x1)*x1-y+sin(theta)/cos(theta)*x)/(sin(theta)/cos(theta)-(y2-y1)/(x2-x1));
yp=sin(theta)/cos(theta)*xp+y-sin(theta)/cos(theta)*x
Which can be shortened by using lm=(y2-y1)/(x2-x1) and rm=sin(theta)/cos(theta)
xp=(y1-lm*x1-y+rm*x)/(rm-lm);
yp=rm*xp+y-rm*x;
You can avoid division by zero errors by checking if either the line or the ray is vertical then replacing every x and y with each other. Here’s the corresponding pseudocode:
if(x2-x1==0||cos(theta)==0){
let rm=cos(theta)/sin(theta);
let lm=(x2-x1)/(y2-y1);
yp=(x1-lm*y1-x+rm*y)/(rm-lm);
xp=rm*yp+x-rm*y;
}else{
let rm=sin(theta)/cos(theta);
let lm=(y2-y1)/(x2-x1);
xp=(y1-lm*x1-y+rm*x)/(rm-lm);
yp=rm*xp+y-rm*x;
}
TL;DR:
bool intersects(x1, y1, x2, y2, x, y, theta){
theta1=atan2(y1-y, x1-x);
theta2=atan2(y2-y, x2-x);
dtheta=theta2-theta1;
ntheta=theta-theta1;
dtheta=atan2(sin(dtheta), cos(dtheta))
ntheta=atan2(sin(ntheta), cos(ntheta))
return (sign(ntheta)==sign(dtheta)&&abs(ntheta)<=abs(dtheta));
}
point intersection(x1, y1, x2, y2, x, y, theta){
let xp, yp;
if(x2-x1==0||cos(theta)==0){
let rm=cos(theta)/sin(theta);
let lm=(x2-x1)/(y2-y1);
yp=(x1-lm*y1-x+rm*y)/(rm-lm);
xp=rm*yp+x-rm*y;
}else{
let rm=sin(theta)/cos(theta);
let lm=(y2-y1)/(x2-x1);
xp=(y1-lm*x1-y+rm*x)/(rm-lm);
yp=rm*xp+y-rm*x;
}
return (xp, yp);
}

Related

Solving cars moving in multiple lanes simulation problem

I am trying to simulate cars moving in multiple lanes in python. The problem is like this:
The number of cars, the roadlength, the probability and vmax are all input values.
Rules:
1. If vi < vmax, increase the velocity vi of car i by one unit, that is, vi → vi + 1. This change models the process of acceleration to the maximum velocity.
2. Compute the distance to the next car in the same lane and the distance to the cars in both (if there are 2) lanes next to the car.
If d=max([d1,d2,d3]) and vi ≥ d, then reduce the velocity to vi = d − 1 to prevent crashes and switch lane to the lane where the distance to the next car is d (if there are multiple choose one at random or whichever you want).
Else (meaning there is at least one lane next to the car's lane or it could be the same lane that the car is in where d > vi) go in that lane and don't change the velocity of the car if there is more than one lane, pick one at random.
3. With probability p, reduce the velocity of a moving car by one unit: vi → vi − 1, only do this when v > 0 to avoid negative velocities
4. Update the position xi of car i so that xi(t + 1) = xi(t) + vi
Also the path of the cars is circular, meaning there will be cars in front and behind.
Below is my attempt to solve the problem. Don't get confused over the variables theta and r. theta is just the position and r is the lane.
My attempt:
from matplotlib import pyplot as plt
import random
import math
from matplotlib import animation
import numpy as np
from operator import attrgetter
roadLength = 100
numFrames = 200
nlanes = 3
numCars = 20
posss =[]
theta = []
r = []
color = []
probability = 0.5
vmax = 1
cars=[]
class Car:
def __init__(self, position, velocity, lane):
self.position = position
self.velocity = velocity
self.lane = lane
def pos(car,k):
rand = random.uniform(0,1)
if car[k].velocity < vmax:
car[k].velocity += 1
dist = 0
if car[k].lane == 1:
temp_lanes_between = [0,1]
if car[k].lane == nlanes and nlanes != 1:
temp_lanes_between = [-1 ,0]
if 1 < car[k].lane < nlanes:
temp_lanes_between = [-1 ,0, 1]
iterator = []
for p in range(k+1, numCars):
iterator.append(p)
#if car[k+1].position - car[k].position <= car[k].velocity and car[k].lane == car[k+1].lane:
for p in range(k):
iterator.append(p)
for s in iterator:
if car[s].lane - car[k].lane in temp_lanes_between:
temp_lanes_between.remove(car[s].lane - car[k].lane)
distance = min([abs((car[s].position - car[k].position) % roadLength), roadLength - abs((car[s].position - car[k].position) % roadLength)])
if dist < distance:
dist = distance
l = car[s].lane
if dist <= car[k].velocity:
break
if temp_lanes_between:
j=random.randrange(0, len(temp_lanes_between))
car[k].lane += temp_lanes_between[j]
if temp_lanes_between == [] and dist <= car[k].velocity:
car[k].velocity = dist - 1
car[k].lane = l
if rand < probability and car[k].velocity > 0:
car[k].velocity = car[k].velocity - 1
car[k].position = car[k].position + car[k].velocity
return car[k].position
for i in range(numCars):
cars.append(Car(i, 0, 1))
theta.append(0)
r.append(1)
color.append(i)
posss.append(i)
fig = plt.figure()
ax = fig.add_subplot(111)
point, = ax.plot(posss, r, 'o')
ax.set_xlim(-10, 1.2*numFrames)
ax.set_ylim(-2, nlanes + 3)
def animate(frameNr):
sort_cars = sorted(cars, key=attrgetter("position"))
for i in range(numCars):
pos(sort_cars,i)
for k in range(numCars):
theta[k]=cars[k].position
r[k]=cars[k].lane
print(theta)
print(r)
point.set_data(theta, r)
return point,
def simulate():
anim = animation.FuncAnimation(fig, animate,
frames=numFrames, interval=100, blit=True, repeat=False)
plt.show()
simulate()
I get error saying: "local variable 'l' referenced before assignment" in the line where car[k].lane = l . I know that they mean that l doesn't have any value and therefore I get this error. But I don't see how this is possible. Every time pos() is run it should always go through the line l = car[s].lane and there it gets assigned a value. Maybe there are more errors in the code above but I have really given it my best shot and I don't know what to do.
Thanks in advance!

Magnetic dipole in python

I looked at this code:
import numpy as np
from matplotlib import pyplot as plt
def dipole(m, r, r0):
"""
Calculation of field B in point r. B is created by a dipole moment m located in r0.
"""
# R = r - r0 - subtraction of elements of vectors r and r0, transposition of array
R = np.subtract(np.transpose(r), r0).T
# Spatial components of r are the outermost axis
norm_R = np.sqrt(np.einsum("i...,i...", R, R)) # einsum - Einsteinova sumace
# Dot product of R and m
m_dot_R = np.tensordot(m, R, axes=1)
# Computation of B
B = 3 * m_dot_R * R / norm_R**5 - np.tensordot(m, 1 / norm_R**3, axes=0)
B *= 1e-7 # abbreviation for B = B * 1e-7, multiplication B of 1e-7, permeability of vacuum: 4\pi * 10^(-7)
# The result is the magnetic field B
return B
X = np.linspace(-1, 1)
Y = np.linspace(-1, 1)
Bx, By = dipole(m=[0, 1], r=np.meshgrid(X, Y), r0=[-0.2,0.8])
plt.figure(figsize=(8, 8))
plt.streamplot(X, Y, Bx, By)
plt.margins(0, 0)
plt.show()
It shows the following figure:
Is it possible to get coordinates of one line of force? I don't understand how it is plotted.
The streamplot returns a container object 'StreamplotSet' with two parts:
lines: a LineCollection of the streamlines
arrows: a PatchCollection containing FancyArrowPatch objects (these are the triangular arrows)
c.lines.get_paths() gives all the segments. Iterating through these segments, their vertices can be examined. When a segment starts where the previous ended, both belong to the same curve. Note that each segment is a short straight line; many segments are used together to form a streamline curve.
The code below demonstrates iterating through the segments. To show what's happening, each segment is converted to an array of 2D points suitable for plt.plot. Default, plt.plot colors each curve with a new color (repeating every 10). The dots show where each of the short straight segments are located.
To find one particular curve, you could hover with the mouse over the starting point, and note the x coordinate of that point. And then test for that coordinate in the code. As an example, the curve that starts near x=0.48 is drawn in a special way.
import numpy as np
from matplotlib import pyplot as plt
from matplotlib import patches
def dipole(m, r, r0):
R = np.subtract(np.transpose(r), r0).T
norm_R = np.sqrt(np.einsum("i...,i...", R, R))
m_dot_R = np.tensordot(m, R, axes=1)
B = 3 * m_dot_R * R / norm_R**5 - np.tensordot(m, 1 / norm_R**3, axes=0)
B *= 1e-7
return B
X = np.linspace(-1, 1)
Y = np.linspace(-1, 1)
Bx, By = dipole(m=[0, 1], r=np.meshgrid(X, Y), r0=[-0.2,0.8])
plt.figure(figsize=(8, 8))
c = plt.streamplot(X, Y, Bx, By)
c.lines.set_visible(False)
paths = c.lines.get_paths()
prev_end = None
start_indices = []
for index, segment in enumerate(paths):
if not np.array_equal(prev_end, segment.vertices[0]): # new segment
start_indices.append(index)
prev_end = segment.vertices[-1]
for i0, i1 in zip(start_indices, start_indices[1:] + [len(paths)]):
# get all the points of the curve that starts at index i0
curve = np.array([paths[i].vertices[0] for i in range(i0, i1)] + [paths[i1 - 1].vertices[-1]])
special_x_coord = 0.48
for i0, i1 in zip(start_indices, start_indices[1:] + [len(paths)]):
# get all the points of the curve that starts at index i0
curve = np.array([paths[i].vertices[0] for i in range(i0, i1)] + [paths[i1 - 1].vertices[-1]])
if abs(curve[0,0] - special_x_coord) < 0.01: # draw one curve in a special way
plt.plot(curve[:, 0], curve[:, 1], '-', lw=10, alpha=0.3)
else:
plt.plot(curve[:, 0], curve[:, 1], '.', ls='-')
plt.margins(0, 0)
plt.show()

Best-Fit without point interpolation

I have two sets of data. One is nominal form. The other is actual form. The problem is that when I wish to calculate the form error alone. It's a big problem when the two sets of data isn't "on top of each other". That gives errors that also include positional error.
Both curves are read from a series of data. The nominal shape (black) is made up from many different size radius that are tangent to each other. Its the leading edge of an airfoil profile.
I have tried various methods of "Best-Fit" I've found both here and on where ever google took me. But the problem is that they all smooth my "actual" data. So it get modified and is not keeping it's actual form.
Is there any function in scipy or any other python lib that "simply" can fit my two curves together without altering the actual shape?
I wish for the green curve with red dots to lie as much as possible on top of the black.
Might it be possible to calculate the center of gravity of both curves and then move the actual curve in x and y depending on the value difference from the center point? It might not be the ultimate solution, but it would get closer?
Here is a solution assuming that the nominal form can be described as a conic, i.a as solution of the equation ax^2 + by^2 + cxy + dx + ey = 1. Then, a least square fit can be applied to find the coefficients (a, b, c, d, e).
import numpy as np
import matplotlib.pylab as plt
# Generate example data
t = np.linspace(-2, 2.5, 25)
e, theta = 0.5, 0.3 # ratio minor axis/major & orientation angle major axis
c, s = np.cos(theta), np.sin(theta)
x = c*np.cos(t) - s*e*np.sin(t)
y = s*np.cos(t) + c*e*np.sin(t)
# add noise:
xy = 4*np.vstack((x, y))
xy += .08 *np.random.randn(*xy.shape) + np.random.randn(2, 1)
# Least square fit by a generic conic equation
# a*x^2 + b*y^2 + c*x*y + d*x + e*y = 1
x, y = xy
x = x - x.mean()
y = y - y.mean()
M = np.vstack([x**2, y**2, x*y, x, y]).T
b = np.ones_like(x)
# solve M*w = b
w, res, rank, s = np.linalg.lstsq(M, b, rcond=None)
a, b, c, d, e = w
# Get x, y coordinates for the fitted ellipse:
# using polar coordinates
# x = r*cos(theta), y = r*sin(theta)
# for a given theta, the radius is obtained with the 2nd order eq.:
# (a*ct^2 + b*st^2 + c*cs*st)*r^2 + (d*ct + e*st)*r - 1 = 0
# with ct = cos(theta) and st = sin(theta)
theta = np.linspace(-np.pi, np.pi, 97)
ct, st = np.cos(theta), np.sin(theta)
A = a*ct**2 + b*st**2 + c*ct*st
B = d*ct + e*st
D = B**2 + 4*A
radius = (-B + np.sqrt(D))/2/A
# Graph
plt.plot(radius*ct, radius*st, '-k', label='fitted ellipse');
plt.plot(x, y, 'or', label='measured points');
plt.axis('equal'); plt.legend();
plt.xlabel('x'); plt.ylabel('y');

Python 3.x Runge Kutta simple orbit

I am in the early stages of creating a program to plot orbits using the Runge-Kutta method, and would like to plot the orbit in 2D, however, no matter what the initial conditions are, i get a straight line. I have seen a similar question but it didn't solve my problem. Why is this happening?
import numpy as np
import matplotlib.pyplot as mpl
def derX(vx):
return vx
def derY(vy):
return vy
def derVx(x,y):
return -(G*M*x)/((x**2 + y**2)**(3/2))
def timestep(x,k1,k2,k3,k4):
return x + (step/6)*(k1 + 2*k2 +2*k3 + k4)
G=6.67408E-11 #m^3/kg s^2
M=5.972E24 #kg, mass of Earth
step=100 #seconds
x=4596194 #initial conditions in m and m/s
y=4596194
vx=-6646
vy=6646
t=0
T=3600
bodyx = 444 #stationary body position metres
bodyy = 444
tarray=[]
xarray=[]
yarray=[]
vxarray=[]
vyarray=[]
while t<T:
k1 = np.zeros(4)
k2 = np.zeros(4)
k3 = np.zeros(4)
k4 = np.zeros(4)
tarray.append(t)
xarray.append(x)
yarray.append(y)
vxarray.append(vx)
vyarray.append(vy)
x = bodyx - x
y = bodyy - y
k1[0]=derX(vx)
k1[1]=derY(vy)
k1[2]=derVx(x,y)
k1[3]=derVx(y,x)
k2[0]=derX(vx+(step/2)*k1[2])
k2[1]=derY(vy+(step/2)*k1[3])
k2[2]=derVx(x+(step/2)*k1[0],y+(step/2)*k1[1])
k2[3]=derVx(y+(step/2)*k1[1],x+(step/2)*k1[0])
k3[0]=derX(vx+(step/2)*k2[2])
k3[1]=derY(vy+(step/2)*k2[3])
k3[2]=derVx(x+(step/2)*k2[0],y+(step/2)*k2[1])
k3[3]=derVx(y+(step/2)*k2[1],x+(step/2)*k2[0])
k4[0]=derX(vx+step*k3[2])
k4[1]=derY(vy+step*k3[3])
k4[2]=derVx(x+step*k3[0],y+step*k3[1])
k4[3]=derVx(y+step*k3[1],vx+step*k3[0])
t=t+step
x=timestep(x,k1[0],k2[0],k3[0],k4[0])
y=timestep(x,k1[1],k2[1],k3[1],k4[1])
vx=timestep(x,k1[2],k2[2],k3[2],k4[2])
vy=timestep(x,k1[3],k2[3],k3[3],k4[3])
mpl.plot(xarray, yarray)
There is a spurious v in the computation of k4[3].
The call of timestep has x as argument where it should be y, vx, vy.
And another error seems to be that in the difference computation
x = bodyx - x
y = bodyy - y
you also change the absolute position. Also the force direction becomes reversed.
Change that to something like
diffx = x - bodyx
diffy = y - bodyy
and use these relative positions in the force computation.
To compare, the built-in procedures produce scipy.integrate.odeint with data
G=6.67408E-11 #m^3/kg s^2
M=5.972E24 #kg, mass of Earth
bodyx = 444 #stationary body position metres
bodyy = 444
def system(u,t):
x,y,vx,vy = u
x -= bodyx
y -= bodyy
f = -(G*M)/((x**2 + y**2)**(1.5))
return [ vx, vy, f*x, f*y ]
x0=4596194 #initial conditions in m and m/s
y0=4596194
vx0=-6646
vy0=6646
u0 = [ x0, y0, vx0, vy0 ]
T= np.linspace(0,3600,36+1)
sol = odeint(system, u0, T)
mpl.plot(sol[:,0], sol[:,1]); mpl.show()
gives a nicely curved bow, about 1/4 of a full orbit.

Closest point on a cubic Bezier curve?

How can I find the point B(t) along a cubic Bezier curve that is closest to an arbitrary point P in the plane?
I've written some quick-and-dirty code that estimates this for Bézier curves of any degree. (Note: this is pseudo-brute force, not a closed-form solution.)
Demo: http://phrogz.net/svg/closest-point-on-bezier.html
/** Find the ~closest point on a Bézier curve to a point you supply.
* out : A vector to modify to be the point on the curve
* curve : Array of vectors representing control points for a Bézier curve
* pt : The point (vector) you want to find out to be near
* tmps : Array of temporary vectors (reduces memory allocations)
* returns: The parameter t representing the location of `out`
*/
function closestPoint(out, curve, pt, tmps) {
let mindex, scans=25; // More scans -> better chance of being correct
const vec=vmath['w' in curve[0]?'vec4':'z' in curve[0]?'vec3':'vec2'];
for (let min=Infinity, i=scans+1;i--;) {
let d2 = vec.squaredDistance(pt, bézierPoint(out, curve, i/scans, tmps));
if (d2<min) { min=d2; mindex=i }
}
let t0 = Math.max((mindex-1)/scans,0);
let t1 = Math.min((mindex+1)/scans,1);
let d2ForT = t => vec.squaredDistance(pt, bézierPoint(out,curve,t,tmps));
return localMinimum(t0, t1, d2ForT, 1e-4);
}
/** Find a minimum point for a bounded function. May be a local minimum.
* minX : the smallest input value
* maxX : the largest input value
* ƒ : a function that returns a value `y` given an `x`
* ε : how close in `x` the bounds must be before returning
* returns: the `x` value that produces the smallest `y`
*/
function localMinimum(minX, maxX, ƒ, ε) {
if (ε===undefined) ε=1e-10;
let m=minX, n=maxX, k;
while ((n-m)>ε) {
k = (n+m)/2;
if (ƒ(k-ε)<ƒ(k+ε)) n=k;
else m=k;
}
return k;
}
/** Calculate a point along a Bézier segment for a given parameter.
* out : A vector to modify to be the point on the curve
* curve : Array of vectors representing control points for a Bézier curve
* t : Parameter [0,1] for how far along the curve the point should be
* tmps : Array of temporary vectors (reduces memory allocations)
* returns: out (the vector that was modified)
*/
function bézierPoint(out, curve, t, tmps) {
if (curve.length<2) console.error('At least 2 control points are required');
const vec=vmath['w' in curve[0]?'vec4':'z' in curve[0]?'vec3':'vec2'];
if (!tmps) tmps = curve.map( pt=>vec.clone(pt) );
else tmps.forEach( (pt,i)=>{ vec.copy(pt,curve[i]) } );
for (var degree=curve.length-1;degree--;) {
for (var i=0;i<=degree;++i) vec.lerp(tmps[i],tmps[i],tmps[i+1],t);
}
return vec.copy(out,tmps[0]);
}
The code above uses the vmath library to efficiently lerp between vectors (in 2D, 3D, or 4D), but it would be trivial to replace the lerp() call in bézierPoint() with your own code.
Tuning the Algorithm
The closestPoint() function works in two phases:
First, calculate points all along the curve (uniformly-spaced values of the t parameter). Record which value of t has the smallest distance to the point.
Then, use the localMinimum() function to hunt the region around the smallest distance, using a binary search to find the t and point that produces the true smallest distance.
The value of scans in closestPoint() determines how many samples to use in the first pass. Fewer scans is faster, but increases the chances of missing the true minimum point.
The ε limit passed to the localMinimum() function controls how long it continues to hunt for the best value. A value of 1e-2 quantizes the curve into ~100 points, and thus you can see the points returned from closestPoint() popping along the line. Each additional decimal point of precision—1e-3, 1e-4, …—costs about 6-8 additional calls to bézierPoint().
After lots of searching I found a paper that discusses a method for finding the closest point on a Bezier curve to a given point:
Improved Algebraic Algorithm On Point
Projection For Bezier Curves, by
Xiao-Diao Chen, Yin Zhou, Zhenyu Shu,
Hua Su, and Jean-Claude Paul.
Furthermore, I found Wikipedia and MathWorld's descriptions of Sturm sequences useful in understanding the first part of the algoritm, as the paper itself isn't very clear in its own description.
Seeing as the other methods on this page seem to be approximation, this answer will provide a simple numerical solution. It is a python implementation depending on the numpy library to supply Bezier class. In my tests, this approach performed about three times better than my brute-force implementation (using samples and subdivision).
Look at the interactive example here.
Click to enlarge.
I used basic algebra to solve this minimal problem.
Start with the bezier curve equation.
B(t) = (1 - t)^3 * p0 + 3 * (1 - t)^2 * t * p1 + 3 * (1 - t) * t^2 * p2 + t^3 * p3
Since I'm using numpy, my points are represented as numpy vectors (matrices). This means that p0 is a one-dimensional, e.g. (1, 4.2). If you are handling two floating point variables you probably need mutliple equations (for x and y): Bx(t) = (1-t)^3*px_0 + ...
Convert it to a standard form with four coefficients.
You can write the four coefficients by expanding the original equation.
The distance from a point p to the curve B(t) can be calculated using the pythagorean theorem.
Here a and b are the two dimensions of our points x and y. This means that the squared distance D(t) is:
I'm not calculating a square root just now, because it is enough if we compare relative squared distances. All following equation will refer to the squared distance.
This function D(t) describes the distance between the graph and the points. We are interested in the minima in the range of t in [0, 1]. To find them, we have to derive the function twice. The first derivative of the distance function is a 5 order polynomial:
The second derivative is:
A desmos graph let's us examine the different functions.
D(t) has its local minima where d'(t) = 0 and d''(t) >= 0. This means, that we have to find the t for d'(t) = 0'.
black: the bezier curve, green: d(t), purple: d'(t), red:d''(t)
Find the roots of d'(t). I use the numpy library, which takes the coefficients of a polynomial.
dcoeffs = np.stack([da, db, dc, dd, de, df])
roots = np.roots(dcoeffs)
Remove the imaginary roots (keep only the real roots) and remove any roots which are < 0 or > 1. With a cubic bezier, there will probably be about 0-3 roots left.
Next, check the distances of each |B(t) - pt| for each t in roots. Also check the distances for B(0) and B(1) since start and end of the Bezier curve could be the closest points (although they aren't local minima of the distance function).
Return the closest point.
I am attaching the class for the Bezier in python. Check the github link for a usage example.
import numpy as np
# Bezier Class representing a CUBIC bezier defined by four
# control points.
#
# at(t): gets a point on the curve at t
# distance2(pt) returns the closest distance^2 of
# pt and the curve
# closest(pt) returns the point on the curve
# which is closest to pt
# maxes(pt) plots the curve using matplotlib
class Bezier(object):
exp3 = np.array([[3, 3], [2, 2], [1, 1], [0, 0]], dtype=np.float32)
exp3_1 = np.array([[[3, 3], [2, 2], [1, 1], [0, 0]]], dtype=np.float32)
exp4 = np.array([[4], [3], [2], [1], [0]], dtype=np.float32)
boundaries = np.array([0, 1], dtype=np.float32)
# Initialize the curve by assigning the control points.
# Then create the coefficients.
def __init__(self, points):
assert isinstance(points, np.ndarray)
assert points.dtype == np.float32
self.points = points
self.create_coefficients()
# Create the coefficients of the bezier equation, bringing
# the bezier in the form:
# f(t) = a * t^3 + b * t^2 + c * t^1 + d
#
# The coefficients have the same dimensions as the control
# points.
def create_coefficients(self):
points = self.points
a = - points[0] + 3*points[1] - 3*points[2] + points[3]
b = 3*points[0] - 6*points[1] + 3*points[2]
c = -3*points[0] + 3*points[1]
d = points[0]
self.coeffs = np.stack([a, b, c, d]).reshape(-1, 4, 2)
# Return a point on the curve at the parameter t.
def at(self, t):
if type(t) != np.ndarray:
t = np.array(t)
pts = self.coeffs * np.power(t, self.exp3_1)
return np.sum(pts, axis = 1)
# Return the closest DISTANCE (squared) between the point pt
# and the curve.
def distance2(self, pt):
points, distances, index = self.measure_distance(pt)
return distances[index]
# Return the closest POINT between the point pt
# and the curve.
def closest(self, pt):
points, distances, index = self.measure_distance(pt)
return points[index]
# Measure the distance^2 and closest point on the curve of
# the point pt and the curve. This is done in a few steps:
# 1 Define the distance^2 depending on the pt. I am
# using the squared distance because it is sufficient
# for comparing distances and doesn't have the over-
# head of an additional root operation.
# D(t) = (f(t) - pt)^2
# 2 Get the roots of D'(t). These are the extremes of
# D(t) and contain the closest points on the unclipped
# curve. Only keep the minima by checking if
# D''(roots) > 0 and discard imaginary roots.
# 3 Calculate the distances of the pt to the minima as
# well as the start and end of the curve and return
# the index of the shortest distance.
#
# This desmos graph is a helpful visualization.
# https://www.desmos.com/calculator/ktglugn1ya
def measure_distance(self, pt):
coeffs = self.coeffs
# These are the coefficients of the derivatives d/dx and d/(d/dx).
da = 6*np.sum(coeffs[0][0]*coeffs[0][0])
db = 10*np.sum(coeffs[0][0]*coeffs[0][1])
dc = 4*(np.sum(coeffs[0][1]*coeffs[0][1]) + 2*np.sum(coeffs[0][0]*coeffs[0][2]))
dd = 6*(np.sum(coeffs[0][0]*(coeffs[0][3]-pt)) + np.sum(coeffs[0][1]*coeffs[0][2]))
de = 2*(np.sum(coeffs[0][2]*coeffs[0][2])) + 4*np.sum(coeffs[0][1]*(coeffs[0][3]-pt))
df = 2*np.sum(coeffs[0][2]*(coeffs[0][3]-pt))
dda = 5*da
ddb = 4*db
ddc = 3*dc
ddd = 2*dd
dde = de
dcoeffs = np.stack([da, db, dc, dd, de, df])
ddcoeffs = np.stack([dda, ddb, ddc, ddd, dde]).reshape(-1, 1)
# Calculate the real extremes, by getting the roots of the first
# derivativ of the distance function.
extrema = np_real_roots(dcoeffs)
# Remove the roots which are out of bounds of the clipped range [0, 1].
# [future reference] https://stackoverflow.com/questions/47100903/deleting-every-3rd-element-of-a-tensor-in-tensorflow
dd_clip = (np.sum(ddcoeffs * np.power(extrema, self.exp4)) >= 0) & (extrema > 0) & (extrema < 1)
minima = extrema[dd_clip]
# Add the start and end position as possible positions.
potentials = np.concatenate((minima, self.boundaries))
# Calculate the points at the possible parameters t and
# get the index of the closest
points = self.at(potentials.reshape(-1, 1, 1))
distances = np.sum(np.square(points - pt), axis = 1)
index = np.argmin(distances)
return points, distances, index
# Point the curve to a matplotlib figure.
# maxes ... the axes of a matplotlib figure
def plot(self, maxes):
import matplotlib.path as mpath
import matplotlib.patches as mpatches
Path = mpath.Path
pp1 = mpatches.PathPatch(
Path(self.points, [Path.MOVETO, Path.CURVE4, Path.CURVE4, Path.CURVE4]),
fc="none")#, transform=ax.transData)
pp1.set_alpha(1)
pp1.set_color('#00cc00')
pp1.set_fill(False)
pp2 = mpatches.PathPatch(
Path(self.points, [Path.MOVETO, Path.LINETO , Path.LINETO , Path.LINETO]),
fc="none")#, transform=ax.transData)
pp2.set_alpha(0.2)
pp2.set_color('#666666')
pp2.set_fill(False)
maxes.scatter(*zip(*self.points), s=4, c=((0, 0.8, 1, 1), (0, 1, 0.5, 0.8), (0, 1, 0.5, 0.8),
(0, 0.8, 1, 1)))
maxes.add_patch(pp2)
maxes.add_patch(pp1)
# Wrapper around np.roots, but only returning real
# roots and ignoring imaginary results.
def np_real_roots(coefficients, EPSILON=1e-6):
r = np.roots(coefficients)
return r.real[abs(r.imag) < EPSILON]
Depending on your tolerances. Brute force and being accepting of error. This algorithm could be wrong for some rare cases. But, in the majority of them it will find a point very close to the right answer and the results will improve the higher you set the slices. It just tries each point along the curve at regular intervals and returns the best one it found.
public double getClosestPointToCubicBezier(double fx, double fy, int slices, double x0, double y0, double x1, double y1, double x2, double y2, double x3, double y3) {
double tick = 1d / (double) slices;
double x;
double y;
double t;
double best = 0;
double bestDistance = Double.POSITIVE_INFINITY;
double currentDistance;
for (int i = 0; i <= slices; i++) {
t = i * tick;
//B(t) = (1-t)**3 p0 + 3(1 - t)**2 t P1 + 3(1-t)t**2 P2 + t**3 P3
x = (1 - t) * (1 - t) * (1 - t) * x0 + 3 * (1 - t) * (1 - t) * t * x1 + 3 * (1 - t) * t * t * x2 + t * t * t * x3;
y = (1 - t) * (1 - t) * (1 - t) * y0 + 3 * (1 - t) * (1 - t) * t * y1 + 3 * (1 - t) * t * t * y2 + t * t * t * y3;
currentDistance = Point.distanceSq(x,y,fx,fy);
if (currentDistance < bestDistance) {
bestDistance = currentDistance;
best = t;
}
}
return best;
}
You can get a lot better and faster by simply finding the nearest point and recursing around that point.
public double getClosestPointToCubicBezier(double fx, double fy, int slices, int iterations, double x0, double y0, double x1, double y1, double x2, double y2, double x3, double y3) {
return getClosestPointToCubicBezier(iterations, fx, fy, 0, 1d, slices, x0, y0, x1, y1, x2, y2, x3, y3);
}
private double getClosestPointToCubicBezier(int iterations, double fx, double fy, double start, double end, int slices, double x0, double y0, double x1, double y1, double x2, double y2, double x3, double y3) {
if (iterations <= 0) return (start + end) / 2;
double tick = (end - start) / (double) slices;
double x, y, dx, dy;
double best = 0;
double bestDistance = Double.POSITIVE_INFINITY;
double currentDistance;
double t = start;
while (t <= end) {
//B(t) = (1-t)**3 p0 + 3(1 - t)**2 t P1 + 3(1-t)t**2 P2 + t**3 P3
x = (1 - t) * (1 - t) * (1 - t) * x0 + 3 * (1 - t) * (1 - t) * t * x1 + 3 * (1 - t) * t * t * x2 + t * t * t * x3;
y = (1 - t) * (1 - t) * (1 - t) * y0 + 3 * (1 - t) * (1 - t) * t * y1 + 3 * (1 - t) * t * t * y2 + t * t * t * y3;
dx = x - fx;
dy = y - fy;
dx *= dx;
dy *= dy;
currentDistance = dx + dy;
if (currentDistance < bestDistance) {
bestDistance = currentDistance;
best = t;
}
t += tick;
}
return getClosestPointToCubicBezier(iterations - 1, fx, fy, Math.max(best - tick, 0d), Math.min(best + tick, 1d), slices, x0, y0, x1, y1, x2, y2, x3, y3);
}
In both cases you can do the quad just as easily:
x = (1 - t) * (1 - t) * x0 + 2 * (1 - t) * t * x1 + t * t * x2; //quad.
y = (1 - t) * (1 - t) * y0 + 2 * (1 - t) * t * y1 + t * t * y2; //quad.
By switching out the equation there.
While the accepted answer is right, and you really can figure out the roots and compare that stuff. If you really just need to find the nearest point on the curve, this will do it.
In regard to Ben in the comments. You cannot short hand the formula in the many hundreds of control point range, like I did for cubic and quad forms. Because the amount demanded by each new addition of a bezier curve means that you build a Pythagorean pyramids for them, and we're basically dealing with even more and more massive strings of numbers. For quad you go 1, 2, 1, for cubic you go 1, 3, 3, 1. You end up building bigger and bigger pyramids, and end up breaking it down with Casteljau's algorithm, (I wrote this for solid speed):
/**
* Performs deCasteljau's algorithm for a bezier curve defined by the given control points.
*
* A cubic for example requires four points. So it should get at least an array of 8 values
*
* #param controlpoints (x,y) coord list of the Bezier curve.
* #param returnArray Array to store the solved points. (can be null)
* #param t Amount through the curve we are looking at.
* #return returnArray
*/
public static float[] deCasteljau(float[] controlpoints, float[] returnArray, float t) {
int m = controlpoints.length;
int sizeRequired = (m/2) * ((m/2) + 1);
if (returnArray == null) returnArray = new float[sizeRequired];
if (sizeRequired > returnArray.length) returnArray = Arrays.copyOf(controlpoints, sizeRequired); //insure capacity
else System.arraycopy(controlpoints,0,returnArray,0,controlpoints.length);
int index = m; //start after the control points.
int skip = m-2; //skip if first compare is the last control point.
for (int i = 0, s = returnArray.length - 2; i < s; i+=2) {
if (i == skip) {
m = m - 2;
skip += m;
continue;
}
returnArray[index++] = (t * (returnArray[i + 2] - returnArray[i])) + returnArray[i];
returnArray[index++] = (t * (returnArray[i + 3] - returnArray[i + 1])) + returnArray[i + 1];
}
return returnArray;
}
You basically need to use the algorithm directly, not just for the calculation of the x,y which occur on the curve itself, but you also need it to perform actual and proper Bezier subdivision algorithm (there are others but that is what I'd recommend), to calculate not just an approximation as I give by dividing it into line segments, but of the actual curves. Or rather the polygon hull that is certain to contain the curve.
You do this by using the above algorithm to subdivide the curves at the given t. So T=0.5 to cut the curves in half (note 0.2 would cut it 20% 80% through the curve). Then you index the various points at the side of the pyramid and the other side of the pyramid as built from the base. So for example in cubic:
9
7 8
4 5 6
0 1 2 3
You would feed the algorithm 0 1 2 3 as control points, then you would index the two perfectly subdivided curves at 0, 4, 7, 9 and 9, 8, 6, 3. Take special note to see that these curves start and end at the same point. and the final index 9 which is the point on the curve is used as the other new anchor point. Given this you can perfectly subdivide a bezier curve.
Then to find the closest point you'd want to keep subdividing the curve into different parts noting that it is the case that the entire curve of a bezier curve is contained within the hull of the control points. Which is to say if we turn points 0, 1, 2, 3 into a closed path connecting 0,3 that curve must fall completely within that polygon hull. So what we do is define our given point P, then we continue to subdivide curves until such time as we know that the farthest point of one curve is closer than the closest point of another curve. We simply compare this point P to all the control and anchor points of the curves. And discard any curve from our active list whose closest point (whether anchor or control) is further away than the farthest point of another curve. Then we subdivide all the active curves and do this again. Eventually we will have very subdivided curves discarding about half each step (meaning it should be O(n log n)) until our error is basically negligible. At this point we call our active curves the closest point to that point (there could be more than one), and note that the error in that highly subdivided bit of curve is basically equal to a point. Or simply decide the issue by saying whichever of the two anchor point is closest is the closest point to our point P. And we know the error to a very specific degree.
This, though, requires that we actually have a robust solution and do a certainly correct algorithm and correctly find the tiny fraction of curve that will certainly be the closest point to our point. And it should be relatively fast still.
There is also DOM SVG specific implementations of the closest point algorithms from Mike Bostock:
https://bl.ocks.org/mbostock/8027637
https://bl.ocks.org/mbostock/8027835
A solution to this problem would be to get all the possible points on the bezier curve and compare each distance. The number of points can be controlled by the detail variable.
Here is a implementation made in Unity (C#):
public Vector2 FindNearestPointOnBezier(Bezier bezier, Vector2 point)
{
float detail = 100;
List<Vector2> points = new List<Vector2>();
for (float t = 0; t < 1f; t += 1f / detail)
{
// this function can be exchanged for any bezier curve
points.Add(Functions.CalculateBezier(bezier.a, bezier.b, bezier.c, bezier.d, t));
}
Vector2 closest = Vector2.zero;
float minDist = Mathf.Infinity;
foreach (Vector2 p in points)
{
// use sqrMagnitude as it is faster
float dist = (p - point).sqrMagnitude;
if (dist < minDist)
{
minDist = dist;
closest = p;
}
}
return closest;
}
Note that the Bezier class just holds 4 points.
Probably not the best way as it can become very slow depending on the detail.

Resources