feat(perception): Use 2D bounding boxes for Occlusion Modifiers

fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/AbstractPerceptionModule.java

@@ -112,7 +112,7 @@ public List<SpatialObject> getPerceivedObjects() {
 
     private <T extends SpatialObject<T>> List<T> applyPerceptionModifiers(List<T> objectsInRange) {
         List<T> filteredList = new ArrayList<>(objectsInRange);
-        // create copy of all perceived objects to avoid interference with modifiers of other perception modules.
+        // create a copy of all perceived objects to avoid interference with modifiers of other perception modules.
         filteredList.replaceAll(T::copy);
         for (PerceptionModifier perceptionModifier : configuration.getPerceptionModifiers()) {
             filteredList = perceptionModifier.apply(owner, filteredList); // apply filters in sequence

fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/SumoPerceptionModule.java

@@ -56,6 +56,7 @@ List<VehicleObject> getVehiclesInRange() {
                         .setSpeed(v.getSpeed())
                         .setHeading(v.getHeading())
                         .setDimensions(v.getLength(), v.getWidth(), v.getHeight())
+                        .resetBoundingBox()
                 ).collect(Collectors.toList());
     }
 

fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java

@@ -0,0 +1,172 @@
+/*
+ * Copyright (c) 2023 Fraunhofer FOKUS and others. All rights reserved.
+ *
+ * See the NOTICE file(s) distributed with this work for additional
+ * information regarding copyright ownership.
+ *
+ * This program and the accompanying materials are made available under the
+ * terms of the Eclipse Public License 2.0 which is available at
+ * http://www.eclipse.org/legal/epl-2.0
+ *
+ * SPDX-License-Identifier: EPL-2.0
+ *
+ * Contact: [email protected]
+ */
+
+package org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels;
+
+import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.PerceptionModuleOwner;
+import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.SpatialObject;
+import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.SpatialObjectBoundingBox;
+import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.TrafficLightObject;
+import org.eclipse.mosaic.lib.math.MathUtils;
+import org.eclipse.mosaic.lib.math.Vector3d;
+import org.eclipse.mosaic.lib.math.VectorUtils;
+import org.eclipse.mosaic.lib.spatial.Edge;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.stream.Collectors;
+
+public class BoundingBoxOcclusionModifier implements PerceptionModifier {
+
+    private final Vector3d intersectionResult = new Vector3d();
+    /**
+     * This defines how many equidistant points shall be evaluated per edge of a vehicle.
+     * Note generally for the front and rear edge this will result in a higher resolution compared to the sides of the vehicle.
+     * Default: 2
+     */
+    private final int pointsPerSide;
+
+    /**
+     * Threshold that defines how many of the points defined through {@link #pointsPerSide} need to be visible in order for a
+     * object to be treated as detected.
+     * Default: 2
+     */
+    private final int detectionThreshold;
+
+    /**
+     * Default constructor for the {@link BoundingBoxOcclusionModifier}.
+     */
+    public BoundingBoxOcclusionModifier() {
+        this.pointsPerSide = 2;
+        this.detectionThreshold = 2;
+    }
+
+    /**
+     * Constructor for {@link BoundingBoxOcclusionModifier}, validates and sets
+     * the parameters {@link #pointsPerSide} and {@link #detectionThreshold}.
+     *
+     * @param pointsPerSide      the amount of points that will be evaluated per object (corners count towards 2 edges)
+     * @param detectionThreshold how many points have to be visible in order for an object to be treated as detected
+     */
+    public BoundingBoxOcclusionModifier(int pointsPerSide, int detectionThreshold) {
+        if (pointsPerSide < 2) {
+            throw new RuntimeException("Need at least 2 points per edge, meaning every corner will be checked for occlusion.");
+        }
+        if (detectionThreshold < 1) {
+            throw new RuntimeException("At least one point has to be checked for occlusion, else no objects will be occluded");
+        }
+        if (detectionThreshold > pointsPerSide * 4 - 4) {
+            throw new RuntimeException("The detection threshold exceeds the number of points evaluated per object");
+        }
+        this.pointsPerSide = pointsPerSide;
+        this.detectionThreshold = detectionThreshold;
+    }
+
+    @Override
+    public <T extends SpatialObject> List<T> apply(PerceptionModuleOwner owner, List<T> spatialObjects) {
+        List<T> newObjects = new ArrayList<>();
+        // the ego object cannot occlude vision
+        List<T> occludingObjects = spatialObjects.stream()
+                .filter(object -> !object.getId().equals(owner.getId()))
+                .collect(Collectors.toList());
+        for (T spatialObject : spatialObjects) {
+            if (spatialObject instanceof TrafficLightObject) { // Traffic Lights are treated to not be occluded
+                newObjects.add(spatialObject);
+                continue;
+            }
+            List<Vector3d> pointsToEvaluate = createPointsToEvaluate(spatialObject);
+            final int requiredVisiblePoints = pointsToEvaluate.size() == 1 ? 1 : detectionThreshold;
+            int numberOfPointsVisible = 0;
+            for (Vector3d point : pointsToEvaluate) {
+                boolean pointOccluded = false;
+                boundingBoxLoop:
+                for (T occludingObject : occludingObjects) {
+                    if (occludingObject.getId().equals(spatialObject.getId())) {
+                        continue; // cannot be occluded by itself
+                    }
+                    SpatialObjectBoundingBox boundingBox = occludingObject.getBoundingBox();
+                    // SpatialObjects with PointBoundingBoxes won't occlude anything, as they have no edges defined
+                    for (Edge<Vector3d> side : boundingBox.getAllEdges()) {
+                        boolean isOccluded = VectorUtils.computeXZEdgeIntersectionPoint(
+                                owner.getVehicleData().getProjectedPosition().toVector3d(),
+                                point, side.a, side.b, intersectionResult
+                        );
+                        if (isOccluded) {
+                            pointOccluded = true;
+                            break boundingBoxLoop;
+                        }
+                    }
+                }
+                if (!pointOccluded) {
+                    numberOfPointsVisible++;
+                }
+                if (numberOfPointsVisible == requiredVisiblePoints) {
+                    newObjects.add(spatialObject);
+                    break;
+                }
+            }
+        }
+        return newObjects;
+    }
+
+    /**
+     * Creates a list of all points that shall be tested for occlusion. If {@link #pointsPerSide} is set to a value larger than 2,
+     * each side will have additional equidistant points added.
+     * Example for {@code pointsPerSide = 3} (x's are the corners which will be evaluated anyway, o's are the added points):
+     * <pre>
+     *     x-----o-----x
+     *     |           |
+     *     |           |
+     *     o           o
+     *     |           |
+     *     |           |
+     *     x-----o-----y
+     * </pre>
+     * @param spatialObject a {@link SpatialObject} for which the occlusion should be evaluated
+     */
+    private <T extends SpatialObject> List<Vector3d> createPointsToEvaluate(T spatialObject) {
+        List<Vector3d> pointsToEvaluate = new ArrayList<>();
+        SpatialObjectBoundingBox boundingBox = spatialObject.getBoundingBox();
+        // if object has edges and more than 2 points per side are to be evaluated, calculate points that have to be evaluated
+        if (pointsPerSide > 2 && !boundingBox.getAllEdges().isEmpty()) {
+            for (Edge<Vector3d> edge : boundingBox.getAllEdges()) {
+                Vector3d start = edge.a;
+                if (pointNotPresent(pointsToEvaluate, start)) {
+                    pointsToEvaluate.add(start);
+                }
+                Vector3d end = edge.b;
+                if (pointNotPresent(pointsToEvaluate, end)) {
+                    pointsToEvaluate.add(end);
+                }
+                for (int i = 1; i < pointsPerSide - 1; i++) {
+                    double ratio = (double) i / (pointsPerSide + 1);
+                    double xNew = start.x + ratio * (end.x - start.x);
+                    double zNew = start.z + ratio * (end.z - start.z);
+                    Vector3d newPoint = new Vector3d(xNew, 0, zNew);
+                    if (pointNotPresent(pointsToEvaluate, newPoint)) {
+                        pointsToEvaluate.add(newPoint);
+                    }
+                }
+            }
+        } else { // else just add all corners
+            pointsToEvaluate.addAll(boundingBox.getAllCorners());
+        }
+        return pointsToEvaluate;
+    }
+
+    private boolean pointNotPresent(List<Vector3d> points, Vector3d newPoint) {
+        return points.stream().noneMatch(vector3d -> vector3d.isFuzzyEqual(newPoint));
+    }
+}

fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/DistanceModifier.java

@@ -52,7 +52,7 @@ public DistanceModifier(RandomNumberGenerator rng, double offset) {
 
     @Override
     public <T extends SpatialObject> List<T> apply(PerceptionModuleOwner owner, List<T> spatialObjects) {
-        if (spatialObjects.size() == 0) {
+        if (spatialObjects.isEmpty()) {
             return spatialObjects;
         }
         Vector3d ownerPosition = owner.getVehicleData().getProjectedPosition().toVector3d();

fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PositionErrorModifier.java

@@ -17,6 +17,7 @@
 
 import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.PerceptionModuleOwner;
 import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.SpatialObject;
+import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.VehicleObject;
 import org.eclipse.mosaic.lib.math.RandomNumberGenerator;
 import org.eclipse.mosaic.lib.math.Vector3d;
 import org.eclipse.mosaic.lib.math.VectorUtils;
@@ -85,7 +86,7 @@ public <T extends SpatialObject> List<T> apply(PerceptionModuleOwner owner, List
         double angleToNorth = ownerDirection.angle(VectorUtils.NORTH);
         spatialObjects.forEach(
                 spatialObject -> {
-                    Vector3d relativePosition = getVectorRelativeTo(ownerPosition, spatialObject.getPosition()); // get position relative to owner
+                    Vector3d relativePosition = getVectorRelativeTo(ownerPosition, spatialObject.getPosition()); // pos relative to owner
                     Vector3d adjustedVector = new Vector3d(relativePosition);
                     adjustedVector.rotate(-angleToNorth, VectorUtils.UP); // rotate vector according to orientation
                     // add lateral and longitudinal errors

fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/SimpleOcclusionModifier.java

@@ -63,7 +63,7 @@ public SimpleOcclusionModifier(double minDetectionAngle, double maxDetectionAngl
 
     @Override
     public <T extends SpatialObject> List<T> apply(PerceptionModuleOwner owner, List<T> spatialObjects) {
-        if (spatialObjects.size() == 0) {
+        if (spatialObjects.isEmpty()) {
             return spatialObjects;
         }
         Vector3d ownerPosition = owner.getVehicleData().getProjectedPosition().toVector3d();

fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusionModifier.java

@@ -33,37 +33,55 @@
  */
 public class WallOcclusionModifier implements PerceptionModifier {
 
+    private final int requiredVisibleVehicleCorners;
     private final Vector3d intersectionResult = new Vector3d();
 
+    public WallOcclusionModifier() {
+        requiredVisibleVehicleCorners = 2;
+    }
+
+    public WallOcclusionModifier(int requiredVisibleVehicleCorners) {
+        this.requiredVisibleVehicleCorners = requiredVisibleVehicleCorners;
+    }
+
     @Override
     public <T extends SpatialObject> List<T> apply(PerceptionModuleOwner owner, List<T> spatialObjects) {
-        if (spatialObjects.size() == 0) {
+        if (spatialObjects.isEmpty()) {
             return spatialObjects;
         }
 
         final Collection<Edge<Vector3d>> walls = owner.getPerceptionModule().getSurroundingWalls();
         if (walls.isEmpty()) {
             return spatialObjects;
         }
-        final Vector3d ownerPosition = owner.getVehicleData().getProjectedPosition().toVector3d();
-        final Vector3d otherPosition = new Vector3d();
-
         final List<T> result = new ArrayList<>();
-
-        vehicleLoop:
         for (T spatialObject : spatialObjects) {
-            spatialObject.getProjectedPosition().toVector3d(otherPosition);
-
-            for (Edge<Vector3d> wall : walls) {
-                boolean isHidden =
-                        VectorUtils.computeXZEdgeIntersectionPoint(ownerPosition, otherPosition, wall.a, wall.b, intersectionResult);
-                if (isHidden) {
-                    continue vehicleLoop;
+            List<Vector3d> pointsToEvaluate = spatialObject.getBoundingBox().getAllCorners();
+            // we say that at least half of the corners have to be visible rounding up for odd numbers
+            final int requiredVisiblePoints = (int) Math.ceil((double) pointsToEvaluate.size() / 2);
+            int numberOfPointsVisible = 0;
+            for (Vector3d point : pointsToEvaluate) {
+                boolean pointOccluded = false;
+                for (Edge<Vector3d> wall : walls) {
+                    // SpatialObjects with PointBoundingBoxes won't occlude anything, as they have no edges defined
+                    boolean isOccluded = VectorUtils.computeXZEdgeIntersectionPoint(
+                            owner.getVehicleData().getProjectedPosition().toVector3d(),
+                            point, wall.a, wall.b, intersectionResult
+                    );
+                    if (isOccluded) {
+                        pointOccluded = true;
+                        break;
+                    }
+                }
+                if (!pointOccluded) {
+                    numberOfPointsVisible++;
+                }
+                if (numberOfPointsVisible == requiredVisiblePoints) {
+                    result.add(spatialObject);
+                    break;
                 }
             }
-            result.add(spatialObject);
         }
-
         return result;
     }
 

fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/PointBoundingBox.java

@@ -0,0 +1,51 @@
+/*
+ * Copyright (c) 2023 Fraunhofer FOKUS and others. All rights reserved.
+ *
+ * See the NOTICE file(s) distributed with this work for additional
+ * information regarding copyright ownership.
+ *
+ * This program and the accompanying materials are made available under the
+ * terms of the Eclipse Public License 2.0 which is available at
+ * http://www.eclipse.org/legal/epl-2.0
+ *
+ * SPDX-License-Identifier: EPL-2.0
+ *
+ * Contact: [email protected]
+ */
+
+package org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects;
+
+import org.eclipse.mosaic.lib.math.Vector3d;
+import org.eclipse.mosaic.lib.spatial.Edge;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * This bounding box represents
+ */
+public class PointBoundingBox implements SpatialObjectBoundingBox {
+
+    /**
+     * A one element list, defining the position of a {@link SpatialObject} as the only corner.
+     */
+    private final List<Vector3d> allCorners = new ArrayList<>();
+    /**
+     * An empty list as a no bounding box is spanned by a single point.
+     */
+    private final List<Edge<Vector3d>> allEdges = new ArrayList<>();
+
+    public PointBoundingBox(Vector3d position) {
+        allCorners.add(position);
+    }
+
+    @Override
+    public List<Vector3d> getAllCorners() {
+        return allCorners;
+    }
+
+    @Override
+    public List<Edge<Vector3d>> getAllEdges() {
+        return allEdges;
+    }
+}

fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObject.java

@@ -25,14 +25,16 @@ public abstract class SpatialObject<T extends SpatialObject<T>> extends Vector3d
 
     private final String id;
 
-    final MutableCartesianPoint cartesianPosition = new MutableCartesianPoint();
+    protected final MutableCartesianPoint cartesianPosition = new MutableCartesianPoint();
 
     public SpatialObject(String id) {
         this.id = id;
     }
 
     public abstract T setPosition(CartesianPoint position);
 
+    public abstract SpatialObjectBoundingBox getBoundingBox();
+
     /**
      * Sets the position of the {@link SpatialObject}.
      *