159 int32_t initial_shortcut_x, int32_t initial_shortcut_z)
161 struct Math_Vec3I32 shortcut_delta = {initial_shortcut_x, 0, initial_shortcut_z};
163 int32_t neighbor_count;
174 int32_t neighbor_index = neighbor_count - 1;
175 int32_t saved_neighbor_index = neighbor_index;
177 if (neighbor_count - 1 < 0)
180 for (; neighbor_index >= 0; neighbor_index -= 1, saved_neighbor_index = neighbor_index, current_open_entry = current_state)
182 int32_t packed_neighbor;
184 &nav_network->
nodes[(int32_t)current_open_entry->
node_id];
189 int16_t goal_destination_id = (uint16_t)destination_id;
220 goal_position = goal_position_alias;
224 int32_t edge_cost = (uint16_t)(neighbor_list[neighbor_index].
cost);
225 int32_t projection_z = current_nav_node->
pos.
z;
226 int32_t projection_x = current_nav_node->
pos.
x;
228 uint32_t goal_cost = edge_cost + current_open_entry->
cost;
231 goal_projection_point.
x = projection_x;
232 goal_projection_point.
y = current_nav_node->
pos.
y;
234 uint8_t link_bits = neighbor_list[saved_neighbor_index].
packed_id;
236 goal_projection_point.
z = projection_z;
239 uint32_t projection_neighbor_or_sentinel = projection_edge_selector - 1;
242 if (projection_edge_selector - 1 >= 0)
244 int32_t packed_projection_neighbor = (uint16_t)(neighbor_list[projection_edge_selector - 1].packed_id);
246 shortcut_delta.
x = projection_x - goal_position_alias->
x;
247 shortcut_delta.
y = goal_projection_point.
y - goal_position_alias->
y;
251 projection_neighbor_or_sentinel = projection_neighbor_id;
252 shortcut_delta.
z = projection_z - goal_position_alias->
z;
254 struct Nav_Node *projection_neighbor = &nav_network->
nodes[projection_neighbor_id];
255 int32_t neighbor_position_x = projection_neighbor->
pos.
x;
257 projection_delta.
x = neighbor_position_x;
258 projection_delta.
y = projection_neighbor->
pos.
y;
259 projection_delta.
z = projection_neighbor->
pos.
z;
261 int64_t start_projection_cross_x_wide = (int64_t)(neighbor_position_x - projection_x) * shortcut_delta.
x;
262 int64_t start_projection_cross_z_wide = (int64_t)(projection_z - projection_delta.
z) * shortcut_delta.
z;
263 int32_t projection_cross_numerator =
264 ((uint32_t)((uint64_t)start_projection_cross_x_wide >>
MATH_Q12_SHIFT)) - ((uint32_t)((uint64_t)start_projection_cross_z_wide >>
MATH_Q12_SHIFT));
265 int32_t projection_neighbor_x = projection_delta.
x;
266 int64_t end_projection_cross_x_wide = (int64_t)(projection_neighbor_x - projection_x) * (projection_neighbor_x - goal_position_alias->
x);
267 int32_t projection_neighbor_z = projection_delta.
z;
268 int64_t end_projection_cross_z_wide = (int64_t)(projection_z - projection_neighbor_z) * (projection_neighbor_z - goal_position_alias->
z);
269 int32_t projection_fraction_q12 = ((int64_t)projection_cross_numerator *
MATH_Q12_ONE) / (int32_t)(((uint32_t)((uint64_t)end_projection_cross_z_wide >>
MATH_Q12_SHIFT)) - ((uint32_t)((uint64_t)end_projection_cross_x_wide >>
MATH_Q12_SHIFT)) + (uint32_t)projection_cross_numerator);
272 .
x = projection_delta.
x - projection_x,
273 .y = projection_delta.
y - goal_projection_point.
y,
274 .z = projection_delta.
z - projection_z};
276 int64_t projection_x_scaled_wide = (int64_t)projection_fraction_q12 * projection_delta.
x;
278 projection_delta.
x = (uint32_t)((uint64_t)projection_x_scaled_wide >>
MATH_Q12_SHIFT);
280 int64_t projection_y_scaled_wide = (int64_t)projection_fraction_q12 * projection_delta.
y;
282 projection_delta.
y = (uint32_t)((uint64_t)projection_y_scaled_wide >>
MATH_Q12_SHIFT);
284 int64_t projection_z_scaled_wide = (int64_t)projection_fraction_q12 * projection_delta.
z;
285 int32_t projected_offset_z = (uint32_t)((uint64_t)projection_z_scaled_wide >>
MATH_Q12_SHIFT);
287 projection_delta.
z = projected_offset_z;
291 struct Nav_Node *seed_neighbor = &nav_network->
nodes[projection_neighbor_id];
295 .
x = projection_delta.
x,
296 .y = projection_delta.
y,
297 .z = projected_offset_z};
302 shortcut_delta.
x = path_end_for_seed[-1].
x - projection_x;
303 shortcut_delta.
y = path_end_for_seed[-1].
y - goal_projection_point.
y;
305 (uint32_t)path_end_for_seed[-1].
z - projection_z;
307 int32_t shortcut_length_squared =
308 (int32_t)(((uint64_t)((int64_t)shortcut_delta.
x * shortcut_delta.
x) + (uint64_t)((int64_t)shortcut_delta.
y * shortcut_delta.
y) + (uint64_t)((int64_t)shortcut_delta.
z * shortcut_delta.
z)) >>
MATH_Q12_SHIFT);
309 int32_t projection_length_squared =
310 (int32_t)(((uint64_t)((int64_t)projection_delta.
x * projection_delta.
x) + (uint64_t)((int64_t)projection_delta.
y * projection_delta.
y) + (uint64_t)((int64_t)projection_delta.
z * projection_delta.
z)) >>
MATH_Q12_SHIFT);
311 int32_t shorter_length_squared;
313 shorter_length_squared = shortcut_length_squared > projection_length_squared ? projection_length_squared : shortcut_length_squared;
315 int32_t cost_distance_reduction =
Math_SqrtFP12(shorter_length_squared) >> 5;
317 cost_distance_reduction &= ~1;
318 goal_cost -= cost_distance_reduction;
320 int32_t projected_point_x = goal_projection_point.
x + projection_delta.
x;
321 int32_t projected_offset_y = projection_delta.
y;
323 shortcut_delta.
x = projected_point_x;
325 int32_t projected_point_y = projected_offset_y + goal_projection_point.
y;
327 projected_offset_z = projection_delta.
z;
328 shortcut_delta.
y = projected_point_y;
330 int32_t projected_point_z = projected_offset_z + goal_projection_point.
z;
333 shortcut_delta.
z = projected_point_z;
335 path_end_for_shortcut[-1].
x - projected_point_x;
336 goal_position_alias = goal_position;
340 projection_x = goal_projection_point.
x;
341 shortcut_delta.
y = path_end_for_shortcut[-1].
y - projected_point_y;
342 projection_z = goal_projection_point.
z;
344 (uint32_t)path_end_for_shortcut[-1].
z - projected_point_z;
348 projection_x += projection_delta.
x;
349 projection_z += projected_offset_z;
352 .y = goal_projection_point.
y + projection_delta.
y,
356 bool shortcut_valid =
false;
360 shortcut_delta.y = 0;
362 if (((int32_t)(((uint64_t)((int64_t)shortcut_delta.x * shortcut_delta.x) + (uint64_t)((int64_t)shortcut_delta.y * shortcut_delta.y) + (uint64_t)((int64_t)shortcut_delta.z * shortcut_delta.z)) >> MATH_Q12_SHIFT)) < NAVIGATION_SHORTCUT_DISTANCE_SQUARED_LIMIT)
364 int32_t goal_projection_x = goal_projection_point.x;
366 projection_delta.x = goal_position->x - goal_projection_x;
368 int32_t goal_projection_y = goal_projection_point.y;
370 projection_delta.y = goal_position->y - goal_projection_y;
372 int32_t goal_projection_z = goal_projection_point.z;
374 projection_delta.z = goal_position->z - goal_projection_z;
376 struct Math_Vec3I32 *path_end_for_start = g_boneTrailPathEnd;
377 struct Math_Vec3I32 *shortcut_start_position;
379 if ((uintptr_t)path_end_for_start <= (uintptr_t)&g_boneTrailTargetPosition)
381 shortcut_start_position = &path_end_for_start[-1];
382 shortcut_delta.x = goal_projection_x - path_end_for_start[-1].x;
383 shortcut_delta.y = goal_projection_y - path_end_for_start[-1].y;
385 goal_projection_z - (uint32_t)path_end_for_start[-1].z;
389 shortcut_delta.x = path_end_for_start[-1].x - path_end_for_start[-2].x;
390 shortcut_start_position = &path_end_for_start[-2];
391 shortcut_delta.y = path_end_for_start[-1].y - path_end_for_start[-2].y;
392 shortcut_delta.z = (uint32_t)path_end_for_start[-1].z - path_end_for_start[-2].z;
395 int64_t goal_start_dot_x_wide = (int64_t)projection_delta.x * shortcut_delta.x;
396 int64_t goal_start_dot_z_wide = (int64_t)projection_delta.z * shortcut_delta.z;
398 if ((int32_t)(((uint32_t)((uint64_t)goal_start_dot_z_wide >> MATH_Q12_SHIFT)) + ((uint32_t)((uint64_t)goal_start_dot_x_wide >> MATH_Q12_SHIFT))) < 0)
400 projection_delta.y = 0;
402 int32_t goal_direction_length = Math_SqrtFP12((int32_t)(((uint64_t)((int64_t)projection_delta.x * projection_delta.x) + (uint64_t)((int64_t)projection_delta.y * projection_delta.y) + (uint64_t)((int64_t)projection_delta.z * projection_delta.z)) >> MATH_Q12_SHIFT));
404 if (goal_direction_length)
406 int32_t goal_direction_inverse_length = INT64_C(0x100000000) / goal_direction_length;
407 int64_t goal_direction_scaled_x_wide = (int64_t)goal_direction_inverse_length * projection_delta.x;
409 projection_delta.x = (int32_t)((uint32_t)((uint64_t)goal_direction_scaled_x_wide >> MATH_Q12_SHIFT)) >> 8;
411 int64_t goal_direction_scaled_y_wide = (int64_t)goal_direction_inverse_length * projection_delta.y;
413 projection_delta.y = (int32_t)((uint32_t)((uint64_t)goal_direction_scaled_y_wide >> MATH_Q12_SHIFT)) >> 8;
415 int64_t goal_direction_scaled_z_wide = (int64_t)goal_direction_inverse_length * projection_delta.z;
417 projection_delta.z = (int32_t)((uint32_t)((uint64_t)goal_direction_scaled_z_wide >> MATH_Q12_SHIFT)) >> 8;
420 memset(&projection_delta, 0, 0xc);
422 struct Math_Vec3I32 *shortcut_start = shortcut_start_position;
423 struct Math_Vec3I32 *checked_goal_position = goal_position;
424 int64_t goal_cross_zx_wide = (int64_t)projection_delta.z * (shortcut_start->x - checked_goal_position->x);
425 int64_t goal_cross_xz_wide = (int64_t)projection_delta.x * (shortcut_start->z - checked_goal_position->z);
427 if (((uint32_t)((uint64_t)goal_cross_zx_wide >> MATH_Q12_SHIFT)) - ((uint32_t)((uint64_t)goal_cross_xz_wide >> MATH_Q12_SHIFT)) + NAVIGATION_SHORTCUT_LATERAL_LIMIT_Q12 > NAVIGATION_SHORTCUT_LATERAL_RANGE_Q12)
429 shortcut_delta.y = 0;
431 int32_t start_direction_length = Math_SqrtFP12((int32_t)(((uint64_t)((int64_t)shortcut_delta.x * shortcut_delta.x) + (uint64_t)((int64_t)shortcut_delta.y * shortcut_delta.y) + (uint64_t)((int64_t)shortcut_delta.z * shortcut_delta.z)) >> MATH_Q12_SHIFT));
433 if (start_direction_length)
435 int32_t start_direction_inverse_length = INT64_C(0x100000000) / start_direction_length;
436 int64_t start_direction_scaled_x_wide = (int64_t)start_direction_inverse_length * shortcut_delta.x;
438 shortcut_delta.x = (int32_t)((uint32_t)((uint64_t)start_direction_scaled_x_wide >> MATH_Q12_SHIFT)) >> 8;
440 int64_t start_direction_scaled_y_wide = (int64_t)start_direction_inverse_length * shortcut_delta.y;
442 shortcut_delta.y = (int32_t)((uint32_t)((uint64_t)start_direction_scaled_y_wide >> MATH_Q12_SHIFT)) >> 8;
444 int64_t start_direction_scaled_z_wide = (int64_t)start_direction_inverse_length * shortcut_delta.z;
446 shortcut_delta.z = (int32_t)((uint32_t)((uint64_t)start_direction_scaled_z_wide >> MATH_Q12_SHIFT)) >> 8;
449 memset(&shortcut_delta, 0, 0xc);
451 shortcut_start = shortcut_start_position;
452 checked_goal_position = goal_position;
454 int64_t start_cross_zx_wide = (int64_t)shortcut_delta.z * (shortcut_start->x - checked_goal_position->x);
455 int64_t start_cross_xz_wide = (int64_t)shortcut_delta.x * (shortcut_start->z - checked_goal_position->z);
457 if (((uint32_t)((uint64_t)start_cross_zx_wide >> MATH_Q12_SHIFT)) - ((uint32_t)((uint64_t)start_cross_xz_wide >> MATH_Q12_SHIFT)) + NAVIGATION_SHORTCUT_LATERAL_LIMIT_Q12 <= NAVIGATION_SHORTCUT_LATERAL_RANGE_Q12)
459 shortcut_valid = true;
463 shortcut_valid = true;
467 if (!Physics_CalculateMovementWithCollision(shortcut_start, checked_goal_position,
468 NULL, NAVIGATION_SHORTCUT_COLLISION_RADIUS_Q6, 1))
470 shortcut_delta.x = checked_goal_position->x - shortcut_start->x;
471 shortcut_delta.y = checked_goal_position->y - shortcut_start->y;
472 shortcut_delta.z = checked_goal_position->z - shortcut_start->z;
474 goal_cost = Math_SqrtFP12((int32_t)(((uint64_t)((int64_t)shortcut_delta.x * shortcut_delta.x) + (uint64_t)((int64_t)shortcut_delta.y * shortcut_delta.y) + (uint64_t)((int64_t)shortcut_delta.z * shortcut_delta.z)) >> MATH_Q12_SHIFT)) >> 6;
477 shortcut_valid = false;
482 projection_x = goal_projection_point.
x;
483 goal_position_alias = goal_position;
484 projection_z = goal_projection_point.
z;
494 if (!pathfind_state || goal_cost < pathfind_state->
cost)
500 goal_open_entry = new_goal_open_entry;
502 if (new_goal_open_entry)
507 new_goal_open_entry->
cost = goal_cost;
513 pathfind_state->
cost = goal_cost;
520 if ((int32_t)projection_neighbor_or_sentinel >= 0)
523 .y = goal_projection_point.y,
530 uint16_t path_depth = current_state->
path_depth;
532 if (path_depth || !shortcut_valid)
539 uint8_t parent_link_bits = current_neighbors[saved_neighbor_index].
flags_and_node_low;
542 if (parent_projection_edge_selector - 1 >= 0)
544 uint32_t parent_projection_node_index;
548 int32_t packed_parent_projection_neighbor;
550 packed_parent_projection_neighbor = (uint16_t)
551 current_neighbors[parent_projection_edge_selector - 1]
564 neighbor_index = saved_neighbor_index;
567 if (neighbor_index < 0)
570 int32_t previous_static_neighbor_index;
574 struct Nav_Node *static_nodes = nav_network->nodes;
576 &static_nodes[(int32_t)current_state->node_id].
neighbor_list[neighbor_index];
577 int32_t static_edge_cost = (uint16_t)(neighbor_edge->cost);
578 uint32_t static_total_cost = static_edge_cost + current_state->cost;
579 int32_t packed_static_neighbor = (uint16_t)(neighbor_edge->packed_id);
581 struct Nav_OpenSetEntry *neighbor_open_entry = static_nodes[static_neighbor_id].pathfind_state;
583 if (!neighbor_open_entry || ((uintptr_t)neighbor_open_entry != UINTPTR_MAX && static_total_cost < neighbor_open_entry->
cost))
585 if (!neighbor_open_entry)
589 if (neighbor_open_entry)
591 nav_network->nodes[static_neighbor_id].pathfind_state = neighbor_open_entry;
592 neighbor_open_entry->
node_id = (uint16_t)static_neighbor_id;
593 neighbor_open_entry->
cost = static_total_cost;
599 neighbor_open_entry->
cost = static_total_cost;
603 nav_network->nodes[static_neighbor_id].parent_link = current_state->node_id <<
NAVIGATION_LINK_NODE_SHIFT | (uint16_t)saved_neighbor_index;
604 neighbor_index = saved_neighbor_index;
605 neighbor_open_entry->
path_depth = current_state->path_depth + 1;
608 previous_static_neighbor_index = neighbor_index;
610 saved_neighbor_index = neighbor_index;
611 }
while (previous_static_neighbor_index);