dusk/src/dusk/physics/physics.c

/**
 * Copyright (c) 2025 Dominic Masters
 * 
 * This software is released under the MIT License.
 * https://opensource.org/licenses/MIT
 */

#include "physics.h"
#include "world/tiledata.h"

collisionresult_t physicsCheckCircleCircle(
  float_t circle0x, float_t circle0y, float_t circle0r,
  float_t circle1x, float_t circle1y, float_t circle1r
) {
  collisionresult_t result;
  // Compute vector between centers
  float_t dx = circle1x - circle0x;
  float_t dy = circle1y - circle0y;

  // Distance squared between centers
  float_t distSq = (dx * dx) + (dy * dy);

  // Sum of radii
  float_t rSum = circle0r + circle1r;
  float_t rSumSq = rSum * rSum;

  if(distSq > rSumSq) {
    // No collision
    result.hit = false;
    return result;
  }

  // Collision: calculate normal and penetration depth
  float_t dist = sqrtf(distSq);

  // If centers are the same, pick arbitrary normal (1,0)
  if(dist == 0) {
    result.normalX = 1;
    result.normalY = 0;
    result.depth = rSum;
  } else {
    // Normalized direction from circle0 to circle1
    result.normalX = dx / dist;
    result.normalY = dy / dist;
    // Penetration depth = sum of radii - distance
    result.depth = rSum - dist;
  }
  result.hit = true;
  return result;
}

collisionresult_t physicsCheckCircleAABB(
  float_t circleX, float_t circleY, float_t circleR,
  float_t aabbX, float_t aabbY,
  float_t aabbWidth, float_t aabbHeight
) {
  collisionresult_t result;

  // Find the closest point on the AABB to the circle center
  float_t closestX = fmaxf(
    aabbX, fminf(circleX, aabbX + aabbWidth)
  );
  float_t closestY = fmaxf(
    aabbY, fminf(circleY, aabbY + aabbHeight)
  );

  // Vector from circle center to closest point
  float_t dx = closestX - circleX;
  float_t dy = closestY - circleY;

  // Distance squared from circle center to closest point
  float_t distSq = (dx * dx) + (dy * dy);

  // Check if distance is less than radius squared
  if(distSq > (circleR * circleR)) {
    result.hit = false;
    return result;
  }

  // Collision: calculate normal and penetration depth
  float_t dist = sqrtf(distSq);

  if(dist <= 1) {
    // Circle center is at the AABB corner
    result.normalX = 1.0f;
    result.normalY = 0.0f;
    result.depth = circleR;
  } else {
    // Normalized direction from circle center to closest point
    result.normalX = dx / dist;
    result.normalY = dy / dist;
    // Penetration depth = radius - distance
    result.depth = circleR - dist;
  }
  
  result.hit = true;
  return result;
}

void physicsClosestPointOnSegment(
  float_t ax, float_t ay,
  float_t bx, float_t by,
  float_t px, float_t py,
  float_t *outX, float_t *outY
) {
  float_t abx = bx - ax;
  float_t aby = by - ay;
  float_t apx = px - ax;
  float_t apy = py - ay;

  float_t abLenSq = (abx * abx) + (aby * aby);

  if(abLenSq == 0) {
    *outX = ax;
    *outY = ay;
    return;
  }

  float_t t = apx * abx + apy * aby;
  t /= abLenSq;

  if(t < 0) t = 0;
  if(t > 1)  t = 1;

  *outX = ax + (abx * t);
  *outY = ay + (aby * t);
}

