Merge pull request #2608 from Abyss777/polyline_geofence

Implemented polyline geofence

9 files changed, 264 insertions, 66 deletions

diff --git a/src/org/traccar/geofence/GeofenceGeometry.java b/src/org/traccar/geofence/GeofenceGeometry.java
index 6b5b0acb7..857ba3414 100644
--- a/src/org/traccar/geofence/GeofenceGeometry.java
+++ b/src/org/traccar/geofence/GeofenceGeometry.java
@@ -25,4 +25,26 @@ public abstract class GeofenceGeometry {
 
     public abstract void fromWkt(String wkt) throws ParseException;
 
+    public static class Coordinate {
+
+        private double lat;
+        private double lon;
+
+        public double getLat() {
+            return lat;
+        }
+
+        public void setLat(double lat) {
+            this.lat = lat;
+        }
+
+        public double getLon() {
+            return lon;
+        }
+
+        public void setLon(double lon) {
+            this.lon = lon;
+        }
+    }
+
 }
diff --git a/src/org/traccar/geofence/GeofencePolygon.java b/src/org/traccar/geofence/GeofencePolygon.java
index 03638d6c9..2048ba26d 100644
--- a/src/org/traccar/geofence/GeofencePolygon.java
+++ b/src/org/traccar/geofence/GeofencePolygon.java
@@ -27,44 +27,18 @@ public class GeofencePolygon extends GeofenceGeometry {
         fromWkt(wkt);
     }
 
-    private static class Coordinate {
-
-        public static final double DEGREE360 = 360;
-
-        private double lat;
-        private double lon;
-
-        public double getLat() {
-            return lat;
-        }
-
-        public void setLat(double lat) {
-            this.lat = lat;
-        }
-
-        public double getLon() {
-            return lon;
-        }
-
-        // Need not to confuse algorithm by the abrupt reset of longitude
-        public double getLon360() {
-            return lon + DEGREE360;
-        }
-
-        public void setLon(double lon) {
-            this.lon = lon;
-        }
-    }
-
     private ArrayList<Coordinate> coordinates;
 
     private double[] constant;
     private double[] multiple;
 
