also looking at this
feat: coordinate modeled two-carrier cargo handover
#103
25 files
+1654
-13
| @@ -0,0 +1,166 @@ | ||
| 1 | import type { IntakeParcel } from '../controls/physics' | |
| 2 | import type { SensorVector } from '../sensors/types' | |
| 3 | import { multirotorModel } from '../controls/model' | |
| 4 | ||
| 5 | interface StationDeclaration { | |
| 6 | atMs: number | |
| 7 | parcel: IntakeParcel | |
| 8 | pickup: { centerM: SensorVector, radiusM: number } | |
| 9 | destinationM: SensorVector | |
| 10 | maxSpeedMps: number | |
| 11 | maxAccelerationMps2: number | |
| 12 | } | |
| 13 | export interface CargoStationSnapshot { | |
| 14 | evidence: 'modeled-cargo-station-v1' | |
| 15 | physicalActuationEnabled: false | |
| 16 | phase: 'falling' | 'held' | 'missed' | 'moving' | 'braking' | 'cancelled' | 'ready' | 'departed' | |
| 17 | atMs: number | |
| 18 | startedAtMs: number | null | |
| 19 | durationMs: number | null | |
| 20 | cancelRequestedAtMs: number | null | |
| 21 | stoppedAtMs: number | null | |
| 22 | ownedMassKg: number | |
| 23 | parcel: IntakeParcel | null | |
| 24 | forceN: SensorVector | |
| 25 | impact: { atMs: number, positionM: SensorVector, velocityBeforeMps: SensorVector, impulseNs: SensorVector, dissipationJ: number } | null | |
| 26 | energy: { initialJ: number, currentJ: number, departedJ: number, stationNetWorkJ: number, positiveWorkJ: number, negativeWorkJ: number, planeDissipationJ: number, residualJ: number } | |
| 27 | } | |
| 28 | function record(input: unknown, names: string[]) { | |
| 29 | if (!input || typeof input !== 'object' || Object.getPrototypeOf(input) !== Object.prototype || Reflect.ownKeys(input).length !== names.length) throw new TypeError() | |
| 30 | const captured: Record<string, unknown> = {} | |
| 31 | for (const name of names) { | |
| 32 | const d = Object.getOwnPropertyDescriptor(input, name) | |
| 33 | if (!d?.enumerable || !Object.hasOwn(d, 'value')) throw new TypeError() | |
| 34 | captured[name] = d.value | |
| 35 | } | |
| 36 | return captured | |
| 37 | } | |
| 38 | function vector(input: unknown, min: number, max: number): SensorVector { | |
| 39 | if (!Array.isArray(input) || input.length !== 3 || Reflect.ownKeys(input).length !== 4) throw new TypeError() | |
| 40 | const captured: number[] = [] | |
| 41 | for (let i = 0; i < 3; i++) { | |
| 42 | const d = Object.getOwnPropertyDescriptor(input, String(i)) | |
| 43 | if (!d?.enumerable || !Object.hasOwn(d, 'value') || typeof d.value !== 'number' || !Number.isFinite(d.value) || d.value < min || d.value > max) throw new TypeError() | |
| 44 | captured.push(d.value) | |
| 45 | } | |
| 46 | return captured as SensorVector | |
| 47 | } | |
| 48 | const within = (v: unknown, min: number, max: number): v is number => typeof v === 'number' && Number.isFinite(v) && v >= min && v <= max | |
| 49 | const distance = (a: SensorVector, b: SensorVector) => Math.hypot(...a.map((n, i) => n - b[i]!)) | |
| 50 | function durationMs(lengthM: number, speed: number, acceleration: number) { | |
| 51 | // Peak derivatives of 10u^3 - 15u^4 + 6u^5 are 15/8 and 10/sqrt(3). | |
| 52 | return Math.ceil(1000 * Math.max(.5, 1.875 * lengthM / speed, Math.sqrt(10 / Math.sqrt(3) * lengthM / acceleration)) / 8) * 8 | |
| 53 | } | |
| 54 | function capture(input: unknown): StationDeclaration { | |
| 55 | try { | |
| 56 | const v = record(input, ['atMs', 'parcel', 'pickup', 'destinationM', 'maxSpeedMps', 'maxAccelerationMps2']) | |
| 57 | const p = record(v.parcel, ['id', 'massKg', 'positionM', 'velocityMps']), pickup = record(v.pickup, ['centerM', 'radiusM']) | |
| 58 | if (!Number.isSafeInteger(v.atMs) || !within(v.atMs, 0, Number.MAX_SAFE_INTEGER - 180000) || v.atMs % 4 !== 0 | |
| 59 | || typeof p.id !== 'string' || !/^[a-z][a-z0-9-]{0,39}$/.test(p.id) || !within(p.massKg, Number.MIN_VALUE, .25) | |
| 60 | || !within(pickup.radiusM, .05, 2) || !within(v.maxSpeedMps, .05, 1) || !within(v.maxAccelerationMps2, .05, 2)) throw new TypeError() | |
| 61 | const positionM = vector(p.positionM, -1000, 1000), velocityMps = vector(p.velocityMps, -5, 5), centerM = vector(pickup.centerM, -1000, 1000), destinationM = vector(v.destinationM, -1000, 1000) | |
| 62 | if (!within(centerM[2], -1, 0) || !within(positionM[2], centerM[2] - 3, centerM[2]) || destinationM[2] < centerM[2] - 1 || destinationM[2] > centerM[2] | |
| 63 | || distance(centerM, destinationM) + pickup.radiusM > 20 | |
| 64 | || durationMs(distance(centerM, destinationM) + pickup.radiusM, v.maxSpeedMps, v.maxAccelerationMps2) > 120000) throw new TypeError() | |
| 65 | return { atMs: v.atMs, parcel: { id: p.id, massKg: p.massKg, positionM, velocityMps }, pickup: { centerM, radiusM: pickup.radiusM }, destinationM, maxSpeedMps: v.maxSpeedMps, maxAccelerationMps2: v.maxAccelerationMps2 } | |
| 66 | } | |
| 67 | catch { throw new TypeError('cargo-station-constructor') } | |
| 68 | } | |
| 69 | ||
| 70 | /** One point parcel on a prescribed, finite powered station rail. No hardware IO or custody ACK. */ | |
| 71 | export function createCargoStationHandler(input: unknown) { | |
| 72 | const config = capture(input), gravity = multirotorModel.gravityMps2 | |
| 73 | let parcel: IntakeParcel | null = config.parcel, phase: CargoStationSnapshot['phase'] = 'falling', nowMs = config.atMs | |
| 74 | let startedAtMs: number | null = null, motionMs: number | null = null, fromM: SensorVector | null = null | |
| 75 | let cancelRequestedAtMs: number | null = null, stoppedAtMs: number | null = null | |
| 76 | let brake: { atMs: number, positionM: SensorVector, velocityMps: SensorVector, speedMps: number } | null = null | |
| 77 | let forceN: SensorVector = [0, 0, 0], impact: CargoStationSnapshot['impact'] = null | |
| 78 | const mechanical = (p: IntakeParcel) => .5 * p.massKg * p.velocityMps.reduce((sum, n) => sum + n * n, 0) - p.massKg * gravity * p.positionM[2] | |
| 79 | const initialJ = mechanical(parcel) | |
| 80 | let departedJ = 0, stationNetWorkJ = 0, positiveWorkJ = 0, negativeWorkJ = 0, planeDissipationJ = 0 | |
| 81 | function capturePlane(atMs: number) { | |
| 82 | const p = parcel!, before = [...p.velocityMps] as SensorVector | |
| 83 | const dissipationJ = .5 * p.massKg * before.reduce((sum, n) => sum + n * n, 0) | |
| 84 | p.positionM[2] = config.pickup.centerM[2] | |
| 85 | impact = { atMs, positionM: [...p.positionM], velocityBeforeMps: before, impulseNs: before.map(n => -p.massKg * n) as SensorVector, dissipationJ } | |
| 86 | planeDissipationJ += dissipationJ | |
| 87 | p.velocityMps = [0, 0, 0]; forceN = [0, 0, -p.massKg * gravity] | |
| 88 | phase = cancelRequestedAtMs !== null ? 'cancelled' : Math.hypot(p.positionM[0] - config.pickup.centerM[0], p.positionM[1] - config.pickup.centerM[1]) <= config.pickup.radiusM ? 'held' : 'missed' | |
| 89 | if (cancelRequestedAtMs !== null) stoppedAtMs = atMs | |
| 90 | } | |
| 91 | if (parcel.positionM[2] === config.pickup.centerM[2] && parcel.velocityMps[2] >= 0) capturePlane(nowMs) | |
| 92 | function retainWork(beforeJ: number) { | |
| 93 | const work = mechanical(parcel!) - beforeJ | |
| 94 | stationNetWorkJ += work; positiveWorkJ += Math.max(0, work); negativeWorkJ += Math.min(0, work) | |
| 95 | } | |
| 96 | function snapshot(): CargoStationSnapshot { | |
| 97 | const currentJ = parcel ? mechanical(parcel) : 0 | |
| 98 | return structuredClone({ evidence: 'modeled-cargo-station-v1', physicalActuationEnabled: false, phase, atMs: nowMs, startedAtMs, durationMs: motionMs, cancelRequestedAtMs, stoppedAtMs, parcel, | |
| 99 | ownedMassKg: parcel?.massKg ?? 0, forceN, impact, energy: { initialJ, currentJ, departedJ, stationNetWorkJ, positiveWorkJ, negativeWorkJ, planeDissipationJ, residualJ: currentJ + departedJ + planeDissipationJ - initialJ - stationNetWorkJ } }) | |
| 100 | } | |
| 101 | return { | |
| 102 | snapshot, | |
| 103 | start(...args: unknown[]): { outcome: 'started', atMs: number, durationMs: number } | { outcome: 'refused', reason: 'not-held' | 'deadline' } { | |
| 104 | if (args.length) throw new TypeError('cargo-station-arguments') | |
| 105 | if (phase !== 'held') return { outcome: 'refused', reason: 'not-held' } | |
| 106 | const duration = durationMs(distance(parcel!.positionM, config.destinationM), config.maxSpeedMps, config.maxAccelerationMps2) | |
| 107 | if (nowMs - config.atMs + duration > 180000) return { outcome: 'refused', reason: 'deadline' } | |
| 108 | fromM = [...parcel!.positionM]; motionMs = duration; startedAtMs = nowMs; phase = 'moving' | |
| 109 | return { outcome: 'started', atMs: nowMs, durationMs: duration } | |
| 110 | }, | |
| 111 | cancel(...args: unknown[]): { outcome: 'cancelling' | 'cancelled', atMs: number } | { outcome: 'refused', reason: 'already-cancelled' | 'departed' } { | |
| 112 | if (args.length) throw new TypeError('cargo-station-arguments') | |
| 113 | if (phase === 'departed') return { outcome: 'refused', reason: 'departed' } | |
| 114 | if (cancelRequestedAtMs !== null) return { outcome: 'refused', reason: 'already-cancelled' } | |
| 115 | cancelRequestedAtMs = nowMs | |
| 116 | const speedMps = Math.hypot(...parcel!.velocityMps) | |
| 117 | if (phase === 'moving' && speedMps > 0) { | |
| 118 | brake = { atMs: nowMs, positionM: [...parcel!.positionM], velocityMps: [...parcel!.velocityMps], speedMps } | |
| 119 | phase = 'braking' | |
| 120 | } | |
| 121 | else if (phase !== 'falling') { phase = 'cancelled'; stoppedAtMs = nowMs } | |
| 122 | return { outcome: phase === 'cancelled' ? 'cancelled' : 'cancelling', atMs: nowMs } | |
| 123 | }, | |
| 124 | step(atMs: number, ...extra: unknown[]) { | |
| 125 | if (extra.length || !Number.isSafeInteger(atMs) || atMs !== nowMs + 4 || atMs - config.atMs > 180000) throw new TypeError('cargo-station-clock') | |
| 126 | if (phase === 'falling') { | |
| 127 | const p = parcel!, down = p.positionM[2] - config.pickup.centerM[2], vz = p.velocityMps[2] | |
| 128 | const speedAtPlane = Math.sqrt(vz * vz - 2 * gravity * down) | |
| 129 | // Stable positive root for downward motion, including a point already at the plane. | |
| 130 | const hitSeconds = vz >= 0 ? (down === 0 ? 0 : -2 * down / (vz + speedAtPlane)) : (-vz + speedAtPlane) / gravity | |
| 131 | const seconds = Math.min(.004, hitSeconds) | |
| 132 | p.positionM = p.positionM.map((n, i) => n + p.velocityMps[i]! * seconds + (i === 2 ? .5 * gravity * seconds ** 2 : 0)) as SensorVector | |
| 133 | p.velocityMps[2] += gravity * seconds | |
| 134 | if (hitSeconds <= .004) capturePlane(nowMs + hitSeconds * 1000) | |
| 135 | } | |
| 136 | else if (phase === 'braking') { | |
| 137 | const b = brake!, p = parcel!, beforeJ = mechanical(p), stopSeconds = b.speedMps / config.maxAccelerationMps2 | |
| 138 | const seconds = Math.min((atMs - b.atMs) / 1000, stopSeconds), fraction = Math.max(0, 1 - seconds / stopSeconds) | |
| 139 | p.positionM = b.positionM.map((n, i) => n + b.velocityMps[i]! * (seconds - .5 * seconds ** 2 / stopSeconds)) as SensorVector | |
| 140 | p.velocityMps = b.velocityMps.map(n => n * fraction) as SensorVector | |
| 141 | forceN = b.velocityMps.map((n, i) => p.massKg * (-n / stopSeconds - (i === 2 ? gravity : 0))) as SensorVector | |
| 142 | retainWork(beforeJ) | |
| 143 | if (seconds === stopSeconds) { phase = 'cancelled'; stoppedAtMs = b.atMs + stopSeconds * 1000; forceN = [0, 0, -p.massKg * gravity] } | |
| 144 | } | |
| 145 | else if (phase === 'moving') { | |
| 146 | const beforeJ = mechanical(parcel!), seconds = motionMs! / 1000, u = Math.min(1, (atMs - startedAtMs!) / motionMs!) | |
| 147 | const s = 10 * u ** 3 - 15 * u ** 4 + 6 * u ** 5, ds = 30 * u ** 2 * (1 - u) ** 2, dds = 60 * u * (1 - u) * (1 - 2 * u) | |
| 148 | const delta = config.destinationM.map((n, i) => n - fromM![i]!) as SensorVector | |
| 149 | parcel!.positionM = u === 1 ? [...config.destinationM] : fromM!.map((n, i) => n + delta[i]! * s) as SensorVector | |
| 150 | parcel!.velocityMps = delta.map(n => n * ds / seconds) as SensorVector | |
| 151 | forceN = delta.map((n, i) => parcel!.massKg * (n * dds / seconds ** 2 - (i === 2 ? gravity : 0))) as SensorVector | |
| 152 | retainWork(beforeJ) | |
| 153 | if (u === 1) phase = 'ready' | |
| 154 | } | |
| 155 | nowMs = atMs | |
| 156 | return snapshot() | |
| 157 | }, | |
| 158 | takeReadyParcel(...args: unknown[]): { outcome: 'departed', atMs: number, parcel: IntakeParcel } | { outcome: 'refused', reason: 'not-ready' } { | |
| 159 | if (args.length) throw new TypeError('cargo-station-arguments') | |
| 160 | if (phase !== 'ready') return { outcome: 'refused', reason: 'not-ready' } | |
| 161 | const outgoing = structuredClone(parcel!) | |
| 162 | departedJ = mechanical(parcel!); parcel = null; phase = 'departed'; forceN = [0, 0, 0] | |
| 163 | return { outcome: 'departed', atMs: nowMs, parcel: outgoing } | |
| 164 | }, | |
| 165 | } | |
| 166 | } | |