Update and check control strategies for entity and not for controller; * and / operator for vectors

Signed-off-by: Arun Das <arun.das@bmw.de>

MantleAPI/include/MantleAPI/Common/spline.h

+
 /*******************************************************************************
  * Copyright (c) 2021, Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
  *
@@ -17,11 +18,20 @@
 
 #include <MantleAPI/Common/floating_point_helper.h>
 #include <MantleAPI/Common/time_utils.h>
+#include <MantleAPI/Common/vector.h>
 
 #include <array>
 
 namespace mantle_api
 {
+struct SplineSegment
+{
+    mantle_api::Vec3d a;
+    mantle_api::Vec3d b;
+    mantle_api::Vec3d c;
+    mantle_api::Vec3d d;
+};
+
 struct SplineSection
 {
     mantle_api::Time start_time{0};

MantleAPI/include/MantleAPI/Common/vector.h

@@ -62,6 +62,16 @@ inline Vec3d operator*(const Vec3d& lhs, double d) noexcept
     return {lhs.x * d, lhs.y * d, lhs.z * d};
 }
 
+inline Vec3d operator*(double d, const Vec3d& rhs) noexcept
+{
+    return rhs * d;
+}
+
+inline Vec3d operator/(const Vec3d& lhs, double d) noexcept
+{
+    return {lhs.x / d, lhs.y / d, lhs.z / d};
+}
+
 }  // namespace mantle_api
 
 #endif  // MANTLEAPI_COMMON_VECTOR_H

MantleAPI/include/MantleAPI/Execution/i_environment.h

@@ -54,19 +54,19 @@ class IEnvironment
 
     virtual void RemoveControllerFromEntity(std::uint64_t entity_id) = 0;
 
-    /// Updates the control strategies in the composite controller.
+    /// Updates the control strategies for an entity.
     ///
-    /// @param controller_id      Specifies the controller to be updated
+    /// @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 controller_id,
+        std::uint64_t entity_id,
         std::vector<std::unique_ptr<mantle_api::ControlStrategy>>& control_strategies) = 0;
 
-    /// Checks, if a control strategy of a certain type in a specific controller is fulfilled
+    /// Checks, if a control strategy of a certain type for a specific entity has been fulfilled
     ///
-    /// @param controller_id    The controller to check
-    /// @param type             The control strategy type
-    virtual bool HasControlStrategyGoalBeenReached(std::uint64_t controller_id,
+    /// @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 const ILaneLocationQueryService& GetQueryService() const = 0;

MantleAPI/include/MantleAPI/Map/i_lane_location_query_service.h

@@ -36,7 +36,9 @@ class ILaneLocationQueryService
     // virtual const IIdentifiable& GetMapObjectById(UniqueId id) = 0;
 
     virtual Orientation3d GetLaneOrientation(const Vec3d& position) const = 0;
-    virtual Vec3d GetUpwardsShiftedLanePosition(const Vec3d& position, double upwards_shift) 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;
 };
 }  // namespace mantle_api

MantleAPI/test/MantleAPI/Test/test_utils.h

@@ -46,7 +46,7 @@ class MockConverter : public mantle_api::ICoordConverter
 class MockVehicle : public mantle_api::IVehicle
 {
   public:
-    mantle_api::UniqueId GetUniqueId() const override { return 0; }
+    MOCK_METHOD(mantle_api::UniqueId, GetUniqueId, (), (const, override));
 
     void SetName(const std::string& name) override { name_ = name; }
     const std::string& GetName() const override { return name_; }
@@ -98,10 +98,13 @@ class MockQueryService : public mantle_api::ILaneLocationQueryService
         return {};
     }
 
-    Vec3d GetUpwardsShiftedLanePosition(const Vec3d& position, double upwards_shift) const override
+    Vec3d GetUpwardsShiftedLanePosition(const Vec3d& 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();
     }
     bool IsPositionOnLane(const Vec3d& position) const override
@@ -304,13 +307,13 @@ class MockEnvironment : public mantle_api::IEnvironment
 
     MOCK_METHOD(void,
                 UpdateControlStrategies,
-                (std::uint64_t controller_id,
+                (std::uint64_t entity_id,
                  std::vector<std::unique_ptr<mantle_api::ControlStrategy>>& control_strategies),
                 (override));
 
     MOCK_METHOD(bool,
                 HasControlStrategyGoalBeenReached,
-                (std::uint64_t controller_id, mantle_api::ControlStrategyType type),
+                (std::uint64_t entity_id, mantle_api::ControlStrategyType type),
                 (const, override));
 
     const mantle_api::ILaneLocationQueryService& GetQueryService() const override { return query_service_; }