types/map/ScatterPointsElement: Tweaked vehicle tail for better display.

Author: paulosousadias

src/java/pt/lsts/neptus/renderer2d/StateRenderer2D.java

@@ -64,6 +64,7 @@
 import java.awt.image.BufferedImage;
 import java.text.NumberFormat;
 import java.util.Collection;
+import java.util.Date;
 import java.util.Enumeration;
 import java.util.HashSet;
 import java.util.Hashtable;
@@ -570,6 +571,8 @@ public MapGroup getMapGroup() {
      */
     public void vehicleStateChanged(String systemId, SystemPositionAndAttitude state, boolean repaint) {
         if (state == null) {
+            NeptusLog.pub().warn(">>>>>>>>>> Tail for {} state was null, clearing tail!! (was {}) <<<<<<<<<<", systemId,
+                    vehicleTails.containsKey(systemId) ? vehicleTails.get(systemId).getNumberOfPoints() : "-");
             vehicleStates.remove(systemId);
             vehicleTails.remove(systemId);
             vehicles = vehicleStates.keySet().toArray(new String[0]);
@@ -605,7 +608,11 @@ public void vehicleStateChanged(String systemId, SystemPositionAndAttitude state
             vehicleStates.put(systemId, state);
 
             //double[] distFromRef = state.getPosition().getOffsetFrom(vehicleTails.get(systemId).getCenterLocation());
-            vehicleTails.get(systemId).addPoint(state.getPosition());
+            if (state.getTime() > vehicleTails.get(systemId).getLastLocationTimeMillis() || vehicleTails.get(systemId).getLastLocationTimeMillis() == -1) {
+                NeptusLog.pub().trace(">>>>>>>>>> Tail for {} ADD element. (was {})  @ {} <<<<<<<<<<", systemId,
+                    vehicleTails.get(systemId).getPoints().size(), new Date(state.getTime()));
+            }
+            vehicleTails.get(systemId).addPoint(state.getPosition(), state.getTime());
 
             if (!repaint || System.currentTimeMillis() - lastPaintTime < minDelay) {
                 return;

src/java/pt/lsts/neptus/types/map/ScatterPointsElement.java

@@ -86,6 +86,8 @@ public class ScatterPointsElement extends AbstractElement {
     protected Point3d lastRemoved = null;
     private int gradientcolor = 0;
 
+    int maxDistToChangeCenterMeters = 10_000;
+
     public ScatterPointsElement() {
         super();
     }
@@ -221,10 +223,34 @@ public void clearPoints() {
     }
 
     public void addPoint(LocationType loc) {
-        if (loc.getDistanceInMeters(getCenterLocation()) > 3000) {
+        double distanceInMeters = loc.getDistanceInMeters(getCenterLocation());
+        if (distanceInMeters > maxDistToChangeCenterMeters) {
+            NeptusLog.pub().warn("Point {} is too far away ({}m) from the center location ({}). Changing center location to the new point.",
+                    loc, distanceInMeters, getCenterLocation());
+            synchronized (points) {
+                // Iterate from the start to the end, removing points that are too far away
+                for (int i = 0; i < points.size(); i++) {
+                    Point3d pt = points.get(i);
+                    LocationType ptLoc = new LocationType(getCenterLocation());
+                    ptLoc.translatePosition(pt.x, pt.y, pt.z);
+                    double dist = ptLoc.getDistanceInMeters(loc);
+                    if (dist > maxDistToChangeCenterMeters * 3) {
+                        NeptusLog.pub().warn("Removing point {} at distance {}m from the new center location {}",
+                                pt, dist, loc);
+                        points.remove(i);
+                        i--; // Adjust index after removal
+                    } else {
+                        double[] offsets = ptLoc.getOffsetFrom(loc);
+                        pt.x = offsets[0];
+                        pt.y = offsets[1];
+                        pt.z = offsets[2];
+                    }
+                }
+            }
+
             setCenterLocation(loc);
             addPoint(0, 0, 0);
-            clearPoints();
+            // clearPoints();
         }
         else {
             double[] offsets = loc.getOffsetFrom(getCenterLocation());

src/java/pt/lsts/neptus/types/map/VehicleTailElement.java

@@ -32,6 +32,9 @@
  */
 package pt.lsts.neptus.types.map;
 
+import pt.lsts.neptus.NeptusLog;
+import pt.lsts.neptus.types.coord.LocationType;
+
 import java.awt.Color;
 
 
@@ -41,6 +44,8 @@
  */
 public class VehicleTailElement extends ScatterPointsElement {
 
+    private long lastLocationTimeMillis = -1;
+
     public VehicleTailElement() {
         super();
     }
@@ -56,5 +61,33 @@ public VehicleTailElement(MapGroup mg, MapType parentMap, Color baseColor) {
 	@Override
 	public String getType() {
 		return "Vehicle tail";
-	}	
+	}
+
+    public void addPoint(LocationType loc, long timeMillis) {
+        if (timeMillis <= lastLocationTimeMillis && lastLocationTimeMillis != -1) {
+            // If the new point is older than the last one, ignore it
+            NeptusLog.pub().trace("Received a location point with time {} older than or equal to the last one: {}",
+                    timeMillis, lastLocationTimeMillis);
+            return;
+        }
+
+        super.addPoint(loc);
+        lastLocationTimeMillis = timeMillis;
+    }
+
+    @Override
+    public void addPoint(LocationType loc) {
+        super.addPoint(loc);
+        lastLocationTimeMillis = System.currentTimeMillis();
+    }
+
+    @Override
+    public void clearPoints() {
+        super.clearPoints();
+        lastLocationTimeMillis = -1;
+    }
+
+    public long getLastLocationTimeMillis() {
+        return lastLocationTimeMillis;
+    }
 }