diff options
Diffstat (limited to 'src/lib/geo/geo.c')
| -rw-r--r-- | src/lib/geo/geo.c | 22 |
1 files changed, 22 insertions, 0 deletions
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 43105fdba..8aeca5be7 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -465,4 +465,26 @@ __EXPORT float _wrap_360(float bearing) return bearing; } +__EXPORT bool inside_geofence(const struct vehicle_global_position_s *vehicle, const struct fence_s *fence) +{ + + /* Adaptation of algorithm originally presented as + * PNPOLY - Point Inclusion in Polygon Test + * W. Randolph Franklin (WRF) */ + + unsigned int i, j, vertices = fence->count; + bool c = false; + double lat = vehicle->lat / 1e7; + double lon = vehicle->lon / 1e7; + + // skip vertex 0 (return point) + for (i = 0, j = vertices - 1; i < vertices; j = i++) + if (((fence->vertices[i].lon) >= lon != (fence->vertices[j].lon >= lon)) && + (lat <= (fence->vertices[j].lat - fence->vertices[i].lat) * (lon - fence->vertices[i].lon) / + (fence->vertices[j].lon - fence->vertices[i].lon) + fence->vertices[i].lat)) + c = !c; + return c; +} + + |