perf(perception): improved speed of Wall Occlusion (#443)

* achieved by a) using a more sensible pre-selection of walls
* b) not using the 3D intersections eval from VectorUtils

---------

Signed-off-by: Moritz Schweppenhäuser <[email protected]>

Author: schwepmo

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

@@ -34,8 +34,6 @@
  */
 public class WallOcclusion implements PerceptionModifier {
 
-    private final Vector3d intersectionResult = new Vector3d();
-
     @Override
     public <T extends SpatialObject> List<T> apply(PerceptionModuleOwner owner, List<T> spatialObjects) {
         if (spatialObjects.isEmpty()) {
@@ -46,6 +44,7 @@ public <T extends SpatialObject> List<T> apply(PerceptionModuleOwner owner, List
         if (walls.isEmpty()) {
             return spatialObjects;
         }
+        Vector3d ownerPosition = owner.getVehicleData().getProjectedPosition().toVector3d();
         final List<T> result = new ArrayList<>();
         for (T spatialObject : spatialObjects) {
             List<Vector3d> pointsToEvaluate = spatialObject.getBoundingBox().getAllCorners();
@@ -56,11 +55,7 @@ public <T extends SpatialObject> List<T> apply(PerceptionModuleOwner owner, List
                 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) {
+                    if (VectorUtils.doesXZIntersect(ownerPosition, point, wall.a, wall.b)) {
                         pointOccluded = true;
                         break;
                     }
@@ -76,5 +71,4 @@ public <T extends SpatialObject> List<T> apply(PerceptionModuleOwner owner, List
         }
         return result;
     }
-
 }

fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/providers/WallTree.java

@@ -29,7 +29,10 @@
 import java.util.List;
 
 public class WallTree extends WallIndex {
-
+    /**
+     * The longest wall, relevant for tree setup, so that all walls will be included. [m]
+     */
+    private double maxWallLength = 50;
     private final int bucketSize;
 
     private KdTree<Edge<Vector3d>> wallTree;
@@ -42,22 +45,28 @@ public WallTree(int bucketSize) {
     @Override
     public void initialize() {
         List<Edge<Vector3d>> walls = new ArrayList<>();
+        double maxWallLength = 0;
         for (Building building : super.getDatabase().getBuildings()) {
             for (Wall wall : building.getWalls()) {
                 walls.add(new Edge<>(
                         wall.getFromCorner().getCartesianPosition().toVector3d(),
                         wall.getToCorner().getCartesianPosition().toVector3d()
                 ));
+                if (wall.getLength() > maxWallLength) {
+                    maxWallLength = wall.getLength();
+                }
             }
         }
+        this.maxWallLength = maxWallLength;
         wallTree = new KdTree<>(new SpatialItemAdapter.EdgeAdapter<>(), walls, bucketSize);
         wallTraverser = new org.eclipse.mosaic.lib.database.spatial.Edge.InRadius<>();
     }
 
     @Override
     public Collection<Edge<Vector3d>> getSurroundingWalls(PerceptionModel perceptionModel) {
+        // overestimating the initial list of walls by extending max bounding box radius with maximal wall length
         wallTraverser.setup(perceptionModel.getBoundingBox().center,
-                perceptionModel.getBoundingBox().center.distanceSqrTo(perceptionModel.getBoundingBox().min)); // overestimating distance
+                perceptionModel.getBoundingBox().center.distanceTo(perceptionModel.getBoundingBox().min) + maxWallLength);
         wallTraverser.traverse(wallTree);
         return wallTraverser.getResult();
     }

fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java

@@ -192,7 +192,7 @@ public void testHeadingModifier() {
 
         // ASSERT if headings differ from ground truth
         for (VehicleObject realVehicle : getAllVehicles()) {
-            for (VehicleObject perceivedVehicle: perceivedVehicles) {
+            for (VehicleObject perceivedVehicle : perceivedVehicles) {
                 if (realVehicle.getId().equals(perceivedVehicle.getId())) {
                     assertNotEquals(realVehicle.getHeading(), perceivedVehicle.getHeading(), 0.0001);
                     // when adjusting heading the position should change too, since it currently points to the front bumper of the vehicle
@@ -204,7 +204,7 @@ public void testHeadingModifier() {
 
     @Test
     public void testDimensionsModifier() {
-         DimensionsModifier dimensionsModifier = new DimensionsModifier(rng, 1.0, 0.0, 0.0);
+        DimensionsModifier dimensionsModifier = new DimensionsModifier(rng, 1.0, 0.0, 0.0);
         simplePerceptionModule.enable(
                 new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE).addModifier(dimensionsModifier).build()
         );
@@ -217,7 +217,7 @@ public void testDimensionsModifier() {
 
         // ASSERT if headings differ from ground truth
         for (VehicleObject realVehicle : getAllVehicles()) {
-            for (VehicleObject perceivedVehicle: perceivedVehicles) {
+            for (VehicleObject perceivedVehicle : perceivedVehicles) {
                 if (realVehicle.getId().equals(perceivedVehicle.getId())) {
                     assertNotEquals(realVehicle.getLength(), perceivedVehicle.getLength(), 0.0001);
                     // when adjusting length the position should change too, since it currently points to the front bumper of the vehicle
@@ -270,9 +270,7 @@ public void testIndexedObjectsNotChanged() {
                 .stream().collect(Collectors.toMap(VehicleObject::getId, v -> new Vector3d(v.getPosition())));
 
         // ASSERT that all positions in the index are still the same as before applying modifier
-        allVehiclesInIndexPre.forEach((id, pos) -> {
-            assertTrue(pos.isFuzzyEqual(allVehiclesInIndexPost.get(id)));
-        });
+        allVehiclesInIndexPre.forEach((id, pos) -> assertTrue(pos.isFuzzyEqual(allVehiclesInIndexPost.get(id))));
 
         // ASSERT that all modified positions differ from the positions before (or after) applying the modifier
         for (VehicleObject object : perceivedAndAlteredObjects) {

lib/mosaic-geomath/src/main/java/org/eclipse/mosaic/lib/math/VectorUtils.java

@@ -69,6 +69,69 @@ public static Vector3d nearestPointOnLine(Vector3d point, Vector3d linePt1, Vect
         return result;
     }
 
+    /**
+     * Enum to differentiate between orientations of a point relative to a line segment.
+     */
+    private enum Orientation2d {
+        COLLINEAR, CLOCKWISE, COUNTERCLOCKWISE
+    }
+
+    /**
+     * Function to check if two line segments (a1, b1) and (a2, b2) intersect.
+     *
+     * @param a1 First point of first line segment
+     * @param b1 Second point of first line segment
+     * @param a2 First point of second line segment
+     * @param b2 Second point of second line segment
+     * @return {@code true} if the two line segments intersect, else {@code false}
+     */
+    public static boolean doesXZIntersect(Vector3d a1, Vector3d b1, Vector3d a2, Vector3d b2) {
+        Orientation2d o1 = isCollinear(a1.x, a1.z, b1.x, b1.z, a2.x, a2.z);
+        Orientation2d o2 = isCollinear(a1.x, a1.z, b1.x, b1.z, b2.x, b2.z);
+        Orientation2d o3 = isCollinear(a2.x, a2.z, b2.x, b2.z, a1.x, a1.z);
+        Orientation2d o4 = isCollinear(a2.x, a2.z, b2.x, b2.z, b1.x, b1.z);
+
+        // General case
+        if (o1 != o2 && o3 != o4) {
+            return true;
+        }
+        // Special cases
+        if (o1 == Orientation2d.COLLINEAR && isOnSegment(a1.x, a1.z, a2.x, a2.z, b1.x, b1.z)) {
+            return true;
+        }
+        if (o2 == Orientation2d.COLLINEAR && isOnSegment(a1.x, a1.z, b2.x, b2.z, b1.x, b1.z)) {
+            return true;
+        }
+        if (o3 == Orientation2d.COLLINEAR && isOnSegment(a2.x, a2.z, a1.x, a1.z, b2.x, b2.z)) {
+            return true;
+        }
+        if (o4 == Orientation2d.COLLINEAR && isOnSegment(a2.x, a2.z, b1.x, b1.z, b2.x, b2.z)) {
+            return true;
+        }
+        return false; // Doesn't fall in any of the above cases
+    }
+
+    /**
+     * Function to calculate the orientation of the triplet (ax, ay), (bx,bqy), (rx, ry).
+     */
+    private static Orientation2d isCollinear(double ax, double ay, double bx, double by, double rx, double ry) {
+        double val = (by - ay) * (rx - bx) - (bx - ax) * (ry - by);
+        if (Math.abs(val) <= MathUtils.EPSILON_D) { // Collinear
+            return Orientation2d.COLLINEAR;
+        }
+        return (val > 0) ? Orientation2d.CLOCKWISE : Orientation2d.COUNTERCLOCKWISE; // Clockwise or Counterclockwise
+    }
+
+    /**
+     * Function to check if point (ax, ay) lies on a segment (bx, by) to (rx, ry).
+     */
+    private static boolean isOnSegment(double ax, double ay, double bx, double by, double rx, double ry) {
+        return bx <= Math.max(ax, rx)
+                && bx >= Math.min(ax, rx)
+                && by <= Math.max(ay, ry)
+                && by >= Math.min(ay, ry);
+    }
+
     /**
      * Returns the intersection point of two lines in the XZ plane. Notice that, although 3D
      * coordinates are used, only the X and Z components are considered. The Y component of the