also looking at this
feat: model one point-load intake with conserved momentum
#99
45 files
+3133
-169
| @@ -0,0 +1,167 @@ | ||
| 1 | import { expect, test } from 'bun:test' | |
| 2 | import { createReceivingRigidBody, createRigidBody } from '../../resources/functions/drone/controls/physics' | |
| 3 | import type { SensorVector } from '../../resources/functions/drone/sensors/types' | |
| 4 | import type { Quaternion } from '../../resources/functions/drone/estimation/frames' | |
| 5 | ||
| 6 | const plus = (a: SensorVector, b: SensorVector) => a.map((n, i) => n + b[i]!) as SensorVector | |
| 7 | const minus = (a: SensorVector, b: SensorVector) => a.map((n, i) => n - b[i]!) as SensorVector | |
| 8 | const scale = (a: SensorVector, n: number) => a.map(v => v * n) as SensorVector | |
| 9 | const cross = (a: SensorVector, b: SensorVector): SensorVector => [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]] | |
| 10 | const dot = (a: SensorVector, b: SensorVector) => a.reduce((sum, n, i) => sum + n * b[i]!, 0) | |
| 11 | function rotate([w, x, y, z]: Quaternion, v: SensorVector): SensorVector { | |
| 12 | return [[1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y)], [2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x)], [2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y)]].map(row => row.reduce((sum, n, i) => sum + n * v[i]!, 0)) as SensorVector | |
| 13 | } | |
| 14 | function closeVector(actual: SensorVector, expected: SensorVector) { expect(Math.max(...minus(actual, expected).map(Math.abs))).toBeLessThanOrEqual(2e-12) } | |
| 15 | function mechanical(state: ReturnType<ReturnType<typeof createReceivingRigidBody>['snapshot']>, parcel: { massKg: number, positionM: SensorVector, velocityMps: SensorVector }) { | |
| 16 | const origin: SensorVector = [7, -3, 5], inertia: SensorVector = [.03, .03, .05] | |
| 17 | const baseP = scale(state.velocityMps, 1.5), parcelP = scale(parcel.velocityMps, parcel.massKg) | |
| 18 | const spin = rotate(state.quaternionWxyz, state.angularRateRadS.map((n, i) => n * inertia[i]!) as SensorVector) | |
| 19 | return { momentum: plus(baseP, parcelP), angular: plus(spin, plus(cross(minus(state.positionM, origin), baseP), cross(minus(parcel.positionM, origin), parcelP))), | |
| 20 | kinetic: .75 * dot(state.velocityMps, state.velocityMps) + .5 * state.angularRateRadS.reduce((sum, n, i) => sum + inertia[i]! * n * n, 0) + .5 * parcel.massKg * dot(parcel.velocityMps, parcel.velocityMps), | |
| 21 | potential: -9.80665 * (1.5 * state.positionM[2] + parcel.massKg * parcel.positionM[2]) } | |
| 22 | } | |
| 23 | ||
| 24 | // Actual physics seam: proves point-load capture and state continuity, not a | |
| 25 | // mechanism acknowledgement, vehicle authority or fleet custody protocol. | |
| 26 | test('a receiving body captures one coincident point mass with the worked inelastic momentum result', () => { | |
| 27 | const body = createReceivingRigidBody({ positionM: [0, 0, -1], expectedPointLoad: { id: 'parcel-a', massKg: .1, forwardM: .08 } }) | |
| 28 | expect(body.loadSnapshot().properties.phase).toBe('awaiting') | |
| 29 | expect(body.loadSnapshot().properties.totalMassKg).toBe(1.5) | |
| 30 | const before = body.snapshot(), point = body.intakePoint() | |
| 31 | expect(point.positionM).toEqual([.08, 0, -1]) | |
| 32 | const event = body.attachParcel({ id: 'parcel-a', massKg: .1, positionM: point.positionM, velocityMps: [0, 1, 0] }) | |
| 33 | expect(event.outcome).toBe('attached') | |
| 34 | if (event.outcome !== 'attached') throw new Error('Expected actual modeled capture') | |
| 35 | expect(event.before.properties.phase).toBe('awaiting') | |
| 36 | expect(event.after.properties.phase).toBe('attached') | |
| 37 | expect(event.after.properties.totalMassKg).toBe(1.6) | |
| 38 | expect(event.after.properties.cgBodyM).toEqual([.005, 0, 0]) | |
| 39 | // Rational expected values allow the binary64 accumulation error, not a model change. | |
| 40 | for (const [i, expected] of [.03, .0306, .0506].entries()) expect(event.after.properties.inertiaDiagonalKgM2[i]!).toBeCloseTo(expected, 14) | |
| 41 | expect(body.snapshot().angularRateRadS[2]).toBeCloseTo(.1482213438735178, 13) | |
| 42 | expect(body.snapshot().velocityMps[1]).toBeCloseTo(.06175889328063241, 13) | |
| 43 | expect(event.energy.kineticBeforeJ).toBeCloseTo(.05, 14) | |
| 44 | expect(event.energy.kineticAfterJ).toBeCloseTo(.003680830039525692, 14) | |
| 45 | expect(event.energy.dissipatedJ).toBeCloseTo(.04631916996047431, 14) | |
| 46 | expect(body.snapshot().positionM).toEqual(before.positionM) | |
| 47 | expect(body.snapshot().quaternionWxyz).toEqual(before.quaternionWxyz) | |
| 48 | expect(body.snapshot().motorsN).toEqual(before.motorsN) | |
| 49 | expect(body.attachParcel({ id: 'parcel-a', massKg: .1, positionM: point.positionM, velocityMps: [0, 1, 0] })).toEqual({ outcome: 'refused', reason: 'intake-consumed' }) | |
| 50 | const separated = body.separateCarriedLoad() | |
| 51 | expect(separated.outcome).toBe('separated') | |
| 52 | if (separated.outcome !== 'separated') throw new Error('Expected actual subsequent release') | |
| 53 | expect(mechanical(body.snapshot(), separated.parcel).kinetic).toBeCloseTo(.003680830039525692, 14) | |
| 54 | }) | |
| 55 | ||
| 56 | test('rotated captures conserve world momentum about an independent origin and dissipate relative motion', () => { | |
| 57 | for (const massKg of [.01, .1, .25]) for (const forwardM of [-.1, 0, .1]) { | |
| 58 | const body = createReceivingRigidBody({ positionM: [1, -2, -1], motorsN: [2, 2, 2, 2], expectedPointLoad: { id: 'parcel-a', massKg, forwardM } }) | |
| 59 | for (let i = 0; i < 40; i++) body.step([3, 2.9, 3.1, 3], .004, { windMps: [.2, -.1, 0], torqueNm: [.02, -.01, .03] }) | |
| 60 | const before = body.snapshot(), point = body.intakePoint() | |
| 61 | const parcel = { id: 'parcel-a', massKg, ...point, velocityMps: plus(point.velocityMps, [.2, -.3, .1]) } | |
| 62 | closeVector(point.positionM, plus(before.positionM, rotate(before.quaternionWxyz, [forwardM, 0, 0]))) | |
| 63 | const initial = mechanical(before, parcel), event = body.attachParcel(parcel) | |
| 64 | expect(event.outcome).toBe('attached') | |
| 65 | if (event.outcome !== 'attached') throw new Error('Expected rotated capture') | |
| 66 | const after = body.snapshot(), afterPoint = { massKg, positionM: plus(after.positionM, rotate(after.quaternionWxyz, [forwardM, 0, 0])), velocityMps: plus(after.velocityMps, rotate(after.quaternionWxyz, cross(after.angularRateRadS, [forwardM, 0, 0]))) } | |
| 67 | const final = mechanical(after, afterPoint) | |
| 68 | closeVector(final.momentum, initial.momentum) | |
| 69 | closeVector(final.angular, initial.angular) | |
| 70 | expect(Math.abs(final.potential - initial.potential)).toBeLessThanOrEqual(2e-12) | |
| 71 | expect(initial.kinetic - final.kinetic).toBeGreaterThan(0) | |
| 72 | expect(event.energy.kineticBeforeJ).toBeCloseTo(initial.kinetic, 12) | |
| 73 | expect(event.energy.kineticAfterJ).toBeCloseTo(final.kinetic, 12) | |
| 74 | expect(event.energy.dissipatedJ).toBeCloseTo(initial.kinetic - final.kinetic, 12) | |
| 75 | expect(event.energy.potentialBeforeJ).toBeCloseTo(initial.potential, 12) | |
| 76 | expect(event.energy.potentialAfterJ).toBeCloseTo(final.potential, 12) | |
| 77 | expect(after.positionM).toEqual(before.positionM) | |
| 78 | expect(after.quaternionWxyz).toEqual(before.quaternionWxyz) | |
| 79 | expect(after.motorsN).toEqual(before.motorsN) | |
| 80 | event.after.bodyVelocityMps[0] = 999; event.parcel.positionM[0] = 999 | |
| 81 | expect(body.snapshot()).toEqual(after) | |
| 82 | } | |
| 83 | }) | |
| 84 | ||
| 85 | test('co-moving intake preserves body motion through a later separation', () => { | |
| 86 | const body = createReceivingRigidBody({ positionM: [0, 0, -1], expectedPointLoad: { id: 'parcel-a', massKg: .1, forwardM: -.08 } }) | |
| 87 | for (let i = 0; i < 30; i++) body.step([4, 3.8, 4.1, 4], .004, { torqueNm: [.01, -.02, .03] }) | |
| 88 | const before = body.snapshot(), parcel = { id: 'parcel-a', massKg: .1, ...body.intakePoint() }, event = body.attachParcel(parcel) | |
| 89 | expect(event.outcome).toBe('attached') | |
| 90 | if (event.outcome !== 'attached') throw new Error('Expected co-moving capture') | |
| 91 | closeVector(body.snapshot().velocityMps, before.velocityMps) | |
| 92 | closeVector(body.snapshot().angularRateRadS, before.angularRateRadS) | |
| 93 | expect(Math.abs(event.energy.dissipatedJ)).toBeLessThanOrEqual(2e-12) | |
| 94 | const separated = body.separateCarriedLoad() | |
| 95 | expect(separated.outcome).toBe('separated') | |
| 96 | if (separated.outcome !== 'separated') throw new Error('Expected subsequent separation') | |
| 97 | closeVector(separated.parcel.positionM, parcel.positionM) | |
| 98 | closeVector(separated.parcel.velocityMps, parcel.velocityMps) | |
| 99 | closeVector(body.snapshot().velocityMps, before.velocityMps) | |
| 100 | closeVector(body.snapshot().angularRateRadS, before.angularRateRadS) | |
| 101 | expect(body.loadSnapshot().properties.phase).toBe('separated') | |
| 102 | expect(body.loadSnapshot().properties.totalMassKg).toBe(1.5) | |
| 103 | expect(body.attachParcel(parcel)).toEqual({ outcome: 'refused', reason: 'intake-consumed' }) | |
| 104 | expect(body.separateCarriedLoad()).toEqual({ outcome: 'refused', reason: 'already-separated' }) | |
| 105 | }) | |
| 106 | ||
| 107 | test('the actual subsequent acceleration uses the received mass and the empty receiver has no phantom load', () => { | |
| 108 | for (const attached of [false, true]) { | |
| 109 | const body = createReceivingRigidBody({ positionM: [0, 0, -1], expectedPointLoad: { id: 'parcel-a', massKg: .1, forwardM: 0 } }) | |
| 110 | if (attached) expect(body.attachParcel({ id: 'parcel-a', massKg: .1, ...body.intakePoint() }).outcome).toBe('attached') | |
| 111 | const state = body.step([4, 4, 4, 4], .004), mass = attached ? 1.6 : 1.5 | |
| 112 | const thrust = 16 * (1 - Math.exp(-.004 / .04)) | |
| 113 | expect(state.accelerationMps2[2]).toBeCloseTo(9.80665 - thrust / mass, 12) | |
| 114 | expect(body.loadSnapshot().properties.totalMassKg).toBe(mass) | |
| 115 | } | |
| 116 | }) | |
| 117 | ||
| 118 | test('intake refuses foreign or noncoincident parcels without consuming or moving the body', () => { | |
| 119 | const body = createReceivingRigidBody({ positionM: [0, 0, -1], expectedPointLoad: { id: 'parcel-a', massKg: .1, forwardM: .08 } }) | |
| 120 | const before = body.snapshot(), point = { id: 'parcel-a', massKg: .1, ...body.intakePoint() } | |
| 121 | expect(body.separateCarriedLoad()).toEqual({ outcome: 'refused', reason: 'awaiting-load' }) | |
| 122 | for (const candidate of [{ ...point, id: 'parcel-b' }, { ...point, massKg: .2 }]) expect(body.attachParcel(candidate)).toEqual({ outcome: 'refused', reason: 'unexpected-parcel' }) | |
| 123 | expect(body.attachParcel({ ...point, positionM: [.081, 0, -1] })).toEqual({ outcome: 'refused', reason: 'mount-not-coincident' }) | |
| 124 | expect(body.snapshot()).toEqual(before) | |
| 125 | expect(body.loadSnapshot().properties.phase).toBe('awaiting') | |
| 126 | expect(body.attachParcel(point).outcome).toBe('attached') | |
| 127 | }) | |
| 128 | ||
| 129 | test('intake captures exact own data and malformed input cannot evaluate getters or mutate state', () => { | |
| 130 | const body = createReceivingRigidBody({ positionM: [0, 0, -1], expectedPointLoad: { id: 'parcel-a', massKg: .1, forwardM: 0 } }) | |
| 131 | const before = body.snapshot(), point = { id: 'parcel-a', massKg: .1, ...body.intakePoint() } | |
| 132 | let called = false | |
| 133 | const accessor = { ...point, get velocityMps() { called = true; return [0, 0, 0] } } | |
| 134 | const tuple = [0, 0, 0]; Object.defineProperty(tuple, '0', { enumerable: true, get() { called = true; return 0 } }) | |
| 135 | for (const value of [null, { ...point, extra: true }, { ...point, massKg: NaN }, { ...point, velocityMps: [0, Infinity, 0] }, accessor, { ...point, velocityMps: tuple }, { ...point, positionM: [, 0, -1] }]) expect(() => body.attachParcel(value)).toThrow('cargo-intake-parcel') | |
| 136 | const proxy = new Proxy(point, { ownKeys() { throw new Error('private-caller-sentinel') } }) | |
| 137 | expect(() => body.attachParcel(proxy)).toThrow(/^cargo-intake-parcel$/) | |
| 138 | expect(() => Reflect.apply(body.attachParcel, body, [point, true])).toThrow('cargo-intake-parcel') | |
| 139 | expect(called).toBe(false) | |
| 140 | expect(body.snapshot()).toEqual(before) | |
| 141 | expect(body.loadSnapshot().properties.phase).toBe('awaiting') | |
| 142 | expect(() => createReceivingRigidBody({ expectedPointLoad: { get id() { called = true; return 'parcel-a' }, massKg: .1, forwardM: 0 } })).toThrow() | |
| 143 | expect(called).toBe(false) | |
| 144 | expect(body.attachParcel(point).outcome).toBe('attached') | |
| 145 | }) | |
| 146 | ||
| 147 | test('joint contact is current physical state and the planned identity is captured once', () => { | |
| 148 | const expected = { id: 'parcel-a', massKg: .1, forwardM: .08 } | |
| 149 | const body = createReceivingRigidBody({ positionM: [0, 0, -1], expectedPointLoad: expected }) | |
| 150 | const stale = { id: 'parcel-a', massKg: .1, ...body.intakePoint() } | |
| 151 | expected.id = 'parcel-b'; expected.massKg = .2; expected.forwardM = -.08 | |
| 152 | body.step([0, 0, 0, 0], .004) | |
| 153 | expect(body.attachParcel(stale)).toEqual({ outcome: 'refused', reason: 'mount-not-coincident' }) | |
| 154 | expect(body.loadSnapshot().properties.phase).toBe('awaiting') | |
| 155 | const current = { id: 'parcel-a', massKg: .1, ...body.intakePoint() } | |
| 156 | expect(body.attachParcel(current).outcome).toBe('attached') | |
| 157 | expect(body.loadSnapshot().properties.forwardM).toBe(.08) | |
| 158 | }) | |
| 159 | ||
| 160 | test('legacy body factories keep their original methods and attached/separated phases', () => { | |
| 161 | expect(Object.keys(createRigidBody())).toEqual(['snapshot', 'step']) | |
| 162 | const legacy = createRigidBody({ carriedPointLoad: { id: 'parcel-a', massKg: .1, forwardM: .08 } }) | |
| 163 | expect(Object.keys(legacy)).toEqual(['snapshot', 'step', 'loadSnapshot', 'separateCarriedLoad']) | |
| 164 | expect(legacy.loadSnapshot().properties.phase).toBe('attached') | |
| 165 | expect(legacy.separateCarriedLoad().outcome).toBe('separated') | |
| 166 | expect(legacy.loadSnapshot().properties.phase).toBe('separated') | |
| 167 | }) | |