diff --git a/docs/example/fig/fmt_star.png b/docs/example/fig/fmt_star.png
index 686da8c2..d5416815 100644
Binary files a/docs/example/fig/fmt_star.png and b/docs/example/fig/fmt_star.png differ
diff --git a/docs/example/fig/navigation_diffbot.gif b/docs/example/fig/navigation_diffbot.gif
index e9719fd6..a75d7344 100644
Binary files a/docs/example/fig/navigation_diffbot.gif and b/docs/example/fig/navigation_diffbot.gif differ
diff --git a/example/path_planning/fmt_star.cpp b/example/path_planning/fmt_star.cpp
index 6e921159..cd3cbef9 100644
--- a/example/path_planning/fmt_star.cpp
+++ b/example/path_planning/fmt_star.cpp
@@ -44,29 +44,32 @@ int main() {
         {8, 1},
     };
     double obstacle_size = 0.5;
-    GridMap2d map(obstalces, Eigen::Vector2i(10, 10), obstacle_size);
+    double margin = 0.15;
+    GridMap2d map(obstalces, Eigen::Vector2i(10, 10), obstacle_size, margin);
 
     Eigen::Vector2d x_init(0.25, 0.25);
     Eigen::Vector2d x_goal(4.75, 4.75);
-    auto path = fmt_star<Eigen::Vector2d>(map, x_init, x_goal);
+    FMTStarConfig config;
+    config.sampling_num = 1000;
+    FMTStar<Eigen::Vector2d> fmt_star(map, config);
+    auto path = fmt_star.solve(x_init, x_goal);
 
     namespace plt = matplotlibcpp;
-    // result
-    if(path.size() == 0)
-    {
-        std::cout << "No path found" << std::endl;
-    }
-    else
+
+    // sampling points
     {
+        auto nodes = fmt_star.get_nodes();
         std::vector<double> x, y;
-        for(const auto& p : path)
+        for(const auto& p : nodes)
         {
-            x.push_back(p(0));
-            y.push_back(p(1));
+            x.push_back(p.point(0));
+            y.push_back(p.point(1));
         }
-        plt::plot(x, y, "b");
-        plt::plot({x_init(0)}, {x_init(1)}, "go");
-        plt::plot({x_goal(0)}, {x_goal(1)}, "ro");
+        std::map<std::string, std::string> kwargs;
+        kwargs["c"] = "gray";
+        kwargs["marker"] = ".";
+        kwargs["linestyle"] = "none";
+        plt::plot(x, y, kwargs);
     }
 
     // obstacles
@@ -87,6 +90,24 @@ int main() {
         plt::fill(x, y, std::map<std::string, std::string>{{"c", "k"}});
     }
 
+    // result
+    if(path.size() == 0)
+    {
+        std::cout << "No path found" << std::endl;
+    }
+    else
+    {
+        std::vector<double> x, y;
+        for(const auto& p : path)
+        {
+            x.push_back(p(0));
+            y.push_back(p(1));
+        }
+        plt::plot(x, y, "b");
+        plt::plot({x_init(0)}, {x_init(1)}, "go");
+        plt::plot({x_goal(0)}, {x_goal(1)}, "ro");
+    }
+
     plt::xlim(0, 5);
     plt::ylim(0, 5);
     plt::set_aspect_equal();
