AABB and sphere collision detection

collision_detection.c

// This file shows the algorithms for detecting 3D AABB-AABB, sphere-sphere and AABB-sphere collisions together with collision normal and penetration.
// - This was thought for use with an ECS system, specifically flecs. https://www.flecs.dev/flecs/
// - Based on https://gamedevelopment.tutsplus.com/how-to-create-a-custom-2d-physics-engine-the-basics-and-impulse-resolution--gamedev-6331t
// - This won't compile as-is, you have to provide your own math functions.
// - An AABB is represented by a center, min and max coordinates. The code assumes that an AABB's min corners coords are always less than the max corners.
// - A sphere is represented by a center and a radius.
// - Uses double-precision floating point numbers for translation, penetration, AABBs and sphere's radius. Single precision for everything else. You can change this easily.
// - Supports scale.
//   - For AABBs, scale is applied to its extents.
//   - For spheres, scale vector components are averaged and applied to the radius. That is, it works intuitively only with a uniform scale.
//   - Only positive scales have been tested.
// - Licensed under the WTFPL: http://www.wtfpl.net/

// Drop a comment if you have suggestions or improvements!

typedef struct vec3 {
  float x;
  float y;
  float z;
} vec3;

typedef struct dvec3 {
  double x;
  double y;
  double z;
} dvec3;

// Multiplication is cheaper, and the compiler will likely not optimize a division.
#define VEC3_AVERAGE(vec) (((vec).x + (vec).y + (vec).z) * .33333333333f)
#define DVEC3_TO_VEC3(vec) (vec3){{ (float)(vec).x, (float)(vec).y, (float)(vec).z }}
#define VEC3_TO_DVEC3(vec) (dvec3){{ (double)(vec).x, (double)(vec).y, (double)(vec).z }}
#define INVERT_VEC3(vec) (vec3){{ -(vec).x, -(vec).y, -(vec).z }}

typedef uint64_t ecs_entity_t;

typedef struct manifold {
  ecs_entity_t a, b;
  double penetration;
  vec3 normal;
} manifold;

// ECS components.

typedef dvec3 Translation;
typedef vec3 Scale;
typedef struct Aabb {
  dvec3 min;
  dvec3 max;
} Aabb;
typedef double Radius;

// Functions.

bool aabb_vs_aabb(
  manifold* manifold,
  const Translation* a_translation,
  const Translation* b_translation,
  const Aabb* a_aabb,
  const Aabb* b_aabb,
  const Scale* a_scale,
  const Scale* b_scale
) {
  dvec3 result, result2;

  dvec3_sub(&a_aabb->max, &a_aabb->min, &result);
  dvec3 a_half_extents;
  dvec3_mul_scalar(&result, .5, &a_half_extents);

  dvec3_sub(&b_aabb->max, &b_aabb->min, &result);
  dvec3 b_half_extents;
  dvec3_mul_scalar(&result, .5, &b_half_extents);

  dvec3_add(&a_aabb->min, a_translation, &result);
  dvec3 a_center;
  dvec3_add(&result, &a_half_extents, &a_center);
  dvec3_add(&b_aabb->min, b_translation, &result);
  dvec3 b_center;
  dvec3_add(&result, &b_half_extents, &b_center);

  if (a_scale) {
    const dvec3 dscale = VEC3_TO_DVEC3(*a_scale);
    dvec3_mul(&a_half_extents, &dscale, &a_half_extents);
  }

  if (b_scale) {
    const dvec3 dscale = VEC3_TO_DVEC3(*b_scale);
    dvec3_mul(&b_half_extents, &dscale, &b_half_extents);
  }

  dvec3 a_to_b;
  dvec3_sub(&b_center, &a_center, &a_to_b);
  dvec3 overlaps;
  dvec3_add(&a_half_extents, &b_half_extents, &result);
  dvec3_abs(&a_to_b, &result2);
  dvec3_sub(&result, &result2, &overlaps);

  if (overlaps.x > 0 && overlaps.y > 0 && overlaps.z > 0) {
    if (!manifold)
      return true;

    if (overlaps.x < overlaps.y) {
      if (overlaps.x < overlaps.z) {
        // x is the axis of least penetration.
        if (a_to_b.x > 0)
          manifold->normal = (vec3){ { 1.f, 0.f, 0.f } };
        else
          manifold->normal = (vec3){ { -1.f, 0.f, 0.f } };
        manifold->penetration = overlaps.x;
        return true;
      }

      // z is the axis of least penetration.
      if (a_to_b.z > 0)
        manifold->normal = (vec3){ { 0.f, 0.f, 1.f } };
      else
        manifold->normal = (vec3){ { 0.f, 0.f, -1.f } };
      manifold->penetration = overlaps.z;
      return true;
    }

    if (overlaps.y < overlaps.z) {
      // y is the axis of least penetration.
      if (a_to_b.y > 0)
        manifold->normal = (vec3){ { 0.f, 1.f, 0.f } };
      else
        manifold->normal = (vec3){ { 0.f, -1.f, 0.f } };
      manifold->penetration = overlaps.y;
      return true;
    }

    // z is the axis of least penetration.
    if (a_to_b.z > 0)
      manifold->normal = (vec3){ { 0.f, 0.f, 1.f } };
    else
      manifold->normal = (vec3){ { 0.f, 0.f, -1.f } };
    manifold->penetration = overlaps.z;
    return true;
  }

  return false;
}

