Working on pathfinding, testing the Q16.16 format

src/CharacterState.cpp

@@ -93,8 +93,7 @@ void CharacterState::setTarget(
     const Eigen::Vector3f& target,
     std::string callbackName,
     const LocationNetworkState& location,
-    const std::string& entityId,
-    const std::vector<PathFinder::DynamicObstacle>& dynamicObstacles)
+    const std::string& entityId)
 {
     const Eigen::Vector3f normalizedTarget(target.x(), 0.f, target.z());
     const bool sameRequestedTarget =
@@ -115,10 +114,9 @@ void CharacterState::setTarget(
     movementBlockedTicks = 0;
     navigationRetryTicks = 0;
     blockingCharacterId.clear();
-    pathWaypoints = location.navigationMaps.empty()
-        ? std::vector<Eigen::Vector3f>{}
-        : location.planPathExceptMyself(
-            position, normalizedTarget, entityId, dynamicObstacles);
+    
+    pathWaypoints = location.navigationMaps[location.activeNavigationIndex].findPathToNearest(this->position, target);
+
     currentWaypointIndex = 0;
     if (!pathWaypoints.empty()) {
         for (Eigen::Vector3f& waypoint : pathWaypoints) waypoint.y() = 0.f;
@@ -132,15 +130,13 @@ void CharacterState::setTarget(
 
 void CharacterState::forceReplan(
     const LocationNetworkState& location,
-    const std::string& entityId,
-    const std::vector<PathFinder::DynamicObstacle>& dynamicObstacles)
+    const std::string& entityId)
 {
     const Eigen::Vector3f normalizedTarget(
         requestedWalkTarget.x(), 0.f, requestedWalkTarget.z());
-    pathWaypoints = location.navigationMaps.empty()
-        ? std::vector<Eigen::Vector3f>{}
-        : location.planPathExceptMyself(
-            position, normalizedTarget, entityId, dynamicObstacles);
+    
+    pathWaypoints = location.navigationMaps[location.activeNavigationIndex].findPathToNearest(this->position, normalizedTarget);
+
     currentWaypointIndex = 0;
     movementBlockedTicks = 0;
     navigationRetryTicks = 0;