bool_t physicsIsPointInTriangle(
  float_t px, float_t py,
  float_t ax, float_t ay,
  float_t bx, float_t by,
  float_t cx, float_t cy
) {
  float_t abx = bx - ax;
  float_t aby = by - ay;
  float_t bcx = cx - bx;
  float_t bcy = cy - by;
  float_t cax = ax - cx;
  float_t cay = ay - cy;

  float_t apx = px - ax;
  float_t apy = py - ay;
  float_t bpx = px - bx;
  float_t bpy = py - by;
  float_t cpx = px - cx;
  float_t cpy = py - cy;

  float_t cross1 = (abx * apy) - (aby * apx);
  float_t cross2 = (bcx * bpy) - (bcy * bpx);
  float_t cross3 = (cax * cpy) - (cay * cpx);

  bool_t hasNeg = (
    (cross1 < 0) ||
    (cross2 < 0) ||
    (cross3 < 0)
  );
  
  bool_t hasPos = (
    (cross1 > 0) ||
    (cross2 > 0) ||
    (cross3 > 0)
  );

  return !(hasNeg && hasPos);
}

collisionresult_t physicsCheckCircleTriangle(
  float_t circleX, float_t circleY, float_t circleR,
  float_t triX0, float_t triY0,
  float_t triX1, float_t triY1,
  float_t triX2, float_t triY2
) {
  collisionresult_t result = { .hit = false };
  float_t vx[3] = { triX0, triX1, triX2 };
  float_t vy[3] = { triY0, triY1, triY2 };

  float_t closestX = 0;
  float_t closestY = 0;
  float_t minDistSq = FLT_MAX;

  for(uint8_t i = 0; i < 3; ++i) {
    uint8_t j = (i + 1) % 3;

    float_t testX, testY;
    physicsClosestPointOnSegment(
      vx[i], vy[i], vx[j], vy[j],
      circleX, circleY, &testX, &testY
    );

    float_t dx = circleX - testX;
    float_t dy = circleY - testY;
    float_t distSq = (dx * dx) + (dy * dy);

    if(distSq < minDistSq) {
      minDistSq = distSq;
      closestX = testX;
      closestY = testY;
      result.normalX = dx;
      result.normalY = dy;
    }
  }

  float_t dist = sqrtf(minDistSq);
  float_t invDist = (
    (dist != 0) ?
    (1.0f / dist) :
    1.0f
  );

  result.normalX = -result.normalX * invDist;
  result.normalY = -result.normalY * invDist;

  if(physicsIsPointInTriangle(
    circleX, circleY, vx[0], vy[0], vx[1], vy[1], vx[2], vy[2]
  )) {
    result.hit = true;
    result.depth = circleR - dist;
    return result;
  }

  if(dist < circleR) {
    result.hit = true;
    result.depth = circleR - dist;
  }

  return result;
}

collisionresult_t physicsCheckCircleTile(
  float_t circleX, float_t circleY, float_t circleR,
  float_t tileX, float_t tileY, tile_t tile
) {
  collisionresult_t result;

  #define tw (TILE_WIDTH_HEIGHT)
  #define th (TILE_WIDTH_HEIGHT)
  #define lx (tileX * tw)
  #define ty (tileY * th)
  #define rx (lx + tw)
  #define by (ty + th)

  switch(TILE_META_DATA[tile].solidType) {
    case TILE_SOLID_FULL:
      result = physicsCheckCircleAABB(
        circleX, circleY, circleR,
        lx, ty,
        tw, th
      );
      break;

    case TILE_SOLID_TRIANGLE_TOP_RIGHT:
      result = physicsCheckCircleTriangle(
        circleX, circleY, circleR,
        rx, by,
        rx, ty,
        lx, ty
      );
      break;

    case TILE_SOLID_TRIANGLE_TOP_LEFT:
      result = physicsCheckCircleTriangle(
        circleX, circleY, circleR,
        lx, by,
        lx, ty,
        rx, ty
      );
      break;

    case TILE_SOLID_TRIANGLE_BOTTOM_RIGHT:
      result = physicsCheckCircleTriangle(
        circleX, circleY, circleR,
        rx, ty,
        rx, by,
        lx, by
      );
      break;
    case TILE_SOLID_TRIANGLE_BOTTOM_LEFT:
      result = physicsCheckCircleTriangle(
        circleX, circleY, circleR,
        lx, ty,
        lx, by,
        rx, by
      );
      break;

    default:
      result.hit = false;
      break;
  }

  return result;
}