/* Next Drive compute core: line-by-line port of drive_core.py (simulator, perception, drivers, safety check). */ (function (root, factory) { if (typeof module === "object" && module.exports) module.exports = factory(); else root.Core = factory(); })(typeof self !== "undefined" ? self : this, function () { "use strict"; const DT = 0.1, VMAX = 14.0, V_LIMIT = 13.9, T_MAX = 45.0; const ACTIONS = [-6.0, -3.0, -1.5, 0.0, 1.0, 2.0]; const ACTION_NAMES = ["brake hard", "brake", "ease off", "hold speed", "speed up", "accelerate"]; const LANE = 1.75, S_STOP = 60.0, HALF = 2.2; const ROUTES = ["straight", "left", "right"], KINDS = ["light", "crosswalk", "lead_brake", "occlusion", "mixed"]; const TURN = { right: [-5.75, 4.0], left: [-5.25, 7.0] }; const ASSETS = ["model.json"]; const PI = Math.PI; function pathLen(route) { if (route === "straight") return 130.0; const [y0, r] = TURN[route]; return (y0 + 70.0) + r * PI / 2 + 55.0; } function pose(route, s) { s = Math.max(0.0, Math.min(s, pathLen(route))); if (route === "straight") return [LANE, -70.0 + s, PI / 2]; const [y0, r] = TURN[route], L1 = y0 + 70.0; if (s < L1) return [LANE, -70.0 + s, PI / 2]; const arc = r * PI / 2; if (s < L1 + arc) { const th = (s - L1) / r; if (route === "right") return [LANE + r - r * Math.cos(th), y0 + r * Math.sin(th), PI / 2 - th]; return [LANE - r + r * Math.cos(th), y0 + r * Math.sin(th), PI / 2 + th]; } const d = s - L1 - arc; if (route === "right") return [LANE + r + d, y0 + r, 0.0]; return [LANE - r - d, y0 + r, PI]; } function circles(x, y, h) { const c = Math.cos(h), s = Math.sin(h); return [-1.4, 0.0, 1.4].map(d => [x + d * c, y + d * s]); } function hitsCar(e, o, m) { m = m || 0; for (const [ax, ay] of e) for (const [bx, by] of o) { const dx = ax - bx, dy = ay - by, rr = 2.0 + m; if (dx * dx + dy * dy < rr * rr) return true; } return false; } function hitsPed(e, px, py, m) { m = m || 0; for (const [ax, ay] of e) { const dx = ax - px, dy = ay - py, rr = 1.35 + m; if (dx * dx + dy * dy < rr * rr) return true; } return false; } function lightNS(sc, t) { const G = sc.G, Y = 3.0, R = sc.R, u = (t + sc.phase) % (G + Y + R); if (u < G) return [0, G - u]; if (u < G + Y) return [1, G + Y - u]; return [2, G + Y + R - u]; } function lightEW(sc, t) { const G = sc.G, Y = 3.0, R = sc.R, u = (t + sc.phase) % (G + Y + R); if (u < G + Y) return 2; const r = u - G - Y; if (r < 1.0) return 2; if (r < R - 4.0) return 0; if (r < R - 1.0) return 1; return 2; } function makeScenario(seed, kind, density) { kind = kind || "mixed"; density = density || 1; const rng = NLRT.mulberry32((seed * 7919 + KINDS.indexOf(kind) * 101 + density * 13) % 4294967296); const u = rng.random(); let route = u < 0.5 ? "straight" : (u < 0.75 ? "left" : "right"); if ((kind === "occlusion" || kind === "crosswalk") && rng.random() < 0.5) route = "straight"; const G = 7.0 + 6.0 * rng.random(), R = 8.0 + 5.0 * rng.random(); const sc = { seed, kind, density, route, G, R, phase: rng.random() * (G + 3.0 + R), cars: [], peds: [], parked: [], lead: null }; for (let k = 0; k < density + 1; k++) for (const d of [1, -1]) sc.cars.push({ axis: "x", dir: d, lane: -LANE * d, p0: -70.0 * d, v0: 8.0 + 4.0 * rng.random(), t0: k * 7.0 + 5.0 * rng.random() }); if (route === "left" || kind === "mixed") for (let k = 0; k < density; k++) sc.cars.push({ axis: "y", dir: -1, lane: -LANE, p0: 70.0, v0: 8.0 + 4.0 * rng.random(), t0: k * 6.0 + 4.0 * rng.random() }); if (kind === "crosswalk" || kind === "mixed") for (let k = 0; k < density + 1; k++) { const d = rng.random() < 0.5 ? 1 : -1; sc.peds.push({ x0: -9.0 * d, y0: -7.5 + 1.6 * (rng.random() - 0.5), vx: d * (1.0 + 0.6 * rng.random()), vy: 0.0, t0: 1.0 + 11.0 * rng.random(), x_end: 9.0 * d }); } if (kind === "lead_brake" || kind === "mixed") sc.lead = { s0: 14.0 + 10.0 * rng.random(), v0: 9.0 + 3.0 * rng.random(), tb: 2.5 + 5.0 * rng.random(), db: 1.5 + 1.5 * rng.random() }; if (kind === "occlusion" || kind === "mixed") { let y = -46.0; const gaps = []; while (y < -16.0) { const L = 4.6; sc.parked.push([4.6, y + L / 2, 2.0, L]); y += L + 0.4 + (rng.random() < 0.45 ? 2.6 : 0.0); gaps.push(y - 0.2); } const nEmerge = density; for (let k = 0; k < Math.min(nEmerge, gaps.length); k++) { const gy = gaps[rng.randint(gaps.length)] - 1.0, D = 12.0 + 10.0 * rng.random(); sc.peds.push({ x0: 5.8, y0: gy, vx: -(1.2 + 0.4 * rng.random()), vy: 0.0, t0: Math.max(0.0, (gy + 70.0 - HALF - D) / 10.0), x_end: -6.0 }); } } return sc; } function initWorld(sc) { const w = { t: 0.0, s: 0.0, v: 10.0, a: 0.0, cars: [], peds: [], lead: null }; for (const c of sc.cars) w.cars.push({ p: c.p0, v: c.v0, active: false, done: false, axis: c.axis, dir: c.dir, lane: c.lane, v0: c.v0, t0: c.t0 }); for (const p of sc.peds) w.peds.push({ x: p.x0, y: p.y0, vx: 0.0, vy: 0.0, done: false, t0: p.t0, x_end: p.x_end, svx: p.vx }); if (sc.lead) w.lead = { s: sc.lead.s0, v: sc.lead.v0, ...sc.lead }; return w; } function carXYH(c) { if (c.axis === "x") return [c.p, c.lane, c.dir > 0 ? 0.0 : PI]; return [c.lane, c.p, c.dir > 0 ? PI / 2 : -PI / 2]; } function stepAgents(sc, w) { const t = w.t, ew = lightEW(sc, t), ns = lightNS(sc, t)[0]; const lanes = new Map(); for (const c of w.cars) { if (!c.active && !c.done && t >= c.t0) c.active = true; if (c.active) { const k = c.axis + c.dir; if (!lanes.has(k)) lanes.set(k, []); lanes.get(k).push(c); } } for (const cs of lanes.values()) { cs.sort((a, b) => (-a.p * a.dir) - (-b.p * b.dir)); cs.forEach((c, i) => { const light = c.axis === "x" ? ew : ns, front = c.p * c.dir + HALF, dStop = -10.0 - front; let a = Math.min(2.0, 1.5 * (c.v0 - c.v)); if (light !== 0 && dStop > -1.0 && c.v * c.v / (2 * 3.0) < dStop + 1.0) a = Math.min(a, -c.v * c.v / (2 * Math.max(dStop - 0.5, 0.3))); if (i > 0) { const gap = (cs[i - 1].p - c.p) * c.dir - 4.4; if (gap < 6.0 + c.v * 1.2) a = Math.min(a, gap > 3.0 ? -3.0 : -6.0); } c.v = Math.max(0.0, c.v + Math.max(-6.0, a) * DT); c.p += c.dir * c.v * DT; if (c.p * c.dir > 75.0) { c.active = false; c.done = true; } }); } for (const p of w.peds) { if (p.done) continue; if (t >= p.t0) { p.vx = p.svx; p.x += p.vx * DT; if ((p.x - p.x_end) * p.svx > 0) p.done = true; } } const L = w.lead; if (L !== null) { let a; if (L.tb <= t && t < L.tb + L.db) a = -6.0; else { a = Math.min(2.0, 1.5 * (L.v0 - L.v)); const dStop = S_STOP - (L.s + HALF); if (ns !== 0 && dStop > -1.0 && L.v * L.v / (2 * 3.0) < dStop + 1.0) a = Math.min(a, -L.v * L.v / (2 * Math.max(dStop - 0.5, 0.3))); } L.v = Math.max(0.0, L.v + Math.max(-6.0, a) * DT); L.s += L.v * DT; } } function segHitsRect(x0, y0, x1, y1, rect) { const [cx, cy, wd, ht] = rect, xmin = cx - wd / 2, xmax = cx + wd / 2, ymin = cy - ht / 2, ymax = cy + ht / 2; let t0 = 0.0, t1 = 1.0; for (const [p, q] of [[-(x1 - x0), x0 - xmin], [x1 - x0, xmax - x0], [-(y1 - y0), y0 - ymin], [y1 - y0, ymax - y0]]) { if (p === 0) { if (q < 0) return false; } else { const r = q / p; if (p < 0) { if (r > t1) return false; if (r > t0) t0 = r; } else { if (r < t0) return false; if (r < t1) t1 = r; } } } return true; } function observe(sc, w) { const [ex, ey, eh] = pose(sc.route, w.s), eye = [ex + 1.0 * Math.cos(eh), ey + 1.0 * Math.sin(eh)]; const agents = []; for (const c of w.cars) if (c.active) { const [x, y, h] = carXYH(c); agents.push({ kind: "car", x, y, h, vx: c.v * Math.cos(h), vy: c.v * Math.sin(h) }); } const L = w.lead; if (L !== null && L.s < pathLen(sc.route)) { const [x, y, h] = pose(sc.route, L.s); agents.push({ kind: "lead", x, y, h, vx: L.v * Math.cos(h), vy: L.v * Math.sin(h), s: L.s, v: L.v }); } const visible = []; for (const p of w.peds) { if (p.done) continue; const dx = p.x - eye[0], dy = p.y - eye[1]; const vis = dx * dx + dy * dy < 3600.0 && !sc.parked.some(r => segHitsRect(eye[0], eye[1], p.x, p.y, r)); visible.push(vis); if (vis) agents.push({ kind: "ped", x: p.x, y: p.y, h: 0.0, vx: p.vx, vy: p.vy }); } const phantoms = [], sFront = w.s + HALF; for (const r of sc.parked) { const gy = r[1] + r[3] / 2 + 0.3, ahead = (gy + 70.0) - sFront; if (0.0 < ahead && ahead < 35.0 && segHitsRect(eye[0], eye[1], 4.2, gy, r)) phantoms.push({ x: 4.2, y: gy, vx: -1.4, vy: 0.0, ahead }); } const [ls, rem] = lightNS(sc, w.t); return { t: w.t, s: w.s, v: w.v, a: w.a, route: sc.route, agents, phantoms, light: ls, remaining: rem, sc, visible }; } function rolloutHits(obs, plan, horizon, dt, mCar, mPed) { horizon = horizon == null ? 3.0 : horizon; dt = dt == null ? 0.25 : dt; mCar = mCar == null ? 0.4 : mCar; mPed = mPed == null ? 0.6 : mPed; let s = obs.s, v = obs.v; const route = obs.route, n = Math.round(horizon / dt); let excess = 0.0, redT = null; const lead = obs.agents.filter(a => a.kind === "lead"); for (let k = 1; k <= n; k++) { const tau = k * dt, a = plan(k, tau); v = Math.min(VMAX, Math.max(0.0, v + a * dt)); const sPrev = s; s = s + v * dt; if (v > V_LIMIT) excess += v - V_LIMIT; for (const ph of obs.phantoms) { const d = ph.y + 70.0 - (s + HALF); if (d > -1.0) excess += Math.max(0.0, v - (Math.sqrt(2 * 4.0 * Math.max(d - 2.0, 0.0)) + 3.0)); } for (const ag of lead) { const gap = ag.s + ag.v * tau - s - 4.4; excess += 0.5 * Math.max(0.0, 2.0 + 1.2 * v - gap); } if (redT === null && s > sPrev) { const lt = lightNS(obs.sc, obs.t + tau)[0]; const crossing = sPrev + HALF < S_STOP - 0.5 && S_STOP - 0.5 <= s + HALF; const creeping = S_STOP - 0.5 <= obs.s + HALF && obs.s + HALF < S_STOP + 6.0 && obs.v < 3.0; if (lt === 2 && (crossing || creeping)) redT = tau; } const [ex, ey, eh] = pose(route, s), ec = circles(ex, ey, eh); for (const ag of obs.agents) { const px = ag.x + ag.vx * tau, py = ag.y + ag.vy * tau; if ((px - ex) * (px - ex) + (py - ey) * (py - ey) > 64.0) continue; if (ag.kind === "ped") { if (hitsPed(ec, px, py, mPed)) return [tau, redT, s, excess]; } else if (hitsCar(ec, circles(px, py, ag.h), mCar)) return [tau, redT, s, excess]; } } return [null, redT, s, excess]; } function expert(obs) { let best = 0, bestCost = Infinity; ACTIONS.forEach((a1, i) => { for (const a2 of ACTIONS) { const [hit, red, sEnd, excess] = rolloutHits(obs, (k, tau) => (tau <= 1.0 + 1e-9 ? a1 : a2)); let cost = 0.0; if (hit !== null) cost += 1e5 - 1e4 * hit; if (red !== null) cost += 2e4; cost += -10.0 * (sEnd - obs.s); cost += a1 === -6.0 ? 40.0 : (a1 === -3.0 ? 8.0 : 0.0); cost += 3.0 * Math.abs(a1 - obs.a); cost += 50.0 * excess; if (cost < bestCost) { best = i; bestCost = cost; } } }); return best; } function ruleDriver(obs) { const v = obs.v, s = obs.s, route = obs.route, sFront = s + HALF, obst = []; for (const ag of obs.agents) if (ag.kind === "lead") obst.push([ag.s - s - 4.4, ag.v]); const dStop = S_STOP - sFront; if (dStop > 0 && (obs.light === 2 || (obs.light === 1 && v * v / (2 * 3.0) < dStop))) obst.push([dStop - 0.5, 0.0]); for (const ag of obs.agents) { if (ag.kind !== "ped") continue; for (let j = 0; j < 51; j += 2) { const [px, py] = pose(route, s + j), fx = ag.x + ag.vx * 1.5, fy = ag.y + ag.vy * 1.5; const d1 = (ag.x - px) * (ag.x - px) + (ag.y - py) * (ag.y - py), d2 = (fx - px) * (fx - px) + (fy - py) * (fy - py); if (Math.min(d1, d2) < 6.76) { obst.push([j - HALF - 1.5, 0.0]); break; } } } if (route === "left") { const y0 = TURN.left[0], dTurn = (y0 + 70.0) - sFront; if (dTurn > -1.0) for (const ag of obs.agents) if (ag.kind === "car" && ag.vy < -0.5 && -4.0 < ag.y && ag.y < 45.0 && ag.x < 0) { obst.push([dTurn - 0.5, 0.0]); break; } } const q = v / 12.0, q4 = (q * q) * (q * q); let a = 1.5 * (1.0 - q4); for (let [gap, vo] of obst) { gap = Math.max(gap, 0.1); const sstar = 2.0 + v * 1.2 + v * (v - vo) / (2 * Math.sqrt(1.5 * 3.0)); a = Math.min(a, 1.5 * (1.0 - q4 - (sstar / gap) * (sstar / gap))); } a = Math.max(-6.0, Math.min(2.0, a)); let bi = 0; for (let i = 1; i < ACTIONS.length; i++) if (Math.abs(ACTIONS[i] - a) < Math.abs(ACTIONS[bi] - a)) bi = i; return bi; } function features(obs) { const v = obs.v, s = obs.s, dStop = S_STOP - (s + HALF); const f = [v / 14.0, obs.a / 6.0, Math.min(Math.max(dStop, 0.0), 60.0) / 60.0, dStop < 0 ? 1.0 : 0.0, obs.light === 0 ? 1.0 : 0.0, obs.light === 1 ? 1.0 : 0.0, obs.light === 2 ? 1.0 : 0.0, Math.min(obs.remaining, 15.0) / 15.0]; const lead = obs.agents.filter(a => a.kind === "lead"); if (lead.length) f.push(Math.min(Math.max(lead[0].s - s - 4.4, 0.0), 50.0) / 50.0, Math.max(-1.0, Math.min(1.0, (lead[0].v - v) / 14.0))); else f.push(1.0, 0.0); for (const acc of [0.0, -3.0, 1.0]) { const hit = rolloutHits(obs, () => acc, 5.0)[0]; f.push((hit !== null ? hit : 5.0) / 5.0); } const [ex, ey] = pose(obs.route, s); const peds = obs.agents.filter(a => a.kind === "ped").map(a => Math.sqrt((a.x - ex) * (a.x - ex) + (a.y - ey) * (a.y - ey))); f.push(peds.length ? Math.min(Math.min(...peds), 40.0) / 40.0 : 1.0); f.push(obs.phantoms.length ? Math.min(Math.min(...obs.phantoms.map(p => p.ahead)), 40.0) / 40.0 : 1.0); f.push(obs.agents.some(a => a.kind === "car" && Math.abs(a.x) < 12.0 && Math.abs(a.y) < 12.0) ? 1.0 : 0.0); for (const r of ROUTES) f.push(obs.route === r ? 1.0 : 0.0); f.push(Math.min(v * v / 12.0, 60.0) / 60.0); f.push(obs.light !== 0 && dStop > 0 && v * v / 6.0 < dStop + 1.0 ? 1.0 : 0.0); return f; } function learned(obs, layers) { return NLRT.argmax(NLRT.forward(layers, NLRT.T([21], Float64Array.from(features(obs)))).data); } function shield(obs, i) { const unsafe = a => { const [hit, red] = rolloutHits(obs, (k, tau) => (tau <= 0.5 + 1e-9 ? a : -6.0), 3.0, 0.25, 0.2, 0.4); if (hit !== null) return true; if (red !== null) return rolloutHits(obs, () => -6.0, 3.0, 0.25, 0.2, 0.4)[1] === null; return false; }; if (!unsafe(ACTIONS[i])) return [i, false]; for (let j = i - 1; j >= 0; j--) if (!unsafe(ACTIONS[j])) return [j, true]; return [0, true]; } function runEpisode(sc, driver, layers, record) { record = record !== false; const w = initWorld(sc), route = sc.route, goal = pathLen(route) - 5.0; const out = { collision: false, collision_with: null, red_violations: 0, reached: false, hard_brakes: 0, overrides: 0, steps: 0, time: 0.0, decision_ms: [], trace: [], events: [], actions: [] }; let prevA = 0.0; const now = () => (typeof performance !== "undefined" ? performance.now() : Date.now()); for (let step = 0; step < Math.round(T_MAX / DT); step++) { const obs = observe(sc, w); const t0 = now(); let i, override = false; if (driver === "expert") i = expert(obs); else if (driver === "rule") i = ruleDriver(obs); else if (driver === "learned") i = learned(obs, layers); else [i, override] = shield(obs, learned(obs, layers)); out.decision_ms.push(now() - t0); const a = ACTIONS[i]; out.actions.push(i); if (override) out.overrides++; if (a <= -6.0 && prevA > -6.0) { out.hard_brakes++; out.events.push({ t: Math.round(w.t * 100) / 100, type: "hard_brake" }); } prevA = a; const sFrontPrev = w.s + HALF; w.v = Math.min(VMAX, Math.max(0.0, w.v + a * DT)); w.s += w.v * DT; w.a = a; stepAgents(sc, w); w.t = (step + 1) * DT; if (sFrontPrev < S_STOP && S_STOP <= w.s + HALF && lightNS(sc, w.t)[0] === 2) { out.red_violations++; out.events.push({ t: Math.round(w.t * 100) / 100, type: "red_light" }); } const [ex, ey, eh] = pose(route, w.s), ec = circles(ex, ey, eh); let hit = null; for (const c of w.cars) if (c.active && hitsCar(ec, circles(...carXYH(c)))) hit = "car"; if (w.lead !== null && w.lead.s < pathLen(route) && hitsCar(ec, circles(...pose(route, w.lead.s)))) hit = "lead car"; for (const p of w.peds) if (!p.done && hitsPed(ec, p.x, p.y)) hit = "pedestrian"; if (record) { out.trace.push({ t: w.t, x: ex, y: ey, h: eh, v: w.v, i, override, cars: w.cars.filter(c => c.active).map(carXYH).concat(w.lead !== null && w.lead.s < pathLen(route) ? [pose(route, w.lead.s).concat(["lead"])] : []), peds: w.peds.filter(p => !p.done).map(p => [p.x, p.y]), visible: obs.visible, light: lightNS(sc, w.t)[0], ew: lightEW(sc, w.t) }); } out.steps = step + 1; out.time = w.t; if (hit) { out.collision = true; out.collision_with = hit; out.events.push({ t: Math.round(w.t * 100) / 100, type: "collision", with: hit }); break; } if (w.s >= goal) { out.reached = true; break; } } return out; } function init(assets) { const m = NLRT.decode(assets["model.json"]); return { layers: m.nets.policy, meta: m.meta }; } function runCase(ctx, input) { const sc = makeScenario(input.seed, input.kind, input.density); const r = runEpisode(sc, input.driver, ctx.layers, false); return { route: sc.route, collision: r.collision, red_violations: r.red_violations, reached: r.reached, hard_brakes: r.hard_brakes, overrides: r.overrides, steps: r.steps, actions_hash: r.actions.reduce((h, a, k) => (h * 31 + a + k) % 1000000007, 7) }; } return { ASSETS, init, ACTIONS, ACTION_NAMES, KINDS, ROUTES, S_STOP, HALF, LANE, makeScenario, runEpisode, pose, pathLen, lightNS, lightEW, features, runCase }; });