ludic/packages/ludic.render3d/collide.ludic

# ============================================================================
# collide.ludic — the static colliders of the world as circles on the ground
# plane (a trunk, a boulder, a tent), sorted once into 16 m cells over the whole
# terrain. A moving thing asks col_resolve for its position pushed out of every
# circle it overlaps: three by three cells, a few dozen tests, no broad phase
# needed. Float bits, metres.
# ============================================================================

const COL_CELL: int = 16
const COL_CAP: int = 120000

var col_x: floats = null
var col_z: floats = null
var col_r: floats = null
# What a collider occupies VERTICALLY: y0 its base, y1 its top, metres, float bits. A circle used
# to be an infinite pillar - you could not climb a boulder, and a knee-high rock stopped you dead,
# because there was no height to compare against. col_add keeps that shape (a span from far below
# to far above) so every existing caller behaves exactly as it did; col_add_h gives a real one.
var col_y0: floats = null
var col_y1: floats = null
var col_n: int = 0
var col_side: int = 0             # cells per side
var col_start: words = null       # per cell: first index into col_sorted (side*side + 1)
var col_sorted: words = null
var col_built: bool = false
var col_out: floats = null         # the resolved position (x, z)

const COL_LOW: float = -16777216.0    # -16777216.0: below any ground
const COL_HIGH: float = 16777216.0   #  16777216.0: above any sky
function col_add(x: float, z: float, r: float) -> void { col_add_h(x, z, r, COL_LOW, COL_HIGH) }
# a collider that occupies only y0 .. y1: a body above its top walks over it, a body below its base
# passes under, and col_top_at reports it as something to stand on
function col_add_h(x: float, z: float, r: float, y0: float, y1: float) -> void {
  if col_x == null {
    col_x = floats(COL_CAP);  col_z = floats(COL_CAP);  col_r = floats(COL_CAP)
    col_y0 = floats(COL_CAP);  col_y1 = floats(COL_CAP);  col_out = floats(2)
  }
  if col_n >= COL_CAP { return }
  col_x[col_n] = x;  col_z[col_n] = z;  col_r[col_n] = r
  col_y0[col_n] = y0;  col_y1[col_n] = y1
  col_n += 1
  col_built = false
}
function col_cell_of(v: float, origin: float) -> int {
  var c = int(Math.floor((v - origin + float(TERRAIN_HALF)) / float(COL_CELL)))
  if c < 0 { c = 0 }
  if c > col_side - 1 { c = col_side - 1 }
  return c
}
function col_build() -> void {
  col_side = (TERRAIN_HALF * 2) / COL_CELL
  let ncell = col_side * col_side
  if col_start == null { col_start = words(ncell + 1);  col_sorted = words(COL_CAP) }
  for i in 0 .. ncell + 1 { col_start[i] = 0 }
  for i in 0 .. col_n { col_start[col_cell_of(col_z[i], ter_oz) * col_side + col_cell_of(col_x[i], ter_ox) + 1] += 1 }
  for c in 0 .. ncell { col_start[c + 1] += col_start[c] }
  let fill = words(ncell)
  for c in 0 .. ncell { fill[c] = col_start[c] }
  for i in 0 .. col_n {
    let c = col_cell_of(col_z[i], ter_oz) * col_side + col_cell_of(col_x[i], ter_ox)
    col_sorted[fill[c]] = i
    fill[c] += 1
  }
  free(fill)
  col_built = true
  print(`colliders: {col_n}`)
}

