diff --git a/CMakeLists.txt b/CMakeLists.txt
index 65725c07b1731b4239a8d6189b0b066476f88746..de57f895c23c7d1238d16044cb28d716e29528bd 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -57,6 +57,8 @@ target_include_directories(${CMAKE_PROJECT_NAME}
target_sources(${CMAKE_PROJECT_NAME}
PRIVATE
+ src/Component/Points.cpp
+
src/Object/CommonSign.cpp
src/Object/MovingObject.cpp
src/Object/RoadMarking.cpp
@@ -83,16 +85,18 @@ target_sources(${CMAKE_PROJECT_NAME}
src/Street/ReferenceLine.cpp
src/Street/Road.cpp
+ src/Types/Enum/LaneDirection.cpp
+ src/Types/Enum/RoadDirection.cpp
+ src/Types/Enum/Side.cpp
+
src/Types/Area.cpp
src/Types/Bounds.cpp
+ src/Types/Circle.cpp
src/Types/Enum.cpp
src/Types/Geometry.cpp
- src/Types/LaneDirection.cpp
src/Types/LocalBounds.cpp
src/Types/Matrix.cpp
- src/Types/RoadDirection.cpp
src/Types/Shape.cpp
- src/Types/Side.cpp
src/Types/Value.cpp
src/Utility/XYZ.cpp
diff --git a/README.md b/README.md
index 664709819640064d1f81990401a61fc1666a51c2..4d91ff0feaec5090805ec134d9c912be8c55cc0f 100644
--- a/README.md
+++ b/README.md
@@ -11,12 +11,12 @@ All wrapper objects have public handles to their corresponding `osi3` entities.
### Dependencies
-| Library | Version | Additional Information |
-| ------- | ------- | ---- |
-| boost Geometry | Tested with 1.72.0 | [boost doc](https://www.boost.org) and [boost Geometry](https://www.boost.org/doc/libs/1_72_0/libs/geometry/doc/html/index.html) |
-| Open Simulation Interface | 3.5 and 3.6 | [OSI doc](https://opensimulationinterface.github.io/osi-antora-generator/asamosi/latest/interface/setup/installing_prerequisites.html) and [OpenPASS doc](https://www.eclipse.org/openpass/content/html/installation_guide/20_install_prerequisites.html) |
-| - protobuf<br />- protobuf-shared | Tested with 3.20.0 | [OSI doc](https://opensimulationinterface.github.io/osi-antora-generator/asamosi/latest/interface/setup/installing_prerequisites.html) and [OpenPASS doc](https://www.eclipse.org/openpass/content/html/installation_guide/20_install_prerequisites.html) |
-| googletest | Tested with 1.13.0 | https://github.com/google/googletest |
+| Library | Version | Additional Information |
+| --------------------------------- | ------------------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| boost Geometry | Tested with 1.72.0 | [boost doc](https://www.boost.org) and [boost Geometry](https://www.boost.org/doc/libs/1_72_0/libs/geometry/doc/html/index.html) |
+| Open Simulation Interface | 3.5 and 3.6 | [OSI doc](https://opensimulationinterface.github.io/osi-antora-generator/asamosi/latest/interface/setup/installing_prerequisites.html) and [OpenPASS doc](https://www.eclipse.org/openpass/content/html/installation_guide/20_install_prerequisites.html) |
+| - protobuf<br />- protobuf-shared | Tested with 3.20.0 | [OSI doc](https://opensimulationinterface.github.io/osi-antora-generator/asamosi/latest/interface/setup/installing_prerequisites.html) and [OpenPASS doc](https://www.eclipse.org/openpass/content/html/installation_guide/20_install_prerequisites.html) |
+| googletest | Tested with 1.13.0 | https://github.com/google/googletest |
## Build Example
@@ -80,7 +80,7 @@ if (route.empty())
// ... increment timestep here ...
// After modifying the groundTruth, update the osiql query if there has
// been a change in lane assignments or objects have been added/removed:
-query.Update(alteredGroundTruth.moving_object());
+query.UpdateAll<MovingObject>(alteredGroundTruth.moving_object());
route = query.GetRoute(destination);
if (route.empty())
diff --git a/changelog.md b/changelog.md
index e2650165a094ac9095740f12cd6c979b26e23f49..748b0a52e08fb5d5ee2831f04a1dde8094908f8d 100644
--- a/changelog.md
+++ b/changelog.md
@@ -1,3 +1,23 @@
+### 2025-04-01 v2.2.0
+#### Breaking Changes
+- `GetCurvature` now treats its chain of points as lying on the sampled curve. The old behavior (points define edges that are tangents of the curve) is moved to `GetCurvatureFromLocalTangents`.
+
+#### New
+- Add `osiql::Interpolate` & `osiql::Average`, which return linear interpolations of their inputs
+- Add `osiql::SetId(Type, Id)` for any `Type` that is an OSI type (`osi3::Identifier` or has the member `mutable_id()`)
+- Add `World::UpdateAll<StationaryObject>`
+
+#### Fixes
+- Comparison functors with transformers now correctly transform both arguments, not just the first
+- Make Lane routes iterable just like road routes
+
+#### Changes
+- `GetDistanceBetween(a, b)` and related methods now returns negative distances if `b` is behind `a`.
+
+#### Changes
+- Deprecated `World::Update`. Prefer `World::UpdateAll<MovingObject>`
+- Improve lookup performance for `StationaryObject`s and `LightBulb`s
+
### 2025-03-11 v2.1.1
#### Breaking Changes
- Rename `Traversal` to `LaneDirection`
diff --git a/doc/source/Extensions/10_world.rst b/doc/source/Extensions/10_world.rst
index 6853b3c536cfc80b55176846bd000b7e6b26eea5..4b45ee913f8ac975f107249439dc3c9f1597c49a 100644
--- a/doc/source/Extensions/10_world.rst
+++ b/doc/source/Extensions/10_world.rst
@@ -40,7 +40,7 @@ The ``osiql::World`` needs to be informed when its underlying handle has changed
// The user is responsible for managing the handle's lifetime.
World world{groundTruth};
// When the moving objects of the underlying groundTruth change, the world needs to be updated:
- world.Update(groundTruth.moving_object());
+ world.UpdateAll<MovingObject>(groundTruth.moving_object());
See the doxygen documentation for all methods that ``osiql::World`` provides.
diff --git a/include/OsiQueryLibrary/Component/Identifiable.h b/include/OsiQueryLibrary/Component/Identifiable.h
index 9751e8ff78ce8fe959d159e8797dc50b602baa39..16ee75addff79ab205e233b3c85e8fd6e5ebd207 100644
--- a/include/OsiQueryLibrary/Component/Identifiable.h
+++ b/include/OsiQueryLibrary/Component/Identifiable.h
@@ -68,4 +68,10 @@ struct Identifiable : Wrapper<Type>
//! \return Id
Id GetId() const;
};
+
+//! Assigns the id of an OSI object
+//!
+//! \tparam Handle OSI type with an assignable id
+template <typename Handle>
+void SetId(Handle &, Id);
} // namespace osiql
diff --git a/include/OsiQueryLibrary/Component/Identifiable.tpp b/include/OsiQueryLibrary/Component/Identifiable.tpp
index 372a07a6a2073350512b30f6875e555f960fad44..20594e0fba110715065ea7ff20452d87a289c2f5 100644
--- a/include/OsiQueryLibrary/Component/Identifiable.tpp
+++ b/include/OsiQueryLibrary/Component/Identifiable.tpp
@@ -63,4 +63,21 @@ Id Identifiable<Type>::GetId() const
return this->GetHandle().has_id() ? this->GetHandle().id().value() : UNDEFINED_ID;
}
}
+
+template <typename Handle>
+void SetId(Handle &handle, Id id)
+{
+ if constexpr (std::is_same_v<Handle, osi3::Identifier>)
+ {
+ handle.set_value(id);
+ }
+ else if constexpr (OSIQL_HAS_MEMBER(Handle, mutable_id()))
+ {
+ handle.mutable_id()->set_value(id);
+ }
+ else
+ {
+ static_assert(always_false<Handle>, "Not supported");
+ }
+}
} // namespace osiql
diff --git a/include/OsiQueryLibrary/Component/Points.h b/include/OsiQueryLibrary/Component/Points.h
index 924e2a2526656f689d6f17bf8f87d113705368c1..dad83747e87c9ec853145b6d9e1b270d1c80eea3 100644
--- a/include/OsiQueryLibrary/Component/Points.h
+++ b/include/OsiQueryLibrary/Component/Points.h
@@ -1,5 +1,5 @@
/********************************************************************************
- * Copyright (c) 2023-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2023-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License 2.0 which is available at
@@ -61,15 +61,15 @@ struct Points : Iterable<Type, From>
//! \return double
double GetLength() const;
- //! Returns the interpolated point on this chain of points with the given s-coordinate
+ //! Returns the interpolated point on this chain of points with the given s-coordinate.
+ //! If the s-coordinate falls outside the length of this chain, the nearest edge is extended infinitely to allow conversion.
//!
//! \param s s-coordinate representing the distance from the start of a reference line along that reference line
//! \return XY xy-coordinate pair representing a global point in world coordinates
XY GetXY(double s) const;
- //! Returns the global point of any given s- & t-coordinate relative to this Points. This implementation is independent from
- //! other points comprising a lane and thus may produce results deviating from OpenDRIVE specification.
- //! The point is interpolated perpendicular to the (possibly extended) points and is thus ambiguous near the start/end of line segments.
+ //! Returns the global point of any given s- & t-coordinate relative to this chain of points.
+ //! If the s-coordinate falls outside the length of this chain, the nearest edge is extended infinitely to allow conversion.
//!
//! \return XY
XY GetXY(const ST &) const;
@@ -77,6 +77,7 @@ struct Points : Iterable<Type, From>
//! Returns a st-coordinate representation of the given xy-coordinates localized to this chain of points.
//! If the input contains a global angle, the angle will be localized as well as a counter-clockwise
//! ascending angle in radians within [-π, π] from the x-axis.
+ //! If the point lies behind or after the chain of points the nearest edge is extended infinitely to localize said point.
//!
//! \return Either ST or Pose<ST>
template <typename GlobalPoint>
@@ -91,6 +92,7 @@ struct Points : Iterable<Type, From>
//! Returns the global angle of the point set's edge at the given value in the template direction.
//! If the value hits a corner, the angle of the latter edge in the template direction is returned.
+ //! If the s-coordinate falls outside the length of this chain, the nearest edge is extended infinitely to allow conversion.
//!
//! \tparam RoadDirection Downstream or Upstream
//! \param s s-coordinate
@@ -100,34 +102,42 @@ struct Points : Iterable<Type, From>
//! Returns the global angle of the point set's edge at the given value in the given direction.
//! If the value hits a corner, the angle of the latter edge in the given direction is returned.
+ //! If the s-coordinate falls outside the length of this chain, the nearest edge is extended infinitely to allow conversion.
//!
//! \param s s-coordinate
//! \param RoadDirection Downstream or Upstream
//! \return double counter-clockwise ascending angle in radians within [-π, π] from the x-axis
double GetAngle(double s, RoadDirection) const;
- //! Treats this chain of points as though it were a smooth curve and
- //! returns the change in angle per unit of length at the given s-coordinate.
+ //! Returns the interpolated rate of change in angle at the given s-coordinate as though this chain of points were a smooth curve.
+ //! If the s-coordinate falls outside the length of this chain, NaN is returned.
//!
//! \tparam RoadDirection Downstream or Upstream
- //! \param s s-coordinate at which the curvature will be measured
- //! \param epsilon Rounding error tolerance. The result is interpolated by the change in angle
- //! of the closest vertex before and after the given s-coordinate. Any value closer to 0 than
- //! the given epsilon will be replaced by 0.
- //! \return double
+ //! \param s The s-coordinate at which the curvature is to be measured.
+ //! \return Change in angle per unit of length at the given s-coordinate.
template <RoadDirection = RoadDirection::Downstream>
- double GetCurvature(double s, double epsilon = 0.0) const;
+ double GetCurvature(double s) const;
//! Treats this chain of points as though it were a smooth curve and
//! returns the change in angle per unit of length at the given s-coordinate.
+ //! If the s-coordinate falls outside the length of this chain, NaN is returned.
//!
- //! \param s s-coordinate at which the curvature will be measured
+ //! \param s s-coordinate at which the curvature will be measured
//! \param RoadDirection Downstream or Upstream
- //! \param epsilon Rounding error tolerance. The result is interpolated by the change in angle
- //! of the closest vertex before and after the given s-coordinate. Any value closer to 0 than
- //! the given epsilon will be replaced by 0.
- //! \return double
- double GetCurvature(double s, RoadDirection, double epsilon = 0.0) const;
+ //! \return Interpolated rate of change in angle at the given s-coordinate
+ double GetCurvature(double s, RoadDirection) const;
+
+ //! Treats this chain of points as though its edges were the local tangents of a curve and
+ //! returns the interpolated rate of change in angle at the given s-coordinate.
+ //!
+ //! \tparam RoadDirection Downstream or Upstream
+ //! \param s s-coordinate at which the curvature will be measured
+ //! \param epsilon Rounding error tolerance. The result is interpolated by the change in angle
+ //! of the closest vertex before and after the given s-coordinate. Any value closer
+ //! to 0 than the given epsilon will be replaced by 0.
+ //!\return Change in angle per unit of length at the given s-coordinate
+ template <RoadDirection D>
+ double GetCurvatureFromLocalTangents(double s, double epsilon = .0) const; // NOLINT(bugprone-easily-swappable-parameters)
};
template <typename Type, typename From>
diff --git a/include/OsiQueryLibrary/Component/Points.tpp b/include/OsiQueryLibrary/Component/Points.tpp
index 87f53098531ce3b70f341da739d2b5349f3c599f..652ae2fce13e500358b4f5f4b1347d95201e42fd 100644
--- a/include/OsiQueryLibrary/Component/Points.tpp
+++ b/include/OsiQueryLibrary/Component/Points.tpp
@@ -1,5 +1,5 @@
/********************************************************************************
- * Copyright (c) 2023-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2023-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License 2.0 which is available at
@@ -14,6 +14,7 @@
#include "Iterable.tpp"
#include "OsiQueryLibrary/Point/Pose.h"
#include "OsiQueryLibrary/Point/XY.tpp"
+#include "OsiQueryLibrary/Types/Circle.h"
#include "OsiQueryLibrary/Types/Constants.h"
#include "OsiQueryLibrary/Utility/Extract.tpp"
@@ -95,13 +96,13 @@ double Points<Type, From>::GetAngle(double s) const
const double endAngle{(edge + post).Angle()};
if (startAngle - endAngle > pi)
{
- return std::fmod(startAngle * (1.0 - ratio) + (endAngle + twoPi) * ratio, twoPi);
+ return std::fmod(Interpolate(startAngle, endAngle + twoPi, ratio), twoPi);
}
if (endAngle - startAngle > pi)
{
- return std::fmod((startAngle + twoPi) * (1.0 - ratio) + endAngle * ratio, twoPi);
+ return std::fmod(Interpolate(startAngle + twoPi, endAngle, ratio), twoPi);
}
- return startAngle * (1.0 - ratio) + endAngle * ratio;
+ return Interpolate(startAngle, endAngle, ratio);
}
}
@@ -111,56 +112,6 @@ double Points<Type, From>::GetAngle(double s, RoadDirection direction) const
return direction == RoadDirection::Upstream ? GetAngle<RoadDirection::Upstream>(s) : GetAngle<RoadDirection::Downstream>(s);
}
-template <typename Type, typename From>
-template <RoadDirection D>
-double Points<Type, From>::GetCurvature(double s, double epsilon) const // NOLINT(bugprone-easily-swappable-parameters)
-{
- const auto edgeEnd{GetNearestEdge<D>(s)};
- const auto edgeStart{std::prev(edgeEnd)};
- if (Extract<D>::less(s, *edgeStart) || Extract<D>::greater(s, *edgeEnd))
- {
- return std::numeric_limits<double>::quiet_NaN();
- }
- const XY edge{*edgeEnd - *edgeStart};
- const double halfEdgeLength{edge.Length() * 0.5};
- if (halfEdgeLength <= epsilon)
- {
- return 0.0;
- }
- double startCurvature{0.0};
- if (edgeStart != begin<D>())
- {
- const XY priorEdge{*edgeStart - *std::prev(edgeStart)};
- const double deltaAngle{std::atan2(priorEdge.Cross(edge), priorEdge.Dot(edge))};
- const double length{halfEdgeLength + priorEdge.Length() * 0.5};
- startCurvature = deltaAngle / length;
- }
-
- double endCurvature{0.0};
- if (std::next(edgeEnd) != end<D>())
- {
- const XY nextEdge{*std::next(edgeEnd) - *edgeEnd};
- const double deltaAngle{std::atan2(edge.Cross(nextEdge), edge.Dot(nextEdge))};
- const double length{halfEdgeLength + nextEdge.Length() * 0.5};
- endCurvature = deltaAngle / length;
- }
-
- const double startS{get<S>(*edgeStart)};
- const double endS{get<S>(*edgeEnd)};
- if (std::abs(endS - startS) < epsilon)
- {
- return 0.0;
- }
- const double ratio{(s - startS) / (endS - startS)};
- return startCurvature * (1.0 - ratio) + endCurvature * ratio;
-}
-
-template <typename Type, typename From>
-double Points<Type, From>::GetCurvature(double s, RoadDirection direction, double epsilon) const
-{
- return direction == RoadDirection::Upstream ? GetCurvature<RoadDirection::Upstream>(s, epsilon) : GetCurvature<RoadDirection::Downstream>(s, epsilon);
-}
-
template <typename Type, typename From>
double Points<Type, From>::GetT(double s) const
{
@@ -178,7 +129,7 @@ double Points<Type, From>::GetT(double s) const
return extract<Side::Left>(*it);
}
const double ratio{(s - get<S>(*it)) / (get<S>(*std::next(it)) - get<S>(*it))};
- return extract<Side::Left>(*it) * (1.0 - ratio) + extract<Side::Left>(*std::next(it)) * ratio;
+ return Interpolate(extract<Side::Left>(*it), extract<Side::Left>(*std::next(it)), ratio);
}
template <typename Type, typename From>
@@ -188,7 +139,7 @@ XY Points<Type, From>::GetXY(double s) const
const double prevS{get<S>(*std::prev(it))};
const double currS{get<S>(*it)};
const double ratio{(s - prevS) / (currS - prevS)};
- return XY{*std::prev(it)} * (1.0 - ratio) + XY{*it} * ratio;
+ return Interpolate(XY{*std::prev(it)}, XY{*it}, ratio);
}
template <typename Type, typename From>
@@ -203,7 +154,7 @@ XY Points<Type, From>::GetXY(const ST &coordinates) const
const double startDistance{get<S>(*std::prev(it))};
const double endDistance{get<S>(*it)};
const double ratio{(coordinates.s - startDistance) / (endDistance - startDistance)};
- const XY pointOnEdge{edgeStart * (1.0 - ratio) + edgeEnd * ratio};
+ const XY pointOnEdge{Interpolate(edgeStart, edgeEnd, ratio)};
return pointOnEdge + normal * coordinates.t;
}
@@ -229,6 +180,111 @@ decltype(auto) Points<Type, From>::Localize(const GlobalPoint &input) const
}
}
+namespace detail {
+//! Returns the curvature of the circle passing through the three given points.
+//!
+//! \return Signed curvature based on the shorter winding direction from the first to the
+//! last given point along the circle. Positive if counter-clockwise, negative if clockwise.
+double GetCurvature(const XY &, const XY &, const XY &);
+
+//! Computes two circles, One passing through the first three points and another through the last three,
+//! and returns the interpolated curvature between the two circles
+//!
+//!\param ratio Linear interpolation ratio. At 0, the curvature of the first circle is returned, at 1 that of the latter.
+//!\return Signed curvature based on the shorter winding direction from the first to the
+//! last given point along the circle. Positive if counter-clockwise, negative if clockwise.
+double GetCurvature(double ratio, const XY &, const XY &, const XY &, const XY &);
+} // namespace detail
+
+template <typename Type, typename From>
+template <RoadDirection D>
+double Points<Type, From>::GetCurvature(double s) const // NOLINT(bugprone-easily-swappable-parameters)
+{
+ if (s < get<S>(this->front()) || s > get<S>(this->back()))
+ {
+ return std::numeric_limits<double>::quiet_NaN();
+ }
+ if (this->size() <= 2)
+ {
+ return .0;
+ }
+ auto edge{GetNearestEdge<D>(s)};
+ if (std::prev(edge) == begin<D>()) // No prior edge
+ {
+ if (this->size() > 3) // But with successor edge
+ {
+ const double ratio{(s - get<S>(*std::prev(edge))) / (get<S>(*std::next(edge, 2)) - get<S>(*std::prev(edge)))};
+ return detail::GetCurvature(ratio, *std::prev(edge), *edge, *std::next(edge), *std::next(edge, 2));
+ }
+ return detail::GetCurvature(*std::prev(edge), *edge, *std::next(edge));
+ }
+ if (std::next(edge) == end<D>()) // No successor edge
+ {
+ if (this->size() > 3) // But with prior edge
+ {
+ const double ratio{(s - get<S>(*std::prev(edge, 3))) / (get<S>(*edge) - get<S>(*std::prev(edge, 3)))};
+ return detail::GetCurvature(ratio, *std::prev(edge, 3), *std::prev(edge, 2), *std::prev(edge), *edge);
+ }
+ return detail::GetCurvature(*std::prev(edge, 2), *std::prev(edge), *edge);
+ }
+ const double ratio{(s - get<S>(*std::prev(edge, 2))) / (get<S>(*std::next(edge)) - get<S>(*std::prev(edge, 2)))};
+ return detail::GetCurvature(ratio, *std::prev(edge, 2), *std::prev(edge), *edge, *std::next(edge));
+}
+
+template <typename Type, typename From>
+double Points<Type, From>::GetCurvature(double s, RoadDirection direction) const
+{
+ return direction == RoadDirection::Upstream ? GetCurvature<RoadDirection::Upstream>(s) : GetCurvature<RoadDirection::Downstream>(s);
+}
+
+template <typename Type, typename From>
+template <RoadDirection D>
+double Points<Type, From>::GetCurvatureFromLocalTangents(double s, double epsilon) const // NOLINT(bugprone-easily-swappable-parameters)
+{
+ if (s < get<S>(this->front()) || s > get<S>(this->back()))
+ {
+ return std::numeric_limits<double>::quiet_NaN();
+ }
+ if (this->size() <= 2)
+ {
+ return .0;
+ }
+ const auto edgeEnd{GetNearestEdge<D>(s)};
+ const auto edgeStart{std::prev(edgeEnd)};
+ const XY edge{*edgeEnd - *edgeStart};
+ const double halfEdgeLength{edge.Length() * 0.5};
+ if (halfEdgeLength <= epsilon)
+ {
+ return .0;
+ }
+ double startCurvature{.0};
+ if (edgeStart != begin<D>())
+ {
+ const XY priorEdge{*edgeStart - *std::prev(edgeStart)};
+ const double deltaAngle{std::atan2(priorEdge.Cross(edge), priorEdge.Dot(edge))};
+ const double length{halfEdgeLength + priorEdge.Length() * 0.5};
+ startCurvature = deltaAngle / length;
+ }
+
+ double endCurvature{.0};
+ if (std::next(edgeEnd) != end<D>())
+ {
+ const XY nextEdge{*std::next(edgeEnd) - *edgeEnd};
+ const double deltaAngle{std::atan2(edge.Cross(nextEdge), edge.Dot(nextEdge))};
+ const double length{halfEdgeLength + nextEdge.Length() * 0.5};
+ endCurvature = deltaAngle / length;
+ }
+
+ const double startS{get<S>(*edgeStart)};
+ const double endS{get<S>(*edgeEnd)};
+ if (std::abs(endS - startS) < epsilon)
+ {
+ return .0;
+ }
+ const double ratio{(s - startS) / (endS - startS)};
+ return Interpolate(startCurvature, endCurvature, ratio);
+}
+
template <typename Type, typename From>
std::ostream &operator<<(std::ostream &os, const Points<Type, From> &line)
{
diff --git a/include/OsiQueryLibrary/Object/StationaryObject.h b/include/OsiQueryLibrary/Object/StationaryObject.h
index 370d147dd1d7cd5ed9a131bc4ae9400a89b7d976..c096a45a623b962b0eb311c8571ae22c8e12e40d 100644
--- a/include/OsiQueryLibrary/Object/StationaryObject.h
+++ b/include/OsiQueryLibrary/Object/StationaryObject.h
@@ -1,5 +1,5 @@
/********************************************************************************
- * Copyright (c) 2022-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2022-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License 2.0 which is available at
@@ -21,13 +21,18 @@ namespace osiql {
//! \brief A stationary object that will never move.
struct StationaryObject : Object<osi3::StationaryObject>, Collidable<StationaryObject>
{
+ using Collidable<StationaryObject>::GetId;
+
template <typename Base, OSIQL_REQUIRES(std::is_constructible_v<Object<osi3::StationaryObject>, Base>)>
constexpr StationaryObject(Base &&base) : // NOLINT(bugprone-forwarding-reference-overload)
Object<osi3::StationaryObject>{std::forward<Base>(base)}, Collidable<StationaryObject>{Object<osi3::StationaryObject>::GetId()}
{
}
- using Collidable<StationaryObject>::GetId;
+ //! Replaces the handle of this object
+ //!
+ //! \return This object after having replaced its handle
+ StationaryObject &operator=(const osi3::StationaryObject &);
};
std::ostream &operator<<(std::ostream &, const StationaryObject &);
diff --git a/include/OsiQueryLibrary/Point/XY.tpp b/include/OsiQueryLibrary/Point/XY.tpp
index 9ed0579b7a84e320233261e80d4556cd1cfe2e51..2b68ed6aa8f6a6a2c89d09491b15a4773b33101d 100644
--- a/include/OsiQueryLibrary/Point/XY.tpp
+++ b/include/OsiQueryLibrary/Point/XY.tpp
@@ -1,5 +1,5 @@
/********************************************************************************
- * Copyright (c) 2022-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2022-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License 2.0 which is available at
@@ -12,6 +12,7 @@
#include "XY.h"
#include "Coordinates.tpp"
+#include "OsiQueryLibrary/Types/Geometry.h"
#include "OsiQueryLibrary/Utility/XYZ.h"
#include "Vector.tpp"
@@ -107,7 +108,7 @@ XY XY::GetPathTo(const A &a, const B &b, double epsilon) const
if (const double length{ab.SquaredLength()}; length > epsilon)
{
const double ratio{std::max(0.0, std::min(1.0, ac.Dot(ab) / length))};
- const XY projection{a * (1.0 - ratio) + b * ratio};
+ const XY projection{Interpolate(as<XY>(a), as<XY>(b), ratio)};
return *this - projection;
}
return ac;
@@ -192,7 +193,7 @@ ST XY::GetST(const A &a, const B &b) const
}
else
{
- t += extract<Side::Left>(a) * (1.0 - ratio) + extract<Side::Left>(b) * ratio;
+ t += Interpolate(extract<Side::Left>(a), extract<Side::Left>(b), ratio);
}
return {s, t};
}
diff --git a/include/OsiQueryLibrary/Routing/Route.h b/include/OsiQueryLibrary/Routing/Route.h
index 14e739d7364d5179908d4b618c05b266cb279ea4..f104bc51de7f8354b9bebe73e2f37749e30acc52 100644
--- a/include/OsiQueryLibrary/Routing/Route.h
+++ b/include/OsiQueryLibrary/Routing/Route.h
@@ -35,9 +35,9 @@ using StoredNode = std::shared_ptr<const Node<D, Scope>>;
//! \brief A local origin point, destination point and
//! a chain of nodes, one for each traversed road
template <LaneDirection D = LaneDirection::Forward, typename Scope = Road>
-struct Route : public Iterable<std::vector<StoredNode<D, Scope>>, Route<D>, LaneDirection>
+struct Route : public Iterable<std::vector<StoredNode<D, Scope>>, Route<D, Scope>, LaneDirection>
{
- using Base = Iterable<std::vector<StoredNode<D, Scope>>, Route<D>, LaneDirection>;
+ using Base = Iterable<std::vector<StoredNode<D, Scope>>, Route<D, Scope>, LaneDirection>;
static constexpr LaneDirection direction = D;
@@ -282,7 +282,7 @@ struct Route : public Iterable<std::vector<StoredNode<D, Scope>>, Route<D>, Lane
std::deque<std::shared_ptr<const Node<D, Lane>>> GetLaneNodes(const Lane &) const;
template <LaneDirection Toward = LaneDirection::Any>
- std::deque<std::shared_ptr<const Node<D, Lane>>> GetLaneNodes(const Lane &, ConstIterator<Route<D>>) const;
+ std::deque<std::shared_ptr<const Node<D, Lane>>> GetLaneNodes(const Lane &, ConstIterator<Route<D, Road>>) const;
Point<> origin;
Point<> destination;
diff --git a/include/OsiQueryLibrary/SensorView.h b/include/OsiQueryLibrary/SensorView.h
index 25514f3e23c7449022db89271bfd6f1ab59dd245..f432a46a9937677cbfec2795ef1e8627013c30fb 100644
--- a/include/OsiQueryLibrary/SensorView.h
+++ b/include/OsiQueryLibrary/SensorView.h
@@ -81,7 +81,7 @@ struct SensorView : Wrapper<osi3::SensorView>
//! Returns the route of this SensorView's VehicleData. HasRoute must be true.
//!
- //! @return Returns the route of this SensorView's VehicleData.
+ //! \return Returns the route of this SensorView's VehicleData.
const Route<> &GetRoute() const;
std::shared_ptr<World> world;
diff --git a/include/OsiQueryLibrary/Street/Lane/BoundaryEnclosure.h b/include/OsiQueryLibrary/Street/Lane/BoundaryEnclosure.h
index cf4dc5934622b0b9d029b50bd601c29f1209dd8e..d8bd509222c0d99cd20c9cab420f15b8df7daa91 100644
--- a/include/OsiQueryLibrary/Street/Lane/BoundaryEnclosure.h
+++ b/include/OsiQueryLibrary/Street/Lane/BoundaryEnclosure.h
@@ -232,7 +232,7 @@ struct BoundaryEnclosure : Adjoinable<Lane>, Area<XY>
//! Returns the global angle of this lane's centerline at the given s-coordinate.
//!
- //! \param s
+ //! \param s s-coordinate at which the angle is computed
//! \param LaneDirection Forward or Backward
//! \return double
template <auto Towards = LaneDirection::Forward>
@@ -240,27 +240,31 @@ struct BoundaryEnclosure : Adjoinable<Lane>, Area<XY>
//! Returns the global angle of this lane's centerline at the given s-coordinate.
//!
- //! \tparam Towards LaneDirection or Downstream
- //! \param Towards The traversal direction/orientation
- //! \param s
+ //! \tparam Towards LaneDirection or Downstream
+ //! \param Towards The direction in which this lane is traversed. If the s-coordinate is at the start of
+ //! one edge and at the end of another, the angle of the latter edge is preferred.
+ //! \param s s-coordinate at which the angle is computed
//! \return double
template <typename Towards>
double GetAngle(Towards, double s) const;
//! Returns the average of the curvatures of this lane's left and right boundary at the given s-coordinate.
+ //! If the s-coordinate lies outside the range of this lane, NaN is returned.
//!
- //! \param s
- //! \param LaneDirection Forward or Backward
- //! \return double
+ //! \tparam LaneDirection Forward or Backward
+ //! \param s s-coordinate at which the curvature shall be interpolated
+ //! \return Curvature at the given s-coordinate
template <auto Towards = LaneDirection::Forward>
double GetCurvature(double s) const;
//! Returns the average of the curvatures of this lane's left and right boundary at the given s-coordinate.
+ //! If the s-coordinate lies outside the range of this lane, NaN is returned.
//!
- //! \tparam RoadDirection Downstream or Upstream
- //! \param s
- //! \return double
- template <typename Towards>
+ //! \tparam Towards LaneDirection or RoadDirection. The direction in which this lane is traversed.
+ //! A leftward/counter-clockwise curvature is positive, a rightward/clockwise curvature is negative.
+ //! \param s s-coordinate at which the curvature is computed
+ //! \return Curvature at the given s-coordinate
+ template <typename Towards = LaneDirection>
double GetCurvature(Towards, double s) const;
//! Returns the t-coordinate of this lane's centerline at the given s-coordinate. If there is no centerline
diff --git a/include/OsiQueryLibrary/Street/Lane/BoundaryEnclosure.tpp b/include/OsiQueryLibrary/Street/Lane/BoundaryEnclosure.tpp
index 1f1805fc32fa4dd18ee8c30528820467c702c4a0..7745cd4b3cf040929c4efb8a12956292f508a91b 100644
--- a/include/OsiQueryLibrary/Street/Lane/BoundaryEnclosure.tpp
+++ b/include/OsiQueryLibrary/Street/Lane/BoundaryEnclosure.tpp
@@ -1,5 +1,5 @@
/********************************************************************************
- * Copyright (c) 2023-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2023-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License 2.0 which is available at
@@ -16,6 +16,7 @@
#include "Adjoinable.tpp"
#include "OsiQueryLibrary/Point/XY.tpp"
#include "OsiQueryLibrary/Street/BoundaryChain.tpp"
+#include "OsiQueryLibrary/Types/Geometry.h"
namespace osiql::detail {
template <typename Lane>
@@ -238,7 +239,7 @@ double BoundaryEnclosure<Lane>::GetAngle(double s) const
{
const double leftLaneAngle{GetBoundary<Side::Left, Towards>(s).template GetAngle<Towards>(s)};
const double rightLaneAngle{GetBoundary<Side::Right, Towards>(s).template GetAngle<Towards>(s)};
- return 0.5 * leftLaneAngle + 0.5 * rightLaneAngle; // NOLINT(cppcoreguidelines-avoid-magic-numbers, readability-magic-numbers)
+ return Average(leftLaneAngle, rightLaneAngle);
}
else
{
@@ -257,27 +258,24 @@ template <typename Lane>
template <auto Towards>
double BoundaryEnclosure<Lane>::GetCurvature(double s) const
{
+ static_assert(std::is_same_v<decltype(Towards), LaneDirection> || std::is_same_v<decltype(Towards), RoadDirection>);
if constexpr (std::is_same_v<decltype(Towards), LaneDirection>)
{
return GetCurvature(this->GetDirection(Towards), s);
}
- else if constexpr (std::is_same_v<decltype(Towards), RoadDirection>)
+ else
{
const double leftCurvature{GetBoundary<Side::Left, Towards>(s).template GetCurvature<Towards>(s)};
const double rightCurvature{GetBoundary<Side::Right, Towards>(s).template GetCurvature<Towards>(s)};
- return 0.5 * leftCurvature + 0.5 * rightCurvature; // NOLINT(cppcoreguidelines-avoid-magic-numbers, readability-magic-numbers)
- }
- else
- {
- static_assert(always_false<decltype(Towards)>, "Not supported");
+ return Average(leftCurvature, rightCurvature);
}
}
template <typename Lane>
-template <typename Orientation>
-double BoundaryEnclosure<Lane>::GetCurvature(Orientation toward, double s) const
+template <typename Toward>
+double BoundaryEnclosure<Lane>::GetCurvature(Toward toward, double s) const
{
- return IsInverse(toward) ? GetCurvature<!Default<Orientation>>(s) : GetCurvature<Default<Orientation>>(s);
+ return IsInverse(toward) ? GetCurvature<!Default<Toward>>(s) : GetCurvature<Default<Toward>>(s);
}
template <typename Lane>
@@ -285,7 +283,7 @@ double BoundaryEnclosure<Lane>::GetCenterlineT(double s) const
{
const double leftT{GetBoundary<Side::Left>(s).GetT(s)};
const double rightT{GetBoundary<Side::Right>(s).GetT(s)};
- return 0.5 * leftT + 0.5 * rightT; // NOLINT(cppcoreguidelines-avoid-magic-numbers, readability-magic-numbers)
+ return Average(leftT, rightT);
}
template <typename Lane>
diff --git a/include/OsiQueryLibrary/Street/ReferenceLine.h b/include/OsiQueryLibrary/Street/ReferenceLine.h
index a38069a0e5ab60217614ef8e3322888ddda74998..0e3e9f6d8486c155259b47af895817aec4e5fb25 100644
--- a/include/OsiQueryLibrary/Street/ReferenceLine.h
+++ b/include/OsiQueryLibrary/Street/ReferenceLine.h
@@ -84,6 +84,11 @@ struct ReferenceLine : Identifiable<osi3::ReferenceLine>, Points<Container<osi3:
//! \return Type The type of this reference line or the fallback value if it has no type.
constexpr Type GetType(Type fallback = Type::Polyline) const;
+ //! Returns the length of this reference line
+ //!
+ //! \return s-coordinate of the last point of this reference line
+ double GetLength() const;
+
//! Returns the local st-coordinate representation of the given global point
//! according to the coordinate system defined by this reference line.
//!
diff --git a/include/OsiQueryLibrary/Trait/Collection.h b/include/OsiQueryLibrary/Trait/Collection.h
index 3e0a77162fbc0f59ac9790ef64076f2a6d671eef..84d1fb6350ba9bb8e0bd8bc6f98932efe0058393 100644
--- a/include/OsiQueryLibrary/Trait/Collection.h
+++ b/include/OsiQueryLibrary/Trait/Collection.h
@@ -1,5 +1,5 @@
/********************************************************************************
- * Copyright (c) 2022-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2022-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License 2.0 which is available at
@@ -18,6 +18,7 @@
#include "Id.h"
#include "OsiQueryLibrary/Component/Iterable.h"
#include "OsiQueryLibrary/Object/MovingObject.hpp"
+#include "OsiQueryLibrary/Object/StationaryObject.hpp"
#include "OsiQueryLibrary/Types/Container.h"
namespace osiql {
@@ -53,12 +54,20 @@ struct Collection<MovingObject>
using type = HashMap<std::unique_ptr<MovingObject>>;
};
+//! \brief Type trait specifying how stationary objects are stored in osiql::GroundTruth
+template <>
+struct Collection<StationaryObject>
+{
+ //! \brief How stationary objects are stored in osiql::GroundTruth
+ using type = HashMap<std::unique_ptr<StationaryObject>>;
+};
+
//! \brief Type trait specifying how light bulbs are stored in osiql::GroundTruth
template <>
struct Collection<LightBulb>
{
//! \brief How light bulbs are stored in osiql::GroundTruth
- using type = std::map<Id, TrafficLight *>;
+ using type = HashMap<TrafficLight *>;
};
//! \brief Type trait specifying how roads are stored in osiql::GroundTruth
diff --git a/include/OsiQueryLibrary/Trait/Handle.h b/include/OsiQueryLibrary/Trait/Handle.h
index 5a50635fc9b40ade55d3eea33339468363a80f27..8e2d45a0a3a6a351f21e38856c08e82d7b5e1b4f 100644
--- a/include/OsiQueryLibrary/Trait/Handle.h
+++ b/include/OsiQueryLibrary/Trait/Handle.h
@@ -1,5 +1,5 @@
/********************************************************************************
- * Copyright (c) 2023-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2023-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License 2.0 which is available at
@@ -32,6 +32,13 @@ struct Handle<osi3::MovingObject>
//! \brief The full non-decayed type of the wrapper object's handle
using type = const osi3::MovingObject *;
};
+
+template <>
+struct Handle<osi3::StationaryObject>
+{
+ //! \brief The full non-decayed type of the wrapper object's handle
+ using type = const osi3::StationaryObject *;
+};
} // namespace trait
//! OSI object handle. Usually a reference_wrapper
diff --git a/include/OsiQueryLibrary/Types/Circle.h b/include/OsiQueryLibrary/Types/Circle.h
new file mode 100644
index 0000000000000000000000000000000000000000..abc57954331518c55595b0021c1141a4b59b9048
--- /dev/null
+++ b/include/OsiQueryLibrary/Types/Circle.h
@@ -0,0 +1,73 @@
+/********************************************************************************
+ * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ *
+ * 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
+ ********************************************************************************/
+#pragma once
+
+#include <iostream>
+#include <optional>
+
+#include "OsiQueryLibrary/Point/XY.h"
+#include "OsiQueryLibrary/Types/Common.h"
+
+namespace osiql {
+//! \brief Circle represented by a global 2d xy-coordinate pair center and a radius
+struct Circle
+{
+ //! Constructs a circle from its components
+ //!
+ //! \param center The global center of the circle
+ //! \param radius The radius of the circle
+ constexpr Circle(XY center, double radius);
+
+ //! Creates a circle from three unique points on its perimeter
+ //!
+ //! \param a First point on the circle
+ //! \param b Second point on the circle
+ //! \param c Third point on the circle
+ Circle(const XY &a, const XY &b, const XY &c);
+
+ //! Returns the curvature of this circle's perimeter, which is the reciprocal of its radius.
+ //!
+ //! \return Curvature of this circle's perimeter
+ //! \note If the radius is 0, a curvature of 0 is returned
+ constexpr double GetCurvature() const;
+
+ //! \brief Center of this circle
+ XY center;
+
+ //! \brief Distance from the circle's center to its perimeter
+ double radius;
+};
+
+//! Returns the center of the circle on which the three given points lie
+//!
+//! \param XY First point on the circle
+//! \param XY Second point on the circle
+//! \param XY Third point on the circle
+//! \param epsilon Squared rounding error to test for duplicate points. If points are sufficiently
+//! close to one another, returns 0. The first and third point are assumed to be different.
+//! \return nullopt if the points are collinear, otherwise the center of the circle
+std::optional<XY> GetCenterOf(const XY &, const XY &, const XY &, double epsilon = EPSILON);
+
+std::optional<XY> GetTangentCenterOf(const XY &a, const XY &b, const XY &c, double epsilon = EPSILON);
+
+std::ostream &operator<<(std::ostream &, const Circle &);
+} // namespace osiql
+
+namespace osiql {
+constexpr Circle::Circle(XY center, double radius) :
+ center{std::move(center)}, radius{radius}
+{
+}
+
+constexpr double Circle::GetCurvature() const
+{
+ return radius == .0 ? .0 : (1.0 / radius);
+}
+} // namespace osiql
diff --git a/include/OsiQueryLibrary/Types/Geometry.h b/include/OsiQueryLibrary/Types/Geometry.h
index 173d6a85123a59b60dfb5113a09e91e1635144e2..89ca2624ae828c1aebf7de8b20e625cfc343ccdb 100644
--- a/include/OsiQueryLibrary/Types/Geometry.h
+++ b/include/OsiQueryLibrary/Types/Geometry.h
@@ -1,5 +1,5 @@
/********************************************************************************
- * Copyright (c) 2022-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2022-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License 2.0 which is available at
@@ -34,8 +34,26 @@ struct Line
double GetRatio(const XY &) const;
};
-std::ostream &
-operator<<(std::ostream &, const Line &);
+//! Returns the linear interpolation of two inputs at a given value, ergo "A + (B - A) * value".
+//!
+//! \tparam A Type that supports addition with type B and scalar multiplication
+//! \tparam B Type that supports addition with type A and scalar multiplication
+//! \param A "Start" of the interpolation. At value=0, A will be returned
+//! \param B "End" of the interpolation. At value=1, B will be returned
+//! \param value Factor affecting the weighting of each input. It is not restricted to [0, 1].
+//! \return The interpolated value between A and B
+template <typename A, typename B>
+constexpr auto Interpolate(const A &, const B &, double value);
+
+//! Returns the arithmetic average of the two given inputs
+//!
+//! \tparam A Type that supports addition with type B and scalar multiplication
+//! \tparam B Type that supports addition with type A and scalar multiplication
+//! \return The mean between A and B
+template <typename A, typename B>
+constexpr auto Average(const A &, const B &);
+
+std::ostream &operator<<(std::ostream &, const Line &);
//! Returns the equivalent to the given angle within the range (-pi, pi]
//!
@@ -54,4 +72,16 @@ constexpr double WrapAngle(double angle) noexcept
return result + twoPi;
return result;
}
+
+template <typename A, typename B>
+constexpr auto Interpolate(const A &a, const B &b, double value)
+{
+ return a + value * (b - a);
+}
+
+template <typename A, typename B>
+constexpr auto Average(const A &a, const B &b)
+{
+ return Interpolate(a, b, .5);
+}
} // namespace osiql
diff --git a/include/OsiQueryLibrary/Types/Interval.h b/include/OsiQueryLibrary/Types/Interval.h
index 2ccf3859902416a7ab1de69c5c925f59bd3a5b11..f8c3e8c7199846c4556f9ef775dabbfb81f7dec4 100644
--- a/include/OsiQueryLibrary/Types/Interval.h
+++ b/include/OsiQueryLibrary/Types/Interval.h
@@ -11,6 +11,7 @@
#include <iostream>
#include <limits>
+#include <type_traits>
#include "OsiQueryLibrary/Utility/Common.h"
@@ -63,10 +64,35 @@ struct Interval
template <typename Type>
Interval(Type &&, Type &&) -> Interval<Type>;
-constexpr double GetDistanceBetween(const Interval<double> &, const Interval<double> &);
-constexpr double GetDistanceBetween(const Interval<double> &, double);
-constexpr double GetDistanceBetween(double, const Interval<double> &);
-constexpr double GetDistanceBetween(double, double);
+template <typename A, typename B, OSIQL_REQUIRES(std::is_arithmetic_v<A> &&std::is_arithmetic_v<B>)>
+constexpr double GetDistanceBetween(const Interval<A> &a, const Interval<B> &b)
+{
+ if (b.min > a.max) { return b.min - a.max; }
+ if (a.min > b.max) { return b.max - a.min; }
+ return .0;
+}
+
+template <typename A, typename B, OSIQL_REQUIRES(std::is_arithmetic_v<A> &&std::is_arithmetic_v<B>)>
+constexpr double GetDistanceBetween(const Interval<A> &a, B &&b)
+{
+ if (b > a.max) { return b - a.max; }
+ if (b < a.min) { return b - a.min; }
+ return .0;
+}
+
+template <typename A, typename B, OSIQL_REQUIRES(std::is_arithmetic_v<A> &&std::is_arithmetic_v<B>)>
+constexpr double GetDistanceBetween(A &&a, const Interval<B> &b)
+{
+ if (a < b.min) { return b.min - a; }
+ if (a > b.max) { return b.max - a; }
+ return .0;
+}
+
+template <typename A, typename B, OSIQL_REQUIRES(std::is_arithmetic_v<A> &&std::is_arithmetic_v<B>)>
+constexpr double GetDistanceBetween(A &&a, B &&b)
+{
+ return b - a;
+}
template <typename Type>
constexpr bool operator==(const Interval<Type> &, const Interval<Type> &);
diff --git a/include/OsiQueryLibrary/Types/Interval.tpp b/include/OsiQueryLibrary/Types/Interval.tpp
index 5be1148c2c8fdfcb047a6e7e5ab5b65c841fb5ae..882539c23966fccb7b4baa2c7188b48741ec5137 100644
--- a/include/OsiQueryLibrary/Types/Interval.tpp
+++ b/include/OsiQueryLibrary/Types/Interval.tpp
@@ -27,26 +27,6 @@ constexpr Interval<Type>::Interval(const OtherType &u) :
{
}
-constexpr double GetDistanceBetween(const Interval<double> &a, const Interval<double> &b)
-{
- return std::max({0.0, a.min - b.max, b.min - a.max});
-}
-
-constexpr double GetDistanceBetween(const Interval<double> &a, double b)
-{
- return std::max({0.0, a.min - b, b - a.max});
-}
-
-constexpr double GetDistanceBetween(double a, const Interval<double> &b)
-{
- return std::max({0.0, a - b.max, b.min - a});
-}
-
-constexpr double GetDistanceBetween(double a, double b)
-{
- return std::abs(b - a);
-}
-
template <typename Type>
constexpr bool operator==(const Interval<Type> &lhs, const Interval<Type> &rhs)
{
diff --git a/include/OsiQueryLibrary/Utility/Compare.tpp b/include/OsiQueryLibrary/Utility/Compare.tpp
index 91b4ff47db0c1f7b7189356e56e14c2de96066ef..1805f3534c0f55e757f3f21d45c7699c10864ed3 100644
--- a/include/OsiQueryLibrary/Utility/Compare.tpp
+++ b/include/OsiQueryLibrary/Utility/Compare.tpp
@@ -60,7 +60,7 @@ constexpr bool StableCompare<TypeA, TypeB, Comparator>::operator()(A &&a, B &&b)
template <typename A> \
constexpr bool NAME<TypeA, TypeB, Comparator>::SUBNAME<B>::operator()(A &&a) const \
{ \
- return NAME<TypeA, Forward, Comparator>{}(std::forward<A>(a), b); \
+ return NAME<TypeA, TypeB, Comparator>{}(std::forward<A>(a), b); \
}
OSIQL_DEFINE_COMPARISON_TO_OPERATOR(Compare, Than)
diff --git a/include/OsiQueryLibrary/Utility/Has.h b/include/OsiQueryLibrary/Utility/Has.h
index a1e40fd86c3b661691651d5e1a95753f90a71e64..b6bfe47ef01cb58a4016cdbca3a7d2cd3ff4f72c 100644
--- a/include/OsiQueryLibrary/Utility/Has.h
+++ b/include/OsiQueryLibrary/Utility/Has.h
@@ -46,9 +46,9 @@ namespace osiql {
namespace detail {
//! Wrapper of a type that can return its member using Get<Type>()
//!
-//! @tparam Type The decayed type of the stored member. Specified in order to
-//! specialization of this class for sets of types that shared the same decayed type
-//! @tparam StoredType The exact type of the stored member
+//! \tparam Type The decayed type of the stored member. Specified in order to specialize
+//! this class for sets of types that shared the same decayed type
+//! \tparam StoredType The exact type of the stored member
template <typename Type, typename StoredType = Type>
class Has
{
diff --git a/include/OsiQueryLibrary/World.h b/include/OsiQueryLibrary/World.h
index ee78247415e9cac7bdf4d07a51899652b1ea6775..571d102f37962370fbb8860551bd9b9217f67ba8 100644
--- a/include/OsiQueryLibrary/World.h
+++ b/include/OsiQueryLibrary/World.h
@@ -1,5 +1,5 @@
/********************************************************************************
- * Copyright (c) 2022-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2022-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License 2.0 which is available at
@@ -50,17 +50,20 @@ bool HasIntersections(const PolygonA &, const PolygonB &);
template <typename Type>
using RTree = boost::geometry::index::rtree<Type *, boost::geometry::index::quadratic<8, 4>, osiql::Get<Bounds<XY>>>;
-//! \brief A World offers location world methods, xy- <-> st-coordinate conversion and bidirectional road to lane to object mappings.
+//! \brief A World is a spatially indexed, updatable GroundTruth.
struct World : GroundTruth
{
//! Initialize the world with a fully defined ground truth.
World(const osi3::GroundTruth &);
- //! Updates (includes adding and removing) all moving objects given the
- //! collection of object handles. Recomputes all lane & road overlaps
+ //! Replaces all objects given the collection of object handles.
+ //! Recomputes all shapes, bounds & lane/road overlaps
//!
- //! \param allObjects Container of osi3::MovingObject sorted by id
- void Update(const Container<osi3::MovingObject> &allObjects);
+ //! \param Container Container of osi3::MovingObjects
+ template <typename Type>
+ void UpdateAll(const Container<typename Type::Handle> &);
+
+ [[deprecated("Prefer 'UpdateAll<MovingObject>'")]] void Update(const Container<osi3::MovingObject> &);
//! Returns the lane with the given id or nullptr if none exists
//!
@@ -223,7 +226,21 @@ struct World : GroundTruth
//! \return std::ostream&
friend std::ostream &operator<<(std::ostream &, const World &);
+ //! Returns the boost rectangle tree used to efficiently localize object of the given template type
+ //!
+ //! \tparam Type Lane, Road, MovingObject, StationaryObject, TrafficLight or TrafficSign
+ //! \return Rectangle tree of objects of the given template type
+ template <typename Type>
+ constexpr const RTree<Type> &GetRTree() const;
+
private:
+ //! Returns the boost rectangle tree used to efficiently localize object of the given template type
+ //!
+ //! \tparam Type Lane, Road, MovingObject, StationaryObject, TrafficLight or TrafficSign
+ //! \return Rectangle tree of objects of the given template type
+ template <typename Type>
+ constexpr RTree<Type> &GetRTree();
+
template <typename Type = Lane>
std::vector<Overlap<Type>> InternalFindOverlapping(const Area<XY> &);
@@ -233,19 +250,39 @@ private:
//! \return std::vector<const Lane*> Empty if no lane contains the given point
std::vector<const Lane *> GetLanesContaining(const XY &) const;
- //! Updates the global shape, local positions and lane intersection bounds of the given object.
+ //! Updates the local positions and lane intersection bounds of the given object.
//!
//! \param object
template <typename ObjectType>
- void UpdateObject(ObjectType &object);
+ void UpdateOverlaps(ObjectType &object);
- template <typename Type>
- constexpr const RTree<Type> &GetRTree() const;
+ //! Removes all lane and road overlaps.
+ //!
+ //! \tparam ObjectType MovingObject or StationaryObject
+ template <typename ObjectType>
+ void ClearOverlaps();
+
+ //! Updates and all objects of the given template type. Objects not in the input are removed.
+ //! This involves computing their global shape, global bounds, as well as lane and road intersections.
+ //!
+ //! \tparam ObjectType MovingObject or StationaryObject
+ //! \param handles OSI repated pointer field of object handles
+ //! \return Collection of ids of objects not among the given input
+ template <typename ObjectType>
+ std::unordered_set<Id> AddAll(const Container<typename ObjectType::Handle> &handles);
+ //! Removes objects with the given ids.
+ //!
+ //! \tparam Type MovingObject or StationaryObject
template <typename Type>
- constexpr RTree<Type> &GetRTree();
+ void RemoveObjects(const std::unordered_set<Id> &);
+
+ //! Clears and refills the RTree of the given template type
+ //!
+ //! \tparam ObjectType Lane, Road, MovingObject, StationaryObject, TrafficLight or TrafficSign
+ template <typename ObjectType>
+ void UpdateRTree();
-private:
template <LaneDirection D, typename Scope, typename Origin, typename Destination>
std::optional<Route<D, Scope>> GetRouteBetweenGlobalPoints(const Origin &, const Destination &) const;
@@ -264,7 +301,6 @@ private:
template <LaneDirection D, typename Scope, typename PointIterator>
std::optional<Route<D, Scope>> GetRouteFromRangeOfLocalPoints(PointIterator first, PointIterator pastLast) const;
-public:
RTree<Lane> laneRTree;
RTree<Road> roadRTree;
RTree<MovingObject> movingObjectRTree;
diff --git a/include/OsiQueryLibrary/World.tpp b/include/OsiQueryLibrary/World.tpp
index 2ffdba971d719387417dd5474e5ce09aeda4ef46..1eb998474edbec3323ae6d1168ac434b015746d2 100644
--- a/include/OsiQueryLibrary/World.tpp
+++ b/include/OsiQueryLibrary/World.tpp
@@ -1,5 +1,5 @@
/********************************************************************************
- * Copyright (c) 2022-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2022-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License 2.0 which is available at
@@ -20,9 +20,8 @@
namespace osiql {
template <typename ObjectType>
-void World::UpdateObject(ObjectType &object)
+void World::UpdateOverlaps(ObjectType &object)
{
- object.UpdateShapeAndBounds();
object.template SetOverlaps<Road>(InternalFindOverlapping<Road>(object));
// TODO: Performance - Stop computing object centers
object.positions.clear();
@@ -34,6 +33,69 @@ void World::UpdateObject(ObjectType &object)
object.template SetOverlaps<Lane>(InternalFindOverlapping<Lane>(object));
}
+template <typename Type>
+void World::ClearOverlaps()
+{
+ for (auto &[id, lane] : lanes)
+ {
+ lane.template GetOverlapping<Type>().clear();
+ }
+ for (auto &[id, road] : roads)
+ {
+ road.template GetOverlapping<Type>().clear();
+ }
+}
+
+template <typename Type>
+std::unordered_set<Id> World::AddAll(const Container<typename Type::Handle> &objects)
+{
+ std::unordered_set<Id> leftovers;
+ std::transform(GetAll<Type>().begin(), GetAll<Type>().end(), std::inserter(leftovers, leftovers.end()), osiql::Get<Id>{});
+ for (const auto &object : objects)
+ {
+ if (const auto match{GetAll<Type>().find(get<Id>(object))}; match != GetAll<Type>().end())
+ {
+ auto &wrapper{get<Type>(*match)};
+ wrapper = object;
+ UpdateOverlaps<Type>(wrapper);
+ leftovers.erase(get<Id>(object));
+ }
+ else
+ {
+ UpdateOverlaps<Type>(Emplace<Type>(GetAll<Type>(), object));
+ }
+ }
+ return leftovers;
+}
+
+template <typename Type>
+void World::UpdateRTree()
+{
+ GetRTree<Type>().clear();
+ for (auto &item : GetAll<Type>())
+ {
+ GetRTree<Type>().insert(&get<Type>(item));
+ }
+}
+
+template <typename Type>
+void World::RemoveObjects(const std::unordered_set<Id> &ids)
+{
+ for (const auto &id : ids)
+ {
+ GetAll<Type>().erase(get<Id>(id));
+ }
+}
+
+template <typename Type>
+void World::UpdateAll(const Container<typename Type::Handle> &objects)
+{
+ ClearOverlaps<Type>();
+ auto danglingIds{AddAll<Type>(objects)};
+ RemoveObjects<Type>(danglingIds);
+ UpdateRTree<Type>();
+}
+
template <typename GlobalPoint>
std::vector<Localization<GlobalPoint>> World::Localize(const GlobalPoint &point) const
{
diff --git a/include/OsiQueryLibrary/osiql.h b/include/OsiQueryLibrary/osiql.h
index 9c1cc43773892385b896b3c966dc31d061a494e3..8ce51e964e638b1f138216082762baf8c25b08a4 100644
--- a/include/OsiQueryLibrary/osiql.h
+++ b/include/OsiQueryLibrary/osiql.h
@@ -80,6 +80,7 @@
#include "OsiQueryLibrary/Types/Enum/Side.tpp"
//
#include "OsiQueryLibrary/Types/Bounds.h"
+#include "OsiQueryLibrary/Types/Circle.h"
#include "OsiQueryLibrary/Types/Common.h"
#include "OsiQueryLibrary/Types/Constants.h"
#include "OsiQueryLibrary/Types/Container.h"
diff --git a/src/Component/Points.cpp b/src/Component/Points.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..96dbb8ebddb7297fc4e09664b42d130d87081b25
--- /dev/null
+++ b/src/Component/Points.cpp
@@ -0,0 +1,31 @@
+/********************************************************************************
+ * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ *
+ * 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
+ ********************************************************************************/
+#include "OsiQueryLibrary/Component/Points.tpp"
+
+#include "OsiQueryLibrary/Types/Circle.h"
+
+namespace osiql::detail {
+double GetCurvature(const XY &a, const XY &b, const XY &c)
+{
+ if (std::optional<XY> center{GetCenterOf(a, b, c)}; center.has_value())
+ {
+ const double squaredLength{(center - a).SquaredLength()};
+ if (squaredLength < 0.5) { return .0; }
+ const double curvature{1.0 / std::sqrt(squaredLength)};
+ return center.value().GetSide(a, b) == Side::Left ? curvature : -curvature;
+ }
+ return .0;
+}
+
+double GetCurvature(double ratio, const XY &a, const XY &b, const XY &c, const XY &d) // NOLINT(readability-identifier-length)
+{
+ return Interpolate(GetCurvature(a, b, c), GetCurvature(b, c, d), ratio);
+}
+} // namespace osiql::detail
diff --git a/src/GroundTruth.cpp b/src/GroundTruth.cpp
index 658b8c223523ed62daccaffbe4faf8fb8ce413e7..d2e1ecb1a54983788b21854d67781f32d25d3de5 100644
--- a/src/GroundTruth.cpp
+++ b/src/GroundTruth.cpp
@@ -1,5 +1,5 @@
/********************************************************************************
- * Copyright (c) 2022-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2022-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License 2.0 which is available at
@@ -43,13 +43,13 @@ GroundTruth::~GroundTruth()
// When a moving object is destroyed, it detaches itself from its touched lanes. Lanes shouldn't
// be modified directly prior to destruction and object destruction fails if its connected lanes
// no longer exist, so the best option is to prevent objects from detaching themselves in advance:
- for (auto &[id, object] : movingObjects)
+ for (auto &entry : movingObjects)
{
- object->positions.clear();
+ get<MovingObject>(entry).positions.clear();
}
- for (auto &object : stationaryObjects)
+ for (auto &entry : stationaryObjects)
{
- object.positions.clear();
+ get<StationaryObject>(entry).positions.clear();
}
}
diff --git a/src/Object/StationaryObject.cpp b/src/Object/StationaryObject.cpp
index f96bbc3f57090ac38baace7fc77287cc34e16839..ca765a187a12782e21cd6ee7203cbcd2a77b979d 100644
--- a/src/Object/StationaryObject.cpp
+++ b/src/Object/StationaryObject.cpp
@@ -1,5 +1,5 @@
/********************************************************************************
- * Copyright (c) 2022-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2022-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License 2.0 which is available at
@@ -12,10 +12,20 @@
#include "OsiQueryLibrary/Object/Collidable.tpp"
#include "OsiQueryLibrary/Object/Object.tpp"
#include "OsiQueryLibrary/Utility/Common.tpp"
+#include "OsiQueryLibrary/Utility/Name.h"
namespace osiql {
+StationaryObject &StationaryObject::operator=(const osi3::StationaryObject &handle)
+{
+ this->handle = &handle;
+ OutdateTranslation();
+ OutdateRotation();
+ UpdateShapeAndBounds();
+ return *this;
+}
+
std::ostream &operator<<(std::ostream &os, const StationaryObject &object)
{
- return os << "Static object " << object.GetId() << " [" << object.GetShape() << ']';
+ return os << name<StationaryObject> << ' ' << object.GetId() << " [" << object.GetShape() << ']';
}
} // namespace osiql
diff --git a/src/Street/ReferenceLine.cpp b/src/Street/ReferenceLine.cpp
index 4352cfa684d0e4c3c6448cc8247cb84248121fc4..8567bbb1403b459cab5b1052042da67feccdb068 100644
--- a/src/Street/ReferenceLine.cpp
+++ b/src/Street/ReferenceLine.cpp
@@ -61,7 +61,7 @@ XY ReferenceLine::GetXY(const ST &point) const
--it;
}
const double ratio{(point.s - std::prev(it)->s_position()) / (it->s_position() - std::prev(it)->s_position())};
- const XY pointOnEdge{XY{std::prev(it)->world_position()} * (1.0 - ratio) + XY{it->world_position()} * ratio};
+ const XY pointOnEdge{Interpolate(XY{std::prev(it)->world_position()}, XY{it->world_position()}, ratio)};
const size_t edgeIndex{static_cast<size_t>(std::distance(begin(), it) - 1)};
const std::optional<XY> &vanishingPoint{GetLongitudinalAxisIntersection(edgeIndex)};
if (vanishingPoint.has_value())
@@ -95,7 +95,7 @@ ST ReferenceLine::LocalizeUsingTAxes(const XY &input) const
const XY intersection{Line{vanishingPoint.value(), input}.GetIntersection(edge).value()};
const double ratio{edge.GetRatio(intersection)};
return ST{
- extract<RoadDirection::Downstream>(*std::prev(it)) * (1.0 - ratio) + extract<RoadDirection::Downstream>(*it) * ratio,
+ Interpolate(extract<RoadDirection::Downstream>(*std::prev(it)), extract<RoadDirection::Downstream>(*it), ratio),
input.GetSide(edge.start, edge.end) == Side::Right ? -(input - intersection).Length() : (input - intersection).Length() //
};
}
@@ -114,13 +114,18 @@ ST ReferenceLine::LocalizeUsingTAxes(const XY &input) const
const XY intersection{edge.GetIntersection(Line{input, input + axis}).value()};
const double ratio{edge.GetRatio(intersection)};
return ST{
- extract<RoadDirection::Downstream>(*std::prev(it)) * (1.0 - ratio) + extract<RoadDirection::Downstream>(*it) * ratio,
+ Interpolate(extract<RoadDirection::Downstream>(*std::prev(it)), extract<RoadDirection::Downstream>(*it), ratio),
input.GetSide(edge.start, edge.end) == Side::Right ? -(input - intersection).Length() : (input - intersection).Length() //
};
}
return input.GetST(*std::prev(it), *it);
}
+double ReferenceLine::GetLength() const
+{
+ return get<S>(back());
+}
+
std::ostream &operator<<(std::ostream &os, const ReferenceLine &line)
{
return os << "Reference Line " << line.GetId() << ": " << static_cast<const ReferenceLine::Base &>(line);
diff --git a/src/Types/Circle.cpp b/src/Types/Circle.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e78e890f18982691f8d9245327f058f7834aa4ef
--- /dev/null
+++ b/src/Types/Circle.cpp
@@ -0,0 +1,40 @@
+/********************************************************************************
+ * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ *
+ * 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
+ ********************************************************************************/
+#include "OsiQueryLibrary/Types/Circle.h"
+
+#include "OsiQueryLibrary/Point/XY.tpp"
+#include "OsiQueryLibrary/Types/Geometry.h"
+
+namespace osiql {
+Circle::Circle(const XY &a, const XY &b, const XY &c) :
+ center{GetCenterOf(a, b, c).value()}, radius{(center - a).Length()}
+{
+}
+
+std::optional<XY> GetCenterOf(const XY &a, const XY &b, const XY &c, double epsilon)
+{
+ if ((b - a).SquaredLength() <= epsilon || (c - b).SquaredLength() <= epsilon)
+ {
+ return std::nullopt;
+ }
+ const XY abMid{Average(a, b)};
+ const Line abMirror{abMid, abMid + (b - a).GetPerpendicular<Winding::Clockwise>()};
+
+ const XY bcMid{Average(b, c)};
+ const Line bcMirror{bcMid, bcMid + (c - b).GetPerpendicular<Winding::Clockwise>()};
+
+ return abMirror.GetIntersection(bcMirror);
+}
+
+std::ostream &operator<<(std::ostream &os, const Circle &circle)
+{
+ return os << "(Center: " << circle.center << ", Radius: " << circle.radius << ')';
+}
+} // namespace osiql
diff --git a/src/Types/LaneDirection.cpp b/src/Types/Enum/LaneDirection.cpp
similarity index 100%
rename from src/Types/LaneDirection.cpp
rename to src/Types/Enum/LaneDirection.cpp
diff --git a/src/Types/RoadDirection.cpp b/src/Types/Enum/RoadDirection.cpp
similarity index 100%
rename from src/Types/RoadDirection.cpp
rename to src/Types/Enum/RoadDirection.cpp
diff --git a/src/Types/Side.cpp b/src/Types/Enum/Side.cpp
similarity index 100%
rename from src/Types/Side.cpp
rename to src/Types/Enum/Side.cpp
diff --git a/src/World.cpp b/src/World.cpp
index 29deb180c76b2c785a3f5a090c99dd3cf22902c0..4bd82933c9d78e73cceb384e53aed4d8def35e08 100644
--- a/src/World.cpp
+++ b/src/World.cpp
@@ -1,5 +1,5 @@
/********************************************************************************
- * Copyright (c) 2022-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2022-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License 2.0 which is available at
@@ -30,14 +30,14 @@ World::World(const osi3::GroundTruth &groundTruth) :
for (auto &item : GetAll<MovingObject>())
{
- UpdateObject(get<MovingObject>(item));
+ UpdateOverlaps(get<MovingObject>(item));
movingObjectRTree.insert(&get<MovingObject>(item));
}
- for (auto &object : GetAll<StationaryObject>())
+ for (auto &item : GetAll<StationaryObject>())
{
- UpdateObject(object);
- stationaryObjectRTree.insert(&object);
+ UpdateOverlaps(get<StationaryObject>(item));
+ stationaryObjectRTree.insert(&get<StationaryObject>(item));
}
}
@@ -62,49 +62,7 @@ const Vehicle *World::GetVehicle(Id id) const
void World::Update(const Container<osi3::MovingObject> &objects)
{
- // Remove all overlaps
- for (auto &[id, lane] : lanes)
- {
- lane.template GetOverlapping<MovingObject>().clear();
- }
- for (auto &[id, road] : roads)
- {
- road.template GetOverlapping<MovingObject>().clear();
- }
- // Spawn/Update objects
- for (const auto &object : objects)
- {
- if (const auto match{GetAll<MovingObject>().find(get<Id>(object))}; match != GetAll<MovingObject>().end())
- {
- auto &wrapper{*match->second};
- wrapper = object;
- UpdateObject(wrapper);
- }
- else
- {
- UpdateObject(Emplace<MovingObject>(GetAll<MovingObject>(), object));
- }
- }
- // Despawn objects
- if (auto &leftovers{GetAll<MovingObject>()}; leftovers.size() > static_cast<size_t>(objects.size()))
- {
- std::unordered_set<Id> ids;
- std::transform(leftovers.begin(), leftovers.end(), std::inserter(ids, ids.end()), osiql::Get<Id>{});
- for (const auto &object : objects)
- {
- ids.erase(get<Id>(object));
- }
- for (Id id : ids)
- {
- leftovers.erase(id);
- }
- }
- // Refresh RTree
- GetRTree<MovingObject>().clear();
- for (auto &item : GetAll<MovingObject>())
- {
- GetRTree<MovingObject>().insert(&get<MovingObject>(item));
- }
+ UpdateAll<MovingObject>(objects);
}
World *GetWorld(const osi3::GroundTruth &groundTruth)
diff --git a/tests/unitTests/CMakeLists.txt b/tests/unitTests/CMakeLists.txt
index 0eb1f6fd2d0fc61f2dfc2d316493dc27869d8b07..a16aa511f797fe93ffadae155e43a6e34805be6a 100644
--- a/tests/unitTests/CMakeLists.txt
+++ b/tests/unitTests/CMakeLists.txt
@@ -45,10 +45,13 @@ function(add_unit_test NAME FILENAME)
set_tests_properties(${NAME} PROPERTIES DEPENDS ${NAME}_build)
endfunction()
+add_unit_test(Distance_Tests DistanceTests)
+add_unit_test(Functor_Tests FunctorTests)
add_unit_test(Lane_Tests LaneTests)
add_unit_test(Node_Tests NodeTests)
-add_unit_test(Route_Tests RouteTests)
add_unit_test(Pathfinding_Tests PathfindingTests)
+add_unit_test(ReferenceLine_Tests ReferenceLineTests)
+add_unit_test(Route_Tests RouteTests)
add_unit_test(SensorView_Tests SensorViewTests)
# ---------- All Tests ----------
@@ -62,8 +65,11 @@ add_executable(${COMPONENT_TEST_NAME} EXCLUDE_FROM_ALL
# Tests
${COMPONENT_TEST_DIR}/gtest/unitTestMain.cpp
+ ${COMPONENT_TEST_DIR}/unitTests/FunctorTests.cpp
${COMPONENT_TEST_DIR}/unitTests/LaneTests.cpp
${COMPONENT_TEST_DIR}/unitTests/NodeTests.cpp
+ ${COMPONENT_TEST_DIR}/unitTests/PathfindingTests.cpp
+ ${COMPONENT_TEST_DIR}/unitTests/ReferenceLineTests.cpp
${COMPONENT_TEST_DIR}/unitTests/RouteTests.cpp
${COMPONENT_TEST_DIR}/unitTests/SensorViewTests.cpp
diff --git a/tests/unitTests/DistanceTests.cpp b/tests/unitTests/DistanceTests.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..637036c95942f0539ee2bcbcebbadb20cdd864dc
--- /dev/null
+++ b/tests/unitTests/DistanceTests.cpp
@@ -0,0 +1,46 @@
+/********************************************************************************
+ * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ *
+ * 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
+ ********************************************************************************/
+#include "util/Common.tpp"
+
+using namespace osiql;
+
+TEST(GetDistanceBetween, Given2Intervals_WhenComputingDistance_ThenResultsAreAsExpected)
+{
+ EXPECT_EQ(GetDistanceBetween(Interval{-5, -3}, Interval{2, 6}), 5); // a behind b
+ EXPECT_EQ(GetDistanceBetween(Interval{-5, -3}, Interval{-4, 1}), 0); // a partially behind b
+ EXPECT_EQ(GetDistanceBetween(Interval{2, 2}, Interval{2, 2}), 0); // a identical to b
+ EXPECT_EQ(GetDistanceBetween(Interval{-4, 1}, Interval{-5, -3}), 0); // b partially behind a
+ EXPECT_EQ(GetDistanceBetween(Interval{2, 6}, Interval{-5, -3}), -5); // b behind a
+}
+
+TEST(GetDistanceBetween, Given1IntervalAnd1Value_WhenComputingDistance_ThenResultsAreAsExpected)
+{
+ EXPECT_EQ(GetDistanceBetween(Interval{-5, -3}, 2), 5); // Interval behind value
+ EXPECT_EQ(GetDistanceBetween(Interval{-5, -3}, -5), 0); // Value at start of interval
+ EXPECT_EQ(GetDistanceBetween(Interval{-5, -3}, -4), 0); // Value inside interval
+ EXPECT_EQ(GetDistanceBetween(Interval{-5, -3}, -3), 0); // Value at end of interval
+ EXPECT_EQ(GetDistanceBetween(Interval{-5, -3}, -7), -2); // Value behind interval
+}
+
+TEST(GetDistanceBetween, Given1ValueAnd1Interval_WhenComputingDistance_ThenResultsAreAsExpected)
+{
+ EXPECT_EQ(GetDistanceBetween(2, Interval{-5, -3}), -5); // Interval behind value
+ EXPECT_EQ(GetDistanceBetween(-5, Interval{-5, -3}), 0); // Value at start of interval
+ EXPECT_EQ(GetDistanceBetween(-4, Interval{-5, -3}), 0); // Value inside interval
+ EXPECT_EQ(GetDistanceBetween(-3, Interval{-5, -3}), 0); // Value at end of interval
+ EXPECT_EQ(GetDistanceBetween(-7, Interval{-5, -3}), 2); // Value behind interval
+}
+
+TEST(GetDistanceBetween, Given2Values_WhenComputingDistance_ThenResultsAreAsExpected)
+{
+ EXPECT_EQ(GetDistanceBetween(-3, 2), 5); // a behind b
+ EXPECT_EQ(GetDistanceBetween(1, 1), 0); // identical a and b
+ EXPECT_EQ(GetDistanceBetween(2, -3), -5); // b behind a
+}
diff --git a/tests/unitTests/FunctorTests.cpp b/tests/unitTests/FunctorTests.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6466e45c810a206d741736fb189f2b13913433dc
--- /dev/null
+++ b/tests/unitTests/FunctorTests.cpp
@@ -0,0 +1,46 @@
+/********************************************************************************
+ * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ *
+ * 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
+ ********************************************************************************/
+#include "util/Common.tpp"
+
+using namespace osiql;
+
+TEST(Equal_To, GivenMovingObject_WhenComparingId_ThenSucceeds)
+{
+ osi3::MovingObject handle;
+ osiql::SetId(handle, 1);
+ osiql::SetDimensions(handle, 4, 2);
+ osiql::MovingObject object{handle};
+ EXPECT_TRUE(Equal<Id>{}(object, &object));
+ EXPECT_TRUE(Equal<Id>::to(object)(std::ref(object)));
+ EXPECT_FALSE(Equal<Id>{}(object, 2));
+ EXPECT_FALSE(Equal<Id>::to(object)(2));
+}
+
+TEST(Equal_To, GivenMovingObjectPointer_WhenComparingId_ThenSucceeds)
+{
+ osi3::MovingObject handle;
+ osiql::SetId(handle, 1);
+ osiql::SetDimensions(handle, 4, 2);
+ osiql::MovingObject object{handle};
+ EXPECT_TRUE(Equal<Id>{}(&object, object));
+ EXPECT_TRUE(Equal<Id>::to(&object)(std::optional(object)));
+ EXPECT_FALSE(Equal<Id>{}(&object, 2));
+ EXPECT_FALSE(Equal<Id>::to(&object)(2));
+}
+
+TEST(Equal_To, GivenMovingObject_WhenCurrying_ThenStoresReference)
+{
+ osi3::MovingObject handle;
+ osiql::SetId(handle, 1);
+ osiql::SetDimensions(handle, 4, 2);
+ osiql::MovingObject object{handle};
+ auto functor{Equal<Id>::to(object)};
+ EXPECT_EQ(&functor.b, &object);
+}
diff --git a/tests/unitTests/LaneTests.cpp b/tests/unitTests/LaneTests.cpp
index bca78911d3f550d7fc27c318b5231bca522cc7a3..55a080c8edc5b7456615c1700f7eecc3cded6f49 100644
--- a/tests/unitTests/LaneTests.cpp
+++ b/tests/unitTests/LaneTests.cpp
@@ -14,7 +14,7 @@ using namespace osiql;
using ::testing::Contains;
using ::testing::Key;
-TEST(Lane, osiqlLaneTypes_Match_osiLogicalLaneTypes)
+TEST(Lane, GetType_GivenOSIValue_WhenReturnedAsOSIQLValue_ThenValueMatchesExpectation)
{
osi3::LogicalLane handle;
const Lane lane{handle};
diff --git a/tests/unitTests/NodeTests.cpp b/tests/unitTests/NodeTests.cpp
index f18b1f998282ae5b5e1cccf02015c68b0912d855..06b051db7ff35a7614bbb9b451692d567335f97e 100644
--- a/tests/unitTests/NodeTests.cpp
+++ b/tests/unitTests/NodeTests.cpp
@@ -1,5 +1,5 @@
/********************************************************************************
- * Copyright (c) 2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2024-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License 2.0 which is available at
@@ -553,7 +553,7 @@ TEST_F(RoadNetwork, FindAll_GivenObjectsOnEveryRoad_WhenSearchingForwardAndBackw
{
for (const auto &reference_line : groundTruth.handle.reference_line())
{ // Add an object at the center of every road
- XY xy{0.5 * XY{reference_line.poly_line().begin()->world_position()} + 0.5 * XY{reference_line.poly_line().rbegin()->world_position()}};
+ XY xy{Average(XY{reference_line.poly_line().begin()->world_position()}, XY{reference_line.poly_line().rbegin()->world_position()})};
groundTruth.Add<MovingObject>(xy);
}
World world{groundTruth.handle};
diff --git a/tests/unitTests/ReferenceLineTests.cpp b/tests/unitTests/ReferenceLineTests.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..92ef1d062de38c5810284cd80af179fe107f2695
--- /dev/null
+++ b/tests/unitTests/ReferenceLineTests.cpp
@@ -0,0 +1,137 @@
+/********************************************************************************
+ * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ *
+ * 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
+ ********************************************************************************/
+#include <limits>
+
+#include "util/Common.tpp"
+
+using namespace osiql;
+
+TEST(ReferenceLine_GetCurvature, GivenSCoordinate_WhenBeforeOrAfterLine_ThenCurvatureIsNaN)
+{
+ test::GroundTruth groundTruth;
+ ReferenceLine referenceLine{groundTruth.AddReferenceLine(XY{0, 0}, XY{10, 0})};
+ EXPECT_TRUE(std::isnan(referenceLine.GetCurvature(-1)));
+ EXPECT_TRUE(std::isnan(referenceLine.GetCurvature(11)));
+}
+
+TEST(ReferenceLine_GetCurvature, GivenStraightLine_WhenSCoordinateWithinLine_ThenCurvatureIs0)
+{
+ test::GroundTruth groundTruth;
+ {
+ ReferenceLine referenceLine{groundTruth.AddReferenceLine(XY{0, 0}, XY{10, 0})};
+ EXPECT_EQ(referenceLine.GetCurvature(0), .0);
+ EXPECT_EQ(referenceLine.GetCurvature(5), .0);
+ EXPECT_EQ(referenceLine.GetCurvature(10), .0);
+ }
+ {
+ ReferenceLine referenceLine{groundTruth.AddReferenceLine(XY{0, 0}, XY{5, 0}, XY{10, 0})};
+ EXPECT_EQ(referenceLine.GetCurvature(0), .0);
+ EXPECT_EQ(referenceLine.GetCurvature(3), .0);
+ EXPECT_EQ(referenceLine.GetCurvature(6), .0);
+ EXPECT_EQ(referenceLine.GetCurvature(9), .0);
+ }
+ {
+ ReferenceLine referenceLine{groundTruth.AddReferenceLine(XY{0, 0}, XY{3, 0}, XY{6, 0}, XY{9, 0})};
+ EXPECT_EQ(referenceLine.GetCurvature(0), .0);
+ EXPECT_EQ(referenceLine.GetCurvature(2), .0);
+ EXPECT_EQ(referenceLine.GetCurvature(4), .0);
+ EXPECT_EQ(referenceLine.GetCurvature(6), .0);
+ EXPECT_EQ(referenceLine.GetCurvature(8), .0);
+ }
+}
+
+TEST(ReferenceLine_GetCurvature, Given3PointsOnUnitCircle_WhenSCoordinateWithinLine_ThenCurvatureIs1)
+{
+ { // Counter-clockwise -> positve curvature
+ test::GroundTruth groundTruth;
+ ReferenceLine referenceLine{groundTruth.AddReferenceLine(XY{1, 0}, XY{1, 0}.Rotate(quarterPi), XY{0, 1})};
+
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(0), 1);
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * .25), 1);
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * .5), 1);
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * .75), 1);
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(referenceLine.GetLength()), 1);
+ }
+ { // Clockwise -> negative curvature
+ test::GroundTruth groundTruth;
+ ReferenceLine referenceLine{groundTruth.AddReferenceLine(XY{1, 0}, XY{1, 0}.Rotate(-quarterPi), XY{0, -1})};
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(0), -1);
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * .25), -1);
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * .5), -1);
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * .75), -1);
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(referenceLine.GetLength()), -1);
+ }
+}
+
+TEST(ReferenceLine_GetCurvature, Given4PointsOnUnitCircle_WhenSCoordinateWithinLine_ThenCurvatureIs1)
+{
+ { // Counter-clockwise -> positve curvature
+ test::GroundTruth groundTruth;
+ ReferenceLine referenceLine{groundTruth.AddReferenceLine(XY{1, 0}, XY{1, 0}.Rotate(quarterPi), XY{0, 1}, XY{1, 0}.Rotate(threeQuarterPi))};
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(0), 1);
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * (1.0 / 6.0)), 1);
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * (1.0 / 3.0)), 1);
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * .5), 1);
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * (2.0 / 3.0)), 1);
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * (5.0 / 6.0)), 1);
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(referenceLine.GetLength()), 1);
+ }
+ { // Clockwise -> negative curvature
+ test::GroundTruth groundTruth;
+ ReferenceLine referenceLine{groundTruth.AddReferenceLine(XY{1, 0}, XY{1, 0}.Rotate(-quarterPi), XY{0, -1}, XY{1, 0}.Rotate(-threeQuarterPi))};
+ EXPECT_EQ(referenceLine.GetCurvature(0), -1);
+ EXPECT_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * (1.0 / 6.0)), -1);
+ EXPECT_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * (1.0 / 3.0)), -1);
+ EXPECT_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * .5), -1);
+ EXPECT_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * (2.0 / 3.0)), -1);
+ EXPECT_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * (5.0 / 6.0)), -1);
+ EXPECT_EQ(referenceLine.GetCurvature(referenceLine.GetLength()), -1);
+ }
+}
+
+TEST(ReferenceLine_GetCurvature, GivenUniSquare_WhenSCoordinateWithinLine_ThenCurvatureReachesPiHalf)
+{
+ test::GroundTruth groundTruth;
+ // The center of a unit square has a distance of sqrt(2) between its center and each corner
+ ReferenceLine referenceLine{groundTruth.AddReferenceLine(XY{0, 0}, XY{1, 0}, XY{1, 1}, XY{0, 1}, XY{0, 0})};
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(0), std::sqrt(2));
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(.5), std::sqrt(2));
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(1), std::sqrt(2));
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(1.5), std::sqrt(2));
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(2), std::sqrt(2));
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(2.5), std::sqrt(2));
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(3), std::sqrt(2));
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(3.5), std::sqrt(2));
+ EXPECT_DOUBLE_EQ(referenceLine.GetCurvature(4), std::sqrt(2));
+}
+
+TEST(ReferenceLine_GetCurvature, Given4PointSpiral_WhenValidSCoordinate_ThenReturnsInterpolatedCurvature)
+{
+ test::GroundTruth groundTruth;
+ // First three points form a circle with center (1, 0) and radius sqrt(5)
+ // Last three points form a circle with center (1, 1) and radius sqrt(2)
+ ReferenceLine referenceLine{groundTruth.AddReferenceLine(
+ XY{0, -2},
+ XY{0, 2},
+ XY{2, 2},
+ XY{2, 0}
+ )};
+ const double START_CURVATURE{-1.0 / std::sqrt(5)};
+ const double END_CURVATURE{-1.0 / std::sqrt(2)};
+ EXPECT_EQ(referenceLine.GetCurvature(0), START_CURVATURE);
+ EXPECT_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * .125), Interpolate(START_CURVATURE, END_CURVATURE, .125));
+ EXPECT_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * .25), Interpolate(START_CURVATURE, END_CURVATURE, .25));
+ EXPECT_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * .375), Interpolate(START_CURVATURE, END_CURVATURE, .375));
+ EXPECT_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * .5), Interpolate(START_CURVATURE, END_CURVATURE, .5));
+ EXPECT_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * .625), Interpolate(START_CURVATURE, END_CURVATURE, .625));
+ EXPECT_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * .75), Interpolate(START_CURVATURE, END_CURVATURE, .75));
+ EXPECT_EQ(referenceLine.GetCurvature(referenceLine.GetLength() * .875), Interpolate(START_CURVATURE, END_CURVATURE, .875));
+ EXPECT_EQ(referenceLine.GetCurvature(referenceLine.GetLength()), END_CURVATURE);
+}
diff --git a/tests/unitTests/WorldTests/Tests.cpp b/tests/unitTests/WorldTests/Tests.cpp
index ec477f160287bf276b7c44353154287859ce317c..12b60c16563bae8e1b4de734ddf9879f87b599b5 100644
--- a/tests/unitTests/WorldTests/Tests.cpp
+++ b/tests/unitTests/WorldTests/Tests.cpp
@@ -1,5 +1,5 @@
/********************************************************************************
- * Copyright (c) 2022-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2022-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License 2.0 which is available at
@@ -11,10 +11,10 @@
using namespace osiql;
-class QueryTest : public ::testing::Test
+class WorldTest : public ::testing::Test
{
public:
- QueryTest()
+ WorldTest()
{
roads.push_back(groundTruth.AddRoad( // ROAD 0
groundTruth.AddReferenceLine(Pose<XY>{{0, 0}, halfPi}, Pose<XY>{{2, 0}, halfPi}),
@@ -64,7 +64,7 @@ public:
std::vector<std::vector<osi3::LogicalLane *>> roads;
};
-TEST_F(QueryTest, LaneIndices)
+TEST_F(WorldTest, LaneIndices)
{
// On a road with 2 downstream and 2 upstream lanes:
@@ -88,7 +88,7 @@ TEST_F(QueryTest, LaneIndices)
EXPECT_EQ(leftmostLane->GetIndex(Side::Left), 1);
}
-TEST_F(QueryTest, AdjacentLanes)
+TEST_F(WorldTest, AdjacentLanes)
{
const Lane *rightmostLane{world->GetLane(get<Id>(roads[0][3]))};
ASSERT_NE(rightmostLane, nullptr);
@@ -132,7 +132,7 @@ TEST_F(QueryTest, AdjacentLanes)
EXPECT_EQ(leftLane->GetAdjacentLane<Side::Left>(2), rightmostLane);
}
-TEST_F(QueryTest, AdjacentBoundaries)
+TEST_F(WorldTest, AdjacentBoundaries)
{
const Lane *rightmostLane{world->GetLane(get<Id>(roads[0][3]))};
ASSERT_NE(rightmostLane, nullptr);
@@ -151,7 +151,7 @@ TEST_F(QueryTest, AdjacentBoundaries)
// Lane Width Tests
// NOTE: Fails prior to OSI 3.6 due to lack of t_axis_yaw support
-TEST_F(QueryTest, GetLaneWidth_Forward)
+TEST_F(WorldTest, GetLaneWidth_Forward)
{
const Lane *lane{world->GetLane(get<Id>(roads[0][2]))};
ASSERT_NE(lane, nullptr);
@@ -178,7 +178,7 @@ TEST_F(QueryTest, GetLaneWidth_Forward)
}
// NOTE: Fails prior to OSI 3.6 due to lack of t_axis_yaw support
-TEST_F(QueryTest, GetLaneWidth_Backward)
+TEST_F(WorldTest, GetLaneWidth_Backward)
{
const Lane *lane{world->GetLane(get<Id>(roads[2][1]))};
ASSERT_NE(lane, nullptr);
@@ -193,7 +193,7 @@ TEST_F(QueryTest, GetLaneWidth_Backward)
// Distance from Object to Polyline Tests
-TEST_F(QueryTest, GetDistanceTo_LaneBoundaries_Returns_CorrectValue)
+TEST_F(WorldTest, GetDistanceTo_LaneBoundaries_Returns_CorrectValue)
{
const auto &object{world->GetHostVehicle()};
const Lane *lane{world->GetLane(get<Id>(roads[0][2]))};
@@ -203,7 +203,7 @@ TEST_F(QueryTest, GetDistanceTo_LaneBoundaries_Returns_CorrectValue)
EXPECT_DOUBLE_EQ(right, 0.0);
}
-TEST_F(QueryTest, GetDistanceTo_AdjacentLanePolyline_Returns_CorrectValue)
+TEST_F(WorldTest, GetDistanceTo_AdjacentLanePolyline_Returns_CorrectValue)
{
const auto &object{world->GetHostVehicle()};
const auto &rightmostBoundary{*world->GetLane(get<Id>(roads[0][3]))->GetBoundaries<Side::Right>()[0]};
@@ -218,11 +218,11 @@ TEST_F(QueryTest, GetDistanceTo_AdjacentLanePolyline_Returns_CorrectValue)
EXPECT_DOUBLE_EQ(object.GetSignedDistancesTo(rightmostBoundary.begin(), rightmostBoundary.end()).max, -0.75);
}
-TEST_F(QueryTest, GetDistanceTo_IntersectingPolyline_Returns_Positive_And_Negative_Distance)
+TEST_F(WorldTest, GetDistanceTo_IntersectingPolyline_Returns_Positive_And_Negative_Distance)
{
// Make the object wider so that it overlaps the boundary
groundTruth.handle.mutable_moving_object(0)->mutable_base()->mutable_dimension()->set_width(2.5);
- world->Update(groundTruth.handle.moving_object());
+ world->UpdateAll<MovingObject>(groundTruth.handle.moving_object());
const auto &object{world->GetHostVehicle()};
const Lane *lane{world->GetLane(get<Id>(roads[0][2]))};
ASSERT_NE(lane, nullptr);
@@ -233,14 +233,14 @@ TEST_F(QueryTest, GetDistanceTo_IntersectingPolyline_Returns_Positive_And_Negati
// Lane Curvature Tests
-TEST_F(QueryTest, GetCurvature_StraightReferenceLine_Returns_Zero)
+TEST_F(WorldTest, GetCurvature_StraightReferenceLine_Returns_Zero)
{
const ReferenceLine *referenceLine{world->GetReferenceLine(roads[0][0]->reference_line_id().value())};
ASSERT_NE(referenceLine, nullptr);
EXPECT_DOUBLE_EQ(referenceLine->GetCurvature(0.0), 0.0);
}
-TEST_F(QueryTest, GetCurvature_Forward_Equals_BackwardInverted)
+TEST_F(WorldTest, GetCurvature_Forward_Equals_BackwardInverted)
{
const ReferenceLine *referenceLine{world->GetReferenceLine(roads[3][0]->reference_line_id().value())};
ASSERT_NE(referenceLine, nullptr);
@@ -253,7 +253,7 @@ TEST_F(QueryTest, GetCurvature_Forward_Equals_BackwardInverted)
EXPECT_DOUBLE_EQ(referenceLine->GetCurvature(6.0, RoadDirection::Downstream), -referenceLine->GetCurvature(6.0, RoadDirection::Upstream));
}
-TEST_F(QueryTest, GetCurvature_OfLane_IsDifferentFromReferenceLine_OnCurvedRoad)
+TEST_F(WorldTest, GetCurvature_OfLane_IsDifferentFromReferenceLine_OnCurvedRoad)
{
const ReferenceLine *referenceLine{world->GetReferenceLine(roads[3][0]->reference_line_id().value())};
ASSERT_NE(referenceLine, nullptr);
@@ -281,7 +281,7 @@ TEST_F(QueryTest, GetCurvature_OfLane_IsDifferentFromReferenceLine_OnCurvedRoad)
EXPECT_GT(lane1->GetCurvature<RoadDirection::Downstream>(3.0), referenceLine->GetCurvature<RoadDirection::Downstream>(3.0));
}
-TEST_F(QueryTest, GetObstruction_OfVehicleBy_Itself)
+TEST_F(WorldTest, GetObstruction_OfVehicleBy_Itself)
{
const Vehicle &ego{world->GetHostVehicle()};
ASSERT_THAT(ego.GetOverlaps<Road>(), ::testing::Not(IsEmpty()));
@@ -300,7 +300,7 @@ TEST_F(QueryTest, GetObstruction_OfVehicleBy_Itself)
}
// NOTE: Fails prior to OSI 3.6 due to lack of t_axis_yaw support
-TEST_F(QueryTest, GetObstruction_OfVehicleBy_IndividualPoints)
+TEST_F(WorldTest, GetObstruction_OfVehicleBy_IndividualPoints)
{
const Vehicle &ego{world->GetHostVehicle()};
ASSERT_THAT(ego.GetOverlaps<Road>(), ::testing::Not(IsEmpty()));
@@ -326,11 +326,11 @@ TEST_F(QueryTest, GetObstruction_OfVehicleBy_IndividualPoints)
}
// NOTE: Fails prior to OSI 3.6 due to lack of t_axis_yaw support
-TEST_F(QueryTest, GetObstruction_OfVehicleBy_ObjectOneRoadApart)
+TEST_F(WorldTest, GetObstruction_OfVehicleBy_ObjectOneRoadApart)
{
// Object on road 2
groundTruth.Add<MovingObject>(XY{7.5, -2.5}, 1.0, 1.0);
- world->Update(groundTruth.handle.moving_object());
+ world->UpdateAll<MovingObject>(groundTruth.handle.moving_object());
const auto &startObject{world->GetHostVehicle()};
const auto *endObject{world->GetMovingObject(startObject.GetId() + 1)};
@@ -348,11 +348,11 @@ TEST_F(QueryTest, GetObstruction_OfVehicleBy_ObjectOneRoadApart)
}
// NOTE: Fails prior to OSI 3.6 due to lack of t_axis_yaw support
-TEST_F(QueryTest, GetObstruction_OfVehicleBy_ObjectTwoRoadsApart)
+TEST_F(WorldTest, GetObstruction_OfVehicleBy_ObjectTwoRoadsApart)
{
// Object on road 2
groundTruth.Add<MovingObject>(XY{5.5, 1.5}, 1.0, 1.0);
- world->Update(groundTruth.handle.moving_object());
+ world->UpdateAll<MovingObject>(groundTruth.handle.moving_object());
const auto &startObject{world->GetHostVehicle()};
const auto *endObject{world->GetMovingObject(startObject.GetId() + 1)};
@@ -370,11 +370,11 @@ TEST_F(QueryTest, GetObstruction_OfVehicleBy_ObjectTwoRoadsApart)
EXPECT_DOUBLE_EQ(max1, -min2);
}
-TEST_F(QueryTest, GetObstruction_OfVehicleBy_ObjectNotOnRoute)
+TEST_F(WorldTest, GetObstruction_OfVehicleBy_ObjectNotOnRoute)
{
// Object on road 2
groundTruth.Add<MovingObject>(XY{5.5, 1.5}, 1.0, 1.0);
- world->Update(groundTruth.handle.moving_object());
+ world->UpdateAll<MovingObject>(groundTruth.handle.moving_object());
const auto &startObject{world->GetHostVehicle()};
const auto *endObject{world->GetMovingObject(startObject.GetId() + 1)};
@@ -392,10 +392,10 @@ TEST_F(QueryTest, GetObstruction_OfVehicleBy_ObjectNotOnRoute)
EXPECT_DOUBLE_EQ(max2, std::numeric_limits<double>::lowest());
}
-TEST_F(QueryTest, GetObstruction_OfVehicleBy_ObjectNotOnAnyRoad)
+TEST_F(WorldTest, GetObstruction_OfVehicleBy_ObjectNotOnAnyRoad)
{
auto id{groundTruth.Add<MovingObject>(XY{100.0, 100.0}, 1.0, 1.0).id().value()};
- world->Update(groundTruth.handle.moving_object());
+ world->UpdateAll<MovingObject>(groundTruth.handle.moving_object());
const auto *objectOnRoad{world->GetMovingObject(id - 1)};
ASSERT_NE(objectOnRoad, nullptr);
@@ -413,26 +413,28 @@ TEST_F(QueryTest, GetObstruction_OfVehicleBy_ObjectNotOnAnyRoad)
EXPECT_DOUBLE_EQ(max2, std::numeric_limits<double>::lowest());
}
-TEST_F(QueryTest, Update_Objects)
+TEST_F(WorldTest, UpdateAll_MovingObjects)
{
- // One object
+ // Ensure deprecated version is still callable:
world->Update(groundTruth.handle.moving_object());
+ // One object
+ world->UpdateAll<MovingObject>(groundTruth.handle.moving_object());
EXPECT_THAT(world->GetAll<MovingObject>(), SizeIs(1));
// No objects anymore
groundTruth.handle.mutable_moving_object()->Clear();
- world->Update(groundTruth.handle.moving_object());
+ world->UpdateAll<MovingObject>(groundTruth.handle.moving_object());
EXPECT_THAT(world->GetAll<MovingObject>(), IsEmpty());
// Two objects
groundTruth.Add<MovingObject>(XY{0, 0}, 2, 1);
groundTruth.Add<MovingObject>(XY{1, 0}, 2, 1);
- world->Update(groundTruth.handle.moving_object());
+ world->UpdateAll<MovingObject>(groundTruth.handle.moving_object());
EXPECT_THAT(world->GetAll<MovingObject>(), SizeIs(2));
// Replace one of the objects by first deleting then adding
{
auto *objects{groundTruth.handle.mutable_moving_object()};
objects->erase(std::prev(objects->end()));
groundTruth.Add<MovingObject>(XY{2, 0}, 2, 1);
- world->Update(groundTruth.handle.moving_object());
+ world->UpdateAll<MovingObject>(groundTruth.handle.moving_object());
EXPECT_THAT(world->GetAll<MovingObject>(), SizeIs(2));
}
// Replace one of the objects by first adding then deleting
@@ -440,18 +442,58 @@ TEST_F(QueryTest, Update_Objects)
groundTruth.Add<MovingObject>(XY{3, 0}, 2, 1);
auto *objects{groundTruth.handle.mutable_moving_object()};
objects->erase(objects->begin());
- world->Update(groundTruth.handle.moving_object());
+ world->UpdateAll<MovingObject>(groundTruth.handle.moving_object());
EXPECT_THAT(world->GetAll<MovingObject>(), SizeIs(2));
}
// Swap the order of the underlying moving objects
{
auto &range{*groundTruth.handle.mutable_moving_object()};
std::reverse(range.begin(), range.end());
- world->Update(range);
+ world->UpdateAll<MovingObject>(range);
EXPECT_THAT(world->GetAll<MovingObject>(), SizeIs(2));
}
}
+TEST_F(WorldTest, UpdateAll_StationaryObjects)
+{
+ // One object
+ groundTruth.Add<StationaryObject>(XY{0, 0}, 2, 1);
+ world->UpdateAll<StationaryObject>(groundTruth.handle.stationary_object());
+ EXPECT_THAT(world->GetAll<StationaryObject>(), SizeIs(1));
+ // No objects anymore
+ groundTruth.handle.mutable_stationary_object()->Clear();
+ world->UpdateAll<StationaryObject>(groundTruth.handle.stationary_object());
+ EXPECT_THAT(world->GetAll<StationaryObject>(), IsEmpty());
+ // Two objects
+ groundTruth.Add<StationaryObject>(XY{0, 0}, 2, 1);
+ groundTruth.Add<StationaryObject>(XY{1, 0}, 2, 1);
+ world->UpdateAll<StationaryObject>(groundTruth.handle.stationary_object());
+ EXPECT_THAT(world->GetAll<StationaryObject>(), SizeIs(2));
+ // Replace one of the objects by first deleting then adding
+ {
+ auto *objects{groundTruth.handle.mutable_stationary_object()};
+ objects->erase(std::prev(objects->end()));
+ groundTruth.Add<StationaryObject>(XY{2, 0}, 2, 1);
+ world->UpdateAll<StationaryObject>(groundTruth.handle.stationary_object());
+ EXPECT_THAT(world->GetAll<StationaryObject>(), SizeIs(2));
+ }
+ // Replace one of the objects by first adding then deleting
+ {
+ groundTruth.Add<StationaryObject>(XY{3, 0}, 2, 1);
+ auto *objects{groundTruth.handle.mutable_stationary_object()};
+ objects->erase(objects->begin());
+ world->UpdateAll<StationaryObject>(groundTruth.handle.stationary_object());
+ EXPECT_THAT(world->GetAll<StationaryObject>(), SizeIs(2));
+ }
+ // Swap the order of the underlying moving objects
+ {
+ auto &range{*groundTruth.handle.mutable_stationary_object()};
+ std::reverse(range.begin(), range.end());
+ world->UpdateAll<StationaryObject>(range);
+ EXPECT_THAT(world->GetAll<StationaryObject>(), SizeIs(2));
+ }
+}
+
struct OverlapTests : ::testing::Test
{
// Two moving objects: