diff options
Diffstat (limited to 'modules/navigation/nav_agent.cpp')
| -rw-r--r-- | modules/navigation/nav_agent.cpp | 290 |
1 files changed, 282 insertions, 8 deletions
diff --git a/modules/navigation/nav_agent.cpp b/modules/navigation/nav_agent.cpp index 293544c0a5..a0efe4c74c 100644 --- a/modules/navigation/nav_agent.cpp +++ b/modules/navigation/nav_agent.cpp @@ -32,8 +32,66 @@ #include "nav_map.h" +NavAgent::NavAgent() { +} + +void NavAgent::set_avoidance_enabled(bool p_enabled) { + avoidance_enabled = p_enabled; + _update_rvo_agent_properties(); +} + +void NavAgent::set_use_3d_avoidance(bool p_enabled) { + use_3d_avoidance = p_enabled; + _update_rvo_agent_properties(); +} + +void NavAgent::_update_rvo_agent_properties() { + if (use_3d_avoidance) { + rvo_agent_3d.neighborDist_ = neighbor_distance; + rvo_agent_3d.maxNeighbors_ = max_neighbors; + rvo_agent_3d.timeHorizon_ = time_horizon_agents; + rvo_agent_3d.timeHorizonObst_ = time_horizon_obstacles; + rvo_agent_3d.radius_ = radius; + rvo_agent_3d.maxSpeed_ = max_speed; + rvo_agent_3d.position_ = RVO3D::Vector3(position.x, position.y, position.z); + // Replacing the internal velocity directly causes major jitter / bugs due to unpredictable velocity jumps, left line here for testing. + //rvo_agent_3d.velocity_ = RVO3D::Vector3(velocity.x, velocity.y ,velocity.z); + rvo_agent_3d.prefVelocity_ = RVO3D::Vector3(velocity.x, velocity.y, velocity.z); + rvo_agent_3d.height_ = height; + rvo_agent_3d.avoidance_layers_ = avoidance_layers; + rvo_agent_3d.avoidance_mask_ = avoidance_mask; + rvo_agent_3d.avoidance_priority_ = avoidance_priority; + } else { + rvo_agent_2d.neighborDist_ = neighbor_distance; + rvo_agent_2d.maxNeighbors_ = max_neighbors; + rvo_agent_2d.timeHorizon_ = time_horizon_agents; + rvo_agent_2d.timeHorizonObst_ = time_horizon_obstacles; + rvo_agent_2d.radius_ = radius; + rvo_agent_2d.maxSpeed_ = max_speed; + rvo_agent_2d.position_ = RVO2D::Vector2(position.x, position.z); + rvo_agent_2d.elevation_ = position.y; + // Replacing the internal velocity directly causes major jitter / bugs due to unpredictable velocity jumps, left line here for testing. + //rvo_agent_2d.velocity_ = RVO2D::Vector2(velocity.x, velocity.z); + rvo_agent_2d.prefVelocity_ = RVO2D::Vector2(velocity.x, velocity.z); + rvo_agent_2d.height_ = height; + rvo_agent_2d.avoidance_layers_ = avoidance_layers; + rvo_agent_2d.avoidance_mask_ = avoidance_mask; + rvo_agent_2d.avoidance_priority_ = avoidance_priority; + } + + if (map != nullptr) { + if (avoidance_enabled) { + map->set_agent_as_controlled(this); + } else { + map->remove_agent_as_controlled(this); + } + } + agent_dirty = true; +} + void NavAgent::set_map(NavMap *p_map) { map = p_map; + agent_dirty = true; } bool NavAgent::is_map_changed() { @@ -46,25 +104,241 @@ bool NavAgent::is_map_changed() { } } -void NavAgent::set_callback(Callable p_callback) { - callback = p_callback; +void NavAgent::set_avoidance_callback(Callable p_callback) { + avoidance_callback = p_callback; } -bool NavAgent::has_callback() const { - return callback.is_valid(); +bool NavAgent::has_avoidance_callback() const { + return avoidance_callback.is_valid(); } -void NavAgent::dispatch_callback() { - if (!callback.is_valid()) { +void NavAgent::dispatch_avoidance_callback() { + if (!avoidance_callback.is_valid()) { return; } - Vector3 new_velocity = Vector3(agent.newVelocity_.x(), agent.newVelocity_.y(), agent.newVelocity_.z()); + Vector3 new_velocity; + + if (use_3d_avoidance) { + new_velocity = Vector3(rvo_agent_3d.velocity_.x(), rvo_agent_3d.velocity_.y(), rvo_agent_3d.velocity_.z()); + } else { + new_velocity = Vector3(rvo_agent_2d.velocity_.x(), 0.0, rvo_agent_2d.velocity_.y()); + } + + if (clamp_speed) { + new_velocity = new_velocity.limit_length(max_speed); + } // Invoke the callback with the new velocity. Variant args[] = { new_velocity }; const Variant *args_p[] = { &args[0] }; Variant return_value; Callable::CallError call_error; - callback.callp(args_p, 1, return_value, call_error); + + avoidance_callback.callp(args_p, 1, return_value, call_error); +} + +void NavAgent::set_neighbor_distance(real_t p_neighbor_distance) { + neighbor_distance = p_neighbor_distance; + if (use_3d_avoidance) { + rvo_agent_3d.neighborDist_ = neighbor_distance; + } else { + rvo_agent_2d.neighborDist_ = neighbor_distance; + } + agent_dirty = true; +} + +void NavAgent::set_max_neighbors(int p_max_neighbors) { + max_neighbors = p_max_neighbors; + if (use_3d_avoidance) { + rvo_agent_3d.maxNeighbors_ = max_neighbors; + } else { + rvo_agent_2d.maxNeighbors_ = max_neighbors; + } + agent_dirty = true; +} + +void NavAgent::set_time_horizon_agents(real_t p_time_horizon) { + time_horizon_agents = p_time_horizon; + if (use_3d_avoidance) { + rvo_agent_3d.timeHorizon_ = time_horizon_agents; + } else { + rvo_agent_2d.timeHorizon_ = time_horizon_agents; + } + agent_dirty = true; +} + +void NavAgent::set_time_horizon_obstacles(real_t p_time_horizon) { + time_horizon_obstacles = p_time_horizon; + if (use_3d_avoidance) { + rvo_agent_3d.timeHorizonObst_ = time_horizon_obstacles; + } else { + rvo_agent_2d.timeHorizonObst_ = time_horizon_obstacles; + } + agent_dirty = true; +} + +void NavAgent::set_radius(real_t p_radius) { + radius = p_radius; + if (use_3d_avoidance) { + rvo_agent_3d.radius_ = radius; + } else { + rvo_agent_2d.radius_ = radius; + } + agent_dirty = true; +} + +void NavAgent::set_height(real_t p_height) { + height = p_height; + if (use_3d_avoidance) { + rvo_agent_3d.height_ = height; + } else { + rvo_agent_2d.height_ = height; + } + agent_dirty = true; +} + +void NavAgent::set_max_speed(real_t p_max_speed) { + max_speed = p_max_speed; + if (avoidance_enabled) { + if (use_3d_avoidance) { + rvo_agent_3d.maxSpeed_ = max_speed; + } else { + rvo_agent_2d.maxSpeed_ = max_speed; + } + } + agent_dirty = true; +} + +void NavAgent::set_position(const Vector3 p_position) { + position = p_position; + if (avoidance_enabled) { + if (use_3d_avoidance) { + rvo_agent_3d.position_ = RVO3D::Vector3(p_position.x, p_position.y, p_position.z); + } else { + rvo_agent_2d.elevation_ = p_position.y; + rvo_agent_2d.position_ = RVO2D::Vector2(p_position.x, p_position.z); + } + } + agent_dirty = true; +} + +void NavAgent::set_target_position(const Vector3 p_target_position) { + target_position = p_target_position; +} + +void NavAgent::set_velocity(const Vector3 p_velocity) { + // Sets the "wanted" velocity for an agent as a suggestion + // This velocity is not guaranteed, RVO simulation will only try to fulfill it + velocity = p_velocity; + if (avoidance_enabled) { + if (use_3d_avoidance) { + rvo_agent_3d.prefVelocity_ = RVO3D::Vector3(velocity.x, velocity.y, velocity.z); + } else { + rvo_agent_2d.prefVelocity_ = RVO2D::Vector2(velocity.x, velocity.z); + } + } + agent_dirty = true; +} + +void NavAgent::set_velocity_forced(const Vector3 p_velocity) { + // This function replaces the internal rvo simulation velocity + // should only be used after the agent was teleported + // as it destroys consistency in movement in cramped situations + // use velocity instead to update with a safer "suggestion" + velocity_forced = p_velocity; + if (avoidance_enabled) { + if (use_3d_avoidance) { + rvo_agent_3d.velocity_ = RVO3D::Vector3(p_velocity.x, p_velocity.y, p_velocity.z); + } else { + rvo_agent_2d.velocity_ = RVO2D::Vector2(p_velocity.x, p_velocity.z); + } + } + agent_dirty = true; +} + +void NavAgent::update() { + // Updates this agent with the calculated results from the rvo simulation + if (avoidance_enabled) { + if (use_3d_avoidance) { + velocity = Vector3(rvo_agent_3d.velocity_.x(), rvo_agent_3d.velocity_.y(), rvo_agent_3d.velocity_.z()); + } else { + velocity = Vector3(rvo_agent_2d.velocity_.x(), 0.0, rvo_agent_2d.velocity_.y()); + } + } +} + +void NavAgent::set_avoidance_mask(uint32_t p_mask) { + avoidance_mask = p_mask; + if (use_3d_avoidance) { + rvo_agent_3d.avoidance_mask_ = avoidance_mask; + } else { + rvo_agent_2d.avoidance_mask_ = avoidance_mask; + } + agent_dirty = true; +} + +void NavAgent::set_avoidance_layers(uint32_t p_layers) { + avoidance_layers = p_layers; + if (use_3d_avoidance) { + rvo_agent_3d.avoidance_layers_ = avoidance_layers; + } else { + rvo_agent_2d.avoidance_layers_ = avoidance_layers; + } + agent_dirty = true; +} + +void NavAgent::set_avoidance_priority(real_t p_priority) { + ERR_FAIL_COND_MSG(p_priority < 0.0, "Avoidance priority must be between 0.0 and 1.0 inclusive."); + ERR_FAIL_COND_MSG(p_priority > 1.0, "Avoidance priority must be between 0.0 and 1.0 inclusive."); + avoidance_priority = p_priority; + if (use_3d_avoidance) { + rvo_agent_3d.avoidance_priority_ = avoidance_priority; + } else { + rvo_agent_2d.avoidance_priority_ = avoidance_priority; + } + agent_dirty = true; +}; + +bool NavAgent::check_dirty() { + const bool was_dirty = agent_dirty; + agent_dirty = false; + return was_dirty; +} + +const Dictionary NavAgent::get_avoidance_data() const { + // Returns debug data from RVO simulation internals of this agent. + Dictionary _avoidance_data; + if (use_3d_avoidance) { + _avoidance_data["max_neighbors"] = int(rvo_agent_3d.maxNeighbors_); + _avoidance_data["max_speed"] = float(rvo_agent_3d.maxSpeed_); + _avoidance_data["neighbor_distance"] = float(rvo_agent_3d.neighborDist_); + _avoidance_data["new_velocity"] = Vector3(rvo_agent_3d.newVelocity_.x(), rvo_agent_3d.newVelocity_.y(), rvo_agent_3d.newVelocity_.z()); + _avoidance_data["velocity"] = Vector3(rvo_agent_3d.velocity_.x(), rvo_agent_3d.velocity_.y(), rvo_agent_3d.velocity_.z()); + _avoidance_data["position"] = Vector3(rvo_agent_3d.position_.x(), rvo_agent_3d.position_.y(), rvo_agent_3d.position_.z()); + _avoidance_data["prefered_velocity"] = Vector3(rvo_agent_3d.prefVelocity_.x(), rvo_agent_3d.prefVelocity_.y(), rvo_agent_3d.prefVelocity_.z()); + _avoidance_data["radius"] = float(rvo_agent_3d.radius_); + _avoidance_data["time_horizon_agents"] = float(rvo_agent_3d.timeHorizon_); + _avoidance_data["time_horizon_obstacles"] = 0.0; + _avoidance_data["height"] = float(rvo_agent_3d.height_); + _avoidance_data["avoidance_layers"] = int(rvo_agent_3d.avoidance_layers_); + _avoidance_data["avoidance_mask"] = int(rvo_agent_3d.avoidance_mask_); + _avoidance_data["avoidance_priority"] = float(rvo_agent_3d.avoidance_priority_); + } else { + _avoidance_data["max_neighbors"] = int(rvo_agent_2d.maxNeighbors_); + _avoidance_data["max_speed"] = float(rvo_agent_2d.maxSpeed_); + _avoidance_data["neighbor_distance"] = float(rvo_agent_2d.neighborDist_); + _avoidance_data["new_velocity"] = Vector3(rvo_agent_2d.newVelocity_.x(), 0.0, rvo_agent_2d.newVelocity_.y()); + _avoidance_data["velocity"] = Vector3(rvo_agent_2d.velocity_.x(), 0.0, rvo_agent_2d.velocity_.y()); + _avoidance_data["position"] = Vector3(rvo_agent_2d.position_.x(), 0.0, rvo_agent_2d.position_.y()); + _avoidance_data["prefered_velocity"] = Vector3(rvo_agent_2d.prefVelocity_.x(), 0.0, rvo_agent_2d.prefVelocity_.y()); + _avoidance_data["radius"] = float(rvo_agent_2d.radius_); + _avoidance_data["time_horizon_agents"] = float(rvo_agent_2d.timeHorizon_); + _avoidance_data["time_horizon_obstacles"] = float(rvo_agent_2d.timeHorizonObst_); + _avoidance_data["height"] = float(rvo_agent_2d.height_); + _avoidance_data["avoidance_layers"] = int(rvo_agent_2d.avoidance_layers_); + _avoidance_data["avoidance_mask"] = int(rvo_agent_2d.avoidance_mask_); + _avoidance_data["avoidance_priority"] = float(rvo_agent_2d.avoidance_priority_); + } + return _avoidance_data; } |