Merge branch 'mb_proposal_si_units' into 'master'

Replaced the types of all physical quantities related members/arguments with SI unit types as provided by the Units library.

See merge request eclipse/simopenpass/scenario_api!11

MantleAPI/include/MantleAPI/Common/bounding_box.h

@@ -17,22 +17,26 @@
 
 #include <MantleAPI/Common/dimension.h>
 #include <MantleAPI/Common/vector.h>
+#include <units.h>
 
 namespace mantle_api
 {
+
 /// Bounding box is defined in local entity coordinate system.
-/// The origin of the entity coordinate system is defined in relation to the geometric center.
+/// The origin of the entity coordinate system is defined in relation to the
+/// geometric center.
 struct BoundingBox
 {
-    Vec3d geometric_center{0.0, 0.0, 0.0};
-    Dimension3d dimension{0.0, 0.0, 0.0};
+    Vec3<units::length::meter_t> geometric_center{};
+    Dimension3 dimension{};
 };
 
 inline bool operator==(const BoundingBox& lhs, const BoundingBox& rhs) noexcept
 {
-    return lhs.geometric_center == rhs.geometric_center && lhs.dimension == rhs.dimension;
+    return lhs.geometric_center == rhs.geometric_center &&
+        lhs.dimension == rhs.dimension;
 }
 
-}  // namespace mantle_api
+}    // namespace mantle_api
 
-#endif  // MANTLEAPI_COMMON_BOUNDINGBOX_H
+#endif    // MANTLEAPI_COMMON_BOUNDINGBOX_H

MantleAPI/include/MantleAPI/Common/dimension.h

@@ -16,27 +16,24 @@
 #define MANTLEAPI_COMMON_DIMENSION_H
 
 #include <MantleAPI/Common/floating_point_helper.h>
