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

# 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.

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(render3d_st: mut Render3dState, x: float, z: float, r: float) -> void { col_add_h(render3d_st, 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(render3d_st: mut Render3dState, x: float, z: float, r: float, y0: float, y1: float) -> void {
  if render3d_st.col_x == null {
    render3d_st.col_x = floats(COL_CAP);  render3d_st.col_z = floats(COL_CAP);  render3d_st.col_r = floats(COL_CAP)
    render3d_st.col_y0 = floats(COL_CAP);  render3d_st.col_y1 = floats(COL_CAP);  render3d_st.col_out = floats(2)
  }
  if render3d_st.col_n >= COL_CAP { return }
  render3d_st.col_x[render3d_st.col_n] = x;  render3d_st.col_z[render3d_st.col_n] = z;  render3d_st.col_r[render3d_st.col_n] = r
  render3d_st.col_y0[render3d_st.col_n] = y0;  render3d_st.col_y1[render3d_st.col_n] = y1
  render3d_st.col_n += 1
  render3d_st.col_built = false
}
function col_cell_of(render3d_st: Render3dState, v: float, origin: float) -> int {
  var c = int(Math.floor((v - origin + float(render3d_st.TERRAIN_HALF)) / float(COL_CELL)))
  if c < 0 { c = 0 }
  if c > render3d_st.col_side - 1 { c = render3d_st.col_side - 1 }
  return c
}
function col_build(render3d_st: mut Render3dState) -> void {
  render3d_st.col_side = (render3d_st.TERRAIN_HALF * 2) / COL_CELL
  let ncell = render3d_st.col_side * render3d_st.col_side
  if render3d_st.col_start == null { render3d_st.col_start = words(ncell + 1);  render3d_st.col_sorted = words(COL_CAP) }
  for i in 0 .. ncell + 1 { render3d_st.col_start[i] = 0 }
  for i in 0 .. render3d_st.col_n { render3d_st.col_start[col_cell_of(render3d_st, render3d_st.col_z[i], render3d_st.ter_oz) * render3d_st.col_side + col_cell_of(render3d_st, render3d_st.col_x[i], render3d_st.ter_ox) + 1] += 1 }
  for c in 0 .. ncell { render3d_st.col_start[c + 1] += render3d_st.col_start[c] }
  let fill = words(ncell)
  for c in 0 .. ncell { fill[c] = render3d_st.col_start[c] }
  for i in 0 .. render3d_st.col_n {
    let c = col_cell_of(render3d_st, render3d_st.col_z[i], render3d_st.ter_oz) * render3d_st.col_side + col_cell_of(render3d_st, render3d_st.col_x[i], render3d_st.ter_ox)
    render3d_st.col_sorted[fill[c]] = i
    fill[c] += 1
  }
  free(fill)
  render3d_st.col_built = true
  print(`colliders: {render3d_st.col_n}`)
}

# push (px, pz) with radius pr out of every circle it overlaps; the result is in col_out
function col_resolve(render3d_st: mut Render3dState, px: float, pz: float, pr: float) -> bool { return col_resolve_at(render3d_st, 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(render3d_st: mut Render3dState, px: float, pz: float, pr: float, feet: float, head: float) -> bool {
  render3d_st.col_out[0] = px;  render3d_st.col_out[1] = pz
  if not render3d_st.col_built or render3d_st.col_n == 0 { return false }
  var x = px;  var z = pz
  var moved = false
  let cx = col_cell_of(render3d_st, px, render3d_st.ter_ox);  let cz = col_cell_of(render3d_st, pz, render3d_st.ter_oz)
  for pass in 0 .. 2 {
    for dz in 0 .. 3 {
      let zc = cz + dz - 1
      if zc < 0 or zc >= render3d_st.col_side { continue }
      for dx in 0 .. 3 {
        let xc = cx + dx - 1
        if xc < 0 or xc >= render3d_st.col_side { continue }
        let c = zc * render3d_st.col_side + xc
        for k in render3d_st.col_start[c] .. render3d_st.col_start[c + 1] {
          let i = render3d_st.col_sorted[k]
          let ex = x - render3d_st.col_x[i];  let ez = z - render3d_st.col_z[i]
          let d2 = ex * ex + ez * ez
          let rr = render3d_st.col_r[i] + pr
          # nothing to push out of if the body is wholly above its top or below its base
          if not (feet < render3d_st.col_y1[i]) { continue }
          if not (head > render3d_st.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
          }
        }
      }
    }
  }
  render3d_st.col_out[0] = x;  render3d_st.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(render3d_st: Render3dState, px: float, pz: float, pr: float, feet: float, reach: float, floor: float) -> float {
  var top = floor
  if not render3d_st.col_built or render3d_st.col_n == 0 { return top }
  let cx = col_cell_of(render3d_st, px, render3d_st.ter_ox);  let cz = col_cell_of(render3d_st, pz, render3d_st.ter_oz)
  let limit = feet + reach
  for dz in 0 .. 3 {
    let zc = cz + dz - 1
    if zc < 0 or zc >= render3d_st.col_side { continue }
    for dx in 0 .. 3 {
      let xc = cx + dx - 1
      if xc < 0 or xc >= render3d_st.col_side { continue }
      let c = zc * render3d_st.col_side + xc
      for k in render3d_st.col_start[c] .. render3d_st.col_start[c + 1] {
        let i = render3d_st.col_sorted[k]
        let ex = px - render3d_st.col_x[i];  let ez = pz - render3d_st.col_z[i]
        let d2 = ex * ex + ez * ez
        let rr = render3d_st.col_r[i] + pr
        if not (d2 < rr * rr) { continue }
        let t = render3d_st.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(render3d_st: mut Render3dState, 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(render3d_st, 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(render3d_st: mut Render3dState) -> void {
  render3d_st.col_n = 0
  render3d_st.col_built = false
  if render3d_st.col_start != null { free(render3d_st.col_start);  render3d_st.col_start = null }
  if render3d_st.col_sorted != null { free(render3d_st.col_sorted);  render3d_st.col_sorted = null }
}