Add interface function for points and polyline transformation

MantleAPI/include/MantleAPI/Common/i_geometry_helper.h

@@ -17,6 +17,8 @@
 
 #include <MantleAPI/Common/pose.h>
 
+#include <vector>
+
 namespace mantle_api
 {
 /// Interface that provides functionality to perform geometrical calculations
@@ -36,6 +38,26 @@ public:
       const Vec3<units::length::meter_t>& global_position,
       const Orientation3<units::angle::radian_t>& local_orientation,
       const Vec3<units::length::meter_t>& local_translation) const = 0;
+
+  /// @brief Transforms world polyline positions to local coordinate system.
+  /// @param polyline_points world polyline points to be transformed
+  /// @param position local system position
+  /// @param orientation local system orientation
+  /// @return converted polyline points
+  virtual std::vector<Vec3<units::length::meter_t>> TransformPolylinePointsFromWorldToLocal(
+      const std::vector<Vec3<units::length::meter_t>>& polyline_points,
+      const Vec3<units::length::meter_t>& position,
+      const Orientation3<units::angle::radian_t>& orientation) const = 0;
+
+  /// @brief Transforms world position to local coordinate system.
+  /// @param point_position world point position to be transformed
+  /// @param position local system position
+  /// @param orientation local system orientation
+  /// @return transformed point
+  virtual Vec3<units::length::meter_t> TransformPointPositionFromWorldToLocal(
+      const Vec3<units::length::meter_t>& point_position,
+      const Vec3<units::length::meter_t>& position,
+      const Orientation3<units::angle::radian_t>& orientation) const = 0;
 };
 
 }  // namespace mantle_api

MantleAPI/test/MantleAPI/Test/test_utils.h

@@ -43,7 +43,29 @@ public:
     std::ignore = local_orientation;
     std::ignore = local_translation;
     return global_position;
-  };
+  }
+
+  virtual std::vector<Vec3<units::length::meter_t>> TransformPolylinePointsFromWorldToLocal(
+      const std::vector<Vec3<units::length::meter_t>>& polyline_points,
+      const Vec3<units::length::meter_t>& position,
+      const Orientation3<units::angle::radian_t>& orientation) const
+  {
+    // do not translate but return original points
+    std::ignore = position;
+    std::ignore = orientation;
+    return polyline_points;
+  }
+
+  virtual Vec3<units::length::meter_t> TransformPointPositionFromWorldToLocal(
+      const Vec3<units::length::meter_t>& point_position,
+      const Vec3<units::length::meter_t>& position,
+      const Orientation3<units::angle::radian_t>& orientation) const
+  {
+    // do not translate but return original points
+    std::ignore = position;
+    std::ignore = orientation;
+    return point_position;
+  }
 };
 
 class MockConverter : public mantle_api::ICoordConverter