use CPA data request in AIS object context menu to display CPA, TCPA

OsmAnd/src/net/osmand/plus/plugins/aistracker/AisObjectMenuController.java

@@ -1,7 +1,10 @@
 package net.osmand.plus.plugins.aistracker;
 
+import static net.osmand.plus.plugins.aistracker.AisTrackerHelper.getCpa;
 import static net.osmand.plus.utils.OsmAndFormatter.FORMAT_MINUTES;
 
+import static java.lang.Math.ceil;
+
 import android.annotation.SuppressLint;
 
 import androidx.annotation.NonNull;
@@ -67,6 +70,38 @@ private String getOwnLocationAsString(@Nullable OsmAndLocationProvider locationP
         return null;
     }
      */
+    @SuppressLint("DefaultLocale")
+    private void addCpaInfo(@NonNull SortedSet<Integer> msgTypes,
+                            @Nullable OsmAndLocationProvider locationProvider) {
+        if (msgTypes.contains(21) || msgTypes.contains(9)) {
+            return;
+        }
+        if ((aisObject.getCog() != AisObjectConstants.INVALID_COG) &&
+                (aisObject.getSog() != AisObjectConstants.INVALID_SOG)) {
+            AisTrackerHelper.Cpa cpa = new AisTrackerHelper.Cpa();
+            Location aisLocation = aisObject.getLocation();
+            if (aisLocation != null) {
+                getCpa(aisLocation, locationProvider, cpa);
+                if (cpa.isValid()) {
+                    double cpaTime = cpa.getTcpa();
+                    double hours = ceil(cpaTime);
+                    double minutes = (cpaTime - hours) * 60.0;
+                    addMenuItem("CPA", String.format("%.1f nm", cpa.getCpaDist()));
+                    if (cpaTime > 0.0) {
+                        if (hours >= 2.0) {
+                            addMenuItem("TCPA", String.format("%.0f hours %.0f min", hours, minutes));
+                        } else if (hours >= 1.0) {
+                            addMenuItem("TCPA", String.format("%.0f hour %.0f min", hours, minutes));
+                        } else {
+                            addMenuItem("TCPA", String.format("%.0f min", minutes));
+                        }
+                    } else {
+                        addMenuItem("TCPA", String.format("%.1f hours", cpaTime));
+                    }
+                }
+            }
+        }
+    }
 
     private void addMenuItem(@NonNull String type, @Nullable String value) {
         if (value != null) {
@@ -127,6 +162,7 @@ public void addPlainMenuItems(String typeStr, PointDescription pointDescription,
                         addMenuItem("Bearing", String.format("%.1f", bearing));
                     } catch (Exception ignore) { }
                 }
+                addCpaInfo(msgTypes, locationProvider);
                 /*
                 // test:
                 addMenuItem("# loc", getOwnLocationAsString(locationProvider));

OsmAnd/src/net/osmand/plus/plugins/aistracker/AisTrackerHelper.java

@@ -39,6 +39,7 @@ public static class Cpa {
         private float cpaDist; // in miles
         private Location newPos1; // position of first object at time tcpa
         private Location newPos2; // position of first object at time tcpa
+        private boolean valid;
         public Cpa() {
             reset();
         }
@@ -47,6 +48,7 @@ public void reset() {
             tcpa = INVALID_TCPA;
             newPos1 = null;
             newPos2 = null;
+            valid = false;
         }
         public void setTcpa(double x) { this.tcpa = x; }
         public void setCpaDist(float x) { this.cpaDist = x; }
@@ -56,6 +58,8 @@ public void reset() {
         public float getCpaDist() { return cpaDist; }
         public Location getCpaPos1() { return newPos1; }
         public Location getCpaPos2() { return newPos2; }
+        public void validate() { valid = true; }
+        public boolean isValid() { return valid; }
     }
 
     /* calculate the Time to Closest Point of Approach (TCPA) of two moving objects:
@@ -67,8 +71,12 @@ private static double getTcpa(@NonNull Vector x, @NonNull Vector y,
                                   @NonNull Vector vx, @NonNull Vector vy, double lonCorrection) {
         Vector dx = new Vector( y.sub(x));
         Vector dv = new Vector(vy.sub(vx));
-        double divisor = dv.dot(dv); // TODO: check for Div/0
-        return -(((dx.x * dv.x) + (dx.y * dv.y / lonCorrection)) / divisor); // TODO: check for Div/0
+        double divisor = dv.dot(dv);
+        if ((Math.abs(divisor) < 1.0E-10f) || (lonCorrection < 1.0E-10f)) {
+            // avoid div by 0 or invalid lonCorrection
+            return INVALID_TCPA;
+        }
+        return -(((dx.x * dv.x) + (dx.y * dv.y / lonCorrection)) / divisor);
     }
 
     /* to calculate the Time to Closest Point of Approach (TCPA) between the objects x and y,
@@ -77,7 +85,7 @@ private static double getTcpa(@NonNull Location x, @NonNull Location y, double l
         if (checkSpeedAndBearing(x, y)) {
             return INVALID_TCPA;
         }
-        if (lonCorrection < 0.001) {
+        if (lonCorrection < 1.0E-10f) {
             // in this case the lonCorrection is considered invalid -> new calculation
             lonCorrection = getLonCorrection(x);
         }
@@ -113,8 +121,12 @@ private static Location getCpa(@NonNull Location x, @NonNull Location y, boolean
             return null;
         }
         double tcpa = getTcpa(x,y);
-        Location base = useFirstAsReference ? x : y;
-        return getNewPosition(base, tcpa);
+        if (tcpa == INVALID_TCPA) {
+            return null;
+        } else {
+            Location base = useFirstAsReference ? x : y;
+            return getNewPosition(base, tcpa);
+        }
     }
 
     /* to calculate the Closest Point of Approach (CPA) between the objects x and y,
@@ -159,13 +171,16 @@ public static void getCpa(@NonNull Location loc1, @NonNull Location loc2,
                               @NonNull Cpa result) {
         if (!checkSpeedAndBearing(loc1, loc2)) {
             double tcpa = getTcpa(loc1, loc2);
-            Location cpaX = getNewPosition(loc1, tcpa);
-            Location cpaY = getNewPosition(loc2, tcpa);
-            result.setTcpa(tcpa);
-            result.setCpaPos1(cpaX);
-            result.setCpaPos2(cpaY);
-            if ((cpaX != null) && (cpaY != null)) {
-                result.setCpaDist(meterToMiles(cpaX.distanceTo(cpaY)));
+            if (tcpa != INVALID_TCPA) {
+                Location cpaX = getNewPosition(loc1, tcpa);
+                Location cpaY = getNewPosition(loc2, tcpa);
+                result.setTcpa(tcpa);
+                result.setCpaPos1(cpaX);
+                result.setCpaPos2(cpaY);
+                if ((cpaX != null) && (cpaY != null)) {
+                    result.setCpaDist(meterToMiles(cpaX.distanceTo(cpaY)));
+                    result.validate();
+                }
             }
         }
     }
@@ -191,7 +206,8 @@ public static Location getNewPosition(@Nullable Location x, double time) {
         if (x != null) {
             if (x.hasBearing() && x.hasSpeed()) {
                 LatLonPoint a = new LatLonPoint(x.getLatitude(), x.getLongitude());
-                LatLonPoint b = a.getPoint(x.getSpeed() * time * Math.PI / 5556.0, bearingInRad(x.getBearing()));
+                LatLonPoint b = a.getPoint(x.getSpeed() * time * Math.PI / 5556.0,
+                        bearingInRad(x.getBearing()));
                 Location newX = new Location(x);
                 newX.setLongitude(b.getLongitude());
                 newX.setLatitude(b.getLatitude());
@@ -209,7 +225,7 @@ public static Location getNewPosition(@Nullable Location x, double time) {
     private static double getLonCorrection(@Nullable Location loc) {
         if (loc != null) {
             Location x = new Location(loc);
-            // simulate a "measurement" trio towards East...
+            // simulate a "measurement" trip towards East...
             x.setSpeed(knotsToMeterPerSecond(1.0f)); // speed -> 1 kn
             x.setBearing(90.0f);                           // course -> east
             Location yEast = getNewPosition(x, 1.0);  // new position after 1 hour

OsmAnd/src/net/osmand/plus/plugins/aistracker/AisTrackerLayer.java

@@ -62,43 +62,22 @@ public AisTrackerLayer(@NonNull Context context, @NonNull AisTrackerPlugin plugi
         initTimer();
         startNetworkListener();
 
-        initTestObjects();        // for test purposes:
+        // for test purposes: remove later...
+        initTestObjects();
+        testCpa();
     }
 
-    private void initTestObjects() {
-        // passenger ship
-        AisObject ais = new AisObject(34568, 1, 20, 0, 1, 320,
-                320.0, 8.4, 50.738d, 7.099d, 0.0);
-        updateAisObjectList(ais);
-        ais = new AisObject(34568, 5, 0, "TEST-CALLSIGN1", "TEST-Ship", 60 /* passenger */, 56,
-                65, 8, 12, 2,
-                "Potsdam", 8, 15, 22, 5);
-        updateAisObjectList(ais);
-        // sailing boat
-        ais = new AisObject(454011, 1, 20, 8, 0, 120,
-                125.0, 4.4, 50.737d, 7.098d, 0.0);
-        updateAisObjectList(ais);
-        ais = new AisObject(454011, 5, 0, "TEST-CALLSIGN2", "TEST-Sailor", 36 /* sailing  */, 0,
-                0, 0, 0, 0,
-                "@@@", 0, 0, 0, 0);
-        updateAisObjectList(ais);
-        // land station
-        ais = new AisObject(878121, 4, 50.736d, 7.100d);
-        updateAisObjectList(ais);
-        // AIDS
-        ais = new AisObject( 521077, 21, 50.735d, 7.101d, 2,
-                0, 0, 0, 0);
-        updateAisObjectList(ais);
-        // aircraft
-        ais = new AisObject(910323, 9, 15, 65, 180.5, 55.0, 50.734d, 7.102d);
-        updateAisObjectList(ais);
-
+    private void testCpa() {
         // here some tests for the geo (CPA) calculation
         // define 3 (vessel) objects
+        // for coordinate transformation see https://www.koordinaten-umrechner.de
         Location x1 = new Location("test", 49.5d, -1.0d); // 49°30'N, 1°00'W
         Location x2 = new Location("test", 49.916667d, 0.416667d); // 49°55'N, 0°25'E
         Location x3 = new Location("test", 49.666667d, -0.75d); // 49°40'N, 0°45'W
-        Location y1, y2, y3;
+        Location x4 = new Location("test", 49.5d, -4.0d); // 49°30'N, 4°00'W
+        Location x5 = new Location("test", 50.0d, -3.75d); // 50°00'N, 3°45'W
+        // taken from marine chart: distances: x1 - x3: 13.8 nm, x2 - x3: 47,2 nm, x4 - x5: 31.4 nm
+        Location y1, y2, y3, y4, y5;
         Log.d("AisTrackerLayer", "# test0: position 1 after 0 hours: "
                 + LocationConvert.convertLatitude(x1.getLatitude(), FORMAT_MINUTES, true)
                 +  ", " + LocationConvert.convertLongitude(x1.getLongitude(), FORMAT_MINUTES, true));
@@ -108,8 +87,17 @@ private void initTestObjects() {
         Log.d("AisTrackerLayer", "# test0: position 3 after 0 hours: "
                 + LocationConvert.convertLatitude(x3.getLatitude(), FORMAT_MINUTES, true)
                 +  ", " + LocationConvert.convertLongitude(x3.getLongitude(), FORMAT_MINUTES, true));
+        Log.d("AisTrackerLayer", "# test0: position 4 after 0 hours: "
+                + LocationConvert.convertLatitude(x4.getLatitude(), FORMAT_MINUTES, true)
+                +  ", " + LocationConvert.convertLongitude(x4.getLongitude(), FORMAT_MINUTES, true));
+        Log.d("AisTrackerLayer", "# test0: position 5 after 0 hours: "
+                + LocationConvert.convertLatitude(x5.getLatitude(), FORMAT_MINUTES, true)
+                +  ", " + LocationConvert.convertLongitude(x5.getLongitude(), FORMAT_MINUTES, true));
 
-        // use case: x1: course 0°, speed 5kn, x3: course 270°, speed 10kn, time: 1h, 1.5h
+        // test case: x1: course 0°, speed 5kn, x3: course 270°, speed 10kn, time: 1h, 1.5h
+        // taken from marine chart:
+        //  position after 1h: x1: 49°35'N, 1°00'W, x3: 49°40'N, 1°0.5'W, distance: 5.0nm
+        //  position after 1.5h: x1: 49°37.5'N, 1°00'W, x3: 49°40'N, 1°8.5'W, distance: 6.0nm
         x1.setSpeed(knotsToMeterPerSecond(5.0f));
         x1.setBearing(0.0f);
         x3.setSpeed(knotsToMeterPerSecond(10.0f));
@@ -124,7 +112,7 @@ private void initTestObjects() {
         if ((y1 != null) && (y3 != null)) {
             Log.d("AisTrackerLayer", "# test1: position 1 after 1 hour: "
                     + LocationConvert.convertLatitude(y1.getLatitude(), FORMAT_MINUTES, true)
-            +  ", " + LocationConvert.convertLongitude(y1.getLongitude(), FORMAT_MINUTES, true));
+                    +  ", " + LocationConvert.convertLongitude(y1.getLongitude(), FORMAT_MINUTES, true));
             Log.d("AisTrackerLayer", "# test1: position 3 after 1 hour: "
                     + LocationConvert.convertLatitude(y3.getLatitude(), FORMAT_MINUTES, true)
                     +  ", " + LocationConvert.convertLongitude(y3.getLongitude(), FORMAT_MINUTES, true));
@@ -153,7 +141,11 @@ private void initTestObjects() {
             Log.d("AisTrackerLayer", "# test1: dist1: " + meterToMiles(y1.distanceTo(y3)));
         }
 
-        // use case: x1: course 0°, speed 5kn, x3: course 270°, speed 5kn, time 1h, 1.5h, 2h
+        // test case: x1: course 0°, speed 5kn, x3: course 270°, speed 5kn, time 1h, 1.5h, 2h
+        // taken from marine chart:
+        //  position after 1h: x1: 49°35'N, 1°00'W, x3: 49°40'N, 0°52.7'W, distance: 6.8nm
+        //  position after 1.5h: x1: 49°37.5'N, 1°00'W, x3: 49°40'N, 0°56.7'W, distance: 3.1nm
+        //  position after 2h: x1: 49°40'N, 1°00'W, x3: 49°40'N, 1°0.5'W, distance: 0.3nm
         x1.setSpeed(knotsToMeterPerSecond(5.0f));
         x1.setBearing(0.0f);
         x3.setSpeed(knotsToMeterPerSecond(5.0f));
@@ -197,7 +189,9 @@ private void initTestObjects() {
             Log.d("AisTrackerLayer", "# test2: dist1: " + meterToMiles(y1.distanceTo(y3)));
         }
 
-        // use case: x2: course 270°, speed 5kn, x3: course 45°, speed 5kn, time 5h
+        // test case: x2: course 270°, speed 5kn, x3: course 45°, speed 5kn, time 5h
+        // taken from marine chart:
+        //  position after 5h: x2: 49°55'N, 0°14.1'W, x3: 49°57.8'N, 0°17.5'W, distance: 3.5nm
         x2.setSpeed(knotsToMeterPerSecond(5.0f));
         x2.setBearing(270.0f);
         x3.setSpeed(knotsToMeterPerSecond(5.0f));
@@ -219,6 +213,58 @@ private void initTestObjects() {
             Log.d("AisTrackerLayer", "# test3: dist1: " + meterToMiles(y2.distanceTo(y3)));
         }
 
+        // test case: x4: course 45°, speed 10kn, x5: course 70°, speed 5kn, time 6h
+        // taken from marine chart:
+        //  position after 6h: x4: 50°12.1'N, 2°54.4'W, x5: 50°10.1'N, 3°1.5'W, distance: 5nm
+        x4.setSpeed(knotsToMeterPerSecond(10.0f));
+        x4.setBearing(45.0f);
+        x5.setSpeed(knotsToMeterPerSecond(5.0f));
+        x5.setBearing(70.0f);
+        cpa1.reset();
+        getCpa(x4, x5, cpa1);
+        Log.d("AisTrackerLayer", "# test4: tcpa(x4, x5): " + cpa1.getTcpa());
+        Log.d("AisTrackerLayer", "# test4: dist at tcpa: " + cpa1.getCpaDist());
+        Log.d("AisTrackerLayer", "# test4: dist0: " + meterToMiles(x4.distanceTo(x5)));
+        y4 = AisTrackerHelper.getNewPosition(x4, 6.0);
+        y5 = AisTrackerHelper.getNewPosition(x5, 6.0);
+        if ((y4 != null) && (y5 != null)) {
+            Log.d("AisTrackerLayer", "# test4: position 4 after 6 hours: "
+                    + LocationConvert.convertLatitude(y4.getLatitude(), FORMAT_MINUTES, true)
+                    +  ", " + LocationConvert.convertLongitude(y4.getLongitude(), FORMAT_MINUTES, true));
+            Log.d("AisTrackerLayer", "# test4: position 5 after 6 hours: "
+                    + LocationConvert.convertLatitude(y5.getLatitude(), FORMAT_MINUTES, true)
+                    +  ", " + LocationConvert.convertLongitude(y5.getLongitude(), FORMAT_MINUTES, true));
+            Log.d("AisTrackerLayer", "# test4: dist1: " + meterToMiles(y4.distanceTo(y5)));
+        }
+    }
+    private void initTestObjects() {
+        // passenger ship
+        AisObject ais = new AisObject(34568, 1, 20, 0, 1, 320,
+                320.0, 8.4, 50.738d, 7.099d, 0.0);
+        updateAisObjectList(ais);
+        ais = new AisObject(34568, 5, 0, "TEST-CALLSIGN1", "TEST-Ship", 60 /* passenger */, 56,
+                65, 8, 12, 2,
+                "Potsdam", 8, 15, 22, 5);
+        updateAisObjectList(ais);
+        // sailing boat
+        ais = new AisObject(454011, 1, 20, 8, 0, 120,
+                125.0, 4.4, 50.737d, 7.098d, 0.0);
+        updateAisObjectList(ais);
+        ais = new AisObject(454011, 5, 0, "TEST-CALLSIGN2", "TEST-Sailor", 36 /* sailing  */, 0,
+                0, 0, 0, 0,
+                "@@@", 0, 0, 0, 0);
+        updateAisObjectList(ais);
+        // land station
+        ais = new AisObject(878121, 4, 50.736d, 7.100d);
+        updateAisObjectList(ais);
+        // AIDS
+        ais = new AisObject( 521077, 21, 50.735d, 7.101d, 2,
+                0, 0, 0, 0);
+        updateAisObjectList(ais);
+        // aircraft
+        ais = new AisObject(910323, 9, 15, 65, 180.5, 55.0, 50.734d, 7.102d);
+        updateAisObjectList(ais);
+
         //removeOldestAisObjectListEntry();
         //removeLostAisObjects();
     }