vlad.website
A 3D game engine from scratch.
1// (c) 2020 Vlad-Stefan Harbuz <vlad@vladh.net>
2
3#include "logs.hpp"
4#include "gui.hpp"
5#include "debugdraw.hpp"
6#include "physics.hpp"
7#include "intrinsics.hpp"
8
9
10physics::State *physics::state = nullptr;
11
12
13physics::RayCollisionResult
14physics::find_ray_collision(
15 spatial::Ray *ray,
16 // TODO: Replace this with some kind of collision layers.
17 physics::Component *physics_component_to_ignore_or_nullptr
18) {
19 each (candidate, *get_components()) {
20 if (!is_component_valid(candidate)) {
21 continue;
22 }
23
24 if (physics_component_to_ignore_or_nullptr == candidate) {
25 continue;
26 }
27
28 RaycastResult raycast_result = intersect_obb_ray(&candidate->transformed_obb, ray);
29 if (raycast_result.did_intersect) {
30 return {
31 .did_intersect = raycast_result.did_intersect,
32 .distance = raycast_result.distance,
33 .collidee = candidate,
34 };
35 }
36 }
37
38 return {};
39}
40
41
42physics::CollisionManifold
43physics::find_collision(
44 physics::Component *self_physics,
45 spatial::Component *self_spatial
46) {
47 each (candidate_physics, *get_components()) {
48 if (!is_component_valid(candidate_physics)) {
49 continue;
50 }
51 spatial::Component *candidate_spatial = spatial::get_component(candidate_physics->entity_handle);
52 if (!candidate_spatial) {
53 logs::error("Could not get spatial::Component for candidate");
54 return physics::CollisionManifold {};
55 }
56
57 if (self_physics == candidate_physics) {
58 continue;
59 }
60
61 physics::CollisionManifold manifold = intersect_obb_obb(
62 &self_physics->transformed_obb, &candidate_physics->transformed_obb, self_spatial, candidate_spatial);
63 if (manifold.did_collide) {
64 manifold.collidee = candidate_physics;
65 return manifold;
66 }
67 }
68
69 return physics::CollisionManifold {};
70}
71
72
73void
74physics::update()
75{
76 each (physics_component, *get_components()) {
77 if (!is_component_valid(physics_component)) {
78 continue;
79 }
80
81 spatial::Component *spatial_component = spatial::get_component(physics_component->entity_handle);
82
83 if (!spatial::is_spatial_component_valid(spatial_component)) {
84 logs::warning("Tried to update physics component %d but it had no spatial component.",
85 physics_component->entity_handle);
86 continue;
87 }
88
89 physics_component->transformed_obb = transform_obb(physics_component->obb, spatial_component);
90 }
91}
92
93
94Array<physics::Component> *
95physics::get_components()
96{
97 return &physics::state->components;
98}
99
100
101physics::Component *
102physics::get_component(entities::Handle entity_handle)
103{
104 return physics::state->components[entity_handle];
105}
106
107
108void
109physics::init(physics::State *physics_state, memory::Pool *asset_memory_pool)
110{
111 physics::state = physics_state;
112 physics::state->components = Array<physics::Component>(
113 asset_memory_pool, MAX_N_ENTITIES, "physics_components", true, 1);
114}
115
116
117spatial::Obb
118physics::transform_obb(spatial::Obb obb, spatial::Component *spatial)
119{
120 m3 rotation = glm::toMat3(normalize(spatial->rotation));
121 obb.center = spatial->position + (rotation * (spatial->scale * obb.center));
122 obb.x_axis = normalize(rotation * obb.x_axis);
123 obb.y_axis = normalize(rotation * obb.y_axis);
124 obb.extents *= spatial->scale;
125 return obb;
126}
127
128
129physics::RaycastResult
130physics::intersect_obb_ray(spatial::Obb *obb, spatial::Ray *ray)
131{
132 // Gabor Szauer, Game Physics Cookbook, “Raycast Oriented Bounding Box”
133 v3 obb_z_axis = cross(obb->x_axis, obb->y_axis);
134
135 // Get vector pointing from origin of ray to center of OBB
136 v3 p = obb->center - ray->origin;
137
138 // Project direction of ray onto each axis of OBB
139 v3 f = v3(
140 dot(obb->x_axis, ray->direction),
141 dot(obb->y_axis, ray->direction),
142 dot(obb_z_axis, ray->direction));
143
144 // Project p into every axis of OBB
145 v3 e = v3(
146 dot(obb->x_axis, p),
147 dot(obb->y_axis, p),
148 dot(obb_z_axis, p));
149
150 // Calculate slab intersection points for ray
151 // `t is the distance along the ray (or “time” along the ray, as Szauer
152 // calls it) that the intersection happens at.
153 f32 t[6] = {};
154 range_named (i, 0, 3) {
155 if (f[i] == 0) {
156 if (-e[i] - obb->extents[i] > 0 || -e[i] + obb->extents[i] < 0) {
157 // If the ray is parallel to the slab being tested, and the origin
158 // of the ray is not inside the slab, we have no hit.
159 return {};
160 }
161 f[i] = 0.00001f; // Avoid division by zero
162 }
163 t[i * 2 + 0] = (e[i] + obb->extents[i]) / f[i]; // min
164 t[i * 2 + 1] = (e[i] - obb->extents[i]) / f[i]; // max
165 }
166
167 // After the above loop, we've hit all three slabs. We now need to find the
168 // largest minimum `t^{min}` and smallest maximum `t^{max}`.
169 f32 tmin = max(
170 max(min(t[0], t[1]), min(t[2], t[3])),
171 min(t[4], t[5]));
172 f32 tmax = min(
173 min(max(t[0], t[1]), max(t[2], t[3])),
174 max(t[4], t[5]));
175
176 // If `tmax` < 0, the ray is intersecting the OBB in the negative direction.
177 // This means the OBB is behind the origin of the ray, and this should not
178 // count as an intersection.
179 if (tmax < 0.0f) {
180 return {};
181 }
182
183 // If `tmin` > `tmax`, the ray does not intersect the OBB.
184 if (tmin > tmax) {
185 return {};
186 }
187
188 // If `tmin` < 0, the ray started inside of the OBB. This means `tmax` is a
189 // valid intersection.
190 if (tmin < 0.0f) {
191 return {
192 .did_intersect = true,
193 .distance = tmax,
194 };
195 }
196
197 return {
198 .did_intersect = true,
199 .distance = tmin,
200 };
201}
202
203
204bool
205physics::is_component_valid(physics::Component *physics_component) {
206 return physics_component->obb.extents.x > 0;
207}
208
209
210/*!
211 This function gets the nearest contact point between two edges. It's used
212 to determine a collision point for a box collision that has happened
213 edge-to-edge.
214
215 should_use_a_midpoint
216 ---------------------
217 If this is true, and the contact point is outside the edge (in the case of
218 an edge-face contact) then we use a's midpoint, otherwise we use b's.
219
220 Resources
221 ---------
222 This function is heavily based on code from Ian Millington's Cyclone Physics
223 engine.
224
225 idmillington/cyclone-physics/blob/master/src/collide_fine.cpp#contactPoint()
226*/
227v3
228physics::get_edge_contact_point(
229 v3 a_edge_point,
230 v3 a_axis,
231 f32 a_axis_length,
232 v3 b_edge_point,
233 v3 b_axis,
234 f32 b_axis_length,
235 bool should_use_a_midpoint
236) {
237 f32 a_axis_sqlen = length2(a_axis);
238 f32 b_axis_sqlen = length2(b_axis);
239 f32 a_b_axes_dotprod = dot(b_axis, a_axis);
240
241 v3 a_ep_to_b_ep = a_edge_point - b_edge_point;
242 f32 a_ep_projection = dot(a_axis, a_ep_to_b_ep);
243 f32 b_ep_projection = dot(b_axis, a_ep_to_b_ep);
244
245 f32 denom = a_axis_sqlen * b_axis_sqlen - a_b_axes_dotprod * a_b_axes_dotprod;
246
247 // Zero denominator indicates parallel lines
248 if (abs(denom) < 0.0001f) {
249 return should_use_a_midpoint ? a_edge_point : b_edge_point;
250 }
251
252 f32 mua = (a_b_axes_dotprod * b_ep_projection - b_axis_sqlen * a_ep_projection) / denom;
253 f32 mub = (a_axis_sqlen * b_ep_projection - a_b_axes_dotprod * a_ep_projection) / denom;
254
255 // If either of the edges has the nearest point out of bounds, then the edges
256 // aren't crossed, we have an edge-face contact. Our point is on the edge,
257 // which we know from the should_use_a_midpoint parameter.
258 if (mua > a_axis_length || mua < -a_axis_length || mub > b_axis_length || mub < -b_axis_length) {
259 return should_use_a_midpoint ? a_edge_point : b_edge_point;
260 } else {
261 v3 contact_a_component = a_edge_point + a_axis * mua;
262 v3 contact_b_component = b_edge_point + b_axis * mub;
263
264 return contact_a_component * 0.5f + contact_b_component * 0.5f;
265 }
266}
267
268
269spatial::Face
270physics::get_incident_face(
271 m3 *cob, // incident change of base
272 v3 e, // incident extents
273 v3 c, // incident center
274 v3 n // incident normal
275) {
276 spatial::Face face;
277 n = transpose(*cob) * n;
278 v3 abs_n = abs(n);
279
280 if (abs_n.x > abs_n.y && abs_n.x > abs_n.z) {
281 if (n.x > 0.0f) {
282 face = {
283 .vertices = {
284 v3(e.x, e.y, -e.z),
285 v3(e.x, e.y, e.z),
286 v3(e.x, -e.y, e.z),
287 v3(e.x, -e.y, -e.z),
288 },
289 };
290 } else {
291 face = {
292 .vertices = {
293 v3(-e.x, -e.y, e.z),
294 v3(-e.x, e.y, e.z),
295 v3(-e.x, e.y, -e.z),
296 v3(-e.x, -e.y, -e.z),
297 },
298 };
299 }
300 } else if (abs_n.y > abs_n.x && abs_n.y > abs_n.z) {
301 if (n.y > 0.0f) {
302 face = {
303 .vertices = {
304 v3(-e.x, e.y, e.z),
305 v3( e.x, e.y, e.z),
306 v3( e.x, e.y, -e.z),
307 v3(-e.x, e.y, -e.z),
308 },
309 };
310 } else {
311 face = {
312 .vertices = {
313 v3( e.x, -e.y, e.z),
314 v3(-e.x, -e.y, e.z),
315 v3(-e.x, -e.y, -e.z),
316 v3( e.x, -e.y, -e.z),
317 },
318 };
319 }
320 } else {
321 if (n.z > 0.0f) {
322 face = {
323 .vertices = {
324 v3(-e.x, e.y, e.z),
325 v3(-e.x, -e.y, e.z),
326 v3( e.x, -e.y, e.z),
327 v3( e.x, e.y, e.z),
328 },
329 };
330 } else {
331 face = {
332 .vertices = {
333 v3( e.x, -e.y, -e.z),
334 v3(-e.x, -e.y, -e.z),
335 v3(-e.x, e.y, -e.z),
336 v3( e.x, e.y, -e.z),
337 },
338 };
339 }
340 }
341
342 range (0, 4) {
343 face.vertices[idx] = *cob * face.vertices[idx] + c;
344 }
345
346 return face;
347}
348
349
350void
351physics::get_reference_face_edges_and_basis(
352 m3 *cob, // object change of base
353 v3 e, // object extents
354 v3 c, // object center
355 v3 n, // collision normal
356 u32 axis, // axis of separation
357 u32 clip_edges[4], // the indices of the reference face edges
358 m3 *reference_face_cob, // the change of basis of the reference face
359 v3 *reference_face_e // the extents of the reference face
360) {
361 n = transpose(*cob) * n;
362
363 if (axis >= 3) {
364 axis -= 3;
365 }
366
367 if (axis == 0) {
368 if (n.x > 0.0f) {
369 clip_edges[0] = 1;
370 clip_edges[1] = 8;
371 clip_edges[2] = 7;
372 clip_edges[3] = 9;
373 row(*reference_face_cob, 0, row(*cob, 1));
374 row(*reference_face_cob, 1, row(*cob, 2));
375 row(*reference_face_cob, 2, row(*cob, 0));
376 *reference_face_e = v3(e.y, e.z, e.x);
377 } else {
378 clip_edges[0] = 11;
379 clip_edges[1] = 3;
380 clip_edges[2] = 10;
381 clip_edges[3] = 5;
382 row(*reference_face_cob, 0, row(*cob, 2));
383 row(*reference_face_cob, 1, row(*cob, 1));
384 row(*reference_face_cob, 2, -row(*cob, 0));
385 *reference_face_e = v3(e.z, e.y, e.x);
386 }
387 } else if (axis == 1) {
388 if (n.y > 0.0f) {
389 clip_edges[0] = 0;
390 clip_edges[1] = 1;
391 clip_edges[2] = 2;
392 clip_edges[3] = 3;
393 row(*reference_face_cob, 0, row(*cob, 2));
394 row(*reference_face_cob, 1, row(*cob, 0));
395 row(*reference_face_cob, 2, row(*cob, 1));
396 *reference_face_e = v3(e.z, e.x, e.y);
397 } else {
398 clip_edges[0] = 4;
399 clip_edges[1] = 5;
400 clip_edges[2] = 6;
401 clip_edges[3] = 7;
402 row(*reference_face_cob, 0, row(*cob, 2));
403 row(*reference_face_cob, 1, -row(*cob, 0));
404 row(*reference_face_cob, 2, -row(*cob, 1));
405 *reference_face_e = v3(e.z, e.x, e.y);
406 }
407 } else if (axis == 2) {
408 if (n.z > 0.0f) {
409 clip_edges[0] = 11;
410 clip_edges[1] = 4;
411 clip_edges[2] = 8;
412 clip_edges[3] = 0;
413 row(*reference_face_cob, 0, -row(*cob, 1));
414 row(*reference_face_cob, 1, row(*cob, 0));
415 row(*reference_face_cob, 2, row(*cob, 2));
416 *reference_face_e = v3(e.y, e.x, e.z);
417 } else {
418 clip_edges[0] = 6;
419 clip_edges[1] = 10;
420 clip_edges[2] = 2;
421 clip_edges[3] = 9;
422 row(*reference_face_cob, 0, -row(*cob, 1));
423 row(*reference_face_cob, 1, -row(*cob, 0));
424 row(*reference_face_cob, 2, -row(*cob, 2));
425 *reference_face_e = v3(e.y, e.x, e.z);
426 }
427 }
428}
429
430
431u32
432physics::clip_faces(
433 v3 reference_center, v3 reference_face_extents,
434 u32 clip_edges[4], m3 reference_face_cob,
435 spatial::Face incident_face,
436 v3 clip_vertices[8], f32 clip_depths[8]
437) {
438 return 0;
439}
440
441
442void
443physics::update_best_for_face_axis(
444 f32 *best_sep, u32 *best_axis, v3 *best_normal,
445 f32 sep, u32 axis, v3 normal
446) {
447 if (sep > *best_sep) {
448 *best_sep = sep;
449 *best_axis = axis;
450 *best_normal = normal;
451 }
452}
453
454
455void
456physics::update_best_for_edge_axis(
457 f32 *best_sep, u32 *best_axis, v3 *best_normal,
458 f32 sep, u32 axis, v3 normal
459) {
460 f32 normal_len = length(normal);
461 sep /= normal_len;
462 if (sep > *best_sep) {
463 *best_sep = sep;
464 *best_axis = axis;
465 *best_normal = normal / normal_len;
466 }
467}
468
469
470/*!
471 This function implements collision detection between two OBBs.
472
473 We're using the separating axis test (SAT) to check which axes, if any,
474 separates the two.
475
476 For manifold generation, we're using the methods described by Dirk Gregorius,
477 namely Sutherland-Hodgman clipping for face-something, and "just find the
478 closes two points on the edges" for edge-edge.
479
480 A note about normal calculation for the cross axes
481 --------------------------------------------------
482 Normally, we would calculate the normal as the axis we're using,
483 so the cross product between the a axis and the b axis. We're not
484 actually calculating this directly for SAT, because we're using the r
485 matrix as a way around this. However, we do need this axis for the normal.
486 Randy Gaul calculates a normal from the r matrix, which I have included
487 as a comment. However, this is not orthogonal to both a's axis and b's axis.
488 This might still be fine but I've left the cross product in, to be safe.
489 We might look into using the r matrix method as an optimisation.
490
491 Resources
492 ---------
493 * Christer Ericson, Real-Time Collision Detection, 4.4
494 * Dirk Gregorius's GDC 2013 and GDC 2015 talks
495 * Randy Gaul's blog post "Deriving OBB to OBB Intersection and Manifold Generation"
496 * Ian Millington's Cyclone Physics engine (but not for face-something!)
497*/
498physics::CollisionManifold
499physics::intersect_obb_obb(
500 spatial::Obb *a,
501 spatial::Obb *b,
502 spatial::Component *spatial_a,
503 spatial::Component *spatial_b
504) {
505 // The radius from a/b's center to its outer vertex
506 f32 a_radius, b_radius;
507 // The distance between a and b
508 f32 a_to_b;
509 // The separation between a and b
510 f32 sep;
511 // The rotation matrix expression b in a's coordinate frame
512 m3 r;
513 // abs(r) is used in a lot of calculations so we precompute it
514 m3 abs_r;
515 // We need to keep track of the normal on the edge axes
516 v3 normal;
517
518 v3 a_axes[3] = { a->x_axis, a->y_axis, cross(a->x_axis, a->y_axis) };
519 v3 b_axes[3] = { b->x_axis, b->y_axis, cross(b->x_axis, b->y_axis) };
520
521 // Change basis into world space (cob = change of base)
522 m3 a_cob = m3(a_axes[0], a_axes[1], a_axes[2]);
523 m3 b_cob = m3(b_axes[0], b_axes[1], b_axes[2]);
524
525 // Compute rotation matrix expressing b in a's coordinate frame
526 range_named (i, 0, 3) {
527 range_named (j, 0, 3) {
528 r[i][j] = dot(a_axes[i], b_axes[j]);
529 }
530 }
531
532 // Compute translation vector
533 v3 t_translation = b->center - a->center;
534 gui::log(
535 "a: center (%f, %f, %f) extents (%f, %f, %f)",
536 a->center.x, a->center.y, a->center.z,
537 a->extents[0], a->extents[1], a->extents[2]
538 );
539 gui::log(
540 "b: center (%f, %f, %f) extents (%f, %f, %f)",
541 b->center.x, b->center.y, b->center.z,
542 b->extents[0], b->extents[1], b->extents[2]
543 );
544
545 // Bring translation into a's coordinate frame
546 v3 t = v3(
547 dot(t_translation, a_axes[0]),
548 dot(t_translation, a_axes[1]),
549 dot(t_translation, a_axes[2])
550 );
551
552 // If the two OBBs share one axis, we can skip checking their cross product
553 // axes altogether. At the very least, if e.g. a.x and b.x are parallel,
554 // their cross product will give us something we can't use. I'm not sure why
555 // we're not skipping the specific axes specifically, and we're skipping
556 // everything instead.
557 bool do_obbs_share_one_axis = false;
558
559 // Compute common subexpressions. Add in an epsilon term to counteract
560 // arithmetic errors when two edges are parallel and their cross product
561 // is (near) null.
562 range_named (i, 0, 3) {
563 range_named (j, 0, 3) {
564 abs_r[i][j] = abs(r[i][j]) + physics::PARALLEL_FACE_TOLERANCE;
565 if (abs_r[i][j] >= 1.0f) {
566 do_obbs_share_one_axis = true;
567 }
568 }
569 }
570
571 f32 a_face_max_sep = -FLT_MAX;
572 u32 a_face_best_axis = 0;
573 v3 a_face_best_normal = v3(0.0f);
574 f32 b_face_max_sep = -FLT_MAX;
575 u32 b_face_best_axis = 0;
576 v3 b_face_best_normal = v3(0.0f);
577 f32 edge_max_sep = -FLT_MAX;
578 u32 edge_best_axis = 0;
579 v3 edge_best_normal = v3(0.0f);
580
581 // Test a's face axes (a.x, a.y, a.z)
582 range_named (i, 0, 3) {
583 a_radius = a->extents[i];
584 b_radius = b->extents[0] * abs_r[i][0] +
585 b->extents[1] * abs_r[i][1] +
586 b->extents[2] * abs_r[i][2];
587 a_to_b = abs(t[i]);
588 sep = a_to_b - (a_radius + b_radius);
589 if (sep > 0) { return physics::CollisionManifold {}; }
590 update_best_for_face_axis(
591 &a_face_max_sep, &a_face_best_axis, &a_face_best_normal, sep, i, a_axes[i]);
592 }
593
594 // Test b's face axes (b.x, b.y, b.z)
595 range_named (i, 0, 3) {
596 a_radius = a->extents[0] * abs_r[0][i] +
597 a->extents[1] * abs_r[1][i] +
598 a->extents[2] * abs_r[2][i];
599 b_radius = b->extents[i];
600 a_to_b = abs(t[0] * r[0][i] + t[1] * r[1][i] + t[2] * r[2][i]);
601 sep = a_to_b - (a_radius + b_radius);
602 if (sep > 0) { return physics::CollisionManifold {}; }
603 update_best_for_face_axis(
604 &b_face_max_sep, &b_face_best_axis, &b_face_best_normal, sep, 3 + i, b_axes[i]);
605 }
606
607 if (!do_obbs_share_one_axis) {
608 // Test cross axes (a[i] x b[j])
609 range_named(i, 0, 3) {
610 range_named(j, 0, 3) {
611 // These numbers look really crazy, but it's not so bad if you look at
612 // the table they come from.
613 // See Christer Ericson, Real-Time Collision Detection, Table 4.1
614 a_radius =
615 a->extents[i == 0 ? 1 : 0] * abs_r[i < 2 ? 2 : 1][j] +
616 a->extents[i < 2 ? 2 : 1] * abs_r[i == 0 ? 1 : 0][j];
617 b_radius =
618 b->extents[j == 0 ? 1 : 0] * abs_r[i][j < 2 ? 2 : 1] +
619 b->extents[j < 2 ? 2 : 1] * abs_r[i][j == 0 ? 1 : 0];
620 a_to_b = abs(
621 t[(2 + i) % 3] * r[(1 + i) % 3][j] -
622 t[(1 + i) % 3] * r[(2 + i) % 3][j]
623 );
624 sep = a_to_b - (a_radius + b_radius);
625 if (sep > 0) { return physics::CollisionManifold {}; }
626 normal = normalize(cross(a_axes[i], b_axes[j]));
627 update_best_for_edge_axis(
628 &edge_max_sep, &edge_best_axis, &edge_best_normal, sep, 6 + i + j, normal);
629 }
630 }
631 }
632
633 // Find the best option for the face cases
634 f32 face_max_sep;
635 u32 face_best_axis;
636 if (a_face_max_sep > b_face_max_sep) {
637 face_max_sep = a_face_max_sep;
638 face_best_axis = a_face_best_axis;
639 } else {
640 face_max_sep = b_face_max_sep;
641 face_best_axis = b_face_best_axis;
642 }
643
644 // TODO: Remove this debugging code
645 {
646 v3 face_best_normal;
647 if (a_face_max_sep > b_face_max_sep) {
648 face_best_normal = a_face_best_normal;
649 } else {
650 face_best_normal = b_face_best_normal;
651 }
652 gui::log("a_face_max_sep %f", a_face_max_sep);
653 gui::log("b_face_max_sep %f", b_face_max_sep);
654 gui::log(
655 "(face_max_sep (real %f) (adjusted %f) (face_best_axis %d) (face_best_normal %f %f %f))",
656 face_max_sep,
657 face_max_sep + physics::ABSOLUTE_TOLERANCE,
658 face_best_axis,
659 face_best_normal.x, face_best_normal.y, face_best_normal.z);
660 gui::log(
661 "(edge_max_sep (real %f) (adjusted %f) (edge_best_axis %d) (edge_best_normal %f %f %f))",
662 edge_max_sep,
663 edge_max_sep * physics::RELATIVE_TOLERANCE,
664 edge_best_axis,
665 edge_best_normal.x, edge_best_normal.y, edge_best_normal.z);
666 }
667
668 // Set manifold to our best option while taking tolerances into account
669 // We use an artificial axis bias to improve frame coherence
670 // (i.e. stop things from jumping between edge and face in nonsensical ways)
671 physics::CollisionManifold manifold;
672 if (edge_max_sep * physics::RELATIVE_TOLERANCE > face_max_sep + physics::ABSOLUTE_TOLERANCE) {
673 manifold.sep_max = edge_max_sep;
674 manifold.axis = edge_best_axis;
675 manifold.normal = edge_best_normal;
676 } else {
677 if (b_face_max_sep * physics::RELATIVE_TOLERANCE > a_face_max_sep + physics::ABSOLUTE_TOLERANCE) {
678 manifold.sep_max = b_face_max_sep;
679 manifold.axis = b_face_best_axis;
680 manifold.normal = b_face_best_normal;
681 } else {
682 manifold.sep_max = a_face_max_sep;
683 manifold.axis = a_face_best_axis;
684 manifold.normal = a_face_best_normal;
685 }
686 }
687
688 // Correct normal direction
689 if (dot(manifold.normal, t_translation) < 0.0f) {
690 manifold.normal = -manifold.normal;
691 }
692
693 if (manifold.axis < 6) {
694 // spatial::Face-something collision
695 v3 reference_extents, incident_extents, reference_center, incident_center;
696 m3 reference_cob, incident_cob;
697
698 if (manifold.axis < 3) {
699 manifold.normal = -manifold.normal;
700 reference_extents = a->extents;
701 reference_cob = a_cob;
702 reference_center = a->center;
703 incident_extents = b->extents;
704 incident_cob = b_cob;
705 incident_center = b->center;
706 } else {
707 reference_extents = b->extents;
708 reference_cob = b_cob;
709 reference_center = b->center;
710 incident_extents = a->extents;
711 incident_cob = a_cob;
712 incident_center = a->center;
713 }
714
715 spatial::Face incident_face = get_incident_face(&incident_cob, incident_extents, incident_center, manifold.normal);
716
717 u32 clip_edges[4];
718 m3 reference_face_cob;
719 v3 reference_face_extents;
720 get_reference_face_edges_and_basis(
721 &reference_cob, reference_extents, reference_center, manifold.normal,
722 manifold.axis, clip_edges, &reference_face_cob, &reference_face_extents);
723
724 u32 n_clip_vertices;
725 v3 clip_vertices[8];
726 f32 clip_depths[8];
727 n_clip_vertices = clip_faces(
728 reference_center, reference_face_extents,
729 clip_edges, reference_face_cob,
730 incident_face,
731 clip_vertices, clip_depths);
732
733 debugdraw::draw_quad(
734 incident_face.vertices[0],
735 incident_face.vertices[1],
736 incident_face.vertices[2],
737 incident_face.vertices[3],
738 v4(0.0f, 1.0f, 0.0f, 1.0f));
739 debugdraw::draw_point(
740 incident_face.vertices[0],
741 0.1f,
742 v4(0.0f, 1.0f, 0.0f, 1.0f));
743 debugdraw::draw_point(
744 incident_face.vertices[1],
745 0.1f,
746 v4(0.0f, 1.0f, 0.0f, 1.0f));
747 debugdraw::draw_point(
748 incident_face.vertices[2],
749 0.1f,
750 v4(0.0f, 1.0f, 0.0f, 1.0f));
751 debugdraw::draw_point(
752 incident_face.vertices[3],
753 0.1f,
754 v4(0.0f, 1.0f, 0.0f, 1.0f));
755 } else {
756 // Edge-edge collision
757 u32 edge_axis = manifold.axis - 6;
758 u32 a_axis = edge_axis / 3;
759 u32 b_axis = edge_axis % 3;
760
761 v3 a_edge_point = a->extents;
762 v3 b_edge_point = b->extents;
763 range_named (i, 0, 3) {
764 if (i == a_axis) {
765 a_edge_point[i] = 0;
766 } else if (dot(a_axes[i], manifold.normal) < 0) {
767 a_edge_point[i] = -a_edge_point[i];
768 }
769
770 if (i == b_axis) {
771 b_edge_point[i] = 0;
772 } else if (dot(b_axes[i], manifold.normal) > 0) {
773 b_edge_point[i] = -b_edge_point[i];
774 }
775 }
776
777 a_edge_point = a_cob * a_edge_point + a->center;
778 b_edge_point = b_cob * b_edge_point + b->center;
779
780 v3 contact_point = get_edge_contact_point(
781 a_edge_point,
782 a_axes[a_axis],
783 a->extents[a_axis],
784 b_edge_point,
785 b_axes[b_axis],
786 b->extents[b_axis],
787 face_best_axis >= 3);
788 debugdraw::draw_point(
789 contact_point,
790 0.1f,
791 v4(0.0f, 1.0f, 0.0f, 1.0f));
792 }
793
794 // Since no separating axis is found, the OBBs must be intersecting
795 // NOTE: manifold.collidee should be filled in by the caller.
796 manifold.did_collide = true;
797 return manifold;
798}