shift the polygon heap only if index is in range

modules/navigation/3d/nav_mesh_queries_3d.cpp

@@ -314,7 +314,7 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 								owner->get_travel_cost();
 						neighbor_poly.entry = new_entry;
 
-						if (neighbor_poly.poly != nullptr) {
+						if (neighbor_poly.traversable_poly_index != traversable_polys.INVALID_INDEX) {
 							traversable_polys.shift(neighbor_poly.traversable_poly_index);
 						} else {
 							neighbor_poly.poly = connection.polygon;

modules/navigation/nav_utils.h

@@ -201,6 +201,7 @@ class Heap {
 	Indexer _indexer;
 
 public:
+	static constexpr uint32_t INVALID_INDEX = UINT32_MAX;
 	void reserve(uint32_t p_size) {
 		_buffer.reserve(p_size);
 	}
@@ -222,7 +223,7 @@ public:
 	T pop() {
 		ERR_FAIL_COND_V_MSG(_buffer.is_empty(), T(), "Can't pop an empty heap.");
 		T value = _buffer[0];
-		_indexer(value, UINT32_MAX);
+		_indexer(value, INVALID_INDEX);
 		if (_buffer.size() > 1) {
 			_buffer[0] = _buffer[_buffer.size() - 1];
 			_indexer(_buffer[0], 0);
@@ -246,7 +247,7 @@ public:
 
 	void clear() {
 		for (const T &value : _buffer) {
-			_indexer(value, UINT32_MAX);
+			_indexer(value, INVALID_INDEX);
 		}
 		_buffer.clear();
 	}