Shortest distance between a line segement and a point in three dimensions (3D)

Question

I've been building a program in MATLAB off of an excellent thread describing how to find the shortest distance between a point and a line segment in 2D (Shortest distance between a point and a line segment). I need a function that does essentially the same thing as this previously answered question but in 3D instead of 2D and in MATLAB.

None of the top comments for answers to this previous post are in MATLAB so I'm having some trouble understanding what's going on behind the scenes in this code. Maybe some of you smarter or more skilled out there can help me convert this to a 3D MATLAB code?

The line segment would be defined as two points S1 (x1,y1,z1) and S2 (x2,y2,z2) and the point is simply a single coordinate Pnt (x3,y3,z3).

EDIT: There seems to be a little confusion here. I really do mean line segments not an infinite line. I've attached the code that I'm working with. I'd like to add that this code which I've modified was originally written as part of a comment in the above linked thread and the original author Peter Karasev deserves credit for it. As is, the code works in 2D, and I've commented in 3 lines which are a start to make it 3D (vz, uz, and at lenSqr). My specific question is that I really don't understand what's going on mathematically with detP and how I can make detP and the subsequent if statements work in 3D.

The inputs are as defined above in the original question text.

function r = PointToLineSegment3D( S1, S2, Pnt )
% r = PointToLineSegment3D( S1, S2, Pnt )

vx = S1(1)-Pnt(1);
vy = S1(2)-Pnt(2);
% vz = S1(3)-Pnt(3);

ux = S2(1)-S1(1);
uy = S2(2)-S1(2);
% uz = S2(3)-S1(3);

lenSqr= (ux*ux+uy*uy); % +uz*uz
detP= -vx*ux + -vy*uy;

if( detP < 0 )
    r = norm(S1-Pnt,2);

elseif( detP > lenSqr )
    r = norm(S2-Pnt,2);

else
    r = abs(ux*vy-uy*vx)/sqrt(lenSqr);
end
end

What have you tried so far in 3-D? This site is not a code writing service. Please show us your code and describe what specific problem you have. Your problem (assuming you mean a line that is defined by two points) is [fully described on MathWorld here](http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html), including very simple formulae. — horchler, Apr 11 '17 at 20:52
http://stackoverflow.com/questions/43207514/ works for 2d and 3d — MBo, Apr 12 '17 at 02:03
Thank you @horchler for your reply, I've added the information you requested. — AndrewTLombardo, Apr 12 '17 at 16:05

flawr · Answer 1 · 2017-04-11T21:43:46.613

Just define A and B and P as column vectors. Then any point X on the line AB has the form

X = A+t*(B-A)

for some value of t.

And certainly the line XP must be perpendicular to the line AB, that means the corresponding scalar product must be zero:

0 == (A+t*(B-A) - P)' * (B-A) == (A-P)'*(B-A)+t*norm(B-A)^2

This implies

t = (A-P)'*(B-A) / norm(B-A)^2

Then it is just a matter of calculating the distance XP which is

d = norm(X-P)

so

d = norm(A+t*(B-A)-P)

So you just need to use the third and the fifth line of code that I've posted here and if I made no mistake you're good to go.

score 1 · Answer 2 · answered Feb 20 '19 at 12:23

C++ im not sure that it is right answer but some times it work) test data: Pnt=[1 1 1]; S1=[0 0 0]; S2=[0 3 3]; ans=1.0

#include<iostream>
#include<math.h>
#include<stdio.h>
#include<vector>
#include<iterator>
#include <iomanip>

