| 152 | } |
| 153 | |
| 154 | void NavMap3D::query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &p_query_task) { |
| 155 | if (iteration_id == 0) { |
| 156 | return; |
| 157 | } |
| 158 | |
| 159 | GET_MAP_ITERATION(); |
| 160 | |
| 161 | map_iteration.path_query_slots_semaphore.wait(); |
| 162 | |
| 163 | map_iteration.path_query_slots_mutex.lock(); |
| 164 | for (NavMeshQueries3D::PathQuerySlot &p_path_query_slot : map_iteration.path_query_slots) { |
| 165 | if (!p_path_query_slot.in_use) { |
| 166 | p_path_query_slot.in_use = true; |
| 167 | p_query_task.path_query_slot = &p_path_query_slot; |
| 168 | break; |
| 169 | } |
| 170 | } |
| 171 | map_iteration.path_query_slots_mutex.unlock(); |
| 172 | |
| 173 | if (p_query_task.path_query_slot == nullptr) { |
| 174 | map_iteration.path_query_slots_semaphore.post(); |
| 175 | ERR_FAIL_NULL_MSG(p_query_task.path_query_slot, "No unused NavMap3D path query slot found! This should never happen :(."); |
| 176 | } |
| 177 | |
| 178 | p_query_task.map_up = map_iteration.map_up; |
| 179 | |
| 180 | NavMeshQueries3D::query_task_map_iteration_get_path(p_query_task, map_iteration); |
| 181 | |
| 182 | map_iteration.path_query_slots_mutex.lock(); |
| 183 | uint32_t used_slot_index = p_query_task.path_query_slot->slot_index; |
| 184 | map_iteration.path_query_slots[used_slot_index].in_use = false; |
| 185 | p_query_task.path_query_slot = nullptr; |
| 186 | map_iteration.path_query_slots_mutex.unlock(); |
| 187 | |
| 188 | map_iteration.path_query_slots_semaphore.post(); |
| 189 | } |
| 190 | |
| 191 | Vector3 NavMap3D::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const { |
| 192 | if (iteration_id == 0) { |