diff --git a/MantleAPI/include/MantleAPI/Common/floating_point_helper.h b/MantleAPI/include/MantleAPI/Common/floating_point_helper.h
index 690c90e0ed9addaaa5a5b1e983efffb6a465272a..78346376b6b0fa617bfb9096b51c849cd5c620c9 100644
--- a/MantleAPI/include/MantleAPI/Common/floating_point_helper.h
+++ b/MantleAPI/include/MantleAPI/Common/floating_point_helper.h
@@ -59,7 +59,7 @@ inline bool IsEqual(double lhs, double rhs, double precision = std::numeric_limi
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;
+ return units::unit_cast<double>(units::math::abs(lhs - rhs)) < precision;
}
/// @brief Compares two floating point numbers for equality.
diff --git a/MantleAPI/include/MantleAPI/Common/poly_line.h b/MantleAPI/include/MantleAPI/Common/poly_line.h
index 0bed8c5560e6a11a6fc6c18430d05c5ad5f16eb0..9e9f70a9e7000a219d2c482b02ed78fac3855ffa 100644
--- a/MantleAPI/include/MantleAPI/Common/poly_line.h
+++ b/MantleAPI/include/MantleAPI/Common/poly_line.h
@@ -42,7 +42,7 @@ inline std::ostream& operator<<(std::ostream& os, const PolyLinePoint& polyLineP
if (polyLinePoint.time.has_value())
{
- os << ", time in ms " << polyLinePoint.time.value().count();
+ os << ", time in ms " << polyLinePoint.time.value();
}
return os;
diff --git a/MantleAPI/include/MantleAPI/Common/spline.h b/MantleAPI/include/MantleAPI/Common/spline.h
index 8a9a4308f88f889148476bfb92daf2bd4ea2589e..32c97bc9d17b0624d7819acc84ad954968a6f203 100644
--- a/MantleAPI/include/MantleAPI/Common/spline.h
+++ b/MantleAPI/include/MantleAPI/Common/spline.h
@@ -21,7 +21,9 @@
#include <MantleAPI/Common/vector.h>
#include <units.h>
-#include <array>
+#include <tuple>
+
+using namespace units;
namespace mantle_api
{
@@ -34,29 +36,33 @@ struct SplineSegment
Vec3<T> d;
};
-template <typename T, class = typename std::enable_if_t<units::traits::is_unit_t<T>::value>>
+template <typename T, class = typename std::enable_if_t<traits::is_unit<T>::value>>
struct SplineSection
{
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
+ /// The tuple 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<T, 4> polynomial{0, 0, 0, 0};
+ std::tuple<unit_t<compound_unit<T, inverse<cubed<time::second>>>>,
+ unit_t<compound_unit<T, inverse<squared<time::second>>>>,
+ unit_t<compound_unit<T, inverse<time::second>>>,
+ unit_t<T>>
+ polynomial{
+ unit_t<compound_unit<T, inverse<cubed<time::second>>>>(0),
+ unit_t<compound_unit<T, inverse<squared<time::second>>>>(0),
+ unit_t<compound_unit<T, inverse<time::second>>>(0),
+ unit_t<T>(0)};
};
/// @brief Equality comparison for SplineSection.
-template <typename T, class = typename std::enable_if_t<units::traits::is_unit_t<T>::value>>
+template <typename T, class = typename std::enable_if_t<units::traits::is_unit<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 T& a, const T& b) { return IsEqual(a, b); });
+ return lhs.start_time == rhs.start_time && lhs.end_time == rhs.end_time && IsEqual(std::get<0>(lhs.polynomial), std::get<0>(rhs.polynomial)) && IsEqual(std::get<1>(lhs.polynomial), std::get<1>(rhs.polynomial)) && IsEqual(std::get<2>(lhs.polynomial), std::get<2>(rhs.polynomial)) && IsEqual(std::get<3>(lhs.polynomial), std::get<3>(rhs.polynomial));
}
} // namespace mantle_api
diff --git a/MantleAPI/include/MantleAPI/Common/vector.h b/MantleAPI/include/MantleAPI/Common/vector.h
index 5d7a110dc41c9ce0fa117268019c4f35a49d0673..73df1bf6f1a840aebef2317ed0ca66a1949b8fe3 100644
--- a/MantleAPI/include/MantleAPI/Common/vector.h
+++ b/MantleAPI/include/MantleAPI/Common/vector.h
@@ -28,7 +28,7 @@ struct Vec3
T y{0};
T z{0};
- inline T Length() const { return sqrt((x * x) + (y * y) + (z * z)); }
+ inline T Length() const { return units::math::sqrt((x * x) + (y * y) + (z * z)); }
};
template <typename T>
@@ -73,7 +73,8 @@ inline Vec3<T> operator/(const Vec3<T>& lhs, double d) noexcept
return {lhs.x / d, lhs.y / d, lhs.z / d};
}
-inline Vec3d operator+=(Vec3d& lhs, const Vec3d& rhs) noexcept
+template <typename T>
+inline Vec3<T> operator+=(Vec3<T>& lhs, const Vec3<T>& rhs) noexcept
{
lhs.x += rhs.x;
lhs.y += rhs.y;
@@ -81,7 +82,8 @@ inline Vec3d operator+=(Vec3d& lhs, const Vec3d& rhs) noexcept
return lhs;
}
-inline Vec3d operator-=(Vec3d& lhs, const Vec3d& rhs) noexcept
+template <typename T>
+inline Vec3<T> operator-=(Vec3<T>& lhs, const Vec3<T>& rhs) noexcept
{
lhs.x -= rhs.x;
lhs.y -= rhs.y;
@@ -89,15 +91,16 @@ inline Vec3d operator-=(Vec3d& lhs, const Vec3d& rhs) noexcept
return lhs;
}
-inline Vec3d operator+=(Vec3d& lhs, double d) noexcept
+template <typename T>
+inline Vec3<T> operator+=(Vec3<T>& lhs, double d) noexcept
{
lhs.x += d;
lhs.y += d;
lhs.z += d;
return lhs;
}
-
-inline Vec3d operator-=(Vec3d& lhs, double d) noexcept
+template <typename T>
+inline Vec3<T> operator-=(Vec3<T>& lhs, double d) noexcept
{
lhs.x -= d;
lhs.y -= d;
diff --git a/MantleAPI/include/MantleAPI/Traffic/control_strategy.h b/MantleAPI/include/MantleAPI/Traffic/control_strategy.h
index a896c9c9696c54745603c3aedb3418e22b0c35e2..5a8e3d40ee9f0b4e7bedb46f2baae376da839db3 100644
--- a/MantleAPI/include/MantleAPI/Traffic/control_strategy.h
+++ b/MantleAPI/include/MantleAPI/Traffic/control_strategy.h
@@ -92,8 +92,8 @@ struct FollowHeadingSplineControlStrategy : public ControlStrategy
type = ControlStrategyType::kFollowHeadingSpline;
}
- std::vector<mantle_api::SplineSection<units::angle::radian_t>> heading_splines;
- double default_value{0};
+ std::vector<mantle_api::SplineSection<units::angle::radian>> heading_splines;
+ units::angle::radian_t default_value{0};
};
struct FollowVelocitySplineControlStrategy : public ControlStrategy
@@ -104,8 +104,8 @@ struct FollowVelocitySplineControlStrategy : public ControlStrategy
type = ControlStrategyType::kFollowVelocitySpline;
}
- std::vector<mantle_api::SplineSection<units::velocity::meters_per_second_t>> velocity_splines;
- double default_value{0};
+ std::vector<mantle_api::SplineSection<units::velocity::meters_per_second>> velocity_splines;
+ units::velocity::meters_per_second_t default_value{0};
};
inline bool operator==(const FollowVelocitySplineControlStrategy& lhs,
@@ -128,7 +128,7 @@ struct FollowLateralOffsetSplineControlStrategy : public ControlStrategy
type = ControlStrategyType::kFollowLateralOffsetSpline;
}
- std::vector<mantle_api::SplineSection<units::length::meter_t>> lateral_offset_splines;
+ std::vector<mantle_api::SplineSection<units::length::meter>> lateral_offset_splines;
};
struct FollowRouteControlStrategy : public ControlStrategy
@@ -194,7 +194,7 @@ struct FollowTrajectoryControlStrategy : public ControlStrategy
{
ReferenceContext domainAbsoluteRelative;
double scale{0.0};
- double offset{0.0};
+ units::time::second_t offset{0.0};
};
FollowTrajectoryControlStrategy()
diff --git a/MantleAPI/include/MantleAPI/Traffic/entity_helper.h b/MantleAPI/include/MantleAPI/Traffic/entity_helper.h
index 0c65ca40b0707fb757d55575d590ebcdef46a064..a09a4534b122807c65fb6dda7f586fb981a05d1b 100644
--- a/MantleAPI/include/MantleAPI/Traffic/entity_helper.h
+++ b/MantleAPI/include/MantleAPI/Traffic/entity_helper.h
@@ -21,14 +21,15 @@
namespace mantle_api
{
-inline void SetSpeed(mantle_api::IEntity* entity, double velocity)
+
+inline void SetSpeed(mantle_api::IEntity* entity, const units::velocity::meters_per_second_t &velocity)
{
auto orientation = entity->GetOrientation();
- double cos_elevation = std::cos(orientation.pitch);
- mantle_api::Vec3d velocity_vector{velocity * std::cos(orientation.yaw) * cos_elevation,
- velocity * std::sin(orientation.yaw) * cos_elevation,
- velocity * -std::sin(orientation.pitch)};
+ auto cos_elevation = units::math::cos(orientation.pitch);
+ mantle_api::Vec3<units::velocity::meters_per_second_t> velocity_vector{velocity * units::math::cos(orientation.yaw) * cos_elevation,
+ velocity * units::math::sin(orientation.yaw) * cos_elevation,
+ velocity * -units::math::sin(orientation.pitch)};
entity->SetVelocity(velocity_vector);
}
diff --git a/MantleAPI/test/MantleAPI/Test/test_utils.h b/MantleAPI/test/MantleAPI/Test/test_utils.h
index 7c6494cb570a1c05a65a87d6471828eeb9a9c13c..25d4d0ba13cd581066007007711387144b4e9ccd 100644
--- a/MantleAPI/test/MantleAPI/Test/test_utils.h
+++ b/MantleAPI/test/MantleAPI/Test/test_utils.h
@@ -33,9 +33,9 @@ namespace mantle_api
class MockConverter : public mantle_api::ICoordConverter
{
public:
- MOCK_METHOD(mantle_api::Vec3d, Convert, (mantle_api::Position position), (const override));
+ MOCK_METHOD(mantle_api::Vec3<units::length::meter_t>, Convert, (mantle_api::Position position), (const override));
- mantle_api::Position Convert(mantle_api::Vec3d vec) const override
+ mantle_api::Position Convert(mantle_api::Vec3<units::length::meter_t> vec) const override
{
std::ignore = vec;
return mantle_api::Position{};
@@ -50,26 +50,26 @@ public:
void SetName(const std::string& name) override { name_ = name; }
const std::string& GetName() const override { return name_; }
- MOCK_METHOD(void, SetPosition, (const mantle_api::Vec3d& inert_pos), (override));
- MOCK_METHOD(mantle_api::Vec3d, GetPosition, (), (const, override));
+ MOCK_METHOD(void, SetPosition, (const mantle_api::Vec3<units::length::meter_t>& inert_pos), (override));
+ MOCK_METHOD(mantle_api::Vec3<units::length::meter_t>, GetPosition, (), (const, override));
- MOCK_METHOD(void, SetVelocity, (const mantle_api::Vec3d& velocity), (override));
- MOCK_METHOD(mantle_api::Vec3d, GetVelocity, (), (const, override));
+ MOCK_METHOD(void, SetVelocity, (const mantle_api::Vec3<units::velocity::meters_per_second_t>& velocity), (override));
+ MOCK_METHOD(mantle_api::Vec3<units::velocity::meters_per_second_t>, GetVelocity, (), (const, override));
- MOCK_METHOD(void, SetAcceleration, (const mantle_api::Vec3d& acceleration), (override));
- MOCK_METHOD(mantle_api::Vec3d, GetAcceleration, (), (const, override));
+ MOCK_METHOD(void, SetAcceleration, (const mantle_api::Vec3<units::acceleration::meters_per_second_squared_t>& acceleration), (override));
+ MOCK_METHOD(mantle_api::Vec3<units::acceleration::meters_per_second_squared_t>, GetAcceleration, (), (const, override));
- MOCK_METHOD(void, SetOrientation, (const mantle_api::Orientation3d& orientation), (override));
- MOCK_METHOD(mantle_api::Orientation3d, GetOrientation, (), (const, override));
+ MOCK_METHOD(void, SetOrientation, (const mantle_api::Orientation3<units::angle::radian_t>& orientation), (override));
+ MOCK_METHOD(mantle_api::Orientation3<units::angle::radian_t>, GetOrientation, (), (const, override));
- MOCK_METHOD(void, SetOrientationRate, (const mantle_api::Orientation3d& orientation_rate), (override));
- MOCK_METHOD(mantle_api::Orientation3d, GetOrientationRate, (), (const, override));
+ MOCK_METHOD(void, SetOrientationRate, (const mantle_api::Orientation3<units::angular_velocity::radians_per_second_t>& orientation_rate), (override));
+ MOCK_METHOD(mantle_api::Orientation3<units::angular_velocity::radians_per_second_t>, GetOrientationRate, (), (const, override));
MOCK_METHOD(void,
SetOrientationAcceleration,
- (const mantle_api::Orientation3d& orientation_acceleration),
+ (const mantle_api::Orientation3<units::angular_acceleration::radians_per_second_squared_t>& orientation_acceleration),
(override));
- MOCK_METHOD(mantle_api::Orientation3d, GetOrientationAcceleration, (), (const, override));
+ MOCK_METHOD(mantle_api::Orientation3<units::angular_acceleration::radians_per_second_squared_t>, GetOrientationAcceleration, (), (const, override));
MOCK_METHOD(void, SetAssignedLaneIds, (const std::vector<std::uint64_t>& ids), (override));
MOCK_METHOD(std::vector<std::uint64_t>, GetAssignedLaneIds, (), (const, override));
@@ -91,22 +91,22 @@ private:
class MockQueryService : public mantle_api::ILaneLocationQueryService
{
public:
- Orientation3d GetLaneOrientation(const Vec3d& position) const override
+ Orientation3<units::angle::radian_t> GetLaneOrientation(const Vec3<units::length::meter_t>& position) const override
{
std::ignore = position;
return {};
}
- Vec3d GetUpwardsShiftedLanePosition(const Vec3d& position,
+ Vec3<units::length::meter_t> GetUpwardsShiftedLanePosition(const Vec3<units::length::meter_t>& position,
double upwards_shift,
bool allowed_to_leave_lane = false) const override
{
std::ignore = position;
std::ignore = upwards_shift;
std::ignore = allowed_to_leave_lane;
- return mantle_api::Vec3d();
+ return mantle_api::Vec3<units::length::meter_t>();
}
- bool IsPositionOnLane(const Vec3d& position) const override
+ bool IsPositionOnLane(const Vec3<units::length::meter_t>& position) const override
{
std::ignore = position;
return false;
@@ -124,26 +124,26 @@ public:
void SetName(const std::string& name) override { name_ = name; }
const std::string& GetName() const override { return name_; }
- MOCK_METHOD(void, SetPosition, (const mantle_api::Vec3d& inert_pos), (override));
- MOCK_METHOD(mantle_api::Vec3d, GetPosition, (), (const, override));
+ MOCK_METHOD(void, SetPosition, (const mantle_api::Vec3<units::length::meter_t>& inert_pos), (override));
+ MOCK_METHOD(mantle_api::Vec3<units::length::meter_t>, GetPosition, (), (const, override));
- MOCK_METHOD(void, SetVelocity, (const mantle_api::Vec3d& velocity), (override));
- MOCK_METHOD(mantle_api::Vec3d, GetVelocity, (), (const, override));
+ MOCK_METHOD(void, SetVelocity, (const mantle_api::Vec3<units::velocity::meters_per_second_t>& velocity), (override));
+ MOCK_METHOD(mantle_api::Vec3<units::velocity::meters_per_second_t>, GetVelocity, (), (const, override));
- MOCK_METHOD(void, SetAcceleration, (const mantle_api::Vec3d& acceleration), (override));
- MOCK_METHOD(mantle_api::Vec3d, GetAcceleration, (), (const, override));
+ MOCK_METHOD(void, SetAcceleration, (const mantle_api::Vec3<units::acceleration::meters_per_second_squared_t>& acceleration), (override));
+ MOCK_METHOD(mantle_api::Vec3<units::acceleration::meters_per_second_squared_t>, GetAcceleration, (), (const, override));
- MOCK_METHOD(void, SetOrientation, (const mantle_api::Orientation3d& orientation), (override));
- MOCK_METHOD(mantle_api::Orientation3d, GetOrientation, (), (const, override));
+ MOCK_METHOD(void, SetOrientation, (const mantle_api::Orientation3<units::angle::radian_t>& orientation), (override));
+ MOCK_METHOD(mantle_api::Orientation3<units::angle::radian_t>, GetOrientation, (), (const, override));
- MOCK_METHOD(void, SetOrientationRate, (const mantle_api::Orientation3d& orientation_rate), (override));
- MOCK_METHOD(mantle_api::Orientation3d, GetOrientationRate, (), (const, override));
+ MOCK_METHOD(void, SetOrientationRate, (const mantle_api::Orientation3<units::angular_velocity::radians_per_second_t>& orientation_rate), (override));
+ MOCK_METHOD(mantle_api::Orientation3<units::angular_velocity::radians_per_second_t>, GetOrientationRate, (), (const, override));
MOCK_METHOD(void,
SetOrientationAcceleration,
- (const mantle_api::Orientation3d& orientation_acceleration),
+ (const mantle_api::Orientation3<units::angular_acceleration::radians_per_second_squared_t>& orientation_acceleration),
(override));
- MOCK_METHOD(mantle_api::Orientation3d, GetOrientationAcceleration, (), (const, override));
+ MOCK_METHOD(mantle_api::Orientation3<units::angular_acceleration::radians_per_second_squared_t>, GetOrientationAcceleration, (), (const, override));
MOCK_METHOD(void, SetAssignedLaneIds, (const std::vector<std::uint64_t>& ids), (override));
MOCK_METHOD(std::vector<std::uint64_t>, GetAssignedLaneIds, (), (const, override));
@@ -167,26 +167,26 @@ public:
void SetName(const std::string& name) override { name_ = name; }
const std::string& GetName() const override { return name_; }
- MOCK_METHOD(void, SetPosition, (const mantle_api::Vec3d& inert_pos), (override));
- MOCK_METHOD(mantle_api::Vec3d, GetPosition, (), (const, override));
+ MOCK_METHOD(void, SetPosition, (const mantle_api::Vec3<units::length::meter_t>& inert_pos), (override));
+ MOCK_METHOD(mantle_api::Vec3<units::length::meter_t>, GetPosition, (), (const, override));
- MOCK_METHOD(void, SetVelocity, (const mantle_api::Vec3d& velocity), (override));
- MOCK_METHOD(mantle_api::Vec3d, GetVelocity, (), (const, override));
+ MOCK_METHOD(void, SetVelocity, (const mantle_api::Vec3<units::velocity::meters_per_second_t>& velocity), (override));
+ MOCK_METHOD(mantle_api::Vec3<units::velocity::meters_per_second_t>, GetVelocity, (), (const, override));
- MOCK_METHOD(void, SetAcceleration, (const mantle_api::Vec3d& acceleration), (override));
- MOCK_METHOD(mantle_api::Vec3d, GetAcceleration, (), (const, override));
+ MOCK_METHOD(void, SetAcceleration, (const mantle_api::Vec3<units::acceleration::meters_per_second_squared_t>& acceleration), (override));
+ MOCK_METHOD(mantle_api::Vec3<units::acceleration::meters_per_second_squared_t>, GetAcceleration, (), (const, override));
- MOCK_METHOD(void, SetOrientation, (const mantle_api::Orientation3d& orientation), (override));
- MOCK_METHOD(mantle_api::Orientation3d, GetOrientation, (), (const, override));
+ MOCK_METHOD(void, SetOrientation, (const mantle_api::Orientation3<units::angle::radian_t>& orientation), (override));
+ MOCK_METHOD(mantle_api::Orientation3<units::angle::radian_t>, GetOrientation, (), (const, override));
- MOCK_METHOD(void, SetOrientationRate, (const mantle_api::Orientation3d& orientation_rate), (override));
- MOCK_METHOD(mantle_api::Orientation3d, GetOrientationRate, (), (const, override));
+ MOCK_METHOD(void, SetOrientationRate, (const mantle_api::Orientation3<units::angular_velocity::radians_per_second_t>& orientation_rate), (override));
+ MOCK_METHOD(mantle_api::Orientation3<units::angular_velocity::radians_per_second_t>, GetOrientationRate, (), (const, override));
MOCK_METHOD(void,
SetOrientationAcceleration,
- (const mantle_api::Orientation3d& orientation_acceleration),
+ (const mantle_api::Orientation3<units::angular_acceleration::radians_per_second_squared_t>& orientation_acceleration),
(override));
- MOCK_METHOD(mantle_api::Orientation3d, GetOrientationAcceleration, (), (const, override));
+ MOCK_METHOD(mantle_api::Orientation3<units::angular_acceleration::radians_per_second_squared_t>, GetOrientationAcceleration, (), (const, override));
MOCK_METHOD(void, SetAssignedLaneIds, (const std::vector<std::uint64_t>& ids), (override));
MOCK_METHOD(std::vector<std::uint64_t>, GetAssignedLaneIds, (), (const, override));