40 int32_t start_x = start_pos->
x;
60 char first_ground_polygon = 1;
62 if (!ground_collision_node)
80 if (first_ground_polygon)
82 first_ground_polygon = 0;
85 struct Math_Vec3I32 *copied_center_destination = backup_path_cursor;
87 path_cursor->
y += 0x1000;
91 path_cursor = &path_cursor[1];
92 copied_center_destination->
x = polygon_center_point->
x;
93 backup_path_cursor = &backup_path_cursor[1];
94 copied_center_destination->
y = polygon_center_point->
y;
95 copied_center_destination->
z = polygon_center_point->
z;
97 int32_t offset_base_y = path_cursor->
y;
101 int32_t offset_base_z = path_cursor->
z;
110 backup_path_cursor -= 1;
111 path_cursor->
x = (uint32_t)backup_path_cursor->
x;
112 path_cursor->
y = backup_path_cursor->
y;
113 path_cursor->
z = backup_path_cursor->
z;
121 int32_t probe_base_y = ground_probe.
position.
y;
130 path_cursor = &path_cursor[1];
131 backup_path_cursor = &backup_path_cursor[1];
132 ground_probe.
position.
x = next_probe_point->
x;
134 int32_t next_probe_y = next_probe_point->
y;
139 int32_t next_probe_z = next_probe_point->
z;
152 .
x = path_cursor[-1].
x - start_pos->
x,
153 .z = (uint32_t)path_cursor[-1].
z - start_pos->
z,
156 int64_t start_distance_x_square_wide = (int64_t)distance_delta.
x * distance_delta.
x;
157 int64_t start_distance_y_square_wide = (int64_t)distance_delta.
y * distance_delta.
y;
158 int64_t start_distance_z_square_wide = (int64_t)distance_delta.
z * distance_delta.
z;
160 if (((int32_t)(((uint64_t)start_distance_z_square_wide + (uint64_t)start_distance_x_square_wide + (uint64_t)start_distance_y_square_wide) >> 12)) < 0x6400)
161 result_path_end -= 1;
165 int32_t navigation_link = 0;
171 struct Tree_Map *allocated_open_set = active_open_set;
183 if (source_open_entry)
185 source_open_entry->
node_id = (uint16_t)seed_node_id;
190 distance_delta.
x = path_end_for_source[-1].
x - nodes[(int32_t)(int16_t)seed_node_id].
pos.
x;
191 distance_delta.
y = path_end_for_source[-1].
y - nodes[(int32_t)source_open_entry->
node_id].
pos.
y;
192 distance_delta.
z = (uint32_t)path_end_for_source[-1].
z - nodes[(int32_t)source_open_entry->
node_id].
pos.
z;
194 int64_t source_distance_x_square_wide = (int64_t)distance_delta.
x * distance_delta.
x;
195 int64_t source_distance_y_square_wide = (int64_t)distance_delta.
y * distance_delta.
y;
196 int64_t source_distance_z_square_wide = (int64_t)distance_delta.
z * distance_delta.
z;
198 source_open_entry->
cost =
Math_SqrtFP12((int32_t)(((uint64_t)source_distance_z_square_wide + (uint64_t)source_distance_x_square_wide + (uint64_t)source_distance_y_square_wide) >> 12)) >> 6;
210 if (neighbor_open_entry)
212 uint16_t neighbor_node_id =
215 neighbor_open_entry->
node_id = neighbor_node_id;
220 distance_delta.
x = path_end_for_neighbor[-1].
x - neighbor_nodes[(int32_t)neighbor_node_id].pos.x;
221 distance_delta.
y = path_end_for_neighbor[-1].
y - neighbor_nodes[(int32_t)neighbor_open_entry->
node_id].
pos.
y;
222 distance_delta.
z = (uint32_t)path_end_for_neighbor[-1].
z - neighbor_nodes[(int32_t)neighbor_open_entry->
node_id].
pos.
z;
224 int64_t neighbor_distance_x_square_wide = (int64_t)distance_delta.
x * distance_delta.
x;
225 int64_t neighbor_distance_y_square_wide = (int64_t)distance_delta.
y * distance_delta.
y;
226 int64_t neighbor_distance_z_square_wide = (int64_t)distance_delta.
z * distance_delta.
z;
228 neighbor_open_entry->
cost =
Math_SqrtFP12((int32_t)(((uint64_t)neighbor_distance_z_square_wide + (uint64_t)neighbor_distance_x_square_wide + (uint64_t)neighbor_distance_y_square_wide) >> 12)) >> 6;
237 search_network = nav_network;
238 active_open_set = allocated_open_set;
244 if (!seed_open_entry)
245 search_network = nav_network;
248 seed_open_entry->
node_id = (uint16_t)navigation_link - 1;
249 seed_open_entry->
cost = 0;
251 search_network = nav_network;
262 if (popped_open_entry)
270 int32_t goal_selector = 0;
284 shortcut_path_end -= 1;
290 shortcut_path_end->
x = (uint32_t)goal_position->
x;
291 shortcut_path_end->
y = goal_position->
y;
292 shortcut_path_end->
z = goal_position->
z;
300 struct Math_Vec3I32 *reverse_path_cursor = goal_point_destination - 1;
303 goal_point_destination->
x = (uint32_t)goal_position->
x;
304 goal_point_destination->
y = goal_position->
y;
305 goal_point_destination->
z = goal_position->
z;
313 struct Nav_Node *goal_node = &parent_nodes[node_id];
314 int32_t packed_parent_link = goal_node->
parent_link;
315 uint32_t parent_node_id = packed_parent_link >> 5;
320 struct Math_Vec3I32 *projection_point_destination = reverse_path_cursor;
322 reverse_path_cursor -= 1;
323 projection_point_destination->
x = goal_node->
pos.
x;
324 projection_point_destination->
y = goal_node->
pos.
y;
325 projection_point_destination->
z = goal_node->
pos.
z;
329 parent_node_id = node_id;
332 int32_t backtrack_parent_link = 0;
333 int32_t retained_path_points = (int32_t)popped_open_entry->
path_depth + 1;
335 backtrack_parent_link = backtrack_nodes[parent_node_id].
parent_link;
337 int32_t shortcut_path_points = retained_path_points;
343 struct Nav_Node *parent_node = &backtrack_nodes[backtrack_parent_link >> 5];
348 shortcut_path_points = ((
char *)reverse_path_cursor - (
char *)
g_boneTrailPathEnd) / 12 + 1;
351 struct Math_Vec3I32 *parent_point_destination = reverse_path_cursor;
353 reverse_path_cursor -= 1;
354 parent_point_destination->
x = (uint32_t)parent_node->
pos.
x;
355 parent_point_destination->
y = parent_node->
pos.
y;
356 parent_point_destination->
z = parent_node->
pos.
z;
357 backtrack_nodes = search_network->
nodes;
358 backtrack_parent_link = backtrack_nodes[backtrack_parent_link >> 5].
parent_link;
361 retained_path_points = shortcut_path_points;
364 active_open_set = allocated_open_set;
365 result_path_end = &reverse_path_cursor[retained_path_points + 1];
373 skip_dynamic_targets);
378 if (!goal_open_entry)
381 popped_open_entry = goal_open_entry;
387 int32_t cleared_node_count = 0;
395 cleared_node_count += 1;
398 }
while (cleared_node_count <= search_network->node_count);
429 int64_t initial_second_dot_x_wide = (int64_t)initial_path_delta.
x * second_segment_delta.
x;
430 int64_t initial_second_dot_z_wide = (int64_t)initial_path_delta.
z * second_segment_delta.
z;
432 if (((int32_t)((uint64_t)initial_second_dot_z_wide >> 12)) + ((int32_t)((uint64_t)initial_second_dot_x_wide >> 12)) >= 0)
435 second_segment_delta.
y = 0;
437 int64_t second_segment_x_square_wide = (int64_t)second_segment_delta.
x * second_segment_delta.
x;
438 int64_t second_segment_y_square_wide = (int64_t)second_segment_delta.
y * second_segment_delta.
y;
439 int64_t second_segment_z_square_wide = (int64_t)second_segment_delta.
z * second_segment_delta.
z;
440 int32_t second_segment_length =
441 Math_SqrtFP12((int32_t)(((uint64_t)second_segment_z_square_wide + (uint64_t)second_segment_x_square_wide + (uint64_t)second_segment_y_square_wide) >> 12));
443 if (second_segment_length)
445 int32_t second_segment_inverse_length = INT64_C(4294967296) / second_segment_length;
446 int64_t second_segment_scaled_x_wide = (int64_t)second_segment_inverse_length * second_segment_delta.
x;
448 second_segment_delta.
x = ((int32_t)((uint64_t)second_segment_scaled_x_wide >> 12)) >> 8;
450 int64_t second_segment_scaled_y_wide = (int64_t)second_segment_inverse_length * second_segment_delta.
y;
452 second_segment_delta.
y = ((int32_t)((uint64_t)second_segment_scaled_y_wide >> 12)) >> 8;
454 int64_t second_segment_scaled_z_wide = (int64_t)second_segment_inverse_length * second_segment_delta.
z;
456 second_segment_delta.
z = ((int32_t)((uint64_t)second_segment_scaled_z_wide >> 12)) >> 8;
459 __builtin_memset(&second_segment_delta, 0, 0xc);
461 int64_t segment_cross_zx_wide = (int64_t)second_segment_delta.
z * initial_path_delta.
x;
462 int64_t segment_cross_xz_wide = (int64_t)second_segment_delta.
x * initial_path_delta.
z;
464 if ((uint32_t)(((int32_t)((uint64_t)segment_cross_zx_wide >> 12)) - ((int32_t)((uint64_t)segment_cross_xz_wide >> 12)) + 0x5000) <= 0xa000u)
467 for (int32_t i = 1; i < result; ++i)
476 if (ground_collision_node)