From 928e470ffe5946f7051c6f898bcb297406a1d8de Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 8 May 2019 12:21:29 +0200 Subject: [PATCH 1/3] Added util function for checking if a point is inside a polygon --- server/inc/util.h | 2 + server/src/util.c | 132 ++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 134 insertions(+) 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..ceacc9e73 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,13 @@ 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); +static char roughlyEqualsTo(double num1, double num2, double eps); + /*---------------------------------------------s--------------- -- Public functions ------------------------------------------------------------*/ @@ -887,6 +895,130 @@ 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. + * \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 (verticesX[0] != verticesX[nPtsInPolygon-1] || verticesY[0] != verticesY[nPtsInPolygon-1]) + { + 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 ray in X axis direction from 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; +} + +static char roughlyEqualsTo(double num1, double num2, double eps) +{ + return 0; +} + + int UtilSortSpaceTimeAscending(ObjectPosition *OP) { int i, j, index; From b272b5c77171a63c70cabada896f7aaeb18dd5d0 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 8 May 2019 12:23:30 +0200 Subject: [PATCH 2/3] Removed unused function --- server/src/util.c | 6 ------ 1 file changed, 6 deletions(-) diff --git a/server/src/util.c b/server/src/util.c index ceacc9e73..ef13f4f85 100755 --- a/server/src/util.c +++ b/server/src/util.c @@ -55,7 +55,6 @@ static char pcMessageQueueName[1024]; ------------------------------------------------------------*/ static char rayFromPointIntersectsLine(double pointX, double pointY, double polyPointAX, double polyPointAY, double polyPointBX, double polyPointBY); -static char roughlyEqualsTo(double num1, double num2, double eps); /*---------------------------------------------s--------------- -- Public functions @@ -1013,11 +1012,6 @@ static char rayFromPointIntersectsLine(double pointX, double pointY, return 0; } -static char roughlyEqualsTo(double num1, double num2, double eps) -{ - return 0; -} - int UtilSortSpaceTimeAscending(ObjectPosition *OP) { From 491b74d919b2712f33ce37828d1d5a9c742e6122 Mon Sep 17 00:00:00 2001 From: LukasWikander Date: Wed, 8 May 2019 15:51:34 +0200 Subject: [PATCH 3/3] Update util.c Addressed comments --- server/src/util.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/server/src/util.c b/server/src/util.c index ef13f4f85..42ca5a2a6 100755 --- a/server/src/util.c +++ b/server/src/util.c @@ -897,7 +897,7 @@ 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. + * 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 @@ -919,7 +919,7 @@ char UtilIsPointInPolygon(double pointX, double pointY, double * verticesX, doub } // If the first and last points are different, the polygon segment between them must also be included - if (verticesX[0] != verticesX[nPtsInPolygon-1] || verticesY[0] != verticesY[nPtsInPolygon-1]) + 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])) @@ -936,8 +936,8 @@ char UtilIsPointInPolygon(double pointX, double pointY, double * verticesX, doub } /*! - * \brief rayFromPointIntersectsLine checks whether ray in X axis direction from point intersects - * a line AB between two points A and B + * \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