feat: introduce class for point cloud

lib/mosaic-geomath/src/main/java/org/eclipse/mosaic/lib/spatial/PointCloud.java

@@ -0,0 +1,288 @@
+/*
+ * Copyright (c) 2024 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.lib.spatial;
+
+import org.eclipse.mosaic.lib.math.Matrix3d;
+import org.eclipse.mosaic.lib.math.Vector3d;
+
+import java.io.Serializable;
+import java.util.List;
+import java.util.function.Predicate;
+import java.util.stream.Collectors;
+
+/**
+ * This class represents a point cloud based on the {@link Vector3d} coordinate system.
+ * <ul> <li>It consists of a reference point which represents the origin/center of this point cloud
+ * in absolute world coordinates.</li>
+ * <li>A rotation matrix used to translate relative end points of the point cloud to absolute coordinates.</li>
+ * <li>A list of points (x,y,z, hit type, distance to origin) forming this point cloud.</li>
+ * <li>Use {@link Relative} to create a point cloud by a given list of points in relative coordinates.
+ *     When {@link #getAbsoluteEndPoints} is called on such object, a coordinate transformation is done (and cached).</li>
+ * <li>Use {@link Absolute} to create a point cloud by a given list of points in absolute coordinates.
+ *     When {@link #getRelativeEndPoints()} is called on such object, a coordinate transformation is done (and cached).</li>
+ * </ul>
+ */
+public abstract class PointCloud implements Serializable {
+
+    private static final long serialVersionUID = 1L;
+
+    private final Vector3d reference;
+    private final RotationMatrix rotation;
+    private final long timestamp;
+    private final double minRange;
+    private final double maxRange;
+
+    protected final List<Point> points;
+
+    protected PointCloud(RotationMatrix rotation, Vector3d reference, List<Point> points,
+                         long timestamp, double minRange, double maxRange) {
+        this.rotation = rotation;
+        this.reference = reference;
+        this.points = points;
+        this.timestamp = timestamp;
+        this.minRange = minRange;
+        this.maxRange = maxRange;
+    }
+
+    /**
+     * Relative coordinates mean the use of a cartesian coordinate system with its origin (0,0,0) at
+     * the {@link PointCloud}'s reference, returned by {@link #getReference()}. In addition, the
+     * coordinate system is rotated. See {@link #getRotation()} for details.
+     *
+     * @return end points of all rays of this {@link PointCloud} in relative coordinates
+     * @see #getReference() origin of ray / translation of coordinates
+     * @see #getRotation() rotation of coordinates
+     */
+    public abstract List<Point> getRelativeEndPoints();
+
+
+    /**
+     * @return end points of all rays of this {@link PointCloud} that hit something in relative coordinates
+     */
+    public abstract List<Point> getRelativeEndPointsWithHit();
+
+    /**
+     * @return end points of all rays of this {@link PointCloud} in absolute coordinates
+     */
+    public abstract List<Point> getAbsoluteEndPoints();
+
+    /**
+     * @return end points of all rays of this {@link PointCloud} that hit something in absolute coordinates
+     */
+    public abstract List<Point> getAbsoluteEndPointsWithHit();
+
+    /**
+     * A {@link PointCloud} is represented by a constant origin and their end points
+     * returned by {@link #getRelativeEndPoints()} and {@link #getAbsoluteEndPoints()}
+     * Relative coordinates are translated and rotated absolute coordinates.
+     *
+     * @return origin of the rays forming this point cloud in absolute coordinates
+     * @see #getRotation()
+     */
+    public Vector3d getReference() {
+        return reference;
+    }
+
+    /**
+     * Ray end points in relative coordinates of this point cloud are translated and rotated end points in
+     * absolute coordinates.
+     * <p>
+     * Let o be the ray origin in absolute coordinates, let R be the rotation matrix. Thus, the transformation
+     * of relative coordinates r into absolute coordinates a is
+     * <code>a = R * r + o</code>
+     *
+     * @return rotation matrix R
+     * @see #getReference() returns <code>o</code>
+     */
+    public RotationMatrix getRotation() {
+        return rotation;
+    }
+
+    /**
+     * @return simulation time of {@link PointCloud}
+     */
+    public long getTimestamp() {
+        return timestamp;
+    }
+
+    /**
+     * @return sensor's minimum detectable range in distance units of Phabmacs
+     */
+    public double getMinRange() {
+        return minRange;
+    }
+
+    /**
+     * @return sensor's maximum detectable range in distance units of Phabmacs
+     */
+    public double getMaxRange() {
+        return maxRange;
+    }
+
+    /**
+     * A {@link Point} of the point cloud consists of its coordinates, an identifier
+     * of the type of object the point has hit, and the distance to the point cloud origin.
+     */
+    public static class Point extends Vector3d {
+
+        private final byte hitType;
+        private final float distance;
+
+        /**
+         *
+         * @param endPoint the coordinates of the point cloud
+         * @param distance the distance to the origin of the point cloud
+         * @param hitType the type of hit object represented by this point. 0 = no hit
+         */
+        public Point(Vector3d endPoint, float distance, byte hitType) {
+            x = endPoint.x;
+            y = endPoint.y;
+            z = endPoint.z;
+            this.distance = distance;
+            this.hitType = hitType;
+        }
+
+        /**
+         * @return true if the ray generating this {@link Point} has hit an object
+         */
+        public boolean hasHit() {
+            return hitType != 0;
+        }
+
+        /**
+         * @return the type of the object the ray generating this point has hit. (0 = no hit)
+         */
+        public byte getHitType() {
+            return hitType;
+        }
+
+        /**
+         * @return the distance to the origin of point cloud this point belongs to
+         */
+        public float getDistance() {
+            return distance;
+        }
+    }
+
+    public static class Absolute extends PointCloud {
+        private transient List<Point> absoluteEndPointsWithHit = null;
+        private transient List<Point> relativeEndPoints = null;
+        private transient List<Point> relativeEndPointsWithHit = null;
+
+        public Absolute(RotationMatrix rotation, Vector3d reference, List<Point> absoluteEndPoints,
+                        long timestamp, double minRange, double maxRange) {
+            super(rotation, reference, absoluteEndPoints, timestamp, minRange, maxRange);
+        }
+
+        @Override
+        public List<Point> getAbsoluteEndPoints() {
+            return points;
+        }
+
+        @Override
+        public List<Point> getAbsoluteEndPointsWithHit() {
+            if (absoluteEndPointsWithHit == null) {
+                absoluteEndPointsWithHit = getAbsoluteEndPoints().stream().filter(Point::hasHit).collect(Collectors.toList());
+            }
+            return absoluteEndPointsWithHit;
+        }
+
+        @Override
+        public List<Point> getRelativeEndPoints() {
+            if (relativeEndPoints == null) {
+                relativeEndPoints = absoluteToRelative(points, p -> true);
+            }
+            return relativeEndPoints;
+        }
+
+        @Override
+        public List<Point> getRelativeEndPointsWithHit() {
+            if (relativeEndPointsWithHit == null) {
+                relativeEndPointsWithHit = absoluteToRelative(points, Point::hasHit);
+            }
+            return relativeEndPointsWithHit;
+        }
+
+        /**
+         * Transforms list of {@link Point}s in absolute coordinates to relative coordinates.         *
+         *
+         * @param absoluteEndPoints end points in absolute coordinates
+         * @return end points in relative coordinates
+         */
+        private List<Point> absoluteToRelative(List<Point> absoluteEndPoints, Predicate<Point> filter) {
+            Matrix3d inv = getRotation().transpose(new Matrix3d());
+            return absoluteEndPoints
+                    .stream()
+                    .filter(filter)
+                    .map(point -> (Point) inv.multiply(point.subtract(getReference(), new Point(point, point.getDistance(), point.getHitType()))))
+                    .collect(Collectors.toList());
+        }
+    }
+
+    public static class Relative extends PointCloud {
+        private transient List<Point> relativeEndPointsWithHit = null;
+        private transient List<Point> absoluteEndPoints = null;
+        private transient List<Point> absoluteEndPointsWithHit = null;
+
+        public Relative(RotationMatrix rotation, Vector3d reference, List<Point> relativeEndPoints,
+                        long timestamp, double minRange, double maxRange) {
+            super(rotation, reference, relativeEndPoints, timestamp, minRange, maxRange);
+        }
+
+        @Override
+        public List<Point> getAbsoluteEndPoints() {
+            if (absoluteEndPoints == null) {
+                absoluteEndPoints = relativeToAbsolute(points, p -> true);
+            }
+            return absoluteEndPoints;
+        }
+
+        @Override
+        public List<Point> getAbsoluteEndPointsWithHit() {
+            if (absoluteEndPointsWithHit == null) {
+                absoluteEndPointsWithHit = relativeToAbsolute(points, Point::hasHit);
+            }
+            return absoluteEndPointsWithHit;
+        }
+
+        @Override
+        public List<Point> getRelativeEndPoints() {
+            return points;
+        }
+
+        @Override
+        public List<Point> getRelativeEndPointsWithHit() {
+            if (relativeEndPointsWithHit == null) {
+                relativeEndPointsWithHit = getRelativeEndPoints().stream().filter(Point::hasHit).collect(Collectors.toList());
+            }
+            return relativeEndPointsWithHit;
+        }
+
+        /**
+         * Transforms list of {@link Point}s in relative coordinates to absolute coordinates.
+         *
+         * @param relativeEndpoints end points in relative coordinates
+         * @return end points in absolute coordinates
+         */
+        private List<Point> relativeToAbsolute(List<Point> relativeEndpoints, Predicate<Point> filter) {
+            return relativeEndpoints
+                    .stream()
+                    .filter(filter)
+                    .map(point -> (Point) getRotation().multiply(new Point(point, point.getDistance(), point.getHitType())).add(getReference()))
+                    .collect(Collectors.toList());
+        }
+    }
+}

