|
|
@@ -299,7 +299,6 @@ void NavigationAgent3D::_notification(int p_what) {
|
|
|
NavigationServer3D::get_singleton()->agent_set_velocity_forced(agent, velocity_forced);
|
|
|
}
|
|
|
}
|
|
|
- _check_distance_to_target();
|
|
|
}
|
|
|
#ifdef DEBUG_ENABLED
|
|
|
if (debug_path_dirty) {
|
|
|
@@ -622,7 +621,7 @@ Vector3 NavigationAgent3D::get_target_position() const {
|
|
|
}
|
|
|
|
|
|
Vector3 NavigationAgent3D::get_next_path_position() {
|
|
|
- update_navigation();
|
|
|
+ _update_navigation();
|
|
|
|
|
|
const Vector<Vector3> &navigation_path = navigation_result->get_path();
|
|
|
if (navigation_path.size() == 0) {
|
|
|
@@ -643,22 +642,30 @@ bool NavigationAgent3D::is_target_reached() const {
|
|
|
}
|
|
|
|
|
|
bool NavigationAgent3D::is_target_reachable() {
|
|
|
- return target_desired_distance >= get_final_position().distance_to(target_position);
|
|
|
+ _update_navigation();
|
|
|
+ return _is_target_reachable();
|
|
|
+}
|
|
|
+
|
|
|
+bool NavigationAgent3D::_is_target_reachable() const {
|
|
|
+ return target_desired_distance >= _get_final_position().distance_to(target_position);
|
|
|
}
|
|
|
|
|
|
bool NavigationAgent3D::is_navigation_finished() {
|
|
|
- update_navigation();
|
|
|
+ _update_navigation();
|
|
|
return navigation_finished;
|
|
|
}
|
|
|
|
|
|
Vector3 NavigationAgent3D::get_final_position() {
|
|
|
- update_navigation();
|
|
|
+ _update_navigation();
|
|
|
+ return _get_final_position();
|
|
|
+}
|
|
|
|
|
|
+Vector3 NavigationAgent3D::_get_final_position() const {
|
|
|
const Vector<Vector3> &navigation_path = navigation_result->get_path();
|
|
|
if (navigation_path.size() == 0) {
|
|
|
return Vector3();
|
|
|
}
|
|
|
- return navigation_path[navigation_path.size() - 1];
|
|
|
+ return navigation_path[navigation_path.size() - 1] - Vector3(0, path_height_offset, 0);
|
|
|
}
|
|
|
|
|
|
void NavigationAgent3D::set_velocity_forced(Vector3 p_velocity) {
|
|
|
@@ -693,7 +700,7 @@ PackedStringArray NavigationAgent3D::get_configuration_warnings() const {
|
|
|
return warnings;
|
|
|
}
|
|
|
|
|
|
-void NavigationAgent3D::update_navigation() {
|
|
|
+void NavigationAgent3D::_update_navigation() {
|
|
|
if (agent_parent == nullptr) {
|
|
|
return;
|
|
|
}
|
|
|
@@ -749,6 +756,7 @@ void NavigationAgent3D::update_navigation() {
|
|
|
debug_path_dirty = true;
|
|
|
#endif // DEBUG_ENABLED
|
|
|
navigation_finished = false;
|
|
|
+ last_waypoint_reached = false;
|
|
|
navigation_path_index = 0;
|
|
|
emit_signal(SNAME("path_changed"));
|
|
|
}
|
|
|
@@ -757,103 +765,147 @@ void NavigationAgent3D::update_navigation() {
|
|
|
return;
|
|
|
}
|
|
|
|
|
|
- // Check if we can advance the navigation path
|
|
|
- if (navigation_finished == false) {
|
|
|
- // Advances to the next far away position.
|
|
|
- const Vector<Vector3> &navigation_path = navigation_result->get_path();
|
|
|
- const Vector<int32_t> &navigation_path_types = navigation_result->get_path_types();
|
|
|
- const TypedArray<RID> &navigation_path_rids = navigation_result->get_path_rids();
|
|
|
- const Vector<int64_t> &navigation_path_owners = navigation_result->get_path_owner_ids();
|
|
|
+ // Check if the navigation has already finished.
|
|
|
+ if (navigation_finished) {
|
|
|
+ return;
|
|
|
+ }
|
|
|
|
|
|
- while (origin.distance_to(navigation_path[navigation_path_index] - Vector3(0, path_height_offset, 0)) < path_desired_distance) {
|
|
|
- Dictionary details;
|
|
|
+ // Check if we reached the target.
|
|
|
+ if (_is_within_target_distance(origin)) {
|
|
|
+ // Emit waypoint_reached in case we also moved within distance of a waypoint.
|
|
|
+ _advance_waypoints(origin);
|
|
|
+ _transition_to_target_reached();
|
|
|
+ _transition_to_navigation_finished();
|
|
|
+ } else {
|
|
|
+ // Advance waypoints if possible.
|
|
|
+ _advance_waypoints(origin);
|
|
|
+ // Keep navigation running even after reaching the last waypoint if the target is reachable.
|
|
|
+ if (last_waypoint_reached && !_is_target_reachable()) {
|
|
|
+ _transition_to_navigation_finished();
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
|
|
|
- const Vector3 waypoint = navigation_path[navigation_path_index];
|
|
|
- details[SNAME("position")] = waypoint;
|
|
|
+void NavigationAgent3D::_advance_waypoints(const Vector3 &p_origin) {
|
|
|
+ if (last_waypoint_reached) {
|
|
|
+ return;
|
|
|
+ }
|
|
|
|
|
|
- int waypoint_type = -1;
|
|
|
- if (path_metadata_flags.has_flag(NavigationPathQueryParameters3D::PathMetadataFlags::PATH_METADATA_INCLUDE_TYPES)) {
|
|
|
- const NavigationPathQueryResult3D::PathSegmentType type = NavigationPathQueryResult3D::PathSegmentType(navigation_path_types[navigation_path_index]);
|
|
|
+ // Advance to the farthest possible waypoint.
|
|
|
+ while (_is_within_waypoint_distance(p_origin)) {
|
|
|
+ _trigger_waypoint_reached();
|
|
|
|
|
|
- details[SNAME("type")] = type;
|
|
|
- waypoint_type = type;
|
|
|
- }
|
|
|
+ if (_is_last_waypoint()) {
|
|
|
+ last_waypoint_reached = true;
|
|
|
+ break;
|
|
|
+ }
|
|
|
|
|
|
- if (path_metadata_flags.has_flag(NavigationPathQueryParameters3D::PathMetadataFlags::PATH_METADATA_INCLUDE_RIDS)) {
|
|
|
- details[SNAME("rid")] = navigation_path_rids[navigation_path_index];
|
|
|
- }
|
|
|
+ _move_to_next_waypoint();
|
|
|
+ }
|
|
|
+}
|
|
|
|
|
|
- if (path_metadata_flags.has_flag(NavigationPathQueryParameters3D::PathMetadataFlags::PATH_METADATA_INCLUDE_OWNERS)) {
|
|
|
- const ObjectID waypoint_owner_id = ObjectID(navigation_path_owners[navigation_path_index]);
|
|
|
+void NavigationAgent3D::_request_repath() {
|
|
|
+ navigation_result->reset();
|
|
|
+ target_reached = false;
|
|
|
+ navigation_finished = false;
|
|
|
+ last_waypoint_reached = false;
|
|
|
+ update_frame_id = 0;
|
|
|
+}
|
|
|
|
|
|
- // Get a reference to the owning object.
|
|
|
- Object *owner = nullptr;
|
|
|
- if (waypoint_owner_id.is_valid()) {
|
|
|
- owner = ObjectDB::get_instance(waypoint_owner_id);
|
|
|
- }
|
|
|
+bool NavigationAgent3D::_is_last_waypoint() const {
|
|
|
+ return navigation_path_index == navigation_result->get_path().size() - 1;
|
|
|
+}
|
|
|
|
|
|
- details[SNAME("owner")] = owner;
|
|
|
-
|
|
|
- if (waypoint_type == NavigationPathQueryResult3D::PATH_SEGMENT_TYPE_LINK) {
|
|
|
- const NavigationLink3D *navlink = Object::cast_to<NavigationLink3D>(owner);
|
|
|
- if (navlink) {
|
|
|
- Vector3 link_global_start_position = navlink->get_global_start_position();
|
|
|
- Vector3 link_global_end_position = navlink->get_global_end_position();
|
|
|
- if (waypoint.distance_to(link_global_start_position) < waypoint.distance_to(link_global_end_position)) {
|
|
|
- details[SNAME("link_entry_position")] = link_global_start_position;
|
|
|
- details[SNAME("link_exit_position")] = link_global_end_position;
|
|
|
- } else {
|
|
|
- details[SNAME("link_entry_position")] = link_global_end_position;
|
|
|
- details[SNAME("link_exit_position")] = link_global_start_position;
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
+void NavigationAgent3D::_move_to_next_waypoint() {
|
|
|
+ navigation_path_index += 1;
|
|
|
+}
|
|
|
|
|
|
- // Emit a signal for the waypoint
|
|
|
- emit_signal(SNAME("waypoint_reached"), details);
|
|
|
+bool NavigationAgent3D::_is_within_waypoint_distance(const Vector3 &p_origin) const {
|
|
|
+ const Vector<Vector3> &navigation_path = navigation_result->get_path();
|
|
|
+ Vector3 waypoint = navigation_path[navigation_path_index] - Vector3(0, path_height_offset, 0);
|
|
|
+ return p_origin.distance_to(waypoint) < path_desired_distance;
|
|
|
+}
|
|
|
|
|
|
- // Emit a signal if we've reached a navigation link
|
|
|
- if (waypoint_type == NavigationPathQueryResult3D::PATH_SEGMENT_TYPE_LINK) {
|
|
|
- emit_signal(SNAME("link_reached"), details);
|
|
|
- }
|
|
|
+bool NavigationAgent3D::_is_within_target_distance(const Vector3 &p_origin) const {
|
|
|
+ return p_origin.distance_to(target_position) < target_desired_distance;
|
|
|
+}
|
|
|
+
|
|
|
+void NavigationAgent3D::_trigger_waypoint_reached() {
|
|
|
+ const Vector<Vector3> &navigation_path = navigation_result->get_path();
|
|
|
+ const Vector<int32_t> &navigation_path_types = navigation_result->get_path_types();
|
|
|
+ const TypedArray<RID> &navigation_path_rids = navigation_result->get_path_rids();
|
|
|
+ const Vector<int64_t> &navigation_path_owners = navigation_result->get_path_owner_ids();
|
|
|
+
|
|
|
+ Dictionary details;
|
|
|
+
|
|
|
+ const Vector3 waypoint = navigation_path[navigation_path_index];
|
|
|
+ details[SNAME("position")] = waypoint;
|
|
|
+
|
|
|
+ int waypoint_type = -1;
|
|
|
+ if (path_metadata_flags.has_flag(NavigationPathQueryParameters3D::PathMetadataFlags::PATH_METADATA_INCLUDE_TYPES)) {
|
|
|
+ const NavigationPathQueryResult3D::PathSegmentType type = NavigationPathQueryResult3D::PathSegmentType(navigation_path_types[navigation_path_index]);
|
|
|
+
|
|
|
+ details[SNAME("type")] = type;
|
|
|
+ waypoint_type = type;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (path_metadata_flags.has_flag(NavigationPathQueryParameters3D::PathMetadataFlags::PATH_METADATA_INCLUDE_RIDS)) {
|
|
|
+ details[SNAME("rid")] = navigation_path_rids[navigation_path_index];
|
|
|
+ }
|
|
|
+
|
|
|
+ if (path_metadata_flags.has_flag(NavigationPathQueryParameters3D::PathMetadataFlags::PATH_METADATA_INCLUDE_OWNERS)) {
|
|
|
+ const ObjectID waypoint_owner_id = ObjectID(navigation_path_owners[navigation_path_index]);
|
|
|
+
|
|
|
+ // Get a reference to the owning object.
|
|
|
+ Object *owner = nullptr;
|
|
|
+ if (waypoint_owner_id.is_valid()) {
|
|
|
+ owner = ObjectDB::get_instance(waypoint_owner_id);
|
|
|
+ }
|
|
|
|
|
|
- // Move to the next waypoint on the list
|
|
|
- navigation_path_index += 1;
|
|
|
-
|
|
|
- // Check to see if we've finished our route
|
|
|
- if (navigation_path_index == navigation_path.size()) {
|
|
|
- _check_distance_to_target();
|
|
|
- navigation_path_index -= 1;
|
|
|
- navigation_finished = true;
|
|
|
- target_position_submitted = false;
|
|
|
- if (avoidance_enabled) {
|
|
|
- NavigationServer3D::get_singleton()->agent_set_position(agent, agent_parent->get_global_transform().origin);
|
|
|
- NavigationServer3D::get_singleton()->agent_set_velocity(agent, Vector3(0.0, 0.0, 0.0));
|
|
|
- NavigationServer3D::get_singleton()->agent_set_velocity_forced(agent, Vector3(0.0, 0.0, 0.0));
|
|
|
- stored_y_velocity = 0.0;
|
|
|
+ details[SNAME("owner")] = owner;
|
|
|
+
|
|
|
+ if (waypoint_type == NavigationPathQueryResult3D::PATH_SEGMENT_TYPE_LINK) {
|
|
|
+ const NavigationLink3D *navlink = Object::cast_to<NavigationLink3D>(owner);
|
|
|
+ if (navlink) {
|
|
|
+ Vector3 link_global_start_position = navlink->get_global_start_position();
|
|
|
+ Vector3 link_global_end_position = navlink->get_global_end_position();
|
|
|
+ if (waypoint.distance_to(link_global_start_position) < waypoint.distance_to(link_global_end_position)) {
|
|
|
+ details[SNAME("link_entry_position")] = link_global_start_position;
|
|
|
+ details[SNAME("link_exit_position")] = link_global_end_position;
|
|
|
+ } else {
|
|
|
+ details[SNAME("link_entry_position")] = link_global_end_position;
|
|
|
+ details[SNAME("link_exit_position")] = link_global_start_position;
|
|
|
}
|
|
|
- emit_signal(SNAME("navigation_finished"));
|
|
|
- break;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-}
|
|
|
|
|
|
-void NavigationAgent3D::_request_repath() {
|
|
|
- navigation_result->reset();
|
|
|
- target_reached = false;
|
|
|
- navigation_finished = false;
|
|
|
- update_frame_id = 0;
|
|
|
+ // Emit a signal for the waypoint.
|
|
|
+ emit_signal(SNAME("waypoint_reached"), details);
|
|
|
+
|
|
|
+ // Emit a signal if we've reached a navigation link.
|
|
|
+ if (waypoint_type == NavigationPathQueryResult3D::PATH_SEGMENT_TYPE_LINK) {
|
|
|
+ emit_signal(SNAME("link_reached"), details);
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
-void NavigationAgent3D::_check_distance_to_target() {
|
|
|
- if (!target_reached) {
|
|
|
- if (distance_to_target() < target_desired_distance) {
|
|
|
- target_reached = true;
|
|
|
- emit_signal(SNAME("target_reached"));
|
|
|
- }
|
|
|
+void NavigationAgent3D::_transition_to_navigation_finished() {
|
|
|
+ navigation_finished = true;
|
|
|
+ target_position_submitted = false;
|
|
|
+
|
|
|
+ if (avoidance_enabled) {
|
|
|
+ NavigationServer3D::get_singleton()->agent_set_position(agent, agent_parent->get_global_transform().origin);
|
|
|
+ NavigationServer3D::get_singleton()->agent_set_velocity(agent, Vector3(0.0, 0.0, 0.0));
|
|
|
+ NavigationServer3D::get_singleton()->agent_set_velocity_forced(agent, Vector3(0.0, 0.0, 0.0));
|
|
|
+ stored_y_velocity = 0.0;
|
|
|
}
|
|
|
+
|
|
|
+ emit_signal(SNAME("navigation_finished"));
|
|
|
+}
|
|
|
+
|
|
|
+void NavigationAgent3D::_transition_to_target_reached() {
|
|
|
+ target_reached = true;
|
|
|
+ emit_signal(SNAME("target_reached"));
|
|
|
}
|
|
|
|
|
|
void NavigationAgent3D::set_avoidance_layers(uint32_t p_layers) {
|
Yuri Sizov