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: words = null
var col_z: words = null
var col_r: words = 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: words = null         # the resolved position (x, z)

function col_add(x: int, z: int, r: int) -> void {
  if col_x == null { col_x = words(COL_CAP);  col_z = words(COL_CAP);  col_r = words(COL_CAP);  col_out = words(2) }
  if col_n >= COL_CAP { return }
  col_x[col_n] = x;  col_z[col_n] = z;  col_r[col_n] = r
  col_n += 1
  col_built = false
}
function col_cell_of(v: int, origin: int) -> int {
  var c = f_to_int(f_floor(f_div(f_add(f_sub(v, origin), fi(TERRAIN_HALF)), fi(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: int, pz: int, pr: int) -> 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 = f_sub(x, col_x[i]);  let ez = f_sub(z, col_z[i])
          let d2 = f_add(f_mul(ex, ex), f_mul(ez, ez))
          let rr = f_add(col_r[i], pr)
          if f_ls(d2, f_mul(rr, rr)) {
            let d = f_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 = F_ONE;  var uz = F_ZERO
            if f_gt(d, F_ZERO) { ux = f_div(ex, d);  uz = f_div(ez, d) }
            let push = f_sub(rr, d)
            x = f_add(x, f_mul(ux, push))
            z = f_add(z, f_mul(uz, push))
            moved = true
          }
        }
      }
    }
  }
  col_out[0] = x;  col_out[1] = z
  return moved
}
# is the segment from (x0,z0) to (x1,z1) clear of every circle (a camera line of sight)?
function col_clear(x0: int, z0: int, x1: int, z1: int, r: int) -> bool {
  let steps = 6
  for s in 0 .. steps + 1 {
    let t = fr(s, steps)
    let x = f_lerp(x0, x1, t);  let z = f_lerp(z0, z1, t)
    if col_resolve(x, z, r) { return false }
  }
  return true
}