using namespace std;
int main()
{

double vx,vy,vz,ux,uy,uz,r=0,lenSqr,detP, c,tmp;
int i;
vector<double>copy;
vector<double>Pnt;
vector<double>S1;
vector<double>S2;

for(i=0; i<9; i++)
{
    cin>>c;
    copy.push_back(c);
}
for(i=0; i<3; i++)
{
    Pnt.insert(Pnt.begin(), copy[i]);
//  cout<<copy[i]<<endl;        
}
    copy.erase(copy.begin(),copy.begin()+3);
    copy.shrink_to_fit();
for(i=0; i<3; i++)
{
    S1.insert(S1.begin(), copy[i]);
}
copy.erase(copy.begin(),copy.begin()+3);
copy.shrink_to_fit();
for(i=0; i<3; i++)
{
    S2.insert(S2.begin(), copy[i]);
    copy.erase(copy.begin());
}
copy.shrink_to_fit();



/*
vector<float>Pnt(3,1.0);
//for(i=0; i<3; i++)
//cout<<Pnt[i];

vector<float>S1(3,0.0);
//for(i=0; i<3; i++)
//cout<<S1[i];

vector<float>S2;
S2.insert(S2.begin(), 3.0);
S2.insert(S2.begin(), 3.0);
S2.insert(S2.begin(), 0.0);

//for(int i=0; i<3; i++)
//cout<<S2[i];
//cout<<endl;   
*/

vx = S1[0]-Pnt[0];
vy = S1[1]-Pnt[1];
vz = S1[2]-Pnt[2];
//cout<<"V: "<<vx<<vy<<vz<<endl;
ux = S2[0]-S1[0];
uy = S2[1]-S1[1];
uz = S2[2]-S1[2];
//cout<<"U: "<<ux<<uy<<uz<<endl;


lenSqr= (ux*ux+uy*uy+uz*uz);
//cout<<"lenSqr "<<lenSqr<<endl;
detP= (-vx*ux ) + (-vy*uy) + (-vz*uz);
//cout<<"detP "<<detP<<endl;



if( detP < 0 )
{
//  r = norm(S1-Pnt,2)
    for(i=0; i<3; i++)
    {       
        tmp=pow((S1[i]-Pnt[i]),2);
        r += tmp;
//      cout<<"r: "<<r;
    }
    r = sqrt(r);
    cout<<fixed<<r;
}

else if( detP > lenSqr )
{
//  r = norm(S2-Pnt,2);
    for(i=0; i<3; i++)
    {       
        tmp=pow((S2[i]-Pnt[i]),2);
        r += tmp;
//      cout<<"r: "<<r;
    }
    r = sqrt(r);
    cout<<fixed<<r;
}
//if(detP <= lenSqr || detP>=0)
else
{
//  r =norm( abs(cross((S2-S1),(S1-Pnt)))/sqrt(lenSqr));
    float i1,j1,k1;

i1 = uz*vy-uy*vz;
j1 = ux*vz-uz*vx;
k1 = uy*vx-ux*vy;
//cout<<"I J k: "<<i1<<j1<<k1<<endl;
r=sqrt(pow(i1,2)+pow(j1,2)+pow(k1,2))/sqrt(lenSqr);
cout<<fixed<<r;
}

return 0;
}

score 0 · Accepted Answer · answered Apr 14 '17 at 15:34

For future users who find this question, this is the code that I've made to work in 3D in MATLAB. This does not work for an infinite line only a line segment.

function r = PointToLineSegment3D( S1, S2, Pnt )
% r = PointToLineSegment3D( S1, S2, Pnt )

vx = S1(1)-Pnt(1);
vy = S1(2)-Pnt(2);
vz = S1(3)-Pnt(3);

ux = S2(1)-S1(1);
uy = S2(2)-S1(2);
uz = S2(3)-S1(3);

lenSqr= (ux*ux+uy*uy+uz*uz)


detP= -vx*ux + -vy*uy + -vz*uz;

if( detP < 0 )
    r = norm(S1-Pnt,2);

elseif( detP > lenSqr )
    r = norm(S2-Pnt,2);

else
    r =norm( abs(cross((S2-S1),(S1-Pnt)))/sqrt(lenSqr));
end
end

Shortest distance between a line segement and a point in three dimensions (3D)

3 Answers3