refactor: use Heuclid polygon predicates for collision detection

.gitignore

@@ -33,4 +33,8 @@
 
 # Cmake Build Folders
 build/
-test/build/
+test/build/
+
+# Demo generated files
+demo/*.csv
+demo/frames_flat/

CMakeLists.txt

@@ -38,7 +38,7 @@ if(NOT Heuclid_FOUND)
   include(FetchContent)
   FetchContent_Declare(Heuclid
     GIT_REPOSITORY "https://github.com/Mr-tooth/Heuclid.git"
-    GIT_TAG "v2.1"
+    GIT_TAG "v2.2"
     GIT_SHALLOW TRUE)
   set(BUILD_TESTING_HEUCID OFF CACHE BOOL "" FORCE)
   FetchContent_MakeAvailable(Heuclid)
@@ -137,17 +137,11 @@ if(MATPLOTLIB_CPP_AVAILABLE)
     endif()
     message(STATUS "matplotlib_cpp enabled — PlotCheck module included")
 
-    # --- Demo GIF generator ---
-    add_executable(demo_flat demo/demo_flat.cpp)
-    target_link_libraries(demo_flat PRIVATE ${PROJECT_NAME})
-    target_compile_definitions(demo_flat PRIVATE HAS_MATPLOTLIB)
-    if(matplotlib_cpp_FOUND)
-        target_include_directories(demo_flat PRIVATE ${matplotlib_cpp_INCLUDE_DIRS})
-        target_link_libraries(demo_flat PRIVATE ${matplotlib_LIBS})
-    else()
-        target_include_directories(demo_flat PRIVATE ${MATPLOTLIB_CPP_INCLUDE})
-        target_link_libraries(demo_flat PRIVATE Python3::Python Python3::NumPy)
-    endif()
+    # --- Demo GIF generator (data export only, visualization in Python) ---
+    add_executable(demo_export demo/demo_export.cpp)
+    target_link_libraries(demo_export PRIVATE ${PROJECT_NAME})
+    add_executable(demo_obstacle demo/demo_obstacle.cpp)
+    target_link_libraries(demo_obstacle PRIVATE ${PROJECT_NAME})
 else()
     message(STATUS "matplotlib_cpp not found — PlotCheck module disabled")
 endif()

README.md

@@ -44,10 +44,18 @@ This project is a C++ reimplementation of the core algorithms from the [IHMC Foo
   <img src="assets/flat_terrain.gif" alt="Flat terrain footstep planning" width="640">
 </p>
 
-> A* footstep planning from start (blue dot) to goal (red arrow). Red = left foot, orange = right foot. 19 discrete footsteps with body path (green dashed line).
+> A* footstep planning from start (blue dot) to goal (red diamond). Red = left foot, orange = right foot. 25 discrete footsteps evenly straddling the ellipsoid body path (blue line with direction arrows).
+
+### Obstacle Avoidance
+
+<p align="center">
+  <img src="assets/obstacle_avoidance.gif" alt="Obstacle avoidance footstep planning" width="640">
+</p>
+
+> Footsteps navigate around a gray obstacle block placed on the body path. The planner rejects footsteps intersecting the obstacle polygon, producing a natural detour (24 steps).
 
 - ✅ Flat terrain: start → goal footstep sequence
-- ⏳ Obstacle avoidance: navigating around forbidden regions
+- ✅ Obstacle avoidance: navigating around forbidden regions
 - ⏳ Stair climbing: constrained footstep planning on stairs
 
 ## Quick Start

README_CN.md

@@ -44,10 +44,18 @@
   <img src="assets/flat_terrain.gif" alt="平地落脚点规划" width="640">
 </p>
 
-> A* 落脚点规划：从起点（蓝点）到目标点（红箭头）。红色=左脚，橙色=右脚。19 个离散落脚点 + 身体路径（绿色虚线）。
+> A* 落脚点规划：从起点（蓝色方块）到目标点（红色菱形）。红色=左脚，橙色=右脚。25 个离散落脚点均匀分布在椭圆身体路径（蓝色曲线，带方向箭头）两侧。
+
+### 障碍物避障
+
+<p align="center">
+  <img src="assets/obstacle_avoidance.gif" alt="障碍物避落脚点规划" width="640">
+</p>
+
+> 落脚点绕过放置在身体路径上的灰色障碍物块。规划器会拒绝与障碍物多边形相交的落脚点，产生自然的绕行路径（24 步）。
 
 - ✅ 平地：起点 → 终点落脚点序列
-- ⏳ 障碍物回避：绕开禁止区域
+- ✅ 障碍物回避：绕开禁止区域
 - ⏳ 楼梯攀爬：楼梯约束下的落脚点规划
 > - 障碍物避让：绕过禁区的路径规划
 > - 楼梯场景：约束条件下的楼梯落脚点规划

demo/demo_export.cpp

@@ -0,0 +1,135 @@
+/**
+ * Demo: Flat terrain footstep planning with body path.
+ * Uses test7 tuned parameters for optimal body path following.
+ * Exports real planner data to CSV for Python visualization.
+ */
+#include <FootstepPlannerLJH/AStarFootstepPlanner.h>
+#include <FootstepPlannerLJH/parameters.h>
+#include <FootstepPlannerLJH/SimpleBodyPathPlanner/simple2DBodyPathHolder.h>
+#include <FootstepPlannerLJH/StepConstraints/StepConstraintCheck.h>
+#include <FootstepPlannerLJH/PlotCheck/FootPolygon.h>
+
+#include <iostream>
+#include <fstream>
+#include <cmath>
+
+using namespace ljh::heuclid;
+using namespace ljh::path::footstep_planner;
+
+int main()
+{
+    // === Flat terrain scenario (test7) ===
+    double startX = 0.015, startY = 0.0, startZ = 0.0, startYaw = 0.0;
+    double goalX  = 0.663, goalY  = -0.962, goalZ = 0.0, goalYaw = -1.554;
+
+    Pose2D<double> goalPose2D(goalX, goalY, goalYaw);
+    Pose3D<double> goalPose(goalX, goalY, goalZ, goalYaw, 0.0, 0.0);
+    Pose3D<double> startPose(startX, startY, startZ, startYaw, 0.0, 0.0);
+
+    // === Body path (ellipsoid) ===
+    Simple2DBodyPathHolder pathHolder;
+    pathHolder.initialize({startX, startY, startYaw}, goalPose2D);
+    auto waypoints = pathHolder.getWayPointPath();
+
+    {
+        std::ofstream fout("demo/body_path.csv");
+        fout << "idx,x,y,yaw" << std::endl;
+        for (size_t i = 0; i < waypoints.size(); i++)
+        {
+            fout << i << ","
+                 << waypoints[i].getPosition().getX() << ","
+                 << waypoints[i].getPosition().getY() << ","
+                 << waypoints[i].getOrientation().getYaw() << std::endl;
+        }
+        fout.close();
+        std::cout << "Wrote " << waypoints.size() << " body path waypoints" << std::endl;
+    }
+
+    AStarFootstepPlanner planner;
+
+    // Apply test7 tuned parameters
+    parameters param;
+    param.SetEdgeCostDistance(param, 4.0);
+    param.SetEdgeCostYaw(param, 4.0);
+    param.SetEdgeCostStaticPerStep(param, 1.4);
+    param.SetMaxStepYaw(param, pi / 12.0);
+    param.SetMinStepYaw(param, -pi / 12.0);
+    param.SetFinalTurnProximity(param, 0.3);
+    param.SetGoalDistanceProximity(param, 0.04);
+    param.SetGoalYawProximity(param, 4.0 / 180.0 * pi);
+    param.SetFootPolygonExtendedLength(param, 0.025);
+
+    // HWP weights — test7 tuned values
+    param.SetHWPOfWalkDistacne(param, 1.30);
+    // Enable body path following via heuristic
+    param.SetFollowBodyPath(param, true);
+    param.SetHWPOfPathDistance(param, 1.0);       // Body path heuristic weight
+    param.SetEdgeCostPathDev(param, 0.0);         // No edge penalty needed — heuristic handles it
+    param.SetHWPOfInitialTurnDistacne(param, 1.0);
+    param.SetHWPOfFinalTurnDistacne(param, 1.30);
+    param.SetHWPOfFinalWalkDistacne(param, 1.30);
+
+    // Step size constraints
+    param.SetMaxStepLength(param, 0.08);
+    param.SetMinStepLength(param, -0.08);
+    param.SetMaxStepWidth(param, 0.22);
+    param.SetMinStepWidth(param, 0.16);
+    param.SetMaxStepReach(param, sqrt(pow(0.22 - 0.16, 2) + 0.08 * 0.08));
+
+    // Params are static members — set once, used everywhere
+
+    planner.initialize(goalPose2D, goalPose, startPose);
+    planner.doAStarSearch();
+    planner.calFootstepSeries();
+    auto accurateSteps = planner.getOrCalAccurateFootstepSeries();
+
+    std::cout << "Accurate footsteps: " << accurateSteps.size() << std::endl;
+
+    // === Export footstep center positions ===
+    {
+        std::ofstream fout("demo/footsteps.csv");
+        fout << "step,x,y,yaw,side" << std::endl;
+        for (size_t i = 0; i < accurateSteps.size(); i++)
+        {
+            auto& s = accurateSteps[i];
+            std::string side = (s.getStepFlag() == stepL) ? "L" : "R";
+            fout << i << "," << s.getX() << "," << s.getY() << ","
+                 << s.getYaw() << "," << side << std::endl;
+        }
+        fout.close();
+        std::cout << "Wrote " << accurateSteps.size() << " footsteps" << std::endl;
+    }
+
+    // === Export foot polygon vertices ===
+    {
+        std::ofstream fout("demo/foot_polygons.csv");
+        fout << "step,vertex,x,y" << std::endl;
+        for (size_t i = 0; i < accurateSteps.size(); i++)
+        {
+            auto& s = accurateSteps[i];
+            Pose2D<double> pose;
+            pose.setPosition(s.getX(), s.getY());
+            pose.setOrientation(s.getYaw());
+
+            std::vector<double> vx, vy;
+            getFootVertex2D(pose, s.getStepFlag(), vx, vy);
+
+            for (size_t j = 0; j < vx.size(); j++)
+                fout << i << "," << j << "," << vx[j] << "," << vy[j] << std::endl;
+        }
+        fout.close();
+        std::cout << "Wrote foot polygons" << std::endl;
+    }
+
+    // === Export start/goal ===
+    {
+        std::ofstream fout("demo/start_goal.csv");
+        fout << "pose,x,y,yaw" << std::endl;
+        fout << "start," << startX << "," << startY << "," << startYaw << std::endl;
+        fout << "goal," << goalX << "," << goalY << "," << goalYaw << std::endl;
+        fout.close();
+    }
+
+    std::cout << "All data exported." << std::endl;
+    return 0;
+}