added road signal geometry surfaces

rtron-math/src/main/kotlin/io/rtron/math/geometry/euclidean/threed/surface/Rectangle3D.kt

@@ -28,8 +28,8 @@ import io.rtron.math.transform.AffineSequence3D
 /**
  * Rectangle with a certain [length] and [width] whereby the origin is located at the rectangle's center at z=0.
  *
- * @param length length of rectangle in the direction of the x axis
- * @param width width of rectangle in the direction of the y axis
+ * @param length length of rectangle in the direction of the x-axis
+ * @param width width of rectangle in the direction of the y-axis
  */
 data class Rectangle3D(
     val length: Double,

rtron-model/src/main/kotlin/io/rtron/model/opendrive/objects/RoadObjectsObjectRepeat.kt

@@ -20,6 +20,8 @@ import arrow.core.None
 import arrow.core.Option
 import arrow.optics.optics
 import io.rtron.math.analysis.function.univariate.pure.LinearFunction
+import io.rtron.math.geometry.curved.oned.point.CurveRelativeVector1D
+import io.rtron.math.geometry.euclidean.threed.point.Vector3D
 import io.rtron.math.range.Range
 import io.rtron.model.opendrive.additions.identifier.AdditionalRoadObjectRepeatIdentifier
 import io.rtron.model.opendrive.additions.identifier.RoadObjectRepeatIdentifier
@@ -46,6 +48,12 @@ data class RoadObjectsObjectRepeat(
     override var additionalId: Option<RoadObjectRepeatIdentifier> = None
 ) : OpendriveElement(), AdditionalRoadObjectRepeatIdentifier {
 
+    // Properties and Initializers
+    val curveRelativeStartPosition get() = CurveRelativeVector1D(s)
+
+    /** position of the object relative to the point on the road reference line */
+    val referenceLinePointRelativePosition get() = Vector3D(0.0, tStart, zOffsetStart)
+
     // Methods
     fun isContinuous() = distance == 0.0
     fun isDiscrete() = !isContinuous()

rtron-model/src/main/kotlin/io/rtron/model/opendrive/signal/RoadSignalsSignal.kt

@@ -78,9 +78,9 @@ data class RoadSignalsSignal(
         get() = Pose3D(referenceLinePointRelativePosition, referenceLinePointRelativeRotation)
 
     // Methods
-    fun isPolygon() = width.isDefined() && height.isDefined()
-    fun isVerticalLine() = width.isEmpty() && height.isDefined()
-    fun isHorizontalLine() = width.isDefined() && height.isEmpty()
+    fun containsRectangle() = width.isDefined() && height.isDefined()
+    fun containsVerticalLine() = width.isEmpty() && height.isDefined()
+    fun containsHorizontalLine() = width.isDefined() && height.isEmpty()
 
     companion object
 }

rtron-std/src/main/kotlin/io/rtron/std/Int.kt

@@ -0,0 +1,19 @@
+/*
+ * Copyright 2019-2023 Chair of Geoinformatics, Technical University of Munich
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+package io.rtron.std
+
+fun Boolean.toInt() = if (this) 1 else 0

rtron-transformer/src/main/kotlin/io/rtron/transformer/converter/opendrive2roadspaces/geometry/Solid3DBuilder.kt

@@ -55,42 +55,31 @@ object Solid3DBuilder {
      * Builds a list of cuboids from the OpenDRIVE road object class ([RoadObjectsObject]) directly or from the
      * repeated entries defined in [RoadObjectsObjectRepeat].
      */
-    fun buildCuboids(roadObject: RoadObjectsObject, curveAffine: Affine3D, numberTolerance: Double): ContextMessageList<List<Cuboid3D>> {
-        val cuboidList = mutableListOf<Cuboid3D>()
-        val messageList = DefaultMessageList()
-
-        if (roadObject.containsCuboid()) {
-            val objectAffine = Affine3D.of(roadObject.referenceLinePointRelativePose)
-            val affineSequence = AffineSequence3D.of(curveAffine, objectAffine)
-            cuboidList += Cuboid3D.of(roadObject.length, roadObject.width, roadObject.heightValidated, numberTolerance, affineSequence)
-        }
+    fun buildCuboids(roadObject: RoadObjectsObject, curveAffine: Affine3D, numberTolerance: Double): Cuboid3D {
+        require(roadObject.containsCuboid()) { "Road object must contain cuboid." }
 
-        if (roadObject.repeat.any { it.isRepeatedCuboid() }) {
-            messageList += DefaultMessage.of("", "Cuboid geometries in the repeat elements are currently not supported.", roadObject.additionalId, Severity.WARNING, wasFixed = false)
-        }
+        val objectAffine = Affine3D.of(roadObject.referenceLinePointRelativePose)
+        val affineSequence = AffineSequence3D.of(curveAffine, objectAffine)
 
-        return ContextMessageList(cuboidList, messageList)
+        return Cuboid3D.of(
+            roadObject.length,
+            roadObject.width,
+            roadObject.heightValidated,
+            numberTolerance,
+            affineSequence
+        )
     }
 
     /**
      * Builds a list of cylinders from the OpenDRIVE road object class ([RoadObjectsObject]) directly or from the
      * repeated entries defined in [RoadObjectsObjectRepeat].
      */
-    fun buildCylinders(roadObject: RoadObjectsObject, curveAffine: Affine3D, numberTolerance: Double): ContextMessageList<List<Cylinder3D>> {
-        val cylinderList = mutableListOf<Cylinder3D>()
-        val messageList = DefaultMessageList()
-
-        if (roadObject.containsCylinder()) {
-            val objectAffine = Affine3D.of(roadObject.referenceLinePointRelativePose)
-            val affineSequence = AffineSequence3D.of(curveAffine, objectAffine)
-            cylinderList += Cylinder3D.of(roadObject.radius, roadObject.height, numberTolerance, affineSequence)
-        }
-
-        if (roadObject.repeat.any { it.isRepeatCylinder() }) {
-            messageList += DefaultMessage.of("", "Cylinder geometries in the repeat elements are currently not supported.", roadObject.additionalId, Severity.WARNING, wasFixed = false)
-        }
+    fun buildCylinders(roadObject: RoadObjectsObject, curveAffine: Affine3D, numberTolerance: Double): Cylinder3D {
+        require(roadObject.containsCylinder()) { "Road object must contain cylinder." }
 
-        return ContextMessageList(cylinderList, messageList)
+        val objectAffine = Affine3D.of(roadObject.referenceLinePointRelativePose)
+        val affineSequence = AffineSequence3D.of(curveAffine, objectAffine)
+        return Cylinder3D.of(roadObject.radius, roadObject.height, numberTolerance, affineSequence)
     }
 
     /**

rtron-transformer/src/main/kotlin/io/rtron/transformer/converter/opendrive2roadspaces/geometry/Surface3DBuilder.kt

@@ -27,18 +27,21 @@ import io.rtron.io.messages.Severity
 import io.rtron.io.messages.handleMessageList
 import io.rtron.io.messages.mergeToReport
 import io.rtron.math.analysis.function.univariate.combination.StackedFunction
+import io.rtron.math.geometry.euclidean.threed.Rotation3D
 import io.rtron.math.geometry.euclidean.threed.curve.Curve3D
 import io.rtron.math.geometry.euclidean.threed.point.Vector3D
 import io.rtron.math.geometry.euclidean.threed.surface.Circle3D
 import io.rtron.math.geometry.euclidean.threed.surface.LinearRing3D
 import io.rtron.math.geometry.euclidean.threed.surface.ParametricBoundedSurface3D
 import io.rtron.math.geometry.euclidean.threed.surface.Rectangle3D
+import io.rtron.math.std.PI
 import io.rtron.math.transform.Affine3D
 import io.rtron.math.transform.AffineSequence3D
 import io.rtron.model.opendrive.objects.RoadObjectsObject
 import io.rtron.model.opendrive.objects.RoadObjectsObjectOutlinesOutline
 import io.rtron.model.opendrive.objects.RoadObjectsObjectOutlinesOutlineCornerRoad
 import io.rtron.model.opendrive.objects.RoadObjectsObjectRepeat
+import io.rtron.model.opendrive.signal.RoadSignalsSignal
 import io.rtron.transformer.converter.opendrive2roadspaces.analysis.FunctionBuilder
 import io.rtron.transformer.messages.opendrive.of
 
@@ -53,42 +56,33 @@ object Surface3DBuilder {
      * Builds a list of rectangles from the OpenDRIVE road object class ([RoadObjectsObject]) directly or from the
      * repeated entries defined in [RoadObjectsObjectRepeat].
      */
-    fun buildRectangles(roadObject: RoadObjectsObject, curveAffine: Affine3D, numberTolerance: Double): ContextMessageList<List<Rectangle3D>> {
-        val rectangleList = mutableListOf<Rectangle3D>()
-        val messageList = DefaultMessageList()
+    fun buildRectangle(roadObject: RoadObjectsObject, curveAffine: Affine3D, numberTolerance: Double): Rectangle3D {
+        require(roadObject.containsRectangle()) { "Road object must contain rectangle." }
 
-        if (roadObject.containsRectangle()) {
-            val objectAffine = Affine3D.of(roadObject.referenceLinePointRelativePose)
-            val affineSequence = AffineSequence3D.of(curveAffine, objectAffine)
-            rectangleList += Rectangle3D.of(roadObject.length, roadObject.width, numberTolerance, affineSequence)
-        }
+        val objectAffine = Affine3D.of(roadObject.referenceLinePointRelativePose)
+        val affineSequence = AffineSequence3D.of(curveAffine, objectAffine)
+        return Rectangle3D.of(roadObject.length, roadObject.width, numberTolerance, affineSequence)
+    }
 
-        if (roadObject.repeat.any { it.isRepeatedCuboid() }) {
-            messageList += DefaultMessage.of("", "Cuboid geometries in the repeat elements are currently not supported.", roadObject.additionalId, Severity.WARNING, wasFixed = false)
-        }
+    fun buildRectangle(roadObject: RoadSignalsSignal, curveAffine: Affine3D, numberTolerance: Double): Rectangle3D {
+        require(roadObject.containsRectangle()) { "Road signal must contain rectangle." }
 
-        return ContextMessageList(rectangleList, messageList)
+        val objectAffine = Affine3D.of(roadObject.referenceLinePointRelativePose)
+        val objectRotation = Affine3D.of(Rotation3D.of(0.0, -PI / 2.0, 0.0))
+        val affineSequence = AffineSequence3D.of(curveAffine, objectAffine, objectRotation)
+        return Rectangle3D.of(roadObject.height, roadObject.width, numberTolerance, affineSequence)
     }
 
     /**
      * Builds a list of circles from the OpenDRIVE road object class ([RoadObjectsObject]) directly or from the
      * repeated entries defined in [RoadObjectsObjectRepeat].
      */
-    fun buildCircles(roadObject: RoadObjectsObject, curveAffine: Affine3D, numberTolerance: Double): ContextMessageList<List<Circle3D>> {
-        val circleList = mutableListOf<Circle3D>()
-        val messageList = DefaultMessageList()
-
-        if (roadObject.containsCircle()) {
-            val objectAffine = Affine3D.of(roadObject.referenceLinePointRelativePose)
-            val affineSequence = AffineSequence3D.of(curveAffine, objectAffine)
-            circleList += Circle3D.of(roadObject.radius, numberTolerance, affineSequence)
-        }
-
-        if (roadObject.repeat.any { it.isRepeatCylinder() }) {
-            messageList += DefaultMessage.of("", "Cuboid geometries in the repeat elements are currently not supported.", roadObject.additionalId, Severity.WARNING, wasFixed = false)
-        }
+    fun buildCircle(roadObject: RoadObjectsObject, curveAffine: Affine3D, numberTolerance: Double): Circle3D {
+        require(roadObject.containsRectangle()) { "Road object must contain circle." }
 
-        return ContextMessageList(circleList, messageList)
+        val objectAffine = Affine3D.of(roadObject.referenceLinePointRelativePose)
+        val affineSequence = AffineSequence3D.of(curveAffine, objectAffine)
+        return Circle3D.of(roadObject.radius, numberTolerance, affineSequence)
     }
 
     /**

rtron-transformer/src/main/kotlin/io/rtron/transformer/converter/opendrive2roadspaces/roadspaces/RoadspaceObjectBuilder.kt

@@ -26,14 +26,17 @@ import arrow.core.nonEmptyListOf
 import arrow.core.some
 import arrow.core.toNonEmptyListOrNull
 import io.rtron.io.messages.ContextMessageList
+import io.rtron.io.messages.DefaultMessage
 import io.rtron.io.messages.DefaultMessageList
+import io.rtron.io.messages.Severity
 import io.rtron.io.messages.mergeMessageLists
 import io.rtron.math.geometry.curved.threed.point.CurveRelativeVector3D
 import io.rtron.math.geometry.euclidean.threed.AbstractGeometry3D
 import io.rtron.math.geometry.euclidean.threed.Rotation3D
 import io.rtron.math.geometry.euclidean.threed.curve.Curve3D
 import io.rtron.math.geometry.euclidean.threed.point.Vector3D
 import io.rtron.math.range.Range
+import io.rtron.math.transform.AffineSequence3D
 import io.rtron.model.opendrive.objects.EObjectType
 import io.rtron.model.opendrive.signal.RoadSignals
 import io.rtron.model.opendrive.signal.RoadSignalsSignal
@@ -44,11 +47,13 @@ import io.rtron.model.roadspaces.roadspace.attribute.AttributeList
 import io.rtron.model.roadspaces.roadspace.attribute.attributes
 import io.rtron.model.roadspaces.roadspace.objects.RoadObjectType
 import io.rtron.model.roadspaces.roadspace.objects.RoadspaceObject
+import io.rtron.std.toInt
 import io.rtron.transformer.converter.opendrive2roadspaces.Opendrive2RoadspacesParameters
 import io.rtron.transformer.converter.opendrive2roadspaces.geometry.Curve3DBuilder
 import io.rtron.transformer.converter.opendrive2roadspaces.geometry.Solid3DBuilder
 import io.rtron.transformer.converter.opendrive2roadspaces.geometry.Surface3DBuilder
 import io.rtron.transformer.converter.opendrive2roadspaces.geometry.Vector3DBuilder
+import io.rtron.transformer.messages.opendrive.of
 import io.rtron.model.opendrive.objects.RoadObjects as OpendriveRoadObjects
 import io.rtron.model.opendrive.objects.RoadObjectsObject as OpendriveRoadObject
 import io.rtron.model.opendrive.objects.RoadObjectsObjectRepeat as OpendriveRoadObjectRepeat
@@ -101,19 +106,26 @@ class RoadspaceObjectBuilder(
         val laneRelations = buildLaneRelations(roadObject, road)
 
         val roadObjectsFromRepeat = roadObject.repeat.map { currentRoadObjectRepeat ->
+            if (currentRoadObjectRepeat.isRepeatedCuboid()) {
+                messageList += DefaultMessage.of("", "Cuboid geometries in the repeat elements are currently not supported.", roadObject.additionalId, Severity.WARNING, wasFixed = false)
+            }
+            if (currentRoadObjectRepeat.isRepeatCylinder()) {
+                messageList += DefaultMessage.of("", "Cylinder geometries in the repeat elements are currently not supported.", roadObject.additionalId, Severity.WARNING, wasFixed = false)
+            }
+
             val repeatIdentifier = currentRoadObjectRepeat.additionalId.toEither { IllegalStateException("Additional outline ID must be available.") }.getOrElse { throw it }
 
             val roadspaceObjectId = RoadspaceObjectIdentifier(roadObject.id, repeatIdentifier.repeatIndex.some(), roadObject.name, id)
-            val pointGeometry = buildPointGeometry(roadObject, roadReferenceLine)
-            val buildBoundingBoxGeometry = buildBoundingBoxGeometry(roadObject, roadReferenceLine).handleMessageList { messageList += it }
+            val pointGeometry = buildPointGeometry(currentRoadObjectRepeat, roadReferenceLine)
+            val buildBoundingBoxGeometry = buildBoundingBoxGeometry(roadObject, roadReferenceLine)
             val complexGeometry = buildComplexGeometry(roadObject, currentRoadObjectRepeat.some(), roadReferenceLine).handleMessageList { messageList += it }
             RoadspaceObject(roadspaceObjectId, type, pointGeometry, buildBoundingBoxGeometry, complexGeometry, laneRelations, attributes)
         }
 
         val roadObjects = if (roadObjectsFromRepeat.isEmpty()) {
             val roadspaceObjectId = RoadspaceObjectIdentifier(roadObject.id, None, roadObject.name, id)
             val pointGeometry = buildPointGeometry(roadObject, roadReferenceLine)
-            val buildBoundingBoxGeometry = buildBoundingBoxGeometry(roadObject, roadReferenceLine).handleMessageList { messageList += it }
+            val buildBoundingBoxGeometry = buildBoundingBoxGeometry(roadObject, roadReferenceLine)
             val complexGeometry = buildComplexGeometry(roadObject, None, roadReferenceLine).handleMessageList { messageList += it }
             nonEmptyListOf(RoadspaceObject(roadspaceObjectId, type, pointGeometry, buildBoundingBoxGeometry, complexGeometry, laneRelations, attributes))
         } else {
@@ -150,24 +162,40 @@ class RoadspaceObjectBuilder(
         return Vector3DBuilder.buildVector3Ds(roadObject, curveAffine)
     }
 
-    private fun buildBoundingBoxGeometry(roadObject: OpendriveRoadObject, roadReferenceLine: Curve3D): ContextMessageList<Option<AbstractGeometry3D>> {
-        val messageList = DefaultMessageList()
+    private fun buildPointGeometry(roadObjectRepeat: OpendriveRoadObjectRepeat, roadReferenceLine: Curve3D): Vector3D {
+        val curveAffine = roadReferenceLine.calculateAffine(roadObjectRepeat.curveRelativeStartPosition)
+        val affineSequence = AffineSequence3D.of(curveAffine)
+        return roadObjectRepeat.referenceLinePointRelativePosition.copy(affineSequence = affineSequence)
+    }
+
+    private fun buildBoundingBoxGeometry(
+        roadObject: OpendriveRoadObject,
+        roadReferenceLine: Curve3D
+    ): Option<AbstractGeometry3D> {
+        check(
+            roadObject.containsCuboid().toInt() +
+                roadObject.containsCylinder().toInt() +
+                roadObject.containsRectangle().toInt() +
+                roadObject.containsCircle().toInt() <= 1
+        ) { "Bounding box must only be derived for a single geometry." }
 
         // affine transformation matrix at the curve point of the object
         val curveAffine = roadReferenceLine.calculateAffine(roadObject.curveRelativePosition.toCurveRelative1D())
 
-        // build up solid geometrical representations
-        val geometries = mutableListOf<AbstractGeometry3D>()
-        geometries += Solid3DBuilder.buildCuboids(roadObject, curveAffine, parameters.numberTolerance).handleMessageList { messageList += it }
-        geometries += Solid3DBuilder.buildCylinders(roadObject, curveAffine, parameters.numberTolerance).handleMessageList { messageList += it }
-
-        // build up surface geometrical representations
-        geometries += Surface3DBuilder.buildRectangles(roadObject, curveAffine, parameters.numberTolerance).handleMessageList { messageList += it }
-        geometries += Surface3DBuilder.buildCircles(roadObject, curveAffine, parameters.numberTolerance).handleMessageList { messageList += it }
+        if (roadObject.containsCuboid()) {
+            return Solid3DBuilder.buildCuboids(roadObject, curveAffine, parameters.numberTolerance).some()
+        }
+        if (roadObject.containsCylinder()) {
+            return Solid3DBuilder.buildCylinders(roadObject, curveAffine, parameters.numberTolerance).some()
+        }
+        if (roadObject.containsRectangle()) {
+            return Surface3DBuilder.buildRectangle(roadObject, curveAffine, parameters.numberTolerance).some()
+        }
+        if (roadObject.containsCircle()) {
+            return Surface3DBuilder.buildCircle(roadObject, curveAffine, parameters.numberTolerance).some()
+        }
 
-        check(geometries.size <= 1) { "Multiple geometries must not be derived." }
-        val builtGeometry = if (geometries.isEmpty()) None else Some(geometries.first())
-        return ContextMessageList(builtGeometry, messageList)
+        return None
     }
 
     /**
@@ -300,6 +328,15 @@ class RoadspaceObjectBuilder(
     }
 
     private fun buildBoundingBoxGeometry(signal: RoadSignalsSignal, roadReferenceLine: Curve3D): Option<AbstractGeometry3D> {
+        // affine transformation matrix at the curve point of the object
+        val curveAffine = roadReferenceLine.calculateAffine(signal.curveRelativePosition.toCurveRelative1D())
+
+        if (signal.containsRectangle()) {
+            println("signal")
+            println(signal.id)
+            return Surface3DBuilder.buildRectangle(signal, curveAffine, parameters.numberTolerance).some()
+        }
+
         return None
     }