bye, wykobi!

source/Navigation/libraries/core/CMakeLists.txt

@@ -3,8 +3,6 @@ set(LIB_NAME ${PROJECT_NAME}Core)
 armarx_component_set_name("${LIB_NAME}")
 armarx_set_target("Library: ${LIB_NAME}")
 
-find_package(wykobi REQUIRED)
-
 armarx_add_library(
     LIBS
         ArmarXCoreInterfaces
@@ -12,7 +10,6 @@ armarx_add_library(
         # RobotAPI 
         aron 
         RobotAPIInterfaces
-        wykobi::wykobi
         VirtualRobot
     SOURCES
         StaticScene.cpp

source/Navigation/libraries/core/Trajectory.cpp

@@ -9,8 +9,7 @@
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
-#include <wykobi.hpp>
-
+#include <VirtualRobot/MathTools.h>
 #include <VirtualRobot/math/Helpers.h>
 #include <VirtualRobot/math/LinearInterpolatedPose.h>
 
@@ -47,10 +46,7 @@ namespace armarx::nav::core
     Trajectory::getProjection(const Pose& pose,
                               const VelocityInterpolations& velocityInterpolation) const
     {
-        const auto asPt = [](const Pose& pose) -> wykobi::point2d<float>
-        { return wykobi::make_point<float>(pose.translation().x(), pose.translation().y()); };
-
-        const auto point = asPt(pose);
+        const Position point = pose.translation();
 
         float distance = std::numeric_limits<float>::max();
 
@@ -61,20 +57,19 @@ namespace armarx::nav::core
             const auto& wpBefore = points.at(i);
             const auto& wpAfter = points.at(i + 1);
 
-            auto segment =
-                wykobi::make_segment(asPt(wpBefore.waypoint.pose), asPt(wpAfter.waypoint.pose));
-
-            wykobi::point2d<float> closestPoint =
-                wykobi::closest_point_on_segment_from_point(segment, point);
+            const auto closestPoint = VirtualRobot::MathTools::nearestPointOnSegment<Position>(
+                                          wpBefore.waypoint.pose.translation(), wpAfter.waypoint.pose.translation(), point);
 
-            const float currentDistance = wykobi::distance(closestPoint, point);
+            const float currentDistance = (closestPoint - point).norm();
             if (currentDistance < distance)
             {
 
-                const float d1 = wykobi::distance(closestPoint, asPt(wpBefore.waypoint.pose));
+                const float d1 = (closestPoint - wpBefore.waypoint.pose.translation()).norm();
                 const float d =
-                    wykobi::distance(asPt(wpBefore.waypoint.pose), asPt(wpAfter.waypoint.pose));
+                    (wpBefore.waypoint.pose.translation() - wpAfter.waypoint.pose.translation())
+                    .norm();
 
+                // fraction of distance between segment end points
                 const float t = d1 / d;
 
                 math::LinearInterpolatedPose ip(
@@ -104,8 +99,10 @@ namespace armarx::nav::core
     std::vector<Eigen::Vector3f>
     Trajectory::positions() const
     {
-        const auto toPosition = [](const TrajectoryPoint& wp)
-        { return wp.waypoint.pose.translation(); };
+        const auto toPosition = [](const TrajectoryPoint & wp)
+        {
+            return wp.waypoint.pose.translation();
+        };
 
         std::vector<Eigen::Vector3f> positions;
         positions.reserve(points.size());
@@ -134,7 +131,7 @@ namespace armarx::nav::core
             return FromPath({start, goal});
         }
 
-        const auto directedPose = [](const Position& position, const Position& target) -> Pose
+        const auto directedPose = [](const Position & position, const Position & target) -> Pose
         {
             const Direction direction = target - position;