diff --git a/server/inc/util.h b/server/inc/util.h index 7e377d37f..92401a42b 100755 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -728,6 +728,8 @@ int UtilFindCurrentTrajectoryPositionNew(ObjectPosition *OP, int StartIndex, dou int UtilSetSyncPoint(ObjectPosition *OP, double x, double y, double z, double time); float UtilCalculateTimeToSync(ObjectPosition *OP); +char UtilIsPointInPolygon(double pointX, double pointY, double * polyX, double * polyY, int nPtsInPolygon); + int UtilCountFileRows(FILE *fd); int UtilReadLineCntSpecChars(FILE *fd, char *Buffer); int UtilReadLine(FILE *fd, char *Buffer); diff --git a/server/src/util.c b/server/src/util.c index 1e91deb05..42ca5a2a6 100755 --- a/server/src/util.c +++ b/server/src/util.c @@ -23,6 +23,7 @@ #include #include #include +#include #include "util.h" #include "logging.h" @@ -49,6 +50,12 @@ static mqd_t tMQRecv; static mqd_t ptMQSend[MQ_NBR_QUEUES]; static char pcMessageQueueName[1024]; +/*------------------------------------------------------------ +-- Private functions +------------------------------------------------------------*/ + +static char rayFromPointIntersectsLine(double pointX, double pointY, double polyPointAX, double polyPointAY, double polyPointBX, double polyPointBY); + /*---------------------------------------------s--------------- -- Public functions ------------------------------------------------------------*/ @@ -887,6 +894,125 @@ float UtilCalculateTimeToSync(ObjectPosition *OP) + +/*! + * \brief UtilIsPointInPolygon Checks if a point lies within a polygon specified by a number of vertices using + * the ray casting algorithm (see http://rosettacode.org/wiki/Ray-casting_algorithm). + * \param pointX X coordinate of the point to check + * \param pointY Y coordinate of the point to check + * \param verticesX X coordinates of the vertex points defining the polygon to check + * \param verticesY Y coordinates of the vertex points defining the polygon to check + * \param nPtsInPolygon length of the two vertex arrays + * \return true if the point lies within the polygon, false otherwise + */ +char UtilIsPointInPolygon(double pointX, double pointY, double * verticesX, double * verticesY, int nPtsInPolygon) +{ + int nIntersects = 0; + + // Count the number of intersections with the polygon + for (int i = 0; i < nPtsInPolygon-1; ++i) + { + if (rayFromPointIntersectsLine(pointX, pointY, verticesX[i], verticesY[i], verticesX[i+1], verticesY[i+1])) + { + nIntersects++; + } + } + + // If the first and last points are different, the polygon segment between them must also be included + if (fabs(verticesX[0] - verticesX[nPtsInPolygon-1]) > (double)(2*FLT_EPSILON) || fabs(verticesY[0] - verticesY[nPtsInPolygon-1]) > (double)(2*FLT_EPSILON) ) + { + if (rayFromPointIntersectsLine(pointX, pointY, + verticesX[0], verticesY[0], verticesX[nPtsInPolygon-1], verticesY[nPtsInPolygon-1])) + { + nIntersects++; + } + } + + // If the number of ray intersections with the polygon is even, the point must be outside the polygon + if (nIntersects % 2 == 0) + return 0; + else + return 1; +} + +/*! + * \brief rayFromPointIntersectsLine Checks whether a ray in positive x direction, parallel to the x axis, + * from a point intersects a line AB between two points A and B + * \param pointX X coordinate of the point from which ray originates + * \param pointY Y coordinate of the point from which ray originates + * \param linePointAX X coordinate of the point A + * \param linePointAY Y coordinate of the point A + * \param linePointBX X coordinate of the point B + * \param linePointBY Y coordinate of the point B + * \return true if ray intersects line AB, false otherwise + */ +static char rayFromPointIntersectsLine(double pointX, double pointY, + double linePointAX, double linePointAY, double linePointBX, double linePointBY) +{ + double tmp, maxVal, minVal, slopeAB, slopeAP; + + // Need 2x eps because machine epsilon is too small + // Need to use float epsilon in case the double points are just promoted floats + const double algorithmEpsilon = (double)(2*FLT_EPSILON); + + // The algorithm assumes the Y coordinate of A is smaller than that of B + // -> if this is not the case, swap the points + if (linePointBY < linePointAY) + { + tmp = linePointAX; + linePointAX = linePointBX; + linePointBX = tmp; + + tmp = linePointAY; + linePointAY = linePointBY; + linePointBY = tmp; + } + + // If the ray casting would cause intersection with a vertex the result is undefined, + // so we perform a heuristic to avoid this situation: + if (pointY == linePointAY || pointY == linePointBY) + { + pointY += algorithmEpsilon; + } + + // A ray in X axis direction will not intersect if Y coordinates are outside of the range + // spanned by the line AB + if (pointY < linePointAY || pointY > linePointBY) + return 0; + + // A ray in X axis direction will not intersect if it starts at a larger X coordinate than + // any in the range of the line AB + maxVal = linePointAX > linePointBX ? linePointAX : linePointBX; + if (pointX >= maxVal) + return 0; + + // A ray in X axis direction will intersect if it starts at a smaller X coordinate than + // any in the range of the line AB + minVal = linePointAX < linePointBX ? linePointAX : linePointBX; + if (pointX < minVal) + return 1; + + // Having filtered away all the situations where the point lies outside the rectangle + // defined by the two points A and B, it is necessary to do some more costly processing + + // Compare the slopes of the lines AB and AP - if the slope of AB is larger than that of AP + // then a ray in X axis direction will not intersect AB + if (linePointAX != linePointBX) + slopeAB = (linePointBY - linePointAY) / (linePointBX - linePointAX); + else + slopeAB = (double)INFINITY; + if (linePointAX != pointX) + slopeAP = (pointY - linePointAY) / (pointX - linePointAX); + else + slopeAP = (double)INFINITY; + + if (slopeAP >= slopeAB) + return 1; + else + return 0; +} + + int UtilSortSpaceTimeAscending(ObjectPosition *OP) { int i, j, index;