40 int32_t response_scratch)
45 int16_t
x,
y,
z, padding;
46 } accumulated_normal = {0};
56 debug_polygon_list_index = 0;
66 int32_t resolver_mode;
67 int32_t origin_x_or_packed_motion_xy;
68 int32_t origin_y_or_motion_z_bits;
69 int32_t relative_origin_z;
70 int32_t local_motion_x;
75 int32_t initial_resolver_mode = 0;
98 resolver_mode = initial_resolver_mode;
104 int32_t adjusted_origin_z;
111 origin_x_or_packed_motion_xy = relative_origin_x + source_node->
collision_offset.
x;
112 origin_y_or_motion_z_bits = offset_origin_y;
113 adjusted_origin_z = unadjusted_origin_z + source_offset_z;
117 int32_t swept_origin_y = relative_origin_y + actor->
sub_pos.
y;
118 int32_t swept_origin_z = unadjusted_origin_z + actor->
sub_pos.
z;
119 int32_t actor_velocity_z = actor->
velocity.
z;
121 origin_x_or_packed_motion_xy = relative_origin_x + actor->
sub_pos.
x - actor->
velocity.
x;
122 adjusted_origin_z = swept_origin_z - actor_velocity_z;
123 origin_y_or_motion_z_bits = swept_origin_y - actor->
velocity.
y;
126 relative_origin_z = adjusted_origin_z;
138 origin_x_or_packed_motion_xy = box_relative_x;
140 int32_t box_relative_y = actor_world_y - box_center_y;
141 int32_t box_relative_z = actor_world_z - box_center_z;
142 int32_t box_relative_x_q6 = (int32_t)(int16_t)(box_relative_x >> 6);
143 int32_t box_relative_y_q6 = (int32_t)(int16_t)(box_relative_y >> 6);
144 int32_t box_relative_z_q6 = (int32_t)(int16_t)(box_relative_z >> 6);
149 int16_t box_local_y_q6 = (int16_t)((box_rotation_m10 * box_relative_x_q6 + box_rotation_m11 * box_relative_y_q6 + box_rotation_m12 * box_relative_z_q6) >>
MATH_Q12_SHIFT);
153 int32_t actor_motion_x = actor->
sub_pos.
x;
154 int32_t actor_motion_y = actor->
sub_pos.
y;
156 local_motion_x = origin_x_or_packed_motion_xy + actor_motion_x;
158 int32_t actor_motion_z = actor->
sub_pos.
z;
160 int32_t box_motion_z_q6 = (int16_t)(actor_motion_z >> 6);
161 response_scratch = box_motion_z_q6;
163 int32_t box_motion_x_q6 = (int32_t)(int16_t)(actor_motion_x >> 6);
164 int32_t box_motion_y_q6 = (int32_t)(int16_t)(actor_motion_y >> 6);
165 int32_t expanded_box_local_x = (int32_t)box_local_x_q6;
168 int16_t box_local_motion_y = (int16_t)((box_rotation_m10 * box_motion_x_q6 + box_rotation_m11 * box_motion_y_q6 + box_rotation_m12 * box_motion_z_q6) >>
MATH_Q12_SHIFT);
169 int16_t box_local_motion_z = (int16_t)((box_rotation_m20 * box_motion_x_q6 + box_rotation_m21 * box_motion_y_q6 + box_rotation_m22 * box_motion_z_q6) >>
MATH_Q12_SHIFT);
171 actor_box_corners[0].
position_q6.
x = box_local_motion_x;
172 actor_box_corners[0].
position_q6.
y = box_local_motion_y;
173 actor_box_corners[0].
position_q6.
z = box_local_motion_z;
175 int32_t box_sweep_radius;
177 if (box_local_motion_x <= 0)
179 box_sweep_radius = cull_radius;
181 if (!(expanded_box_local_x + box_half_extent_x + box_sweep_radius >= 0))
189 box_sweep_radius = cull_radius;
191 if (expanded_box_local_x - box_half_extent_x - box_sweep_radius > 0 || (int32_t)box_local_motion_x + (int32_t)box_local_x_q6 + (int32_t)source_node->
bounds.
oriented_box.
half_extents_q6.
x + box_sweep_radius < 0)
196 int32_t expanded_box_local_y = (int32_t)box_local_y_q6;
198 if (box_local_motion_y > 0)
200 if (!(expanded_box_local_y - box_half_extent_y - box_sweep_radius <= 0 && (int32_t)box_local_motion_y + (int32_t)box_local_y_q6 + box_half_extent_y + box_sweep_radius >= 0))
205 if (!(expanded_box_local_y + box_half_extent_y + box_sweep_radius >= 0))
208 if ((int32_t)box_local_motion_y - box_half_extent_y - box_sweep_radius + (int32_t)box_local_y_q6 > 0)
213 int32_t box_local_z_q6 =
214 (int32_t)(int16_t)((box_rotation_m20 * box_relative_x_q6 + box_rotation_m21 * box_relative_y_q6 + box_rotation_m22 * box_relative_z_q6) >>
MATH_Q12_SHIFT);
216 if (box_local_motion_z > 0)
218 if (!(box_local_z_q6 - box_half_extent_z - box_sweep_radius <= 0 && (int32_t)box_local_motion_z + box_local_z_q6 + box_half_extent_z + box_sweep_radius >= 0))
223 if (!(box_local_z_q6 + box_half_extent_z + box_sweep_radius >= 0))
226 if ((int32_t)box_local_motion_z - box_half_extent_z - box_sweep_radius + box_local_z_q6 > 0)
237 int32_t source_origin_y = source_node->
origin.
y;
238 int32_t source_origin_z = source_node->
origin.
z;
240 origin_x_or_packed_motion_xy = actor_world_x - source_node->
origin.
x;
242 origin_y_or_motion_z_bits = actor_world_y - source_origin_y;
243 relative_origin_z = actor_world_z - source_origin_z;
248 int16_t mode_or_rotation_element_bits;
250 mode_or_rotation_element_bits = (uint8_t)resolver_mode;
254 __builtin_memcpy(&source_transform_snapshot, &source_node->
transform_matrix, 0x20);
271 int32_t rotated_resolver_mode = resolver_mode;
274 resolver_mode = rotated_resolver_mode;
278 mode_or_rotation_element_bits = source_transform_snapshot.
rotation_q12.
m01;
280 int16_t transpose_corner_element = source_transform_snapshot.
rotation_q12.
m20;
285 int16_t transpose_bottom_element = source_transform_snapshot.
rotation_q12.
m21;
288 world_to_source_rotation.
m10 = mode_or_rotation_element_bits;
289 mode_or_rotation_element_bits = source_transform_snapshot.
rotation_q12.
m12;
290 world_to_source_rotation.
m12 = transpose_bottom_element;
292 world_to_source_rotation.
m21 = mode_or_rotation_element_bits;
294 int32_t saved_relative_origin_x = origin_x_or_packed_motion_xy;
296 world_to_source_rotation.
m22 = transpose_bottom_element;
297 world_to_source_rotation.
m02 = transpose_corner_element;
299 world_to_source_rotation.
m11 =
m11;
300 world_to_source_rotation.
m20 = transpose_corner_element;
301 world_to_source_rotation.
m00 =
m00;
303 int32_t source_relative_x_q6 = (int32_t)(int16_t)(saved_relative_origin_x >> 6);
304 int32_t source_relative_y_q6 = (int32_t)(int16_t)(origin_y_or_motion_z_bits >> 6);
305 int32_t source_relative_z_q6 = (int32_t)(int16_t)(relative_origin_z >> 6);
306 int16_t source_local_x_q6 = (int16_t)(((int32_t)
m00 * source_relative_x_q6 + (int32_t)source_transform_snapshot.
rotation_q12.
m10 * source_relative_y_q6 + (int32_t)source_transform_snapshot.
rotation_q12.
m20 * source_relative_z_q6) >>
MATH_Q12_SHIFT);
307 int16_t source_local_y_q6 = (int16_t)((
308 (int32_t)source_transform_snapshot.
rotation_q12.
m01 * source_relative_x_q6 + (int32_t)source_transform_snapshot.
rotation_q12.
m11 * source_relative_y_q6 + (int32_t)source_transform_snapshot.
rotation_q12.
m21 * source_relative_z_q6) >>
310 int16_t source_local_z_q6 = (int16_t)((
311 (int32_t)source_transform_snapshot.
rotation_q12.
m02 * source_relative_x_q6 + (int32_t)source_transform_snapshot.
rotation_q12.
m12 * source_relative_y_q6 + (int32_t)source_transform_snapshot.
rotation_q12.
m22 * source_relative_z_q6) >>
324 response_scratch = (intptr_t)&actor->
velocity;
329 local_motion_x = local_motion.
x;
331 int32_t local_motion_y = local_motion.
y;
332 int32_t initial_local_motion_z = local_motion.
z;
333 int32_t local_motion_z = local_motion.
z;
335 origin_y_or_motion_z_bits = (int32_t)(((uint32_t)origin_y_or_motion_z_bits & (~(uint32_t)UINT16_MAX)) | (uint16_t)((int16_t)(initial_local_motion_z >> 6)));
336 origin_x_or_packed_motion_xy = (int32_t)(((uint32_t)origin_x_or_packed_motion_xy & (~(uint32_t)UINT16_MAX)) | (uint16_t)((int16_t)(local_motion_x >> 6)));
337 origin_x_or_packed_motion_xy = (int32_t)(((uint32_t)origin_x_or_packed_motion_xy & ((uint32_t)UINT16_MAX)) | ((uint32_t)(uint16_t)((int16_t)(local_motion_y >> 6)) << 16));
339 int32_t polygon_flags_or_contact_work_bits;
341 int32_t radius_margin_squared_q12 = (int32_t)(((int64_t)radius_margin * radius_margin) >>
MATH_Q12_SHIFT);
345 bool polygon_rejected =
false;
348 __builtin_memcpy(&polygon_flags_or_contact_work_bits, &source_polygon->
flags,
sizeof(source_polygon->
flags));
351 uint32_t pending_polygon_flags = (uint32_t)(uint16_t)polygon_flags_or_contact_work_bits;
352 uint32_t effective_polygon_flags = pending_polygon_flags;
353 uint8_t resolver_mode_byte;
356 resolver_mode_byte = (uint8_t)resolver_mode;
359 resolver_mode_byte = (uint8_t)resolver_mode;
364 effective_polygon_flags = pending_polygon_flags;
369 int32_t world_face_normal_y_q12;
370 bool test_original_face;
375 world_face_normal_y_q12 = 0;
376 test_original_face =
true;
384 resolver_mode_byte = (uint8_t)resolver_mode;
385 face_plane_slot = &source_polygon->
face_plane;
387 source_polygon = polygons;
391 face_plane_slot = &source_polygon->
face_plane;
395 polygon_flags_or_contact_work_bits = world_face_normal_y_q12;
402 polygon_flags_or_contact_work_bits = (int32_t)(actor);
411 int16_t vertex_index_or_local_x_bits;
413 if (!test_original_face)
415 int32_t reversed_world_normal_y = -(world_face_normal_y_q12);
417 world_face_normal_y_q12 = reversed_world_normal_y;
428 polygon_flags_or_contact_work_bits = (int32_t)(&reversed_polygon);
430 if (collision_poly == &reversed_polygon)
436 effective_polygon_flags = pending_polygon_flags;
437 reversed_polygon.
flags = (uint16_t)pending_polygon_flags;
439 uint16_t *reverse_vertex_cursor = &collision_poly->
vertex_idx[3];
440 uint16_t *reversed_vertex_cursor = reversed_polygon.
vertex_idx;
441 int32_t reverse_vertices_remaining = 4;
442 int32_t reverse_count_before_advance;
446 vertex_index_or_local_x_bits = *reverse_vertex_cursor;
447 --reverse_vertex_cursor;
448 *reversed_vertex_cursor = vertex_index_or_local_x_bits;
449 ++reversed_vertex_cursor;
450 reverse_count_before_advance = reverse_vertices_remaining;
451 reverse_vertices_remaining -= 1;
452 }
while (reverse_count_before_advance != 1);
455 -((*face_plane_slot)->normal.x);
457 -((*face_plane_slot)->normal.y);
459 -((*face_plane_slot)->normal.z);
461 -((*face_plane_slot)->plane_distance);
462 collision_poly = &reversed_polygon;
465 test_original_face =
false;
469 int32_t face_normal_z = (int32_t)tested_face_plane->
normal.
z;
470 int32_t face_normal_x = (int32_t)tested_face_plane->
normal.
x;
471 int32_t face_normal_y = (int32_t)tested_face_plane->
normal.
y;
472 int32_t normal_motion_projection_q4 = ((int32_t)(int16_t)origin_y_or_motion_z_bits * face_normal_z + (int32_t)(int16_t)origin_x_or_packed_motion_xy * face_normal_x + (int32_t)(int16_t)((uint32_t)origin_x_or_packed_motion_xy >> 16) * face_normal_y) >> 2;
476 if (polygon_flags_or_contact_work_bits > 0)
478 int32_t expanded_source_local_z = (int32_t)source_local_z_q6;
479 int32_t expanded_source_local_y = (int32_t)source_local_y_q6;
480 int32_t expanded_source_local_x = (int32_t)source_local_x_q6;
482 int32_t source_plane_distance = ((expanded_source_local_z * face_normal_z +
483 expanded_source_local_y * face_normal_y + expanded_source_local_x * face_normal_x) >>
486 if ((uint32_t)source_plane_distance <= (uint32_t)polygon_flags_or_contact_work_bits)
490 polygon_flags_or_contact_work_bits = (int32_t)(((uint32_t)polygon_flags_or_contact_work_bits & (~(uint32_t)UINT8_MAX)) | (uint8_t)(collision_poly->
flags));
492 int32_t last_vertex_index = 0;
497 uint16_t *edge_vertex_index_cursor = collision_poly->
vertex_idx;
498 struct Collision_Vertex *previous_edge_position = &edge_vertices[last_vertex_index];
499 int16_t saved_relative_or_corner_x;
500 int16_t relative_or_corner_y;
501 int16_t relative_or_corner_z;
502 int32_t edge_or_clip_normal_x;
503 int32_t edge_or_clip_normal_z;
504 int32_t edge_normal_y;
508 int32_t current_edge_vertex_index = 0;
510 current_edge_vertex_index = *edge_vertex_index_cursor;
511 ++edge_vertex_index_cursor;
513 struct Collision_Vertex *current_edge_position = &edge_vertices[current_edge_vertex_index];
516 int32_t edge_face_normal_x = (int32_t)edge_face_plane->
normal.
x;
517 int32_t edge_face_normal_y = (int32_t)edge_face_plane->
normal.
y;
518 int32_t edge_face_normal_z = (int32_t)edge_face_plane->
normal.
z;
519 int32_t edge_delta_y = (int32_t)(int16_t)(current_edge_position->
pos.
y - previous_edge_position->
pos.
y);
520 int32_t edge_delta_z = (int32_t)(int16_t)(current_edge_position->
pos.
z - previous_edge_position->
pos.
z);
521 int32_t edge_delta_x = (int32_t)(int16_t)((uint16_t)edge_vertices[current_edge_vertex_index].
pos.
x - (uint16_t)previous_edge_position->
pos.
x);
522 int32_t edge_normal_z = (edge_delta_y * edge_face_normal_x - edge_delta_x * edge_face_normal_y) >> 6;
524 polygon_flags_or_contact_work_bits = (edge_delta_z * edge_face_normal_y - edge_delta_y * edge_face_normal_z) >> 6;
526 (edge_delta_x * edge_face_normal_z - edge_delta_z * edge_face_normal_x) >> 6;
527 edge_or_clip_normal_x = polygon_flags_or_contact_work_bits;
528 edge_or_clip_normal_z = edge_normal_z;
529 vertex_index_or_local_x_bits = source_local_x_q6 - (uint16_t)previous_edge_position->
pos.
x;
530 relative_or_corner_y = source_local_y_q6 - previous_edge_position->
pos.
y;
532 int16_t relative_vertex_z = source_local_z_q6 - previous_edge_position->
pos.
z;
534 saved_relative_or_corner_x = vertex_index_or_local_x_bits;
535 relative_or_corner_z = relative_vertex_z;
537 int32_t edge_plane_distance = ((int32_t)relative_vertex_z * edge_normal_z + (int32_t)relative_or_corner_y * edge_normal_y + (int32_t)vertex_index_or_local_x_bits * polygon_flags_or_contact_work_bits) >> 6;
538 int32_t saved_edge_plane_distance = edge_plane_distance;
540 if (edge_plane_distance >= 0)
541 edge_vertices = vertices;
544 int32_t edge_radius_squared_bound;
548 int32_t edge_length_squared = (int32_t)(((int64_t)edge_or_clip_normal_x * edge_or_clip_normal_x + (int64_t)edge_normal_y * edge_normal_y + (int64_t)edge_or_clip_normal_z * edge_or_clip_normal_z) >>
MATH_Q12_SHIFT);
549 int32_t radius_bound = (int32_t)(((int64_t)edge_length_squared * radius_margin_squared_q12) >>
MATH_Q12_SHIFT);
551 edge_vertices = vertices;
552 vertex_index_or_local_x_bits = saved_relative_or_corner_x;
553 edge_plane_distance = saved_edge_plane_distance;
554 edge_radius_squared_bound = radius_bound;
558 edge_vertices = vertices;
559 edge_radius_squared_bound = 0;
562 if ((int32_t)(((int64_t)saved_edge_plane_distance * saved_edge_plane_distance) >>
MATH_Q12_SHIFT) > edge_radius_squared_bound)
564 polygon_flags_or_contact_work_bits = ((int32_t)(int16_t)origin_y_or_motion_z_bits * edge_or_clip_normal_z + (int32_t)(int16_t)((uint32_t)origin_x_or_packed_motion_xy >> 16) * edge_normal_y + (int32_t)(int16_t)origin_x_or_packed_motion_xy * edge_or_clip_normal_x) >> 6;
566 int32_t swept_edge_plane_distance = edge_plane_distance + polygon_flags_or_contact_work_bits;
568 if (edge_plane_distance + polygon_flags_or_contact_work_bits < 0)
570 if ((int32_t)(((int64_t)swept_edge_plane_distance * swept_edge_plane_distance) >>
MATH_Q12_SHIFT) > edge_radius_squared_bound)
572 polygon_rejected =
true;
580 bool has_more_edges = edges_remaining != 1;
582 edges_remaining -= 1;
587 previous_edge_position = current_edge_position;
590 if (polygon_rejected)
600 polygon_flags_or_contact_work_bits = (int32_t)(((uint32_t)polygon_flags_or_contact_work_bits & (~(uint32_t)UINT16_MAX)) | (uint16_t)(face_plane->
normal.
x));
602 int16_t y = face_plane->
normal.
y;
604 accumulated_normal.x += (uint16_t)polygon_flags_or_contact_work_bits;
605 polygon_flags_or_contact_work_bits = (int32_t)(((uint32_t)polygon_flags_or_contact_work_bits & (~(uint32_t)UINT16_MAX)) | (uint16_t)(face_plane->
normal.
z));
606 accumulated_normal.y += y;
607 accumulated_normal.z += (uint16_t)polygon_flags_or_contact_work_bits;
612 int32_t normal_accumulation_mode = resolver_mode;
615 resolver_mode = normal_accumulation_mode;
620 polygon_flags_or_contact_work_bits = (int32_t)((uint32_t)(uint16_t)first_accumulated_plane->
normal.
x |
621 ((uint32_t)(uint16_t)first_accumulated_plane->
normal.
y << 16));
622 accumulated_normal.x = (int16_t)polygon_flags_or_contact_work_bits;
623 accumulated_normal.y = (int16_t)((uint32_t)polygon_flags_or_contact_work_bits >> 16);
627 normal_z_padding_copy.
normal.
z = first_accumulated_plane->
normal.
z;
630 accumulated_normal.z = normal_z_padding_copy.
normal.
z;
631 accumulated_normal.padding =
637 int32_t box_initialization_mode = resolver_mode;
650 resolver_mode = box_initialization_mode;
653 actor_collision_shape_node =
656 accumulated_normal.z = 0;
657 accumulated_normal.y = 0;
658 accumulated_normal.x = 0;
660 int16_t sphere_extents[3];
661 int16_t *saved_box_half_extents;
662 int16_t *box_half_extents;
664 if (!actor_collision_shape_node)
667 saved_box_half_extents = box_half_extents;
674 sphere_extents[0] = sphere_extents[1] = sphere_extents[2] = extent;
675 box_half_extents = saved_box_half_extents = sphere_extents;
679 box_half_extents = (int16_t *)(&actor_collision_shape_node->
shape);
680 saved_box_half_extents = box_half_extents;
686 &actor->
rot_mat, &world_to_source_rotation);
688 int32_t box_corner_index = 0;
692 switch (box_corner_index)
696 saved_relative_or_corner_x =
697 -(box_half_extents[0]);
698 vertex_index_or_local_x_bits = saved_relative_or_corner_x;
699 relative_or_corner_y =
700 -(box_half_extents[1]);
701 relative_or_corner_z =
702 -(box_half_extents[2]);
709 relative_or_corner_z =
717 relative_or_corner_y =
725 relative_or_corner_z =
726 -(box_half_extents[2]);
733 (int32_t)relative_or_corner_y;
734 int32_t corner_x = (int32_t)vertex_index_or_local_x_bits;
736 (int32_t)relative_or_corner_z;
737 int32_t corner_x_rotated_y_term = corner_x * (int32_t)actor_to_source_rotation.
m10;
738 int32_t actor_rotation_m11 =
739 (int32_t)actor_to_source_rotation.
m11;
741 actor_box_corners[box_corner_index].
position_q6.
x = (int16_t)((corner_x * (int32_t)actor_to_source_rotation.
m00 + corner_y * (int32_t)actor_to_source_rotation.
m01 + corner_z * (int32_t)actor_to_source_rotation.
m02) >>
MATH_Q12_SHIFT);
743 int32_t corner_x_rotated_z_term = corner_x * (int32_t)actor_to_source_rotation.
m20;
745 actor_box_corners[box_corner_index].
position_q6.
y = (int16_t)((corner_x_rotated_y_term + corner_y * actor_rotation_m11 + corner_z * (int32_t)actor_to_source_rotation.
m12) >>
MATH_Q12_SHIFT);
746 actor_box_corners[box_corner_index].
position_q6.
z = (int16_t)((corner_x_rotated_z_term + corner_y * (int32_t)actor_to_source_rotation.
m21 + corner_z * (int32_t)actor_to_source_rotation.
m22) >>
MATH_Q12_SHIFT);
747 actor_box_corners[box_corner_index - ((box_corner_index & 1) << 1) + 5].position_q6.x = source_local_x_q6 - actor_box_corners[box_corner_index].
position_q6.
x;
748 actor_box_corners[box_corner_index - ((box_corner_index & 1) << 1) + 5].position_q6.y = source_local_y_q6 - actor_box_corners[box_corner_index].
position_q6.
y;
749 actor_box_corners[box_corner_index - ((box_corner_index & 1) << 1) + 5].position_q6.z = source_local_z_q6 - actor_box_corners[box_corner_index].
position_q6.
z;
756 box_corner_index += 1;
758 if (box_corner_index >= 4)
761 box_half_extents = saved_box_half_extents;
762 vertex_index_or_local_x_bits = saved_relative_or_corner_x;
766 const int16_t *rotation_elements = (
const int16_t *)&actor_to_source_rotation;
768 for (
int pair = 0; pair < 3; ++pair)
770 int axis = pair < 2 ? (pair ^ 1) : pair;
774 near_plane->
normal.
x = rotation_elements[axis];
775 near_plane->
normal.
y = rotation_elements[3 + axis];
776 near_plane->
normal.
z = rotation_elements[6 + axis];
778 int32_t center_projection = (near_plane->
normal.
x * expanded_source_local_x + near_plane->
normal.
y * expanded_source_local_y + near_plane->
normal.
z * expanded_source_local_z) >> 2;
779 int32_t extent = -(int32_t)saved_box_half_extents[axis] * 1024;
781 near_plane->
distance = extent - center_projection;
782 far_plane->
distance = extent + center_projection;
788 edge_vertices = vertices;
794 int32_t response_motion_z = initial_normal_bias_z;
798 response_motion_x += local_motion_x;
799 response_motion_y += local_motion_y;
800 response_motion_z = initial_normal_bias_z + local_motion_z;
803 int32_t clip_source_vertex_zero = 0;
805 clip_source_vertex_zero = collision_poly->
vertex_idx[0];
810 &edge_vertices[clip_source_vertex_zero].
pos.
x,
sizeof(uint32_t));
812 int32_t clip_source_vertex_one = 0;
814 clip_source_vertex_one = collision_poly->
vertex_idx[1];
816 &edge_vertices[clip_source_vertex_zero].
pos.
z,
sizeof(uint32_t));
819 &edge_vertices[clip_source_vertex_one].
pos.
x,
sizeof(uint32_t));
821 &edge_vertices[clip_source_vertex_one].
pos.
z,
sizeof(uint32_t));
823 int32_t clip_source_vertex_two = 0;
825 clip_source_vertex_two = collision_poly->
vertex_idx[2];
828 &edge_vertices[clip_source_vertex_two].
pos.
x,
sizeof(uint32_t));
830 int32_t clip_source_vertex_three = 0;
832 clip_source_vertex_three = collision_poly->
vertex_idx[3];
834 &edge_vertices[clip_source_vertex_two].
pos.
z,
sizeof(uint32_t));
837 &edge_vertices[clip_source_vertex_three].
pos.
x,
sizeof(uint32_t));
839 int32_t next_box_plane_index = 5;
842 __builtin_memcpy(&clip_vertices[3].position_q6.z,
843 &edge_vertices[clip_source_vertex_three].
pos.
z,
sizeof(uint32_t));
844 polygon_flags_or_contact_work_bits = edge_normal_y;
846 int32_t box_plane_index = 5;
848 int32_t plane_motion_or_contact_fraction;
849 int32_t response_z_or_next_clip_distance;
859 response_z_or_next_clip_distance = response_motion_z;
860 plane_motion_or_contact_fraction = ((int32_t)opposite->
normal.
x * response_motion_x + (int32_t)opposite->
normal.
y * response_motion_y + (int32_t)opposite->
normal.
z * response_motion_z) >> 8;
862 if (!(box_plane_index & 1))
863 plane_motion_or_contact_fraction = -plane_motion_or_contact_fraction;
874 int32_t previous_box_plane_index = box_plane_index;
876 next_box_plane_index = box_plane_index - 1;
877 selected_box_plane = saved_box_plane - 1;
878 box_plane_index = next_box_plane_index;
879 saved_box_plane = selected_box_plane;
881 if (!previous_box_plane_index)
883 polygon_rejected =
true;
889 if (polygon_rejected)
892 clip_vertices[0].
next = &clip_vertices[1];
893 clip_vertices[1].
next = &clip_vertices[2];
897 clip_vertices[2].
next = &clip_vertices[3];
904 int32_t clip_plane_side = 0;
906 clip_ring_head = clip_vertices;
908 int32_t saved_clip_plane_side = 0;
910 while (clip_plane_side < 4)
912 int32_t saved_clip_normal_y;
913 int32_t clip_normal_x;
915 if (!((uint8_t)clip_plane_side & 1))
917 int32_t box_edge_start_index = 0;
918 int32_t box_edge_remap_offset = (clip_plane_side >> 1) + (box_plane_index << 2);
919 int32_t box_edge_end_index = 0;
922 box_edge_start_index =
925 int32_t box_edge_delta_y = (int32_t)(int16_t)(actor_box_corners[box_edge_end_index].
position_q6.
y -
927 int32_t box_edge_delta_z = (int32_t)(int16_t)(actor_box_corners[box_edge_end_index].
position_q6.
z -
929 int32_t box_edge_delta_x = (int32_t)(int16_t)(actor_box_corners[box_edge_end_index].
position_q6.
x -
931 int32_t saved_response_motion_x = response_motion_x;
934 (box_edge_delta_z * response_motion_y - box_edge_delta_y * response_z_or_next_clip_distance) >> 6;
935 polygon_flags_or_contact_work_bits =
936 (box_edge_delta_x * response_z_or_next_clip_distance - box_edge_delta_z * saved_response_motion_x) >> 6;
938 int32_t clip_normal_z =
939 (box_edge_delta_y * saved_response_motion_x - box_edge_delta_x * response_motion_y) >> 6;
941 edge_or_clip_normal_x = clip_normal_x;
946 polygon_flags_or_contact_work_bits >>= 1;
950 edge_or_clip_normal_z = clip_normal_z;
951 clip_plane_side = saved_clip_plane_side;
952 saved_clip_normal_y = polygon_flags_or_contact_work_bits;
953 edge_or_clip_normal_x = clip_normal_x;
957 clip_normal_x = -(edge_or_clip_normal_x);
958 polygon_flags_or_contact_work_bits = -(polygon_flags_or_contact_work_bits);
959 edge_or_clip_normal_x = clip_normal_x;
960 saved_clip_normal_y = polygon_flags_or_contact_work_bits;
961 edge_or_clip_normal_z = -(edge_or_clip_normal_z);
964 int32_t next_side_or_anchor_offset = clip_plane_side + 1;
965 int32_t next_clip_plane_side = next_side_or_anchor_offset;
967 next_side_or_anchor_offset &= ~5;
969 int32_t clip_plane_anchor_index = 0;
971 clip_plane_anchor_index =
976 int32_t clip_plane_offset = -(((
977 (int32_t)actor_box_corners[clip_plane_anchor_index].
position_q6.
z * edge_or_clip_normal_z +
978 (int32_t)actor_box_corners[clip_plane_anchor_index].
position_q6.
y * polygon_flags_or_contact_work_bits +
979 (int32_t)actor_box_corners[clip_plane_anchor_index].
position_q6.
x * clip_normal_x) >>
982 int32_t clip_distance_or_fraction = (((int32_t)clip_vertex->
position_q6.
z * edge_or_clip_normal_z + (int32_t)clip_vertex->
position_q6.
y * polygon_flags_or_contact_work_bits + (int32_t)clip_vertex->
position_q6.
x * clip_normal_x) >> 2) + clip_plane_offset;
983 int32_t saved_clip_distance = clip_distance_or_fraction;
988 next_clip_vertex_in_ring =
990 response_z_or_next_clip_distance = (((int32_t)next_clip_vertex_in_ring->
position_q6.
y * polygon_flags_or_contact_work_bits + (int32_t)next_clip_vertex_in_ring->
position_q6.
z * edge_or_clip_normal_z + (int32_t)next_clip_vertex_in_ring->
position_q6.
x * edge_or_clip_normal_x) >> 2) + clip_plane_offset;
992 if ((response_z_or_next_clip_distance ^ clip_distance_or_fraction) < 0)
997 if (response_z_or_next_clip_distance >= 0)
999 clip_distance_or_fraction =
1000 ((int64_t)response_z_or_next_clip_distance *
MATH_Q12_ONE) / (response_z_or_next_clip_distance - clip_distance_or_fraction);
1004 next_clip_vertex->
next =
1005 next_clip_vertex_in_ring;
1007 if (first_intersection)
1009 splice_successor = next_clip_vertex;
1010 splice_predecessor = first_intersection;
1011 splice_predecessor->
next =
1013 polygon_flags_or_contact_work_bits = saved_clip_normal_y;
1020 clip_distance_or_fraction = ((int64_t)saved_clip_distance *
MATH_Q12_ONE) /
1021 (clip_distance_or_fraction - response_z_or_next_clip_distance);
1028 if (first_intersection)
1030 splice_successor = first_intersection;
1031 splice_predecessor = next_clip_vertex;
1032 splice_predecessor->
next =
1034 polygon_flags_or_contact_work_bits = saved_clip_normal_y;
1040 first_intersection = next_clip_vertex;
1044 polygon_flags_or_contact_work_bits = saved_clip_normal_y;
1045 clip_distance_or_fraction = response_z_or_next_clip_distance;
1046 saved_clip_distance = clip_distance_or_fraction;
1047 clip_vertex = next_clip_vertex_in_ring;
1048 }
while (next_clip_vertex_in_ring != clip_ring_start);
1050 if (!first_intersection)
1052 if (clip_distance_or_fraction < 0)
1055 clip_plane_side = next_clip_plane_side;
1056 response_z_or_next_clip_distance = response_motion_z;
1057 clip_ring_head = clip_vertex;
1058 saved_clip_plane_side = clip_plane_side;
1062 response_z_or_next_clip_distance = response_motion_z;
1063 clip_ring_head = next_clip_vertex;
1065 clip_plane_side = next_clip_plane_side;
1066 saved_clip_plane_side = clip_plane_side;
1070 if (clip_plane_side < 4)
1072 int32_t previous_box_plane_index = box_plane_index;
1074 next_box_plane_index = box_plane_index - 1;
1075 selected_box_plane = saved_box_plane - 1;
1076 box_plane_index = next_box_plane_index;
1077 saved_box_plane = selected_box_plane;
1079 if (!previous_box_plane_index)
1081 polygon_rejected =
true;
1092 if (polygon_rejected)
1101 int32_t clipped_vertex_y =
1103 int32_t clipped_vertex_x =
1105 int32_t clipped_vertex_plane_distance = ((clipped_vertex_x * (int32_t)actor_box_planes[box_plane_index].
normal.
x + clipped_vertex_y * (int32_t)actor_box_planes[box_plane_index].
normal.
y + (int32_t)clipped_vertex->
position_q6.
z * (int32_t)actor_box_planes[box_plane_index].
normal.
z) >> 2) + actor_box_planes[box_plane_index].distance;
1109 if (!(clipped_vertex_plane_distance < minimum_contact_depth))
1114 if (clipped_vertex == clip_ring_head)
1116 if (!(clipped_vertex_plane_distance < minimum_contact_depth))
1124 int32_t alternate_plane_pair = box_plane_index & 0xfffffffe;
1126 if ((uint32_t)(-(clipped_vertex_plane_distance)) <= (uint32_t)
COLLISION_PLANE_TOLERANCE_Q4 - actor_box_planes[1 + alternate_plane_pair].distance - actor_box_planes[alternate_plane_pair].distance)
1128 alternate_plane_pair += 2;
1130 if (alternate_plane_pair == 6)
1131 alternate_plane_pair = 0;
1134 actor_box_planes[alternate_plane_pair].
distance;
1135 int32_t clipped_vertex_z = (int32_t)saved_clipped_vertex->
position_q6.
z;
1138 int32_t projected_distance = ((normal->
x * clipped_vertex_x +
1139 normal->
y * clipped_vertex_y + normal->
z * clipped_vertex_z) >>
1143 actor_box_planes[1 + alternate_plane_pair].distance - distance;
1144 if (0u - (uint32_t)projected_distance > plane_span)
1146 clipped_vertex = saved_clipped_vertex;
1152 int32_t second_alternate_plane_pair = alternate_plane_pair + 2;
1154 if (second_alternate_plane_pair == 6)
1155 second_alternate_plane_pair = 0;
1157 const struct Collision_FacePlane *test_plane = &actor_box_planes[second_alternate_plane_pair];
1159 clipped_vertex = saved_clipped_vertex;
1163 if (0u - (uint32_t)(projection + test_plane->
distance) >
1171 if (clipped_vertex == clip_ring_head)
1173 if (!(clipped_vertex_plane_distance < minimum_contact_depth))
1180 minimum_contact_depth = clipped_vertex_plane_distance;
1182 if (clipped_vertex_plane_distance < 0)
1187 minimum_contact_depth = 0;
1189 }
while ((clipped_vertex = clipped_vertex->
next,
1190 saved_clipped_vertex = clipped_vertex,
1191 clipped_vertex != clip_ring_head));
1193 polygon_flags_or_contact_work_bits = (int32_t)clipped_vertex;
1198 int32_t resolved_contact_fraction = 0;
1200 resolved_contact_fraction =
1201 ((int64_t)minimum_contact_depth *
MATH_Q12_ONE) / plane_motion_or_contact_fraction;
1203 int32_t collision_depth = resolved_contact_fraction;
1205 plane_motion_or_contact_fraction = collision_depth;
1209 struct Math_Vec3I16 *selected_response_normal = response_normal;
1219 if (collision_class > 0)
1221 polygon_flags_or_contact_work_bits = (int32_t)actor->
scale_factor;
1223 int32_t collision_callback_result;
1226 if (collision_depth > polygon_flags_or_contact_work_bits)
1228 collision_callback_result = response_scratch;
1229 response_source = collision_source;
1233 if (collision_depth)
1236 if (collision_callback_result >= 0)
1239 response_source = collision_source;
1245 if (collision_depth > (int32_t)((
struct Actor_State *)response_source)->scale_factor)
1249 int32_t)(int8_t)((
struct Actor_State *)response_source)
1251 response_source, actor,
1261 response_source = collision_source;
1264 response_source, collision_poly,
1266 response_scratch = collision_callback_result;
1268 if (collision_callback_result <= 0)
1270 if (collision_callback_result < 0)
1272 (uint16_t)collision_depth;
1276 if (collision_depth)
1279 if (collision_callback_result >= 0)
1282 response_source = collision_source;
1292 int32_t)(int8_t)((
struct Actor_State *)response_source)
1294 response_source, actor,
1309 int32_t response_world_normal_y =
1310 world_face_normal_y_q12;
1313 response_world_normal_y = ((int32_t)response_normal->
z * (int32_t)actor_box_planes[0].
normal.
z + (int32_t)response_normal->
y * (int32_t)actor_box_planes[0].
normal.
y + (int32_t)actor_box_planes[0].
normal.
x * (int32_t)response_normal->
x) >>
MATH_Q12_SHIFT;
1315 if (response_world_normal_y < 0)
1317 projected_face_normal.
x = response_normal->
x - (int16_t)(((int32_t)world_to_source_rotation.
m01 * response_world_normal_y) >>
MATH_Q12_SHIFT);
1318 projected_face_normal.
y = response_normal->
y - (int16_t)(((int32_t)world_to_source_rotation.
m11 * response_world_normal_y) >>
MATH_Q12_SHIFT);
1319 projected_face_normal.
z = response_normal->
z - (int16_t)(((int32_t)world_to_source_rotation.
m21 * response_world_normal_y) >>
MATH_Q12_SHIFT);
1320 selected_response_normal = &projected_face_normal;
1322 int32_t length =
Math_SqrtFP12(((int32_t)projected_face_normal.
x * projected_face_normal.
x + (int32_t)projected_face_normal.
y * projected_face_normal.
y + (int32_t)projected_face_normal.
z * projected_face_normal.
z) >>
MATH_Q12_SHIFT);
1326 int32_t inverse = (int32_t)(INT64_C(0x100000000) / length);
1328 projected_face_normal.
x = (int16_t)(((int64_t)inverse * projected_face_normal.
x) >> 20);
1329 projected_face_normal.
y = (int16_t)(((int64_t)inverse * projected_face_normal.
y) >> 20);
1330 projected_face_normal.
z = (int16_t)(((int64_t)inverse * projected_face_normal.
z) >> 20);
1334 projected_face_normal.
x = projected_face_normal.
y = projected_face_normal.
z = 0;
1342 response_motion_z = projected_normal_bias_z;
1346 response_motion_x += local_motion_x;
1347 response_motion_y += local_motion_y;
1348 response_motion_z = projected_normal_bias_z + local_motion_z;
1351 response_normal = selected_response_normal;
1356 int32_t within_penetration_tolerance = 0;
1363 int32_t normal_motion = (int32_t)(((int64_t)selected_response_normal->
x * response_motion_x + (int64_t)selected_response_normal->
y * response_motion_y + (int64_t)selected_response_normal->
z * response_motion_z) >>
MATH_Q12_SHIFT);
1366 response_normal = selected_response_normal;
1369 response_scratch = correction_distance;
1370 response_motion_x = (int32_t)(((int64_t)correction_distance * response_normal->
x) >>
MATH_Q12_SHIFT);
1372 int32_t correction_y = (int32_t)(((int64_t)correction_distance * response_normal->
y) >>
MATH_Q12_SHIFT);
1373 int32_t correction_z = (int32_t)(((int64_t)correction_distance * response_normal->
z) >>
MATH_Q12_SHIFT);
1377 int32_t correction_length_squared = (int32_t)(((int64_t)response_motion_x * response_motion_x + (int64_t)correction_y * correction_y + (int64_t)correction_z * correction_z) >>
MATH_Q12_SHIFT);
1381 if (!plane_motion_or_contact_fraction && minimum_contact_depth >= 0)
1382 plane_motion_or_contact_fraction = 1;
1384 polygon_flags_or_contact_work_bits =
1387 if (plane_motion_or_contact_fraction < polygon_flags_or_contact_work_bits)
1392 polygon_flags_or_contact_work_bits = (int32_t)(actor);
1394 (uint16_t)plane_motion_or_contact_fraction;
1400 if (minimum_contact_depth < 0)
1403 int32_t corrected_motion_y = local_motion_y + correction_y;
1404 int32_t corrected_motion_x = local_motion_x + response_motion_x;
1405 int32_t corrected_motion_z = local_motion_z + correction_z;
1407 local_motion_x = corrected_motion_x;
1408 local_motion_y = corrected_motion_y;
1409 local_motion_z = corrected_motion_z;
1411 polygon_flags_or_contact_work_bits = corrected_motion_y >> 6;
1412 origin_x_or_packed_motion_xy = (int32_t)(((uint32_t)origin_x_or_packed_motion_xy & (~(uint32_t)UINT16_MAX)) | (uint16_t)((int16_t)(corrected_motion_x >> 6)));
1413 origin_x_or_packed_motion_xy = (int32_t)(((uint32_t)origin_x_or_packed_motion_xy & ((uint32_t)UINT16_MAX)) | ((uint32_t)(uint16_t)((uint16_t)polygon_flags_or_contact_work_bits) << 16));
1414 origin_y_or_motion_z_bits = (int32_t)(((uint32_t)origin_y_or_motion_z_bits & (~(uint32_t)UINT16_MAX)) | (uint16_t)((int16_t)(corrected_motion_z >> 6)));
1421 polygon_flags_or_contact_work_bits = (int32_t)(((uint32_t)polygon_flags_or_contact_work_bits & (~(uint32_t)UINT16_MAX)) | (uint16_t)(final_accumulated_plane->
normal.
y));
1423 int16_t accumulated_face_normal_z = final_accumulated_plane->
normal.
z;
1425 accumulated_normal.x += final_accumulated_plane->
normal.
x;
1426 accumulated_normal.y += (uint16_t)polygon_flags_or_contact_work_bits;
1427 accumulated_normal.z += accumulated_face_normal_z;
1433 face_plane_slot = &collision_poly->
face_plane;
1434 resolver_mode_byte = (uint8_t)resolver_mode;
1435 pending_polygon_flags = effective_polygon_flags;
1439 bool has_more_polygons = polygon_count != 1;
1441 polygons = &polygons[1];
1444 if (!has_more_polygons)
1451 (
struct Math_Vec3I32){local_motion_x, local_motion_y, local_motion_z});
1500 (
struct Math_Vec3I32){accumulated_normal.x, accumulated_normal.y, accumulated_normal.z});