diff --git a/SNNBot_garage/src/reservoir.nim b/SNNBot_garage/src/reservoir.nim index b943180..da5bfb1 100644 --- a/SNNBot_garage/src/reservoir.nim +++ b/SNNBot_garage/src/reservoir.nim @@ -19,54 +19,37 @@ type proc initLeadGrid*(): LeadGrid = result = LeadGrid() -proc cellIndex(vPerp: float, distance: float): int {.inline.} = - let vBin = clamp(int(vPerp + 8.5), 0, VPERP_BINS - 1) - let dBin = clamp(int(distance / DIST_BAND), 0, DIST_BINS - 1) - result = vBin * DIST_BINS + dBin - proc forward*(grid: var LeadGrid, vPerp, distance: float): float = - ## Returns circular mean offset in degrees, or -999.0 when no data. - let vBin = clamp(int(vPerp + 8.5), 0, VPERP_BINS - 1) - let dBin = clamp(int(distance / DIST_BAND), 0, DIST_BINS - 1) - - # Accumulate weighted contributions: direct cell (w=1), cardinal neighbors (w=0.5), diagonals (w=0.25) - var sinSum = 0.0 - var cosSum = 0.0 - var totalW = 0.0 - for dv in -1 .. 1: - for dd in -1 .. 1: - let vb = vBin + dv - let db = dBin + dd - if vb < 0 or vb >= VPERP_BINS or db < 0 or db >= DIST_BINS: continue - let c = grid.cells[vb * DIST_BINS + db] - if c.count == 0: continue - let w = if dv == 0 and dd == 0: 1.0 - elif dv == 0 or dd == 0: 0.5 - else: 0.25 - sinSum += w * c.sumSin / float(c.count) - cosSum += w * c.sumCos / float(c.count) - totalW += w - if totalW == 0.0: - return -999.0 + ## Returns bilinear-interpolated circular mean offset in degrees, or -999.0 when no data. + let vCont = clamp(vPerp + 8.0, 0.0, 15.999) + let dCont = clamp(distance / DIST_BAND, 0.0, 6.999) + let v0 = int(floor(vCont)); let v1 = min(v0 + 1, VPERP_BINS - 1) + let d0 = int(floor(dCont)); let d1 = min(d0 + 1, DIST_BINS - 1) + let fv = vCont - float(v0) + let fd = dCont - float(d0) + let corners = [(v0, d0, (1-fv)*(1-fd)), + (v1, d0, fv*(1-fd)), + (v0, d1, (1-fv)*fd), + (v1, d1, fv*fd)] + var sinSum, cosSum, totalW = 0.0 + for (vi, di, w) in corners: + let cell = grid.cells[vi * DIST_BINS + di] + if cell.count == 0: continue + sinSum += w * cell.sumSin / cell.count + cosSum += w * cell.sumCos / cell.count + totalW += w + if totalW == 0.0: return -999.0 result = radToDeg(arctan2(sinSum, cosSum)) proc learn*(grid: var LeadGrid, vPerp, distance, correctOffset: float) = let s = sin(degToRad(correctOffset)) let c = cos(degToRad(correctOffset)) - let vBin = clamp(int(vPerp + 8.5), 0, VPERP_BINS - 1) - let dBin = clamp(int(distance / DIST_BAND), 0, DIST_BINS - 1) - for dv in -1 .. 1: - for dd in -1 .. 1: - let vb = vBin + dv - let db = dBin + dd - if vb < 0 or vb >= VPERP_BINS or db < 0 or db >= DIST_BINS: continue - let w = if dv == 0 and dd == 0: 1.0 - elif dv == 0 or dd == 0: 0.3 - else: 0.1 - let i = vb * DIST_BINS + db - grid.cells[i].sumSin += w * s - grid.cells[i].sumCos += w * c - grid.cells[i].count += w + let vBin = clamp(int(floor(vPerp + 8.0)), 0, VPERP_BINS - 1) + let dBin = clamp(int(floor(distance / DIST_BAND)), 0, DIST_BINS - 1) + let i = vBin * DIST_BINS + dBin + grid.cells[i].sumSin += s + grid.cells[i].sumCos += c + grid.cells[i].count += 1.0 proc totalCount*(grid: LeadGrid): int = var s = 0.0