jakubtomsu · September 21, 2024 16:10 · Memresable · Aug 26, 2024 · Memresable · Aug 26, 2024
diff --git a/realtime_collision_detection.odin b/realtime_collision_detection.odin
 // Port of some collision functions to Odin by Jakub Tomšů.
 //
 // from Real-Time Collision Detection by Christer Ericson, published by Morgan Kaufmann Publishers, © 2005 Elsevier Inc
 //
 // This should serve as an reference implementation for common collision queries for games.
 // The goal is good numerical robustness, handling edge cases and optimized math equations.
 // The code isn't necessarily very optimized.
 //
 // There are a few cases you don't want to use the procedures below directly, but instead manually inline the math and adapt it to your needs.
 // In my experience this method is clearer when writing complex level queries where I need to handle edge cases differently etc.
 package realtime_collision_detection

 import "core:math"
 import "core:math/linalg"

 Vec3 :: [3]f32

 sqrt :: math.sqrt

 dot :: linalg.dot
 cross :: linalg.cross
 length2 :: linalg.vector_length2

 Aabb :: struct {
    min: Vec3,
    max: Vec3,
 }

 // Infinitely small
 AABB_INVALID :: Aabb {
    min = 1e20,
    max = -1e20,
 }

 Sphere :: struct {
    pos: Vec3,
    rad: f32,
 }

 // Radius is the half size
 Box :: struct {
    pos: Vec3,
    rad: Vec3,
 }

 Plane :: struct {
    normal: Vec3,
    dist:   f32,
 }

 Capsule :: struct {
    a:   Vec3,
    b:   Vec3,
    rad: f32,
 }

 // Same layout, slightly different meaning
 Cylinder :: distinct Capsule


 aabb_center :: proc(a: Aabb) -> Vec3 {
    return (a.min + a.max) * 0.5
 }

 aabb_half_size :: proc(a: Aabb) -> Vec3 {
    return (a.max - a.min) * 0.5
 }

 aabb_to_box :: proc(a: Aabb) -> Box {
    center := aabb_center(a)
    return {pos = center, rad = a.max - center}
 }

 box_to_aabb :: proc(a: Box) -> Aabb {
    return {min = a.pos - a.rad, max = a.pos + a.rad}
 }

 plane_from_point_normal :: proc(point: Vec3, normal: Vec3) -> Plane {
    return {normal = normal, dist = dot(point, normal)}
 }



 //////////////////////////////////////////////////////////////////////////////////
 // Distance to closest point
 //

 signed_distance_plane :: proc(point: Vec3, plane: Plane) -> f32 {
    // If plane equation normalized (||p.n||==1)
    return dot(point, plane.normal) - plane.dist
    // If not normalized
    // return (dot(plane.normal, point) - plane.dist) / Ddt(plane.normal, plane.normal);
 }

 squared_distance_aabb :: proc(point: Vec3, aabb: Aabb) -> (dist: f32) {
    for i in 0 ..< 3 {
        // For each axis count any excess distance outside box extents
        if point[i] < aabb.min[i] do dist += (aabb.min[i] - point[i]) * (aabb.min[i] - point[i])
        if point[i] > aabb.max[i] do dist += (point[i] - aabb.max[i]) * (point[i] - aabb.max[i])
    }
    return dist
 }

 // Returns the squared distance between point and segment ab
 squared_distance_segment :: proc(point, a, b: Vec3) -> f32 {
    ab := b - a
    ac := point - a
    bc := point - b
    e := dot(ac, ab)
    // Handle cases where c projects outside ab
    if e <= 0.0 {
        return dot(ac, ac)
    }
    f := dot(ab, ab)
    if e >= f {
        return dot(bc, bc)
    }
    // Handle cases where c projects onto ab
    return dot(ac, ac) - e * e / f
 }



 //////////////////////////////////////////////////////////////////////////////////
 // Closest point
 //

 closest_point_plane :: proc(point: Vec3, plane: Plane) -> Vec3 {
    t := dot(plane.normal, point) - plane.dist
    return point - t * plane.normal
 }

 closest_point_aabb :: proc(point: Vec3, aabb: Aabb) -> Vec3 {
    return {
        clamp(point.x, aabb.min.x, aabb.max.x),
        clamp(point.y, aabb.min.y, aabb.max.y),
        clamp(point.z, aabb.min.z, aabb.max.z),
    }
 }

 // Given segment ab and point c, computes closest point d on ab.
 // Also returns t for the position of d, d(t)=a+ t*(b - a)
 closest_point_segment :: proc(pos, a, b: Vec3) -> (t: f32, point: Vec3) {
    ab := b - a
    // Project pos onto ab, computing parameterized position d(t)=a+ t*(b – a)
    t = dot(pos - a, ab) / dot(ab, ab)
    t = clamp(t, 0, 1)
    // Compute projected position from the clamped t
    point = a + t * ab
    return t, point
 }

 // Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and
 // S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared
 // distance between between S1(s) and S2(t)
 // TODO: [2]Vec3
 closest_point_between_segments :: proc(p1, q1, p2, q2: Vec3) -> (t: [2]f32, points: [2]Vec3) {
    d1 := q1 - p1 // Direction vector of segment S1
    d2 := q2 - p2 // Direction vector of segment S2
    r := p1 - p2
    a := dot(d1, d1) // Squared length of segment S1, always nonnegative
    e := dot(d2, d2) // Squared length of segment S2, always nonnegative
    f := dot(d2, r)

    EPS :: 1e-6

    // Check if either or both segments degenerate into points
    if a <= EPS && e <= EPS {
        // Both segments degenerate into points
        t = 0
        points = {p1, p2}
        return t, points
    }
    if a <= EPS {
        // First segment degenerates into a point
        t[0] = 0
        t[1] = clamp(f / e, 0, 1) // s = 0 => t = (b*s + f) / e = f / e
    } else {
        c := dot(d1, r)
        if e <= EPS {
            // Second segment degenerates into a point
            t[1] = 0
            t[0] = clamp(-c / a, 0, 1) // t = 0 => s = (b*t - c) / a = -c / a
        } else {
            // The general nondegenerate case starts here
            b := dot(d1, d2)
            denom := a * e - b * b // Always nonnegative

            // If segments not parallel, compute closest point on L1 to L2 and
            // clamp to segment S1. Else pick arbitrary s (here 0)
            if denom != 0.0 {
                t[0] = clamp((b * f - c * e) / denom, 0, 1)
            } else {
                t[0] = 0
            }
            // Compute point on L2 closest to S1(s) using
            // t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e
            tnom := (b * t[0] + f)

            // If t in [0,1] done. Else clamp t, recompute s for the new value
            // of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a
            // and clamp s to [0, 1]
            if tnom < 0 {
                t[1] = 0
                t[0] = clamp(-c / a, 0, 1)
            } else if tnom > 1 {
                t[1] = 1
                t[0] = clamp((b - c) / a, 0, 1)
            } else {
                t[1] = tnom / e
            }
        }
    }

    points[0] = p1 + d1 * t[0]
    points[1] = p2 + d2 * t[1]
    return t, points
 }

 closest_point_triangle :: proc(point, a, b, c: Vec3) -> Vec3 {
    ab := b - a
    ac := c - a
    ap := point - a
    d1 := dot(ab, ap)
    d2 := dot(ac, ap)
    if d1 <= 0 && d2 <= 0 do return a // barycentric coordinates (1,0,0)

    // Check if P in vertex region outside B
    bp := point - b
    d3 := dot(ab, bp)
    d4 := dot(ac, bp)
    if d3 >= 0 && d4 <= d3 do return b // barycentric coordinates (0,1,0)

    // Check if P in edge region of AB, if so return projection of P onto AB
    vc := d1 * d4 - d3 * d2
    if vc < 0 && d1 >= 0 && d3 <= 0 {
        v := d1 / (d1 - d3)
        return a + v * ab // barycentric coordinates (1-v,v,0)
    }

    // Check if P in vertex region outside C
    cp := point - c
    d5 := dot(ab, cp)
    d6 := dot(ac, cp)
    if d6 >= 0 && d5 <= d6 do return c // barycentric coordinates (0,0,1)

    // Check if P in edge region of AC, if so return projection of P onto AC
    vb := d5 * d2 - d1 * d6
    if vb <= 0 && d2 >= 0 && d6 <= 0 {
        w := d2 / (d2 - d6)
        return a + w * ac // barycentric coordinates (1-w,0,w)
    }

    // Check if P in edge region of BC, if so return projection of P onto BC
    va := d3 * d6 - d5 * d4
    if va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0 {
        w := (d4 - d3) / ((d4 - d3) + (d5 - d6))
        return b + w * (c - b) // barycentric coordinates (0,1-w,w)
    }

    // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
    denom := 1.0 / (va + vb + vc)
    v := vb * denom
    w := vc * denom
    return a + ab * v + ac * w // = u*a + v*b + w*c, u = va * denom = 1.0f-v-w
 }



 //////////////////////////////////////////////////////////////////////////////////
 // Tests
 //

 test_aabb_vs_aabb :: proc(a, b: Aabb) -> bool {
    // Exit with no intersection if separated along an axis
    if a.max[0] < b.min[0] || a.min[0] > b.max[0] do return false
    if a.max[1] < b.min[1] || a.min[1] > b.max[1] do return false
    if a.max[2] < b.min[2] || a.min[2] > b.max[2] do return false
    // Overlapping on all axes means AABBs are intersecting
    return true
 }

 test_sphere_vs_aabb :: proc(sphere: Sphere, aabb: Aabb) -> bool {
    s := squared_distance_aabb(sphere.pos, aabb)
    return s <= sphere.rad * sphere.rad
 }

 test_sphere_vs_plane :: proc(sphere: Sphere, plane: Plane) -> bool {
    dist := signed_distance_plane(sphere.pos, plane)
    return abs(dist) <= sphere.rad
 }

 test_point_vs_halfspace :: proc(pos: Vec3, plane: Plane) -> bool {
    return signed_distance_plane(pos, plane) <= 0.0
 }

 test_sphere_vs_halfspace :: proc(sphere: Sphere, plane: Plane) -> bool {
    dist := signed_distance_plane(sphere.pos, plane)
    return dist <= sphere.rad
 }

 test_box_vs_plane :: proc(box: Box, plane: Plane) -> bool {
    // Compute the projection interval radius of b onto L(t) = b.c + t * p.n
    r := box.rad.x * abs(plane.normal.x) + box.rad.y * abs(plane.normal.y) + box.rad.z * abs(plane.normal.z)
    s := signed_distance_plane(box.pos, plane)
    return abs(s) <= r
 }

 test_capsule_vs_capsule :: proc(a, b: Capsule) -> bool {
    // Compute (squared) distance between the inner structures of the capsules
    _, points := closest_point_between_segments(a.a, a.b, b.a, b.b)
    squared_dist := length2(points[1] - points[0])
    // If (squared) distance smaller than (squared) sum of radii, they collide
    rad := a.rad + b.rad
    return squared_dist <= rad * rad
 }

 test_sphere_vs_capsule :: proc(sphere: Sphere, capsule: Capsule) -> bool {
    // Compute (squared) distance between sphere center and capsule line segment
    dist2 := squared_distance_segment(point = sphere.pos, a = capsule.a, b = capsule.b)
    // If (squared) distance smaller than (squared) sum of radii, they collide
    rad := sphere.rad + capsule.rad
    return dist2 <= rad * rad
 }

 test_capsule_vs_plane :: proc(capsule: Capsule, plane: Plane) -> bool {
    adist := dot(capsule.a, plane.normal) - plane.dist
    bdist := dot(capsule.b, plane.normal) - plane.dist
    // Intersects if on different sides of plane (distances have different signs)
    if adist * bdist < 0.0 do return true
    // Intersects if start or end position within radius from plane
    if abs(adist) <= capsule.rad || abs(bdist) <= capsule.rad do return true
    return false
 }

 test_capsule_vs_halfspace :: proc(capsule: Capsule, plane: Plane) -> bool {
    adist := dot(capsule.a, plane.normal) - plane.dist
    bdist := dot(capsule.b, plane.normal) - plane.dist
    return min(adist, bdist) <= capsule.rad
 }

 test_ray_sphere :: proc(pos, dir: Vec3, sphere: Sphere) -> bool {
    m := pos - sphere.pos
    c := dot(m, m) - sphere.rad * sphere.rad
    // If there is definitely at least one real root, there must be an intersection
    if c <= 0 do return true
    b := dot(m, dir)
    // Early exit if ray origin outside sphere and ray pointing away from sphere
    if b > 0 do return false
    discr := b * b - c
    // A negative discriminant corresponds to ray missing sphere
    return discr >= 0
 }

 test_point_polyhedron :: proc(pos: Vec3, planes: []Plane) -> bool {
    for plane in planes {
        if signed_distance_plane(pos, plane) > 0.0 {
            return false
        }
    }
    return true
 }



 //////////////////////////////////////////////////////////////////////////////////
 // Intersections
 //

 // Given planes a and b, compute line L = p+t*d of their intersection.
 intersect_planes :: proc(a, b: Plane) -> (point, dir: Vec3, ok: bool) {
    // Compute direction of intersection line
    dir = cross(a.normal, b.normal)
    // If d is (near) zero, the planes are parallel (and separated)
    // or coincident, so they’re not considered intersecting
    denom := dot(dir, dir)
    EPS :: 1e-6
    if denom < EPS do return {}, dir, false
    // Compute point on intersection line
    point = cross(a.dist * b.normal - b.dist * a.normal, dir) / denom
    return point, dir, true
 }

 // TODO: moving vs static
 intersect_moving_spheres :: proc(a, b: Sphere, vel_a, vel_b: Vec3) -> (t: f32, ok: bool) {
    s := b.pos - a.pos
    v := vel_b - vel_a // Relative motion of s1 with respect to stationary s0
    r := a.rad + b.rad
    c := dot(s, s) - r * r
    if c < 0 {
        // Spheres initially overlapping so exit directly
        return 0, true
    }
    a := dot(v, v)
    EPS :: 1e-6
    if a < EPS {
        return 1, false // Spheres not moving relative each other
    }
    b := dot(v, s)
    if b >= 0 {
        return 1, false // Spheres not moving towards each other
    }
    d := b * b - a * c
    if d < 0 {
        return 1, false // No real-valued root, spheres do not intersect
    }
    t = (-b - sqrt(d)) / a
    return t, true
 }

 intersect_moving_aabbs :: proc(a, b: Aabb, vel_a, vel_b: Vec3) -> (t: [2]f32, ok: bool) {
    // Use relative velocity; effectively treating ’a’ as stationary
    return intersect_static_aabb_vs_moving_aabb(a, b, vel_relative = vel_b - vel_a)
 }

 // 'a' is static, 'b' is moving
 intersect_static_aabb_vs_moving_aabb :: proc(a, b: Aabb, vel_relative: Vec3) -> (t: [2]f32, ok: bool) {
    // Exit early if ‘a’ and ‘b’ initially overlapping
    if test_aabb_vs_aabb(a, b) {
        return 0, true
    }

    // Initialize ts of first and last contact
    t = {0, 1}

    // For each axis, determine ts of first and last contact, if any
    for i in 0 ..< 3 {
        if vel_relative[i] < 0.0 {
            if b.max[i] < a.min[i] do return 1, false // Nonintersecting and moving apart
            if a.max[i] < b.min[i] do t[0] = max(t[0], (a.max[i] - b.min[i]) / vel_relative[i])
            if b.max[i] > a.min[i] do t[1] = min(t[1], (a.min[i] - b.max[i]) / vel_relative[i])
        }

        if vel_relative[i] > 0.0 {
            if b.min[i] > a.max[i] do return 1, false // Nonintersecting and moving apart
            if b.max[i] < a.min[i] do t[0] = max(t[0], (a.min[i] - b.max[i]) / vel_relative[i])
            if a.max[i] > b.min[i] do t[1] = min(t[1], (a.max[i] - b.min[i]) / vel_relative[i])
        }

        // No overlap possible if t of first contact occurs after t of last contact
        if t[0] > t[1] do return 1, false
    }

    return t, true
 }

 // Intersect sphere s with movement vector v with plane p. If intersecting
 // return t t of collision and point at which sphere hits plane
 intersect_moving_sphere_vs_plane :: proc(sphere: Sphere, vel: Vec3, plane: Plane) -> (t: f32, point: Vec3, ok: bool) {
    // Compute distance of sphere center to plane
    dist := dot(plane.normal, sphere.pos) - plane.dist
    if abs(dist) <= sphere.rad {
        // The sphere is already overlapping the plane. Set t of
        // intersection to zero and q to sphere center
        return 0.0, sphere.pos, true
    }

    denom := dot(plane.normal, vel)
    if (denom * dist >= 0.0) {
        // No intersection as sphere moving parallel to or away from plane
        return 1.0, sphere.pos, false
    }

    // Sphere is moving towards the plane

    // Use +r in computations if sphere in front of plane, else -r
    r := dist > 0.0 ? sphere.rad : -sphere.rad
    t = (r - dist) / denom
    point = sphere.pos + vel * t - r * plane.normal
    return t, point, t <= 1.0
 }

 intersect_ray_sphere :: proc(pos: Vec3, dir: Vec3, sphere: Sphere) -> (t: f32, ok: bool) {
    m := pos - sphere.pos
    b := dot(m, dir)
    c := dot(m, m) - sphere.rad * sphere.rad
    // Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0)
    if c > 0 && b > 0 {
        return 0, false
    }
    discr := b * b - c
    // A negative discriminant corresponds to ray missing sphere
    if discr < 0 do return 0, false
    // Ray now found to intersect sphere, compute smallest t value of intersection
    t = -b - sqrt(discr)
    // If t is negative, ray started inside sphere so clamp t to zero
    t = max(0, t)
    return t, true
 }

 intersect_ray_aabb :: proc(pos: Vec3, dir: Vec3, aabb: Aabb, range: f32 = max(f32)) -> (t: [2]f32, ok: bool) {
    // https://tavianator.com/cgit/dimension.git/tree/libdimension/bvh/bvh.c#n196

    // This is actually correct, even though it appears not to handle edge cases
    // (dir.{x,y,z} == 0).  It works because the infinities that result from
    // dividing by zero will still behave correctly in the comparisons.  Rays
    // which are parallel to an axis and outside the box will have tmin == inf
    // or tmax == -inf, while rays inside the box will have tmin and tmax
    // unchanged.

    inv_dir := 1.0 / dir

    t1 := (aabb.min - pos) * inv_dir
    t2 := (aabb.max - pos) * inv_dir

    t = {max(min(t1.x, t2.x), min(t1.y, t2.y), min(t1.z, t2.z)), min(max(t1.x, t2.x), max(t1.y, t2.y), max(t1.z, t2.z))}

    return t, t[1] >= max(0.0, t[0]) && t[0] < range
 }

 intersect_ray_polyhedron :: proc(pos, dir: Vec3, planes: []Plane, segment: [2]f32 = {0.0, max(f32)}) -> (t: [2]f32, ok: bool) {
    t = segment
    for plane in planes {
        denom := dot(plane.normal, dir)
        dist := plane.dist - dot(plane.normal, pos)
        // Test if segment runs parallel to the plane
        if denom == 0.0 {
            // If so, return “no intersection” if segment lies outside plane
            if dist > 0.0 {
                return 0, false
            }
        } else {
            // Compute parameterized t value for intersection with current plane
            tplane := dist / denom
            if denom < 0.0 {
                // When entering halfspace, update tfirst if t is larger
                t[0] = max(t[0], tplane)
            } else {
                // When exiting halfspace, update tlast if t is smaller
                t[1] = min(t[1], tplane)
            }
            if t[0] > t[1] {
                return 0, false
            }
        }
    }
    return t, true
 }

 intersect_segment_triangle :: proc(
    segment: [2]Vec3,
    triangle: [3]Vec3,
 ) -> (
    t: f32,
    normal: Vec3,
    barycentric: [3]f32,
    ok: bool,
 ) {
    ab := triangle[1] - triangle[0]
    ac := triangle[2] - triangle[0]
    qp := segment[0] - segment[1]

    normal = cross(ab, ac)

    denom := dot(qp, normal)
    // If denom <= 0, segment is parallel to or points away from triangle
    if denom <= 0 {
        return 0, normal, 0, false
    }

    // Compute intersection t value of pq with plane of triangle. A ray
    // intersects if 0 <= t. Segment intersects iff 0 <= t <= 1. Delay
    // dividing by d until intersection has been found to pierce triangle
    ap := segment[0] - triangle[0]
    t = dot(ap, normal)
    if t < 0 {
        return
    }
    if t > denom {
        // For segment; exclude this code line for a ray test
        return
    }

    // Compute barycentric coordinate components and test if within bounds
    e := cross(qp, ap)
    barycentric.y = dot(ac, e)
    if barycentric.y < 0 || barycentric.y > denom {
        return
    }
    barycentric.z = -dot(ab, e)
    if barycentric.z < 0 || barycentric.y + barycentric.z > denom {
        return
    }

    // Segment/ray intersects triangle. Perform delayed division and
    // compute the last barycentric coordinate component
    ood := 1.0 / denom
    t *= ood
    barycentric.yz *= ood
    barycentric.x = 1.0 - barycentric.y - barycentric.z
    return t, normal, barycentric, true
 }

 intersect_segment_plane :: proc(segment: [2]Vec3, plane: Plane) -> (t: f32, point: Vec3, ok: bool) {
    ab := segment[1] - segment[0]
    t = (plane.dist - dot(plane.normal, segment[0])) / dot(plane.normal, ab)

    if t >= 0 && t <= 1 {
        point = segment[0] + t * ab
        return t, point, true
    }

    return t, segment[0], false
 }

 // TODO: alternative with capsule endcaps
 intersect_segment_cylinder :: proc(segment: [2]Vec3, cylinder: Cylinder) -> (t: f32, ok: bool) {
    d := cylinder.b - cylinder.a
    m := segment[0] - cylinder.a
    n := segment[1] - segment[0]
    md := dot(m, d)
    nd := dot(n, d)
    dd := dot(d, d)
    // Test if segment fully outside either endcap of cylinder
    if md < 0 && md + nd < 0 {
        return 0, false // Segment outside ’a’ side of cylinder
    }
    if md > dd && md + nd > dd {
        return 0, false // Segment outside ’b’ side of cylinder
    }
    nn := dot(n, n)
    mn := dot(m, n)
    a := dd * nn - nd * nd
    k := dot(m, m) - cylinder.rad * cylinder.rad
    c := dd * k - md * md
    EPS :: 1e-6
    if abs(a) < EPS {
        // Segment runs parallel to cylinder axis
        if c > 0 {
            return 0, false
        }
        // Now known that segment intersects cylinder; figure out how it intersects
        if md < 0 {
            // Intersect segment against ’a’ endcap
            t = -mn / nn
        } else if md > dd {
            // Intersect segment against ’b’ endcap
            t = (nd - mn) / nn
        } else {
            // ’a’ lies inside cylinder
            t = 0
        }
        return t, true
    }
    b := dd * mn - nd * md
    discr := b * b - a * c
    if discr < 0 {
        return 0, false // no real roots
    }
    t = (-b - sqrt(discr)) / a
    if t < 0 || t > 1 {
        return 0, false // intersection outside segment
    }
    if md + t * nd < 0 {
        // Intersection outside cylinder on ’a’ side
        if nd <= 0 {
            // Segment pointing away from endcap
            return 0, false
        }
        t = -md / nd
        ok = k + 2 * t * (mn + t * nn) <= 0
        return t, ok
    } else if md + t * nd > dd {
        // Intersection outside cylinder on ’b’ side
        if nd >= 0 {
            // Segment pointing away from endcap
            return 0, false
        }
        t = (dd - md) / nd
        ok = k + dd - 2 * md + t * (2 * (mn - nd) + t * nn) <= 0
        return t, ok
    }
    return t, true
 }
	// Port of some collision functions to Odin by Jakub Tomšů.
	//
	// from Real-Time Collision Detection by Christer Ericson, published by Morgan Kaufmann Publishers, © 2005 Elsevier Inc
	//
	// This should serve as an reference implementation for common collision queries for games.
	// The goal is good numerical robustness, handling edge cases and optimized math equations.
	// The code isn't necessarily very optimized.
	//
	// There are a few cases you don't want to use the procedures below directly, but instead manually inline the math and adapt it to your needs.
	// In my experience this method is clearer when writing complex level queries where I need to handle edge cases differently etc.
	package realtime_collision_detection

	import "core:math"
	import "core:math/linalg"

	Vec3 :: [3]f32

	sqrt :: math.sqrt

	dot :: linalg.dot
	cross :: linalg.cross
	length2 :: linalg.vector_length2

	Aabb :: struct {
	min: Vec3,
	max: Vec3,
	}

	// Infinitely small
	AABB_INVALID :: Aabb {
	min = 1e20,
	max = -1e20,
	}

	Sphere :: struct {
	pos: Vec3,
	rad: f32,
	}

	// Radius is the half size
	Box :: struct {
	pos: Vec3,
	rad: Vec3,
	}

	Plane :: struct {
	normal: Vec3,
	dist: f32,
	}

	Capsule :: struct {
	a: Vec3,
	b: Vec3,
	rad: f32,
	}

	// Same layout, slightly different meaning
	Cylinder :: distinct Capsule


	aabb_center :: proc(a: Aabb) -> Vec3 {
	return (a.min + a.max) * 0.5
	}

	aabb_half_size :: proc(a: Aabb) -> Vec3 {
	return (a.max - a.min) * 0.5
	}

	aabb_to_box :: proc(a: Aabb) -> Box {
	center := aabb_center(a)
	return {pos = center, rad = a.max - center}
	}

	box_to_aabb :: proc(a: Box) -> Aabb {
	return {min = a.pos - a.rad, max = a.pos + a.rad}
	}

	plane_from_point_normal :: proc(point: Vec3, normal: Vec3) -> Plane {
	return {normal = normal, dist = dot(point, normal)}
	}



	//////////////////////////////////////////////////////////////////////////////////
	// Distance to closest point
	//

	signed_distance_plane :: proc(point: Vec3, plane: Plane) -> f32 {
	// If plane equation normalized (\|\|p.n\|\|==1)
	return dot(point, plane.normal) - plane.dist
	// If not normalized
	// return (dot(plane.normal, point) - plane.dist) / Ddt(plane.normal, plane.normal);
	}

	squared_distance_aabb :: proc(point: Vec3, aabb: Aabb) -> (dist: f32) {
	for i in 0 ..< 3 {
	// For each axis count any excess distance outside box extents
	if point[i] < aabb.min[i] do dist += (aabb.min[i] - point[i]) * (aabb.min[i] - point[i])
	if point[i] > aabb.max[i] do dist += (point[i] - aabb.max[i]) * (point[i] - aabb.max[i])
	}
	return dist
	}

	// Returns the squared distance between point and segment ab
	squared_distance_segment :: proc(point, a, b: Vec3) -> f32 {
	ab := b - a
	ac := point - a
	bc := point - b
	e := dot(ac, ab)
	// Handle cases where c projects outside ab
	if e <= 0.0 {
	return dot(ac, ac)
	}
	f := dot(ab, ab)
	if e >= f {
	return dot(bc, bc)
	}
	// Handle cases where c projects onto ab
	return dot(ac, ac) - e * e / f
	}



	//////////////////////////////////////////////////////////////////////////////////
	// Closest point
	//

	closest_point_plane :: proc(point: Vec3, plane: Plane) -> Vec3 {
	t := dot(plane.normal, point) - plane.dist
	return point - t * plane.normal
	}

	closest_point_aabb :: proc(point: Vec3, aabb: Aabb) -> Vec3 {
	return {
	clamp(point.x, aabb.min.x, aabb.max.x),
	clamp(point.y, aabb.min.y, aabb.max.y),
	clamp(point.z, aabb.min.z, aabb.max.z),
	}
	}

	// Given segment ab and point c, computes closest point d on ab.
	// Also returns t for the position of d, d(t)=a+ t*(b - a)
	closest_point_segment :: proc(pos, a, b: Vec3) -> (t: f32, point: Vec3) {
	ab := b - a
	// Project pos onto ab, computing parameterized position d(t)=a+ t*(b – a)
	t = dot(pos - a, ab) / dot(ab, ab)
	t = clamp(t, 0, 1)
	// Compute projected position from the clamped t
	point = a + t * ab
	return t, point
	}

	// Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and
	// S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared
	// distance between between S1(s) and S2(t)
	// TODO: [2]Vec3
	closest_point_between_segments :: proc(p1, q1, p2, q2: Vec3) -> (t: [2]f32, points: [2]Vec3) {
	d1 := q1 - p1 // Direction vector of segment S1
	d2 := q2 - p2 // Direction vector of segment S2
	r := p1 - p2
	a := dot(d1, d1) // Squared length of segment S1, always nonnegative
	e := dot(d2, d2) // Squared length of segment S2, always nonnegative
	f := dot(d2, r)

	EPS :: 1e-6

	// Check if either or both segments degenerate into points
	if a <= EPS && e <= EPS {
	// Both segments degenerate into points
	t = 0
	points = {p1, p2}
	return t, points
	}
	if a <= EPS {
	// First segment degenerates into a point
	t[0] = 0
	t[1] = clamp(f / e, 0, 1) // s = 0 => t = (b*s + f) / e = f / e
	} else {
	c := dot(d1, r)
	if e <= EPS {
	// Second segment degenerates into a point
	t[1] = 0
	t[0] = clamp(-c / a, 0, 1) // t = 0 => s = (b*t - c) / a = -c / a
	} else {
	// The general nondegenerate case starts here
	b := dot(d1, d2)
	denom := a * e - b * b // Always nonnegative

	// If segments not parallel, compute closest point on L1 to L2 and
	// clamp to segment S1. Else pick arbitrary s (here 0)
	if denom != 0.0 {
	t[0] = clamp((b * f - c * e) / denom, 0, 1)
	} else {
	t[0] = 0
	}
	// Compute point on L2 closest to S1(s) using
	// t = Dot((P1 + D1s) - P2,D2) / Dot(D2,D2) = (bs + f) / e
	tnom := (b * t[0] + f)

	// If t in [0,1] done. Else clamp t, recompute s for the new value
	// of t using s = Dot((P2 + D2t) - P1,D1) / Dot(D1,D1)= (tb - c) / a
	// and clamp s to [0, 1]
	if tnom < 0 {
	t[1] = 0
	t[0] = clamp(-c / a, 0, 1)
	} else if tnom > 1 {
	t[1] = 1
	t[0] = clamp((b - c) / a, 0, 1)
	} else {
	t[1] = tnom / e
	}
	}
	}

	points[0] = p1 + d1 * t[0]
	points[1] = p2 + d2 * t[1]
	return t, points
	}

	closest_point_triangle :: proc(point, a, b, c: Vec3) -> Vec3 {
	ab := b - a
	ac := c - a
	ap := point - a
	d1 := dot(ab, ap)
	d2 := dot(ac, ap)
	if d1 <= 0 && d2 <= 0 do return a // barycentric coordinates (1,0,0)

	// Check if P in vertex region outside B
	bp := point - b
	d3 := dot(ab, bp)
	d4 := dot(ac, bp)
	if d3 >= 0 && d4 <= d3 do return b // barycentric coordinates (0,1,0)

	// Check if P in edge region of AB, if so return projection of P onto AB
	vc := d1 * d4 - d3 * d2
	if vc < 0 && d1 >= 0 && d3 <= 0 {
	v := d1 / (d1 - d3)
	return a + v * ab // barycentric coordinates (1-v,v,0)
	}

	// Check if P in vertex region outside C
	cp := point - c
	d5 := dot(ab, cp)
	d6 := dot(ac, cp)
	if d6 >= 0 && d5 <= d6 do return c // barycentric coordinates (0,0,1)

	// Check if P in edge region of AC, if so return projection of P onto AC
	vb := d5 * d2 - d1 * d6
	if vb <= 0 && d2 >= 0 && d6 <= 0 {
	w := d2 / (d2 - d6)
	return a + w * ac // barycentric coordinates (1-w,0,w)
	}

	// Check if P in edge region of BC, if so return projection of P onto BC
	va := d3 * d6 - d5 * d4
	if va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0 {
	w := (d4 - d3) / ((d4 - d3) + (d5 - d6))
	return b + w * (c - b) // barycentric coordinates (0,1-w,w)
	}

	// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
	denom := 1.0 / (va + vb + vc)
	v := vb * denom
	w := vc * denom
	return a + ab * v + ac * w // = ua + vb + wc, u = va denom = 1.0f-v-w
	}



	//////////////////////////////////////////////////////////////////////////////////
	// Tests
	//

	test_aabb_vs_aabb :: proc(a, b: Aabb) -> bool {
	// Exit with no intersection if separated along an axis
	if a.max[0] < b.min[0] \|\| a.min[0] > b.max[0] do return false
	if a.max[1] < b.min[1] \|\| a.min[1] > b.max[1] do return false
	if a.max[2] < b.min[2] \|\| a.min[2] > b.max[2] do return false
	// Overlapping on all axes means AABBs are intersecting
	return true
	}

	test_sphere_vs_aabb :: proc(sphere: Sphere, aabb: Aabb) -> bool {
	s := squared_distance_aabb(sphere.pos, aabb)
	return s <= sphere.rad * sphere.rad
	}

	test_sphere_vs_plane :: proc(sphere: Sphere, plane: Plane) -> bool {
	dist := signed_distance_plane(sphere.pos, plane)
	return abs(dist) <= sphere.rad
	}

	test_point_vs_halfspace :: proc(pos: Vec3, plane: Plane) -> bool {
	return signed_distance_plane(pos, plane) <= 0.0
	}

	test_sphere_vs_halfspace :: proc(sphere: Sphere, plane: Plane) -> bool {
	dist := signed_distance_plane(sphere.pos, plane)
	return dist <= sphere.rad
	}

	test_box_vs_plane :: proc(box: Box, plane: Plane) -> bool {
	// Compute the projection interval radius of b onto L(t) = b.c + t * p.n
	r := box.rad.x * abs(plane.normal.x) + box.rad.y * abs(plane.normal.y) + box.rad.z * abs(plane.normal.z)
	s := signed_distance_plane(box.pos, plane)
	return abs(s) <= r
	}

	test_capsule_vs_capsule :: proc(a, b: Capsule) -> bool {
	// Compute (squared) distance between the inner structures of the capsules
	_, points := closest_point_between_segments(a.a, a.b, b.a, b.b)
	squared_dist := length2(points[1] - points[0])
	// If (squared) distance smaller than (squared) sum of radii, they collide
	rad := a.rad + b.rad
	return squared_dist <= rad * rad
	}

	test_sphere_vs_capsule :: proc(sphere: Sphere, capsule: Capsule) -> bool {
	// Compute (squared) distance between sphere center and capsule line segment
	dist2 := squared_distance_segment(point = sphere.pos, a = capsule.a, b = capsule.b)
	// If (squared) distance smaller than (squared) sum of radii, they collide
	rad := sphere.rad + capsule.rad
	return dist2 <= rad * rad
	}

	test_capsule_vs_plane :: proc(capsule: Capsule, plane: Plane) -> bool {
	adist := dot(capsule.a, plane.normal) - plane.dist
	bdist := dot(capsule.b, plane.normal) - plane.dist
	// Intersects if on different sides of plane (distances have different signs)
	if adist * bdist < 0.0 do return true
	// Intersects if start or end position within radius from plane
	if abs(adist) <= capsule.rad \|\| abs(bdist) <= capsule.rad do return true
	return false
	}

	test_capsule_vs_halfspace :: proc(capsule: Capsule, plane: Plane) -> bool {
	adist := dot(capsule.a, plane.normal) - plane.dist
	bdist := dot(capsule.b, plane.normal) - plane.dist
	return min(adist, bdist) <= capsule.rad
	}

	test_ray_sphere :: proc(pos, dir: Vec3, sphere: Sphere) -> bool {
	m := pos - sphere.pos
	c := dot(m, m) - sphere.rad * sphere.rad
	// If there is definitely at least one real root, there must be an intersection
	if c <= 0 do return true
	b := dot(m, dir)
	// Early exit if ray origin outside sphere and ray pointing away from sphere
	if b > 0 do return false
	discr := b * b - c
	// A negative discriminant corresponds to ray missing sphere
	return discr >= 0
	}

	test_point_polyhedron :: proc(pos: Vec3, planes: []Plane) -> bool {
	for plane in planes {
	if signed_distance_plane(pos, plane) > 0.0 {
	return false
	}
	}
	return true
	}



	//////////////////////////////////////////////////////////////////////////////////
	// Intersections
	//

	// Given planes a and b, compute line L = p+t*d of their intersection.
	intersect_planes :: proc(a, b: Plane) -> (point, dir: Vec3, ok: bool) {
	// Compute direction of intersection line
	dir = cross(a.normal, b.normal)
	// If d is (near) zero, the planes are parallel (and separated)
	// or coincident, so they’re not considered intersecting
	denom := dot(dir, dir)
	EPS :: 1e-6
	if denom < EPS do return {}, dir, false
	// Compute point on intersection line
	point = cross(a.dist * b.normal - b.dist * a.normal, dir) / denom
	return point, dir, true
	}

	// TODO: moving vs static
	intersect_moving_spheres :: proc(a, b: Sphere, vel_a, vel_b: Vec3) -> (t: f32, ok: bool) {
	s := b.pos - a.pos
	v := vel_b - vel_a // Relative motion of s1 with respect to stationary s0
	r := a.rad + b.rad
	c := dot(s, s) - r * r
	if c < 0 {
	// Spheres initially overlapping so exit directly
	return 0, true
	}
	a := dot(v, v)
	EPS :: 1e-6
	if a < EPS {
	return 1, false // Spheres not moving relative each other
	}
	b := dot(v, s)
	if b >= 0 {
	return 1, false // Spheres not moving towards each other
	}
	d := b * b - a * c
	if d < 0 {
	return 1, false // No real-valued root, spheres do not intersect
	}
	t = (-b - sqrt(d)) / a
	return t, true
	}

	intersect_moving_aabbs :: proc(a, b: Aabb, vel_a, vel_b: Vec3) -> (t: [2]f32, ok: bool) {
	// Use relative velocity; effectively treating ’a’ as stationary
	return intersect_static_aabb_vs_moving_aabb(a, b, vel_relative = vel_b - vel_a)
	}

	// 'a' is static, 'b' is moving
	intersect_static_aabb_vs_moving_aabb :: proc(a, b: Aabb, vel_relative: Vec3) -> (t: [2]f32, ok: bool) {
	// Exit early if ‘a’ and ‘b’ initially overlapping
	if test_aabb_vs_aabb(a, b) {
	return 0, true
	}

	// Initialize ts of first and last contact
	t = {0, 1}

	// For each axis, determine ts of first and last contact, if any
	for i in 0 ..< 3 {
	if vel_relative[i] < 0.0 {
	if b.max[i] < a.min[i] do return 1, false // Nonintersecting and moving apart
	if a.max[i] < b.min[i] do t[0] = max(t[0], (a.max[i] - b.min[i]) / vel_relative[i])
	if b.max[i] > a.min[i] do t[1] = min(t[1], (a.min[i] - b.max[i]) / vel_relative[i])
	}

	if vel_relative[i] > 0.0 {
	if b.min[i] > a.max[i] do return 1, false // Nonintersecting and moving apart
	if b.max[i] < a.min[i] do t[0] = max(t[0], (a.min[i] - b.max[i]) / vel_relative[i])
	if a.max[i] > b.min[i] do t[1] = min(t[1], (a.max[i] - b.min[i]) / vel_relative[i])
	}

	// No overlap possible if t of first contact occurs after t of last contact
	if t[0] > t[1] do return 1, false
	}

	return t, true
	}

	// Intersect sphere s with movement vector v with plane p. If intersecting
	// return t t of collision and point at which sphere hits plane
	intersect_moving_sphere_vs_plane :: proc(sphere: Sphere, vel: Vec3, plane: Plane) -> (t: f32, point: Vec3, ok: bool) {
	// Compute distance of sphere center to plane
	dist := dot(plane.normal, sphere.pos) - plane.dist
	if abs(dist) <= sphere.rad {
	// The sphere is already overlapping the plane. Set t of
	// intersection to zero and q to sphere center
	return 0.0, sphere.pos, true
	}

	denom := dot(plane.normal, vel)
	if (denom * dist >= 0.0) {
	// No intersection as sphere moving parallel to or away from plane
	return 1.0, sphere.pos, false
	}

	// Sphere is moving towards the plane

	// Use +r in computations if sphere in front of plane, else -r
	r := dist > 0.0 ? sphere.rad : -sphere.rad
	t = (r - dist) / denom
	point = sphere.pos + vel * t - r * plane.normal
	return t, point, t <= 1.0
	}

	intersect_ray_sphere :: proc(pos: Vec3, dir: Vec3, sphere: Sphere) -> (t: f32, ok: bool) {
	m := pos - sphere.pos
	b := dot(m, dir)
	c := dot(m, m) - sphere.rad * sphere.rad
	// Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0)
	if c > 0 && b > 0 {
	return 0, false
	}
	discr := b * b - c
	// A negative discriminant corresponds to ray missing sphere
	if discr < 0 do return 0, false
	// Ray now found to intersect sphere, compute smallest t value of intersection
	t = -b - sqrt(discr)
	// If t is negative, ray started inside sphere so clamp t to zero
	t = max(0, t)
	return t, true
	}

	intersect_ray_aabb :: proc(pos: Vec3, dir: Vec3, aabb: Aabb, range: f32 = max(f32)) -> (t: [2]f32, ok: bool) {
	// https://tavianator.com/cgit/dimension.git/tree/libdimension/bvh/bvh.c#n196

	// This is actually correct, even though it appears not to handle edge cases
	// (dir.{x,y,z} == 0). It works because the infinities that result from
	// dividing by zero will still behave correctly in the comparisons. Rays
	// which are parallel to an axis and outside the box will have tmin == inf
	// or tmax == -inf, while rays inside the box will have tmin and tmax
	// unchanged.

	inv_dir := 1.0 / dir

	t1 := (aabb.min - pos) * inv_dir
	t2 := (aabb.max - pos) * inv_dir

	t = {max(min(t1.x, t2.x), min(t1.y, t2.y), min(t1.z, t2.z)), min(max(t1.x, t2.x), max(t1.y, t2.y), max(t1.z, t2.z))}

	return t, t[1] >= max(0.0, t[0]) && t[0] < range
	}

	intersect_ray_polyhedron :: proc(pos, dir: Vec3, planes: []Plane, segment: [2]f32 = {0.0, max(f32)}) -> (t: [2]f32, ok: bool) {
	t = segment
	for plane in planes {
	denom := dot(plane.normal, dir)
	dist := plane.dist - dot(plane.normal, pos)
	// Test if segment runs parallel to the plane
	if denom == 0.0 {
	// If so, return “no intersection” if segment lies outside plane
	if dist > 0.0 {
	return 0, false
	}
	} else {
	// Compute parameterized t value for intersection with current plane
	tplane := dist / denom
	if denom < 0.0 {
	// When entering halfspace, update tfirst if t is larger
	t[0] = max(t[0], tplane)
	} else {
	// When exiting halfspace, update tlast if t is smaller
	t[1] = min(t[1], tplane)
	}
	if t[0] > t[1] {
	return 0, false
	}
	}
	}
	return t, true
	}

	intersect_segment_triangle :: proc(
	segment: [2]Vec3,
	triangle: [3]Vec3,
	) -> (
	t: f32,
	normal: Vec3,
	barycentric: [3]f32,
	ok: bool,
	) {
	ab := triangle[1] - triangle[0]
	ac := triangle[2] - triangle[0]
	qp := segment[0] - segment[1]

	normal = cross(ab, ac)

	denom := dot(qp, normal)
	// If denom <= 0, segment is parallel to or points away from triangle
	if denom <= 0 {
	return 0, normal, 0, false
	}

	// Compute intersection t value of pq with plane of triangle. A ray
	// intersects if 0 <= t. Segment intersects iff 0 <= t <= 1. Delay
	// dividing by d until intersection has been found to pierce triangle
	ap := segment[0] - triangle[0]
	t = dot(ap, normal)
	if t < 0 {
	return
	}
	if t > denom {
	// For segment; exclude this code line for a ray test
	return
	}

	// Compute barycentric coordinate components and test if within bounds
	e := cross(qp, ap)
	barycentric.y = dot(ac, e)
	if barycentric.y < 0 \|\| barycentric.y > denom {
	return
	}
	barycentric.z = -dot(ab, e)
	if barycentric.z < 0 \|\| barycentric.y + barycentric.z > denom {
	return
	}

	// Segment/ray intersects triangle. Perform delayed division and
	// compute the last barycentric coordinate component
	ood := 1.0 / denom
	t *= ood
	barycentric.yz *= ood
	barycentric.x = 1.0 - barycentric.y - barycentric.z
	return t, normal, barycentric, true
	}

	intersect_segment_plane :: proc(segment: [2]Vec3, plane: Plane) -> (t: f32, point: Vec3, ok: bool) {
	ab := segment[1] - segment[0]
	t = (plane.dist - dot(plane.normal, segment[0])) / dot(plane.normal, ab)

	if t >= 0 && t <= 1 {
	point = segment[0] + t * ab
	return t, point, true
	}

	return t, segment[0], false
	}

	// TODO: alternative with capsule endcaps
	intersect_segment_cylinder :: proc(segment: [2]Vec3, cylinder: Cylinder) -> (t: f32, ok: bool) {
	d := cylinder.b - cylinder.a
	m := segment[0] - cylinder.a
	n := segment[1] - segment[0]
	md := dot(m, d)
	nd := dot(n, d)
	dd := dot(d, d)
	// Test if segment fully outside either endcap of cylinder
	if md < 0 && md + nd < 0 {
	return 0, false // Segment outside ’a’ side of cylinder
	}
	if md > dd && md + nd > dd {
	return 0, false // Segment outside ’b’ side of cylinder
	}
	nn := dot(n, n)
	mn := dot(m, n)
	a := dd * nn - nd * nd
	k := dot(m, m) - cylinder.rad * cylinder.rad
	c := dd * k - md * md
	EPS :: 1e-6
	if abs(a) < EPS {
	// Segment runs parallel to cylinder axis
	if c > 0 {
	return 0, false
	}
	// Now known that segment intersects cylinder; figure out how it intersects
	if md < 0 {
	// Intersect segment against ’a’ endcap
	t = -mn / nn
	} else if md > dd {
	// Intersect segment against ’b’ endcap
	t = (nd - mn) / nn
	} else {
	// ’a’ lies inside cylinder
	t = 0
	}
	return t, true
	}
	b := dd * mn - nd * md
	discr := b * b - a * c
	if discr < 0 {
	return 0, false // no real roots
	}
	t = (-b - sqrt(discr)) / a
	if t < 0 \|\| t > 1 {
	return 0, false // intersection outside segment
	}
	if md + t * nd < 0 {
	// Intersection outside cylinder on ’a’ side
	if nd <= 0 {
	// Segment pointing away from endcap
	return 0, false
	}
	t = -md / nd
	ok = k + 2 * t * (mn + t * nn) <= 0
	return t, ok
	} else if md + t * nd > dd {
	// Intersection outside cylinder on ’b’ side
	if nd >= 0 {
	// Segment pointing away from endcap
	return 0, false
	}
	t = (dd - md) / nd
	ok = k + dd - 2 * md + t * (2 * (mn - nd) + t * nn) <= 0
	return t, ok
	}
	return t, true
	}