fix: remove items from index implementations grid and tree (regressio…

…n) (#357) * fix: remove items from index implementations grid and tree (regression) * clean: hiding ItemKey creation
eclipse · Oct 23, 2023 · 1999ffd · 1999ffd
1 parent bbd754a
commit 1999ffd
Show file tree

Hide file tree

Showing 7 changed files with 106 additions and 25 deletions.
diff --git a/.../mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObject.java b/.../mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObject.java
@@ -20,6 +20,7 @@
 import org.eclipse.mosaic.lib.math.Vector3d;
 
 import org.apache.commons.lang3.builder.EqualsBuilder;
+import org.apache.commons.lang3.builder.HashCodeBuilder;
 
 public abstract class SpatialObject<T extends SpatialObject<T>> extends Vector3d {
 
@@ -89,8 +90,11 @@ public boolean equals(Object o) {
 
     @Override
     public int hashCode() {
-        // use id as hashcode to store only one VehicleObject per vehicle id in perception index (e.q. quadtree)
-        return this.id.hashCode();
+        return new HashCodeBuilder(5, 11)
+                .appendSuper(super.hashCode())
+                .append(id)
+                .append(cartesianPosition)
+                .toHashCode();
     }
 
     /**

diff --git a/.../fed/application/ambassador/simulation/perception/index/objects/SpatialObjectAdapter.java b/.../fed/application/ambassador/simulation/perception/index/objects/SpatialObjectAdapter.java
@@ -18,6 +18,12 @@
 import org.eclipse.mosaic.lib.spatial.SpatialItemAdapter;
 
 public class SpatialObjectAdapter<T extends SpatialObject> implements SpatialItemAdapter<T> {
+
+    @Override
+    public int getItemHash(T item) {
+        return item.getId().hashCode();
+    }
+
     @Override
     public double getCenterX(T item) {
         return item.getProjectedPosition().getX();

diff --git a/.../mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java b/.../mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java
@@ -58,7 +58,7 @@ public class VehicleObject extends SpatialObject<VehicleObject> {
     /**
      * The 2D bounding box of a vehicle from birds eye view.
      */
-    private VehicleBoundingBox boundingBox = null;
+    private transient VehicleBoundingBox boundingBox = null;
 
     public VehicleObject(String id) {
         super(id);
@@ -159,7 +159,6 @@ public boolean equals(Object o) {
                 .append(length, that.length)
                 .append(width, that.width)
                 .append(height, that.height)
-                .append(boundingBox, that.boundingBox)
                 .isEquals();
     }
 
@@ -174,7 +173,6 @@ public int hashCode() {
                 .append(length)
                 .append(width)
                 .append(height)
-                .append(boundingBox)
                 .toHashCode();
     }
 

diff --git a/...e/mosaic/fed/application/ambassador/simulation/perception/SimplePerceptionModuleTest.java b/...e/mosaic/fed/application/ambassador/simulation/perception/SimplePerceptionModuleTest.java
@@ -15,6 +15,7 @@
 
 package org.eclipse.mosaic.fed.application.ambassador.simulation.perception;
 
+import static junit.framework.TestCase.assertTrue;
 import static org.junit.Assert.assertEquals;
 import static org.mockito.Mockito.doReturn;
 import static org.mockito.Mockito.mock;
@@ -29,6 +30,7 @@
 import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.VehicleObject;
 import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.providers.TrafficLightTree;
 import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.providers.VehicleGrid;
+import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.providers.VehicleIndex;
 import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.providers.VehicleTree;
 import org.eclipse.mosaic.fed.application.config.CApplicationAmbassador;
 import org.eclipse.mosaic.lib.geo.CartesianPoint;
@@ -61,6 +63,7 @@
 import java.util.HashMap;
 import java.util.List;
 import java.util.Map;
+import java.util.stream.Collectors;
 
 @RunWith(Parameterized.class)
 public class SimplePerceptionModuleTest {
@@ -106,11 +109,20 @@ public void setup() {
                 .thenReturn(new CartesianRectangle(new MutableCartesianPoint(100, 90, 0), new MutableCartesianPoint(310, 115, 0)));
         SimulationKernel.SimulationKernel.setConfiguration(new CApplicationAmbassador());
 
+        VehicleIndex vehicleIndex;
+        switch (vehicleIndexType) {
+            case "tree":
+                vehicleIndex = new VehicleTree(20, 12);
+                break;
+            case "grid":
+                vehicleIndex = new VehicleGrid(5, 5);
+                break;
+            default:
+                vehicleIndex = null;
+        }
+
         trafficObjectIndex = new TrafficObjectIndex.Builder(mock((Logger.class)))
-                .withVehicleIndex(
-                        vehicleIndexType.equals("tree") ? new VehicleTree(20, 12) :
-                        vehicleIndexType.equals("grid") ? new VehicleGrid(5, 5) : null
-                )
+                .withVehicleIndex(vehicleIndex)
                 .withTrafficLightIndex(new TrafficLightTree(20))
                 .build();
         // setup cpc
@@ -187,6 +199,33 @@ public void vehicleCanBePerceived_FarRight_270viewingAngle() {
         assertEquals(1, simplePerceptionModule.getPerceivedVehicles().size());
     }
 
+    @Test
+    public void vehicleCanBeRemoved() {
+        simplePerceptionModule.enable(new SimplePerceptionConfiguration.Builder(360d, 1000d).build());
+        List<VehicleData> addedVehicles = setupVehicles(
+                new MutableCartesianPoint(105, 90, 0),
+                new MutableCartesianPoint(105, 90, 0),
+                new MutableCartesianPoint(105, 90, 0)
+        );
+        // assert all vehicles have been added
+        assertEquals(3, trafficObjectIndex.getNumberOfVehicles());
+        assertEquals(3, simplePerceptionModule.getPerceivedVehicles().size());
+        // modify vehicle objects
+        List<VehicleData> adjustedVehicles = addedVehicles.stream()
+                .map(vehicleData ->
+                        new VehicleData.Builder(vehicleData.getTime(), vehicleData.getName())
+                                .copyFrom(vehicleData)
+                                .movement(1d, 1d, 1d) // adjust vehicles
+                                .create())
+                .collect(Collectors.toList());
+        trafficObjectIndex.updateVehicles(adjustedVehicles);
+        // try to remove vehicles
+        trafficObjectIndex.removeVehicles(addedVehicles.stream().map(VehicleData::getName).collect(Collectors.toList()));
+        // assert vehicles have been properly removed
+        assertEquals(0, trafficObjectIndex.getNumberOfVehicles());
+        assertTrue(simplePerceptionModule.getPerceivedVehicles().isEmpty());
+    }
+
     @Test
     public void trafficLightsCanBePerceived() {
         setupTrafficLights(new MutableCartesianPoint(110, 100, 0));
@@ -196,7 +235,7 @@ public void trafficLightsCanBePerceived() {
         assertEquals(TrafficLightState.GREEN, simplePerceptionModule.getTrafficLightsInRange().get(0).getTrafficLightState());
     }
 
-    private void setupVehicles(CartesianPoint... positions) {
+    private List<VehicleData> setupVehicles(CartesianPoint... positions) {
         List<VehicleData> vehiclesInIndex = new ArrayList<>();
         int i = 1;
         for (CartesianPoint position : positions) {
@@ -213,6 +252,7 @@ private void setupVehicles(CartesianPoint... positions) {
             trafficObjectIndex.registerVehicleType(vehicleName, vehicleType);
         }
         trafficObjectIndex.updateVehicles(vehiclesInIndex);
+        return vehiclesInIndex;
     }
 
     private void setupTrafficLights(CartesianPoint... positions) {

diff --git a/lib/mosaic-geomath/src/main/java/org/eclipse/mosaic/lib/spatial/Grid.java b/lib/mosaic-geomath/src/main/java/org/eclipse/mosaic/lib/spatial/Grid.java
@@ -19,6 +19,7 @@
 import java.util.HashMap;
 import java.util.List;
 import java.util.Map;
+import java.util.Objects;
 import java.util.function.Predicate;
 
 /**
@@ -37,7 +38,7 @@ public class Grid<T> {
     private final double maxZ;
 
     private final List<List<GridCell<T>>> grid;
-    private final Map<T, CellIndex> items = new HashMap<>();
+    private final Map<ItemKey<T>, CellIndex> items = new HashMap<>();
 
     private final CellIndex tmpIndexA = new CellIndex();
     private final CellIndex tmpIndexB = new CellIndex();
@@ -118,7 +119,7 @@ public List<T> getItemsInBoundingArea(BoundingBox area, Predicate<T> filter, Lis
     public boolean addItem(T item) {
         synchronized (tmpIndexA) {
             CellIndex newCellIndex = toCellIndex(adapter.getCenterX(item), adapter.getCenterZ(item), new CellIndex());
-            CellIndex oldCellIndex = items.put(item, newCellIndex);
+            CellIndex oldCellIndex = items.put(getItemKey(item), newCellIndex);
             if (oldCellIndex != null) {
                 getGridCell(oldCellIndex).remove(item);
                 return false;
@@ -130,22 +131,22 @@ public boolean addItem(T item) {
 
     public void updateGrid() {
         synchronized (tmpIndexA) {
-            items.forEach((item, currentIndex) -> {
-                CellIndex newCellIndex = toCellIndex(adapter.getCenterX(item), adapter.getCenterZ(item), tmpIndexA);
+            items.forEach((key, currentIndex) -> {
+                CellIndex newCellIndex = toCellIndex(adapter.getCenterX(key.item), adapter.getCenterZ(key.item), tmpIndexA);
                 if (newCellIndex.isEqualTo(currentIndex)) {
                     // no index change -> do nothing
                     return;
                 }
-                getGridCell(currentIndex).remove(item);
+                getGridCell(currentIndex).remove(key.item);
                 currentIndex.set(newCellIndex);
-                getGridCell(currentIndex).add(item);
+                getGridCell(currentIndex).add(key.item);
             });
         }
     }
 
     public void removeItem(T item) {
         synchronized (tmpIndexA) {
-            CellIndex cellIndex = items.remove(item);
+            CellIndex cellIndex = items.remove(getItemKey(item));
             if (cellIndex != null) {
                 getGridCell(cellIndex).remove(item);
             }
@@ -167,6 +168,10 @@ private GridCell<T> getGridCell(int col, int row) {
         return grid.get(col).get(row);
     }
 
+    private ItemKey<T> getItemKey(T item) {
+        return new ItemKey<>(adapter.getItemHash(item), item);
+    }
+
     private static class GridCell<T> extends ArrayList<T> {
         // marker class for better readability
         // we use an arrayList since efficiently iterating over all elements is more important than remove/add
@@ -186,4 +191,31 @@ public boolean isEqualTo(CellIndex other) {
             return other != null && this.row == other.row && this.col == other.col;
         }
     }
+
+    private static class ItemKey<T> {
+
+        private final T item;
+        private final Integer hash;
+
+        public ItemKey(Integer hash, T item) {
+            this.hash = hash;
+            this.item = item;
+        }
+
+        @Override
+        public boolean equals(Object o) {
+            if (this == o) {
+                return true;
+            }
+            if (o == null || getClass() != o.getClass()) {
+                return false;
+            }
+            return Objects.equals(((ItemKey<?>) o).hash, this.hash);
+        }
+
+        @Override
+        public int hashCode() {
+            return hash;
+        }
+    }
 }
diff --git a/lib/mosaic-geomath/src/main/java/org/eclipse/mosaic/lib/spatial/QuadTree.java b/lib/mosaic-geomath/src/main/java/org/eclipse/mosaic/lib/spatial/QuadTree.java
@@ -18,7 +18,6 @@
 import org.eclipse.mosaic.lib.math.Vector3d;
 
 import java.util.ArrayList;
-import java.util.Collection;
 import java.util.HashMap;
 import java.util.List;
 import java.util.Map;
@@ -40,7 +39,7 @@ public class QuadTree<T> {
 
     private final TreeNode root;
 
-    private final Map<T, ObjectAndNode> objects = new HashMap<>();
+    private final Map<Integer, ObjectAndNode> objects = new HashMap<>();
     private final SpatialItemAdapter<T> adapter;
 
     /**
@@ -164,22 +163,18 @@ public int getSize() {
         return root.objectsCount;
     }
 
-    public Collection<T> getAllObjects() {
-        return objects.keySet();
-    }
-
     public boolean addItem(T item) {
         ObjectAndNode oan = new ObjectAndNode(item);
         if (root.isInBounds(oan.objectPos)) {
-            objects.put(item, oan);
+            objects.put(adapter.getItemHash(item), oan);
             root.addObjectNode(oan);
             return true;
         }
         return false;
     }
 
     public void removeObject(T object) {
-        ObjectAndNode oan = objects.remove(object);
+        ObjectAndNode oan = objects.remove(adapter.getItemHash(object));
         if (oan != null) {
             root.removeObjectNode(oan);
         }

diff --git a/lib/mosaic-geomath/src/main/java/org/eclipse/mosaic/lib/spatial/SpatialItemAdapter.java b/lib/mosaic-geomath/src/main/java/org/eclipse/mosaic/lib/spatial/SpatialItemAdapter.java
@@ -19,6 +19,11 @@
 import org.eclipse.mosaic.lib.math.Vector3d;
 
 public interface SpatialItemAdapter<T> {
+
+    default int getItemHash(T item) {
+        return item.hashCode();
+    }
+
     double getMinX(T item);
 
     double getMinY(T item);
@@ -65,6 +70,7 @@ default void setNode(T item, SpatialTree<T>.Node node) {
     }
 
     class PointAdapter<T extends Vector3d> implements SpatialItemAdapter<T> {
+
         @Override
         public double getMinX(T item) {
             return item.x;