From cd9fb3c47ad305b9c595dcd712c8b437f60c7d5f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Dec 2011 14:34:58 +1100 Subject: [PATCH] AP_Math: added a Polygon_outside() function this tests if a point is outside of a polygon. This will be used as part of our geo-fencing support, and also for a new 'fenced mode' --- libraries/AP_Math/AP_Math.h | 1 + libraries/AP_Math/examples/polygon/Makefile | 1 + .../AP_Math/examples/polygon/polygon.pde | 89 +++++++++++++++++++ libraries/AP_Math/polygon.cpp | 78 ++++++++++++++++ libraries/AP_Math/polygon.h | 21 +++++ 5 files changed, 190 insertions(+) create mode 100644 libraries/AP_Math/examples/polygon/Makefile create mode 100644 libraries/AP_Math/examples/polygon/polygon.pde create mode 100644 libraries/AP_Math/polygon.cpp create mode 100644 libraries/AP_Math/polygon.h diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index b94ed1c696..97d98bc28d 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -5,3 +5,4 @@ #include "vector2.h" #include "vector3.h" #include "matrix3.h" +#include "polygon.h" diff --git a/libraries/AP_Math/examples/polygon/Makefile b/libraries/AP_Math/examples/polygon/Makefile new file mode 100644 index 0000000000..d1f40fd90f --- /dev/null +++ b/libraries/AP_Math/examples/polygon/Makefile @@ -0,0 +1 @@ +include ../../../AP_Common/Arduino.mk diff --git a/libraries/AP_Math/examples/polygon/polygon.pde b/libraries/AP_Math/examples/polygon/polygon.pde new file mode 100644 index 0000000000..ff3c6432e8 --- /dev/null +++ b/libraries/AP_Math/examples/polygon/polygon.pde @@ -0,0 +1,89 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +// +// Unit tests for the AP_Meta_class and AP_Var classes. +// + +#include +#include +#include + +FastSerialPort(Serial, 0); + +/* + this is the boundary of the 2010 outback challenge + Note that the last point must be the same as the first for the + Polygon_outside() algorithm + */ +static const Vector2f OBC_boundary[] = { + Vector2f(-26.569564000, 151.837373000), + Vector2f(-26.569956000, 151.839405000), + Vector2f(-26.576823000, 151.841142000), + Vector2f(-26.577308000, 151.840344000), + Vector2f(-26.581511000, 151.841950000), + Vector2f(-26.578486000, 151.847469000), + Vector2f(-26.599489000, 151.852886000), + Vector2f(-26.609211000, 151.874742000), + Vector2f(-26.645478000, 151.882053000), + Vector2f(-26.643572000, 151.830350000), + Vector2f(-26.587599000, 151.834405000), + Vector2f(-26.569564000, 151.837373000) +}; + +static const struct { + Vector2f point; + bool outside; +} test_points[] = { + { Vector2f(-26.639887, 151.822281), true }, + { Vector2f(-26.641870, 151.870926), false }, + { Vector2f(-35.0, 149.0), true }, + { Vector2f(0, 0), true }, + { Vector2f(-26.576815, 151.840825), false }, + { Vector2f(-26.577406, 151.840586), true }, +}; + +#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) + +/* + polygon tests + */ +void setup(void) +{ + unsigned i, count; + bool all_passed = true; + uint32_t start_time; + + Serial.begin(115200); + Serial.println("polygon unit tests\n"); + + for (i=0; i. + */ + +#include "AP_Math.h" + +/* + NOTE: the winding number crossing algorithm is based on + the code from + http://www.softsurfer.com/Archive/algorithm_0103/algorithm_0103.htm + + which has the following copyright notice: + +// Copyright 2001, softSurfer (www.softsurfer.com) +// This code may be freely used and modified for any purpose +// providing that this copyright notice is included with it. +// SoftSurfer makes no warranty for this code, and cannot be held +// liable for any real or imagined damage resulting from its use. +// Users of this code must verify correctness for their application. + +*/ + +/* isLeft(): tests if a point is Left|On|Right of an infinite line. + Input: three points P0, P1, and P2 + Return: >0 for P2 left of the line through P0 and P1 + =0 for P2 on the line + <0 for P2 right of the line + See: the January 2001 Algorithm "Area of 2D and 3D Triangles and Polygons" +*/ +static int isLeft(const Vector2f *P0, const Vector2f *P1, const Vector2f *P2) +{ + float ret = ( (P1->x - P0->x) * (P2->y - P0->y) + - (P2->x - P0->x) * (P1->y - P0->y) ); + if (ret > 0) return 1; + if (ret < 0) return -1; + return 0; +} + +/* + Polygon_outside(): winding number test for a point in a polygon + Input: P = a point, + V[] = vertex points of a polygon V[n+1] with V[n]=V[0] + Return: true if P is outside the polygon +*/ +bool Polygon_outside(const Vector2f *P, const Vector2f *V, unsigned n) +{ + int wn = 0; // the winding number counter + + // loop through all edges of the polygon + for (unsigned i=0; iy) { // start y <= P.y + if (V[i+1].y > P->y) // an upward crossing + if (isLeft(&V[i], &V[i+1], P) > 0) // P left of edge + ++wn; // have a valid up intersect + } + else { // start y > P.y (no test needed) + if (V[i+1].y <= P->y) // a downward crossing + if (isLeft(&V[i], &V[i+1], P) < 0) // P right of edge + --wn; // have a valid down intersect + } + } + return wn == 0; +} diff --git a/libraries/AP_Math/polygon.h b/libraries/AP_Math/polygon.h new file mode 100644 index 0000000000..1ae785db09 --- /dev/null +++ b/libraries/AP_Math/polygon.h @@ -0,0 +1,21 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + * polygon.h + * Copyright (C) Andrew Tridgell 2011 + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +bool Polygon_outside(const Vector2f *P, const Vector2f *V, unsigned n); +