diff --git a/example/path_planning/navigation_diffbot.cpp b/example/path_planning/navigation_diffbot.cpp
index 882476e9..5baaa817 100644
--- a/example/path_planning/navigation_diffbot.cpp
+++ b/example/path_planning/navigation_diffbot.cpp
@@ -41,7 +41,7 @@ void draw_robot(Eigen::Vector3d state, double width = 0.4, double height = 0.4,
     double y4 = y + sin(theta) * (-width / 2) + cos(theta) * (height / 2);
     std::vector<double> x_data = {x1, x2, x3, x4, x1};
     std::vector<double> y_data = {y1, y2, y3, y4, y1};
-    plt::plot(x_data, y_data, "b-");
+    plt::plot(x_data, y_data, "k-");
 
     std::vector<double> dir_x_data = {x, x + cos(theta) * heading};
     std::vector<double> dir_y_data = {y, y + sin(theta) * heading};
@@ -91,11 +91,14 @@ int main() {
         {8, 1},
     };
     double obstacle_size = 0.5;
-    GridMap2d map(obstalces, Eigen::Vector2i(10, 10), obstacle_size);
+    double margin = 0.2;
+    GridMap2d global_planning_map(obstalces, Eigen::Vector2i(10, 10), obstacle_size, margin);
 
     Eigen::Vector2d x_init(0.25, 0.25);
     Eigen::Vector2d x_goal(4.75, 4.75);
-    auto waypoint_list = fmt_star<Eigen::Vector2d>(map, x_init, x_goal);
+    FMTStarConfig fmt_star_config;
+    fmt_star_config.sampling_num = 2000;
+    auto waypoint_list = fmt_star<Eigen::Vector2d>(global_planning_map, x_init, x_goal, fmt_star_config);
 
     if(waypoint_list.size() == 0)
     {
@@ -111,17 +114,18 @@ int main() {
 
     LinePath path(waypoint_list);
 
-    DWAConfig config;
-    config.robot_radius = 0.5;
-    config.max_velocity = 3.0;
-    config.max_angular_velocity = 3.0;
-    config.windows_size = 100;
-    config.dt = 0.1;
-    config.predict_time = 0.5;
-    config.heading_angle_weight = 3.0;
-    config.heading_velocity_weight = 0.5;
-    config.to_goal_weight = 3.0;
-    DWA dwa(map, config);
+    GridMap2d local_planning_map(obstalces, Eigen::Vector2i(10, 10), obstacle_size);
+    DWAConfig dwa_config;
+    dwa_config.robot_radius = 0.5;
+    dwa_config.max_velocity = 3.0;
+    dwa_config.max_angular_velocity = 3.0;
+    dwa_config.windows_size = 100;
+    dwa_config.dt = 0.1;
+    dwa_config.predict_time = 0.5;
+    dwa_config.heading_angle_weight = 3.0;
+    dwa_config.heading_velocity_weight = 0.5;
+    dwa_config.to_goal_weight = 3.0;
+    DWA dwa(local_planning_map, dwa_config);
 
     const double sim_dt = 0.1;
     DiffBot robot(sim_dt);
@@ -147,11 +151,11 @@ int main() {
         // シミュレーション
         x = robot.eval(x, u);
 
-        draw_robot(x);
         plt::plot(waypoint_list_x, waypoint_list_y, "b");
         plt::plot({x_init(0)}, {x_init(1)}, "go");
         plt::plot({x_goal(0)}, {x_goal(1)}, "ro");
         plt::plot({dwa_goal(0)}, {dwa_goal(1)}, "bo");
+        draw_robot(x);
 
         // obstacles
         for(auto o : obstalces)
diff --git a/include/cpp_robotics/algorithm/kdtree.hpp b/include/cpp_robotics/algorithm/kdtree.hpp
index 68a99d49..4c435648 100644
--- a/include/cpp_robotics/algorithm/kdtree.hpp
+++ b/include/cpp_robotics/algorithm/kdtree.hpp
@@ -197,8 +197,8 @@ class KDTree
             return;
         
         auto p = points_[node->idx];
-        double dist = distance(p, point);
-        if(dist < radius)
+        double sq_dist = squared_distance(p, point);
+        if(sq_dist < radius*radius)
         {
             result.push_back(node->idx);
         }
@@ -225,14 +225,19 @@ class KDTree
         debug_node_recursive(node->child[1], depth+1);
     }
 
-    double distance(const point_type &a, const point_type &b) const
+    double squared_distance(const point_type &a, const point_type &b) const
     {
         double len_sq = 0;
         for(size_t i = 0; i < dimention_; i++)
         {
             len_sq += std::pow(a[i]-b[i], 2);
         }
-        return std::sqrt(len_sq);
+        return len_sq;
+    }
+
+    double distance(const point_type &a, const point_type &b) const
+    {
+        return std::sqrt(squared_distance(a, b));
     }
 
     size_t dimention_;
diff --git a/include/cpp_robotics/path_planning/path_planning_utils.hpp b/include/cpp_robotics/path_planning/path_planning_utils.hpp
index 6d2d7364..a62be89d 100644
--- a/include/cpp_robotics/path_planning/path_planning_utils.hpp
+++ b/include/cpp_robotics/path_planning/path_planning_utils.hpp
@@ -2,6 +2,8 @@
 
 #include <Eigen/Dense>
 #include "cpp_robotics/utility/random.hpp"
+#include "cpp_robotics/algorithm/kdtree.hpp"
+#include <unordered_set>
 
 namespace cpp_robotics
 {
@@ -63,25 +65,61 @@ class GridMap : public PathPlanningMap<Eigen::Matrix<double, Dim, 1>>
 
     template <int N = Vector::RowsAtCompileTime,
               typename std::enable_if<N != Eigen::Dynamic, int>::type = 0>
-    GridMap(std::vector<GridVector> obstacles, GridVector map_size, double resolution)
-        : PathPlanningMap<Vector>(), obstacles_(obstacles), map_size_(map_size), resolution_(resolution) 
+    GridMap(std::vector<GridVector> obstacles, GridVector map_size, double resolution, double margin = 0.0)
+        : PathPlanningMap<Vector>(), map_size_(map_size), resolution_(resolution), margin_(margin)
     {
+        for(const auto& o : obstacles)
+        {
+            obstacles_.insert(o);
+        }
+        if(0.0 < margin_)
+        {
+            kdtree_.build(obstacles, this->dimension());
+        }
     }
 
     template <int N = Vector::RowsAtCompileTime,
               typename std::enable_if<N == Eigen::Dynamic, int>::type = 0>
-    GridMap(size_t dim, std::vector<GridVector> obstacles, GridVector map_size, double resolution)
-        : PathPlanningMap<Vector>(dim), obstacles_(obstacles), map_size_(map_size), resolution_(resolution) 
+    GridMap(size_t dim, std::vector<GridVector> obstacles, GridVector map_size, double resolution, double margin = 0.0)
+        : PathPlanningMap<Vector>(dim), map_size_(map_size), resolution_(resolution), margin_(margin)
     {
+        for(const auto& o : obstacles)
+        {
+            obstacles_.insert(o);
+        }
+        if(0.0 < margin_)
+        {
+            kdtree_.build(obstacles, this->dimension());
+        }
     }
 
     bool is_valid(const Vector &point) const override
     {
         GridVector gp = (point/resolution_).template cast<int>();
-        if(auto it = std::find(obstacles_.begin(), obstacles_.end(), gp); it != obstacles_.end())
+        if(obstacles_.find(gp) != obstacles_.end())
         {
             return false;
         }
+        if(0.0 < margin_)
+        {
+            Vector min_diff;
+            if constexpr( Dim == Eigen::Dynamic )
+            {
+                min_diff.resize(this->dimension());
+            }
+            auto near_points = kdtree_.radius_search_points(gp, margin_/resolution_ + 2);
+            for(auto& ngp : near_points)
+            {
+                for(size_t i = 0; i < this->dimension(); i++)
+                {
+                    min_diff(i) = std::max({ngp(i)*resolution_ - point(i), point(i) - (ngp(i)+1)*resolution_, 0.0});
+                }
+                if(min_diff.squaredNorm() < margin_*margin_)
+                {
+                    return false;
+                }
+            }
+        }
         return true;
     }
 
@@ -98,21 +136,36 @@ class GridMap : public PathPlanningMap<Eigen::Matrix<double, Dim, 1>>
             return false;
         }
 
-        GridVector look = gp1;
-        Eigen::VectorXd look_start = (from/resolution_);
-        Eigen::VectorXd diff = (to/resolution_) - look_start;
-        for(size_t i = 0; i < 100; i++) {
-            GridVector next = (look_start + i*diff/100.0).template cast<int>();
-            if(next == gp2) {
-                break;
-            }
-            if(next != look) {
-                if(auto it = std::find(obstacles_.begin(), obstacles_.end(), next); it != obstacles_.end()) {
+        const size_t split = 30;
+        if (0.0 < margin_)
+        {
+            Eigen::VectorXd diff = to - from;
+            for(size_t i = 0; i < split; i++) {
+                Vector nextd = (from + i*diff/split);
+                if(not is_valid(nextd)) {
                     return false;
                 }
-                look = next;
             }
         }
+        else
+        {
+            GridVector look = gp1;
+            Eigen::VectorXd diff = to - from;
+            for(size_t i = 0; i < split; i++) {
+                Vector nextd = (from + i*diff/split);
+                GridVector next = (nextd/resolution_).template cast<int>();
+                if(next == gp2) {
+                    break;
+                }
+                if(next != look) {
+                    if(not is_valid(nextd)) {
+                        return false;
+                    }
+                    look = next;
+                }
+            }
+        }
+
         return true;
     }
 
@@ -123,8 +176,15 @@ class GridMap : public PathPlanningMap<Eigen::Matrix<double, Dim, 1>>
         {
             p.resize(this->dimension());
         }
-        for (size_t i = 0; i < this->dimension(); i++) {
-            p(i) = rand_engine_.value() * map_size_(i)*resolution_;
+        for(size_t r = 0; r < 1000; r++)
+        {
+            for (size_t i = 0; i < this->dimension(); i++) {
+                p(i) = rand_engine_.value() * map_size_(i)*resolution_;
+            }
+            if(is_valid(p))
+            {
+                return p;
+            }
         }
         return p;
     }
@@ -135,9 +195,26 @@ class GridMap : public PathPlanningMap<Eigen::Matrix<double, Dim, 1>>
     }
 
 private:
-    std::vector<GridVector> obstacles_;
+    struct VectorHash {
+        size_t operator()(const GridVector& v) const {
+            size_t seed = 0;
+            for (int i = 0; i < v.size(); ++i) {
+                seed ^= std::hash<int>()(v[i]) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
+            }
+            return seed;
+        }
+    };
+
+    struct VectorEqual {
+        bool operator()(const GridVector& v1, const GridVector& v2) const {
+            return v1 == v2;
+        }
+    };
+    std::unordered_set<GridVector, VectorHash, VectorEqual> obstacles_;
+    KDTree<GridVector> kdtree_;
     GridVector map_size_;
     double resolution_;
+    double margin_;
     UniformRealRandomEngine rand_engine_ = {0.0, 1.0};
 };