-#include <MantleAPI/Common/vector.h>
+#include <units.h>
 
 namespace mantle_api
 {
 
-template <typename T>
 struct Dimension3
 {
-    T length{};
-    T width{};
-    T height{};
+    units::length::meter_t length{0};
+    units::length::meter_t width{0};
+    units::length::meter_t height{0};
 };
 
-using Dimension3d = Dimension3<double>;
-
-inline bool operator==(const Dimension3d& lhs, const Dimension3d& rhs) noexcept
+inline bool operator==(const Dimension3& lhs, const Dimension3& rhs) noexcept
 {
     return IsEqual(lhs.length, rhs.length) && IsEqual(lhs.width, rhs.width) && IsEqual(lhs.height, rhs.height);
 }
 
-inline bool operator!=(const Dimension3d& lhs, const Dimension3d& rhs) noexcept
+inline bool operator!=(const Dimension3& lhs, const Dimension3& rhs) noexcept
 {
     return !(lhs == rhs);
 }

MantleAPI/include/MantleAPI/Common/floating_point_helper.h

@@ -15,6 +15,8 @@
 #ifndef MANTLEAPI_COMMON_FLOATING_POINT_HELPER_H
 #define MANTLEAPI_COMMON_FLOATING_POINT_HELPER_H
 
+#include <units.h>
+
 #include <cmath>
 #include <limits>
 #include <type_traits>
@@ -36,12 +38,31 @@ namespace mantle_api
 /// @param rhs Right-hand operand
 /// @param precision Given precision, defaults to `std::numeric_limits<T>::epsilon()`.
 /// @return True if both numbers are equal within the given precision.
-template <typename T, typename std::enable_if_t<std::is_floating_point_v<T>>* = nullptr>
-bool IsEqual(T lhs, T rhs, T precision = std::numeric_limits<T>::epsilon())
+inline bool IsEqual(double lhs, double rhs, double precision = std::numeric_limits<double>::epsilon())
 {
     return std::abs(lhs - rhs) < precision;
 }
 
+/// @brief Compares two floating point numbers for equality.
+///
+/// **Attention** No approximation of the correct precision for the actual value is done!
+///
+/// This function uses a default precision of `std::numeric_limts<T>::epsilon()`, which means that without providing the
+/// precision yourself, it is only suited for
+/// * bit-wise equal numbers
+/// * numbers around 1 or smaller
+///
+/// @tparam T Floating point type, ensured by SFINAE
+/// @param lhs Left-hand operand
+/// @param rhs Right-hand operand
+/// @param precision Given precision, defaults to `std::numeric_limits<T>::epsilon()`.
+/// @return True if both numbers are equal within the given precision.
+template <typename T, class = typename std::enable_if_t<units::traits::is_unit_t<T>::value>>
+bool IsEqual(T lhs, T rhs, double precision = std::numeric_limits<double>::epsilon())
+{
+    return units::math::abs(lhs - rhs) < precision;
+}
+
 /// @brief Compares two floating point numbers for equality.
 ///
 /// **Attention** No approximation of the correct precision for the actual value is done!
@@ -56,8 +77,8 @@ bool IsEqual(T lhs, T rhs, T precision = std::numeric_limits<T>::epsilon())
 /// @param rhs Right-hand operand
 /// @param precision Given precision, defaults to `std::numeric_limits<T>::epsilon()`.
 /// @return True if lhs is greater than rhs or both numbers are equal within the given precision.
-template <typename T, typename std::enable_if_t<std::is_floating_point_v<T>>* = nullptr>
-bool GreaterOrEqual(T lhs, T rhs, T precision = std::numeric_limits<T>::epsilon())
+template <typename T, class = typename std::enable_if_t<units::traits::is_unit_t<T>::value>>
+bool GreaterOrEqual(T lhs, T rhs, double precision = std::numeric_limits<double>::epsilon())
 {
     if (lhs > rhs)
     {
@@ -80,8 +101,8 @@ bool GreaterOrEqual(T lhs, T rhs, T precision = std::numeric_limits<T>::epsilon(
 /// @param rhs Right-hand operand
 /// @param precision Given precision, defaults to `std::numeric_limits<T>::epsilon()`.
 /// @return True if lhs is less than rhs or both numbers are equal within the given precision.
-template <typename T, typename std::enable_if_t<std::is_floating_point_v<T>>* = nullptr>
-bool LessOrEqual(T lhs, T rhs, T precision = std::numeric_limits<T>::epsilon())
+template <typename T, class = typename std::enable_if_t<units::traits::is_unit_t<T>::value>>
+bool LessOrEqual(T lhs, T rhs, double precision = std::numeric_limits<double>::epsilon())
 {
     if (lhs < rhs)
     {
@@ -89,6 +110,7 @@ bool LessOrEqual(T lhs, T rhs, T precision = std::numeric_limits<T>::epsilon())
     }
     return IsEqual(lhs, rhs, precision);
 }
+
 }  // namespace mantle_api
 
 #endif  // MANTLEAPI_COMMON_FLOATING_POINT_HELPER_H

MantleAPI/include/MantleAPI/Common/orientation.h

@@ -16,41 +16,62 @@
 #define MANTLEAPI_COMMON_ORIENTATION_H
 
 #include <MantleAPI/Common/floating_point_helper.h>
+#include <units.h>
+
+namespace units
+{
+
+UNIT_ADD(angular_acceleration,
+         radians_per_second_squared,
+         radians_per_second_squared,
+         rad_per_s_sq,
+         compound_unit<angle::radians, inverse<squared<time::seconds>>>)
+
+namespace category
+{
+typedef base_unit<detail::meter_ratio<0>, std::ratio<0>, std::ratio<-2>, std::ratio<1>> angular_acceleration_unit;
+}
+
+UNIT_ADD_CATEGORY_TRAIT(angular_acceleration)
+
+}  // namespace units
 
 namespace mantle_api
 {
 
-template <typename T>
+template <typename T,
+          class = typename std::enable_if_t<units::traits::is_angle_unit<T>::value ||
+                                            units::traits::is_angular_velocity_unit<T>::value ||
+                                            units::traits::is_angular_acceleration_unit<T>::value>>
 struct Orientation3
 {
-    Orientation3() = default;
-    Orientation3(T yaw, T pitch, T roll) : yaw{yaw}, pitch{pitch}, roll{roll} {}
-
     T yaw{};
     T pitch{};
     T roll{};
 };
 
-using Orientation3d = Orientation3<double>;
-
-inline bool operator==(const Orientation3d& lhs, const Orientation3d& rhs) noexcept
+template <typename T>
+inline bool operator==(const Orientation3<T>& lhs, const Orientation3<T>& rhs) noexcept
 {
     return IsEqual(lhs.yaw, rhs.yaw) && IsEqual(lhs.pitch, rhs.pitch) && IsEqual(lhs.roll, rhs.roll);
 }
 
-inline bool operator!=(const Orientation3d& lhs, const Orientation3d& rhs) noexcept
+template <typename T>
+inline bool operator!=(const Orientation3<T>& lhs, const Orientation3<T>& rhs) noexcept
 {
     return !(lhs == rhs);
 }
 
-inline Orientation3d operator+(const Orientation3d& lhs, const Orientation3d& rhs) noexcept
+template <typename T>
+inline Orientation3<T> operator+(const Orientation3<T>& lhs, const Orientation3<T>& rhs) noexcept
 {
-    return Orientation3d{lhs.yaw + rhs.yaw, lhs.pitch + rhs.pitch, lhs.roll + rhs.roll};
+    return Orientation3<T>{lhs.yaw + rhs.yaw, lhs.pitch + rhs.pitch, lhs.roll + rhs.roll};
 }
 
-inline Orientation3d operator-(const Orientation3d& lhs, const Orientation3d& rhs) noexcept
+template <typename T>
+inline Orientation3<T> operator-(const Orientation3<T>& lhs, const Orientation3<T>& rhs) noexcept
 {
-    return Orientation3d{lhs.yaw - rhs.yaw, lhs.pitch - rhs.pitch, lhs.roll - rhs.roll};
+    return Orientation3<T>{lhs.yaw - rhs.yaw, lhs.pitch - rhs.pitch, lhs.roll - rhs.roll};
 }
 
 }  // namespace mantle_api

MantleAPI/include/MantleAPI/Common/pose.h

@@ -17,14 +17,19 @@
 
 #include <MantleAPI/Common/orientation.h>
 #include <MantleAPI/Common/vector.h>
+
+#include <units.h>
+
 #include <iostream>
 
+
 namespace mantle_api
 {
+
 struct Pose
 {
-    Vec3d position{};
-    Orientation3d orientation{};
+    Vec3<units::length::meter_t> position{};
+    Orientation3<units::angle::radian_t> orientation{};
 
     bool operator== (const Pose& other) const
     {

MantleAPI/include/MantleAPI/Common/position.h

@@ -17,6 +17,7 @@
 
 #include <MantleAPI/Common/floating_point_helper.h>
 #include <MantleAPI/Common/vector.h>
+#include <units.h>
 
 #include <cmath>
 #include <cstdint>
@@ -24,6 +25,7 @@
 
 namespace mantle_api
 {
+
 struct ReferencedObject
 {
     std::int32_t road{0};
@@ -35,20 +37,20 @@ struct OpenDrivePosition
     ReferencedObject referenced_object{};
 
     /// @brief Offset in s direction, unit: [m]
-    double s_offset{0.0};
+    units::length::meter_t s_offset{0.0};
     /// @brief Offset in t direction, unit: [m]
-    double t_offset{0.0};
+    units::length::meter_t t_offset{0.0};
 };
 
 struct LatLonPosition
 {
     /// @brief GPS latitude, unit: [rad]
-    double latitude{0.0};
+    units::angle::radian_t latitude{0.0};
     /// @brief GPS longitude, unit: [rad]
-    double longitude{0.0};
+    units::angle::radian_t longitude{0.0};
 };
 
-using Position = std::variant<OpenDrivePosition, LatLonPosition, Vec3d>;
+using Position = std::variant<OpenDrivePosition, LatLonPosition, Vec3<units::length::meter_t>>;
 
 /// @brief Equality comparison for OpenDrivePosition.
 ///

MantleAPI/include/MantleAPI/Common/simulation_time.h

@@ -15,15 +15,15 @@
 #ifndef MANTLEAPI_COMMON_SIMULATION_TIME_H
 #define MANTLEAPI_COMMON_SIMULATION_TIME_H
 
-#include "time_utils.h"
+#include <MantleAPI/Common/time_utils.h>
 
 namespace mantle_api
 {
 
 struct SimulationTime
 {
-    mantle_api::Time current_sim_time_ms{0};
-    mantle_api::Time last_delta_time_ms{0};
+    Time current_sim_time{0};
+    Time last_delta_time{0};
 };
 
 }  // namespace mantle_api

MantleAPI/include/MantleAPI/Common/spline.h

@@ -19,40 +19,45 @@
 #include <MantleAPI/Common/floating_point_helper.h>
 #include <MantleAPI/Common/time_utils.h>
 #include <MantleAPI/Common/vector.h>
+#include <units.h>
 
 #include <array>
 
 namespace mantle_api
 {
+
+template <typename T>
 struct SplineSegment
 {
-    mantle_api::Vec3d a;
-    mantle_api::Vec3d b;
-    mantle_api::Vec3d c;
-    mantle_api::Vec3d d;
+    Vec3<T> a;
+    Vec3<T> b;
+    Vec3<T> c;
+    Vec3<T> d;
 };
 
+template <typename T, class = typename std::enable_if_t<units::traits::is_unit_t<T>::value>>
 struct SplineSection
 {
-    mantle_api::Time start_time{0};
-    mantle_api::Time end_time{0};
+    Time start_time{0};
+    Time end_time{0};
     /// @brief Represents the polynomial.
     ///
     /// The array stores in format \f$[a_3, a_2, a_1, a_0]\f$ for a polynomial in form
     /// \f[
     ///   P(x) = \sum_{i=0}^{3} a_{i} x^{i} = a_3 x^3 + a_2 x^2 + a_1 x + a_0
     /// \f]
-    std::array<double, 4> polynomial{0, 0, 0, 0};
+    std::array<T, 4> polynomial{0, 0, 0, 0};
 };
 
 /// @brief Equality comparison for SplineSection.
-inline bool operator==(const SplineSection& lhs, const SplineSection& rhs) noexcept
+template <typename T, class = typename std::enable_if_t<units::traits::is_unit_t<T>::value>>
+inline bool operator==(const SplineSection<T>& lhs, const SplineSection<T>& rhs) noexcept
 {
     return lhs.start_time == rhs.start_time && lhs.end_time == rhs.end_time &&
            std::equal(lhs.polynomial.begin(),
                       lhs.polynomial.end(),
                       rhs.polynomial.begin(),
-                      [](const double a, const double b) { return IsEqual(a, b); });
+                      [](const T& a, const T& b) { return IsEqual(a, b); });
 }
 
 }  // namespace mantle_api

MantleAPI/include/MantleAPI/Common/time_utils.h

@@ -15,11 +15,11 @@
 #ifndef MANTLEAPI_COMMON_TIME_UTILS_H
 #define MANTLEAPI_COMMON_TIME_UTILS_H
 
-#include <chrono>
+#include <units.h>
 
 namespace mantle_api
 {
-using Time = std::chrono::duration<std::int64_t, std::milli>;
+using Time = units::time::millisecond_t;
 
 /// @brief Converts input in [s] to @ref Time.
 /// @tparam T Input type, eg. `double`.
@@ -28,7 +28,7 @@ using Time = std::chrono::duration<std::int64_t, std::milli>;
 template <typename T>
 inline Time SecondsToTime(T duration)
 {
-    return std::chrono::duration_cast<Time>(std::chrono::duration<T>{duration});
+    return units::convert<units::time::seconds, Time>(duration);
 }
 
 /// @brief Converts input @ref Time to [s].
@@ -36,7 +36,7 @@ inline Time SecondsToTime(T duration)
 /// @return Duration in seconds representing the passed in @ref Time.
 inline double TimeToSeconds(const Time& time)
 {
-    return static_cast<double>(time.count()) / 1000.0;
+    return units::time::second_t{time}.value();
 }
 
 }  // namespace mantle_api

MantleAPI/include/MantleAPI/Common/vector.h

@@ -21,53 +21,54 @@
 
 namespace mantle_api
 {
-
-template <typename T>
+template <typename T, class = typename std::enable_if_t<units::traits::is_unit_t<T>::value>>
 struct Vec3
 {
-    Vec3() = default;
-    Vec3(T x, T y, T z) : x{x}, y{y}, z{z} {}
-
-    T x{};
-    T y{};
-    T z{};
+    T x{0};
+    T y{0};
+    T z{0};
 
     inline T Length() const { return sqrt((x * x) + (y * y) + (z * z)); }
 };
 
-using Vec3d = Vec3<double>;
-
-inline bool operator==(const Vec3d& lhs, const Vec3d& rhs) noexcept
+template <typename T>
+inline bool operator==(const Vec3<T>& lhs, const Vec3<T>& rhs) noexcept
 {
     return IsEqual(lhs.x, rhs.x) && IsEqual(lhs.y, rhs.y) && IsEqual(lhs.z, rhs.z);
 }
 
-inline bool operator!=(const Vec3d& lhs, const Vec3d& rhs) noexcept
+template <typename T>
+inline bool operator!=(const Vec3<T>& lhs, const Vec3<T>& rhs) noexcept
 {
     return !(lhs == rhs);
 }
 
-inline Vec3d operator-(const Vec3d& lhs, const Vec3d& rhs) noexcept
+template <typename T>
+inline Vec3<T> operator-(const Vec3<T>& lhs, const Vec3<T>& rhs) noexcept
 {
     return {lhs.x - rhs.x, lhs.y - rhs.y, lhs.z - rhs.z};
 }
 
-inline Vec3d operator+(const Vec3d& lhs, const Vec3d& rhs) noexcept
+template <typename T>
+inline Vec3<T> operator+(const Vec3<T>& lhs, const Vec3<T>& rhs) noexcept
 {
     return {lhs.x + rhs.x, lhs.y + rhs.y, lhs.z + rhs.z};
 }
 
-inline Vec3d operator*(const Vec3d& lhs, double d) noexcept
+template <typename T>
+inline Vec3<T> operator*(const Vec3<T>& lhs, double d) noexcept
 {
     return {lhs.x * d, lhs.y * d, lhs.z * d};
 }
 
-inline Vec3d operator*(double d, const Vec3d& rhs) noexcept
+template <typename T>
+inline Vec3<T> operator*(double d, const Vec3<T>& rhs) noexcept
 {
     return rhs * d;
 }
 
-inline Vec3d operator/(const Vec3d& lhs, double d) noexcept
+template <typename T>
+inline Vec3<T> operator/(const Vec3<T>& lhs, double d) noexcept
 {
     return {lhs.x / d, lhs.y / d, lhs.z / d};
 }

MantleAPI/include/MantleAPI/EnvironmentalConditions/date_time.h

@@ -17,15 +17,13 @@
 
 #include <MantleAPI/Common/time_utils.h>
 
-#include <chrono>
-
 namespace mantle_api
 {
 
 // TODO: Delete this struct and use Time directly in Get/SetDateTime once the move to the MantleAPI is complete
 struct [[deprecated]] DateTime
 {
-    Time date_time_ms{0};
+    Time date_time{0};
 };
 
 }  // namespace mantle_api

MantleAPI/include/MantleAPI/EnvironmentalConditions/road_condition.h

@@ -16,19 +16,21 @@
 #define MANTLEAPI_ENVIRONMENTALCONDITIONS_ROADCONDITION_H
 
 #include <MantleAPI/Common/position.h>
+#include <units.h>
+
 namespace mantle_api
 {
 
 struct Rectangle
 {
-    Position bottom_left;
-    Position top_right;
+    Position bottom_left{};
+    Position top_right{};
 };
 
 struct FrictionPatch
 {
-    Rectangle bounding_box;
-    double friction;
+    Rectangle bounding_box{};
+    units::concentration::percent_t friction{100.0};
 };
 }  // namespace mantle_api
 #endif  // MANTLEAPI_ENVIRONMENTALCONDITIONS_ROADCONDITION_H

MantleAPI/include/MantleAPI/EnvironmentalConditions/weather.h

@@ -15,10 +15,11 @@
 #ifndef MANTLEAPI_ENVIRONMENTALCONDITIONS_WEATHER_H
 #define MANTLEAPI_ENVIRONMENTALCONDITIONS_WEATHER_H
 
-#include <chrono>
+#include <units.h>
 
 namespace mantle_api
 {
+
 enum class Precipitation
 {
     kUnknown,
@@ -66,10 +67,11 @@ struct Weather
     Fog fog{Fog::kExcellentVisibility};
     Precipitation precipitation{Precipitation::kNone};
     Illumination illumination{Illumination::kOther};
-    double humidity_percent{0.0};
-    double temperature_kelvin{0.0};
-    double atmospheric_pressure_pascal{0.0};
+    units::concentration::percent_t humidity{0.0};
+    units::temperature::kelvin_t temperature{0.0};
+    units::pressure::pascal_t atmospheric_pressure{0.0};
 };
+
 }  // namespace mantle_api
 
 #endif  // MANTLEAPI_ENVIRONMENTALCONDITIONS_WEATHER_H

MantleAPI/include/MantleAPI/Execution/i_environment.h

@@ -27,7 +27,6 @@
 
 namespace mantle_api
 {
-
 class IEnvironment
 {
   public:
@@ -50,24 +49,22 @@ class IEnvironment
     ///
     /// @param entity The entity to be manipulated by the specified controller.
     /// @param controller_id Identifies the controller to manipulate the entity.
-    virtual void AddEntityToController(IEntity& entity, std::uint64_t controller_id) = 0;
+    virtual void AddEntityToController(IEntity& entity, UniqueId controller_id) = 0;
 
-    virtual void RemoveControllerFromEntity(std::uint64_t entity_id) = 0;
+    virtual void RemoveControllerFromEntity(UniqueId entity_id) = 0;
 
     /// Updates the control strategies for an entity.
     ///
     /// @param entity_id          Specifies the entity to be updated
     /// @param control_strategies Specifies the desired movement behaviour for the entity
     virtual void UpdateControlStrategies(
-        std::uint64_t entity_id,
-        std::vector<std::unique_ptr<mantle_api::ControlStrategy>>& control_strategies) = 0;
+        UniqueId entity_id, std::vector<std::unique_ptr<mantle_api::ControlStrategy>>& control_strategies) = 0;
 
     /// Checks, if a control strategy of a certain type for a specific entity has been fulfilled
     ///
     /// @param entity_id    The entity to check
     /// @param type         The control strategy type
-    virtual bool HasControlStrategyGoalBeenReached(std::uint64_t entity_id,
-                                                   mantle_api::ControlStrategyType type) const = 0;
+    virtual bool HasControlStrategyGoalBeenReached(UniqueId entity_id, mantle_api::ControlStrategyType type) const = 0;
 
     virtual const ILaneLocationQueryService& GetQueryService() const = 0;
     virtual const ICoordConverter* GetConverter() const = 0;

MantleAPI/include/MantleAPI/Map/i_coord_converter.h

@@ -17,6 +17,7 @@
 
 #include <MantleAPI/Common/position.h>
 #include <MantleAPI/Common/vector.h>
+#include <units.h>
 
 namespace mantle_api
 {
@@ -28,11 +29,12 @@ class ICoordConverter
   public:
     virtual ~ICoordConverter() = default;
     /// Converts a track position to its corresponding inertial position.
-    virtual Vec3d Convert(Position position) const = 0;
+    virtual Vec3<units::length::meter_t> Convert(Position position) const = 0;
 
     /// Converts alane position to its corresponding inertial position.
-    virtual Position Convert(Vec3d) const = 0;
+    virtual Position Convert(const Vec3<units::length::meter_t>& inert_pos) const = 0;
 };
+
 }  // namespace mantle_api
 
 #endif  // MANTLEAPI_MAP_ICOORDCONVERTER_H

MantleAPI/include/MantleAPI/Map/i_lane_location_query_service.h

@@ -18,6 +18,7 @@
 #include <MantleAPI/Common/i_identifiable.h>
 #include <MantleAPI/Common/orientation.h>
 #include <MantleAPI/Common/vector.h>
+#include <units.h>
 
 namespace mantle_api
 {
@@ -35,12 +36,14 @@ class ILaneLocationQueryService
     /// would add a rat-tail of more interfaces we need to define (ILaneLocation, ILane, ...?)
     // virtual const IIdentifiable& GetMapObjectById(UniqueId id) = 0;
 
-    virtual Orientation3d GetLaneOrientation(const Vec3d& position) const = 0;
-    virtual Vec3d GetUpwardsShiftedLanePosition(const Vec3d& position,
-                                                double upwards_shift,
-                                                bool allow_invalid_positions = false) const = 0;
-    virtual bool IsPositionOnLane(const Vec3d& position) const = 0;
+    virtual Orientation3<units::angle::radian_t> GetLaneOrientation(
+        const Vec3<units::length::meter_t>& position) const = 0;
+    virtual Vec3<units::length::meter_t> GetUpwardsShiftedLanePosition(const Vec3<units::length::meter_t>& position,
+                                                                       double upwards_shift,
+                                                                       bool allow_invalid_positions = false) const = 0;
+    virtual bool IsPositionOnLane(const Vec3<units::length::meter_t>& position) const = 0;
 };
+
 }  // namespace mantle_api
 
 #endif  // MANTLEAPI_MAP_ILANELOCATIONQUERYSERVICE_H

MantleAPI/include/MantleAPI/Map/i_route.h

@@ -17,9 +17,11 @@
 
 #include <MantleAPI/Common/i_identifiable.h>
 #include <MantleAPI/Common/vector.h>
+#include <units.h>
 
 namespace mantle_api
 {
+
 enum class LaneId
 {
     kLeft9,
@@ -46,14 +48,18 @@ enum class LaneId
 class IRoute : public virtual IIdentifiable
 {
   public:
-    virtual IRoute& AddWaypoint(const Vec3d& inert_pos) = 0;
-    virtual IRoute& AddWaypoint(Vec3d&& inert_pos) = 0;
-    virtual Vec3d GetInertPos(double route_pos, LaneId lane_id, double lane_offset = 0) const = 0;
-    virtual double GetLaneWidth(double route_pos, LaneId lane_id) const = 0;
-    virtual LaneId GetLaneId(const Vec3d& inert_pos) const = 0;
-    virtual double GetDistanceFromStartTo(const Vec3d& inert_pos) const = 0;
-    virtual double GetLength() const = 0;
+    virtual IRoute& AddWaypoint(const Vec3<units::length::meter_t>& inert_pos) = 0;
+    virtual IRoute& AddWaypoint(Vec3<units::length::meter_t>&& inert_pos) = 0;
+    virtual Vec3<units::length::meter_t> GetInertPos(units::length::meter_t route_pos,
+                                                     LaneId lane_id,
+                                                     units::length::meter_t lane_offset = units::length::meter_t{
+                                                         0.0}) const = 0;
+    virtual units::length::meter_t GetLaneWidth(units::length::meter_t route_pos, LaneId lane_id) const = 0;
+    virtual LaneId GetLaneId(const Vec3<units::length::meter_t>& inert_pos) const = 0;
+    virtual units::length::meter_t GetDistanceFromStartTo(const Vec3<units::length::meter_t>& inert_pos) const = 0;
+    virtual units::length::meter_t GetLength() const = 0;
 };
+
 }  // namespace mantle_api
 
 #endif  // MANTLEAPI_MAP_IROUTE_H

MantleAPI/include/MantleAPI/Traffic/control_strategy.h

@@ -93,7 +93,7 @@ struct FollowHeadingSplineControlStrategy : public ControlStrategy
         type = ControlStrategyType::kFollowHeadingSpline;
     }
 
-    std::vector<mantle_api::SplineSection> heading_splines;
+    std::vector<mantle_api::SplineSection<units::angle::radian_t>> heading_splines;
     double default_value{0};
 };
 
@@ -105,7 +105,7 @@ struct FollowVelocitySplineControlStrategy : public ControlStrategy
         type = ControlStrategyType::kFollowVelocitySpline;
     }
 
-    std::vector<mantle_api::SplineSection> velocity_splines;
+    std::vector<mantle_api::SplineSection<units::velocity::meters_per_second_t>> velocity_splines;
     double default_value{0};
 };
 
@@ -129,7 +129,7 @@ struct FollowLateralOffsetSplineControlStrategy : public ControlStrategy
         type = ControlStrategyType::kFollowLateralOffsetSpline;
     }
 
-    std::vector<mantle_api::SplineSection> lateral_offset_splines;
+    std::vector<mantle_api::SplineSection<units::length::meter_t>> lateral_offset_splines;
 };
 
 struct FollowRouteControlStrategy : public ControlStrategy
@@ -140,7 +140,7 @@ struct FollowRouteControlStrategy : public ControlStrategy
         type = ControlStrategyType::kFollowRoute;
     }
 
-    std::vector<mantle_api::Vec3d> waypoints;
+    std::vector<mantle_api::Vec3<units::length::meter_t>> waypoints;
 };
 
 enum class Dimension