+    private boolean needNormalize = false;
+
     private void precalc() {
         if (coordinates == null) {
             return;
         }
+
         int polyCorners = coordinates.size();
         int i;
         int j = polyCorners - 1;
@@ -79,38 +53,55 @@ public class GeofencePolygon extends GeofenceGeometry {
         constant = new double[polyCorners];
         multiple = new double[polyCorners];
 
+        boolean hasNegative = false;
+        boolean hasPositive = false;
+        for (i = 0; i < polyCorners; i++) {
+            if (coordinates.get(i).getLon() > 90) {
+                hasPositive = true;
+            } else if (coordinates.get(i).getLon() < -90) {
+                hasNegative = true;
+            }
+        }
+        needNormalize = hasPositive && hasNegative;
 
         for (i = 0; i < polyCorners; j = i++) {
-            if (coordinates.get(j).getLon360() == coordinates.get(i).getLon360()) {
+            if (normalizeLon(coordinates.get(j).getLon()) == normalizeLon(coordinates.get(i).getLon())) {
                 constant[i] = coordinates.get(i).getLat();
                 multiple[i] = 0;
             } else {
                 constant[i] = coordinates.get(i).getLat()
-                        - (coordinates.get(i).getLon360() * coordinates.get(j).getLat())
-                        / (coordinates.get(j).getLon360() - coordinates.get(i).getLon360())
-                        + (coordinates.get(i).getLon360() * coordinates.get(i).getLat())
-                        / (coordinates.get(j).getLon360() - coordinates.get(i).getLon360());
+                        - (normalizeLon(coordinates.get(i).getLon()) * coordinates.get(j).getLat())
+                        / (normalizeLon(coordinates.get(j).getLon()) - normalizeLon(coordinates.get(i).getLon()))
+                        + (normalizeLon(coordinates.get(i).getLon()) * coordinates.get(i).getLat())
+                        / (normalizeLon(coordinates.get(j).getLon()) - normalizeLon(coordinates.get(i).getLon()));
                 multiple[i] = (coordinates.get(j).getLat() - coordinates.get(i).getLat())
-                        / (coordinates.get(j).getLon360() - coordinates.get(i).getLon360());
+                        / (normalizeLon(coordinates.get(j).getLon()) - normalizeLon(coordinates.get(i).getLon()));
             }
         }
     }
 
+    private double normalizeLon(double lon) {
+        if (needNormalize && lon < -90) {
+            return lon + 360;
+        }
+        return lon;
+    }
+
     @Override
     public boolean containsPoint(double latitude, double longitude) {
 
         int polyCorners = coordinates.size();
         int i;
         int j = polyCorners - 1;
-        double longitude360 = longitude + Coordinate.DEGREE360;
+        double longitudeNorm = normalizeLon(longitude);
         boolean oddNodes = false;
 
         for (i = 0; i < polyCorners; j = i++) {
-            if (coordinates.get(i).getLon360() < longitude360
-                    && coordinates.get(j).getLon360() >= longitude360
-                    || coordinates.get(j).getLon360() < longitude360
-                    && coordinates.get(i).getLon360() >= longitude360) {
-                oddNodes ^= longitude360 * multiple[i] + constant[i] < latitude;
+            if (normalizeLon(coordinates.get(i).getLon()) < longitudeNorm
+                    && normalizeLon(coordinates.get(j).getLon()) >= longitudeNorm
+                    || normalizeLon(coordinates.get(j).getLon()) < longitudeNorm
+                    && normalizeLon(coordinates.get(i).getLon()) >= longitudeNorm) {
+                oddNodes ^= longitudeNorm * multiple[i] + constant[i] < latitude;
             }
         }
         return oddNodes;
diff --git a/src/org/traccar/geofence/GeofencePolyline.java b/src/org/traccar/geofence/GeofencePolyline.java
new file mode 100644
index 000000000..d84f512e3
--- /dev/null
+++ b/src/org/traccar/geofence/GeofencePolyline.java
@@ -0,0 +1,107 @@
+/*
+ * Copyright 2016 Anton Tananaev (anton@traccar.org)
+ * Copyright 2016 Andrey Kunitsyn (andrey@traccar.org)
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package org.traccar.geofence;
+
+import java.text.ParseException;
+import java.util.ArrayList;
+
+import org.traccar.helper.DistanceCalculator;
+
+public class GeofencePolyline extends GeofenceGeometry {
+
+    private ArrayList<Coordinate> coordinates;
+    private double distance;
+
+    public GeofencePolyline() {
+    }
+
+    public GeofencePolyline(String wkt, double distance) throws ParseException {
+        fromWkt(wkt);
+        this.distance = distance;
+    }
+
+    @Override
+    public boolean containsPoint(double latitude, double longitude) {
+        for (int i = 1; i < coordinates.size(); i++) {
+            if (DistanceCalculator.distanceToLine(
+                    latitude, longitude, coordinates.get(i - 1).getLat(), coordinates.get(i - 1).getLon(),
+                    coordinates.get(i).getLat(), coordinates.get(i).getLon()) <= distance) {
+                return true;
+            }
+        }
+        return false;
+    }
+
+    @Override
+    public String toWkt() {
+        StringBuilder buf = new StringBuilder();
+        buf.append("LINESTRING (");
+        for (Coordinate coordinate : coordinates) {
+            buf.append(String.valueOf(coordinate.getLat()));
+            buf.append(" ");
+            buf.append(String.valueOf(coordinate.getLon()));
+            buf.append(", ");
+        }
+        return buf.substring(0, buf.length() - 2) + ")";
+    }
+
+    @Override
+    public void fromWkt(String wkt) throws ParseException {
+        if (coordinates == null) {
+            coordinates = new ArrayList<>();
+        } else {
+            coordinates.clear();
+        }
+
+        if (!wkt.startsWith("LINESTRING")) {
+            throw new ParseException("Mismatch geometry type", 0);
+        }
+        String content = wkt.substring(wkt.indexOf("(") + 1, wkt.indexOf(")"));
+        if (content.isEmpty()) {
+            throw new ParseException("No content", 0);
+        }
+        String[] commaTokens = content.split(",");
+        if (commaTokens.length < 2) {
+            throw new ParseException("Not valid content", 0);
+        }
+
+        for (String commaToken : commaTokens) {
+            String[] tokens = commaToken.trim().split("\\s");
+            if (tokens.length != 2) {
+                throw new ParseException("Here must be two coordinates: " + commaToken, 0);
+            }
+            Coordinate coordinate = new Coordinate();
+            try {
+                coordinate.setLat(Double.parseDouble(tokens[0]));
+            } catch (NumberFormatException e) {
+                throw new ParseException(tokens[0] + " is not a double", 0);
+            }
+            try {
+                coordinate.setLon(Double.parseDouble(tokens[1]));
+            } catch (NumberFormatException e) {
+                throw new ParseException(tokens[1] + " is not a double", 0);
+            }
+            coordinates.add(coordinate);
+        }
+
+    }
+
+    public void setDistance(double distance) {
+        this.distance = distance;
+    }
+
+}
diff --git a/src/org/traccar/helper/DistanceCalculator.java b/src/org/traccar/helper/DistanceCalculator.java
index 3452597ab..88d4ef8a4 100644
--- a/src/org/traccar/helper/DistanceCalculator.java
+++ b/src/org/traccar/helper/DistanceCalculator.java
@@ -1,5 +1,6 @@
 /*
- * Copyright 2014 Anton Tananaev (anton@traccar.org)
+ * Copyright 2014 - 2016 Anton Tananaev (anton@traccar.org)
+ * Copyright 2016 Andrey Kunitsyn (andrey@traccar.org)
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -33,4 +34,20 @@ public final class DistanceCalculator {
         return d * 1000;
     }
 
+    public static double distanceToLine(
+            double pointLat, double pointLon, double lat1, double lon1, double lat2, double lon2) {
+        double d0 = distance(pointLat, pointLon, lat1, lon1);
+        double d1 = distance(lat1, lon1, lat2, lon2);
+        double d2 = distance(lat2, lon2, pointLat, pointLon);
+        if (Math.pow(d0, 2) > Math.pow(d1, 2) + Math.pow(d2, 2)) {
+            return d2;
+        }
+        if (Math.pow(d2, 2) > Math.pow(d1, 2) + Math.pow(d0, 2)) {
+            return d0;
+        }
+        double halfP = (d0 + d1 + d2) * 0.5;
+        double area = Math.sqrt(halfP * (halfP - d0) * (halfP - d1) * (halfP - d2));
+        return 2 * area / d1;
+    }
+
 }
diff --git a/src/org/traccar/model/Geofence.java b/src/org/traccar/model/Geofence.java
index ecfd3101c..326c45b5f 100644
--- a/src/org/traccar/model/Geofence.java
+++ b/src/org/traccar/model/Geofence.java
@@ -17,9 +17,11 @@ package org.traccar.model;
 
 import java.text.ParseException;
 
+import org.traccar.Context;
 import org.traccar.geofence.GeofenceCircle;
 import org.traccar.geofence.GeofenceGeometry;
 import org.traccar.geofence.GeofencePolygon;
+import org.traccar.geofence.GeofencePolyline;
 
 import com.fasterxml.jackson.annotation.JsonIgnore;
 
@@ -27,6 +29,7 @@ public class Geofence extends Extensible {
 
     public static final String TYPE_GEOFENCE_CILCLE = "geofenceCircle";
     public static final String TYPE_GEOFENCE_POLYGON = "geofencePolygon";
+    public static final String TYPE_GEOFENCE_POLYLINE = "geofencePolyline";
 
     private String name;
 
@@ -60,6 +63,8 @@ public class Geofence extends Extensible {
             geometry = new GeofenceCircle(area);
         } else if (area.startsWith("POLYGON")) {
             geometry = new GeofencePolygon(area);
+        } else if (area.startsWith("LINESTRING")) {
+            geometry = new GeofencePolyline(area, Context.getConfig().getDouble("geofence.polylineDistance", 25));
         } else {
             throw new ParseException("Unknown geometry type", 0);
         }
diff --git a/test/org/traccar/geofence/GeofenceCircleTest.java b/test/org/traccar/geofence/GeofenceCircleTest.java
index 52c214b53..133ca1db8 100644
--- a/test/org/traccar/geofence/GeofenceCircleTest.java
+++ b/test/org/traccar/geofence/GeofenceCircleTest.java
@@ -8,29 +8,19 @@ import org.junit.Test;
 public class GeofenceCircleTest {
 
     @Test
-    public void testCircleWKT() {
+    public void testCircleWkt() throws ParseException {
         String test = "CIRCLE (55.75414 37.6204, 100)";
         GeofenceGeometry geofenceGeometry = new GeofenceCircle();
-        try {
         geofenceGeometry.fromWkt(test);
-        } catch (ParseException e){
-            Assert.assertTrue("ParseExceprion: " + e.getMessage(), true);
-        }
         Assert.assertEquals(geofenceGeometry.toWkt(), test);
     }
 
     @Test
-    public void testContainsCircle() {
+    public void testContainsCircle() throws ParseException {
         String test = "CIRCLE (55.75414 37.6204, 100)";
         GeofenceGeometry geofenceGeometry = new GeofenceCircle();
-        try {
         geofenceGeometry.fromWkt(test);
-        } catch (ParseException e){
-            Assert.assertTrue("ParseExceprion: " + e.getMessage(), true);
-        }
-
         Assert.assertTrue(geofenceGeometry.containsPoint(55.75477, 37.62025));
-
         Assert.assertTrue(!geofenceGeometry.containsPoint(55.75545, 37.61921));
     }
 }
diff --git a/test/org/traccar/geofence/GeofencePolygonTest.java b/test/org/traccar/geofence/GeofencePolygonTest.java
index e3d6aaa29..361e7b70f 100644
--- a/test/org/traccar/geofence/GeofencePolygonTest.java
+++ b/test/org/traccar/geofence/GeofencePolygonTest.java
@@ -8,31 +8,43 @@ import org.junit.Test;
 public class GeofencePolygonTest {
 
     @Test
-    public void testPolygonWKT() {
+    public void testPolygonWkt() throws ParseException {
         String test = "POLYGON ((55.75474 37.61823, 55.75513 37.61888, 55.7535 37.6222, 55.75315 37.62165))";
         GeofenceGeometry geofenceGeometry = new GeofencePolygon();
-        try {
         geofenceGeometry.fromWkt(test);
-        } catch (ParseException e){
-            Assert.assertTrue("ParseExceprion: " + e.getMessage(), true);
-        }
         Assert.assertEquals(geofenceGeometry.toWkt(), test);
     }
 
     @Test
-    public void testContainsPolygon() {
+    public void testContainsPolygon() throws ParseException {
         String test = "POLYGON ((55.75474 37.61823, 55.75513 37.61888, 55.7535 37.6222, 55.75315 37.62165))";
         GeofenceGeometry geofenceGeometry = new GeofencePolygon();
-        try {
         geofenceGeometry.fromWkt(test);
-        } catch (ParseException e){
-            Assert.assertTrue("ParseExceprion: " + e.getMessage(), true);
-        }
-
         Assert.assertTrue(geofenceGeometry.containsPoint(55.75476, 37.61915));
-
         Assert.assertTrue(!geofenceGeometry.containsPoint(55.75545, 37.61921));
 
     }
+    
+    @Test
+    public void testContainsPolygon180() throws ParseException {
+        String test = "POLYGON ((66.9494 179.838, 66.9508 -179.8496, 66.8406 -180.0014))";
+        GeofenceGeometry geofenceGeometry = new GeofencePolygon();
+        geofenceGeometry.fromWkt(test);
+        Assert.assertTrue(geofenceGeometry.containsPoint(66.9015, -180.0096));
+        Assert.assertTrue(geofenceGeometry.containsPoint(66.9015, 179.991));
+        Assert.assertTrue(!geofenceGeometry.containsPoint(66.8368, -179.8792));
+
+    }
+    
+    @Test
+    public void testContainsPolygon0() throws ParseException {
+        String test = "POLYGON ((51.1966 -0.6207, 51.1897 0.4147, 50.9377 0.5136, 50.8675 -0.6082))";
+        GeofenceGeometry geofenceGeometry = new GeofencePolygon();
+        geofenceGeometry.fromWkt(test);
+        Assert.assertTrue(geofenceGeometry.containsPoint(51.0466, -0.0165));
+        Assert.assertTrue(geofenceGeometry.containsPoint(51.0466, 0.018));
+        Assert.assertTrue(!geofenceGeometry.containsPoint(50.9477, 0.5836));
+
+    }
 
 }
diff --git a/test/org/traccar/geofence/GeofencePolylineTest.java b/test/org/traccar/geofence/GeofencePolylineTest.java
new file mode 100644
index 000000000..71aeb4081
--- /dev/null
+++ b/test/org/traccar/geofence/GeofencePolylineTest.java
@@ -0,0 +1,45 @@
+package org.traccar.geofence;
+
+import java.text.ParseException;
+
+import org.junit.Assert;
+import org.junit.Test;
+
+public class GeofencePolylineTest {
+
+    @Test
+    public void testPolylineWkt() throws ParseException {
+        String test = "LINESTRING (55.75474 37.61823, 55.75513 37.61888, 55.7535 37.6222, 55.75315 37.62165)";
+        GeofenceGeometry geofenceGeometry = new GeofencePolyline();
+        geofenceGeometry.fromWkt(test);
+        Assert.assertEquals(geofenceGeometry.toWkt(), test);
+    }
+    
+    @Test
+    public void testContainsPolyline1Interval() throws ParseException {
+        String test = "LINESTRING (56.83777 60.59833, 56.83766 60.5968)";
+        GeofenceGeometry geofenceGeometry = new GeofencePolyline(test, 35);
+        Assert.assertTrue(geofenceGeometry.containsPoint(56.83801, 60.59748));
+        ((GeofencePolyline) geofenceGeometry).setDistance(15);
+        Assert.assertTrue(!geofenceGeometry.containsPoint(56.83801, 60.59748));
+    }
+
+    @Test
+    public void testContainsPolyline3Intervals() throws ParseException {
+        String test = "LINESTRING (56.836 60.6126, 56.8393 60.6114, 56.83887 60.60811, 56.83782 60.5988)";
+        GeofenceGeometry geofenceGeometry = new GeofencePolyline(test, 15);
+        Assert.assertTrue(geofenceGeometry.containsPoint(56.83847, 60.60458));
+        Assert.assertTrue(!geofenceGeometry.containsPoint(56.83764, 60.59725));
+        Assert.assertTrue(!geofenceGeometry.containsPoint(56.83861, 60.60822));
+
+    }
+    
+    @Test
+    public void testContainsPolylineNear180() throws ParseException {
+        String test = "LINESTRING (66.9494 179.838, 66.9508 -179.8496)";
+        GeofenceGeometry geofenceGeometry = new GeofencePolyline(test, 25);
+        Assert.assertTrue(geofenceGeometry.containsPoint(66.95, 180.0));
+        Assert.assertTrue(!geofenceGeometry.containsPoint(66.96, 180.0));
+        Assert.assertTrue(!geofenceGeometry.containsPoint(66.9509, -179.83));
+    }
+}
diff --git a/test/org/traccar/helper/DistanceCalculatorTest.java b/test/org/traccar/helper/DistanceCalculatorTest.java
index 7bbb4e3b1..22e41c9a3 100644
--- a/test/org/traccar/helper/DistanceCalculatorTest.java
+++ b/test/org/traccar/helper/DistanceCalculatorTest.java
@@ -10,5 +10,14 @@ public class DistanceCalculatorTest {
         Assert.assertEquals(
                 DistanceCalculator.distance(0.0, 0.0, 0.05, 0.05), 7863.0, 10.0);
     }
+    
+    @Test
+    public void testDistanceToLine() {
+        Assert.assertEquals(DistanceCalculator.distanceToLine(
+                56.83801, 60.59748, 56.83777, 60.59833, 56.83766, 60.5968), 33.0, 5.0);
+        
+        Assert.assertEquals(DistanceCalculator.distanceToLine(
+                56.83753, 60.59508, 56.83777, 60.59833, 56.83766, 60.5968), 105.0, 5.0);
+    }
 
 }