Related
I want need to know how detect center coordinates of quadraticCurve in HTML5 canvas. I want to draw arrow in this center point of curve.
There is my draw curve method:
function draw_curve(Ax, Ay, Bx, By, M, context) {
var dx = Bx - Ax,
dy = By - Ay,
dr = Math.sqrt(dx * dx + dy * dy);
// side is either 1 or -1 depending on which side you want the curve to be on.
// Find midpoint J
var Jx = Ax + (Bx - Ax) / 2
var Jy = Ay + (By - Ay) / 2
// We need a and b to find theta, and we need to know the sign of each to make sure that the orientation is correct.
var a = Bx - Ax
var asign = (a < 0 ? -1 : 1)
var b = By - Ay
var bsign = (b < 0 ? -1 : 1)
var theta = Math.atan(b / a)
// Find the point that's perpendicular to J on side
var costheta = asign * Math.cos(theta)
var sintheta = asign * Math.sin(theta)
// Find c and d
var c = M * sintheta
var d = M * costheta
// Use c and d to find Kx and Ky
var Kx = Jx - c
var Ky = Jy + d
// context.bezierCurveTo(Kx, Ky,Bx,By, Ax, Ax);
context.quadraticCurveTo(Kx, Ky, Bx, By);
// draw the ending arrowhead
var endRadians = Math.atan((dx) / (dy));
context.stroke();
var t = 0.5; // given example value
var xx = (1 - t) * (1 - t) * Ax + 2 * (1 - t) * t * Kx + t * t * Bx;
var yy = (1 - t) * (1 - t) * Ay + 2 * (1 - t) * t * Ky + t * t * By;
var k = {};
k.x = xx;
k.y = yy;
SOLVED BY THIS CODE, T is parameter which set position on the curve:
var t = 0.5; // given example value
var xx = (1 - t) * (1 - t) * Ax + 2 * (1 - t) * t * Kx + t * t * Bx;
var yy = (1 - t) * (1 - t) * Ay + 2 * (1 - t) * t * Ky + t * t * By;
var k = {};
k.x = xx;
k.y = yy;
I created this class in actionscript, it returns a given point of the bezier. And what I am trying to achieve is to get the angle of the current point. I searched on the internet but I couldn't found much. How can I do this?
public static function quadraticBezierPoint(u:Number, anchor1:Point, anchor2:Point, control:Point):Point {
var uc:Number = 1 - u;
var posx:Number = Math.pow(uc, 2) * anchor1.x + 2 * uc * u * control.x + Math.pow(u, 2) * anchor2.x;
var posy:Number = Math.pow(uc, 2) * anchor1.y + 2 * uc * u * control.y + Math.pow(u, 2) * anchor2.y;
return new Point(posx, posy);
}
Given:
control points p0, p1, p2
time t
point B is the point on the quadratic bezier curve described by p0, p1, and p2 at time t.
q0 is the point on the linear bezier curve described by p0 and p1 at time t.
q1 is the point on the linear bezier curve described by p1 and p2 at time t.
The line segment between q0 and q1 is tangent to your quadratic bezier curve at point B.
Therefore, the angle of the bezier curve at time t is equal to the slope of the line segment between q0 and q1.
Wikipedia has a lovely gif demonstrating this. The black dot is point B, and the endpoints of the green line segment are q0 and q1.
The principle is identical for bezier curves of higher dimensions. To find the angle of a point on an N-degree bezier curve, find q0 and q1, which are the points on the N-1-degree bezier curves for control points [p0,p1,...,p(N-1)] and [p1, p2,...,pN]. The angle is equal to the slope of the q0-q1 line segment.
In pseudocode:
def bezierCurve(controlPoints, t):
if len(controlPoints) == 1:
return controlPoints[0]
else:
allControlPointsButTheLastOne = controlPoints[:-1]
allControlPointsButTheFirstOne = controlPoints[1:]
q0 = bezierCurve(allControlPointsButTheLatOne, t)
q1 = bezierCurve(allControlPointsButTheFirstOne, t)
return (1-t) * q0 + t * q1
def bezierAngle(controlPoints, t):
q0 = bezierCurve(controlPoints[:-1], t)
q1 = bezierCurve(controlPoints[1:], t)
return math.atan2(q1.y - q0.y, q1.x - q0.x)
After the explanation from Kevin I made a dynamic but simple solution:
public static function quadraticBezierAngle(u:Number, anchor1:Point, anchor2:Point, control:Point):Number {
var uc:Number = 1 - u;
var dx:Number = (uc * control.x + u * anchor2.x) - (uc * anchor1.x + u * control.x);
var dy:Number = (uc * control.y + u * anchor2.y) - (uc * anchor1.y + u * control.y);
return Math.atan2(dy, dx);
}
I have an equation for a parabolic curve intersecting a specified point, in my case where the user clicked on a graph.
// this would typically be mouse coords on the graph
var _target:Point = new Point(100, 50);
public static function plot(x:Number, target:Point):Number{
return (x * x) / target.x * (target.y / target.x);
}
This gives a graph such as this:
I also have a series of line segments defined by start and end coordinates:
startX:Number, startY:Number, endX:Number, endY:Number
I need to find if and where this curve intersects these segments (A):
If it's any help, startX is always < endX
I get the feeling there's a fairly straight forward way to do this, but I don't really know what to search for, nor am I very well versed in "proper" math, so actual code examples would be very much appreciated.
UPDATE:
I've got the intersection working, but my solution gives me the coordinate for the wrong side of the y-axis.
Replacing my target coords with A and B respectively, gives this equation for the plot:
(x * x) / A * (B/A)
// this simplifies down to:
(B * x * x) / (A * A)
// which i am the equating to the line's equation
(B * x * x) / (A * A) = m * x + b
// i run this through wolfram alpha (because i have no idea what i'm doing) and get:
(A * A * m - A * Math.sqrt(A * A * m * m + 4 * b * B)) / (2 * B)
This is a correct answer, but I want the second possible variation.
I've managed to correct this by multiplying m with -1 before the calculation and doing the same with the x value the last calculation returns, but that feels like a hack.
SOLUTION:
public static function intersectsSegment(targetX:Number, targetY:Number, startX:Number, startY:Number, endX:Number, endY:Number):Point {
// slope of the line
var m:Number = (endY - startY) / (endX - startX);
// where the line intersects the y-axis
var b:Number = startY - startX * m;
// solve the two variatons of the equation, we may need both
var ix1:Number = solve(targetX, targetY, m, b);
var ix2:Number = solveInverse(targetX, targetY, m, b);
var intersection1:Point;
var intersection2:Point;
// if the intersection is outside the line segment startX/endX it's discarded
if (ix1 > startX && ix1 < endX) intersection1 = new Point(ix1, plot(ix1, targetX, targetY));
if (ix2 > startX && ix2 < endX) intersection2 = new Point(ix2, plot(ix2, targetX, targetY));
// somewhat fiddly code to return the smallest set intersection
if (intersection1 && intersection2) {
// return the intersection with the smaller x value
return intersection1.x < intersection2.x ? intersection1 : intersection2;
} else if (intersection1) {
return intersection1;
}
// this effectively means that we return intersection2 or if that's unset, null
return intersection2;
}
private static function solve(A:Number, B:Number, m:Number, b:Number):Number {
return (m + Math.sqrt(4 * (B / (A * A)) * b + m * m)) / (2 * (B / (A * A)));
}
private static function solveInverse(A:Number, B:Number, m:Number, b:Number):Number {
return (m - Math.sqrt(4 * (B / (A * A)) * b + m * m)) / (2 * (B / (A * A)));
}
public static function plot(x:Number, targetX:Number, targetY:Number):Number{
return (targetY * x * x) / (targetX * targetX);
}
Or, more explicit yet.
If your parabolic curve is
y(x)= A x2+ B x + C (Eq 1)
and your line is
y(x) = m x + b (Eq 2)
The two possible solutions (+ and -) for x are
x = ((-B + m +- Sqrt[4 A b + B^2 - 4 A C - 2 B m + m^2])/(2 A)) (Eq 3)
You should check if your segment endpoints (in x) contains any of these two points. If they do, just replace the corresponding x in the y=m x + b equation to get the y coordinate for the intersection
Edit>
To get the last equation you just say that the "y" in eq 1 is equal to the "y" in eq 2 (because you are looking for an intersection!).
That gives you:
A x2+ B x + C = m x + b
and regrouping
A x2+ (B-m) x + (C-b) = 0
Which is a quadratic equation.
Equation 3 are just the two possible solutions for this quadratic.
Edit 2>
re-reading your code, it seems that your parabola is defined by
y(x) = A x2
where
A = (target.y / (target.x)2)
So in your case Eq 3 becomes simply
x = ((m +- Sqrt[4 A b + m^2])/(2 A)) (Eq 3b)
HTH!
Take the equation for the curve and put your line into y = mx +b form. Solve for x and then determine if X is between your your start and end points for you line segment.
Check out: http://mathcentral.uregina.ca/QQ/database/QQ.09.03/senthil1.html
Are you doing this often enough to desire a separate test to see if an intersection exists before actually computing the intersection point? If so, consider the fact that your parabola is a level set for the function f(x, y) = y - (B * x * x) / (A * A) -- specifically, the one for which f(x, y) = 0. Plug your two endpoints into f(x,y) -- if they have the same sign, they're on the same side of the parabola, while if they have different signs, they're on different sides of the parabola.
Now, you still might have a segment that intersects the parabola twice, and this test doesn't catch that. But something about the way you're defining the problem makes me feel that maybe that's OK for your application.
In other words, you need to calulate the equation for each line segment y = Ax + B compare it to curve equation y = Cx^2 + Dx + E so Ax + B - Cx^2 - Dx - E = 0 and see if there is a solution between startX and endX values.
I need a function to find the shortest distance between two line segments. A line segment is defined by two endpoints. So for example one of my line segments (AB) would be defined by the two points A (x1,y1) and B (x2,y2) and the other (CD) would be defined by the two points C (x1,y1) and D (x2,y2).
Feel free to write the solution in any language you want and I can translate it into javascript. Please keep in mind my geometry skills are pretty rusty. I have already seen here and I am not sure how to translate this into a function. Thank you so much for help.
This is my solution in Python. Works with 3d points and you can simplify for 2d.
import numpy as np
def closestDistanceBetweenLines(a0,a1,b0,b1,clampAll=False,clampA0=False,clampA1=False,clampB0=False,clampB1=False):
''' Given two lines defined by numpy.array pairs (a0,a1,b0,b1)
Return the closest points on each segment and their distance
'''
# If clampAll=True, set all clamps to True
if clampAll:
clampA0=True
clampA1=True
clampB0=True
clampB1=True
# Calculate denomitator
A = a1 - a0
B = b1 - b0
magA = np.linalg.norm(A)
magB = np.linalg.norm(B)
_A = A / magA
_B = B / magB
cross = np.cross(_A, _B);
denom = np.linalg.norm(cross)**2
# If lines are parallel (denom=0) test if lines overlap.
# If they don't overlap then there is a closest point solution.
# If they do overlap, there are infinite closest positions, but there is a closest distance
if not denom:
d0 = np.dot(_A,(b0-a0))
# Overlap only possible with clamping
if clampA0 or clampA1 or clampB0 or clampB1:
d1 = np.dot(_A,(b1-a0))
# Is segment B before A?
if d0 <= 0 >= d1:
if clampA0 and clampB1:
if np.absolute(d0) < np.absolute(d1):
return a0,b0,np.linalg.norm(a0-b0)
return a0,b1,np.linalg.norm(a0-b1)
# Is segment B after A?
elif d0 >= magA <= d1:
if clampA1 and clampB0:
if np.absolute(d0) < np.absolute(d1):
return a1,b0,np.linalg.norm(a1-b0)
return a1,b1,np.linalg.norm(a1-b1)
# Segments overlap, return distance between parallel segments
return None,None,np.linalg.norm(((d0*_A)+a0)-b0)
# Lines criss-cross: Calculate the projected closest points
t = (b0 - a0);
detA = np.linalg.det([t, _B, cross])
detB = np.linalg.det([t, _A, cross])
t0 = detA/denom;
t1 = detB/denom;
pA = a0 + (_A * t0) # Projected closest point on segment A
pB = b0 + (_B * t1) # Projected closest point on segment B
# Clamp projections
if clampA0 or clampA1 or clampB0 or clampB1:
if clampA0 and t0 < 0:
pA = a0
elif clampA1 and t0 > magA:
pA = a1
if clampB0 and t1 < 0:
pB = b0
elif clampB1 and t1 > magB:
pB = b1
# Clamp projection A
if (clampA0 and t0 < 0) or (clampA1 and t0 > magA):
dot = np.dot(_B,(pA-b0))
if clampB0 and dot < 0:
dot = 0
elif clampB1 and dot > magB:
dot = magB
pB = b0 + (_B * dot)
# Clamp projection B
if (clampB0 and t1 < 0) or (clampB1 and t1 > magB):
dot = np.dot(_A,(pB-a0))
if clampA0 and dot < 0:
dot = 0
elif clampA1 and dot > magA:
dot = magA
pA = a0 + (_A * dot)
return pA,pB,np.linalg.norm(pA-pB)
Test example with pictures to help visualize:
a1=np.array([13.43, 21.77, 46.81])
a0=np.array([27.83, 31.74, -26.60])
b0=np.array([77.54, 7.53, 6.22])
b1=np.array([26.99, 12.39, 11.18])
closestDistanceBetweenLines(a0,a1,b0,b1,clampAll=True)
# Result: (array([ 20.29994362, 26.5264818 , 11.78759994]), array([ 26.99, 12.39, 11.18]), 15.651394495590445) #
closestDistanceBetweenLines(a0,a1,b0,b1,clampAll=False)
# Result: (array([ 19.85163563, 26.21609078, 14.07303667]), array([ 18.40058604, 13.21580716, 12.02279907]), 13.240709703623198) #
Taken from this example, which also comes with a simple explanation of why it works as well as VB code (that does more than you need, so I've simplified as I translated to Python -- note: I have translated, but not tested, so a typo might have slipped by...):
def segments_distance(x11, y11, x12, y12, x21, y21, x22, y22):
""" distance between two segments in the plane:
one segment is (x11, y11) to (x12, y12)
the other is (x21, y21) to (x22, y22)
"""
if segments_intersect(x11, y11, x12, y12, x21, y21, x22, y22): return 0
# try each of the 4 vertices w/the other segment
distances = []
distances.append(point_segment_distance(x11, y11, x21, y21, x22, y22))
distances.append(point_segment_distance(x12, y12, x21, y21, x22, y22))
distances.append(point_segment_distance(x21, y21, x11, y11, x12, y12))
distances.append(point_segment_distance(x22, y22, x11, y11, x12, y12))
return min(distances)
def segments_intersect(x11, y11, x12, y12, x21, y21, x22, y22):
""" whether two segments in the plane intersect:
one segment is (x11, y11) to (x12, y12)
the other is (x21, y21) to (x22, y22)
"""
dx1 = x12 - x11
dy1 = y12 - y11
dx2 = x22 - x21
dy2 = y22 - y21
delta = dx2 * dy1 - dy2 * dx1
if delta == 0: return False # parallel segments
s = (dx1 * (y21 - y11) + dy1 * (x11 - x21)) / delta
t = (dx2 * (y11 - y21) + dy2 * (x21 - x11)) / (-delta)
return (0 <= s <= 1) and (0 <= t <= 1)
import math
def point_segment_distance(px, py, x1, y1, x2, y2):
dx = x2 - x1
dy = y2 - y1
if dx == dy == 0: # the segment's just a point
return math.hypot(px - x1, py - y1)
# Calculate the t that minimizes the distance.
t = ((px - x1) * dx + (py - y1) * dy) / (dx * dx + dy * dy)
# See if this represents one of the segment's
# end points or a point in the middle.
if t < 0:
dx = px - x1
dy = py - y1
elif t > 1:
dx = px - x2
dy = py - y2
else:
near_x = x1 + t * dx
near_y = y1 + t * dy
dx = px - near_x
dy = py - near_y
return math.hypot(dx, dy)
Is this in 2 dimensions? If so, the answer is simply the shortest of the distance between point A and line segment CD, B and CD, C and AB or D and AB. So it's a fairly simple "distance between point and line" calculation (if the distances are all the same, then the lines are parallel).
This site explains the algorithm for distance between a point and a line pretty well.
It's slightly more tricky in the 3 dimensions because the lines are not necessarily in the same plane, but that doesn't seem to be the case here?
This is my solution. It's programmed in Lua. It is very concise, so maybe it will be appreciated. Please do make sure the lengths of the line segments are non 0.
local eta = 1e-6
local function nearestPointsOnLineSegments(a0, a1, b0, b1)
local r = b0 - a0
local u = a1 - a0
local v = b1 - b0
local ru = r:Dot(u)
local rv = r:Dot(v)
local uu = u:Dot(u)
local uv = u:Dot(v)
local vv = v:Dot(v)
local det = uu*vv - uv*uv
local s, t
if det < eta*uu*vv then
s = math.clamp(ru/uu, 0, 1)
t = 0
else
s = math.clamp((ru*vv - rv*uv)/det, 0, 1)
t = math.clamp((ru*uv - rv*uu)/det, 0, 1)
end
local S = math.clamp((t*uv + ru)/uu, 0, 1)
local T = math.clamp((s*uv - rv)/vv, 0, 1)
local A = a0 + S*u
local B = b0 + T*v
return A, B, (B - A):Length()
end
My solution is a translation of Fnord solution. I do in javascript and C.
In Javascript. You need to include mathjs.
var closestDistanceBetweenLines = function(a0, a1, b0, b1, clampAll, clampA0, clampA1, clampB0, clampB1){
//Given two lines defined by numpy.array pairs (a0,a1,b0,b1)
//Return distance, the two closest points, and their average
clampA0 = clampA0 || false;
clampA1 = clampA1 || false;
clampB0 = clampB0 || false;
clampB1 = clampB1 || false;
clampAll = clampAll || false;
if(clampAll){
clampA0 = true;
clampA1 = true;
clampB0 = true;
clampB1 = true;
}
//Calculate denomitator
var A = math.subtract(a1, a0);
var B = math.subtract(b1, b0);
var _A = math.divide(A, math.norm(A))
var _B = math.divide(B, math.norm(B))
var cross = math.cross(_A, _B);
var denom = math.pow(math.norm(cross), 2);
//If denominator is 0, lines are parallel: Calculate distance with a projection and evaluate clamp edge cases
if (denom == 0){
var d0 = math.dot(_A, math.subtract(b0, a0));
var d = math.norm(math.subtract(math.add(math.multiply(d0, _A), a0), b0));
//If clamping: the only time we'll get closest points will be when lines don't overlap at all. Find if segments overlap using dot products.
if(clampA0 || clampA1 || clampB0 || clampB1){
var d1 = math.dot(_A, math.subtract(b1, a0));
//Is segment B before A?
if(d0 <= 0 && 0 >= d1){
if(clampA0 == true && clampB1 == true){
if(math.absolute(d0) < math.absolute(d1)){
return [b0, a0, math.norm(math.subtract(b0, a0))];
}
return [b1, a0, math.norm(math.subtract(b1, a0))];
}
}
//Is segment B after A?
else if(d0 >= math.norm(A) && math.norm(A) <= d1){
if(clampA1 == true && clampB0 == true){
if(math.absolute(d0) < math.absolute(d1)){
return [b0, a1, math.norm(math.subtract(b0, a1))];
}
return [b1, a1, math.norm(math.subtract(b1,a1))];
}
}
}
//If clamping is off, or segments overlapped, we have infinite results, just return position.
return [null, null, d];
}
//Lines criss-cross: Calculate the dereminent and return points
var t = math.subtract(b0, a0);
var det0 = math.det([t, _B, cross]);
var det1 = math.det([t, _A, cross]);
var t0 = math.divide(det0, denom);
var t1 = math.divide(det1, denom);
var pA = math.add(a0, math.multiply(_A, t0));
var pB = math.add(b0, math.multiply(_B, t1));
//Clamp results to line segments if needed
if(clampA0 || clampA1 || clampB0 || clampB1){
if(t0 < 0 && clampA0)
pA = a0;
else if(t0 > math.norm(A) && clampA1)
pA = a1;
if(t1 < 0 && clampB0)
pB = b0;
else if(t1 > math.norm(B) && clampB1)
pB = b1;
}
var d = math.norm(math.subtract(pA, pB))
return [pA, pB, d];
}
//example
var a1=[13.43, 21.77, 46.81];
var a0=[27.83, 31.74, -26.60];
var b0=[77.54, 7.53, 6.22];
var b1=[26.99, 12.39, 11.18];
closestDistanceBetweenLines(a0,a1,b0,b1,true);
In pure C
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
double determinante3(double* a, double* v1, double* v2){
return a[0] * (v1[1] * v2[2] - v1[2] * v2[1]) + a[1] * (v1[2] * v2[0] - v1[0] * v2[2]) + a[2] * (v1[0] * v2[1] - v1[1] * v2[0]);
}
double* cross3(double* v1, double* v2){
double* v = (double*)malloc(3 * sizeof(double));
v[0] = v1[1] * v2[2] - v1[2] * v2[1];
v[1] = v1[2] * v2[0] - v1[0] * v2[2];
v[2] = v1[0] * v2[1] - v1[1] * v2[0];
return v;
}
double dot3(double* v1, double* v2){
return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
}
double norma3(double* v1){
double soma = 0;
for (int i = 0; i < 3; i++) {
soma += pow(v1[i], 2);
}
return sqrt(soma);
}
double* multiplica3(double* v1, double v){
double* v2 = (double*)malloc(3 * sizeof(double));
for (int i = 0; i < 3; i++) {
v2[i] = v1[i] * v;
}
return v2;
}
double* soma3(double* v1, double* v2, int sinal){
double* v = (double*)malloc(3 * sizeof(double));
for (int i = 0; i < 3; i++) {
v[i] = v1[i] + sinal * v2[i];
}
return v;
}
Result_distance* closestDistanceBetweenLines(double* a0, double* a1, double* b0, double* b1, int clampAll, int clampA0, int clampA1, int clampB0, int clampB1){
double denom, det0, det1, t0, t1, d;
double *A, *B, *_A, *_B, *cross, *t, *pA, *pB;
Result_distance *rd = (Result_distance *)malloc(sizeof(Result_distance));
if (clampAll){
clampA0 = 1;
clampA1 = 1;
clampB0 = 1;
clampB1 = 1;
}
A = soma3(a1, a0, -1);
B = soma3(b1, b0, -1);
_A = multiplica3(A, 1 / norma3(A));
_B = multiplica3(B, 1 / norma3(B));
cross = cross3(_A, _B);
denom = pow(norma3(cross), 2);
if (denom == 0){
double d0 = dot3(_A, soma3(b0, a0, -1));
d = norma3(soma3(soma3(multiplica3(_A, d0), a0, 1), b0, -1));
if (clampA0 || clampA1 || clampB0 || clampB1){
double d1 = dot3(_A, soma3(b1, a0, -1));
if (d0 <= 0 && 0 >= d1){
if (clampA0 && clampB1){
if (abs(d0) < abs(d1)){
rd->pA = b0;
rd->pB = a0;
rd->d = norma3(soma3(b0, a0, -1));
}
else{
rd->pA = b1;
rd->pB = a0;
rd->d = norma3(soma3(b1, a0, -1));
}
}
}
else if (d0 >= norma3(A) && norma3(A) <= d1){
if (clampA1 && clampB0){
if (abs(d0) <abs(d1)){
rd->pA = b0;
rd->pB = a1;
rd->d = norma3(soma3(b0, a1, -1));
}
else{
rd->pA = b1;
rd->pB = a1;
rd->d = norma3(soma3(b1, a1, -1));
}
}
}
}
else{
rd->pA = NULL;
rd->pB = NULL;
rd->d = d;
}
}
else{
t = soma3(b0, a0, -1);
det0 = determinante3(t, _B, cross);
det1 = determinante3(t, _A, cross);
t0 = det0 / denom;
t1 = det1 / denom;
pA = soma3(a0, multiplica3(_A, t0), 1);
pB = soma3(b0, multiplica3(_B, t1), 1);
if (clampA0 || clampA1 || clampB0 || clampB1){
if (t0 < 0 && clampA0)
pA = a0;
else if (t0 > norma3(A) && clampA1)
pA = a1;
if (t1 < 0 && clampB0)
pB = b0;
else if (t1 > norma3(B) && clampB1)
pB = b1;
}
d = norma3(soma3(pA, pB, -1));
rd->pA = pA;
rd->pB = pB;
rd->d = d;
}
free(A);
free(B);
free(cross);
free(t);
return rd;
}
int main(void){
//example
double a1[] = { 13.43, 21.77, 46.81 };
double a0[] = { 27.83, 31.74, -26.60 };
double b0[] = { 77.54, 7.53, 6.22 };
double b1[] = { 26.99, 12.39, 11.18 };
Result_distance* rd = closestDistanceBetweenLines(a0, a1, b0, b1, 1, 0, 0, 0, 0);
printf("pA = [%f, %f, %f]\n", rd->pA[0], rd->pA[1], rd->pA[2]);
printf("pB = [%f, %f, %f]\n", rd->pB[0], rd->pB[1], rd->pB[2]);
printf("d = %f\n", rd->d);
return 0;
}
For calculating the minimum distance between 2 2D line segments it is true that you have to perform 4 perpendicular distance from endpoint to other line checks successively using each of the 4 endpoints. However, if you find that the perpendicular line drawn out does not intersect the line segment in any of the 4 cases then you have to perform 4 additional endpoint to endpoint distance checks to find the shortest distance.
Whether there is a more elegent solution to this I do not know.
Please note that the above solutions are correct under the assumption that the line segments do not intersect! If the line segments intersect it is clear that their distance should be 0. It is therefore necessary to one final check which is: Suppose the distance between point A and CD, d(A,CD), was the smallest of the 4 checks mentioned by Dean. Then take a small step along the segment AB from point A. Denote this point E. If d(E,CD) < d(A,CD), the segments must be intersecting! Note that this will never be the case addressed by Stephen.
This is the basic code I follow for the shortest distance between any two plan or any two points in the 3d plane it works well metrics can be changed for the given input
Code In PYTHON
def dot(c1,c2):
return c1[0]* c2[0] + c1[1] * c2[1] + c1[2] * c2[2]
def norm(c1):
return math.sqrt(dot(c1, c1))
def getShortestDistance(x1,x2,x3,x4,y1,y2,y3,y4,z1,z2,z3,z4):
print(x1,x2,x3,x4,y1,y2,y3,y4,z1,z2,z3,z4)
EPS = 0.00000001
delta21 = [1,2,3]
delta21[0] = x2 - x1
delta21[1] = y2 - y1
delta21[2] = z2 - z1
delta41 = [1,2,3]
delta41[0] = x4 - x3
delta41[1] = y4 - y3
delta41[2] = z4 - z3
delta13 = [1,2,3]
delta13[0] = x1 - x3
delta13[1] = y1 - y3
delta13[2] = z1 - z3
a = dot(delta21, delta21)
b = dot(delta21, delta41)
c = dot(delta41, delta41)
d = dot(delta21, delta13)
e = dot(delta41, delta13)
D = a * c - b * b
sc = D
sN = D
sD = D
tc = D
tN = D
tD = D
if D < EPS:
sN = 0.0
sD = 1.0
tN = e
tD = c
else:
sN = (b * e - c * d)
tN = (a * e - b * d)
if sN < 0.0:
sN = 0.0
tN = e
tD = c
elif sN > sD:
sN = sD
tN = e + b
tD = c
if tN < 0.0:
tN = 0.0
if -d < 0.0:
sN = 0.0
elif -d > a:
sN = sD
else:
sN = -d
sD = a
elif tN > tD:
tN = tD
if ((-d + b) < 0.0):
sN = 0
elif ((-d + b) > a):
sN = sD
else:
sN = (-d + b)
sD = a
if (abs(sN) < EPS):
sc = 0.0
else:
sc = sN / sD
if (abs(tN) < EPS):
tc = 0.0
else:
tc = tN / tD
dP = [1,2,3]
dP[0] = delta13[0] + (sc * delta21[0]) - (tc * delta41[0])
dP[1] = delta13[1] + (sc * delta21[1]) - (tc * delta41[1])
dP[2] = delta13[2] + (sc * delta21[2]) - (tc * delta41[2])
return math.sqrt(dot(dP, dP))
This solution is in essence the one from Alex Martelli, but I've added a Point and a LineSegment class to make reading easier. I also adjusted the formatting and added some tests.
The line segment intersection is wrong, but it seems not to matter for the calculation of the distance of line segments. If you're interested in a correct line segment intersection thest, look here: How do you detect whether or not two line segments intersect?
#!/usr/bin/env python
"""Calculate the distance between line segments."""
import math
class Point(object):
"""A two dimensional point."""
def __init__(self, x, y):
self.x = float(x)
self.y = float(y)
class LineSegment(object):
"""A line segment in a two dimensional space."""
def __init__(self, p1, p2):
assert isinstance(p1, Point), \
"p1 is not of type Point, but of %r" % type(p1)
assert isinstance(p2, Point), \
"p2 is not of type Point, but of %r" % type(p2)
self.p1 = p1
self.p2 = p2
def segments_distance(segment1, segment2):
"""Calculate the distance between two line segments in the plane.
>>> a = LineSegment(Point(1,0), Point(2,0))
>>> b = LineSegment(Point(0,1), Point(0,2))
>>> "%0.2f" % segments_distance(a, b)
'1.41'
>>> c = LineSegment(Point(0,0), Point(5,5))
>>> d = LineSegment(Point(2,2), Point(4,4))
>>> e = LineSegment(Point(2,2), Point(7,7))
>>> "%0.2f" % segments_distance(c, d)
'0.00'
>>> "%0.2f" % segments_distance(c, e)
'0.00'
"""
if segments_intersect(segment1, segment2):
return 0
# try each of the 4 vertices w/the other segment
distances = []
distances.append(point_segment_distance(segment1.p1, segment2))
distances.append(point_segment_distance(segment1.p2, segment2))
distances.append(point_segment_distance(segment2.p1, segment1))
distances.append(point_segment_distance(segment2.p2, segment1))
return min(distances)
def segments_intersect(segment1, segment2):
"""Check if two line segments in the plane intersect.
>>> segments_intersect(LineSegment(Point(0,0), Point(1,0)), \
LineSegment(Point(0,0), Point(1,0)))
True
"""
dx1 = segment1.p2.x - segment1.p1.x
dy1 = segment1.p2.y - segment1.p2.y
dx2 = segment2.p2.x - segment2.p1.x
dy2 = segment2.p2.y - segment2.p1.y
delta = dx2 * dy1 - dy2 * dx1
if delta == 0: # parallel segments
# TODO: Could be (partially) identical!
return False
s = (dx1 * (segment2.p1.y - segment1.p1.y) +
dy1 * (segment1.p1.x - segment2.p1.x)) / delta
t = (dx2 * (segment1.p1.y - segment2.p1.y) +
dy2 * (segment2.p1.x - segment1.p1.x)) / (-delta)
return (0 <= s <= 1) and (0 <= t <= 1)
def point_segment_distance(point, segment):
"""
>>> a = LineSegment(Point(1,0), Point(2,0))
>>> b = LineSegment(Point(2,0), Point(0,2))
>>> point_segment_distance(Point(0,0), a)
1.0
>>> "%0.2f" % point_segment_distance(Point(0,0), b)
'1.41'
"""
assert isinstance(point, Point), \
"point is not of type Point, but of %r" % type(point)
dx = segment.p2.x - segment.p1.x
dy = segment.p2.y - segment.p1.y
if dx == dy == 0: # the segment's just a point
return math.hypot(point.x - segment.p1.x, point.y - segment.p1.y)
if dx == 0:
if (point.y <= segment.p1.y or point.y <= segment.p2.y) and \
(point.y >= segment.p2.y or point.y >= segment.p2.y):
return abs(point.x - segment.p1.x)
if dy == 0:
if (point.x <= segment.p1.x or point.x <= segment.p2.x) and \
(point.x >= segment.p2.x or point.x >= segment.p2.x):
return abs(point.y - segment.p1.y)
# Calculate the t that minimizes the distance.
t = ((point.x - segment.p1.x) * dx + (point.y - segment.p1.y) * dy) / \
(dx * dx + dy * dy)
# See if this represents one of the segment's
# end points or a point in the middle.
if t < 0:
dx = point.x - segment.p1.x
dy = point.y - segment.p1.y
elif t > 1:
dx = point.x - segment.p2.x
dy = point.y - segment.p2.y
else:
near_x = segment.p1.x + t * dx
near_y = segment.p1.y + t * dy
dx = point.x - near_x
dy = point.y - near_y
return math.hypot(dx, dy)
if __name__ == '__main__':
import doctest
doctest.testmod()
I have made a Swift port based on Pratik Deoghare's answer above. Pratik references Dan Sunday's excellent write-up and code examples found here: http://geomalgorithms.com/a07-_distance.html
The following functions calculate the minimum distance between two lines or two line segments, and is a direct port of Dan Sunday's C++ examples.
The LASwift linear algebra package is used to do the matrix and vector calculations.
//
// This is a Swift port of the C++ code here
// http://geomalgorithms.com/a07-_distance.html
//
// Copyright 2001 softSurfer, 2012 Dan Sunday
// This code may be freely used, distributed and modified for any purpose
// providing that this copyright notice is included with it.
// SoftSurfer makes no warranty for this code, and cannot be held
// liable for any real or imagined damage resulting from its use.
// Users of this code must verify correctness for their application.
//
//
// LASwift is a "Linear Algebra library for Swift language" by Alexander Taraymovich
// https://github.com/AlexanderTar/LASwift
// LASwift is available under the BSD-3-Clause license.
//
// I've modified the lineToLineDistance and segmentToSegmentDistance functions
// to also return the points on each line/segment where the distance is shortest.
//
import LASwift
import Foundation
func norm(_ v: Vector) -> Double {
return sqrt(dot(v,v)) // norm = length of vector
}
func d(_ u: Vector, _ v: Vector) -> Double {
return norm(u-v) // distance = norm of difference
}
let SMALL_NUM = 0.000000000000000001 // anything that avoids division overflow
typealias Point = Vector
struct Line {
let P0: Point
let P1: Point
}
struct Segment {
let P0: Point
let P1: Point
}
// lineToLineDistance(): get the 3D minimum distance between 2 lines
// Input: two 3D lines L1 and L2
// Return: the shortest distance between L1 and L2
func lineToLineDistance(L1: Line, L2: Line) -> (P1: Point, P2: Point, D: Double) {
let u = L1.P1 - L1.P0
let v = L2.P1 - L2.P0
let w = L1.P0 - L2.P0
let a = dot(u,u) // always >= 0
let b = dot(u,v)
let c = dot(v,v) // always >= 0
let d = dot(u,w)
let e = dot(v,w)
let D = a*c - b*b // always >= 0
var sc, tc: Double
// compute the line parameters of the two closest points
if D < SMALL_NUM { // the lines are almost parallel
sc = 0.0
tc = b>c ? d/b : e/c // use the largest denominator
}
else {
sc = (b*e - c*d) / D
tc = (a*e - b*d) / D
}
// get the difference of the two closest points
let dP = w + (sc .* u) - (tc .* v) // = L1(sc) - L2(tc)
let Psc = L1.P0 + sc .* u
let Qtc = L2.P0 + tc .* v
let dP2 = Psc - Qtc
assert(dP == dP2)
return (P1: Psc, P2: Qtc, D: norm(dP)) // return the closest distance
}
// segmentToSegmentDistance(): get the 3D minimum distance between 2 segments
// Input: two 3D line segments S1 and S2
// Return: the shortest distance between S1 and S2
func segmentToSegmentDistance(S1: Segment, S2: Segment) -> (P1: Point, P2: Point, D: Double) {
let u = S1.P1 - S1.P0
let v = S2.P1 - S2.P0
let w = S1.P0 - S2.P0
let a = dot(u,u) // always >= 0
let b = dot(u,v)
let c = dot(v,v) // always >= 0
let d = dot(u,w)
let e = dot(v,w)
let D = a*c - b*b // always >= 0
let sc: Double
var sN: Double
var sD = D // sc = sN / sD, default sD = D >= 0
let tc: Double
var tN: Double
var tD = D // tc = tN / tD, default tD = D >= 0
// compute the line parameters of the two closest points
if (D < SMALL_NUM) { // the lines are almost parallel
sN = 0.0 // force using point P0 on segment S1
sD = 1.0 // to prevent possible division by 0.0 later
tN = e
tD = c
}
else { // get the closest points on the infinite lines
sN = (b*e - c*d)
tN = (a*e - b*d)
if (sN < 0.0) { // sc < 0 => the s=0 edge is visible
sN = 0.0
tN = e
tD = c
}
else if (sN > sD) { // sc > 1 => the s=1 edge is visible
sN = sD
tN = e + b
tD = c
}
}
if (tN < 0.0) { // tc < 0 => the t=0 edge is visible
tN = 0.0
// recompute sc for this edge
if (-d < 0.0) {
sN = 0.0
}
else if (-d > a) {
sN = sD
}
else {
sN = -d
sD = a
}
}
else if (tN > tD) { // tc > 1 => the t=1 edge is visible
tN = tD;
// recompute sc for this edge
if ((-d + b) < 0.0) {
sN = 0
}
else if ((-d + b) > a) {
sN = sD
}
else {
sN = (-d + b)
sD = a
}
}
// finally do the division to get sc and tc
sc = (abs(sN) < SMALL_NUM ? 0.0 : sN / sD)
tc = (abs(tN) < SMALL_NUM ? 0.0 : tN / tD)
// get the difference of the two closest points
let dP = w + (sc .* u) - (tc .* v) // = S1(sc) - S2(tc)
let Psc = S1.P0 + sc .* u
let Qtc = S2.P0 + tc .* v
let dP2 = Psc - Qtc
assert(dP == dP2)
return (P1: Psc, P2: Qtc, D: norm(dP)) // return the closest distance
}
Here's a Java solution (done the easy way with point checking, so probably not as efficient):
public static double getDistanceBetweenLineSegments(
PointDouble line1Start, PointDouble line1End,
PointDouble line2Start, PointDouble line2End) {
double result = 0;
// If they don't intersect, then work out the distance
if (!isLineIntersectingLine(line1Start, line1End, line2Start, line2End)) {
double p1 = getDistanceBetweenPointAndLine(line1Start, line2Start, line2End);
double p2 = getDistanceBetweenPointAndLine(line1End, line2Start, line2End);
double p3 = getDistanceBetweenPointAndLine(line2Start, line1Start, line1End);
double p4 = getDistanceBetweenPointAndLine(line2End, line1Start, line1End);
result = MathSafe.min(p1, MathSafe.min(p2, MathSafe.min(p3, p4)));
}
return result;
}
And all the other code you need:
public class PointDouble {
private double x;
private double y;
public PointDouble(double x, double y) {
this.x = x;
this.y = y;
}
public double getX() {
return x;
}
public double getY() {
return y;
}
}
private static int relativeCCW(
double x1, double y1,
double x2, double y2,
double px, double py) {
x2 -= x1;
y2 -= y1;
px -= x1;
py -= y1;
double ccw = px * y2 - py * x2;
if (ccw == 0.0) {
ccw = px * x2 + py * y2;
if (ccw > 0.0) {
px -= x2;
py -= y2;
ccw = px * x2 + py * y2;
if (ccw < 0.0) {
ccw = 0.0;
}
}
}
return (ccw < 0.0) ? -1 : ((ccw > 0.0) ? 1 : 0);
}
public static boolean isLineIntersectingLine(
PointDouble line1Start, PointDouble line1End,
PointDouble line2Start, PointDouble line2End) {
return (
(relativeCCW(line1Start.getX(), line1Start.getY(), line1End.getX(), line1End.getY(), line2Start.getX(), line2Start.getY()) *
relativeCCW(line1Start.getX(), line1Start.getY(), line1End.getX(), line1End.getY(), line2End.getX(), line2End.getY()) <= 0)
&&
(relativeCCW(line2Start.getX(), line2Start.getY(), line2End.getX(), line2End.getY(), line1Start.getX(), line1Start.getY()) *
relativeCCW(line2Start.getX(), line2Start.getY(), line2End.getX(), line2End.getY(), line1End.getX(), line1End.getY()) <= 0));
}
public static double getDistanceBetweenPointAndLine(PointDouble pt, PointDouble linePt1, PointDouble linePt2) {
double lineX = linePt2.getX() - linePt1.getX();
double lineY = linePt2.getY() - linePt1.getY();
double dot = (pt.getX() - linePt1.getX()) * lineX + (pt.getY() - linePt1.getY()) * lineY;
double len_sq = lineX * lineX + lineY * lineY;
double param = -1;
double xx;
double yy;
if (len_sq != 0) {
param = dot / len_sq;
}
if (param < 0) {
xx = linePt1.getX();
yy = linePt1.getY();
}
else if (param > 1) {
xx = linePt2.getX();
yy = linePt2.getY();
}
else {
xx = linePt1.getX() + param * lineX;
yy = linePt1.getY() + param * lineY;
}
return MathSafe.hypot(pt.getX() - xx, pt.getY() - yy);
}
Here is one in perl with a few differences from Fnord's:
It always clamps (add non-clamping behavior flags, if you wish, by following Fnord's example).
It catches lines of zero-length line segments that would otherwise cause a divide by zero.
The 1e-6 values below are used to deal with possible floating point errors. Adjust or remove those if you need a different tolerance:
use strict;
use warnings;
use Math::Vector::Real;
use Math::Matrix;
# True if $a >= $b within $tolerance
sub approx_greater
{
my ($a, $b, $tolerance) = #_;
$tolerance //= 1e-6;
return ($a-$b) > -$tolerance;
}
# True if $a <= $b within $tolerance
sub approx_lesser
{
my ($a, $b, $tolerance) = #_;
$tolerance //= 1e-6;
return ($b-$a) > -$tolerance;
}
# True if $a == $b within $tolerance
sub approx_equal
{
my ($a, $b, $tolerance) = #_;
$tolerance //= 1e-6;
return abs($a-$b) < $tolerance;
}
# Returns shortest line: [ [x1,y1,z1], [x2,y2,z2], distance ].
# If askew lines cross (or nearly-intersect) then xyz1 and xyz2 are undefined
# and only distance is returned.
#
# Thank you to #Fnord: https://stackoverflow.com/a/18994296/14055985
sub line_intersect
{
# Map #_ as vectors:
my ($a0, $a1, $b0, $b1) = map { V(#{ $_ }) } #_;
my $A = ($a1-$a0);
my $B = ($b1-$b0);
my $magA = abs($A);
my $magB = abs($B);
# If length line segment:
if ($magA == 0 || $magB == 0)
{
return V(undef, undef, 0);
}
my $_A = $A / $magA;
my $_B = $B / $magB;
my $cross = $_A x $_B;
my $denom = $cross->norm2;
# If lines are parallel (denom=0) test if lines overlap.
# If they don't overlap then there is a closest point solution.
# If they do overlap, there are infinite closest positions, but there is a closest distance
#if ($denom == 0)
if (approx_equal($denom, 0))
{
my $d0 = $_A * ($b0-$a0);
my $d1 = $_A * ($b1-$a0);
# Is segment B before A?
#if ($d0 <= 0 && 0 >= $d1)
if (approx_lesser($d0, 0) && approx_greater(0, $d1))
{
if (abs($d0) < abs($d1))
{
return V($a0, $b0, abs($a0-$b0));
}
else
{
return V($a0, $b1, abs($a0-$b1));
}
}
# Is segment B after A?
#elsif ($d0 >= $magA && $magA <= $d1)
elsif (approx_greater($d0, $magA) && approx_lesser($magA, $d1))
{
if (abs($d0) < abs($d1))
{
return V($a1, $b0, abs($a1-$b0));
}
else
{
return V($a1, $b1, abs($a1-$b1));
}
}
else
{
# Segments overlap, return distance between parallel segments
return V(V(), V(), abs((($d0*$_A)+$a0)-$b0));
}
}
else
{
# Lines criss-cross: Calculate the projected closest points
my $t = ($b0 - $a0);
# Math::Matrix won't wirth with Math::Vector::Real
# even though they are blessed arrays,
# so convert them to arrays and back to refs:
my $detA = Math::Matrix->new([ [ #$t ], [ #$_B ], [ #$cross] ])->det;
my $detB = Math::Matrix->new([ [ #$t ], [ #$_A ], [ #$cross] ])->det;
my $t0 = $detA / $denom;
my $t1 = $detB / $denom;
my $pA = $a0 + ($_A * $t0); # Projected closest point on segment A
my $pB = $b0 + ($_B * $t1); # Projected closest point on segment A
if ($t0 < 0)
{
$pA = $a0;
}
elsif ($t0 > $magA)
{
$pA = $a1;
}
if ($t1 < 0)
{
$pB = $b0;
}
elsif ($t1 > $magB)
{
$pB = $b1;
}
# Clamp projection A
if ($t0 < 0 || $t0 > $magA)
{
my $dot = $_B * ($pA-$b0);
if ($dot < 0)
{
$dot = 0;
}
elsif ($dot > $magB)
{
$dot = $magB;
}
$pB = $b0 + ($_B * $dot)
}
# Clamp projection B
if ($t1 < 0 || $t1 > $magB)
{
my $dot = $_A * ($pB-$a0);
if ($dot < 0)
{
$dot = 0;
}
elsif ($dot > $magA)
{
$dot = $magA;
}
$pA = $a0 + ($_A * $dot)
}
return V($pA, $pB, abs($pA-$pB));
}
}
print "example: " . line_intersect(
[13.43, 21.77, 46.81 ], [27.83, 31.74, -26.60 ],
[77.54, 7.53, 6.22 ], [26.99, 12.39, 11.18 ]) . "\n" ;
print "contiguous: " . line_intersect(
[0, 0, 0 ], [ 0, 0, 1 ],
[0, 0, 1 ], [ 0, 0, 2 ],
) . "\n" ;
print "contiguous 90: " . line_intersect(
[0, 0, 0 ], [ 0, 0, 1 ],
[0, 0, 1 ], [ 0, 1, 1 ],
) . "\n" ;
print "colinear separate: " . line_intersect(
[0, 0, 0 ], [ 0, 0, 1 ],
[0, 0, 2 ], [ 0, 0, 3 ],
) . "\n" ;
print "cross: " . line_intersect(
[1, 1, 0 ], [ -1, -1, 0 ],
[-1, 1, 0 ], [ 1, -1, 0 ],
) . "\n" ;
print "cross+z: " . line_intersect(
[1, 1, 0 ], [ -1, -1, 0 ],
[-1, 1, 1 ], [ 1, -1, 1 ],
) . "\n" ;
print "full overlap1: " . line_intersect(
[2, 0, 0 ], [ 5, 0, 0 ],
[3, 0, 0 ], [ 4, 0, 0 ],
) . "\n" ;
print "full overlap2: " . line_intersect(
[3, 0, 0 ], [ 4, 0, 0 ],
[2, 0, 0 ], [ 5, 0, 0 ],
) . "\n" ;
print "partial overlap1: " . line_intersect(
[2, 0, 0 ], [ 5, 0, 0 ],
[3, 0, 0 ], [ 6, 0, 0 ],
) . "\n" ;
print "partial overlap2: " . line_intersect(
[3, 0, 0 ], [ 6, 0, 0 ],
[2, 0, 0 ], [ 5, 0, 0 ],
) . "\n" ;
print "parallel: " . line_intersect(
[3, 0, 0 ], [ 6, 0, 0 ],
[3, 0, 1 ], [ 6, 0, 1 ],
) . "\n" ;
Output
example: {{20.29994361624, 26.5264817954106, 11.7875999397098}, {26.99, 12.39, 11.18}, 15.6513944955904}
contiguous: {{0, 0, 1}, {0, 0, 1}, 0}
contiguous 90: {{0, 0, 1}, {0, 0, 1}, 0}
colinear separate: {{0, 0, 1}, {0, 0, 2}, 1}
cross: {{-2.22044604925031e-16, -2.22044604925031e-16, 0}, {2.22044604925031e-16, -2.22044604925031e-16, 0}, 4.44089209850063e-16}
cross+z: {{-2.22044604925031e-16, -2.22044604925031e-16, 0}, {2.22044604925031e-16, -2.22044604925031e-16, 1}, 1}
full overlap1: {{}, {}, 0}
full overlap2: {{}, {}, 0}
partial overlap1: {{}, {}, 0}
partial overlap2: {{}, {}, 0}
parallel: {{}, {}, 1}
Here is the solution from Fnord just for Ray-Ray Intersection in c# (infinite Lines, not Line segments)
It requires System.Numerics.Vector3
public static (Vector3 pointRayA, Vector3 pointRayB) ClosestPointRayRay((Vector3 point, Vector3 dir) rayA,
(Vector3 point, Vector3 dir) rayB)
{
var a = Normalize(rayA.dir);
var b = Normalize(rayB.dir);
var cross = Vector3.Cross(a, b);
var crossMag = cross.Length();
var denominator = crossMag * crossMag;
var t = rayB.point - rayA.point;
var detA = new Matrix3X3(t, b, cross).Det;
var detB = new Matrix3X3(t, a, cross).Det;
var t0 = detA / denominator;
var t1 = detB / denominator;
var pa = rayA.point + (a * t0);
var pb = rayB.point + (b * t1);
return (pa, pb);
}
public struct Matrix3X3
{
private float a11, a12, a13, a21, a22, a23, a31, a32, a33;
public Matrix3X3(Vector3 col1, Vector3 col2, Vector3 col3)
{
a11 = col1.X;
a21 = col1.Y;
a31 = col1.Z;
a12 = col2.X;
a22 = col2.Y;
a32 = col2.Z;
a13 = col3.X;
a23 = col3.Y;
a33 = col3.Z;
}
public float Det =>
a11 * a22 * a33 + a12 * a23 * a31 + a13 * a21 * a32 -
(a31 * a22 * a13 + a32 * a23 * a11 + a33 * a21 * a12);
}
it returns the closest point on RayA to RayB ,named pointRayA, and vice versa.
A short test:
[Test]
public void RayRayIntersection()
{
var lineABase = new Vector3(0, 0, 0);
var lineADir = new Vector3(0, 0, 1);
var lineBBase = new Vector3(1, 0, 0);
var lineBDir = new Vector3(0, 1, 0);
var res = ClosestPointRayRay((lineABase, lineADir), (lineBBase, lineBDir));
Console.WriteLine(res);
}
returns (<0, 0, 0>, <1, 0, 0>)
I was looking for a way to compute the shortest distance between a large number of 3D lines. The code from Fnord was a start, but it was for only a single batch. I ported most of it to multi-batch, below.
import numpy as np
import time
def closest_distance_between_lines(a0, a1, b0, b1):
# equation for a line
number_of_lines = a0.shape[0]
A = a1 - a0
B = b1 - b0
# get the magnitude of each line
magA = np.broadcast_to(np.expand_dims(np.linalg.norm(A, axis=1), axis=1), (number_of_lines, 3))
magB = np.broadcast_to(np.expand_dims(np.linalg.norm(B, axis=1), axis=1), (number_of_lines, 3))
# get the unit-normalized lines
_A = A / magA
_B = B / magB
# get the perpendicular line
cross = np.cross(_A, _B, axis=1)
# normalize the perpendicular line
denom = np.linalg.norm(cross, axis=1)**2
# get the line between the start of the previous two lines
t = (b0 - a0)
stacked_matrices_A = np.stack([t, _B, cross], axis=1)
detA = np.linalg.det(stacked_matrices_A)
stacked_matrices_B = np.stack([t, _A, cross], axis=1)
detB = np.linalg.det(stacked_matrices_B)
t0 = detA/denom
t1 = detB/denom
t0 = np.broadcast_to(np.expand_dims(t0, axis=1), (number_of_lines, 3))
t1 = np.broadcast_to(np.expand_dims(t1, axis=1), (number_of_lines, 3))
# get the points that represent the closest projected distance between the two lines
pA = a0 + (_A * t0)
pB = b0 + (_B * t1)
return pA, pB, np.linalg.norm(pA-pB, axis=1)
number_of_samples = 10000000
a0 = np.random.rand(number_of_samples,3)
a1 = np.random.rand(number_of_samples,3)
b0 = np.random.rand(number_of_samples,3)
b1 = np.random.rand(number_of_samples,3)
start_time = time.time()
point_line_a, point_line_b, distance = closest_distance_between_lines(a0, a1, b0, b1)
end_time = time.time()
print("\nPoints pA on line A that are closest to line B with shape {}:\n{}".format(point_line_a.shape, point_line_a))
print("\nPoints pB on line B that are closest to line A with shape {}:\n{}".format(point_line_b.shape, point_line_b))
print("\nDistances between pA and pB with shape {}:\n{}".format(distance.shape, distance))
print("\n{:.2} seconds to compute closest distances between {:,} lines\n".format(end_time - start_time, number_of_samples))
Here is the output of above:
Points pA on line A that are closest to line B with shape (10000000, 3):
[[0.51075268 0.63261352 0.41417815]
[0.88225225 0.41163515 0.06090485]
[0.27712801 0.99045177 0.58932854]
...
[0.25982217 0.11225041 0.79015618]
[0.58653313 0.47864821 0.11680724]
[0.38297058 0.2251661 1.11088736]]
Points pB on line B that are closest to line A with shape (10000000, 3):
[[0.5084189 0.42417817 0.5847618 ]
[0.83041058 0.38914519 0.02384158]
[0.26068716 0.98567827 0.5010647 ]
...
[0.34356827 0.42162445 0.75820875]
[0.44523571 0.40278146 0.0014156 ]
[0.28498604 0.23670301 1.00712087]]
Distances between pA and pB with shape (10000000,):
[0.26935018 0.0675799 0.08990881 ... 0.32209679 0.19757518 0.14318364]
8.3 seconds to compute closest distances between 10,000,000 lines
Note, the code does not check for parallel lines, and it technically considers the entire line (rather than just the line segment).
To make it even faster, below is the same code ported to PyTorch:
import torch
import time
device = torch.device('cuda:0')
def closest_distance_between_lines(a0, a1, b0, b1):
number_of_lines = a0.shape[0]
# equation for a line
A = a1 - a0
B = b1 - b0
# get the magnitude of each line
magA = torch.broadcast_to(torch.unsqueeze(torch.linalg.norm(A, dim=1), dim=1), (number_of_lines, 3))
magB = torch.broadcast_to(torch.unsqueeze(torch.linalg.norm(B, dim=1), dim=1), (number_of_lines, 3))
# get the unit-normalized lines
_A = A / magA
_B = B / magB
# get the perpendicular line
cross = torch.cross(_A, _B, dim=1)
# normalize the perpendicular line
denom = torch.linalg.norm(cross, dim=1)**2
# get the line between the start of the previous two lines
t = (b0 - a0)
stacked_matrices_A = torch.stack([t, _B, cross], dim=1)
detA = torch.linalg.det(stacked_matrices_A)
stacked_matrices_B = torch.stack([t, _A, cross], dim=1)
detB = torch.linalg.det(stacked_matrices_B)
t0 = detA / denom
t1 = detB / denom
t0 = torch.broadcast_to(torch.unsqueeze(t0, dim=1), (number_of_lines, 3))
t1 = torch.broadcast_to(torch.unsqueeze(t1, dim=1), (number_of_lines, 3))
# get the points that represent the closest projected distance between the two lines
pA = a0 + (_A * t0)
pB = b0 + (_B * t1)
return pA, pB, torch.linalg.norm(pA-pB, dim=1)
number_of_samples = 10000000
a0 = torch.rand(number_of_samples,3).to(device=device)
a1 = torch.rand(number_of_samples,3).to(device=device)
b0 = torch.rand(number_of_samples,3).to(device=device)
b1 = torch.rand(number_of_samples,3).to(device=device)
start_time = time.time()
point_line_a, point_line_b, distance = closest_distance_between_lines(a0, a1, b0, b1)
end_time = time.time()
print("\nPoints pA on line A that are closest to line B with shape {}:\n{}".format(point_line_a.shape, point_line_a))
print("\nPoints pB on line B that are closest to line A with shape {}:\n{}".format(point_line_b.shape, point_line_b))
print("\nDistances between pA and pB with shape {}:\n{}".format(distance.shape, distance))
print("\n{:.2} seconds to compute closest distances between {:,} lines\n".format(end_time - start_time, number_of_samples))
On my GPU, the runtime from NumPy to Torch speeds up from 8.3 seconds to 0.13 seconds.
Enjoy.
I need a basic function to find the shortest distance between a point and a line segment. Feel free to write the solution in any language you want; I can translate it into what I'm using (Javascript).
EDIT: My line segment is defined by two endpoints. So my line segment AB is defined by the two points A (x1,y1) and B (x2,y2). I'm trying to find the distance between this line segment and a point C (x3,y3). My geometry skills are rusty, so the examples I've seen are confusing, I'm sorry to admit.
Eli, the code you've settled on is incorrect. A point near the line on which the segment lies but far off one end of the segment would be incorrectly judged near the segment. Update: The incorrect answer mentioned is no longer the accepted one.
Here's some correct code, in C++. It presumes a class 2D-vector class vec2 {float x,y;}, essentially, with operators to add, subract, scale, etc, and a distance and dot product function (i.e. x1 x2 + y1 y2).
float minimum_distance(vec2 v, vec2 w, vec2 p) {
// Return minimum distance between line segment vw and point p
const float l2 = length_squared(v, w); // i.e. |w-v|^2 - avoid a sqrt
if (l2 == 0.0) return distance(p, v); // v == w case
// Consider the line extending the segment, parameterized as v + t (w - v).
// We find projection of point p onto the line.
// It falls where t = [(p-v) . (w-v)] / |w-v|^2
// We clamp t from [0,1] to handle points outside the segment vw.
const float t = max(0, min(1, dot(p - v, w - v) / l2));
const vec2 projection = v + t * (w - v); // Projection falls on the segment
return distance(p, projection);
}
EDIT: I needed a Javascript implementation, so here it is, with no dependencies (or comments, but it's a direct port of the above). Points are represented as objects with x and y attributes.
function sqr(x) { return x * x }
function dist2(v, w) { return sqr(v.x - w.x) + sqr(v.y - w.y) }
function distToSegmentSquared(p, v, w) {
var l2 = dist2(v, w);
if (l2 == 0) return dist2(p, v);
var t = ((p.x - v.x) * (w.x - v.x) + (p.y - v.y) * (w.y - v.y)) / l2;
t = Math.max(0, Math.min(1, t));
return dist2(p, { x: v.x + t * (w.x - v.x),
y: v.y + t * (w.y - v.y) });
}
function distToSegment(p, v, w) { return Math.sqrt(distToSegmentSquared(p, v, w)); }
EDIT 2: I needed a Java version, but more important, I needed it in 3d instead of 2d.
float dist_to_segment_squared(float px, float py, float pz, float lx1, float ly1, float lz1, float lx2, float ly2, float lz2) {
float line_dist = dist_sq(lx1, ly1, lz1, lx2, ly2, lz2);
if (line_dist == 0) return dist_sq(px, py, pz, lx1, ly1, lz1);
float t = ((px - lx1) * (lx2 - lx1) + (py - ly1) * (ly2 - ly1) + (pz - lz1) * (lz2 - lz1)) / line_dist;
t = constrain(t, 0, 1);
return dist_sq(px, py, pz, lx1 + t * (lx2 - lx1), ly1 + t * (ly2 - ly1), lz1 + t * (lz2 - lz1));
}
Here, in the function parameters, <px,py,pz> is the point in question and the line segment has the endpoints <lx1,ly1,lz1> and <lx2,ly2,lz2>. The function dist_sq (which is assumed to exist) finds the square of the distance between two points.
Here is the simplest complete code in Javascript.
x, y is your target point and x1, y1 to x2, y2 is your line segment.
UPDATED: fix for 0 length line problem from comments.
function pDistance(x, y, x1, y1, x2, y2) {
var A = x - x1;
var B = y - y1;
var C = x2 - x1;
var D = y2 - y1;
var dot = A * C + B * D;
var len_sq = C * C + D * D;
var param = -1;
if (len_sq != 0) //in case of 0 length line
param = dot / len_sq;
var xx, yy;
if (param < 0) {
xx = x1;
yy = y1;
}
else if (param > 1) {
xx = x2;
yy = y2;
}
else {
xx = x1 + param * C;
yy = y1 + param * D;
}
var dx = x - xx;
var dy = y - yy;
return Math.sqrt(dx * dx + dy * dy);
}
UPDATED: Kotlin version
fun getDistance(x: Double, y: Double, x1: Double, y1: Double, x2: Double, y2: Double): Double {
val a = x - x1
val b = y - y1
val c = x2 - x1
val d = y2 - y1
val lenSq = c * c + d * d
val param = if (lenSq != .0) { //in case of 0 length line
val dot = a * c + b * d
dot / lenSq
} else {
-1.0
}
val (xx, yy) = when {
param < 0 -> x1 to y1
param > 1 -> x2 to y2
else -> x1 + param * c to y1 + param * d
}
val dx = x - xx
val dy = y - yy
return hypot(dx, dy)
}
This is an implementation made for FINITE LINE SEGMENTS, not infinite lines like most other functions here seem to be (that's why I made this).
Implementation of theory by Paul Bourke.
Python:
def dist(x1, y1, x2, y2, x3, y3): # x3,y3 is the point
px = x2-x1
py = y2-y1
norm = px*px + py*py
u = ((x3 - x1) * px + (y3 - y1) * py) / float(norm)
if u > 1:
u = 1
elif u < 0:
u = 0
x = x1 + u * px
y = y1 + u * py
dx = x - x3
dy = y - y3
# Note: If the actual distance does not matter,
# if you only want to compare what this function
# returns to other results of this function, you
# can just return the squared distance instead
# (i.e. remove the sqrt) to gain a little performance
dist = (dx*dx + dy*dy)**.5
return dist
AS3:
public static function segmentDistToPoint(segA:Point, segB:Point, p:Point):Number
{
var p2:Point = new Point(segB.x - segA.x, segB.y - segA.y);
var something:Number = p2.x*p2.x + p2.y*p2.y;
var u:Number = ((p.x - segA.x) * p2.x + (p.y - segA.y) * p2.y) / something;
if (u > 1)
u = 1;
else if (u < 0)
u = 0;
var x:Number = segA.x + u * p2.x;
var y:Number = segA.y + u * p2.y;
var dx:Number = x - p.x;
var dy:Number = y - p.y;
var dist:Number = Math.sqrt(dx*dx + dy*dy);
return dist;
}
Java
private double shortestDistance(float x1,float y1,float x2,float y2,float x3,float y3)
{
float px=x2-x1;
float py=y2-y1;
float temp=(px*px)+(py*py);
float u=((x3 - x1) * px + (y3 - y1) * py) / (temp);
if(u>1){
u=1;
}
else if(u<0){
u=0;
}
float x = x1 + u * px;
float y = y1 + u * py;
float dx = x - x3;
float dy = y - y3;
double dist = Math.sqrt(dx*dx + dy*dy);
return dist;
}
In my own question thread how to calculate shortest 2D distance between a point and a line segment in all cases in C, C# / .NET 2.0 or Java? I was asked to put a C# answer here when I find one: so here it is, modified from http://www.topcoder.com/tc?d1=tutorials&d2=geometry1&module=Static :
//Compute the dot product AB . BC
private double DotProduct(double[] pointA, double[] pointB, double[] pointC)
{
double[] AB = new double[2];
double[] BC = new double[2];
AB[0] = pointB[0] - pointA[0];
AB[1] = pointB[1] - pointA[1];
BC[0] = pointC[0] - pointB[0];
BC[1] = pointC[1] - pointB[1];
double dot = AB[0] * BC[0] + AB[1] * BC[1];
return dot;
}
//Compute the cross product AB x AC
private double CrossProduct(double[] pointA, double[] pointB, double[] pointC)
{
double[] AB = new double[2];
double[] AC = new double[2];
AB[0] = pointB[0] - pointA[0];
AB[1] = pointB[1] - pointA[1];
AC[0] = pointC[0] - pointA[0];
AC[1] = pointC[1] - pointA[1];
double cross = AB[0] * AC[1] - AB[1] * AC[0];
return cross;
}
//Compute the distance from A to B
double Distance(double[] pointA, double[] pointB)
{
double d1 = pointA[0] - pointB[0];
double d2 = pointA[1] - pointB[1];
return Math.Sqrt(d1 * d1 + d2 * d2);
}
//Compute the distance from AB to C
//if isSegment is true, AB is a segment, not a line.
double LineToPointDistance2D(double[] pointA, double[] pointB, double[] pointC,
bool isSegment)
{
double dist = CrossProduct(pointA, pointB, pointC) / Distance(pointA, pointB);
if (isSegment)
{
double dot1 = DotProduct(pointA, pointB, pointC);
if (dot1 > 0)
return Distance(pointB, pointC);
double dot2 = DotProduct(pointB, pointA, pointC);
if (dot2 > 0)
return Distance(pointA, pointC);
}
return Math.Abs(dist);
}
I'm #SO not to answer but ask questions so I hope I don't get million down votes for some reasons but constructing critic. I just wanted (and was encouraged) to share somebody else's ideas since the solutions in this thread are either with some exotic language (Fortran, Mathematica) or tagged as faulty by somebody. The only useful one (by Grumdrig) for me is written with C++ and nobody tagged it faulty. But it's missing the methods (dot etc.) that are called.
For anyone interested, here's a trivial conversion of Joshua's Javascript code to Objective-C:
- (double)distanceToPoint:(CGPoint)p fromLineSegmentBetween:(CGPoint)l1 and:(CGPoint)l2
{
double A = p.x - l1.x;
double B = p.y - l1.y;
double C = l2.x - l1.x;
double D = l2.y - l1.y;
double dot = A * C + B * D;
double len_sq = C * C + D * D;
double param = dot / len_sq;
double xx, yy;
if (param < 0 || (l1.x == l2.x && l1.y == l2.y)) {
xx = l1.x;
yy = l1.y;
}
else if (param > 1) {
xx = l2.x;
yy = l2.y;
}
else {
xx = l1.x + param * C;
yy = l1.y + param * D;
}
double dx = p.x - xx;
double dy = p.y - yy;
return sqrtf(dx * dx + dy * dy);
}
I needed this solution to work with MKMapPoint so I will share it in case someone else needs it. Just some minor change and this will return the distance in meters :
- (double)distanceToPoint:(MKMapPoint)p fromLineSegmentBetween:(MKMapPoint)l1 and:(MKMapPoint)l2
{
double A = p.x - l1.x;
double B = p.y - l1.y;
double C = l2.x - l1.x;
double D = l2.y - l1.y;
double dot = A * C + B * D;
double len_sq = C * C + D * D;
double param = dot / len_sq;
double xx, yy;
if (param < 0 || (l1.x == l2.x && l1.y == l2.y)) {
xx = l1.x;
yy = l1.y;
}
else if (param > 1) {
xx = l2.x;
yy = l2.y;
}
else {
xx = l1.x + param * C;
yy = l1.y + param * D;
}
return MKMetersBetweenMapPoints(p, MKMapPointMake(xx, yy));
}
In F#, the distance from the point c to the line segment between a and b is given by:
let pointToLineSegmentDistance (a: Vector, b: Vector) (c: Vector) =
let d = b - a
let s = d.Length
let lambda = (c - a) * d / s
let p = (lambda |> max 0.0 |> min s) * d / s
(a + p - c).Length
The vector d points from a to b along the line segment. The dot product of d/s with c-a gives the parameter of the point of closest approach between the infinite line and the point c. The min and max function are used to clamp this parameter to the range 0..s so that the point lies between a and b. Finally, the length of a+p-c is the distance from c to the closest point on the line segment.
Example use:
pointToLineSegmentDistance (Vector(0.0, 0.0), Vector(1.0, 0.0)) (Vector(-1.0, 1.0))
In Mathematica
It uses a parametric description of the segment, and projects the point into the line defined by the segment. As the parameter goes from 0 to 1 in the segment, if the projection is outside this bounds, we compute the distance to the corresponding enpoint, instead of the straight line normal to the segment.
Clear["Global`*"];
distance[{start_, end_}, pt_] :=
Module[{param},
param = ((pt - start).(end - start))/Norm[end - start]^2; (*parameter. the "."
here means vector product*)
Which[
param < 0, EuclideanDistance[start, pt], (*If outside bounds*)
param > 1, EuclideanDistance[end, pt],
True, EuclideanDistance[pt, start + param (end - start)] (*Normal distance*)
]
];
Plotting result:
Plot3D[distance[{{0, 0}, {1, 0}}, {xp, yp}], {xp, -1, 2}, {yp, -1, 2}]
Plot those points nearer than a cutoff distance:
Contour Plot:
Hey, I just wrote this yesterday. It's in Actionscript 3.0, which is basically Javascript, though you might not have the same Point class.
//st = start of line segment
//b = the line segment (as in: st + b = end of line segment)
//pt = point to test
//Returns distance from point to line segment.
//Note: nearest point on the segment to the test point is right there if we ever need it
public static function linePointDist( st:Point, b:Point, pt:Point ):Number
{
var nearestPt:Point; //closest point on seqment to pt
var keyDot:Number = dot( b, pt.subtract( st ) ); //key dot product
var bLenSq:Number = dot( b, b ); //Segment length squared
if( keyDot <= 0 ) //pt is "behind" st, use st
{
nearestPt = st
}
else if( keyDot >= bLenSq ) //pt is "past" end of segment, use end (notice we are saving twin sqrts here cuz)
{
nearestPt = st.add(b);
}
else //pt is inside segment, reuse keyDot and bLenSq to get percent of seqment to move in to find closest point
{
var keyDotToPctOfB:Number = keyDot/bLenSq; //REM dot product comes squared
var partOfB:Point = new Point( b.x * keyDotToPctOfB, b.y * keyDotToPctOfB );
nearestPt = st.add(partOfB);
}
var dist:Number = (pt.subtract(nearestPt)).length;
return dist;
}
Also, there's a pretty complete and readable discussion of the problem here: notejot.com
One line solution using arctangents:
The idea is to move A to (0, 0) and rotate triangle clockwise to make C lay on X axis,
when this happen, By will be the distance.
a angle = Atan(Cy - Ay, Cx - Ax);
b angle = Atan(By - Ay, Bx - Ax);
AB length = Sqrt( (Bx - Ax)^2 + (By - Ay)^2 )
By = Sin ( bAngle - aAngle) * ABLength
C#
public double Distance(Point a, Point b, Point c)
{
// normalize points
Point cn = new Point(c.X - a.X, c.Y - a.Y);
Point bn = new Point(b.X - a.X, b.Y - a.Y);
double angle = Math.Atan2(bn.Y, bn.X) - Math.Atan2(cn.Y, cn.X);
double abLength = Math.Sqrt(bn.X*bn.X + bn.Y*bn.Y);
return Math.Sin(angle)*abLength;
}
One line C# (to be converted to SQL)
double distance = Math.Sin(Math.Atan2(b.Y - a.Y, b.X - a.X) - Math.Atan2(c.Y - a.Y, c.X - a.X)) * Math.Sqrt((b.X - a.X) * (b.X - a.X) + (b.Y - a.Y) * (b.Y - a.Y))
For the lazy, here's my Objective-C port of #Grumdrig's solution above:
CGFloat sqr(CGFloat x) { return x*x; }
CGFloat dist2(CGPoint v, CGPoint w) { return sqr(v.x - w.x) + sqr(v.y - w.y); }
CGFloat distanceToSegmentSquared(CGPoint p, CGPoint v, CGPoint w)
{
CGFloat l2 = dist2(v, w);
if (l2 == 0.0f) return dist2(p, v);
CGFloat t = ((p.x - v.x) * (w.x - v.x) + (p.y - v.y) * (w.y - v.y)) / l2;
if (t < 0.0f) return dist2(p, v);
if (t > 1.0f) return dist2(p, w);
return dist2(p, CGPointMake(v.x + t * (w.x - v.x), v.y + t * (w.y - v.y)));
}
CGFloat distanceToSegment(CGPoint point, CGPoint segmentPointV, CGPoint segmentPointW)
{
return sqrtf(distanceToSegmentSquared(point, segmentPointV, segmentPointW));
}
Couldn't resist coding it in python :)
from math import sqrt, fabs
def pdis(a, b, c):
t = b[0]-a[0], b[1]-a[1] # Vector ab
dd = sqrt(t[0]**2+t[1]**2) # Length of ab
t = t[0]/dd, t[1]/dd # unit vector of ab
n = -t[1], t[0] # normal unit vector to ab
ac = c[0]-a[0], c[1]-a[1] # vector ac
return fabs(ac[0]*n[0]+ac[1]*n[1]) # Projection of ac to n (the minimum distance)
print pdis((1,1), (2,2), (2,0)) # Example (answer is 1.414)
Ditto for fortran :)
real function pdis(a, b, c)
real, dimension(0:1), intent(in) :: a, b, c
real, dimension(0:1) :: t, n, ac
real :: dd
t = b - a ! Vector ab
dd = sqrt(t(0)**2+t(1)**2) ! Length of ab
t = t/dd ! unit vector of ab
n = (/-t(1), t(0)/) ! normal unit vector to ab
ac = c - a ! vector ac
pdis = abs(ac(0)*n(0)+ac(1)*n(1)) ! Projection of ac to n (the minimum distance)
end function pdis
program test
print *, pdis((/1.0,1.0/), (/2.0,2.0/), (/2.0,0.0/)) ! Example (answer is 1.414)
end program test
Here is a more complete spelling out of Grumdrig's solution. This version also returns the closest point itself.
#include "stdio.h"
#include "math.h"
class Vec2
{
public:
float _x;
float _y;
Vec2()
{
_x = 0;
_y = 0;
}
Vec2( const float x, const float y )
{
_x = x;
_y = y;
}
Vec2 operator+( const Vec2 &v ) const
{
return Vec2( this->_x + v._x, this->_y + v._y );
}
Vec2 operator-( const Vec2 &v ) const
{
return Vec2( this->_x - v._x, this->_y - v._y );
}
Vec2 operator*( const float f ) const
{
return Vec2( this->_x * f, this->_y * f );
}
float DistanceToSquared( const Vec2 p ) const
{
const float dX = p._x - this->_x;
const float dY = p._y - this->_y;
return dX * dX + dY * dY;
}
float DistanceTo( const Vec2 p ) const
{
return sqrt( this->DistanceToSquared( p ) );
}
float DotProduct( const Vec2 p ) const
{
return this->_x * p._x + this->_y * p._y;
}
};
// return minimum distance between line segment vw and point p, and the closest point on the line segment, q
float DistanceFromLineSegmentToPoint( const Vec2 v, const Vec2 w, const Vec2 p, Vec2 * const q )
{
const float distSq = v.DistanceToSquared( w ); // i.e. |w-v|^2 ... avoid a sqrt
if ( distSq == 0.0 )
{
// v == w case
(*q) = v;
return v.DistanceTo( p );
}
// consider the line extending the segment, parameterized as v + t (w - v)
// we find projection of point p onto the line
// it falls where t = [(p-v) . (w-v)] / |w-v|^2
const float t = ( p - v ).DotProduct( w - v ) / distSq;
if ( t < 0.0 )
{
// beyond the v end of the segment
(*q) = v;
return v.DistanceTo( p );
}
else if ( t > 1.0 )
{
// beyond the w end of the segment
(*q) = w;
return w.DistanceTo( p );
}
// projection falls on the segment
const Vec2 projection = v + ( ( w - v ) * t );
(*q) = projection;
return p.DistanceTo( projection );
}
float DistanceFromLineSegmentToPoint( float segmentX1, float segmentY1, float segmentX2, float segmentY2, float pX, float pY, float *qX, float *qY )
{
Vec2 q;
float distance = DistanceFromLineSegmentToPoint( Vec2( segmentX1, segmentY1 ), Vec2( segmentX2, segmentY2 ), Vec2( pX, pY ), &q );
(*qX) = q._x;
(*qY) = q._y;
return distance;
}
void TestDistanceFromLineSegmentToPoint( float segmentX1, float segmentY1, float segmentX2, float segmentY2, float pX, float pY )
{
float qX;
float qY;
float d = DistanceFromLineSegmentToPoint( segmentX1, segmentY1, segmentX2, segmentY2, pX, pY, &qX, &qY );
printf( "line segment = ( ( %f, %f ), ( %f, %f ) ), p = ( %f, %f ), distance = %f, q = ( %f, %f )\n",
segmentX1, segmentY1, segmentX2, segmentY2, pX, pY, d, qX, qY );
}
void TestDistanceFromLineSegmentToPoint()
{
TestDistanceFromLineSegmentToPoint( 0, 0, 1, 1, 1, 0 );
TestDistanceFromLineSegmentToPoint( 0, 0, 20, 10, 5, 4 );
TestDistanceFromLineSegmentToPoint( 0, 0, 20, 10, 30, 15 );
TestDistanceFromLineSegmentToPoint( 0, 0, 20, 10, -30, 15 );
TestDistanceFromLineSegmentToPoint( 0, 0, 10, 0, 5, 1 );
TestDistanceFromLineSegmentToPoint( 0, 0, 0, 10, 1, 5 );
}
Consider this modification to Grumdrig's answer above. Many times you'll find that floating point imprecision can cause problems. I'm using doubles in the version below, but you can easily change to floats. The important part is that it uses an epsilon to handle the "slop". In addition, you'll many times want to know WHERE the intersection happened, or if it happened at all. If the returned t is < 0.0 or > 1.0, no collision occurred. However, even if no collision occurred, many times you'll want to know where the closest point on the segment to P is, and thus I use qx and qy to return this location.
double PointSegmentDistanceSquared( double px, double py,
double p1x, double p1y,
double p2x, double p2y,
double& t,
double& qx, double& qy)
{
static const double kMinSegmentLenSquared = 0.00000001; // adjust to suit. If you use float, you'll probably want something like 0.000001f
static const double kEpsilon = 1.0E-14; // adjust to suit. If you use floats, you'll probably want something like 1E-7f
double dx = p2x - p1x;
double dy = p2y - p1y;
double dp1x = px - p1x;
double dp1y = py - p1y;
const double segLenSquared = (dx * dx) + (dy * dy);
if (segLenSquared >= -kMinSegmentLenSquared && segLenSquared <= kMinSegmentLenSquared)
{
// segment is a point.
qx = p1x;
qy = p1y;
t = 0.0;
return ((dp1x * dp1x) + (dp1y * dp1y));
}
else
{
// Project a line from p to the segment [p1,p2]. By considering the line
// extending the segment, parameterized as p1 + (t * (p2 - p1)),
// we find projection of point p onto the line.
// It falls where t = [(p - p1) . (p2 - p1)] / |p2 - p1|^2
t = ((dp1x * dx) + (dp1y * dy)) / segLenSquared;
if (t < kEpsilon)
{
// intersects at or to the "left" of first segment vertex (p1x, p1y). If t is approximately 0.0, then
// intersection is at p1. If t is less than that, then there is no intersection (i.e. p is not within
// the 'bounds' of the segment)
if (t > -kEpsilon)
{
// intersects at 1st segment vertex
t = 0.0;
}
// set our 'intersection' point to p1.
qx = p1x;
qy = p1y;
// Note: If you wanted the ACTUAL intersection point of where the projected lines would intersect if
// we were doing PointLineDistanceSquared, then qx would be (p1x + (t * dx)) and qy would be (p1y + (t * dy)).
}
else if (t > (1.0 - kEpsilon))
{
// intersects at or to the "right" of second segment vertex (p2x, p2y). If t is approximately 1.0, then
// intersection is at p2. If t is greater than that, then there is no intersection (i.e. p is not within
// the 'bounds' of the segment)
if (t < (1.0 + kEpsilon))
{
// intersects at 2nd segment vertex
t = 1.0;
}
// set our 'intersection' point to p2.
qx = p2x;
qy = p2y;
// Note: If you wanted the ACTUAL intersection point of where the projected lines would intersect if
// we were doing PointLineDistanceSquared, then qx would be (p1x + (t * dx)) and qy would be (p1y + (t * dy)).
}
else
{
// The projection of the point to the point on the segment that is perpendicular succeeded and the point
// is 'within' the bounds of the segment. Set the intersection point as that projected point.
qx = p1x + (t * dx);
qy = p1y + (t * dy);
}
// return the squared distance from p to the intersection point. Note that we return the squared distance
// as an optimization because many times you just need to compare relative distances and the squared values
// works fine for that. If you want the ACTUAL distance, just take the square root of this value.
double dpqx = px - qx;
double dpqy = py - qy;
return ((dpqx * dpqx) + (dpqy * dpqy));
}
}
I'm assuming you want to find the shortest distance between the point and a line segment; to do this, you need to find the line (lineA) which is perpendicular to your line segment (lineB) which goes through your point, determine the intersection between that line (lineA) and your line which goes through your line segment (lineB); if that point is between the two points of your line segment, then the distance is the distance between your point and the point you just found which is the intersection of lineA and lineB; if the point is not between the two points of your line segment, you need to get the distance between your point and the closer of two ends of the line segment; this can be done easily by taking the square distance (to avoid a square root) between the point and the two points of the line segment; whichever is closer, take the square root of that one.
Here it is using Swift
/* Distance from a point (p1) to line l1 l2 */
func distanceFromPoint(p: CGPoint, toLineSegment l1: CGPoint, and l2: CGPoint) -> CGFloat {
let A = p.x - l1.x
let B = p.y - l1.y
let C = l2.x - l1.x
let D = l2.y - l1.y
let dot = A * C + B * D
let len_sq = C * C + D * D
let param = dot / len_sq
var xx, yy: CGFloat
if param < 0 || (l1.x == l2.x && l1.y == l2.y) {
xx = l1.x
yy = l1.y
} else if param > 1 {
xx = l2.x
yy = l2.y
} else {
xx = l1.x + param * C
yy = l1.y + param * D
}
let dx = p.x - xx
let dy = p.y - yy
return sqrt(dx * dx + dy * dy)
}
Grumdrig's C++/JavaScript implementation was very useful to me, so I have provided a Python direct port that I am using. The complete code is here.
class Point(object):
def __init__(self, x, y):
self.x = float(x)
self.y = float(y)
def square(x):
return x * x
def distance_squared(v, w):
return square(v.x - w.x) + square(v.y - w.y)
def distance_point_segment_squared(p, v, w):
# Segment length squared, |w-v|^2
d2 = distance_squared(v, w)
if d2 == 0:
# v == w, return distance to v
return distance_squared(p, v)
# Consider the line extending the segment, parameterized as v + t (w - v).
# We find projection of point p onto the line.
# It falls where t = [(p-v) . (w-v)] / |w-v|^2
t = ((p.x - v.x) * (w.x - v.x) + (p.y - v.y) * (w.y - v.y)) / d2;
if t < 0:
# Beyond v end of the segment
return distance_squared(p, v)
elif t > 1.0:
# Beyond w end of the segment
return distance_squared(p, w)
else:
# Projection falls on the segment.
proj = Point(v.x + t * (w.x - v.x), v.y + t * (w.y - v.y))
# print proj.x, proj.y
return distance_squared(p, proj)
And now my solution as well......
(Javascript)
It is very fast because I try to avoid any Math.pow functions.
As you can see, at the end of the function I have the distance of the line.
code is from the lib http://www.draw2d.org/graphiti/jsdoc/#!/example
/**
* Static util function to determine is a point(px,py) on the line(x1,y1,x2,y2)
* A simple hit test.
*
* #return {boolean}
* #static
* #private
* #param {Number} coronaWidth the accepted corona for the hit test
* #param {Number} X1 x coordinate of the start point of the line
* #param {Number} Y1 y coordinate of the start point of the line
* #param {Number} X2 x coordinate of the end point of the line
* #param {Number} Y2 y coordinate of the end point of the line
* #param {Number} px x coordinate of the point to test
* #param {Number} py y coordinate of the point to test
**/
graphiti.shape.basic.Line.hit= function( coronaWidth, X1, Y1, X2, Y2, px, py)
{
// Adjust vectors relative to X1,Y1
// X2,Y2 becomes relative vector from X1,Y1 to end of segment
X2 -= X1;
Y2 -= Y1;
// px,py becomes relative vector from X1,Y1 to test point
px -= X1;
py -= Y1;
var dotprod = px * X2 + py * Y2;
var projlenSq;
if (dotprod <= 0.0) {
// px,py is on the side of X1,Y1 away from X2,Y2
// distance to segment is length of px,py vector
// "length of its (clipped) projection" is now 0.0
projlenSq = 0.0;
} else {
// switch to backwards vectors relative to X2,Y2
// X2,Y2 are already the negative of X1,Y1=>X2,Y2
// to get px,py to be the negative of px,py=>X2,Y2
// the dot product of two negated vectors is the same
// as the dot product of the two normal vectors
px = X2 - px;
py = Y2 - py;
dotprod = px * X2 + py * Y2;
if (dotprod <= 0.0) {
// px,py is on the side of X2,Y2 away from X1,Y1
// distance to segment is length of (backwards) px,py vector
// "length of its (clipped) projection" is now 0.0
projlenSq = 0.0;
} else {
// px,py is between X1,Y1 and X2,Y2
// dotprod is the length of the px,py vector
// projected on the X2,Y2=>X1,Y1 vector times the
// length of the X2,Y2=>X1,Y1 vector
projlenSq = dotprod * dotprod / (X2 * X2 + Y2 * Y2);
}
}
// Distance to line is now the length of the relative point
// vector minus the length of its projection onto the line
// (which is zero if the projection falls outside the range
// of the line segment).
var lenSq = px * px + py * py - projlenSq;
if (lenSq < 0) {
lenSq = 0;
}
return Math.sqrt(lenSq)<coronaWidth;
};
C#
Adapted from #Grumdrig
public static double MinimumDistanceToLineSegment(this Point p,
Line line)
{
var v = line.StartPoint;
var w = line.EndPoint;
double lengthSquared = DistanceSquared(v, w);
if (lengthSquared == 0.0)
return Distance(p, v);
double t = Math.Max(0, Math.Min(1, DotProduct(p - v, w - v) / lengthSquared));
var projection = v + t * (w - v);
return Distance(p, projection);
}
public static double Distance(Point a, Point b)
{
return Math.Sqrt(DistanceSquared(a, b));
}
public static double DistanceSquared(Point a, Point b)
{
var d = a - b;
return DotProduct(d, d);
}
public static double DotProduct(Point a, Point b)
{
return (a.X * b.X) + (a.Y * b.Y);
}
Matlab code, with built-in "self test" if they call the function with no arguments:
function r = distPointToLineSegment( xy0, xy1, xyP )
% r = distPointToLineSegment( xy0, xy1, xyP )
if( nargin < 3 )
selfTest();
r=0;
else
vx = xy0(1)-xyP(1);
vy = xy0(2)-xyP(2);
ux = xy1(1)-xy0(1);
uy = xy1(2)-xy0(2);
lenSqr= (ux*ux+uy*uy);
detP= -vx*ux + -vy*uy;
if( detP < 0 )
r = norm(xy0-xyP,2);
elseif( detP > lenSqr )
r = norm(xy1-xyP,2);
else
r = abs(ux*vy-uy*vx)/sqrt(lenSqr);
end
end
function selfTest()
%#ok<*NASGU>
disp(['invalid args, distPointToLineSegment running (recursive) self-test...']);
ptA = [1;1]; ptB = [-1;-1];
ptC = [1/2;1/2]; % on the line
ptD = [-2;-1.5]; % too far from line segment
ptE = [1/2;0]; % should be same as perpendicular distance to line
ptF = [1.5;1.5]; % along the A-B but outside of the segment
distCtoAB = distPointToLineSegment(ptA,ptB,ptC)
distDtoAB = distPointToLineSegment(ptA,ptB,ptD)
distEtoAB = distPointToLineSegment(ptA,ptB,ptE)
distFtoAB = distPointToLineSegment(ptA,ptB,ptF)
figure(1); clf;
circle = #(x, y, r, c) rectangle('Position', [x-r, y-r, 2*r, 2*r], ...
'Curvature', [1 1], 'EdgeColor', c);
plot([ptA(1) ptB(1)],[ptA(2) ptB(2)],'r-x'); hold on;
plot(ptC(1),ptC(2),'b+'); circle(ptC(1),ptC(2), 0.5e-1, 'b');
plot(ptD(1),ptD(2),'g+'); circle(ptD(1),ptD(2), distDtoAB, 'g');
plot(ptE(1),ptE(2),'k+'); circle(ptE(1),ptE(2), distEtoAB, 'k');
plot(ptF(1),ptF(2),'m+'); circle(ptF(1),ptF(2), distFtoAB, 'm');
hold off;
axis([-3 3 -3 3]); axis equal;
end
end
coded in t-sql
the point is (#px, #py) and the line segment runs from (#ax, #ay) to (#bx, #by)
create function fn_sqr (#NumberToSquare decimal(18,10))
returns decimal(18,10)
as
begin
declare #Result decimal(18,10)
set #Result = #NumberToSquare * #NumberToSquare
return #Result
end
go
create function fn_Distance(#ax decimal (18,10) , #ay decimal (18,10), #bx decimal(18,10), #by decimal(18,10))
returns decimal(18,10)
as
begin
declare #Result decimal(18,10)
set #Result = (select dbo.fn_sqr(#ax - #bx) + dbo.fn_sqr(#ay - #by) )
return #Result
end
go
create function fn_DistanceToSegmentSquared(#px decimal(18,10), #py decimal(18,10), #ax decimal(18,10), #ay decimal(18,10), #bx decimal(18,10), #by decimal(18,10))
returns decimal(18,10)
as
begin
declare #l2 decimal(18,10)
set #l2 = (select dbo.fn_Distance(#ax, #ay, #bx, #by))
if #l2 = 0
return dbo.fn_Distance(#px, #py, #ax, #ay)
declare #t decimal(18,10)
set #t = ((#px - #ax) * (#bx - #ax) + (#py - #ay) * (#by - #ay)) / #l2
if (#t < 0)
return dbo.fn_Distance(#px, #py, #ax, #ay);
if (#t > 1)
return dbo.fn_Distance(#px, #py, #bx, #by);
return dbo.fn_Distance(#px, #py, #ax + #t * (#bx - #ax), #ay + #t * (#by - #ay))
end
go
create function fn_DistanceToSegment(#px decimal(18,10), #py decimal(18,10), #ax decimal(18,10), #ay decimal(18,10), #bx decimal(18,10), #by decimal(18,10))
returns decimal(18,10)
as
begin
return sqrt(dbo.fn_DistanceToSegmentSquared(#px, #py , #ax , #ay , #bx , #by ))
end
go
--example execution for distance from a point at (6,1) to line segment that runs from (4,2) to (2,1)
select dbo.fn_DistanceToSegment(6, 1, 4, 2, 2, 1)
--result = 2.2360679775
--example execution for distance from a point at (-3,-2) to line segment that runs from (0,-2) to (-2,1)
select dbo.fn_DistanceToSegment(-3, -2, 0, -2, -2, 1)
--result = 2.4961508830
--example execution for distance from a point at (0,-2) to line segment that runs from (0,-2) to (-2,1)
select dbo.fn_DistanceToSegment(0,-2, 0, -2, -2, 1)
--result = 0.0000000000
Looks like just about everyone else on StackOverflow has contributed an answer (23 answers so far), so here's my contribution for C#. This is mostly based on the answer by M. Katz, which in turn is based on the answer by Grumdrig.
public struct MyVector
{
private readonly double _x, _y;
// Constructor
public MyVector(double x, double y)
{
_x = x;
_y = y;
}
// Distance from this point to another point, squared
private double DistanceSquared(MyVector otherPoint)
{
double dx = otherPoint._x - this._x;
double dy = otherPoint._y - this._y;
return dx * dx + dy * dy;
}
// Find the distance from this point to a line segment (which is not the same as from this
// point to anywhere on an infinite line). Also returns the closest point.
public double DistanceToLineSegment(MyVector lineSegmentPoint1, MyVector lineSegmentPoint2,
out MyVector closestPoint)
{
return Math.Sqrt(DistanceToLineSegmentSquared(lineSegmentPoint1, lineSegmentPoint2,
out closestPoint));
}
// Same as above, but avoid using Sqrt(), saves a new nanoseconds in cases where you only want
// to compare several distances to find the smallest or largest, but don't need the distance
public double DistanceToLineSegmentSquared(MyVector lineSegmentPoint1,
MyVector lineSegmentPoint2, out MyVector closestPoint)
{
// Compute length of line segment (squared) and handle special case of coincident points
double segmentLengthSquared = lineSegmentPoint1.DistanceSquared(lineSegmentPoint2);
if (segmentLengthSquared < 1E-7f) // Arbitrary "close enough for government work" value
{
closestPoint = lineSegmentPoint1;
return this.DistanceSquared(closestPoint);
}
// Use the magic formula to compute the "projection" of this point on the infinite line
MyVector lineSegment = lineSegmentPoint2 - lineSegmentPoint1;
double t = (this - lineSegmentPoint1).DotProduct(lineSegment) / segmentLengthSquared;
// Handle the two cases where the projection is not on the line segment, and the case where
// the projection is on the segment
if (t <= 0)
closestPoint = lineSegmentPoint1;
else if (t >= 1)
closestPoint = lineSegmentPoint2;
else
closestPoint = lineSegmentPoint1 + (lineSegment * t);
return this.DistanceSquared(closestPoint);
}
public double DotProduct(MyVector otherVector)
{
return this._x * otherVector._x + this._y * otherVector._y;
}
public static MyVector operator +(MyVector leftVector, MyVector rightVector)
{
return new MyVector(leftVector._x + rightVector._x, leftVector._y + rightVector._y);
}
public static MyVector operator -(MyVector leftVector, MyVector rightVector)
{
return new MyVector(leftVector._x - rightVector._x, leftVector._y - rightVector._y);
}
public static MyVector operator *(MyVector aVector, double aScalar)
{
return new MyVector(aVector._x * aScalar, aVector._y * aScalar);
}
// Added using ReSharper due to CodeAnalysis nagging
public bool Equals(MyVector other)
{
return _x.Equals(other._x) && _y.Equals(other._y);
}
public override bool Equals(object obj)
{
if (ReferenceEquals(null, obj)) return false;
return obj is MyVector && Equals((MyVector) obj);
}
public override int GetHashCode()
{
unchecked
{
return (_x.GetHashCode()*397) ^ _y.GetHashCode();
}
}
public static bool operator ==(MyVector left, MyVector right)
{
return left.Equals(right);
}
public static bool operator !=(MyVector left, MyVector right)
{
return !left.Equals(right);
}
}
And here's a little test program.
public static class JustTesting
{
public static void Main()
{
Stopwatch stopwatch = new Stopwatch();
stopwatch.Start();
for (int i = 0; i < 10000000; i++)
{
TestIt(1, 0, 0, 0, 1, 1, 0.70710678118654757);
TestIt(5, 4, 0, 0, 20, 10, 1.3416407864998738);
TestIt(30, 15, 0, 0, 20, 10, 11.180339887498949);
TestIt(-30, 15, 0, 0, 20, 10, 33.541019662496844);
TestIt(5, 1, 0, 0, 10, 0, 1.0);
TestIt(1, 5, 0, 0, 0, 10, 1.0);
}
stopwatch.Stop();
TimeSpan timeSpan = stopwatch.Elapsed;
}
private static void TestIt(float aPointX, float aPointY,
float lineSegmentPoint1X, float lineSegmentPoint1Y,
float lineSegmentPoint2X, float lineSegmentPoint2Y,
double expectedAnswer)
{
// Katz
double d1 = DistanceFromPointToLineSegment(new MyVector(aPointX, aPointY),
new MyVector(lineSegmentPoint1X, lineSegmentPoint1Y),
new MyVector(lineSegmentPoint2X, lineSegmentPoint2Y));
Debug.Assert(d1 == expectedAnswer);
/*
// Katz using squared distance
double d2 = DistanceFromPointToLineSegmentSquared(new MyVector(aPointX, aPointY),
new MyVector(lineSegmentPoint1X, lineSegmentPoint1Y),
new MyVector(lineSegmentPoint2X, lineSegmentPoint2Y));
Debug.Assert(Math.Abs(d2 - expectedAnswer * expectedAnswer) < 1E-7f);
*/
/*
// Matti (optimized)
double d3 = FloatVector.DistanceToLineSegment(new PointF(aPointX, aPointY),
new PointF(lineSegmentPoint1X, lineSegmentPoint1Y),
new PointF(lineSegmentPoint2X, lineSegmentPoint2Y));
Debug.Assert(Math.Abs(d3 - expectedAnswer) < 1E-7f);
*/
}
private static double DistanceFromPointToLineSegment(MyVector aPoint,
MyVector lineSegmentPoint1, MyVector lineSegmentPoint2)
{
MyVector closestPoint; // Not used
return aPoint.DistanceToLineSegment(lineSegmentPoint1, lineSegmentPoint2,
out closestPoint);
}
private static double DistanceFromPointToLineSegmentSquared(MyVector aPoint,
MyVector lineSegmentPoint1, MyVector lineSegmentPoint2)
{
MyVector closestPoint; // Not used
return aPoint.DistanceToLineSegmentSquared(lineSegmentPoint1, lineSegmentPoint2,
out closestPoint);
}
}
As you can see, I tried to measure the difference between using the version that avoids the Sqrt() method and the normal version. My tests indicate you can maybe save about 2.5%, but I'm not even sure of that - the variations within the various test runs were of the same order of magnitude. I also tried measuring the version posted by Matti (plus an obvious optimization), and that version seems to be about 4% slower than the version based on Katz/Grumdrig code.
Edit: Incidentally, I've also tried measuring a method that finds the distance to an infinite line (not a line segment) using a cross product (and a Sqrt()), and it's about 32% faster.
Here is devnullicus's C++ version converted to C#. For my implementation I needed to know the point of intersection and found his solution to work well.
public static bool PointSegmentDistanceSquared(PointF point, PointF lineStart, PointF lineEnd, out double distance, out PointF intersectPoint)
{
const double kMinSegmentLenSquared = 0.00000001; // adjust to suit. If you use float, you'll probably want something like 0.000001f
const double kEpsilon = 1.0E-14; // adjust to suit. If you use floats, you'll probably want something like 1E-7f
double dX = lineEnd.X - lineStart.X;
double dY = lineEnd.Y - lineStart.Y;
double dp1X = point.X - lineStart.X;
double dp1Y = point.Y - lineStart.Y;
double segLenSquared = (dX * dX) + (dY * dY);
double t = 0.0;
if (segLenSquared >= -kMinSegmentLenSquared && segLenSquared <= kMinSegmentLenSquared)
{
// segment is a point.
intersectPoint = lineStart;
t = 0.0;
distance = ((dp1X * dp1X) + (dp1Y * dp1Y));
}
else
{
// Project a line from p to the segment [p1,p2]. By considering the line
// extending the segment, parameterized as p1 + (t * (p2 - p1)),
// we find projection of point p onto the line.
// It falls where t = [(p - p1) . (p2 - p1)] / |p2 - p1|^2
t = ((dp1X * dX) + (dp1Y * dY)) / segLenSquared;
if (t < kEpsilon)
{
// intersects at or to the "left" of first segment vertex (lineStart.X, lineStart.Y). If t is approximately 0.0, then
// intersection is at p1. If t is less than that, then there is no intersection (i.e. p is not within
// the 'bounds' of the segment)
if (t > -kEpsilon)
{
// intersects at 1st segment vertex
t = 0.0;
}
// set our 'intersection' point to p1.
intersectPoint = lineStart;
// Note: If you wanted the ACTUAL intersection point of where the projected lines would intersect if
// we were doing PointLineDistanceSquared, then intersectPoint.X would be (lineStart.X + (t * dx)) and intersectPoint.Y would be (lineStart.Y + (t * dy)).
}
else if (t > (1.0 - kEpsilon))
{
// intersects at or to the "right" of second segment vertex (lineEnd.X, lineEnd.Y). If t is approximately 1.0, then
// intersection is at p2. If t is greater than that, then there is no intersection (i.e. p is not within
// the 'bounds' of the segment)
if (t < (1.0 + kEpsilon))
{
// intersects at 2nd segment vertex
t = 1.0;
}
// set our 'intersection' point to p2.
intersectPoint = lineEnd;
// Note: If you wanted the ACTUAL intersection point of where the projected lines would intersect if
// we were doing PointLineDistanceSquared, then intersectPoint.X would be (lineStart.X + (t * dx)) and intersectPoint.Y would be (lineStart.Y + (t * dy)).
}
else
{
// The projection of the point to the point on the segment that is perpendicular succeeded and the point
// is 'within' the bounds of the segment. Set the intersection point as that projected point.
intersectPoint = new PointF((float)(lineStart.X + (t * dX)), (float)(lineStart.Y + (t * dY)));
}
// return the squared distance from p to the intersection point. Note that we return the squared distance
// as an optimization because many times you just need to compare relative distances and the squared values
// works fine for that. If you want the ACTUAL distance, just take the square root of this value.
double dpqX = point.X - intersectPoint.X;
double dpqY = point.Y - intersectPoint.Y;
distance = ((dpqX * dpqX) + (dpqY * dpqY));
}
return true;
}
A 2D and 3D solution
Consider a change of basis such that the line segment becomes (0, 0, 0)-(d, 0, 0) and the point (u, v, 0). The shortest distance occurs in that plane and is given by
u ≤ 0 -> d(A, C)
0 ≤ u ≤ d -> |v|
d ≤ u -> d(B, C)
(the distance to one of the endpoints or to the supporting line, depending on the projection to the line. The iso-distance locus is made of two half-circles and two line segments.)
In the above expression, d is the length of the segment AB, and u, v are respectivey the scalar product and (modulus of the) cross product of AB/d (unit vector in the direction of AB) and AC. Hence vectorially,
AB.AC ≤ 0 -> |AC|
0 ≤ AB.AC ≤ AB² -> |ABxAC|/|AB|
AB² ≤ AB.AC -> |BC|
see the Matlab GEOMETRY toolbox in the following website:
http://people.sc.fsu.edu/~jburkardt/m_src/geometry/geometry.html
ctrl+f and type "segment" to find line segment related functions. the functions "segment_point_dist_2d.m" and "segment_point_dist_3d.m" are what you need.
The GEOMETRY codes are available in a C version and a C++ version and a FORTRAN77 version and a FORTRAN90 version and a MATLAB version.
AutoHotkeys version based on Joshua's Javascript:
plDist(x, y, x1, y1, x2, y2) {
A:= x - x1
B:= y - y1
C:= x2 - x1
D:= y2 - y1
dot:= A*C + B*D
sqLen:= C*C + D*D
param:= dot / sqLen
if (param < 0 || ((x1 = x2) && (y1 = y2))) {
xx:= x1
yy:= y1
} else if (param > 1) {
xx:= x2
yy:= y2
} else {
xx:= x1 + param*C
yy:= y1 + param*D
}
dx:= x - xx
dy:= y - yy
return sqrt(dx*dx + dy*dy)
}
the accepted answer does not work
(e.g. distance between 0,0 and (-10,2,10,2) should be 2).
here's code that works:
def dist2line2(x,y,line):
x1,y1,x2,y2=line
vx = x1 - x
vy = y1 - y
ux = x2-x1
uy = y2-y1
length = ux * ux + uy * uy
det = (-vx * ux) + (-vy * uy) #//if this is < 0 or > length then its outside the line segment
if det < 0:
return (x1 - x)**2 + (y1 - y)**2
if det > length:
return (x2 - x)**2 + (y2 - y)**2
det = ux * vy - uy * vx
return det**2 / length
def dist2line(x,y,line): return math.sqrt(dist2line2(x,y,line))
Didn't see a Java implementation here, so I translated the Javascript function from the accepted answer to Java code:
static double sqr(double x) {
return x * x;
}
static double dist2(DoublePoint v, DoublePoint w) {
return sqr(v.x - w.x) + sqr(v.y - w.y);
}
static double distToSegmentSquared(DoublePoint p, DoublePoint v, DoublePoint w) {
double l2 = dist2(v, w);
if (l2 == 0) return dist2(p, v);
double t = ((p.x - v.x) * (w.x - v.x) + (p.y - v.y) * (w.y - v.y)) / l2;
if (t < 0) return dist2(p, v);
if (t > 1) return dist2(p, w);
return dist2(p, new DoublePoint(
v.x + t * (w.x - v.x),
v.y + t * (w.y - v.y)
));
}
static double distToSegment(DoublePoint p, DoublePoint v, DoublePoint w) {
return Math.sqrt(distToSegmentSquared(p, v, w));
}
static class DoublePoint {
public double x;
public double y;
public DoublePoint(double x, double y) {
this.x = x;
this.y = y;
}
}
WPF version:
public class LineSegment
{
private readonly Vector _offset;
private readonly Vector _vector;
public LineSegment(Point start, Point end)
{
_offset = (Vector)start;
_vector = (Vector)(end - _offset);
}
public double DistanceTo(Point pt)
{
var v = (Vector)pt - _offset;
// first, find a projection point on the segment in parametric form (0..1)
var p = (v * _vector) / _vector.LengthSquared;
// and limit it so it lays inside the segment
p = Math.Min(Math.Max(p, 0), 1);
// now, find the distance from that point to our point
return (_vector * p - v).Length;
}
}
I've made an interactive Desmos graph to demonstrate how to achieve this:
https://www.desmos.com/calculator/kswrm8ddum
The red point is A, the green point is B, and the point C is blue.
You can drag the points in the graph to see the values change.
On the left, the value 's' is the parameter of the line segment (i.e. s = 0 means the point A, and s = 1 means the point B).
The value 'd' is the distance from the third point to the line through A and B.
EDIT:
Fun little insight: the coordinate (s, d) is the coordinate of the third point C in the coordinate system where AB is the unit x-axis, and the unit y-axis is perpendicular to AB.
Python Numpy implementation for 2D coordinate array:
import numpy as np
def dist2d(p1, p2, coords):
''' Distance from points to a finite line btwn p1 -> p2 '''
assert coords.ndim == 2 and coords.shape[1] == 2, 'coords is not 2 dim'
dp = p2 - p1
st = dp[0]**2 + dp[1]**2
u = ((coords[:, 0] - p1[0]) * dp[0] + (coords[:, 1] - p1[1]) * dp[1]) / st
u[u > 1.] = 1.
u[u < 0.] = 0.
dx = (p1[0] + u * dp[0]) - coords[:, 0]
dy = (p1[1] + u * dp[1]) - coords[:, 1]
return np.sqrt(dx**2 + dy**2)
# Usage:
p1 = np.array([0., 0.])
p2 = np.array([0., 10.])
# List of coordinates
coords = np.array(
[[0., 0.],
[5., 5.],
[10., 10.],
[20., 20.]
])
d = dist2d(p1, p2, coords)
# Single coordinate
coord = np.array([25., 25.])
d = dist2d(p1, p2, coord[np.newaxis, :])