diff options
Diffstat (limited to 'modules/navigation/nav_map.cpp')
| -rw-r--r-- | modules/navigation/nav_map.cpp | 13 |
1 files changed, 12 insertions, 1 deletions
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index ca1034dcc0..7a0799a735 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -1107,8 +1107,14 @@ void NavMap::_update_rvo_obstacles_tree_2d() { obstacle_vertex_count += obstacle->get_vertices().size(); } + // Cleaning old obstacles. + for (size_t i = 0; i < rvo_simulation_2d.obstacles_.size(); ++i) { + delete rvo_simulation_2d.obstacles_[i]; + } + rvo_simulation_2d.obstacles_.clear(); + // Cannot use LocalVector here as RVO library expects std::vector to build KdTree - std::vector<RVO2D::Obstacle2D *> raw_obstacles; + std::vector<RVO2D::Obstacle2D *> &raw_obstacles = rvo_simulation_2d.obstacles_; raw_obstacles.reserve(obstacle_vertex_count); // The following block is modified copy from RVO2D::AddObstacle() @@ -1128,6 +1134,11 @@ void NavMap::_update_rvo_obstacles_tree_2d() { real_t _obstacle_height = obstacle->get_height(); for (const Vector3 &_obstacle_vertex : _obstacle_vertices) { +#ifdef TOOLS_ENABLED + if (_obstacle_vertex.y != 0) { + WARN_PRINT_ONCE("Y coordinates of static obstacle vertices are ignored. Please use obstacle position Y to change elevation of obstacle."); + } +#endif rvo_2d_vertices.push_back(RVO2D::Vector2(_obstacle_vertex.x + _obstacle_position.x, _obstacle_vertex.z + _obstacle_position.z)); } |