I wrote the following code to find distance between point and polygon (expressed in latitude, longitude). However, to my surprise I am getting the wrong distance -- the actual distance should be 50 km. Can someone please help me figure out as to where am I going wrong.
#include <cstdlib>
#include <iostream>
using namespace std;
//---------------------------------------------------------------------------
double dot( double* V1, double* V2 )
{
return V1[0]*V2[0] + V1[1]*V2[1];
}
//---------------------------------------------------------------------------
double distance2(double* A, double* B)
//Compute the square of the distance from point A to point B
{
double d1 = A[0] - B[0];
double d2 = A[1] - B[1];
return d1*d1+d2*d2;
}
//---------------------------------------------------------------------------
double pointLineDist2(double* point, double* segment)
//Compute the square of the minimum distance between a point and a line segment
//point: an array of 2 doubles [norm_long,norm_lat]
//segment: an array of 4 doubles [norm_long1,norm_lat1,norm_long2,norm_lat2], i.e., the points that define the line segment
{
double v[2],w[2];
//Vector v = S.P1 - S.P0;
v[0] = segment[2] - segment[0];
v[1] = segment[3] - segment[1];
//Vector w = P - S.P0;
w[0] = point[0] - segment[0];
w[1] = point[1] - segment[1];
double c1 = dot(w,v);
if ( c1 <= 0 )
return distance2(point,segment); //distance2(P, S.P0);
double c2 = dot(v,v);
if ( c2 <= c1 )
return distance2(point,&segment[2]); //distance2(P, S.P1);
double b = c1 / c2;
double pb[2];
//Point Pb = S.P0 + b * v;
pb[0] = segment[0] + b*v[0];
pb[1] = segment[1] + b*v[1];
return distance2(point, pb); //return distance2(P, Pb);
}
//---------------------------------------------------------------------------
double pointPolygonDist2(double* point, double* polygon, unsigned size)
//Compute the square of the minimum distance between a line segment and a polygon
{
double d, mD = 100000000; //the minimum distance
size -= 2;
//for each line of the given polygon
for(unsigned i=0;i<size;i+=2){
//distance between the point and the line
d = pointLineDist2(point,&polygon[i]);
cout<<"d="<<d<<"\n";
if(!d) return 0;
if(d<mD) mD = d;
}
return mD;
}
int main(int argc, char** argv) {
double polygon[28*2];
double point[2];
point[0]=2.336691; point[1]=-126.846931; //point[0] is latitude, point[1] is longitude of a point
polygon[0]=2.469674; polygon[1]=-126.401421; //polygon[0] is latitude, polygon[1] is longitude of a vertex of a polygon
polygon[2]=2.468683; polygon[3]=-126.401096; //polygon[2] is latitude, polygon[3] is longitude of a vertex of a polygon. similarly for the other vertexes of the polygon
polygon[4]=2.468221; polygon[2*2+1]=-126.401230;
polygon[3*2]=2.466333; polygon[3*2+1]=-126.401485;
polygon[4*2]=2.465378; polygon[4*2+1]=-126.401701;
polygon[5*2]=2.463752; polygon[5*2+1]=-126.401963;
polygon[6*2]=2.463043; polygon[6*2+1]=-126.402638;
polygon[7*2]=2.463816; polygon[7*2+1]=-126.402829 ;
polygon[8*2]=2.464138; polygon[8*2+1]=-126.403007 ;
polygon[9*2]=2.464224; polygon[9*2+1]=-126.403237 ;
polygon[10*2]=2.463494; polygon[10*2+1]=-126.403440 ;
polygon[11*2]=2.463280; polygon[11*2+1]=-126.403682 ;
polygon[12*2]=2.462743; polygon[12*2+1]=-126.403542 ;
polygon[13*2]=2.462475; polygon[13*2+1]=-126.403446 ;
polygon[14*2]=2.462250; polygon[14*2+1]=-126.404465 ;
polygon[15*2]=2.462368; polygon[15*2+1]=-126.404898 ;
polygon[16*2]=2.463172; polygon[16*2+1]=-126.405707 ;
polygon[17*2]=2.463848; polygon[17*2+1]=-126.406070 ;
polygon[18*2]=2.464449; polygon[18*2+1]=-126.405496 ;
polygon[19*2]=2.464771; polygon[19*2+1]=-126.405248 ;
polygon[20*2]=2.465822; polygon[20*2+1]=-126.405274 ;
polygon[21*2]=2.466970; polygon[21*2+1]=-126.405095 ;
polygon[22*2]=2.468730; polygon[22*2+1]=-126.404261 ;
polygon[23*2]=2.469642; polygon[23*2+1]=-126.403344 ;
polygon[24*2]=2.470822; polygon[24*2+1]=-126.402790 ;
polygon[25*2]=2.471101; polygon[25*2+1]=-126.402555 ;
polygon[26*2]=2.470168; polygon[26*2+1]=-126.401714 ;
polygon[27*2]=2.469674; polygon[27*2+1]=-126.401421 ;
cout<<"dist="<<pointPolygonDist2(point, polygon, 28);
return 0;
}
Aucun commentaire:
Enregistrer un commentaire