Files
vibemake/scripts/lib/assembly-axis-planner.mjs
T
Jerry 1c837766cd feat: integrate robot assembly and simulation with resilient loading
Add backend-driven assembly and simulation experiences, MAKE branding, mobile rendering budgets and desktop-only CAD/PCB routes. Improve logo loading progress, document image recovery and URDF request handling; include dev packaging and regression coverage.
2026-10-30 18:38:39 +08:00

106 lines
5.9 KiB
JavaScript

// Offline only: no collision code is loaded by the community player.
import { Box3, Matrix4, Vector3 } from 'three';
import { MeshBVH, SAH } from 'three-mesh-bvh';
export const VIEW_AXES = [
{ name: '+Y', label: '从上往下', vector: [0, 1, 0] },
{ name: '+Z', label: '从前往后', vector: [0, 0, 1] },
{ name: '-Z', label: '从后往前', vector: [0, 0, -1] },
{ name: '+X', label: '从右往左', vector: [1, 0, 0] },
{ name: '-X', label: '从左往右', vector: [-1, 0, 0] },
{ name: '-Y', label: '从下往上', vector: [0, -1, 0] },
];
export function collider(id, geometry) {
// Caller provides a collision-only copy in display/world meters.
geometry.clearGroups();
geometry.computeBoundingBox();
geometry.boundsTree = new MeshBVH(geometry, { strategy: SAH, targetLeafSize: 8 });
return { id, geometry, box: geometry.boundingBox.clone(), bvh: geometry.boundsTree };
}
function union(items) {
const b = new Box3();
for (const item of items) b.union(item.box);
return b;
}
function boxDistance(a, b) {
return Math.hypot(...['x', 'y', 'z'].map(k => Math.max(0, a.min[k] - b.max[k], b.min[k] - a.max[k])));
}
// Minimum surface separation. An AABB is only a broad phase, never proof that
// shapes intersect: hollow shells and bearing holes must remain navigable.
function separation(moving, obstacles, offset) {
const matrix = new Matrix4().makeTranslation(...offset.toArray());
let distance = Infinity, blocker = null;
const pairs = [];
for (const a of moving) for (const b of obstacles) {
const lower = boxDistance(a.box.clone().translate(offset), b.box);
pairs.push({ a, b, lower });
}
pairs.sort((a, b) => a.lower - b.lower);
for (const { a, b, lower } of pairs) {
if (lower >= distance) continue;
const hit = b.bvh.closestPointToGeometry(a.geometry, matrix, {}, null, 0, distance);
if (hit && hit.distance < distance) { distance = hit.distance; blocker = b.id; }
if (distance === 0) break;
}
return { distance, blocker };
}
// Conservative advancement for fixed-rotation, straight translation. Surface
// separation cannot shrink faster than translation distance. Never jump by a
// fixed sampling interval: that could tunnel through a thin sheet.
// Starting outside the complete obstacle AABB also prevents an initially
// contained closed mesh from being mistaken for a clear route.
export function checkAxis(moving, obstacles, vector, {
minimumTravel = 0.035, startMargin = 0.004, seatingTolerance = 0.00015,
numericTolerance = 0.000001, maxIterations = 600, maxMilliseconds = 15000,
} = {}) {
if (!obstacles.length) return { status: 'foundation', travel: 0, checks: 0 };
const axis = new Vector3(...vector).normalize();
if (!axis.lengthSq() || !axis.toArray().every(Number.isFinite)) throw new Error('Invalid insertion axis');
const a = union(moving), b = union(obstacles);
const project = (box, sign) => ['x','y','z'].reduce((s,k) => s + axis[k] * box[axis[k] * sign >= 0 ? 'max' : 'min'][k], 0);
const travel = Math.max(minimumTravel, startMargin + project(b, 1) - project(a, -1));
let remaining = travel;
const started = performance.now();
for (let checks = 1; checks <= maxIterations; checks++) {
if (performance.now() - started > maxMilliseconds) return { status: 'needs-review', reason: 'time-budget', travel, remaining, checks };
const { distance, blocker } = separation(moving, obstacles, axis.clone().multiplyScalar(remaining));
if (!Number.isFinite(distance)) return { status: 'needs-review', reason: 'invalid-distance', travel, remaining, checks };
if (distance <= numericTolerance * 1.05) return { status: 'blocked-or-touching', blocker, travel, remaining, checks };
if (remaining <= seatingTolerance) return { status: 'clear-to-seating', travel, checks, uncheckedFinalDistance: remaining };
// Leave numerical slack; reaching the seating point uses the same bound.
const advance = Math.min(0.8 * (distance - numericTolerance), remaining - seatingTolerance);
if (advance <= Number.EPSILON) return { status: 'needs-review', reason: 'stalled', travel, remaining, checks };
// Assign the exact endpoint when the advance reaches seating. Subtracting
// nearly equal floats can otherwise leave a sub-ulp gap and report stalled.
remaining = advance === remaining - seatingTolerance ? seatingTolerance : remaining - advance;
}
return { status: 'needs-review', reason: 'iteration-budget', travel, remaining, checks: maxIterations };
}
export function chooseAxis(moving, obstacles, options = {}) {
const candidates = (options.axes ?? VIEW_AXES).map(axis => ({ ...axis, ...checkAxis(moving, obstacles, axis.vector, options) }));
const selected = candidates.find(c => c.status === 'clear-to-seating' || c.status === 'foundation') ?? null;
return { selected, candidates };
}
// Check an actual, finite entry offset. Obstacles outside every swept AABB cannot
// touch that path. Internally still start outside the relevant obstacle bounds:
// this intentionally rejects/flags a potentially contained start, rather than
// interpreting a lack of surface crossings inside a closed solid as clearance.
export function checkFiniteAxis(moving, obstacles, vector, travel, options = {}) {
if (!Number.isFinite(travel) || travel <= 0) throw new Error('Invalid entry travel');
const axis = new Vector3(...vector).normalize();
if (!axis.lengthSq() || !axis.toArray().every(Number.isFinite)) throw new Error('Invalid insertion axis');
const offset = axis.clone().multiplyScalar(travel);
const sweeps = moving.map(m => m.box.clone().union(m.box.clone().translate(offset)).expandByScalar(0.000002));
const relevant = obstacles.filter(b => sweeps.some(s => s.intersectsBox(b.box)));
const checked = checkAxis(moving, relevant, axis.toArray(), { ...options, minimumTravel: travel });
// Consumers must use travel, not checkedFromTravel, for the finite animation.
return { ...checked, checkedFromTravel: checked.travel, travel, relevantObstacles: relevant.length };
}