spherecast

physics.c (7997B)

---

 #include <float.h>
 #include <raylib.h>
 
 #include "config.h"
 #include "physics.h"
 #include "player.h"
 
 #define SQR(x) ((x) * (x))
 
 /*
  * Find closest point on a line segment [a, b] to point p
  */
 Vector3 ClosestPointOnSegment(Vector3 p, Vector3 a, Vector3 b) {
   Vector3 ab = Vector3Subtract(b, a);
   float t = Vector3DotProduct(Vector3Subtract(p, a), ab);
   float lenSq = Vector3LengthSqr(ab);
 
   /* Validate length to avoid division by zero */
   if (lenSq < EPSILON)
     return a;
 
   t = t / lenSq;
 
   /* Clamp t to [0, 1] */
   t = fmaxf(0.0f, fminf(1.0f, t));
 
   return Vector3Add(a, Vector3Scale(ab, t));
 }
 
 /*
  * Check for if capsule is within bounds of triangle t
  */
 static inline bool TriangleLikelyContains(Vector3 pos, float height, float r,
                                           const Triangle *t) {
   float r2 = r + SKIN;
 
   /*
    * AABB check: Triangle must overlap the AABB of the capsule
    * Capsule Y Range: [pos.y - r, pos.y + height + r]
    */
   float minX = fminf(fminf(t->a.x, t->b.x), t->c.x);
   float maxX = fmaxf(fmaxf(t->a.x, t->b.x), t->c.x);
   if (pos.x + r2 < minX || pos.x - r2 > maxX)
     return false;
 
   float minZ = fminf(fminf(t->a.z, t->b.z), t->c.z);
   float maxZ = fmaxf(fmaxf(t->a.z, t->b.z), t->c.z);
   if (pos.z + r2 < minZ || pos.z - r2 > maxZ)
     return false;
 
   float minY = fminf(fminf(t->a.y, t->b.y), t->c.y);
   float maxY = fmaxf(fmaxf(t->a.y, t->b.y), t->c.y);
 
   /* Check capsule bottom vs tri top and capsule top vs tri bottom */
   if ((pos.y + height + r2) < minY)
     return false;
   if ((pos.y - r2) > maxY)
     return false;
 
   return true;
 }
 
 Vector3 ClosestPointOnTriangle(Vector3 p, Vector3 a, Vector3 b, Vector3 c) {
   Vector3 ab = Vector3Subtract(b, a);
   Vector3 ac = Vector3Subtract(c, a);
   Vector3 ap = Vector3Subtract(p, a);
   float d1 = Vector3DotProduct(ab, ap);
   float d2 = Vector3DotProduct(ac, ap);
   if (d1 <= 0.0f && d2 <= 0.0f)
     return a;
 
   Vector3 bp = Vector3Subtract(p, b);
   float d3 = Vector3DotProduct(ab, bp);
   float d4 = Vector3DotProduct(ac, bp);
   if (d3 >= 0.0f && d4 <= d3)
     return b;
 
   float vc = d1 * d4 - d3 * d2;
   if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f)
     return Vector3Add(a, Vector3Scale(ab, d1 / (d1 - d3)));
 
   Vector3 cp = Vector3Subtract(p, c);
   float d5 = Vector3DotProduct(ab, cp);
   float d6 = Vector3DotProduct(ac, cp);
   if (d6 >= 0.0f && d5 <= d6)
     return c;
 
   float vb = d5 * d2 - d1 * d6;
   if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f)
     return Vector3Add(a, Vector3Scale(ac, d2 / (d2 - d6)));
 
   float va = d3 * d6 - d5 * d4;
   if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f)
     return Vector3Add(b, Vector3Scale(Vector3Subtract(c, b),
                                       (d4 - d3) / ((d4 - d3) + (d5 - d6))));
 
   float denom = 1.0f / (va + vb + vc);
   return Vector3Add(a, Vector3Add(Vector3Scale(ab, vb * denom),
                                   Vector3Scale(ac, vc * denom)));
 }
 
 bool SphereCastTriangle(Vector3 pos, Vector3 dir, float radius, float maxDist,
                         const Triangle *tri, HitInfo *hit) {
   Vector3 n = tri->normal;
   float denom = Vector3DotProduct(n, dir);
   if (denom >= -EPSILON)
     return false; /* Backface culling */
 
   float distToPlane = Vector3DotProduct(n, Vector3Subtract(tri->a, pos));
   float tPlane = (distToPlane + radius) / denom;
 
   if (tPlane > maxDist)
     return false;
 
   Vector3 planeIntersectionPoint = Vector3Add(pos, Vector3Scale(dir, tPlane));
   Vector3 centerOnPlane =
       Vector3Subtract(planeIntersectionPoint, Vector3Scale(n, radius));
   Vector3 closestPt =
       ClosestPointOnTriangle(centerOnPlane, tri->a, tri->b, tri->c);
   float distSq = Vector3DistanceSqr(centerOnPlane, closestPt);
 
   /* CASE A: Hit the FACE */
   if (distSq < EPSILON) {
     if (tPlane < 0)
       return false;
     hit->hit = true;
     hit->distance = tPlane;
     hit->normal = n;
     return true;
   }
 
   /* CASE B: Hit Edge/Vertex */
   Vector3 L = Vector3Subtract(pos, closestPt);
   float a = 1.0f;
   float b = 2.0f * Vector3DotProduct(L, dir);
   float c = Vector3DotProduct(L, L) - SQR(radius);
 
   float delta = b * b - 4 * a * c;
 
   if (delta >= 0.0f) {
     float tEdge = (-b - sqrtf(delta)) / 2.0f;
     if (tEdge >= 0 && tEdge < maxDist) {
       hit->hit = true;
       hit->distance = tEdge;
       Vector3 hitPos = Vector3Add(pos, Vector3Scale(dir, tEdge));
       hit->normal = Vector3Normalize(Vector3Subtract(hitPos, closestPt));
       return true;
     }
   }
   return false;
 }
 
 bool SphereCast(Vector3 pos, Vector3 dir, float dist, float radius,
                 Triangle *tris, int triCount, HitInfo *outHit) {
   HitInfo best = {0};
   best.distance = dist;
   bool found = false;
 
   for (int i = 0; i < triCount; i++) {
     HitInfo h;
     if (SphereCastTriangle(pos, dir, radius, best.distance, &tris[i], &h)) {
       best = h;
       found = true;
     }
   }
 
   if (!found)
     return false;
   *outHit = best;
   return true;
 }
 
 /*
  * Resolves capsule overlaps. Pos is the center of the BOTTOM sphere.
  */
 void ResolveCollisions(Player *p, Triangle *tris, int count) {
   float rSq = SQR(p->radius);
   Vector3 capsTopOffset = {0, p->height, 0};
 
   HitInfo hit;
   if (p->mode != MOVE_SLIDE && p->vel.y <= 0 &&
       SphereCast(p->pos, DOWN, GROUND_SNAP_DIST, p->radius, tris, count,
                  &hit)) {
     p->pos = Vector3Add(p->pos, Vector3Scale(DOWN, hit.distance));
     p->grounded = true;
   }
 
   /* Four iterations should be max: ceil, floor, wall1, wall2 */
   for (int iter = 0; iter < 4; iter++) {
     bool collisionFound = false;
 
     Vector3 pBottom = p->pos;
     Vector3 pTop = Vector3Add(pBottom, capsTopOffset);
     Vector3 midPoint = Vector3Scale(Vector3Add(pBottom, pTop), 0.5f);
 
     for (int i = 0; i < count; i++) {
       Triangle *t = &tris[i];
 
       Vector3 closestOnTri = ClosestPointOnTriangle(midPoint, t->a, t->b, t->c);
       Vector3 closestOnSegment =
           ClosestPointOnSegment(closestOnTri, pBottom, pTop);
 
       // Re-refine
       closestOnTri = ClosestPointOnTriangle(closestOnSegment, t->a, t->b, t->c);
 
       Vector3 pushVec = Vector3Subtract(closestOnSegment, closestOnTri);
       float d2 = Vector3LengthSqr(pushVec);
 
       /* Early exit if no collision or invalid math */
       if (d2 <= EPSILON || d2 >= rSq)
         continue;
 
       float d = sqrtf(d2);
       Vector3 n = Vector3Scale(pushVec, 1.0f / d);
 
       /* Add small skin width to prevent floating point sinking */
       float penetration = p->radius - d + SKIN;
 
       /* Check if the surface is "Walkable" (Flat floor or manageable slope) */
       if (n.y > GROUND_NORMAL_Y) {
         /*
          * If we are on walkable ground, we want to push the player STRICTLY UP.
          * hypotenuse = adjacent / cos(theta)
          * vertical_move = penetration / n.y
          */
 
         p->grounded = true;
         float verticalPush = penetration / n.y;
         p->pos.y += verticalPush;
 
       } else {
         /*
          * If it is a steep slope, wall, or ceiling, push along the normal
          * to prevent the player from walking up walls.
          */
         p->pos = Vector3Add(p->pos, Vector3Scale(n, penetration));
       }
 
       pBottom = p->pos;
       pTop = Vector3Add(pBottom, capsTopOffset);
       midPoint = Vector3Scale(Vector3Add(pBottom, pTop), 0.5f);
 
       float vn = Vector3DotProduct(p->vel, n);
 
       /* Only act if checking velocity moving INTO the wall/floor */
       if (vn < 0.0f) {
 
         if (n.y > GROUND_NORMAL_Y && p->mode != MOVE_SLIDE) {
           p->groundedTimer = COYOTE_TIME;
 
           /*
            * To prevent sliding on slopes, we zero out the Y velocity.
            * This stops gravity from accumulating and sliding you down.
            */
           p->vel.y = 0.0f;
           p->grounded = true;
         } else {
           /* Remove the velocity moving into the wall (Slide along wall) */
           p->vel = Vector3Subtract(p->vel, Vector3Scale(n, vn));
         }
       }
 
       collisionFound = true;
     }
 
     if (!collisionFound)
       break;
   }
 }