102 Dalmatians: Puppies to the Rescue
Decompilation Source Reference | GitLab repository
Loading...
Searching...
No Matches
world.c
Go to the documentation of this file.
1/* Recovered game implementation; original addresses identify provenance. */
14#include "pcdogs/math/types.h"
17#include <math.h>
18#include <stdarg.h>
19#include <string.h>
20
21// Original pcdogs.exe: no direct counterpart (decomp helper)
22// Rotates a 32-bit vector by a Q12 matrix using signed 64-bit products and returns the shifted 32-bit components.
24 struct Math_Vec3I32 v)
25{
26 const int16_t *r = (const int16_t *)m;
27 struct Math_Vec3I32 out;
28
29 out.x = (int32_t)(((int64_t)r[0] * v.x + (int64_t)r[1] * v.y + (int64_t)r[2] * v.z) >> MATH_Q12_SHIFT);
30 out.y = (int32_t)(((int64_t)r[3] * v.x + (int64_t)r[4] * v.y + (int64_t)r[5] * v.z) >> MATH_Q12_SHIFT);
31 out.z = (int32_t)(((int64_t)r[6] * v.x + (int64_t)r[7] * v.y + (int64_t)r[8] * v.z) >> MATH_Q12_SHIFT);
32
33 return out;
34}
35
36// Original pcdogs.exe: no direct counterpart (C body for the x86 polygon resolver entry)
37// Resolves swept actor contact against tagged collision-source geometry, clipping polygons and applying callback policy before committing motion and normals. The source can be a collision node, temporary node prefix or actor with the required compatible layout.
38void Collision_ResolveSweptPolygons(struct Actor_State *actor, struct Collision_Node *collision_source,
39 struct Collision_Vertex *vertices, struct Collision_Polygon *polygons, int32_t polygon_count,
40 int32_t response_scratch)
41{
42 /* Native stack slots EBP-0xd8..-0xd1 persist across all polygons. */
43 struct
44 {
45 int16_t x, y, z, padding;
46 } accumulated_normal = {0};
47
48 struct Collision_FacePlane actor_box_planes[6];
49 struct Math_Vec3I16 projected_face_normal;
50
52 {
53 int32_t debug_polygon_list_index = Collision_DebugPolygonListCount;
54
55 if (debug_polygon_list_index > COLLISION_DEBUG_POLYGON_LIST_CAPACITY - 1)
56 debug_polygon_list_index = 0;
57
58 Collision_DebugPolygonArrays[debug_polygon_list_index] = polygons;
59 Collision_DebugPolygonCounts[debug_polygon_list_index] = polygon_count;
60 Collision_DebugPolygonListCount = debug_polygon_list_index + 1;
61 }
62
63 int32_t cull_radius = (int32_t)actor->cull_radius;
64 struct Collision_Node *source_node = collision_source;
65 uint8_t node_type = source_node->node_type;
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;
71 struct Collision_BoxCornerQ6 actor_box_corners[0x8];
72
73 if (node_type != COLLISION_NODE_ORIENTED_BOX)
74 {
75 int32_t initial_resolver_mode = 0;
76
77 resolver_mode = 0;
78
79 if (node_type == COLLISION_NODE_ACTOR)
80 {
81 int32_t dynamic_source_center_y = (uint32_t)source_node->bounds.sphere.center.y;
82
83 origin_x_or_packed_motion_xy = actor->world_render_pos.x - (uint32_t)source_node->bounds.sphere.center.x;
84 origin_y_or_motion_z_bits = actor->world_render_pos.y - dynamic_source_center_y;
85 relative_origin_z = actor->world_render_pos.z - (uint32_t)source_node->bounds.sphere.center.z;
86 }
87 else
88 {
89 if (node_type == COLLISION_NODE_MESH)
90 {
92 initial_resolver_mode = COLLISION_RESOLVER_VELOCITY_FRAME;
93
94 if (!source_node->collision_offset.x && !source_node->collision_offset.y && !source_node->collision_offset.z)
95 initial_resolver_mode |= COLLISION_RESOLVER_SOURCE_STATIONARY;
96
97 initial_resolver_mode |= COLLISION_RESOLVER_WORLD_SOURCE;
98 resolver_mode = initial_resolver_mode;
99 }
100
101 int32_t relative_origin_x = actor->world_render_pos.x - source_node->origin.x;
102 int32_t relative_origin_y = actor->world_render_pos.y - source_node->origin.y;
103 int32_t unadjusted_origin_z = actor->world_render_pos.z - source_node->origin.z;
104 int32_t adjusted_origin_z;
105
106 if (!((uint8_t)initial_resolver_mode & COLLISION_RESOLVER_VELOCITY_FRAME))
107 {
108 int32_t source_offset_z = source_node->collision_offset.z;
109 int32_t offset_origin_y = relative_origin_y + source_node->collision_offset.y;
110
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;
114 }
115 else
116 {
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;
120
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;
124 }
125
126 relative_origin_z = adjusted_origin_z;
127 }
128 }
129 else
130 {
131 int32_t actor_world_x = actor->world_render_pos.x;
132 int32_t actor_world_y = actor->world_render_pos.y;
133 int32_t box_center_y = (uint32_t)source_node->bounds.oriented_box.center.y;
134 int32_t actor_world_z = actor->world_render_pos.z;
135 int32_t box_relative_x = actor_world_x - (uint32_t)source_node->bounds.oriented_box.center.x;
136 int32_t box_center_z = (uint32_t)source_node->bounds.oriented_box.center.z;
137
138 origin_x_or_packed_motion_xy = box_relative_x;
139
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);
145 int32_t box_rotation_m12 = (int32_t)source_node->bounds.oriented_box.orientation_q12.m12;
146 int32_t box_rotation_m10 = (int32_t)source_node->bounds.oriented_box.orientation_q12.m10;
147 int16_t box_local_x_q6 = (int16_t)((box_relative_x_q6 * (int32_t)source_node->bounds.oriented_box.orientation_q12.m00 + box_relative_y_q6 * (int32_t)source_node->bounds.oriented_box.orientation_q12.m01 + box_relative_z_q6 * (int32_t)source_node->bounds.oriented_box.orientation_q12.m02) >> MATH_Q12_SHIFT);
148 int32_t box_rotation_m11 = (int32_t)source_node->bounds.oriented_box.orientation_q12.m11;
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);
150 int32_t box_rotation_m22 = (int32_t)source_node->bounds.oriented_box.orientation_q12.m22;
151 int32_t box_rotation_m20 = (int32_t)source_node->bounds.oriented_box.orientation_q12.m20;
152 int32_t box_rotation_m21 = (int32_t)source_node->bounds.oriented_box.orientation_q12.m21;
153 int32_t actor_motion_x = actor->sub_pos.x;
154 int32_t actor_motion_y = actor->sub_pos.y;
155
156 local_motion_x = origin_x_or_packed_motion_xy + actor_motion_x;
157
158 int32_t actor_motion_z = actor->sub_pos.z;
159
160 int32_t box_motion_z_q6 = (int16_t)(actor_motion_z >> 6);
161 response_scratch = box_motion_z_q6;
162
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;
166 int32_t box_half_extent_x = (int32_t)source_node->bounds.oriented_box.half_extents_q6.x;
167 int16_t box_local_motion_x = (int16_t)((box_motion_x_q6 * (int32_t)source_node->bounds.oriented_box.orientation_q12.m00 + box_motion_y_q6 * (int32_t)source_node->bounds.oriented_box.orientation_q12.m01 + box_motion_z_q6 * (int32_t)source_node->bounds.oriented_box.orientation_q12.m02) >> MATH_Q12_SHIFT);
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);
170
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;
174
175 int32_t box_sweep_radius;
176
177 if (box_local_motion_x <= 0)
178 {
179 box_sweep_radius = cull_radius;
180
181 if (!(expanded_box_local_x + box_half_extent_x + box_sweep_radius >= 0))
182 return;
183
184 if ((int32_t)box_local_motion_x - (int32_t)source_node->bounds.oriented_box.half_extents_q6.x - box_sweep_radius + (int32_t)box_local_x_q6 > 0)
185 return;
186 }
187 else
188 {
189 box_sweep_radius = cull_radius;
190
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)
192 return;
193 }
194
195 int32_t box_half_extent_y = (int32_t)source_node->bounds.oriented_box.half_extents_q6.y;
196 int32_t expanded_box_local_y = (int32_t)box_local_y_q6;
197
198 if (box_local_motion_y > 0)
199 {
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))
201 return;
202 }
203 else
204 {
205 if (!(expanded_box_local_y + box_half_extent_y + box_sweep_radius >= 0))
206 return;
207
208 if ((int32_t)box_local_motion_y - box_half_extent_y - box_sweep_radius + (int32_t)box_local_y_q6 > 0)
209 return;
210 }
211 {
212 int32_t box_half_extent_z = (int32_t)source_node->bounds.oriented_box.half_extents_q6.z;
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);
215
216 if (box_local_motion_z > 0)
217 {
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))
219 return;
220 }
221 else
222 {
223 if (!(box_local_z_q6 + box_half_extent_z + box_sweep_radius >= 0))
224 return;
225
226 if ((int32_t)box_local_motion_z - box_half_extent_z - box_sweep_radius + box_local_z_q6 > 0)
227 return;
228 }
229 {
230 if (source_node->flags_high & (COLLISION_NODE_PROXY_GEOMETRY >> 8))
231 {
232 collision_source = (struct Collision_Node *)(vertices);
233 source_node = (struct Collision_Node *)(vertices);
235 }
236
237 int32_t source_origin_y = source_node->origin.y;
238 int32_t source_origin_z = source_node->origin.z;
239
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;
244 }
245 }
246 }
247
248 int16_t mode_or_rotation_element_bits;
249
250 mode_or_rotation_element_bits = (uint8_t)resolver_mode;
251
252 struct Collision_TransformOriginSnapshot source_transform_snapshot;
253
254 __builtin_memcpy(&source_transform_snapshot, &source_node->transform_matrix, 0x20);
255
256 int16_t m11 = source_transform_snapshot.rotation_q12.m11;
257
258 if ((uint8_t)mode_or_rotation_element_bits & COLLISION_RESOLVER_WORLD_SOURCE)
259 {
261 {
262 while (polygon_count && polygons[polygon_count - 1].flags & COLLISION_POLYGON_CAMERA_PROBE_SKIP)
263 polygon_count -= 1;
264 }
265
266 if (!(actor->collision_class != COLLISION_CLASS_CAMERA_PROBE || polygon_count))
267 return;
268
269 if (m11 != MATH_Q12_ONE)
270 {
271 int32_t rotated_resolver_mode = resolver_mode;
272
273 rotated_resolver_mode |= COLLISION_RESOLVER_TRANSFORM_NORMAL;
274 resolver_mode = rotated_resolver_mode;
275 }
276 }
277 {
278 mode_or_rotation_element_bits = source_transform_snapshot.rotation_q12.m01;
279
280 int16_t transpose_corner_element = source_transform_snapshot.rotation_q12.m20;
281 struct Math_Matrix3x3I16 world_to_source_rotation;
282
283 world_to_source_rotation.m01 = source_transform_snapshot.rotation_q12.m10;
284
285 int16_t transpose_bottom_element = source_transform_snapshot.rotation_q12.m21;
286 int16_t m00 = source_transform_snapshot.rotation_q12.m00;
287
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;
291 transpose_bottom_element = source_transform_snapshot.rotation_q12.m22;
292 world_to_source_rotation.m21 = mode_or_rotation_element_bits;
293
294 int32_t saved_relative_origin_x = origin_x_or_packed_motion_xy;
295
296 world_to_source_rotation.m22 = transpose_bottom_element;
297 world_to_source_rotation.m02 = transpose_corner_element;
298 transpose_corner_element = source_transform_snapshot.rotation_q12.m02;
299 world_to_source_rotation.m11 = m11;
300 world_to_source_rotation.m20 = transpose_corner_element;
301 world_to_source_rotation.m00 = m00;
302
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) >>
309 0xc);
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) >>
312 0xc);
313 struct Math_Vec3I32 selected_motion = actor->velocity;
314
315 if (!(resolver_mode & COLLISION_RESOLVER_VELOCITY_FRAME))
316 {
317 selected_motion.x = actor->sub_pos.x - source_node->collision_offset.x;
318 selected_motion.y = actor->sub_pos.y - source_node->collision_offset.y;
319 selected_motion.z = actor->sub_pos.z - source_node->collision_offset.z;
320 }
321 else
322 {
323 // The velocity transform keeps this pointer in the native response scratch slot.
324 response_scratch = (intptr_t)&actor->velocity;
325 }
326
327 struct Math_Vec3I32 local_motion = Collision_RotateVectorQ12(&world_to_source_rotation, selected_motion);
328
329 local_motion_x = local_motion.x;
330
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;
334
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));
338
339 int32_t polygon_flags_or_contact_work_bits;
340 int32_t radius_margin = (cull_radius << 6) + COLLISION_EDGE_SWEEP_PADDING_Q6;
341 int32_t radius_margin_squared_q12 = (int32_t)(((int64_t)radius_margin * radius_margin) >> MATH_Q12_SHIFT);
342
343 while (true)
344 {
345 bool polygon_rejected = false;
346 struct Collision_Polygon *source_polygon = polygons;
347
348 __builtin_memcpy(&polygon_flags_or_contact_work_bits, &source_polygon->flags, sizeof(source_polygon->flags));
349
350 struct Collision_Plane *plane_data = source_polygon->plane_data;
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;
354
355 if (!plane_data)
356 resolver_mode_byte = (uint8_t)resolver_mode;
357 else
358 {
359 resolver_mode_byte = (uint8_t)resolver_mode;
360
361 if (!(pending_polygon_flags & COLLISION_POLYGON_REVERSE_FACE) && resolver_mode_byte & COLLISION_RESOLVER_WORLD_SOURCE && !(resolver_mode_byte & COLLISION_RESOLVER_TRANSFORM_NORMAL) && plane_data->normal.x & COLLISION_PLANE_REVERSE_TEST && !((uint8_t)polygon_flags_or_contact_work_bits & COLLISION_POLYGON_TRIANGLE))
362 {
363 pending_polygon_flags |= COLLISION_POLYGON_REVERSE_FACE;
364 effective_polygon_flags = pending_polygon_flags;
365 }
366 }
367
368 struct Collision_Polygon *collision_poly = source_polygon;
369 int32_t world_face_normal_y_q12;
370 bool test_original_face;
371 struct Pkg_CollisionFacePlane **face_plane_slot;
372
373 if (!(resolver_mode_byte & COLLISION_RESOLVER_WORLD_SOURCE))
374 {
375 world_face_normal_y_q12 = 0;
376 test_original_face = true;
377 }
378 else
379 {
380 if (resolver_mode_byte & COLLISION_RESOLVER_TRANSFORM_NORMAL)
381 {
382 struct Pkg_CollisionFacePlane *rotated_ground_plane = source_polygon->face_plane;
383
384 resolver_mode_byte = (uint8_t)resolver_mode;
385 face_plane_slot = &source_polygon->face_plane;
386 world_face_normal_y_q12 = ((int32_t)rotated_ground_plane->normal.z * (int32_t)collision_source->transform_matrix.m12 + (int32_t)rotated_ground_plane->normal.y * (int32_t)collision_source->transform_matrix.m11 + (int32_t)rotated_ground_plane->normal.x * (int32_t)collision_source->transform_matrix.m10) >> MATH_Q12_SHIFT;
387 source_polygon = polygons;
388 }
389 else
390 {
391 face_plane_slot = &source_polygon->face_plane;
392 world_face_normal_y_q12 = (int32_t)source_polygon->face_plane->normal.y;
393 }
394
395 polygon_flags_or_contact_work_bits = world_face_normal_y_q12;
396
397 test_original_face = polygon_flags_or_contact_work_bits <= COLLISION_GROUND_NORMAL_MIN_Q12 ||
398 (pending_polygon_flags & COLLISION_POLYGON_WALL_RESPONSE) ||
399 (!(resolver_mode_byte & (COLLISION_RESOLVER_VELOCITY_FRAME | COLLISION_RESOLVER_SOURCE_STATIONARY)) && polygon_flags_or_contact_work_bits >= COLLISION_NEAR_VERTICAL_NORMAL_Q12);
400
401 if (!test_original_face && (resolver_mode_byte & COLLISION_RESOLVER_VELOCITY_FRAME))
402 polygon_flags_or_contact_work_bits = (int32_t)(actor);
403 }
404
405 if (test_original_face || !(resolver_mode_byte & COLLISION_RESOLVER_VELOCITY_FRAME) || source_polygon != ((struct Actor_State *)polygon_flags_or_contact_work_bits)->ground_contact_polygon)
406 {
407 while (test_original_face || (pending_polygon_flags & COLLISION_POLYGON_REVERSE_FACE))
408 {
409 struct Collision_Polygon reversed_polygon;
410 struct Collision_FacePlane reversed_plane;
411 int16_t vertex_index_or_local_x_bits;
412
413 if (!test_original_face)
414 {
415 int32_t reversed_world_normal_y = -(world_face_normal_y_q12);
416
417 world_face_normal_y_q12 = reversed_world_normal_y;
418
419 if (reversed_world_normal_y > COLLISION_GROUND_NORMAL_MIN_Q12 && !(pending_polygon_flags & COLLISION_POLYGON_WALL_RESPONSE))
420 {
422 break;
423
424 if (reversed_world_normal_y < COLLISION_NEAR_VERTICAL_NORMAL_Q12)
425 break;
426 }
427
428 polygon_flags_or_contact_work_bits = (int32_t)(&reversed_polygon);
429
430 if (collision_poly == &reversed_polygon)
431 break;
432
433 pending_polygon_flags &= ~(uint32_t)COLLISION_POLYGON_REVERSE_FACE;
434
435 reversed_polygon.face_plane = (struct Pkg_CollisionFacePlane *)(&reversed_plane);
436 effective_polygon_flags = pending_polygon_flags;
437 reversed_polygon.flags = (uint16_t)pending_polygon_flags;
438
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;
443
444 do
445 {
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);
453
454 reversed_polygon.face_plane->normal.x =
455 -((*face_plane_slot)->normal.x);
456 reversed_polygon.face_plane->normal.y =
457 -((*face_plane_slot)->normal.y);
458 reversed_polygon.face_plane->normal.z =
459 -((*face_plane_slot)->normal.z);
460 reversed_polygon.face_plane->plane_distance =
461 -((*face_plane_slot)->plane_distance);
462 collision_poly = &reversed_polygon;
463 }
464
465 test_original_face = false;
466
467 struct Pkg_CollisionFacePlane *tested_face_plane =
468 collision_poly->face_plane;
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;
473
474 polygon_flags_or_contact_work_bits = (((cull_radius << 6) + COLLISION_EDGE_SWEEP_PADDING_Q6) << 4) - normal_motion_projection_q4;
475
476 if (polygon_flags_or_contact_work_bits > 0)
477 {
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;
481
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) >>
484 2) +
485 tested_face_plane->plane_distance;
486 if ((uint32_t)source_plane_distance <= (uint32_t)polygon_flags_or_contact_work_bits)
487 {
488 struct Collision_Vertex *edge_vertices = vertices;
489
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));
491
492 int32_t last_vertex_index = 0;
493 int32_t edges_remaining = 4 - (polygon_flags_or_contact_work_bits & COLLISION_POLYGON_TRIANGLE);
494
495 last_vertex_index = collision_poly->vertex_idx[4 - (polygon_flags_or_contact_work_bits & COLLISION_POLYGON_TRIANGLE) - 1];
496
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;
505
506 while (true)
507 {
508 int32_t current_edge_vertex_index = 0;
509
510 current_edge_vertex_index = *edge_vertex_index_cursor;
511 ++edge_vertex_index_cursor;
512
513 struct Collision_Vertex *current_edge_position = &edge_vertices[current_edge_vertex_index];
514 struct Pkg_CollisionFacePlane *edge_face_plane =
515 collision_poly->face_plane;
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;
523
524 polygon_flags_or_contact_work_bits = (edge_delta_z * edge_face_normal_y - edge_delta_y * edge_face_normal_z) >> 6;
525 edge_normal_y =
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;
531
532 int16_t relative_vertex_z = source_local_z_q6 - previous_edge_position->pos.z;
533
534 saved_relative_or_corner_x = vertex_index_or_local_x_bits;
535 relative_or_corner_z = relative_vertex_z;
536
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;
539
540 if (edge_plane_distance >= 0)
541 edge_vertices = vertices;
542 else
543 {
544 int32_t edge_radius_squared_bound;
545
546 if (world_face_normal_y_q12 <= COLLISION_GROUND_NORMAL_MIN_Q12 || effective_polygon_flags & COLLISION_POLYGON_WALL_RESPONSE || !(actor->behavior_flags & ACTOR_BEHAVIOR_AIRBORNE))
547 {
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);
550
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;
555 }
556 else
557 {
558 edge_vertices = vertices;
559 edge_radius_squared_bound = 0;
560 }
561
562 if ((int32_t)(((int64_t)saved_edge_plane_distance * saved_edge_plane_distance) >> MATH_Q12_SHIFT) > edge_radius_squared_bound)
563 {
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;
565
566 int32_t swept_edge_plane_distance = edge_plane_distance + polygon_flags_or_contact_work_bits;
567
568 if (edge_plane_distance + polygon_flags_or_contact_work_bits < 0)
569 {
570 if ((int32_t)(((int64_t)swept_edge_plane_distance * swept_edge_plane_distance) >> MATH_Q12_SHIFT) > edge_radius_squared_bound)
571 {
572 polygon_rejected = true;
573
574 break;
575 }
576 }
577 }
578 }
579
580 bool has_more_edges = edges_remaining != 1;
581
582 edges_remaining -= 1;
583
584 if (!has_more_edges)
585 break;
586
587 previous_edge_position = current_edge_position;
588 }
589
590 if (polygon_rejected)
591 break;
592
593 if (!actor->collision_class)
594 {
595 if ((uint8_t)resolver_mode & COLLISION_RESOLVER_CONTACT_INITIALIZED)
596 {
597 struct Pkg_CollisionFacePlane *face_plane =
598 collision_poly->face_plane;
599
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));
601
602 int16_t y = face_plane->normal.y;
603
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;
608
609 break;
610 }
611
612 int32_t normal_accumulation_mode = resolver_mode;
613
614 normal_accumulation_mode |= COLLISION_RESOLVER_CONTACT_INITIALIZED;
615 resolver_mode = normal_accumulation_mode;
616
617 struct Pkg_CollisionFacePlane *first_accumulated_plane =
618 collision_poly->face_plane;
619
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);
624
625 struct Pkg_CollisionFacePlane normal_z_padding_copy;
626
627 normal_z_padding_copy.normal.z = first_accumulated_plane->normal.z;
628 normal_z_padding_copy.navigation_offset_present =
629 first_accumulated_plane->navigation_offset_present;
630 accumulated_normal.z = normal_z_padding_copy.normal.z;
631 accumulated_normal.padding =
632 normal_z_padding_copy.navigation_offset_present;
633
634 break;
635 }
636
637 int32_t box_initialization_mode = resolver_mode;
638
639 if (!((uint8_t)box_initialization_mode & COLLISION_RESOLVER_SOURCE_STATIONARY) && world_face_normal_y_q12 > COLLISION_GROUND_NORMAL_MIN_Q12 && !(effective_polygon_flags & COLLISION_POLYGON_WALL_RESPONSE))
640 {
641 int32_t ground_contact_behavior_flags = actor->behavior_flags;
642
643 ground_contact_behavior_flags |= ACTOR_BEHAVIOR_MOVEMENT_ACTIVE;
644 actor->behavior_flags = ground_contact_behavior_flags;
645 }
646
647 if (!((uint8_t)box_initialization_mode & COLLISION_RESOLVER_CONTACT_INITIALIZED))
648 {
649 box_initialization_mode |= COLLISION_RESOLVER_CONTACT_INITIALIZED;
650 resolver_mode = box_initialization_mode;
651
653 actor_collision_shape_node =
655
656 accumulated_normal.z = 0;
657 accumulated_normal.y = 0;
658 accumulated_normal.x = 0;
659
660 int16_t sphere_extents[3];
661 int16_t *saved_box_half_extents;
662 int16_t *box_half_extents;
663
664 if (!actor_collision_shape_node)
665 {
666 box_half_extents = &actor->collision_height;
667 saved_box_half_extents = box_half_extents;
668 }
669 else if (
670 actor_collision_shape_node->extent_encoding_mode != COLLISION_EXTENTS_PACKED_PAIRS)
671 {
672 int16_t extent = (int16_t)(actor_collision_shape_node->shape.sphere.radius_q12 >> 6);
673
674 sphere_extents[0] = sphere_extents[1] = sphere_extents[2] = extent;
675 box_half_extents = saved_box_half_extents = sphere_extents;
676 }
677 else
678 {
679 box_half_extents = (int16_t *)(&actor_collision_shape_node->shape);
680 saved_box_half_extents = box_half_extents;
681 }
682
683 struct Math_Matrix3x3I16 actor_to_source_rotation;
684
685 Math_MulMat3x3Q12(&actor_to_source_rotation,
686 &actor->rot_mat, &world_to_source_rotation);
687
688 int32_t box_corner_index = 0;
689
690 while (true)
691 {
692 switch (box_corner_index)
693 {
694 case 0:
695 {
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]);
703
704 break;
705 }
706
707 case 1:
708 {
709 relative_or_corner_z =
710 box_half_extents[2];
711
712 break;
713 }
714
715 case 2:
716 {
717 relative_or_corner_y =
718 box_half_extents[1];
719
720 break;
721 }
722
723 case 3:
724 {
725 relative_or_corner_z =
726 -(box_half_extents[2]);
727
728 break;
729 }
730 }
731
732 int32_t corner_y =
733 (int32_t)relative_or_corner_y;
734 int32_t corner_x = (int32_t)vertex_index_or_local_x_bits;
735 int32_t corner_z =
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;
740
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);
742
743 int32_t corner_x_rotated_z_term = corner_x * (int32_t)actor_to_source_rotation.m20;
744
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;
750 actor_box_corners[box_corner_index].position_q6.x +=
751 source_local_x_q6;
752 actor_box_corners[box_corner_index].position_q6.y +=
753 source_local_y_q6;
754 actor_box_corners[box_corner_index].position_q6.z +=
755 source_local_z_q6;
756 box_corner_index += 1;
757
758 if (box_corner_index >= 4)
759 break;
760
761 box_half_extents = saved_box_half_extents;
762 vertex_index_or_local_x_bits = saved_relative_or_corner_x;
763 }
764
765 /* Native 0x431d65..0x431dfd: paired box planes. */
766 const int16_t *rotation_elements = (const int16_t *)&actor_to_source_rotation;
767
768 for (int pair = 0; pair < 3; ++pair)
769 {
770 int axis = pair < 2 ? (pair ^ 1) : pair;
771 struct Collision_FacePlane *near_plane = &actor_box_planes[pair * 2];
772 struct Collision_FacePlane *far_plane = near_plane + 1;
773
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];
777
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;
780
781 near_plane->distance = extent - center_projection;
782 far_plane->distance = extent + center_projection;
783 far_plane->normal.x = -near_plane->normal.x;
784 far_plane->normal.y = -near_plane->normal.y;
785 far_plane->normal.z = -near_plane->normal.z;
786 }
787
788 edge_vertices = vertices;
789 }
790
791 int32_t response_motion_x = (-COLLISION_SEPARATION_BIAS_Q6 * (int32_t)collision_poly->face_plane->normal.x) >> MATH_Q12_SHIFT;
792 int32_t response_motion_y = (-COLLISION_SEPARATION_BIAS_Q6 * (int32_t)collision_poly->face_plane->normal.y) >> MATH_Q12_SHIFT;
793 int32_t initial_normal_bias_z = (-COLLISION_SEPARATION_BIAS_Q6 * (int32_t)collision_poly->face_plane->normal.z) >> MATH_Q12_SHIFT;
794 int32_t response_motion_z = initial_normal_bias_z;
795
796 if (normal_motion_projection_q4 <= -COLLISION_MOTION_PROJECTION_TOLERANCE_Q4)
797 {
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;
801 }
802
803 int32_t clip_source_vertex_zero = 0;
804
805 clip_source_vertex_zero = collision_poly->vertex_idx[0];
806
807 struct Collision_ClipVertexQ6 clip_vertices[0xc];
808
809 __builtin_memcpy(&clip_vertices[0].position_q6.x,
810 &edge_vertices[clip_source_vertex_zero].pos.x, sizeof(uint32_t));
811
812 int32_t clip_source_vertex_one = 0;
813
814 clip_source_vertex_one = collision_poly->vertex_idx[1];
815 __builtin_memcpy(&clip_vertices[0].position_q6.z,
816 &edge_vertices[clip_source_vertex_zero].pos.z, sizeof(uint32_t));
817
818 __builtin_memcpy(&clip_vertices[1].position_q6.x,
819 &edge_vertices[clip_source_vertex_one].pos.x, sizeof(uint32_t));
820 __builtin_memcpy(&clip_vertices[1].position_q6.z,
821 &edge_vertices[clip_source_vertex_one].pos.z, sizeof(uint32_t));
822
823 int32_t clip_source_vertex_two = 0;
824
825 clip_source_vertex_two = collision_poly->vertex_idx[2];
826
827 __builtin_memcpy(&clip_vertices[2].position_q6.x,
828 &edge_vertices[clip_source_vertex_two].pos.x, sizeof(uint32_t));
829
830 int32_t clip_source_vertex_three = 0;
831
832 clip_source_vertex_three = collision_poly->vertex_idx[3];
833 __builtin_memcpy(&clip_vertices[2].position_q6.z,
834 &edge_vertices[clip_source_vertex_two].pos.z, sizeof(uint32_t));
835
836 __builtin_memcpy(&clip_vertices[3].position_q6.x,
837 &edge_vertices[clip_source_vertex_three].pos.x, sizeof(uint32_t));
838
839 int32_t next_box_plane_index = 5;
840 struct Collision_FacePlane *selected_box_plane = &actor_box_planes[5];
841
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;
845
846 int32_t box_plane_index = 5;
847 struct Collision_FacePlane *saved_box_plane = &actor_box_planes[5];
848 int32_t plane_motion_or_contact_fraction;
849 int32_t response_z_or_next_clip_distance;
850 struct Collision_ClipVertexQ6 *clip_ring_head;
851
852 while (true)
853 {
854 while (true)
855 {
856 /* Opposite planes share the same projection with its sign reversed. */
857 const struct Collision_FacePlane *opposite = &actor_box_planes[box_plane_index | 1];
858
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;
861
862 if (!(box_plane_index & 1))
863 plane_motion_or_contact_fraction = -plane_motion_or_contact_fraction;
864
865 if (plane_motion_or_contact_fraction >= COLLISION_MINIMUM_FRACTION_MOTION_Q12)
866 {
867 struct Pkg_CollisionFacePlane *box_alignment_plane =
868 collision_poly->face_plane;
869
870 if ((((int32_t)box_alignment_plane->normal.z * (int32_t)selected_box_plane->normal.z + (int32_t)box_alignment_plane->normal.x * (int32_t)selected_box_plane->normal.x + (int32_t)box_alignment_plane->normal.y * (int32_t)selected_box_plane->normal.y) & -MATH_Q12_ONE) < -COLLISION_PLANE_ALIGNMENT_REJECTION_Q24)
871 break;
872 }
873
874 int32_t previous_box_plane_index = box_plane_index;
875
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;
880
881 if (!previous_box_plane_index)
882 {
883 polygon_rejected = true;
884
885 break;
886 }
887 }
888
889 if (polygon_rejected)
890 break;
891
892 clip_vertices[0].next = &clip_vertices[1];
893 clip_vertices[1].next = &clip_vertices[2];
894
895 if (!(collision_poly->flags & COLLISION_POLYGON_TRIANGLE))
896 {
897 clip_vertices[2].next = &clip_vertices[3];
898 clip_vertices[3].next = (struct Collision_ClipVertexQ6 *)(&clip_vertices);
899 }
900 else
901 clip_vertices[2].next = (struct Collision_ClipVertexQ6 *)(&clip_vertices);
902
903 struct Collision_ClipVertexQ6 *next_clip_vertex = &clip_vertices[4];
904 int32_t clip_plane_side = 0;
905
906 clip_ring_head = clip_vertices;
907
908 int32_t saved_clip_plane_side = 0;
909
910 while (clip_plane_side < 4)
911 {
912 int32_t saved_clip_normal_y;
913 int32_t clip_normal_x;
914
915 if (!((uint8_t)clip_plane_side & 1))
916 {
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;
920
921 box_edge_end_index = Collision_BoxFaceVertexRemap[box_edge_remap_offset + 1];
922 box_edge_start_index =
923 Collision_BoxFaceVertexRemap[box_edge_remap_offset];
924
925 int32_t box_edge_delta_y = (int32_t)(int16_t)(actor_box_corners[box_edge_end_index].position_q6.y -
926 actor_box_corners[box_edge_start_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 -
928 actor_box_corners[box_edge_start_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 -
930 actor_box_corners[box_edge_start_index].position_q6.x);
931 int32_t saved_response_motion_x = response_motion_x;
932
933 clip_normal_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;
937
938 int32_t clip_normal_z =
939 (box_edge_delta_y * saved_response_motion_x - box_edge_delta_x * response_motion_y) >> 6;
940
941 edge_or_clip_normal_x = clip_normal_x;
942
944 {
945 clip_normal_x >>= 1;
946 polygon_flags_or_contact_work_bits >>= 1;
947 clip_normal_z >>= 1;
948 }
949
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;
954 }
955 else
956 {
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);
962 }
963
964 int32_t next_side_or_anchor_offset = clip_plane_side + 1;
965 int32_t next_clip_plane_side = next_side_or_anchor_offset;
966
967 next_side_or_anchor_offset &= ~5;
968
969 int32_t clip_plane_anchor_index = 0;
970
971 clip_plane_anchor_index =
972 Collision_BoxFaceVertexRemap[next_side_or_anchor_offset + (box_plane_index << 2)];
973
974 struct Collision_ClipVertexQ6 *clip_vertex = clip_ring_head;
975 struct Collision_ClipVertexQ6 *clip_ring_start = clip_vertex;
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) >>
980 2));
981 struct Collision_ClipVertexQ6 *first_intersection = 0;
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;
984 struct Collision_ClipVertexQ6 *next_clip_vertex_in_ring;
985
986 do
987 {
988 next_clip_vertex_in_ring =
989 clip_vertex->next;
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;
991
992 if ((response_z_or_next_clip_distance ^ clip_distance_or_fraction) < 0)
993 {
994 struct Collision_ClipVertexQ6 *splice_successor;
995 struct Collision_ClipVertexQ6 *splice_predecessor;
996
997 if (response_z_or_next_clip_distance >= 0)
998 {
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);
1001 next_clip_vertex->position_q6.x = (int16_t)(((int32_t)next_clip_vertex_in_ring->position_q6.x * (MATH_Q12_ONE - clip_distance_or_fraction) + (int32_t)clip_vertex->position_q6.x * clip_distance_or_fraction) >> MATH_Q12_SHIFT);
1002 next_clip_vertex->position_q6.y = (int16_t)(((int32_t)clip_vertex->position_q6.y * clip_distance_or_fraction + (int32_t)next_clip_vertex_in_ring->position_q6.y * (MATH_Q12_ONE - clip_distance_or_fraction)) >> MATH_Q12_SHIFT);
1003 next_clip_vertex->position_q6.z = (int16_t)(((int32_t)clip_vertex->position_q6.z * clip_distance_or_fraction + (int32_t)next_clip_vertex_in_ring->position_q6.z * (MATH_Q12_ONE - clip_distance_or_fraction)) >> MATH_Q12_SHIFT);
1004 next_clip_vertex->next =
1005 next_clip_vertex_in_ring;
1006
1007 if (first_intersection)
1008 {
1009 splice_successor = next_clip_vertex;
1010 splice_predecessor = first_intersection;
1011 splice_predecessor->next =
1012 splice_successor;
1013 polygon_flags_or_contact_work_bits = saved_clip_normal_y;
1014
1015 break;
1016 }
1017 }
1018 else
1019 {
1020 clip_distance_or_fraction = ((int64_t)saved_clip_distance * MATH_Q12_ONE) /
1021 (clip_distance_or_fraction - response_z_or_next_clip_distance);
1022 next_clip_vertex->position_q6.x = (int16_t)(((int32_t)clip_vertex->position_q6.x * (MATH_Q12_ONE - clip_distance_or_fraction) + (int32_t)next_clip_vertex_in_ring->position_q6.x * clip_distance_or_fraction) >> MATH_Q12_SHIFT);
1023 next_clip_vertex->position_q6.y = (int16_t)(((int32_t)clip_vertex->position_q6.y * (MATH_Q12_ONE - clip_distance_or_fraction) + (int32_t)next_clip_vertex_in_ring->position_q6.y * clip_distance_or_fraction) >> MATH_Q12_SHIFT);
1024 next_clip_vertex->position_q6.z = (int16_t)(((int32_t)clip_vertex->position_q6.z * (MATH_Q12_ONE - clip_distance_or_fraction) + (int32_t)next_clip_vertex_in_ring->position_q6.z * clip_distance_or_fraction) >> MATH_Q12_SHIFT);
1025 clip_vertex->next =
1026 next_clip_vertex;
1027
1028 if (first_intersection)
1029 {
1030 splice_successor = first_intersection;
1031 splice_predecessor = next_clip_vertex;
1032 splice_predecessor->next =
1033 splice_successor;
1034 polygon_flags_or_contact_work_bits = saved_clip_normal_y;
1035
1036 break;
1037 }
1038 }
1039
1040 first_intersection = next_clip_vertex;
1041 ++next_clip_vertex;
1042 }
1043
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);
1049
1050 if (!first_intersection)
1051 {
1052 if (clip_distance_or_fraction < 0)
1053 break;
1054
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;
1059 }
1060 else
1061 {
1062 response_z_or_next_clip_distance = response_motion_z;
1063 clip_ring_head = next_clip_vertex;
1064 ++next_clip_vertex;
1065 clip_plane_side = next_clip_plane_side;
1066 saved_clip_plane_side = clip_plane_side;
1067 }
1068 }
1069
1070 if (clip_plane_side < 4)
1071 {
1072 int32_t previous_box_plane_index = box_plane_index;
1073
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;
1078
1079 if (!previous_box_plane_index)
1080 {
1081 polygon_rejected = true;
1082
1083 break;
1084 }
1085
1086 continue;
1087 }
1088
1089 break;
1090 }
1091
1092 if (polygon_rejected)
1093 break;
1094
1095 struct Collision_ClipVertexQ6 *clipped_vertex = clip_ring_head;
1096 struct Collision_ClipVertexQ6 *saved_clipped_vertex = clipped_vertex;
1097 int32_t minimum_contact_depth = COLLISION_UNSET_CONTACT_DEPTH_Q12;
1098
1099 do
1100 {
1101 int32_t clipped_vertex_y =
1102 (int32_t)clipped_vertex->position_q6.y;
1103 int32_t clipped_vertex_x =
1104 (int32_t)clipped_vertex->position_q6.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;
1106
1107 if (minimum_contact_depth != COLLISION_UNSET_CONTACT_DEPTH_Q12)
1108 {
1109 if (!(clipped_vertex_plane_distance < minimum_contact_depth))
1110 continue;
1111 }
1112 else if (clipped_vertex_plane_distance >= -COLLISION_PLANE_TOLERANCE_Q4)
1113 {
1114 if (clipped_vertex == clip_ring_head)
1115 {
1116 if (!(clipped_vertex_plane_distance < minimum_contact_depth))
1117 continue;
1118 }
1119 else
1120 clipped_vertex_plane_distance = COLLISION_REJECTED_CONTACT_DEPTH_BITS;
1121 }
1122 else
1123 {
1124 int32_t alternate_plane_pair = box_plane_index & 0xfffffffe;
1125
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)
1127 {
1128 alternate_plane_pair += 2;
1129
1130 if (alternate_plane_pair == 6)
1131 alternate_plane_pair = 0;
1132
1133 int32_t distance =
1134 actor_box_planes[alternate_plane_pair].distance;
1135 int32_t clipped_vertex_z = (int32_t)saved_clipped_vertex->position_q6.z;
1136
1137 const struct Math_Vec3I16 *normal = &actor_box_planes[alternate_plane_pair].normal;
1138 int32_t projected_distance = ((normal->x * clipped_vertex_x +
1139 normal->y * clipped_vertex_y + normal->z * clipped_vertex_z) >>
1140 2) +
1141 distance;
1142 uint32_t plane_span = (uint32_t)COLLISION_PLANE_TOLERANCE_Q4 -
1143 actor_box_planes[1 + alternate_plane_pair].distance - distance;
1144 if (0u - (uint32_t)projected_distance > plane_span)
1145 {
1146 clipped_vertex = saved_clipped_vertex;
1147
1148 continue;
1149 }
1150 else
1151 {
1152 int32_t second_alternate_plane_pair = alternate_plane_pair + 2;
1153
1154 if (second_alternate_plane_pair == 6)
1155 second_alternate_plane_pair = 0;
1156
1157 const struct Collision_FacePlane *test_plane = &actor_box_planes[second_alternate_plane_pair];
1158
1159 clipped_vertex = saved_clipped_vertex;
1160
1161 int32_t projection = ((int32_t)test_plane->normal.x * clipped_vertex->position_q6.x + (int32_t)test_plane->normal.y * clipped_vertex->position_q6.y + (int32_t)test_plane->normal.z * clipped_vertex_z) >> 2;
1162
1163 if (0u - (uint32_t)(projection + test_plane->distance) >
1164 (uint32_t)COLLISION_PLANE_TOLERANCE_Q4 - test_plane[1].distance - test_plane->distance)
1165 continue;
1166 }
1167 }
1168 else
1169 continue;
1170
1171 if (clipped_vertex == clip_ring_head)
1172 {
1173 if (!(clipped_vertex_plane_distance < minimum_contact_depth))
1174 continue;
1175 }
1176 else
1177 clipped_vertex_plane_distance = COLLISION_REJECTED_CONTACT_DEPTH_BITS;
1178 }
1179
1180 minimum_contact_depth = clipped_vertex_plane_distance;
1181
1182 if (clipped_vertex_plane_distance < 0)
1183 {
1184 if (clipped_vertex_plane_distance < -COLLISION_PLANE_TOLERANCE_Q4)
1185 break;
1186
1187 minimum_contact_depth = 0;
1188 }
1189 } while ((clipped_vertex = clipped_vertex->next,
1190 saved_clipped_vertex = clipped_vertex,
1191 clipped_vertex != clip_ring_head));
1192
1193 polygon_flags_or_contact_work_bits = (int32_t)clipped_vertex;
1194
1195 if (minimum_contact_depth < -COLLISION_PLANE_TOLERANCE_Q4 ||
1196 (normal_motion_projection_q4 <= -COLLISION_MOTION_PROJECTION_TOLERANCE_Q4 && minimum_contact_depth <= plane_motion_or_contact_fraction))
1197 {
1198 int32_t resolved_contact_fraction = 0;
1199 if (minimum_contact_depth >= -COLLISION_PLANE_TOLERANCE_Q4)
1200 resolved_contact_fraction =
1201 ((int64_t)minimum_contact_depth * MATH_Q12_ONE) / plane_motion_or_contact_fraction;
1202
1203 int32_t collision_depth = resolved_contact_fraction;
1204
1205 plane_motion_or_contact_fraction = collision_depth;
1206
1207 struct Math_Vec3I16 *response_normal =
1208 &collision_poly->face_plane->normal;
1209 struct Math_Vec3I16 *selected_response_normal = response_normal;
1210
1211 if (collision_depth > MATH_Q12_ONE)
1212 {
1213 plane_motion_or_contact_fraction = MATH_Q12_ONE;
1214 collision_depth = MATH_Q12_ONE;
1215 }
1216
1217 int8_t collision_class = (int8_t)actor->collision_class;
1218
1219 if (collision_class > 0)
1220 {
1221 polygon_flags_or_contact_work_bits = (int32_t)actor->scale_factor;
1222
1223 int32_t collision_callback_result;
1224 struct Collision_Node *response_source;
1225
1226 if (collision_depth > polygon_flags_or_contact_work_bits)
1227 {
1228 collision_callback_result = response_scratch;
1229 response_source = collision_source;
1230
1231 if (response_source->node_type < COLLISION_NODE_ACTOR)
1232 {
1233 if (collision_depth)
1234 break;
1235
1236 if (collision_callback_result >= 0)
1237 break;
1238
1239 response_source = collision_source;
1240 }
1241
1242 if (response_source->node_type != COLLISION_NODE_ACTOR)
1243 return;
1244
1245 if (collision_depth > (int32_t)((struct Actor_State *)response_source)->scale_factor)
1246 return;
1247
1249 int32_t)(int8_t)((struct Actor_State *)response_source)
1250 ->collision_class](
1251 response_source, actor,
1252 &Collision_BoxFacePolygons[box_plane_index],
1253 collision_depth))
1254 return;
1255
1256 ((struct Actor_State *)response_source)->scale_factor = (uint16_t)collision_depth;
1257
1258 return;
1259 }
1260
1261 response_source = collision_source;
1262 collision_callback_result = Actor_DefaultUpdateCallbackSlot[(
1263 int32_t)collision_class](actor,
1264 response_source, collision_poly,
1265 collision_depth);
1266 response_scratch = collision_callback_result;
1267
1268 if (collision_callback_result <= 0)
1269 {
1270 if (collision_callback_result < 0)
1271 actor->scale_factor =
1272 (uint16_t)collision_depth;
1273
1274 if (response_source->node_type < COLLISION_NODE_ACTOR)
1275 {
1276 if (collision_depth)
1277 break;
1278
1279 if (collision_callback_result >= 0)
1280 break;
1281
1282 response_source = collision_source;
1283 }
1284
1285 if (response_source->node_type != COLLISION_NODE_ACTOR)
1286 return;
1287
1288 if (collision_depth > (int32_t)((struct Actor_State *)response_source)->scale_factor)
1289 return;
1290
1292 int32_t)(int8_t)((struct Actor_State *)response_source)
1293 ->collision_class](
1294 response_source, actor,
1295 &Collision_BoxFacePolygons[box_plane_index],
1296 collision_depth))
1297 return;
1298
1299 ((struct Actor_State *)response_source)->scale_factor = (uint16_t)collision_depth;
1300
1301 return;
1302 }
1303
1304 int16_t behavior_flags =
1305 (int16_t)actor->behavior_flags;
1306
1307 if (!((uint8_t)behavior_flags & ACTOR_BEHAVIOR_AIRBORNE))
1308 {
1309 int32_t response_world_normal_y =
1310 world_face_normal_y_q12;
1311
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;
1314
1315 if (response_world_normal_y < 0)
1316 {
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;
1321
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);
1323
1324 if (length)
1325 {
1326 int32_t inverse = (int32_t)(INT64_C(0x100000000) / length);
1327
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);
1331 }
1332 else
1333 {
1334 projected_face_normal.x = projected_face_normal.y = projected_face_normal.z = 0;
1335 }
1336
1337 response_motion_x = (-COLLISION_SEPARATION_BIAS_Q6 * (int32_t)projected_face_normal.x) >> MATH_Q12_SHIFT;
1338 response_motion_y = (-COLLISION_SEPARATION_BIAS_Q6 * (int32_t)projected_face_normal.y) >> MATH_Q12_SHIFT;
1339
1340 int32_t projected_normal_bias_z = (-COLLISION_SEPARATION_BIAS_Q6 * (int32_t)projected_face_normal.z) >> MATH_Q12_SHIFT;
1341
1342 response_motion_z = projected_normal_bias_z;
1343
1344 if (normal_motion_projection_q4 <= -COLLISION_MOTION_PROJECTION_TOLERANCE_Q4)
1345 {
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;
1349 }
1350
1351 response_normal = selected_response_normal;
1352 }
1353 }
1354 }
1355
1356 int32_t within_penetration_tolerance = 0;
1357
1358 within_penetration_tolerance = minimum_contact_depth >= -COLLISION_DEEP_PENETRATION_TOLERANCE_Q4;
1359 int32_t correction_distance = ((within_penetration_tolerance - 1) & COLLISION_SEPARATION_BIAS_Q6) + COLLISION_SEPARATION_BIAS_Q6;
1360
1361 if (normal_motion_projection_q4 <= -COLLISION_MOTION_PROJECTION_TOLERANCE_Q4 && resolved_contact_fraction != MATH_Q12_ONE)
1362 {
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);
1364
1365 correction_distance += (int32_t)(((int64_t)(resolved_contact_fraction - MATH_Q12_ONE) * normal_motion) >> MATH_Q12_SHIFT);
1366 response_normal = selected_response_normal;
1367 }
1368
1369 response_scratch = correction_distance;
1370 response_motion_x = (int32_t)(((int64_t)correction_distance * response_normal->x) >> MATH_Q12_SHIFT);
1371
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);
1374
1375 if ((int8_t)actor->collision_class <= 0)
1376 {
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);
1378
1379 if (correction_length_squared > COLLISION_CORRECTION_SQUARED_TOLERANCE)
1380 {
1381 if (!plane_motion_or_contact_fraction && minimum_contact_depth >= 0)
1382 plane_motion_or_contact_fraction = 1;
1383
1384 polygon_flags_or_contact_work_bits =
1385 (int32_t)actor->scale_factor;
1386
1387 if (plane_motion_or_contact_fraction < polygon_flags_or_contact_work_bits)
1388 {
1389 if (actor->collision_class == COLLISION_CLASS_WALL_PROBE && world_face_normal_y_q12 > COLLISION_GROUND_NORMAL_MIN_Q12)
1390 break;
1391
1392 polygon_flags_or_contact_work_bits = (int32_t)(actor);
1393 ((struct Actor_State *)polygon_flags_or_contact_work_bits)->scale_factor =
1394 (uint16_t)plane_motion_or_contact_fraction;
1395 }
1396 }
1397 }
1398 else
1399 {
1400 if (minimum_contact_depth < 0)
1402
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;
1406
1407 local_motion_x = corrected_motion_x;
1408 local_motion_y = corrected_motion_y;
1409 local_motion_z = corrected_motion_z;
1410 resolver_mode |= COLLISION_RESOLVER_MOTION_CORRECTED;
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)));
1415 }
1416 }
1417
1418 struct Pkg_CollisionFacePlane *final_accumulated_plane =
1419 collision_poly->face_plane;
1420
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));
1422
1423 int16_t accumulated_face_normal_z = final_accumulated_plane->normal.z;
1424
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;
1428
1429 break;
1430 }
1431 }
1432
1433 face_plane_slot = &collision_poly->face_plane;
1434 resolver_mode_byte = (uint8_t)resolver_mode;
1435 pending_polygon_flags = effective_polygon_flags;
1436 }
1437 }
1438
1439 bool has_more_polygons = polygon_count != 1;
1440
1441 polygons = &polygons[1];
1442 polygon_count -= 1;
1443
1444 if (!has_more_polygons)
1445 {
1446 /* 0x432a75..0x432da0: preserve all old components before committing. */
1447 if (resolver_mode & COLLISION_RESOLVER_MOTION_CORRECTED)
1448 {
1449 struct Math_Vec3I32 corrected = Collision_RotateVectorQ12(
1450 &source_transform_snapshot.rotation_q12,
1451 (struct Math_Vec3I32){local_motion_x, local_motion_y, local_motion_z});
1452 struct Math_Vec3I32 old;
1453
1454 if (resolver_mode & COLLISION_RESOLVER_VELOCITY_FRAME)
1455 {
1456 old = actor->velocity;
1457 actor->velocity = corrected;
1458 actor->sub_pos.x += corrected.x - old.x;
1459 actor->sub_pos.y += corrected.y - old.y;
1460 actor->sub_pos.z += corrected.z - old.z;
1461 }
1462 else
1463 {
1464 corrected.x += collision_source->collision_offset.x;
1465 corrected.y += collision_source->collision_offset.y;
1466 corrected.z += collision_source->collision_offset.z;
1467 old = actor->sub_pos;
1468 actor->sub_pos = corrected;
1469 actor->velocity.x += corrected.x - old.x;
1470 actor->velocity.y += corrected.y - old.y;
1471 actor->velocity.z += corrected.z - old.z;
1472 }
1473
1475 }
1476
1478 break;
1479
1480 int32_t fraction = (int16_t)actor->scale_factor;
1481
1482 if (fraction < MATH_Q12_ONE && (int8_t)actor->collision_class > 0)
1483 {
1484 struct Math_Vec3I32 movement = (resolver_mode & COLLISION_RESOLVER_VELOCITY_FRAME) ? actor->velocity : actor->sub_pos;
1485 int32_t dx = (int32_t)(((int64_t)(fraction - MATH_Q12_ONE) * movement.x) >> MATH_Q12_SHIFT);
1486 int32_t dy = (int32_t)(((int64_t)(fraction - MATH_Q12_ONE) * movement.y) >> MATH_Q12_SHIFT);
1487 int32_t dz = (int32_t)(((int64_t)(fraction - MATH_Q12_ONE) * movement.z) >> MATH_Q12_SHIFT);
1488
1489 actor->velocity.x += dx;
1490 actor->sub_pos.x += dx;
1491 actor->velocity.y += dy;
1492 actor->sub_pos.y += dy;
1493 actor->velocity.z += dz;
1494 actor->sub_pos.z += dz;
1495 actor->scale_factor = MATH_Q12_ONE;
1496 }
1497
1498 struct Math_Vec3I32 normal_world = Collision_RotateVectorQ12(
1499 &source_transform_snapshot.rotation_q12,
1500 (struct Math_Vec3I32){accumulated_normal.x, accumulated_normal.y, accumulated_normal.z});
1501
1502 actor->collision_push_components.x += (int16_t)normal_world.x;
1503 actor->collision_push_components.z += (int16_t)normal_world.z;
1504
1505 break;
1506 }
1507 }
1508 }
1509}
#define ACTOR_BEHAVIOR_AIRBORNE
Definition constants.h:13
#define ACTOR_BEHAVIOR_GROUND_TILT_ENABLED
Definition constants.h:21
#define ACTOR_BEHAVIOR_PENETRATING
Definition constants.h:31
#define ACTOR_BEHAVIOR_GROUNDED_MOTION
Definition constants.h:14
#define ACTOR_BEHAVIOR_MOVEMENT_ACTIVE
Definition constants.h:19
#define ACTOR_BEHAVIOR_COLLISION_CORRECTION
Definition constants.h:33
#define Actor_DefaultUpdateCallbackSlot
Definition globals.h:30
#define COLLISION_PLANE_TOLERANCE_Q4
Definition constants.h:52
#define COLLISION_RESOLVER_SOURCE_STATIONARY
Definition constants.h:21
#define COLLISION_CLIP_NORMAL_COMPONENT_RANGE
Definition constants.h:84
#define COLLISION_NEAR_VERTICAL_NORMAL_Q12
Definition constants.h:51
#define COLLISION_RESOLVER_CONTACT_INITIALIZED
Definition constants.h:19
#define COLLISION_POLYGON_TRIANGLE
Definition constants.h:26
#define COLLISION_RESOLVER_MOTION_CORRECTED
Definition constants.h:24
#define COLLISION_GROUND_NORMAL_MIN_Q12
Definition constants.h:50
#define COLLISION_SEPARATION_BIAS_Q6
Definition constants.h:53
#define COLLISION_POLYGON_REVERSE_FACE
Definition constants.h:31
#define COLLISION_MOTION_PROJECTION_TOLERANCE_Q4
Definition constants.h:117
#define COLLISION_PLANE_REVERSE_TEST
Definition constants.h:63
#define COLLISION_DEBUG_POLYGON_LIST_CAPACITY
Definition constants.h:118
#define COLLISION_CLIP_NORMAL_COMPONENT_LIMIT
Definition constants.h:83
@ COLLISION_EXTENTS_PACKED_PAIRS
Definition constants.h:75
#define COLLISION_EDGE_SWEEP_PADDING_Q6
Definition constants.h:81
#define COLLISION_UNSET_CONTACT_DEPTH_Q12
Definition constants.h:85
#define COLLISION_DEEP_PENETRATION_TOLERANCE_Q4
Definition constants.h:54
#define COLLISION_PLANE_ALIGNMENT_REJECTION_Q24
Definition constants.h:116
#define COLLISION_RESOLVER_TRANSFORM_NORMAL
Definition constants.h:22
#define COLLISION_REJECTED_CONTACT_DEPTH_BITS
Definition constants.h:86
@ COLLISION_CLASS_WALL_PROBE
Definition constants.h:14
@ COLLISION_CLASS_CAMERA_PROBE
Definition constants.h:15
#define COLLISION_RESOLVER_WORLD_SOURCE
Definition constants.h:23
@ COLLISION_NODE_ACTOR
Definition constants.h:9
@ COLLISION_NODE_MESH
Definition constants.h:7
@ COLLISION_NODE_ORIENTED_BOX
Definition constants.h:8
#define COLLISION_NODE_PROXY_GEOMETRY
Definition constants.h:43
#define COLLISION_POLYGON_WALL_RESPONSE
Definition constants.h:34
#define COLLISION_CORRECTION_SQUARED_TOLERANCE
Definition constants.h:122
#define COLLISION_MINIMUM_FRACTION_MOTION_Q12
Definition constants.h:82
#define COLLISION_POLYGON_CAMERA_PROBE_SKIP
Definition constants.h:30
#define COLLISION_RESOLVER_VELOCITY_FRAME
Definition constants.h:20
#define Collision_DebugPolygonArrays
Definition globals.h:102
#define Collision_DebugPolygonListCount
Definition globals.h:75
#define Collision_DebugPolygonCounts
Definition globals.h:105
#define Collision_BoxFacePolygons
Definition globals.h:93
#define Collision_BoxFaceVertexRemap
Definition globals.h:99
struct Actor_State * Entity_GetCurrentOrFirstActiveActor(void)
Definition lifecycle.c:753
#define MATH_Q12_SHIFT
Definition constants.h:8
#define MATH_Q12_ONE
Definition constants.h:9
int32_t Math_SqrtFP12(uint32_t value)
void Math_MulMat3x3Q12(struct Math_Matrix3x3I16 *out_matrix, struct Math_Matrix3x3I16 const *right_factor, struct Math_Matrix3x3I16 const *left_factor)
Definition matrices.c:11
union Actor_CollisionShapeExtentOverlay shape
Definition types.h:562
uint8_t collision_class
Definition types.h:435
struct Math_Vec3I32 velocity
Definition types.h:446
struct Component_Instance * collision_component_or_parent_component
Definition types.h:437
uint16_t x
Definition types.h:496
int16_t cull_radius
Definition types.h:424
struct Collision_Node * ground_collision_node
Definition types.h:451
int32_t behavior_flags
Definition types.h:407
uint16_t z
Definition types.h:497
struct Actor_State::@000347335321225257200273166125221262042042274352::@355017126165076161135046166331324200333365100231 collision_push_components
int16_t scale_factor
Definition types.h:439
struct Math_Vec3I32 world_render_pos
Definition types.h:420
int16_t collision_height
Definition types.h:421
struct Math_Vec3I32 sub_pos
Definition types.h:386
struct Math_Matrix3x3I16 rot_mat
Definition types.h:383
struct Math_Vec3I16 position_q6
Definition types.h:133
struct Math_Vec3I16 position_q6
Definition types.h:144
struct Collision_ClipVertexQ6 * next
Definition types.h:146
int32_t distance
Definition types.h:156
struct Math_Vec3I16 normal
Definition types.h:157
union Collision_NodeBoundsOverlay bounds
Definition types.h:308
struct Collision_Vertex * vertices
Definition types.h:292
uint8_t node_type
Definition types.h:286
struct Math_Vec3I32 collision_offset
Definition types.h:284
uint8_t flags_high
Definition types.h:302
struct Math_Matrix3x3I16 transform_matrix
Definition types.h:281
struct Math_Vec3I32 origin
Definition types.h:283
struct Math_Vec3I16 half_extents_q6
Definition types.h:183
struct Math_Vec3I32 center
Definition types.h:182
struct Math_Matrix3x3I16 orientation_q12
Definition types.h:180
struct Collision_Plane::@141200322312224221361126047031300120201020162362 normal
int32_t x
Definition types.h:42
uint16_t vertex_idx[0x4]
Definition types.h:61
struct Collision_Plane * plane_data
Definition types.h:60
uint16_t flags
Definition types.h:66
struct Pkg_CollisionFacePlane * face_plane
Definition types.h:62
struct Math_Vec3I32 center
Definition types.h:209
struct Math_Matrix3x3I16 rotation_q12
Definition types.h:240
int16_t z
Definition types.h:111
struct Collision_Vertex::@111126275124057154070202104324102117233165176236::@061322026074334072070347216127267241215241121036 pos
int16_t x
Definition types.h:109
int16_t y
Definition types.h:110
int16_t m21
Definition types.h:169
int16_t m00
Definition types.h:162
int16_t m01
Definition types.h:163
int16_t m12
Definition types.h:167
int16_t m11
Definition types.h:166
int16_t m20
Definition types.h:168
int16_t m22
Definition types.h:170
int16_t m02
Definition types.h:164
int16_t m10
Definition types.h:165
int16_t z
Definition types.h:460
int16_t x
Definition types.h:458
int16_t y
Definition types.h:459
int32_t y
Definition types.h:471
int32_t x
Definition types.h:470
int32_t z
Definition types.h:472
struct Math_Vec3I16 normal
Definition types.h:1135
uint16_t navigation_offset_present
Definition types.h:1136
struct Actor_CollisionSphereExtentView sphere
Definition types.h:287
struct Collision_OrientedBoxNodeBoundsView oriented_box
Definition types.h:264
struct Collision_SphereNodeBoundsView sphere
Definition types.h:263
void Collision_ResolveSweptPolygons(struct Actor_State *actor, struct Collision_Node *collision_source, struct Collision_Vertex *vertices, struct Collision_Polygon *polygons, int32_t polygon_count, int32_t response_scratch)
Definition world.c:38
static struct Math_Vec3I32 Collision_RotateVectorQ12(const struct Math_Matrix3x3I16 *m, struct Math_Vec3I32 v)
Definition world.c:23