feat: add stable physical conveyor foundation
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -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):
|
* Lifecycle:
|
||||||
* 1. kinematicPosition — follows getPhysicalItemPose exactly (domain truth);
|
* PREPARING → PHYSICAL_CONVEYOR (dynamic + belt force) → JUNCTION
|
||||||
* 2. at getDropHandoffTimeMs → dynamic with deterministic initial velocity
|
* (existing drop/settle authority at getDropHandoffTimeMs) → FROZEN.
|
||||||
* (B: belt edge carry-over; C/D: pusher impulse, scaled per SKU profile);
|
*
|
||||||
* 3. gravity/collision/friction/restitution/angular velocity govern the drop;
|
* Temporary handoff: belt drive stops at JUNCTION_ENTRY_S / handoffMs; current
|
||||||
* 4. on sleep (or controlled 4.5 s timeout) the final position is verified
|
* classifier+diverter routing remains responsible for basket assignment.
|
||||||
* against the DOMAIN-decided receiver volume and the body is frozen
|
|
||||||
* (kinematic) — no drift, clean replay, no teleportation at any point.
|
|
||||||
*/
|
*/
|
||||||
import { memo, useCallback, useEffect, useMemo, useRef, useState } from 'react';
|
import { memo, useCallback, useEffect, useMemo, useRef, useState } from 'react';
|
||||||
import * as THREE from 'three';
|
import * as THREE from 'three';
|
||||||
@@ -18,11 +16,23 @@ import {
|
|||||||
CuboidCollider,
|
CuboidCollider,
|
||||||
CapsuleCollider,
|
CapsuleCollider,
|
||||||
CylinderCollider,
|
CylinderCollider,
|
||||||
|
useBeforePhysicsStep,
|
||||||
type RapierRigidBody,
|
type RapierRigidBody,
|
||||||
} from '@react-three/rapier';
|
} from '@react-three/rapier';
|
||||||
import { RigidBodyType } from '@dimforge/rapier3d-compat';
|
import { RigidBodyType } from '@dimforge/rapier3d-compat';
|
||||||
import { getPhysicalItemPose, getDropHandoffTimeMs } from '../../domain/physicalItemMotion';
|
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 { resolveItem } from '../../data/resolveItem';
|
||||||
import { classifyItem } from '../../domain/classifier';
|
import { classifyItem } from '../../domain/classifier';
|
||||||
import { receiverContains } from '../../domain/receiverVolumes';
|
import { receiverContains } from '../../domain/receiverVolumes';
|
||||||
@@ -32,23 +42,21 @@ import { recordDropResult, physicsSimClock } from './SorterPhysics';
|
|||||||
import { getModelAsset } from '../../data/modelAssets';
|
import { getModelAsset } from '../../data/modelAssets';
|
||||||
import { isProductAssetReady } from './RealItemModel';
|
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 SETTLE_BUDGET_SEC = 4.5;
|
||||||
|
const TELEMETRY_INTERVAL_MS = 200;
|
||||||
|
|
||||||
function colliderDensity(profile: ReturnType<typeof getVisualPhysicsProfile>): number {
|
function colliderDensity(profile: ReturnType<typeof getProductPhysicsProfile>): number {
|
||||||
if (profile.collider === 'cuboid' && profile.cuboidHalfExtents) {
|
const c = profile.collider;
|
||||||
const [hx, hy, hz] = profile.cuboidHalfExtents;
|
if (c.type === 'cuboid') {
|
||||||
return profile.approximateMassKg / (8 * hx * hy * hz);
|
const [hx, hy, hz] = c.halfExtents;
|
||||||
|
return profile.massKg / (8 * hx * hy * hz);
|
||||||
}
|
}
|
||||||
const [r, hh] = profile.capsule ?? [0.05, 0.1];
|
const volume = c.type === 'capsule'
|
||||||
const volume = profile.collider === 'capsule'
|
? Math.PI * c.radius * c.radius * (2 * c.halfHeight + (4 / 3) * c.radius)
|
||||||
? Math.PI * r * r * (2 * hh + (4 / 3) * r)
|
: Math.PI * c.radius * c.radius * 2 * c.halfHeight;
|
||||||
: Math.PI * r * r * 2 * hh;
|
return profile.massKg / Math.max(volume, 1e-6);
|
||||||
return profile.approximateMassKg / volume;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
export const PhysicalPlaybackItemPhysics = memo(function PhysicalPlaybackItemPhysics({
|
export const PhysicalPlaybackItemPhysics = memo(function PhysicalPlaybackItemPhysics({
|
||||||
@@ -70,18 +78,24 @@ export const PhysicalPlaybackItemPhysics = memo(function PhysicalPlaybackItemPhy
|
|||||||
const classification = useMemo(() => classifyItem(itemData), [itemData]);
|
const classification = useMemo(() => classifyItem(itemData), [itemData]);
|
||||||
const category = classification.category as 'B' | 'C' | 'D';
|
const category = classification.category as 'B' | 'C' | 'D';
|
||||||
const itemId = itemData.id.replace('-LC', '');
|
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 handoffMs = getDropHandoffTimeMs(classification.category, caseData.faultType);
|
||||||
|
const isFault = Boolean(caseData.faultType);
|
||||||
|
|
||||||
const bodyRef = useRef<RapierRigidBody>(null);
|
const bodyRef = useRef<RapierRigidBody>(null);
|
||||||
const traceEnabled = useRef(
|
const authority = useRef<Authority>(isFault ? 'fault_kinematic' : 'preparing');
|
||||||
typeof window !== 'undefined'
|
const phaseRef = useRef<ProductPhysicsPhase>('preparing');
|
||||||
&& new URLSearchParams(window.location.search).get('trace') === '1',
|
|
||||||
);
|
|
||||||
const authority = useRef<Authority>('kinematic');
|
|
||||||
const frozenPose = useRef<{ p: [number, number, number]; q: THREE.Quaternion } | null>(null);
|
const frozenPose = useRef<{ p: [number, number, number]; q: THREE.Quaternion } | null>(null);
|
||||||
const handedOffAtSimSec = useRef<number | null>(null);
|
const handedOffAtSimSec = useRef<number | null>(null);
|
||||||
const verified = useRef(false);
|
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 asset = getModelAsset(itemId);
|
||||||
const needsRealAsset = Boolean(asset?.defaultRealAsset && asset?.runtimePath);
|
const needsRealAsset = Boolean(asset?.defaultRealAsset && asset?.runtimePath);
|
||||||
const [spawned, setSpawned] = useState(
|
const [spawned, setSpawned] = useState(
|
||||||
@@ -101,6 +115,24 @@ export const PhysicalPlaybackItemPhysics = memo(function PhysicalPlaybackItemPhy
|
|||||||
jitter,
|
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(() => {
|
const handoffPose = useMemo(() => {
|
||||||
if (handoffMs == null) return null;
|
if (handoffMs == null) return null;
|
||||||
return getPhysicalItemPose({
|
return getPhysicalItemPose({
|
||||||
@@ -115,16 +147,14 @@ export const PhysicalPlaybackItemPhysics = memo(function PhysicalPlaybackItemPhy
|
|||||||
// eslint-disable-next-line react-hooks/exhaustive-deps
|
// eslint-disable-next-line react-hooks/exhaustive-deps
|
||||||
}, [handoffMs, caseData.id]);
|
}, [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(() => {
|
useEffect(() => {
|
||||||
authority.current = 'kinematic';
|
authority.current = isFault ? 'fault_kinematic' : 'preparing';
|
||||||
|
phaseRef.current = 'preparing';
|
||||||
frozenPose.current = null;
|
frozenPose.current = null;
|
||||||
handedOffAtSimSec.current = null;
|
handedOffAtSimSec.current = null;
|
||||||
verified.current = false;
|
verified.current = false;
|
||||||
|
activated.current = false;
|
||||||
|
invalidLogged.current = false;
|
||||||
const ready = !needsRealAsset || isProductAssetReady(asset?.runtimePath);
|
const ready = !needsRealAsset || isProductAssetReady(asset?.runtimePath);
|
||||||
setSpawned(ready);
|
setSpawned(ready);
|
||||||
const body = bodyRef.current;
|
const body = bodyRef.current;
|
||||||
@@ -132,24 +162,136 @@ export const PhysicalPlaybackItemPhysics = memo(function PhysicalPlaybackItemPhy
|
|||||||
body.setBodyType(RigidBodyType.KinematicPositionBased, false);
|
body.setBodyType(RigidBodyType.KinematicPositionBased, false);
|
||||||
body.setLinvel({ x: 0, y: 0, z: 0 }, true);
|
body.setLinvel({ x: 0, y: 0, z: 0 }, true);
|
||||||
body.setAngvel({ 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);
|
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);
|
const q = new THREE.Quaternion().setFromEuler(e);
|
||||||
body.setRotation({ x: q.x, y: q.y, z: q.z, w: q.w }, true);
|
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]);
|
}, [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(() => {
|
useFrame(() => {
|
||||||
const body = bodyRef.current;
|
const body = bodyRef.current;
|
||||||
if (!body) return;
|
if (!body) return;
|
||||||
|
|
||||||
// PREPARING: hold at spawn pose, zero velocity, keep invisible until visual ready.
|
|
||||||
if (!spawned) {
|
if (!spawned) {
|
||||||
const p = pose.position;
|
const p = spawnPose.position;
|
||||||
body.setNextKinematicTranslation({ x: p[0], y: p[1], z: p[2] });
|
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);
|
const q = new THREE.Quaternion().setFromEuler(e);
|
||||||
body.setNextKinematicRotation({ x: q.x, y: q.y, z: q.z, w: q.w });
|
body.setNextKinematicRotation({ x: q.x, y: q.y, z: q.z, w: q.w });
|
||||||
body.setLinvel({ x: 0, y: 0, z: 0 }, true);
|
body.setLinvel({ x: 0, y: 0, z: 0 }, true);
|
||||||
@@ -157,34 +299,12 @@ export const PhysicalPlaybackItemPhysics = memo(function PhysicalPlaybackItemPhy
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (authority.current === 'kinematic') {
|
if (authority.current === 'preparing' && !activated.current && !isFault) {
|
||||||
// Physics handoff at pusher contact / belt edge — never for fault cases.
|
activateDynamic(body);
|
||||||
// MUST be checked BEFORE the kinematic drive: under render lag a single
|
return;
|
||||||
// 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;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Kinematic drive: domain pose is truth (belt travel, inspection dwell).
|
if (authority.current === 'fault_kinematic') {
|
||||||
const p = pose.position;
|
const p = pose.position;
|
||||||
const e = new THREE.Euler(pose.rotation[0], pose.rotation[1], pose.rotation[2]);
|
const e = new THREE.Euler(pose.rotation[0], pose.rotation[1], pose.rotation[2]);
|
||||||
const q = new THREE.Quaternion().setFromEuler(e);
|
const q = new THREE.Quaternion().setFromEuler(e);
|
||||||
@@ -193,25 +313,13 @@ export const PhysicalPlaybackItemPhysics = memo(function PhysicalPlaybackItemPhy
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (authority.current === 'dynamic') {
|
if (authority.current === 'junction') {
|
||||||
const slept = body.isSleeping();
|
const slept = body.isSleeping();
|
||||||
const lv = body.linvel();
|
const lv = body.linvel();
|
||||||
const av = body.angvel();
|
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;
|
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
|
const timedOut = handedOffAtSimSec.current != null
|
||||||
&& physicsSimClock.simSec - handedOffAtSimSec.current > SETTLE_BUDGET_SEC;
|
&& 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) {
|
if ((slept || (timedOut && slow)) && !verified.current) {
|
||||||
verified.current = true;
|
verified.current = true;
|
||||||
const t = body.translation();
|
const t = body.translation();
|
||||||
@@ -231,12 +339,12 @@ export const PhysicalPlaybackItemPhysics = memo(function PhysicalPlaybackItemPhy
|
|||||||
body.setLinvel({ x: 0, y: 0, z: 0 }, false);
|
body.setLinvel({ x: 0, y: 0, z: 0 }, false);
|
||||||
body.setAngvel({ x: 0, y: 0, z: 0 }, false);
|
body.setAngvel({ x: 0, y: 0, z: 0 }, false);
|
||||||
authority.current = 'frozen';
|
authority.current = 'frozen';
|
||||||
|
phaseRef.current = 'settled';
|
||||||
}
|
}
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// frozen: hold the verified rest pose (no drift across replays).
|
if (authority.current === 'frozen' && frozenPose.current) {
|
||||||
if (frozenPose.current) {
|
|
||||||
const { p, q } = frozenPose.current;
|
const { p, q } = frozenPose.current;
|
||||||
body.setNextKinematicTranslation({ x: p[0], y: p[1], z: p[2] });
|
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 });
|
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;
|
if (elapsedMs < 0) return null;
|
||||||
|
|
||||||
const density = colliderDensity(profile);
|
const density = colliderDensity(profile);
|
||||||
// CCD for small/fast items (pen) and thin items (plate) — mirrors the sim.
|
const enabledRotations: [boolean, boolean, boolean] = [
|
||||||
const ccd = profile.approximateMassKg < 0.05 || itemData.dimensionsMm.height < 50;
|
!profile.lockRotationX,
|
||||||
|
!profile.lockRotationY,
|
||||||
|
!profile.lockRotationZ,
|
||||||
|
];
|
||||||
|
|
||||||
return (
|
return (
|
||||||
<RigidBody
|
<RigidBody
|
||||||
ref={bodyRef}
|
ref={bodyRef}
|
||||||
type="kinematicPosition"
|
type="kinematicPosition"
|
||||||
colliders={false}
|
colliders={false}
|
||||||
friction={profile.friction}
|
friction={profile.beltFriction}
|
||||||
restitution={profile.restitution}
|
restitution={profile.restitution}
|
||||||
linearDamping={profile.linearDamping}
|
linearDamping={profile.linearDamping}
|
||||||
angularDamping={profile.angularDamping}
|
angularDamping={profile.angularDamping}
|
||||||
ccd={ccd}
|
ccd={profile.ccd}
|
||||||
enabledRotations={[true, true, true]}
|
canSleep={false}
|
||||||
position={pose.position}
|
gravityScale={1}
|
||||||
|
enabledRotations={enabledRotations}
|
||||||
|
position={spawnPose.position}
|
||||||
|
rotation={spawnPose.rotation}
|
||||||
>
|
>
|
||||||
{/* Colliders only after visual ready — avoids stale/orphan contact. */}
|
{spawned && profile.collider.type === 'cuboid' && (
|
||||||
{spawned && profile.collider === 'cuboid' && profile.cuboidHalfExtents && (
|
<CuboidCollider args={profile.collider.halfExtents} density={density} friction={profile.beltFriction} />
|
||||||
<CuboidCollider args={profile.cuboidHalfExtents} density={density} />
|
|
||||||
)}
|
)}
|
||||||
{spawned && profile.collider === 'capsule' && profile.capsule && (
|
{spawned && profile.collider.type === 'capsule' && (
|
||||||
<CapsuleCollider
|
<CapsuleCollider
|
||||||
args={[profile.capsule[1], profile.capsule[0]]}
|
args={[profile.collider.halfHeight, profile.collider.radius]}
|
||||||
density={density}
|
density={density}
|
||||||
rotation={profile.colliderAxis === 'x' ? [0, 0, Math.PI / 2] : undefined}
|
friction={profile.beltFriction}
|
||||||
|
rotation={profile.collider.axis === 'x' ? [0, 0, Math.PI / 2] : undefined}
|
||||||
/>
|
/>
|
||||||
)}
|
)}
|
||||||
{spawned && profile.collider === 'cylinder' && profile.capsule && (
|
{spawned && profile.collider.type === 'cylinder' && (
|
||||||
<CylinderCollider
|
<CylinderCollider
|
||||||
args={[profile.capsule[1], profile.capsule[0]]}
|
args={[profile.collider.halfHeight, profile.collider.radius]}
|
||||||
density={density}
|
density={density}
|
||||||
rotation={profile.colliderAxis === 'x' ? [0, 0, Math.PI / 2] : undefined}
|
friction={profile.beltFriction}
|
||||||
|
rotation={profile.collider.axis === 'x' ? [0, 0, Math.PI / 2] : undefined}
|
||||||
/>
|
/>
|
||||||
)}
|
)}
|
||||||
<group visible={spawned}>
|
<group visible={spawned}>
|
||||||
|
|||||||
@@ -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):
|
* Physics is mounted `paused` so FrameStepper does not auto-step. RapierStepper
|
||||||
* - items on the belt are KINEMATIC (domain pose is truth);
|
* is the only caller of context.step(), which runs before/after hooks, fixed
|
||||||
* - at the drop handoff (pusher contact / belt edge) the body switches to
|
* substeps, and mesh sync. Playback speed scales the clamped frame delta.
|
||||||
* 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.
|
|
||||||
*/
|
*/
|
||||||
import { useRef, type ReactNode } from 'react';
|
import { useRef, type ReactNode } from 'react';
|
||||||
import { useFrame } from '@react-three/fiber';
|
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 { 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 {
|
import {
|
||||||
PhysicsPerfSampler,
|
PhysicsPerfSampler,
|
||||||
isPhysicsPerfQueryEnabled,
|
isPhysicsPerfQueryEnabled,
|
||||||
@@ -28,7 +28,7 @@ import {
|
|||||||
} from '../../domain/physicsPerf';
|
} from '../../domain/physicsPerf';
|
||||||
|
|
||||||
export const PHYSICS_DT = PHYSICS_TIMESTEP_SEC;
|
export const PHYSICS_DT = PHYSICS_TIMESTEP_SEC;
|
||||||
export const PHYSICS_MAX_SUBSTEPS = 4;
|
export { PHYSICS_MAX_SUBSTEPS };
|
||||||
const MAX_SUBSTEPS = PHYSICS_MAX_SUBSTEPS;
|
const MAX_SUBSTEPS = PHYSICS_MAX_SUBSTEPS;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -59,6 +59,7 @@ declare global {
|
|||||||
__DROP_RESULTS?: DropResult[];
|
__DROP_RESULTS?: DropResult[];
|
||||||
__PHYSICS_PERF__?: PhysicsPerfSnapshot;
|
__PHYSICS_PERF__?: PhysicsPerfSnapshot;
|
||||||
__PHYSICS_PERF_RESET__?: () => void;
|
__PHYSICS_PERF_RESET__?: () => void;
|
||||||
|
__CONVEYOR_PHYSICS_DEBUG__?: Record<string, unknown>;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -73,7 +74,6 @@ function readWorldMeta(world: {
|
|||||||
colliders?: { len: () => number };
|
colliders?: { len: () => number };
|
||||||
}): { activeBodies: number; sleepingBodies: number; colliders: number; contactPairs: number } {
|
}): { activeBodies: number; sleepingBodies: number; colliders: number; contactPairs: number } {
|
||||||
try {
|
try {
|
||||||
// @react-three/rapier wraps Rapier world; body counts via forEach when available
|
|
||||||
const w = world as unknown as {
|
const w = world as unknown as {
|
||||||
forEachRigidBody?: (cb: (b: { isSleeping: () => boolean; numColliders: () => number }) => void) => void;
|
forEachRigidBody?: (cb: (b: { isSleeping: () => boolean; numColliders: () => number }) => void) => void;
|
||||||
bodies?: { len: () => number };
|
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 }) {
|
function RapierStepper({ running, speed }: { running: boolean; speed: number }) {
|
||||||
const { world } = useRapier();
|
const { step, world } = useRapier();
|
||||||
const accumulator = useRef(0);
|
const wasRunning = useRef(false);
|
||||||
const sampler = useRef(new PhysicsPerfSampler(PHYSICS_DT, MAX_SUBSTEPS));
|
const sampler = useRef(new PhysicsPerfSampler(PHYSICS_DT, MAX_SUBSTEPS));
|
||||||
const perfOn = useRef(false);
|
const perfOn = useRef(false);
|
||||||
|
const stepsThisFrame = useRef(0);
|
||||||
|
|
||||||
// Latch query once (and expose reset) — no React state.
|
|
||||||
if (typeof window !== 'undefined' && !perfOn.current) {
|
if (typeof window !== 'undefined' && !perfOn.current) {
|
||||||
perfOn.current = isPhysicsPerfQueryEnabled();
|
perfOn.current = isPhysicsPerfQueryEnabled();
|
||||||
if (perfOn.current) {
|
if (perfOn.current) {
|
||||||
@@ -118,34 +128,37 @@ function RapierStepper({ running, speed }: { running: boolean; speed: number })
|
|||||||
}
|
}
|
||||||
|
|
||||||
useFrame((_, delta) => {
|
useFrame((_, delta) => {
|
||||||
if (!running) return;
|
if (!running) {
|
||||||
accumulator.current += Math.min(delta, 0.1) * speed;
|
// Pause: do not step; do not accumulate paused wall-clock time.
|
||||||
let steps = 0;
|
wasRunning.current = false;
|
||||||
let framePhysicsMs = 0;
|
return;
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// 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) {
|
if (perfOn.current && steps > 0) {
|
||||||
// Record per-frame physics cost (sum of substeps this frame), not render.
|
sampler.current.pushStepMs(performance.now() - before, steps);
|
||||||
sampler.current.pushStepMs(framePhysicsMs, steps);
|
|
||||||
window.__PHYSICS_PERF__ = sampler.current.snapshot(readWorldMeta(world));
|
window.__PHYSICS_PERF__ = sampler.current.snapshot(readWorldMeta(world));
|
||||||
}
|
}
|
||||||
if (steps === MAX_SUBSTEPS) accumulator.current = 0;
|
|
||||||
});
|
});
|
||||||
return null;
|
return null;
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Static colliders for the whole working area (fixed bodies, cheap cuboids).
|
/** Static colliders for the whole working area (fixed bodies, cheap cuboids). */
|
||||||
* Layout data lives in domain/physicsWorldLayout — shared with headless tests. */
|
|
||||||
export function SorterStaticColliders() {
|
export function SorterStaticColliders() {
|
||||||
return (
|
return (
|
||||||
<RigidBody type="fixed" colliders={false}>
|
<RigidBody type="fixed" colliders={false}>
|
||||||
@@ -172,7 +185,14 @@ export function SorterPhysicsWorld({
|
|||||||
children: ReactNode;
|
children: ReactNode;
|
||||||
}) {
|
}) {
|
||||||
return (
|
return (
|
||||||
<Physics updateLoop="independent" paused timeStep={PHYSICS_DT} gravity={[0, -9.81, 0]}>
|
<Physics
|
||||||
|
updateLoop="independent"
|
||||||
|
paused
|
||||||
|
timeStep={PHYSICS_DT}
|
||||||
|
gravity={PHYSICS_GRAVITY}
|
||||||
|
interpolate
|
||||||
|
>
|
||||||
|
<PhysicsSimClock />
|
||||||
<RapierStepper running={running} speed={speed} />
|
<RapierStepper running={running} speed={speed} />
|
||||||
<SorterStaticColliders />
|
<SorterStaticColliders />
|
||||||
{children}
|
{children}
|
||||||
|
|||||||
@@ -5,15 +5,20 @@
|
|||||||
*/
|
*/
|
||||||
import { getStaticColliders } from './physicsWorldLayout';
|
import { getStaticColliders } from './physicsWorldLayout';
|
||||||
import { PUSHER } from './pusherMotion';
|
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 { allVisualPhysicsProfiles } from './visualPhysicsProfiles';
|
||||||
import { SIM_SETTLE_SECONDS } from './physicsDropSim';
|
import { SIM_SETTLE_SECONDS } from './physicsDropSim';
|
||||||
import { B_RECEIVER, ROLL_CAGE, ZONES } from './physicalLayout';
|
import { B_RECEIVER, ROLL_CAGE, ZONES } from './physicalLayout';
|
||||||
|
|
||||||
export { PHYSICS_TIMESTEP_SEC, PHYSICS_GRAVITY } from './physicsTimestep';
|
export {
|
||||||
|
PHYSICS_TIMESTEP_SEC,
|
||||||
/** Must match SorterPhysics PHYSICS_MAX_SUBSTEPS. */
|
PHYSICS_GRAVITY,
|
||||||
export const PHYSICS_MAX_SUBSTEPS = 4;
|
PHYSICS_MAX_SUBSTEPS,
|
||||||
|
} from './physicsTimestep';
|
||||||
|
|
||||||
export interface PhysicsConfigSnapshot {
|
export interface PhysicsConfigSnapshot {
|
||||||
timestep: number;
|
timestep: number;
|
||||||
|
|||||||
@@ -1,3 +1,6 @@
|
|||||||
/** Shared fixed physics timestep — runtime Rapier and headless must match. */
|
/** 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];
|
export const PHYSICS_GRAVITY: [number, number, number] = [0, -9.81, 0];
|
||||||
|
|||||||
202
src/domain/productPhysicsProfiles.test.ts
Normal file
202
src/domain/productPhysicsProfiles.test.ts
Normal file
@@ -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);
|
||||||
|
});
|
||||||
|
});
|
||||||
283
src/domain/productPhysicsProfiles.ts
Normal file
283
src/domain/productPhysicsProfiles.ts
Normal file
@@ -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<string, ProductPhysicsProfile> = {
|
||||||
|
'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;
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user