dimanche 1 janvier 2017

Find distance between point and polygon

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