bool sphere_vs_sphere(
  manifold* manifold,
  const Translation* a_translation,
  const Translation* b_translation,
  Radius a_radius,
  Radius b_radius,
  const Scale* a_scale,
  const Scale* b_scale
) {
  if (a_scale)
    a_radius *= VEC3_AVERAGE(*a_scale);
  if (b_scale)
    b_radius *= VEC3_AVERAGE(*b_scale);

  Radius radiuses_sum2 = a_radius + b_radius;
  radiuses_sum2 *= radiuses_sum2;

  const bool collision = dvec3_distance2(a_translation, b_translation) < radiuses_sum2;
  if (!manifold)
    return collision;

  if (collision) {
    dvec3 normal;
    dvec3_sub(b_translation, a_translation, &normal);
    const double distance = dvec3_length(&normal);

    if (distance != 0.) {
      dvec3_div_scalar(&normal, distance, &normal);
      manifold->normal = DVEC3_TO_VEC3(normal);
      manifold->penetration = a_radius + b_radius - distance;
    } else {
      // Arbitrary values.
      manifold->normal = (vec3){ { 1.f, 0.f, 0.f } };
      manifold->penetration = a_radius;
    }
  }

  return collision;
}

bool aabb_vs_sphere(
  manifold* manifold,
  const Translation* aabb_translation,
  const Translation* sphere_translation,
  const Aabb* aabb,
  Radius radius,
  const Scale* aabb_scale,
  const Scale* sphere_scale
) {
  if (sphere_scale)
    radius *= VEC3_AVERAGE(*sphere_scale);

  dvec3 result, dscale;

  dvec3_sub(&aabb->max, &aabb->min, &result);
  if (aabb_scale) {
    dscale = VEC3_TO_DVEC3(*aabb_scale);
    dvec3_mul(&result, &dscale, &result);
  }
  dvec3 half_extents;
  dvec3_mul_scalar(&result, .5, &half_extents);

  dvec3_add(&aabb->min, aabb_translation, &result);
  dvec3 aabb_center;
  dvec3_add(&result, &half_extents, &aabb_center);

  dvec3 a_to_b;
  dvec3_sub(sphere_translation, &aabb_center, &a_to_b);

  const dvec3 min = (dvec3){ { -half_extents.x, -half_extents.y, -half_extents.z } };
  dvec3 closest = a_to_b;
  dvec3_clamp(&closest, &min, &half_extents, &closest);

  bool inside = false;
  if (dvec3_equals(&closest, &a_to_b)) {
    inside = true;

    // Find closest axis and clamp to closest extent.
    if (fabs(closest.x) > fabs(closest.y))
      if (fabs(closest.x) > fabs(closest.z))
        // x is the closest axis.
        if (closest.x > 0)
          closest.x = half_extents.x;
        else
          closest.x = -half_extents.x;
      else
        // z is the closest axis.
        if (closest.z > 0)
          closest.z = half_extents.z;
        else
          closest.z = -half_extents.z;
    else
      if (fabs(closest.y) > fabs(closest.z))
        // y is the closest axis.
        if (closest.y > 0)
          closest.y = half_extents.y;
        else
          closest.y = -half_extents.y;
      else
        // z is the closest axis.
        if (closest.z > 0)
          closest.z = half_extents.z;
        else
          closest.z = -half_extents.z;
  }

  dvec3 normal;
  dvec3_sub(&a_to_b, &closest, &normal);
  double length = dvec3_length2(&normal);

  // Early out if the radius is shorter than distance to closest point and sphere is not inside the AABB.
  if (!inside && length >= radius * radius)
    return false;

  length = sqrt(length);

  // Collision normal needs to be flipped to point outside if circle was inside the AABB.
  if (inside) {
    manifold->normal = INVERT_VEC3(normal);
    manifold->penetration = radius + length;
  } else {
    manifold->normal = DVEC3_TO_VEC3(normal);
    manifold->penetration = radius - length;
  }
  glm_vec3_divs(manifold->normal.raw, (float)length, manifold->normal.raw);

  return true;
}