@@ -177,7 +177,7 @@ struct AcquireLaneOffsetControlStrategy : public ControlStrategy
 
     int road_id{};
     int lane_id{};
-    double offset{};
+    units::length::meter_t offset{};
     TransitionDynamics transition_dynamics;
 };
 

MantleAPI/include/MantleAPI/Traffic/entity_properties.h

@@ -24,6 +24,7 @@
 
 namespace mantle_api
 {
+
 enum class EntityType
 {
     // Other (unspecified but known) type of moving object.
@@ -41,7 +42,7 @@ struct EntityProperties
 {
     virtual ~EntityProperties() = default;
 
-    BoundingBox bounding_box{Vec3d{}, Dimension3d{}};
+    BoundingBox bounding_box{};
     EntityType type{EntityType::kOther};
     std::string model{};
     std::map<std::string, std::string> properties{};
@@ -118,12 +119,9 @@ enum class ExternalControlState
 
 struct Performance
 {
-    // in m/s
-    double max_speed{0.0};
-    // in m/s²
-    double max_acceleration{0.0};
-    // in m/s²
-    double max_deceleration{0.0};
+    units::velocity::meters_per_second_t max_speed{0.0};
+    units::acceleration::meters_per_second_squared_t max_acceleration{0.0};
+    units::acceleration::meters_per_second_squared_t max_deceleration{0.0};
 };
 
 inline bool operator==(const Performance& lhs, const Performance& rhs) noexcept
@@ -135,14 +133,10 @@ inline bool operator==(const Performance& lhs, const Performance& rhs) noexcept
 
 struct Axle
 {
-    // in radian
-    double max_steering{0.0};
-    // in m
-    double wheel_diameter{0.0};
-    // in m
-    double track_width{0.0};
-    // in m
-    Vec3d bb_center_to_axle_center{};
+    units::angle::radian_t max_steering{0.0};
+    units::length::meter_t wheel_diameter{0.0};
+    units::length::meter_t track_width{0.0};
+    Vec3<units::length::meter_t> bb_center_to_axle_center{};
 };
 
 inline bool operator==(const Axle& lhs, const Axle& rhs) noexcept

MantleAPI/include/MantleAPI/Traffic/i_controller_config.h

@@ -15,11 +15,10 @@
 #ifndef MANTLEAPI_TRAFFIC_ICONTROLLERCONFIG_H
 #define MANTLEAPI_TRAFFIC_ICONTROLLERCONFIG_H
 
-#include "control_strategy.h"
-
 #include <MantleAPI/Common/spline.h>
 #include <MantleAPI/Common/vector.h>
 #include <MantleAPI/Map/i_lane_location_query_service.h>
+#include <MantleAPI/Traffic/control_strategy.h>
 
 #include <algorithm>
 #include <map>
@@ -35,11 +34,11 @@ struct IControllerConfig
     IControllerConfig(const IControllerConfig& controller_config)
         : map_query_service(controller_config.map_query_service), id(controller_config.id)
     {
-        std::transform(
-            controller_config.control_strategies.begin(),
-            controller_config.control_strategies.end(),
-            std::back_inserter(control_strategies),
-            [](auto& control_strategy) { return std::make_unique<mantle_api::ControlStrategy>(*control_strategy); });
+        std::transform(controller_config.control_strategies.begin(),
+                       controller_config.control_strategies.end(),
+                       std::back_inserter(control_strategies),
+                       [](auto& control_strategy)
+                       { return std::make_unique<mantle_api::ControlStrategy>(*control_strategy); });
     }
 
     virtual ~IControllerConfig() = default;