From 819822e1722dc31c0f97b8494c96a6c292b07185 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sat, 4 Jan 2014 22:01:31 +0100
Subject: navigator/geofence: add isEmpty() function and checks

---
 src/modules/navigator/geofence.cpp | 66 +++++++++++++++++++++-----------------
 src/modules/navigator/geofence.h   |  4 ++-
 2 files changed, 39 insertions(+), 31 deletions(-)

(limited to 'src')

diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 1063fd2e2..a49a71e1b 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -81,41 +81,48 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
 bool Geofence::inside(double lat, double lon, float altitude)
 {
 	if (valid()) {
-		/* Vertical check */
-		if (altitude > _altitude_max || altitude < _altitude_min)
-			return false;
 
-		/*Horizontal check */
-		/* Adaptation of algorithm originally presented as
-		 * PNPOLY - Point Inclusion in Polygon Test
-		 * W. Randolph Franklin (WRF) */
+		if (!isEmpty()) {
+			/* Vertical check */
+			if (altitude > _altitude_max || altitude < _altitude_min)
+				return false;
+
+			/*Horizontal check */
+			/* Adaptation of algorithm originally presented as
+			 * PNPOLY - Point Inclusion in Polygon Test
+			 * W. Randolph Franklin (WRF) */
+
+			bool c = false;
+
+			struct fence_vertex_s temp_vertex_i;
+			struct fence_vertex_s temp_vertex_j;
+
+			/* Red until fence is finished */
+			for (unsigned i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) {
+				if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
+					break;
+				}
+				if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
+					break;
+				}
+
+				// skip vertex 0 (return point)
+				if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) &&
+							(lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) /
+							 (temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) {
+							c = !c;
+				}
 
-		bool c = false;
-
-		struct fence_vertex_s temp_vertex_i;
-		struct fence_vertex_s temp_vertex_j;
-
-		/* Red until fence is finished */
-		for (int i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) {
-			if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
-				break;
-			}
-			if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
-				break;
-			}
-
-			// skip vertex 0 (return point)
-			if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) &&
-						(lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) /
-						 (temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) {
-						c = !c;
 			}
 
+			return c;
+		} else {
+			/* Empty fence --> accept all points */
+			return true;
 		}
 
-		return c;
-
 	} else {
+		/* Invalid fence --> accept all points */
 		return true;
 	}
 }
@@ -124,9 +131,8 @@ bool
 Geofence::valid()
 {
 	// NULL fence is valid
-	if (_verticesCount == 0) {
+	if (isEmpty())
 		return true;
-	}
 
 	// Otherwise
 	if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) {
diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
index 9c753a11d..781e7a263 100644
--- a/src/modules/navigator/geofence.h
+++ b/src/modules/navigator/geofence.h
@@ -51,7 +51,7 @@ private:
 	float			_altitude_min;
 	float			_altitude_max;
 
-	int 			_verticesCount;
+	unsigned 			_verticesCount;
 public:
 	Geofence();
 	~Geofence();
@@ -79,6 +79,8 @@ public:
 	void publishFence(unsigned vertices);
 
 	int loadFromFile(const char *filename);
+
+	bool isEmpty() {return _verticesCount == 0;}
 };
 
 
-- 
cgit v1.2.3