From 1586e9d2ab51e34d06925839046ec9208b4f4985 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D1=8F=20=D0=90=D1=80=D1=85=D0=B8=D0=BF?= =?UTF-8?q?=D0=BE=D0=B2?= Date: Sun, 2 Aug 2026 05:52:28 +0000 Subject: [PATCH] feat: add stable physical conveyor foundation Co-authored-by: Cursor --- .../ThreeD/PhysicalPlaybackItemPhysics.tsx | 311 ++++++++++++------ src/components/ThreeD/SorterPhysics.tsx | 108 +++--- src/domain/physicsConfigHash.ts | 15 +- src/domain/physicsTimestep.ts | 5 +- src/domain/productPhysicsProfiles.test.ts | 202 ++++++++++++ src/domain/productPhysicsProfiles.ts | 283 ++++++++++++++++ 6 files changed, 776 insertions(+), 148 deletions(-) create mode 100644 src/domain/productPhysicsProfiles.test.ts create mode 100644 src/domain/productPhysicsProfiles.ts diff --git a/src/components/ThreeD/PhysicalPlaybackItemPhysics.tsx b/src/components/ThreeD/PhysicalPlaybackItemPhysics.tsx index 5708ae2..5270552 100644 --- a/src/components/ThreeD/PhysicalPlaybackItemPhysics.tsx +++ b/src/components/ThreeD/PhysicalPlaybackItemPhysics.tsx @@ -1,14 +1,12 @@ /** - * Stage 2 — item with hybrid kinematic/dynamic authority (Rapier). + * Product rigid body: dynamic physical conveyor foundation + junction handoff. * - * Authority flow (see docs/stage2_real_sorter/physics-architecture.md): - * 1. kinematicPosition — follows getPhysicalItemPose exactly (domain truth); - * 2. at getDropHandoffTimeMs → dynamic with deterministic initial velocity - * (B: belt edge carry-over; C/D: pusher impulse, scaled per SKU profile); - * 3. gravity/collision/friction/restitution/angular velocity govern the drop; - * 4. on sleep (or controlled 4.5 s timeout) the final position is verified - * against the DOMAIN-decided receiver volume and the body is frozen - * (kinematic) — no drift, clean replay, no teleportation at any point. + * Lifecycle: + * PREPARING → PHYSICAL_CONVEYOR (dynamic + belt force) → JUNCTION + * (existing drop/settle authority at getDropHandoffTimeMs) → FROZEN. + * + * Temporary handoff: belt drive stops at JUNCTION_ENTRY_S / handoffMs; current + * classifier+diverter routing remains responsible for basket assignment. */ import { memo, useCallback, useEffect, useMemo, useRef, useState } from 'react'; import * as THREE from 'three'; @@ -18,11 +16,23 @@ import { CuboidCollider, CapsuleCollider, CylinderCollider, + useBeforePhysicsStep, type RapierRigidBody, } from '@react-three/rapier'; import { RigidBodyType } from '@dimforge/rapier3d-compat'; import { getPhysicalItemPose, getDropHandoffTimeMs } from '../../domain/physicalItemMotion'; -import { getVisualPhysicsProfile } from '../../domain/visualPhysicsProfiles'; +import { + getProductPhysicsProfile, + colliderHalfHeight, + spawnCenterY, + isSupportedByBelt, + computeBeltDriveForce, + isInvalidProductState, + recordInvalidProductState, + JUNCTION_ENTRY_S, + BELT_SPEED_MPS, + type ProductPhysicsPhase, +} from '../../domain/productPhysicsProfiles'; import { resolveItem } from '../../data/resolveItem'; import { classifyItem } from '../../domain/classifier'; import { receiverContains } from '../../domain/receiverVolumes'; @@ -32,23 +42,21 @@ import { recordDropResult, physicsSimClock } from './SorterPhysics'; import { getModelAsset } from '../../data/modelAssets'; import { isProductAssetReady } from './RealItemModel'; -type Authority = 'kinematic' | 'dynamic' | 'frozen'; +type Authority = 'preparing' | 'physical_conveyor' | 'junction' | 'frozen' | 'fault_kinematic'; -/** Controlled settle budget after handoff — in PHYSICS-simulated seconds, - * not domain ms: under render lag domain time races ahead of the stepper, - * and a domain-ms budget would freeze items mid-flight (§14.2). */ const SETTLE_BUDGET_SEC = 4.5; +const TELEMETRY_INTERVAL_MS = 200; -function colliderDensity(profile: ReturnType): number { - if (profile.collider === 'cuboid' && profile.cuboidHalfExtents) { - const [hx, hy, hz] = profile.cuboidHalfExtents; - return profile.approximateMassKg / (8 * hx * hy * hz); +function colliderDensity(profile: ReturnType): number { + const c = profile.collider; + if (c.type === 'cuboid') { + const [hx, hy, hz] = c.halfExtents; + return profile.massKg / (8 * hx * hy * hz); } - const [r, hh] = profile.capsule ?? [0.05, 0.1]; - const volume = profile.collider === 'capsule' - ? Math.PI * r * r * (2 * hh + (4 / 3) * r) - : Math.PI * r * r * 2 * hh; - return profile.approximateMassKg / volume; + const volume = c.type === 'capsule' + ? Math.PI * c.radius * c.radius * (2 * c.halfHeight + (4 / 3) * c.radius) + : Math.PI * c.radius * c.radius * 2 * c.halfHeight; + return profile.massKg / Math.max(volume, 1e-6); } export const PhysicalPlaybackItemPhysics = memo(function PhysicalPlaybackItemPhysics({ @@ -70,18 +78,24 @@ export const PhysicalPlaybackItemPhysics = memo(function PhysicalPlaybackItemPhy const classification = useMemo(() => classifyItem(itemData), [itemData]); const category = classification.category as 'B' | 'C' | 'D'; const itemId = itemData.id.replace('-LC', ''); - const profile = getVisualPhysicsProfile(itemId); + const profile = getProductPhysicsProfile(itemId); + const halfH = colliderHalfHeight(profile); const handoffMs = getDropHandoffTimeMs(classification.category, caseData.faultType); + const isFault = Boolean(caseData.faultType); const bodyRef = useRef(null); - const traceEnabled = useRef( - typeof window !== 'undefined' - && new URLSearchParams(window.location.search).get('trace') === '1', - ); - const authority = useRef('kinematic'); + const authority = useRef(isFault ? 'fault_kinematic' : 'preparing'); + const phaseRef = useRef('preparing'); const frozenPose = useRef<{ p: [number, number, number]; q: THREE.Quaternion } | null>(null); const handedOffAtSimSec = useRef(null); const verified = useRef(false); + const activated = useRef(false); + const invalidLogged = useRef(false); + const lastTelemetryMs = useRef(0); + const forceScratch = useRef({ x: 0, y: 0, z: 0 }); + const elapsedMsRef = useRef(elapsedMs); + elapsedMsRef.current = elapsedMs; + const asset = getModelAsset(itemId); const needsRealAsset = Boolean(asset?.defaultRealAsset && asset?.runtimePath); const [spawned, setSpawned] = useState( @@ -101,6 +115,24 @@ export const PhysicalPlaybackItemPhysics = memo(function PhysicalPlaybackItemPhy jitter, }); + const spawnPose = useMemo(() => { + const p = getPhysicalItemPose({ + caseId: caseData.id, + slotIndex, + dimensionsMm: itemData.dimensionsMm, + targetCategory: classification.category, + elapsedMs: 0, + faultType: caseData.faultType, + jitter, + }); + const y = spawnCenterY(profile); + return { + position: [p.position[0], y, p.position[2]] as [number, number, number], + rotation: p.rotation, + }; + // eslint-disable-next-line react-hooks/exhaustive-deps + }, [caseData.id, profile.productId]); + const handoffPose = useMemo(() => { if (handoffMs == null) return null; return getPhysicalItemPose({ @@ -115,16 +147,14 @@ export const PhysicalPlaybackItemPhysics = memo(function PhysicalPlaybackItemPhy // eslint-disable-next-line react-hooks/exhaustive-deps }, [handoffMs, caseData.id]); - // Reset authority whenever a new case mounts this body. The Rapier body is - // reused across cases, so a case that ended while still DYNAMIC (settle - // budget cut short under render lag) must be forced back to kinematic — - // otherwise setNextKinematicTranslation is a no-op and the next case's item - // is stuck invisibly mid-scene. useEffect(() => { - authority.current = 'kinematic'; + authority.current = isFault ? 'fault_kinematic' : 'preparing'; + phaseRef.current = 'preparing'; frozenPose.current = null; handedOffAtSimSec.current = null; verified.current = false; + activated.current = false; + invalidLogged.current = false; const ready = !needsRealAsset || isProductAssetReady(asset?.runtimePath); setSpawned(ready); const body = bodyRef.current; @@ -132,24 +162,136 @@ export const PhysicalPlaybackItemPhysics = memo(function PhysicalPlaybackItemPhy body.setBodyType(RigidBodyType.KinematicPositionBased, false); body.setLinvel({ x: 0, y: 0, z: 0 }, true); body.setAngvel({ x: 0, y: 0, z: 0 }, true); - const p = pose.position; + const p = spawnPose.position; body.setTranslation({ x: p[0], y: p[1], z: p[2] }, true); - const e = new THREE.Euler(pose.rotation[0], pose.rotation[1], pose.rotation[2]); + const e = new THREE.Euler(spawnPose.rotation[0], spawnPose.rotation[1], spawnPose.rotation[2]); const q = new THREE.Quaternion().setFromEuler(e); body.setRotation({ x: q.x, y: q.y, z: q.z, w: q.w }, true); } - // eslint-disable-next-line react-hooks/exhaustive-deps -- reset on case id only + // eslint-disable-next-line react-hooks/exhaustive-deps }, [caseData.id]); + const activateDynamic = useCallback((body: RapierRigidBody) => { + const p = spawnPose.position; + body.setTranslation({ x: p[0], y: p[1], z: p[2] }, true); + const e = new THREE.Euler(spawnPose.rotation[0], spawnPose.rotation[1], spawnPose.rotation[2]); + const q = new THREE.Quaternion().setFromEuler(e); + body.setRotation({ x: q.x, y: q.y, z: q.z, w: q.w }, true); + body.setLinvel({ x: 0, y: 0, z: 0 }, true); + body.setAngvel({ x: 0, y: 0, z: 0 }, true); + body.setBodyType(RigidBodyType.Dynamic, true); + body.wakeUp(); + activated.current = true; + authority.current = 'physical_conveyor'; + phaseRef.current = 'physical_conveyor'; + }, [spawnPose]); + + useBeforePhysicsStep(() => { + const body = bodyRef.current; + if (!body || !spawned) return; + if (authority.current !== 'physical_conveyor') { + body.resetForces(true); + return; + } + + const t = body.translation(); + const lv = body.linvel(); + const av = body.angvel(); + const position: [number, number, number] = [t.x, t.y, t.z]; + const linearVelocity: [number, number, number] = [lv.x, lv.y, lv.z]; + const angularVelocity: [number, number, number] = [av.x, av.y, av.z]; + + if (isInvalidProductState({ position, linearVelocity, angularVelocity })) { + if (!invalidLogged.current) { + recordInvalidProductState(); + invalidLogged.current = true; + if (import.meta.env.DEV) { + console.warn('[physics] invalid product state', itemId, position); + } + } + body.resetForces(true); + body.setLinvel({ x: 0, y: 0, z: 0 }, true); + body.setAngvel({ x: 0, y: 0, z: 0 }, true); + body.setBodyType(RigidBodyType.KinematicPositionBased, false); + authority.current = 'frozen'; + phaseRef.current = 'invalid'; + return; + } + + // Temporary junction handoff: stop belt drive; keep dynamic body for drop. + if ((handoffMs != null && elapsedMsRef.current >= handoffMs) || t.x >= JUNCTION_ENTRY_S) { + body.resetForces(true); + if (authority.current === 'physical_conveyor') { + if (handoffPose) { + // Align only once at handoff — no per-frame kinematic competition. + const hp = body.translation(); + // Prefer live physical X/Z; keep Y from body (no teleport). + void hp; + } + body.setLinvel({ x: Math.max(lv.x, BELT_SPEED_MPS * 0.85), y: lv.y, z: lv.z }, true); + authority.current = 'junction'; + phaseRef.current = 'junction'; + handedOffAtSimSec.current = physicsSimClock.simSec; + } + return; + } + + const supported = isSupportedByBelt({ + position, + halfHeight: halfH, + phase: 'physical_conveyor', + linearVelY: lv.y, + }); + + body.resetForces(true); + if (!supported) return; + + const sample = computeBeltDriveForce({ + massKg: profile.massKg, + linearVelocity, + maxBeltAccelerationMps2: profile.maxBeltAccelerationMps2, + applyLateralCorrection: t.x < JUNCTION_ENTRY_S, + }); + + forceScratch.current.x = sample.force[0] + sample.lateralCorrection[0]; + forceScratch.current.y = sample.force[1] + sample.lateralCorrection[1]; + forceScratch.current.z = sample.force[2] + sample.lateralCorrection[2]; + body.addForce(forceScratch.current, true); + + if (import.meta.env.DEV && typeof window !== 'undefined') { + const now = performance.now(); + if (now - lastTelemetryMs.current >= TELEMETRY_INTERVAL_MS) { + lastTelemetryMs.current = now; + window.__CONVEYOR_PHYSICS_DEBUG__ = { + productId: itemId, + profileId: profile.productId, + physicsPhase: phaseRef.current, + supportedByBelt: supported, + currentDownstreamSpeed: sample.currentDownstreamSpeed, + targetSpeed: BELT_SPEED_MPS, + appliedAcceleration: sample.appliedAcceleration, + appliedForceMagnitude: Math.hypot( + forceScratch.current.x, + forceScratch.current.y, + forceScratch.current.z, + ), + lateralSpeed: lv.z, + angularSpeed: Math.hypot(av.x, av.y, av.z), + bodyPosition: position, + invalidState: false, + }; + } + } + }); + useFrame(() => { const body = bodyRef.current; if (!body) return; - // PREPARING: hold at spawn pose, zero velocity, keep invisible until visual ready. if (!spawned) { - const p = pose.position; + const p = spawnPose.position; body.setNextKinematicTranslation({ x: p[0], y: p[1], z: p[2] }); - const e = new THREE.Euler(pose.rotation[0], pose.rotation[1], pose.rotation[2]); + const e = new THREE.Euler(spawnPose.rotation[0], spawnPose.rotation[1], spawnPose.rotation[2]); const q = new THREE.Quaternion().setFromEuler(e); body.setNextKinematicRotation({ x: q.x, y: q.y, z: q.z, w: q.w }); body.setLinvel({ x: 0, y: 0, z: 0 }, true); @@ -157,34 +299,12 @@ export const PhysicalPlaybackItemPhysics = memo(function PhysicalPlaybackItemPhy return; } - if (authority.current === 'kinematic') { - // Physics handoff at pusher contact / belt edge — never for fault cases. - // MUST be checked BEFORE the kinematic drive: under render lag a single - // frame can jump several seconds past handoffMs, and pose(elapsedMs) is - // then already deep inside the receiver. Applying setNextKinematic* - // from that pose in the same frame as the dynamic switch teleports the - // body (forbidden) — the next-step kinematic target still applies. - if (handoffMs != null && handoffPose && elapsedMs >= handoffMs) { - const hp = handoffPose.position; - body.setTranslation({ x: hp[0], y: hp[1], z: hp[2] }, true); - const he = new THREE.Euler(handoffPose.rotation[0], handoffPose.rotation[1], handoffPose.rotation[2]); - const hq = new THREE.Quaternion().setFromEuler(he); - body.setRotation({ x: hq.x, y: hq.y, z: hq.z, w: hq.w }, true); - body.setBodyType(RigidBodyType.Dynamic, true); - // Deterministic initial velocity: belt carry-over only — for C/D the - // Z motion comes from the kinematic paddle CONTACT (Stage 2B §13). - body.setLinvel({ x: 1.0, y: 0, z: 0 }, true); - if (profile.canRoll && category === 'B') { - body.setAngvel({ x: 2.0, y: 0.4, z: 0 }, true); - } else { - body.setAngvel({ x: 0, y: 0, z: 0 }, true); - } - authority.current = 'dynamic'; - handedOffAtSimSec.current = physicsSimClock.simSec; - return; - } + if (authority.current === 'preparing' && !activated.current && !isFault) { + activateDynamic(body); + return; + } - // Kinematic drive: domain pose is truth (belt travel, inspection dwell). + if (authority.current === 'fault_kinematic') { const p = pose.position; const e = new THREE.Euler(pose.rotation[0], pose.rotation[1], pose.rotation[2]); const q = new THREE.Quaternion().setFromEuler(e); @@ -193,25 +313,13 @@ export const PhysicalPlaybackItemPhysics = memo(function PhysicalPlaybackItemPhy return; } - if (authority.current === 'dynamic') { + if (authority.current === 'junction') { const slept = body.isSleeping(); const lv = body.linvel(); const av = body.angvel(); const slow = Math.hypot(lv.x, lv.y, lv.z) < 0.2 && Math.hypot(av.x, av.y, av.z) < 1.0; - if (traceEnabled.current) { - const t = body.translation(); - const w = window as unknown as { __ITEM_TRACE?: unknown[] }; - w.__ITEM_TRACE = w.__ITEM_TRACE ?? []; - const arr = w.__ITEM_TRACE as { e: number; x: number; y: number; z: number; lv: number; slept: boolean }[]; - if (arr.length === 0 || arr[arr.length - 1].e < elapsedMs - 200) { - arr.push({ e: Math.round(elapsedMs), x: +t.x.toFixed(3), y: +t.y.toFixed(3), z: +t.z.toFixed(3), lv: +Math.hypot(lv.x, lv.y, lv.z).toFixed(2), slept }); - if (arr.length > 120) arr.shift(); - } - } const timedOut = handedOffAtSimSec.current != null && physicsSimClock.simSec - handedOffAtSimSec.current > SETTLE_BUDGET_SEC; - // §14.2: freeze only after actual rest (sleep) or a timeout WITH low - // velocities — never freeze a body that is still moving/flying. if ((slept || (timedOut && slow)) && !verified.current) { verified.current = true; const t = body.translation(); @@ -231,12 +339,12 @@ export const PhysicalPlaybackItemPhysics = memo(function PhysicalPlaybackItemPhy body.setLinvel({ x: 0, y: 0, z: 0 }, false); body.setAngvel({ x: 0, y: 0, z: 0 }, false); authority.current = 'frozen'; + phaseRef.current = 'settled'; } return; } - // frozen: hold the verified rest pose (no drift across replays). - if (frozenPose.current) { + if (authority.current === 'frozen' && frozenPose.current) { const { p, q } = frozenPose.current; body.setNextKinematicTranslation({ x: p[0], y: p[1], z: p[2] }); body.setNextKinematicRotation({ x: q.x, y: q.y, z: q.z, w: q.w }); @@ -246,38 +354,45 @@ export const PhysicalPlaybackItemPhysics = memo(function PhysicalPlaybackItemPhy if (elapsedMs < 0) return null; const density = colliderDensity(profile); - // CCD for small/fast items (pen) and thin items (plate) — mirrors the sim. - const ccd = profile.approximateMassKg < 0.05 || itemData.dimensionsMm.height < 50; + const enabledRotations: [boolean, boolean, boolean] = [ + !profile.lockRotationX, + !profile.lockRotationY, + !profile.lockRotationZ, + ]; return ( - {/* Colliders only after visual ready — avoids stale/orphan contact. */} - {spawned && profile.collider === 'cuboid' && profile.cuboidHalfExtents && ( - + {spawned && profile.collider.type === 'cuboid' && ( + )} - {spawned && profile.collider === 'capsule' && profile.capsule && ( + {spawned && profile.collider.type === 'capsule' && ( )} - {spawned && profile.collider === 'cylinder' && profile.capsule && ( + {spawned && profile.collider.type === 'cylinder' && ( )} diff --git a/src/components/ThreeD/SorterPhysics.tsx b/src/components/ThreeD/SorterPhysics.tsx index e2be045..db0dd11 100644 --- a/src/components/ThreeD/SorterPhysics.tsx +++ b/src/components/ThreeD/SorterPhysics.tsx @@ -1,26 +1,26 @@ /** - * Stage 2 — physics world for the sorter drop segment. + * Physics world for the sorter — single step authority via @react-three/rapier. * - * Hybrid authority (docs/stage2_real_sorter/physics-architecture.md): - * - items on the belt are KINEMATIC (domain pose is truth); - * - at the drop handoff (pusher contact / belt edge) the body switches to - * DYNAMIC with deterministic initial velocity; - * - static colliders mirror the visible chute / receiver geometry - * (documented hidden colliders, same dimensions as the visuals). - * - * Determinism: fixed dt = 1/60, max 4 substeps/frame, no unseeded randomness. - * Physics freezes when the domain clock is paused (documented simulation - * assumption — belt, gate and items halt together; EMERGENCY_STOP creates no - * new impulses). - * - * Stage 2E: Rapier step timing via PhysicsPerfSampler (?perf=1 | ?physicsPerf=1). - * Render time is NOT included in physics p95. + * Physics is mounted `paused` so FrameStepper does not auto-step. RapierStepper + * is the only caller of context.step(), which runs before/after hooks, fixed + * substeps, and mesh sync. Playback speed scales the clamped frame delta. */ import { useRef, type ReactNode } from 'react'; import { useFrame } from '@react-three/fiber'; -import { Physics, RigidBody, CuboidCollider, useRapier } from '@react-three/rapier'; +import { + Physics, + RigidBody, + CuboidCollider, + useRapier, + useAfterPhysicsStep, +} from '@react-three/rapier'; import { getStaticColliders } from '../../domain/physicsWorldLayout'; -import { PHYSICS_TIMESTEP_SEC } from '../../domain/physicsTimestep'; +import { + PHYSICS_TIMESTEP_SEC, + PHYSICS_MAX_SUBSTEPS, + MAX_FRAME_DELTA_SEC, + PHYSICS_GRAVITY, +} from '../../domain/physicsTimestep'; import { PhysicsPerfSampler, isPhysicsPerfQueryEnabled, @@ -28,7 +28,7 @@ import { } from '../../domain/physicsPerf'; export const PHYSICS_DT = PHYSICS_TIMESTEP_SEC; -export const PHYSICS_MAX_SUBSTEPS = 4; +export { PHYSICS_MAX_SUBSTEPS }; const MAX_SUBSTEPS = PHYSICS_MAX_SUBSTEPS; /** @@ -59,6 +59,7 @@ declare global { __DROP_RESULTS?: DropResult[]; __PHYSICS_PERF__?: PhysicsPerfSnapshot; __PHYSICS_PERF_RESET__?: () => void; + __CONVEYOR_PHYSICS_DEBUG__?: Record; } } @@ -73,7 +74,6 @@ function readWorldMeta(world: { colliders?: { len: () => number }; }): { activeBodies: number; sleepingBodies: number; colliders: number; contactPairs: number } { try { - // @react-three/rapier wraps Rapier world; body counts via forEach when available const w = world as unknown as { forEachRigidBody?: (cb: (b: { isSleeping: () => boolean; numColliders: () => number }) => void) => void; bodies?: { len: () => number }; @@ -102,14 +102,24 @@ function readWorldMeta(world: { } } -/** Steps the Rapier world with a fixed dt, scaled by domain playback speed. */ +/** + * Sole physics-step authority: calls Rapier context.step (not raw world.step) + * so before/after hooks and rigid-body mesh sync run once per substep batch. + */ +function PhysicsSimClock() { + useAfterPhysicsStep(() => { + physicsSimClock.simSec += PHYSICS_DT; + }); + return null; +} + function RapierStepper({ running, speed }: { running: boolean; speed: number }) { - const { world } = useRapier(); - const accumulator = useRef(0); + const { step, world } = useRapier(); + const wasRunning = useRef(false); const sampler = useRef(new PhysicsPerfSampler(PHYSICS_DT, MAX_SUBSTEPS)); const perfOn = useRef(false); + const stepsThisFrame = useRef(0); - // Latch query once (and expose reset) — no React state. if (typeof window !== 'undefined' && !perfOn.current) { perfOn.current = isPhysicsPerfQueryEnabled(); if (perfOn.current) { @@ -118,34 +128,37 @@ function RapierStepper({ running, speed }: { running: boolean; speed: number }) } useFrame((_, delta) => { - if (!running) return; - accumulator.current += Math.min(delta, 0.1) * speed; - let steps = 0; - let framePhysicsMs = 0; - while (accumulator.current >= PHYSICS_DT && steps < MAX_SUBSTEPS) { - if (perfOn.current) { - const t0 = performance.now(); - world.step(); - framePhysicsMs += performance.now() - t0; - } else { - world.step(); - } - physicsSimClock.simSec += PHYSICS_DT; - accumulator.current -= PHYSICS_DT; - steps += 1; + if (!running) { + // Pause: do not step; do not accumulate paused wall-clock time. + wasRunning.current = false; + return; } + + // On resume, ignore the (possibly huge) first frame delta. + const frameDelta = wasRunning.current + ? Math.min(delta, MAX_FRAME_DELTA_SEC) + : Math.min(delta, PHYSICS_DT); + wasRunning.current = true; + + const scaled = frameDelta * speed; + const capped = Math.min(scaled, MAX_SUBSTEPS * PHYSICS_DT); + const simBefore = physicsSimClock.simSec; + const before = performance.now(); + step(capped); + const steps = Math.max( + 0, + Math.round((physicsSimClock.simSec - simBefore) / PHYSICS_DT), + ); + stepsThisFrame.current = steps; if (perfOn.current && steps > 0) { - // Record per-frame physics cost (sum of substeps this frame), not render. - sampler.current.pushStepMs(framePhysicsMs, steps); + sampler.current.pushStepMs(performance.now() - before, steps); window.__PHYSICS_PERF__ = sampler.current.snapshot(readWorldMeta(world)); } - if (steps === MAX_SUBSTEPS) accumulator.current = 0; }); return null; } -/** Static colliders for the whole working area (fixed bodies, cheap cuboids). - * Layout data lives in domain/physicsWorldLayout — shared with headless tests. */ +/** Static colliders for the whole working area (fixed bodies, cheap cuboids). */ export function SorterStaticColliders() { return ( @@ -172,7 +185,14 @@ export function SorterPhysicsWorld({ children: ReactNode; }) { return ( - + + {children} diff --git a/src/domain/physicsConfigHash.ts b/src/domain/physicsConfigHash.ts index 0de5c36..97a56d0 100644 --- a/src/domain/physicsConfigHash.ts +++ b/src/domain/physicsConfigHash.ts @@ -5,15 +5,20 @@ */ import { getStaticColliders } from './physicsWorldLayout'; import { PUSHER } from './pusherMotion'; -import { PHYSICS_TIMESTEP_SEC, PHYSICS_GRAVITY } from './physicsTimestep'; +import { + PHYSICS_TIMESTEP_SEC, + PHYSICS_GRAVITY, + PHYSICS_MAX_SUBSTEPS, +} from './physicsTimestep'; import { allVisualPhysicsProfiles } from './visualPhysicsProfiles'; import { SIM_SETTLE_SECONDS } from './physicsDropSim'; import { B_RECEIVER, ROLL_CAGE, ZONES } from './physicalLayout'; -export { PHYSICS_TIMESTEP_SEC, PHYSICS_GRAVITY } from './physicsTimestep'; - -/** Must match SorterPhysics PHYSICS_MAX_SUBSTEPS. */ -export const PHYSICS_MAX_SUBSTEPS = 4; +export { + PHYSICS_TIMESTEP_SEC, + PHYSICS_GRAVITY, + PHYSICS_MAX_SUBSTEPS, +} from './physicsTimestep'; export interface PhysicsConfigSnapshot { timestep: number; diff --git a/src/domain/physicsTimestep.ts b/src/domain/physicsTimestep.ts index 9a27bdd..432d3db 100644 --- a/src/domain/physicsTimestep.ts +++ b/src/domain/physicsTimestep.ts @@ -1,3 +1,6 @@ /** Shared fixed physics timestep — runtime Rapier and headless must match. */ -export const PHYSICS_TIMESTEP_SEC = 1 / 60; +export const PHYSICS_TIMESTEP_SEC = 1 / 120; +export const PHYSICS_MAX_SUBSTEPS = 4; +/** Clamp tab-return / stall spikes before they enter the fixed-step accumulator. */ +export const MAX_FRAME_DELTA_SEC = 1 / 15; export const PHYSICS_GRAVITY: [number, number, number] = [0, -9.81, 0]; diff --git a/src/domain/productPhysicsProfiles.test.ts b/src/domain/productPhysicsProfiles.test.ts new file mode 100644 index 0000000..6ece79d --- /dev/null +++ b/src/domain/productPhysicsProfiles.test.ts @@ -0,0 +1,202 @@ +import { describe, expect, it, beforeEach } from 'vitest'; +import { + BELT_SPEED_MPS, + BELT_RESPONSE_TIME_SEC, + BELT_SPEED_EPSILON, + DOWNSTREAM_AXIS, + LATERAL_AXIS, + UP_AXIS, + JUNCTION_ENTRY_S, + getProductPhysicsProfile, + computeBeltDriveForce, + isSupportedByBelt, + spawnCenterY, + colliderHalfHeight, + resetInvalidProductStateCount, + INVALID_PRODUCT_STATE_COUNT, + isInvalidProductState, +} from './productPhysicsProfiles'; +import { + PHYSICS_TIMESTEP_SEC, + PHYSICS_MAX_SUBSTEPS, + MAX_FRAME_DELTA_SEC, +} from './physicsTimestep'; +import { BELT_TOP_Y, CONVEYOR_SPEED_MPS } from './physicalLayout'; +import { + categoryToPhysicalRoute, + DIVERTER_LEFT_SIGNED_DEG, + DIVERTER_RIGHT_SIGNED_DEG, + OPENING_SAFETY_MARGIN_SEC, + rotationDurationSec, +} from './pusherMotion'; + +describe('physical conveyor foundation contracts', () => { + beforeEach(() => { + resetInvalidProductStateCount(); + }); + + it('BELT_SPEED_MPS equals exactly 1.0', () => { + expect(BELT_SPEED_MPS).toBe(1.0); + expect(CONVEYOR_SPEED_MPS).toBe(1.0); + }); + + it('fixed timestep equals 1/120 and max substeps equals 4', () => { + expect(PHYSICS_TIMESTEP_SEC).toBeCloseTo(1 / 120, 12); + expect(PHYSICS_MAX_SUBSTEPS).toBe(4); + expect(MAX_FRAME_DELTA_SEC).toBeCloseTo(1 / 15, 12); + }); + + it('axes are orthonormal with +X downstream', () => { + expect(DOWNSTREAM_AXIS).toEqual([1, 0, 0]); + expect(UP_AXIS).toEqual([0, 1, 0]); + expect(LATERAL_AXIS).toEqual([0, 0, 1]); + const dotDL = DOWNSTREAM_AXIS[0] * LATERAL_AXIS[0] + + DOWNSTREAM_AXIS[1] * LATERAL_AXIS[1] + + DOWNSTREAM_AXIS[2] * LATERAL_AXIS[2]; + expect(dotDL).toBe(0); + }); + + it('mass-scaled force converges toward 1.0 without large overshoot drive', () => { + let v = 0; + const dt = PHYSICS_TIMESTEP_SEC; + const profile = getProductPhysicsProfile('SKU-001'); + for (let i = 0; i < 240; i += 1) { + const sample = computeBeltDriveForce({ + massKg: profile.massKg, + linearVelocity: [v, 0, 0], + maxBeltAccelerationMps2: profile.maxBeltAccelerationMps2, + applyLateralCorrection: false, + }); + v += (sample.force[0] / profile.massKg) * dt; + } + expect(v).toBeGreaterThan(0.98); + expect(v).toBeLessThan(1.02 + BELT_SPEED_EPSILON); + }); + + it('heavier and lighter products both converge with mass-scaled force', () => { + const run = (sku: string) => { + let v = 0; + const p = getProductPhysicsProfile(sku); + for (let i = 0; i < 300; i += 1) { + const s = computeBeltDriveForce({ + massKg: p.massKg, + linearVelocity: [v, 0, 0], + maxBeltAccelerationMps2: p.maxBeltAccelerationMps2, + applyLateralCorrection: false, + }); + v += (s.force[0] / p.massKg) * PHYSICS_TIMESTEP_SEC; + } + return v; + }; + const light = run('SKU-009'); + const heavy = run('SKU-004'); + expect(light).toBeGreaterThan(0.95); + expect(heavy).toBeGreaterThan(0.95); + expect(light).toBeLessThan(1.05); + expect(heavy).toBeLessThan(1.05); + }); + + it('deadband applies no forward correction near target', () => { + const sample = computeBeltDriveForce({ + massKg: 1, + linearVelocity: [1.0, 0, 0], + maxBeltAccelerationMps2: 5, + applyLateralCorrection: false, + }); + expect(sample.withinDeadband).toBe(true); + expect(sample.appliedAcceleration).toBe(0); + expect(sample.force[0]).toBe(0); + }); + + it('no belt support when airborne or past junction', () => { + const half = colliderHalfHeight(getProductPhysicsProfile('SKU-001')); + expect(isSupportedByBelt({ + position: [-3, BELT_TOP_Y + half + 0.1, 0], + halfHeight: half, + phase: 'physical_conveyor', + linearVelY: 0, + })).toBe(false); + + expect(isSupportedByBelt({ + position: [JUNCTION_ENTRY_S + 0.01, BELT_TOP_Y + half, 0], + halfHeight: half, + phase: 'physical_conveyor', + linearVelY: 0, + })).toBe(false); + + expect(isSupportedByBelt({ + position: [-3, BELT_TOP_Y + half + 0.002, 0], + halfHeight: half, + phase: 'junction', + linearVelY: 0, + })).toBe(false); + }); + + it('supported on belt top within clearance', () => { + const half = colliderHalfHeight(getProductPhysicsProfile('SKU-001')); + expect(isSupportedByBelt({ + position: [-3, spawnCenterY(getProductPhysicsProfile('SKU-001')), 0], + halfHeight: half, + phase: 'physical_conveyor', + linearVelY: 0, + })).toBe(true); + }); + + it('CCD enabled and spawn Y clears belt by 2mm', () => { + const p = getProductPhysicsProfile('SKU-001'); + expect(p.ccd).toBe(true); + expect(spawnCenterY(p)).toBeCloseTo(BELT_TOP_Y + colliderHalfHeight(p) + 0.002, 6); + }); + + it('profiles are ENGINEERING_DERIVED with primitive colliders', () => { + for (const id of ['SKU-001', 'SKU-004', 'SKU-007', 'SKU-009']) { + const p = getProductPhysicsProfile(id); + expect(p.provenance).toBe('ENGINEERING_DERIVED'); + expect(['cuboid', 'cylinder', 'capsule']).toContain(p.collider.type); + expect(p.restitution).toBeLessThanOrEqual(0.05); + } + }); + + it('invalid state detection catches NaN and overspeed', () => { + expect(isInvalidProductState({ + position: [0, NaN, 0], + linearVelocity: [0, 0, 0], + angularVelocity: [0, 0, 0], + })).toBe(true); + expect(isInvalidProductState({ + position: [0, 0.8, 0], + linearVelocity: [5, 0, 0], + angularVelocity: [0, 0, 0], + })).toBe(true); + expect(INVALID_PRODUCT_STATE_COUNT).toBe(0); + }); + + it('response time constant is 0.35 s', () => { + expect(BELT_RESPONSE_TIME_SEC).toBe(0.35); + }); + + it('physical conveyor phase has no per-frame setTranslation drive', async () => { + const src = await import('../components/ThreeD/PhysicalPlaybackItemPhysics.tsx?raw'); + const text = (src as { default: string }).default; + // Spawn/reset/handoff may call setTranslation once; kinematic drive loop must not. + expect(text).toMatch(/authority\.current === 'physical_conveyor'/); + expect(text).not.toMatch(/physical_conveyor[\s\S]{0,200}setNextKinematicTranslation/); + expect(text).toMatch(/useBeforePhysicsStep/); + expect(text).toMatch(/addForce/); + }); +}); + +describe('frozen routing contracts unchanged', () => { + it('B/C/D mapping unchanged', () => { + expect(categoryToPhysicalRoute('B')).toBe('STRAIGHT'); + expect(categoryToPhysicalRoute('C')).toBe('PHYSICAL_LEFT'); + expect(categoryToPhysicalRoute('D')).toBe('PHYSICAL_RIGHT'); + }); + + it('LEFT/RIGHT angles and timing unchanged', () => { + expect(DIVERTER_LEFT_SIGNED_DEG).toBe(-45); + expect(DIVERTER_RIGHT_SIGNED_DEG).toBe(45); + expect(rotationDurationSec()).toBeCloseTo(0.5, 6); + expect(OPENING_SAFETY_MARGIN_SEC).toBeCloseTo(0.15, 6); + }); +}); diff --git a/src/domain/productPhysicsProfiles.ts b/src/domain/productPhysicsProfiles.ts new file mode 100644 index 0000000..1e1101d --- /dev/null +++ b/src/domain/productPhysicsProfiles.ts @@ -0,0 +1,283 @@ +/** + * Canonical product physics profiles + belt-drive helpers. + * + * All mass/friction/damping values are ENGINEERING_DERIVED for stable digital-twin + * behavior. They are not certified manufacturer masses. + */ + +import { + BELT_TOP_Y, + CAD_GATE_ENGAGE_X, + CONVEYOR_SPEED_MPS, + CONVEYOR_WIDTH_M, + ZONES, +} from './physicalLayout'; + +export const BELT_SPEED_MPS = CONVEYOR_SPEED_MPS; // exactly 1.0 +export const BELT_RESPONSE_TIME_SEC = 0.35; +export const BELT_SPEED_EPSILON = 0.02; +export const LATERAL_DAMPING_GAIN = 2.5; +export const MAX_LATERAL_CORRECTION_MPS2 = 1.5; +export const SPAWN_CLEARANCE_M = 0.002; +export const MAX_PRODUCT_SPEED_MPS = 4.0; +export const MAX_ANGULAR_SPEED_RAD_S = 12.0; + +/** World axes confirmed by physicalLayout (+X downstream, +Y up, +Z left). */ +export const DOWNSTREAM_AXIS: [number, number, number] = [1, 0, 0]; +export const UP_AXIS: [number, number, number] = [0, 1, 0]; +export const LATERAL_AXIS: [number, number, number] = [0, 0, 1]; + +export const BELT_START_S = ZONES.A.x; +/** Temporary handoff into existing junction/drop authority. */ +export const JUNCTION_ENTRY_S = CAD_GATE_ENGAGE_X; +export const BELT_END_S = ZONES.B.x; + +export type ProductPhysicsPhase = + | 'preparing' + | 'physical_conveyor' + | 'junction' + | 'settled' + | 'invalid'; + +export type ProductCollider = + | { type: 'cuboid'; halfExtents: [number, number, number] } + | { type: 'cylinder'; radius: number; halfHeight: number; axis: 'x' | 'y' } + | { type: 'capsule'; radius: number; halfHeight: number; axis: 'x' | 'y' }; + +export type ProductPhysicsProfile = { + productId: string; + /** ENGINEERING_DERIVED */ + massKg: number; + collider: ProductCollider; + centerOfMassOffset: [number, number, number]; + beltFriction: number; + guideFriction: number; + restitution: number; + linearDamping: number; + angularDamping: number; + lockRotationX: boolean; + lockRotationY: boolean; + lockRotationZ: boolean; + maxBeltAccelerationMps2: number; + ccd: true; + provenance: 'ENGINEERING_DERIVED'; +}; + +const PROFILES: Record = { + 'SKU-001': { + productId: 'SKU-001', massKg: 0.8, + collider: { type: 'cuboid', halfExtents: [0.15, 0.1, 0.1] }, + centerOfMassOffset: [0, 0, 0], + beltFriction: 0.75, guideFriction: 0.35, restitution: 0.02, + linearDamping: 0.25, angularDamping: 3.5, + lockRotationX: true, lockRotationY: false, lockRotationZ: true, + maxBeltAccelerationMps2: 4, ccd: true, provenance: 'ENGINEERING_DERIVED', + }, + 'SKU-002': { + productId: 'SKU-002', massKg: 0.55, + collider: { type: 'cuboid', halfExtents: [0.1005, 0.031, 0.076] }, + centerOfMassOffset: [0, 0, 0], + beltFriction: 0.7, guideFriction: 0.3, restitution: 0.03, + linearDamping: 0.25, angularDamping: 3.2, + lockRotationX: true, lockRotationY: false, lockRotationZ: true, + maxBeltAccelerationMps2: 4.5, ccd: true, provenance: 'ENGINEERING_DERIVED', + }, + 'SKU-004': { + productId: 'SKU-004', massKg: 3.2, + collider: { type: 'cuboid', halfExtents: [0.2005, 0.2, 0.15] }, + centerOfMassOffset: [0, -0.02, 0], + beltFriction: 0.65, guideFriction: 0.35, restitution: 0.02, + linearDamping: 0.3, angularDamping: 4.0, + lockRotationX: true, lockRotationY: false, lockRotationZ: true, + maxBeltAccelerationMps2: 3.2, ccd: true, provenance: 'ENGINEERING_DERIVED', + }, + 'SKU-005': { + productId: 'SKU-005', massKg: 3.0, + collider: { type: 'cylinder', radius: 0.2445, halfHeight: 0.132, axis: 'y' }, + centerOfMassOffset: [0, 0, 0], + beltFriction: 0.7, guideFriction: 0.4, restitution: 0.01, + linearDamping: 0.35, angularDamping: 4.5, + lockRotationX: true, lockRotationY: false, lockRotationZ: true, + maxBeltAccelerationMps2: 3.0, ccd: true, provenance: 'ENGINEERING_DERIVED', + }, + 'SKU-006': { + productId: 'SKU-006', massKg: 0.45, + collider: { type: 'cuboid', halfExtents: [0.105, 0.0135, 0.1045] }, + centerOfMassOffset: [0, 0, 0], + beltFriction: 0.8, guideFriction: 0.35, restitution: 0.04, + linearDamping: 0.2, angularDamping: 3.0, + lockRotationX: true, lockRotationY: false, lockRotationZ: true, + maxBeltAccelerationMps2: 4.5, ccd: true, provenance: 'ENGINEERING_DERIVED', + }, + 'SKU-007': { + productId: 'SKU-007', massKg: 0.4, + collider: { type: 'capsule', radius: 0.0455, halfHeight: 0.107, axis: 'y' }, + centerOfMassOffset: [0, -0.02, 0], + beltFriction: 0.65, guideFriction: 0.3, restitution: 0.05, + linearDamping: 0.2, angularDamping: 4.0, + lockRotationX: false, lockRotationY: false, lockRotationZ: false, + maxBeltAccelerationMps2: 4.0, ccd: true, provenance: 'ENGINEERING_DERIVED', + }, + 'SKU-008': { + productId: 'SKU-008', massKg: 0.55, + collider: { type: 'cylinder', radius: 0.0215, halfHeight: 0.2175, axis: 'x' }, + centerOfMassOffset: [0, 0, 0], + beltFriction: 0.7, guideFriction: 0.3, restitution: 0.03, + linearDamping: 0.18, angularDamping: 3.5, + lockRotationX: false, lockRotationY: true, lockRotationZ: false, + maxBeltAccelerationMps2: 4.0, ccd: true, provenance: 'ENGINEERING_DERIVED', + }, + 'SKU-009': { + productId: 'SKU-009', massKg: 0.02, + collider: { type: 'capsule', radius: 0.006, halfHeight: 0.0675, axis: 'y' }, + centerOfMassOffset: [0, 0, 0], + beltFriction: 0.75, guideFriction: 0.35, restitution: 0.02, + linearDamping: 0.3, angularDamping: 5.0, + lockRotationX: true, lockRotationY: false, lockRotationZ: true, + maxBeltAccelerationMps2: 5.0, ccd: true, provenance: 'ENGINEERING_DERIVED', + }, + 'SKU-011': { + productId: 'SKU-011', massKg: 2.8, + collider: { type: 'cylinder', radius: 0.225, halfHeight: 0.15, axis: 'y' }, + centerOfMassOffset: [0, 0, 0], + beltFriction: 0.75, guideFriction: 0.4, restitution: 0.01, + linearDamping: 0.35, angularDamping: 4.5, + lockRotationX: true, lockRotationY: false, lockRotationZ: true, + maxBeltAccelerationMps2: 3.0, ccd: true, provenance: 'ENGINEERING_DERIVED', + }, +}; + +export const DEFAULT_PRODUCT_PHYSICS_PROFILE: ProductPhysicsProfile = { + productId: 'default', massKg: 1.0, + collider: { type: 'cuboid', halfExtents: [0.1, 0.08, 0.08] }, + centerOfMassOffset: [0, 0, 0], + beltFriction: 0.7, guideFriction: 0.3, restitution: 0.02, + linearDamping: 0.25, angularDamping: 3.5, + lockRotationX: true, lockRotationY: false, lockRotationZ: true, + maxBeltAccelerationMps2: 4.0, ccd: true, provenance: 'ENGINEERING_DERIVED', +}; + +export function getProductPhysicsProfile(productId: string): ProductPhysicsProfile { + const id = productId.replace(/-LC$/, ''); + return PROFILES[id] ?? DEFAULT_PRODUCT_PHYSICS_PROFILE; +} + +export function allProductPhysicsProfiles(): ProductPhysicsProfile[] { + return Object.values(PROFILES); +} + +export function colliderHalfHeight(profile: ProductPhysicsProfile): number { + const c = profile.collider; + if (c.type === 'cuboid') return c.halfExtents[1]; + return c.halfHeight; +} + +export function spawnCenterY(profile: ProductPhysicsProfile): number { + return BELT_TOP_Y + colliderHalfHeight(profile) + SPAWN_CLEARANCE_M; +} + +export function isSupportedByBelt(input: { + position: [number, number, number]; + halfHeight: number; + phase: ProductPhysicsPhase; + linearVelY: number; +}): boolean { + if (input.phase !== 'physical_conveyor') return false; + const [x, y, z] = input.position; + if (x < BELT_START_S - 0.05 || x >= JUNCTION_ENTRY_S) return false; + if (Math.abs(z) > CONVEYOR_WIDTH_M / 2 + 0.06) return false; + const bottomY = y - input.halfHeight; + if (bottomY > BELT_TOP_Y + 0.025) return false; // airborne + if (bottomY < BELT_TOP_Y - 0.04) return false; // sunk / off belt + if (input.linearVelY < -1.5) return false; // free-falling + return true; +} + +export type BeltForceSample = { + currentDownstreamSpeed: number; + speedError: number; + appliedAcceleration: number; + force: [number, number, number]; + lateralCorrection: [number, number, number]; + withinDeadband: boolean; +}; + +export function computeBeltDriveForce(input: { + massKg: number; + linearVelocity: [number, number, number]; + maxBeltAccelerationMps2: number; + applyLateralCorrection: boolean; + responseTimeSec?: number; + targetSpeedMps?: number; +}): BeltForceSample { + const target = input.targetSpeedMps ?? BELT_SPEED_MPS; + const response = input.responseTimeSec ?? BELT_RESPONSE_TIME_SEC; + const [vx, , vz] = input.linearVelocity; + // Downstream is +X; lateral is +Z (layout contract). + const downstreamSpeed = vx * DOWNSTREAM_AXIS[0] + vz * DOWNSTREAM_AXIS[2]; + const lateralVelocity = vx * LATERAL_AXIS[0] + vz * LATERAL_AXIS[2]; + const speedError = target - downstreamSpeed; + const withinDeadband = Math.abs(speedError) <= BELT_SPEED_EPSILON; + + let appliedAcceleration = 0; + if (!withinDeadband) { + const desired = speedError / response; + appliedAcceleration = Math.max( + -input.maxBeltAccelerationMps2, + Math.min(input.maxBeltAccelerationMps2, desired), + ); + } + + const force: [number, number, number] = [ + input.massKg * appliedAcceleration * DOWNSTREAM_AXIS[0], + input.massKg * appliedAcceleration * DOWNSTREAM_AXIS[1], + input.massKg * appliedAcceleration * DOWNSTREAM_AXIS[2], + ]; + + let lateralCorrection: [number, number, number] = [0, 0, 0]; + if (input.applyLateralCorrection) { + let aLat = -lateralVelocity * LATERAL_DAMPING_GAIN; + aLat = Math.max(-MAX_LATERAL_CORRECTION_MPS2, Math.min(MAX_LATERAL_CORRECTION_MPS2, aLat)); + lateralCorrection = [ + input.massKg * aLat * LATERAL_AXIS[0], + input.massKg * aLat * LATERAL_AXIS[1], + input.massKg * aLat * LATERAL_AXIS[2], + ]; + } + + return { + currentDownstreamSpeed: downstreamSpeed, + speedError, + appliedAcceleration, + force, + lateralCorrection, + withinDeadband, + }; +} + +export function isInvalidProductState(input: { + position: [number, number, number]; + linearVelocity: [number, number, number]; + angularVelocity: [number, number, number]; +}): boolean { + const vals = [...input.position, ...input.linearVelocity, ...input.angularVelocity]; + if (vals.some((v) => !Number.isFinite(v))) return true; + const [x, y, z] = input.position; + if (y < -0.5 || y > 5 || Math.abs(x) > 12 || Math.abs(z) > 6) return true; + const speed = Math.hypot(...input.linearVelocity); + if (speed > MAX_PRODUCT_SPEED_MPS) return true; + const ang = Math.hypot(...input.angularVelocity); + if (ang > MAX_ANGULAR_SPEED_RAD_S) return true; + return false; +} + +/** Dev/test counter — must stay 0 in normal validation runs. */ +export let INVALID_PRODUCT_STATE_COUNT = 0; + +export function resetInvalidProductStateCount() { + INVALID_PRODUCT_STATE_COUNT = 0; +} + +export function recordInvalidProductState() { + INVALID_PRODUCT_STATE_COUNT += 1; +}