# push (px, pz) with radius pr out of every circle it overlaps; the result is in col_out
function col_resolve(px: float, pz: float, pr: float) -> bool { return col_resolve_at(px, pz, pr, COL_LOW, COL_HIGH) }
# the same, for a body that occupies feet .. head: a collider whose span misses that is not in the
# way at all. This is what lets a hiker stand on top of a boulder rather than inside it.
function col_resolve_at(px: float, pz: float, pr: float, feet: float, head: float) -> bool {
  col_out[0] = px;  col_out[1] = pz
  if not col_built or col_n == 0 { return false }
  var x = px;  var z = pz
  var moved = false
  let cx = col_cell_of(px, ter_ox);  let cz = col_cell_of(pz, ter_oz)
  for pass in 0 .. 2 {
    for dz in 0 .. 3 {
      let zc = cz + dz - 1
      if zc < 0 or zc >= col_side { continue }
      for dx in 0 .. 3 {
        let xc = cx + dx - 1
        if xc < 0 or xc >= col_side { continue }
        let c = zc * col_side + xc
        for k in col_start[c] .. col_start[c + 1] {
          let i = col_sorted[k]
          let ex = x - col_x[i];  let ez = z - col_z[i]
          let d2 = ex * ex + ez * ez
          let rr = col_r[i] + pr
          # nothing to push out of if the body is wholly above its top or below its base
          if not (feet < col_y1[i]) { continue }
          if not (head > col_y0[i]) { continue }
          if d2 < rr * rr {
            let d = Math.sqrt(d2)
            # The UNIT normal out of this circle. (ex, ez) / d is always unit for d > 0,
            # because d is its own length - there is nothing to clamp and nothing that can
            # grow. Exactly at the centre there is no direction to be had, so any one will
            # do and +x is as good as another.
            #
            # This used to clamp d to 0.001 and then divide by it, having ALREADY set the
            # degenerate normal to (1, 0): a unit vector divided by a thousandth, so the
            # push came out a thousand times too big. A body standing dead centre on a
            # 0.5 m trunk was thrown roughly 800 m across the map instead of 0.85 m clear
            # of it. Off-centre - which is how anything actually arrives at a trunk - the
            # arithmetic was right, so it never showed up in play.
            var ux = 1.0;  var uz = 0.0
            if d > 0.0 { ux = ex / d;  uz = ez / d }
            let push = rr - d
            x = x + ux * push
            z = z + uz * push
            moved = true
          }
        }
      }
    }
  }
  col_out[0] = x;  col_out[1] = z
  return moved
}
# The highest collider top under (px, pz) that a body at `feet` could be standing on or step up to:
# tops above `reach` are a wall, not a step. F_ZERO-safe: returns `floor` when there is nothing, so
# a caller can pass the terrain height and use the answer directly as the ground.
function col_top_at(px: float, pz: float, pr: float, feet: float, reach: float, floor: float) -> float {
  var top = floor
  if not col_built or col_n == 0 { return top }
  let cx = col_cell_of(px, ter_ox);  let cz = col_cell_of(pz, ter_oz)
  let limit = feet + reach
  for dz in 0 .. 3 {
    let zc = cz + dz - 1
    if zc < 0 or zc >= col_side { continue }
    for dx in 0 .. 3 {
      let xc = cx + dx - 1
      if xc < 0 or xc >= col_side { continue }
      let c = zc * col_side + xc
      for k in col_start[c] .. col_start[c + 1] {
        let i = col_sorted[k]
        let ex = px - col_x[i];  let ez = pz - col_z[i]
        let d2 = ex * ex + ez * ez
        let rr = col_r[i] + pr
        if not (d2 < rr * rr) { continue }
        let t = col_y1[i]
        if t > limit { continue }          # too tall to step onto: it is a wall
        if t > top { top = t }
      }
    }
  }
  return top
}
# is the segment from (x0,z0) to (x1,z1) clear of every circle (a camera line of sight)?
function col_clear(x0: float, z0: float, x1: float, z1: float, r: float) -> bool {
  let steps = 6
  for s in 0 .. steps + 1 {
    let t = float(s) / float(steps)
    let x = Math.lerp(x0, x1, t);  let z = Math.lerp(z0, z1, t)
    if col_resolve(x, z, r) { return false }
  }
  return true
}

# No colliders, ready for another map's. The cell index is sized from TERRAIN_HALF on its
# first build and kept, so it is released too: a larger map would overrun the old one.
function col_reset() -> void {
  col_n = 0
  col_built = false
  if col_start != null { free(col_start);  col_start = null }
  if col_sorted != null { free(col_sorted);  col_sorted = null }
}