lib/mosaic-geomath/src/test/java/org/eclipse/mosaic/lib/spatial/PointCloudTest.java

@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2024 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.lib.spatial;
+
+import static org.hamcrest.CoreMatchers.is;
+import static org.hamcrest.MatcherAssert.assertThat;
+
+import org.eclipse.mosaic.lib.math.MathUtils;
+import org.eclipse.mosaic.lib.math.Vector3d;
+import org.eclipse.mosaic.lib.math.VectorUtils;
+
+import org.hamcrest.core.IsEqual;
+import org.junit.Test;
+
+import java.util.List;
+
+public class PointCloudTest {
+
+    @Test
+    public void relativeToAbsolute() {
+        // SETUP
+        PointCloud.Point p1 = new PointCloud.Point(new Vector3d(4.0, 5.0, 0.0), 0f, (byte) 0);
+        PointCloud.Point p2 = new PointCloud.Point(new Vector3d(-1.0, 2.0, 4.0), 0f, (byte) 1);
+
+        PointCloud pc = new PointCloud.Relative(
+                new RotationMatrix().rotate(90, VectorUtils.UP), new Vector3d(3, 1, 0),
+                List.of(p1, p2), 0, 0, 100
+        );
+
+        // RUN
+        List<PointCloud.Point> absolutePoints = pc.getAbsoluteEndPointsWithHit();
+
+        // ASSERT
+        PointCloud.Point p2absolute = new PointCloud.Point(new Vector3d(7, 3, 1.0), 0f, (byte) 1);
+        assertThat(absolutePoints.size(), is(1));
+        assertThat(absolutePoints.get(0), is(fuzzyEqualTo(p2absolute)));
+    }
+
+    @Test
+    public void absoluteToRelative() {
+        // SETUP
+        PointCloud.Point p1 = new PointCloud.Point(new Vector3d(4.0, 5.0, 0.0), 0f, (byte) 0);
+        PointCloud.Point p2 = new PointCloud.Point(new Vector3d(-1.0, 2.0, 4.0), 0f, (byte) 1);
+
+        PointCloud pc = new PointCloud.Absolute(
+                new RotationMatrix().rotate(90, VectorUtils.UP), new Vector3d(3, 1, 0),
+                List.of(p1, p2), 0, 0, 100
+        );
+
+        // RUN
+        PointCloud.Point p2relative = new PointCloud.Point(new Vector3d(-4, 1, -4), 0f, (byte) 1);
+
+        // ASSERT
+        List<PointCloud.Point> relativePoints = pc.getRelativeEndPointsWithHit();
+        assertThat(relativePoints.size(), is(1));
+        assertThat(relativePoints.get(0), is(fuzzyEqualTo(p2relative)));
+    }
+
+    private static <T extends Vector3d> IsEqual<T> fuzzyEqualTo(T base) {
+        return new IsEqual<>(base) {
+            @Override
+            public boolean matches(Object actualValue) {
+                if (actualValue instanceof Vector3d) {
+                    Vector3d actual = (Vector3d) actualValue;
+                    return MathUtils.isFuzzyEqual(base.x, actual.x) &&
+                            MathUtils.isFuzzyEqual(base.y, actual.y) &&
+                            MathUtils.isFuzzyEqual(base.z, actual.z);
+                }
+                return super.matches(actualValue);
+            }
+        };
+    }
+
+
+}