diff --git a/fission/src/Synthesis.tsx b/fission/src/Synthesis.tsx index d2e0df1b66..3d7bcfc6c8 100644 --- a/fission/src/Synthesis.tsx +++ b/fission/src/Synthesis.tsx @@ -16,6 +16,7 @@ import ProgressNotifications from "@/components/ProgressNotification.tsx" import SceneOverlay from "@/components/overlays/SceneOverlay.tsx" import PortraitOverlay from "@/components/overlays/PortraitOverlay.tsx" import TouchControls from "./ui/components/TouchControls.tsx" +import WheelAssignmentDebugPanel from "./ui/components/WheelAssignmentDebugPanel.tsx" import { StateProvider } from "./ui/StateProvider.tsx" import { ThemeProvider } from "./ui/ThemeProvider.tsx" import { UIProvider } from "./ui/UIProvider.tsx" @@ -112,6 +113,7 @@ const Synthesis = () => { + {!consentPopupDisable && ( diff --git a/fission/src/mirabuf/WheelJointBuilder.ts b/fission/src/mirabuf/WheelJointBuilder.ts new file mode 100644 index 0000000000..ff7cd5cbfc --- /dev/null +++ b/fission/src/mirabuf/WheelJointBuilder.ts @@ -0,0 +1,155 @@ +import MirabufParser from "@/mirabuf/MirabufParser" +import { mirabuf } from "@/proto/mirabuf" +import { isWheel } from "@/systems/physics/ConstraintSettingsUtilities" +import type { WheelAxis } from "@/util/geometry/WheelAxisFit" + +/** Prefix for throwaway separator joints; never made into real constraints. */ +export const WHEEL_SEPARATOR_JOINT_PREFIX = "manual_wheel_separator_" + +export interface WheelAssignment { + /** Part-instance GUID of the occurrence the user picked as the wheel. */ + wheelPartGuid: string + /** Part-instance GUID of the occurrence the user picked as the wheel's parent/chassis. */ + parentPartGuid: string + /** World-space (assembly rest-pose) axis fit — center in metres, axis normalized. */ + axisFit: WheelAxis +} + +/** Dry-run parse with RigidGroups emptied to find the wheel's true post-split subtree. */ +function computeWheelSubtreeParts(assembly: mirabuf.Assembly, wheelPartGuid: string): ReadonlySet { + const joints = assembly.data!.joints! + const savedRigidGroups = joints.rigidGroups + joints.rigidGroups = [] + try { + const dryRunParser = new MirabufParser(assembly) + const node = dryRunParser.partToNodeMap.get(wheelPartGuid) + if (!node) { + return new Set([wheelPartGuid]) + } + return new Set(node.parts) + } finally { + joints.rigidGroups = savedRigidGroups + } +} + +/** Removes wheelSubtree's parts from every RigidGroup, dropping groups left under 2 occurrences. */ +function unbandageWheelSubtree(assembly: mirabuf.Assembly, wheelSubtree: ReadonlySet): void { + const rigidGroups = assembly.data?.joints?.rigidGroups + if (!rigidGroups) { + return + } + + for (let index = rigidGroups.length - 1; index >= 0; index--) { + const group = rigidGroups[index] + if (!group.occurrences) continue + + const before = group.occurrences.length + group.occurrences = group.occurrences.filter(guid => !wheelSubtree.has(guid)) + if (group.occurrences.length === before) continue + + if (group.occurrences.length < 2) rigidGroups.splice(index, 1) + } +} + +/** Adds a throwaway separator joint between every pair of wheels (existing + new) to keep each in its own rigid node. */ +function addWheelSeparatorJoints(assembly: mirabuf.Assembly, assignments: WheelAssignment[]): void { + const joints = assembly.data!.joints! + + const existingWheelParts = Object.values(joints.jointInstances!) + .filter(inst => { + const jDef = joints.jointDefinitions?.[inst.jointReference!] as mirabuf.joint.Joint | undefined + return jDef && isWheel(jDef) + }) + .map(inst => inst.childPart!) + const wheelParts = [...existingWheelParts, ...assignments.map(a => a.wheelPartGuid)] + + for (let i = 0; i < wheelParts.length; i++) { + for (let j = i + 1; j < wheelParts.length; j++) { + const token = `${WHEEL_SEPARATOR_JOINT_PREFIX}${crypto.randomUUID()}` + const name = `Manual Wheel Separator ${i}-${j}` + + joints.jointDefinitions![token] = { + info: { GUID: token, name, version: 1 }, + origin: { x: 0, y: 0, z: 0 }, + jointMotionType: mirabuf.joint.JointMotion.REVOLUTE, + rotational: { + rotationalFreedom: { + axis: { x: 0, y: 1, z: 0 }, + dynamics: { damping: 0, friction: 0 }, + value: 0, + }, + }, + } + + joints.jointInstances![token] = { + info: { GUID: token, name, version: 1 }, + parentPart: wheelParts[i], + childPart: wheelParts[j], + jointReference: token, + offset: { x: 0, y: 0, z: 0 }, + } + } + } +} + +/** Mutates assembly in place: adds a REVOLUTE wheel joint per assignment and unbandages its rigid-node subtree. */ +export function applyWheelAssignments(assembly: mirabuf.Assembly, assignments: WheelAssignment[]): void { + const joints = assembly.data?.joints + if (!joints) throw new Error("Assembly has no joints container") + + joints.jointDefinitions ??= {} + joints.jointInstances ??= {} + + // Must run before the per-assignment loop below. + addWheelSeparatorJoints(assembly, assignments) + + assignments.forEach((assignment, i) => { + const token = `manual_wheel_${crypto.randomUUID()}` + const name = `Manual Wheel ${i + 1}` + + const origin: mirabuf.IVector3 = { + x: assignment.axisFit.center.x * 100, + y: assignment.axisFit.center.y * 100, + z: assignment.axisFit.center.z * 100, + } + const axis: mirabuf.IVector3 = { + x: assignment.axisFit.axis.x, + y: assignment.axisFit.axis.y, + z: assignment.axisFit.axis.z, + } + + joints.jointDefinitions![token] = { + info: { GUID: token, name, version: 1 }, + origin, + jointMotionType: mirabuf.joint.JointMotion.REVOLUTE, + rotational: { + rotationalFreedom: { + axis, + dynamics: { damping: 0, friction: 0 }, + value: 0, + }, + }, + // wheelRadius/wheelWidth are centimetres, matching origin. + userData: { + data: { + wheel: "true", + wheelType: "0", + wheelRadius: String(assignment.axisFit.radius * 100), + wheelWidth: String(assignment.axisFit.width * 100), + }, + }, + } + + joints.jointInstances![token] = { + info: { GUID: token, name, version: 1 }, + parentPart: assignment.parentPartGuid, + childPart: assignment.wheelPartGuid, + jointReference: token, + offset: { x: 0, y: 0, z: 0 }, + } + + // Must run after the joint above is added. + const wheelSubtree = computeWheelSubtreeParts(assembly, assignment.wheelPartGuid) + unbandageWheelSubtree(assembly, wheelSubtree) + }) +} diff --git a/fission/src/systems/EventSystem.ts b/fission/src/systems/EventSystem.ts index 221cbd8729..0813cb0aab 100644 --- a/fission/src/systems/EventSystem.ts +++ b/fission/src/systems/EventSystem.ts @@ -51,6 +51,9 @@ interface EventDataMap { SetTouchControlsVisibilityEvent: boolean DragModeToggled: { enabled: boolean } + WheelAssignmentModeToggled: { enabled: boolean } + WheelAssignmentPendingCountChanged: { count: number } + WheelAssignmentDriveReversedChanged: { reversed: boolean } CameraModeChangedEvent: { mode: string } CameraFocusChangedEvent: { focusProvider: MirabufSceneObject | undefined } diff --git a/fission/src/systems/World.ts b/fission/src/systems/World.ts index b125f33aaa..02f12efccf 100644 --- a/fission/src/systems/World.ts +++ b/fission/src/systems/World.ts @@ -8,6 +8,7 @@ import type MultiplayerSystem from "./multiplayer/MultiplayerSystem" import PhysicsSystem from "./physics/PhysicsSystem" import DragModeSystem from "./scene/DragModeSystem" import SceneRenderer from "./scene/SceneRenderer" +import WheelAssignmentMode from "./scene/WheelAssignmentMode" import RobotPositionTracker from "./simulation/RobotPositionTracker" import SimulationSystem from "./simulation/SimulationSystem" @@ -25,6 +26,7 @@ class World { private _multiplayerSystem?: MultiplayerSystem private _analyticsSystem: AnalyticsSystem | undefined = undefined private _dragModeSystem: DragModeSystem + private _wheelAssignmentMode: WheelAssignmentMode private _performanceMonitorSystem: PerformanceMonitoringSystem private _scoreTracker: ScoreTracker = new ScoreTracker() @@ -71,6 +73,9 @@ class World { public static get scoreTracker() { return this._instance?._scoreTracker! } + public static get wheelAssignmentMode() { + return this._instance?._wheelAssignmentMode! + } public static getOwnRobots() { return World.multiplayerSystem?.getOwnRobots() ?? World.sceneRenderer.mirabufSceneObjects.getRobots() @@ -104,6 +109,7 @@ class World { this._simulationSystem = new SimulationSystem() this._inputSystem = new InputSystem() this._dragModeSystem = new DragModeSystem() + this._wheelAssignmentMode = new WheelAssignmentMode() this._performanceMonitorSystem = new PerformanceMonitoringSystem() try { @@ -146,6 +152,7 @@ class World { this._inputSystem.destroy() this._multiplayerSystem?.destroy() this._dragModeSystem.destroy() + this._wheelAssignmentMode.destroy() this._performanceMonitorSystem.destroy() this._analyticsSystem?.destroy() @@ -167,6 +174,7 @@ class World { this._accumTimes.inputTime += this.time(() => this._inputSystem.update(this._currentDeltaT)) this._accumTimes.sceneTime += this.time(() => this._sceneRenderer.update(this._currentDeltaT)) this._dragModeSystem.update(this._currentDeltaT) + this._wheelAssignmentMode.update(this._currentDeltaT) }) this._analyticsSystem?.update(this._currentDeltaT) diff --git a/fission/src/systems/physics/ConstraintSettingsUtilities.ts b/fission/src/systems/physics/ConstraintSettingsUtilities.ts index efa9875b45..fc9fd3ed76 100644 --- a/fission/src/systems/physics/ConstraintSettingsUtilities.ts +++ b/fission/src/systems/physics/ConstraintSettingsUtilities.ts @@ -147,3 +147,19 @@ export function createDOFSpecs(dofs: mirabuf.joint.IDOF[]): DOFSpecs[] { export function isWheel(jDef: mirabuf.joint.Joint): boolean { return (jDef.info?.name !== "grounded" && (jDef.userData?.data?.wheel ?? "false") === "true") ?? false } + +/** Explicit radius set on a manually-assigned wheel's userData (centimetres); undefined falls back to AABB inference. */ +export function getExplicitWheelRadius(jDef: mirabuf.joint.Joint): number | undefined { + const raw = jDef.userData?.data?.wheelRadius + if (raw === undefined) return undefined + const radius = Number(raw) / 100.0 + return Number.isFinite(radius) && radius > 0 ? radius : undefined +} + +/** Same idea as getExplicitWheelRadius, but for axle-direction width. */ +export function getExplicitWheelWidth(jDef: mirabuf.joint.Joint): number | undefined { + const raw = jDef.userData?.data?.wheelWidth + if (raw === undefined) return undefined + const width = Number(raw) / 100.0 + return Number.isFinite(width) && width > 0 ? width : undefined +} diff --git a/fission/src/systems/physics/Mechanism.ts b/fission/src/systems/physics/Mechanism.ts index 9148843242..7b273f6151 100644 --- a/fission/src/systems/physics/Mechanism.ts +++ b/fission/src/systems/physics/Mechanism.ts @@ -26,6 +26,8 @@ class Mechanism { public ghostBodies: Jolt.BodyID[] = [] public touchedBodies: [SceneObjectId, RigidNodeId][] = [] + public urdfWheelForward?: { x: number; y: number; z: number } + public constructor( rootBody: string, bodyMap: Map, diff --git a/fission/src/systems/physics/PhysicsSystem.ts b/fission/src/systems/physics/PhysicsSystem.ts index 150e2579f9..03f86be6a1 100644 --- a/fission/src/systems/physics/PhysicsSystem.ts +++ b/fission/src/systems/physics/PhysicsSystem.ts @@ -19,6 +19,7 @@ import { type RigidNodeId, type RigidNodeReadOnly, } from "@/mirabuf/MirabufParser.ts" +import { WHEEL_SEPARATOR_JOINT_PREFIX } from "@/mirabuf/WheelJointBuilder.ts" import { mirabuf } from "@/proto/mirabuf" import type { Message } from "../multiplayer/MultiplayerTypes.ts" import PreferencesSystem from "../preferences/PreferencesSystem" @@ -43,6 +44,8 @@ import { createDOFSpecs, createVehicleController, getAxis, + getExplicitWheelRadius, + getExplicitWheelWidth, getPerpendicular, isWheel, setAxes, @@ -126,6 +129,9 @@ const DEFAULT_FRICTION = 0.7 const SUSPENSION_MIN_FACTOR = 0.0001 const SUSPENSION_MAX_FACTOR = 0.0001 +// Manually-assigned wheels need more suspension travel to tolerate circle-fit origin error. +const MANUAL_WHEEL_SUSPENSION_MAX_FACTOR = 0.2 + // Wheels whose inferred radii fall within this relative tolerance of each other are treated as the // same size and snapped to a common radius. Sits well above mesh-tessellation noise (<1%) and well // below the gap between genuinely different wheel sizes, so distinct sizes stay in separate groups. @@ -478,6 +484,8 @@ class PhysicsSystem extends WorldSystem { joints.forEach(([jointGuid, jointInst]) => { if (jointGuid == GROUNDED_JOINT_ID) return + // Structural-only separator joint, not a real constraint. + if (jointGuid.startsWith(WHEEL_SEPARATOR_JOINT_PREFIX)) return const rnA = parser.partToNodeMap.get(jointInst.parentPart!) const rnB = parser.partToNodeMap.get(jointInst.childPart!) @@ -832,21 +840,31 @@ class PhysicsSystem extends WorldSystem { wheelDimensions.radius = resolvedRadius } + // Manual wheels carry explicit width too; AABB width would read the whole shared rigid body. + const explicitWidth = getExplicitWheelWidth(jointDefinition) + if (explicitWidth !== undefined) { + wheelDimensions.width = explicitWidth + } + const wheelPos = urdfWheelBasis ? convertJoltRVec3ToJoltVec3(anchorPoint) : convertJoltRVec3ToJoltVec3(anchorPoint.Add(axis)) const wheelSettings = new JOLT.WheelSettingsWV() + const simulatedRadius = wheelDimensions.radius * 1.05 + wheelSettings.mPosition = wheelPos JOLT.destroy(wheelPos) wheelSettings.mMaxSteerAngle = 0.0 wheelSettings.mMaxHandBrakeTorque = 0.0 - wheelSettings.mRadius = wheelDimensions.radius * 1.05 + wheelSettings.mRadius = simulatedRadius wheelSettings.mWidth = wheelDimensions.width + const isManualWheel = getExplicitWheelRadius(jointDefinition) !== undefined wheelSettings.mSuspensionMinLength = wheelDimensions.radius * SUSPENSION_MIN_FACTOR - wheelSettings.mSuspensionMaxLength = wheelDimensions.radius * SUSPENSION_MAX_FACTOR + wheelSettings.mSuspensionMaxLength = + wheelDimensions.radius * (isManualWheel ? MANUAL_WHEEL_SUSPENSION_MAX_FACTOR : SUSPENSION_MAX_FACTOR) wheelSettings.mInertia = 1 if (urdfWheelBasis) { @@ -980,6 +998,7 @@ class PhysicsSystem extends WorldSystem { const maxBounds = new JOLT.Vec3(-1000000.0, -1000000.0, -1000000.0) nonPhysicsNodes.forEach(rn => { + // Note: CompoundShapeSubShape.GetPositionCOM() is COM-relative, not assembly-space. const compoundShapeSettings = new JOLT.StaticCompoundShapeSettings() let shapesAdded = 0 diff --git a/fission/src/systems/scene/WheelAssignmentMode.ts b/fission/src/systems/scene/WheelAssignmentMode.ts new file mode 100644 index 0000000000..568e8cdfd3 --- /dev/null +++ b/fission/src/systems/scene/WheelAssignmentMode.ts @@ -0,0 +1,347 @@ +import * as THREE from "three" +import { GROUNDED_JOINT_ID } from "@/mirabuf/MirabufParser" +import { createMirabuf } from "@/mirabuf/MirabufSceneObject" +import type MirabufSceneObject from "@/mirabuf/MirabufSceneObject" +import { applyWheelAssignments, type WheelAssignment } from "@/mirabuf/WheelJointBuilder" +import EventSystem from "@/systems/EventSystem.ts" +import SynthesisBrain from "@/systems/simulation/synthesis_brain/SynthesisBrain" +import { globalAddToast } from "@/ui/components/GlobalUIControls" +import { + computeWheelAxisFromAABB, + computeWheelAxisFromCircleFit, + transformWheelAxis, +} from "@/util/geometry/WheelAxisFit" +import World from "../World" +import WorldSystem from "../WorldSystem" +import { type InteractionStart, PRIMARY_MOUSE_INTERACTION } from "./ScreenInteractionHandler" + +interface PartPick { + sceneObject: MirabufSceneObject + guid: string + object: THREE.Object3D + instanceId: number +} + +interface BatchedMeshRangeApi { + getGeometryIdAt?: (instanceId: number) => number + getGeometryRangeAt?: (geometryId: number, target?: object) => { vertexStart: number; vertexCount: number } +} + +/** Local-space vertices for just this part's slice of a shared BatchedMesh buffer; whole geometry otherwise. */ +function getPartLocalVertices(object: THREE.Object3D, instanceId: number): THREE.Vector3[] | undefined { + const mesh = object as THREE.Mesh + const position = mesh.geometry?.getAttribute("position") + if (!position) return undefined + + let start = 0 + let count = position.count + + const batched = object as unknown as BatchedMeshRangeApi + if (typeof batched.getGeometryIdAt === "function" && typeof batched.getGeometryRangeAt === "function") { + const geometryId = batched.getGeometryIdAt(instanceId) + const range = batched.getGeometryRangeAt(geometryId) + start = range.vertexStart + count = range.vertexCount + } + + const points: THREE.Vector3[] = [] + for (let i = start; i < start + count; i++) { + points.push(new THREE.Vector3().fromBufferAttribute(position, i)) + } + return points +} + +interface PendingAssignment { + sceneObject: MirabufSceneObject + assignment: WheelAssignment +} + +interface HoverHighlight { + mesh: THREE.BatchedMesh + instanceId: number +} + +/** Tint for the part under the cursor. */ +const HOVER_HIGHLIGHT_COLOR = new THREE.Color(2.2, 1.6, 0.2) +/** Default BatchedMesh instance color; used to un-tint. */ +const DEFAULT_INSTANCE_COLOR = new THREE.Color(1, 1, 1) + +/** Reused across picks to avoid reallocating. */ +const raycaster = new THREE.Raycaster() +const ndc = new THREE.Vector2() + +interface PickIndexEntry { + sceneObject: MirabufSceneObject + guid: string +} + +/** Interaction mode: click a wheel's rim to fit a joint axis, using the assembly's grounded part as parent. */ +class WheelAssignmentMode extends WorldSystem { + private _enabled = false + private _pending: PendingAssignment[] = [] + + private _originalInteractionStart: ((i: InteractionStart) => void) | undefined + private _pointerMoveListener: ((e: PointerEvent) => void) | undefined + private _hover: HoverHighlight | undefined + private _latestMousePos: [number, number] | undefined + private _lastProcessedMousePos: [number, number] | undefined + + // Rebuilt on enable and after apply() to avoid rescanning every mesh entry per raycast. + private _candidateBatches: THREE.BatchedMesh[] = [] + private _pickIndex = new Map>() + + private _driveReversed = false + + public get enabled(): boolean { + return this._enabled + } + + public set enabled(enabled: boolean) { + if (this._enabled === enabled) return + this._enabled = enabled + + if (enabled) this.hookInteractionHandlers() + else this.unhookInteractionHandlers() + + EventSystem.dispatch("WheelAssignmentModeToggled", { enabled }) + } + + public get pendingCount(): number { + return this._pending.length + } + + public get driveReversed(): boolean { + return this._driveReversed + } + + public update(_deltaT: number): void { + if (!this._enabled || !this._latestMousePos) return + + const [x, y] = this._latestMousePos + const last = this._lastProcessedMousePos + if (last && last[0] === x && last[1] === y) return + + this._lastProcessedMousePos = this._latestMousePos + this.updateHover(this._latestMousePos) + } + + public destroy(): void { + this.enabled = false + } + + public toggleReverseDrive(): void { + this._driveReversed = !this._driveReversed + + for (const sceneObject of World.sceneRenderer.mirabufSceneObjects.getAll()) { + if (!sceneObject.mechanism.urdfWheelForward) continue + if (!(sceneObject.brain instanceof SynthesisBrain)) continue + + for (const driver of sceneObject.brain.getWheelDrivers()) { + driver.reversed = this._driveReversed + } + } + + EventSystem.dispatch("WheelAssignmentDriveReversedChanged", { reversed: this._driveReversed }) + } + + private hookInteractionHandlers(): void { + const screenHandler = World.sceneRenderer.screenInteractionHandler + this._originalInteractionStart = screenHandler.interactionStart + screenHandler.interactionStart = (interaction: InteractionStart) => this.onInteractionStart(interaction) + + this._pointerMoveListener = (e: PointerEvent) => { + this._latestMousePos = [e.clientX, e.clientY] + } + World.sceneRenderer.renderer.domElement.addEventListener("pointermove", this._pointerMoveListener) + + this.rebuildPickIndex() + } + + private unhookInteractionHandlers(): void { + const screenHandler = World.sceneRenderer.screenInteractionHandler + if (this._originalInteractionStart) screenHandler.interactionStart = this._originalInteractionStart + + if (this._pointerMoveListener) { + World.sceneRenderer.renderer.domElement.removeEventListener("pointermove", this._pointerMoveListener) + this._pointerMoveListener = undefined + } + this._latestMousePos = undefined + this._lastProcessedMousePos = undefined + this.clearHover() + + this._candidateBatches = [] + this._pickIndex = new Map() + } + + /** Flattens scene objects' batches and mesh-entry GUIDs into lookup structures for pickPart(). */ + private rebuildPickIndex(): void { + this._candidateBatches = [] + this._pickIndex = new Map() + + for (const sceneObject of World.sceneRenderer.mirabufSceneObjects.getAll()) { + for (const batch of sceneObject.mirabufInstance.batches) { + this._candidateBatches.push(batch) + } + + for (const [guid, entries] of sceneObject.mirabufInstance.meshes) { + for (const [mesh, instanceId] of entries) { + let byInstance = this._pickIndex.get(mesh) + if (!byInstance) { + byInstance = new Map() + this._pickIndex.set(mesh, byInstance) + } + byInstance.set(instanceId, { sceneObject, guid }) + } + } + } + } + + /** Raycasts the cached candidate batches for the part-instance GUID under the mouse. */ + private pickPart(mousePos: [number, number]): PartPick | undefined { + const camera = World.sceneRenderer.mainCamera + ndc.set((mousePos[0] / window.innerWidth) * 2 - 1, -(mousePos[1] / window.innerHeight) * 2 + 1) + raycaster.setFromCamera(ndc, camera) + + const hits = raycaster.intersectObjects(this._candidateBatches, false) + if (hits.length === 0) return undefined + + const hit = hits[0] + const object = hit.object as THREE.BatchedMesh + const instanceId = (hit as unknown as { batchId?: number }).batchId ?? 0 + + const resolved = this._pickIndex.get(object)?.get(instanceId) + if (!resolved) return undefined + + return { sceneObject: resolved.sceneObject, guid: resolved.guid, object, instanceId } + } + + /** Tints the part under the cursor. */ + private updateHover(mousePos: [number, number]): void { + const pick = this.pickPart(mousePos) + if (!pick) { + this.clearHover() + return + } + + const mesh = pick.object as THREE.BatchedMesh + if (this._hover && this._hover.mesh === mesh && this._hover.instanceId === pick.instanceId) return + + this.clearHover() + mesh.setColorAt(pick.instanceId, HOVER_HIGHLIGHT_COLOR) + this._hover = { mesh, instanceId: pick.instanceId } + } + + private clearHover(): void { + if (!this._hover) return + this._hover.mesh.setColorAt(this._hover.instanceId, DEFAULT_INSTANCE_COLOR) + this._hover = undefined + } + + private onInteractionStart(interaction: InteractionStart): void { + if (interaction.interactionType !== PRIMARY_MOUSE_INTERACTION) { + this._originalInteractionStart?.(interaction) + return + } + + this.handleWheelPick(interaction.position) + } + + private handleWheelPick(mousePos: [number, number]): void { + const pick = this.pickPart(mousePos) + if (!pick) { + globalAddToast("warning", "Wheel Assignment", "Click directly on a part's mesh.") + return + } + + const points = getPartLocalVertices(pick.object, pick.instanceId) + if (!points || points.length === 0) { + globalAddToast("warning", "Wheel Assignment", "Couldn't read this part's geometry.") + return + } + + const localAxisFit = computeWheelAxisFromCircleFit(points) ?? computeWheelAxisFromAABB(points) + if (!localAxisFit) { + globalAddToast("warning", "Wheel Assignment", "Couldn't derive a wheel axis from this part's geometry.") + return + } + + // Assembly-space transform, not the live scene matrix (which bakes in the physics body's world transform). + const assemblySpaceTransform = pick.sceneObject.mirabufInstance.parser.globalTransforms.get(pick.guid)! + const worldAxisFit = transformWheelAxis(localAxisFit, assemblySpaceTransform) + + // Grounded/root part doubles as the parent -- no second click needed. + const groundedInstance = + pick.sceneObject.mirabufInstance.parser.assembly.data!.joints!.jointInstances![GROUNDED_JOINT_ID] + const parentPartGuid = groundedInstance.parts!.nodes!.at(0)!.value! + if (parentPartGuid === pick.guid) { + globalAddToast("warning", "Wheel Assignment", "This part is the assembly's grounded/root part.") + return + } + + this._pending.push({ + sceneObject: pick.sceneObject, + assignment: { wheelPartGuid: pick.guid, parentPartGuid, axisFit: worldAxisFit }, + }) + + EventSystem.dispatch("WheelAssignmentPendingCountChanged", { count: this._pending.length }) + globalAddToast( + "success", + "Wheel Assignment", + `Wheel staged (${this._pending.length} pending). Pick the next wheel, or Apply.` + ) + } + + /** Mutates each affected assembly and fully rebuilds its MirabufSceneObject. */ + public async apply(): Promise { + if (this._pending.length === 0) return + + // Clear before rebuild destroys the hovered mesh's batches. + this.clearHover() + + const bySceneObject = new Map() + for (const { sceneObject, assignment } of this._pending) { + const list = bySceneObject.get(sceneObject) + if (list) list.push(assignment) + else bySceneObject.set(sceneObject, [assignment]) + } + + for (const [sceneObject, assignments] of bySceneObject) { + const assembly = sceneObject.mirabufInstance.parser.assembly + applyWheelAssignments(assembly, assignments) + + const sceneId = sceneObject.id + World.sceneRenderer.removeSceneObject(sceneId) + + const rebuilt = await createMirabuf(assembly.info!.GUID!, assembly) + if (!rebuilt) { + globalAddToast("error", "Wheel Assignment", "Failed to rebuild assembly after applying wheel joints.") + continue + } + World.sceneRenderer.registerSceneObject(rebuilt, sceneId) + + const parser = rebuilt.mirabufInstance.parser + + let hadMismatch = false + for (const assignment of assignments) { + const wheelNode = parser.partToNodeMap.get(assignment.wheelPartGuid) + const parentNode = parser.partToNodeMap.get(assignment.parentPartGuid) + if (!wheelNode || !parentNode) continue + if (wheelNode.id !== parentNode.id) continue + hadMismatch = true + } + + if (hadMismatch) { + globalAddToast("warning", "Wheel Assignment", "Wheel and parent ended up in the same rigid node.") + } + } + + this._pending = [] + EventSystem.dispatch("WheelAssignmentPendingCountChanged", { count: 0 }) + globalAddToast("success", "Wheel Assignment", "Applied wheel joints and rebuilt the affected assembly.") + + // Rebuilt assemblies got new batches/instance ids; refresh the stale pick index. + if (this._enabled) this.rebuildPickIndex() + } +} + +export default WheelAssignmentMode diff --git a/fission/src/systems/simulation/behavior/synthesis/drive/SkidSteerDriveBehavior.ts b/fission/src/systems/simulation/behavior/synthesis/drive/SkidSteerDriveBehavior.ts index bf2b037408..955b3521ba 100644 --- a/fission/src/systems/simulation/behavior/synthesis/drive/SkidSteerDriveBehavior.ts +++ b/fission/src/systems/simulation/behavior/synthesis/drive/SkidSteerDriveBehavior.ts @@ -34,6 +34,7 @@ class SkidSteerDriveBehavior extends DriveBehavior { protected driveSpeeds(leftInput: number, rightInput: number) { const leftDirection = clamp(leftInput, -1, 1) const rightDirection = clamp(rightInput, -1, 1) + this._leftWheels.forEach(wheel => { wheel.accelerationDirection = leftDirection }) diff --git a/fission/src/systems/simulation/driver/WheelDriver.ts b/fission/src/systems/simulation/driver/WheelDriver.ts index 7192d5decf..52ad5484a0 100644 --- a/fission/src/systems/simulation/driver/WheelDriver.ts +++ b/fission/src/systems/simulation/driver/WheelDriver.ts @@ -99,6 +99,14 @@ class WheelDriver extends Driver { return this._constraint } + public get reversed(): boolean { + return this._reversed + } + + public set reversed(value: boolean) { + this._reversed = value + } + public constructor( id: DriverID, constraint: Jolt.VehicleConstraint, diff --git a/fission/src/systems/simulation/synthesis_brain/SynthesisBrain.ts b/fission/src/systems/simulation/synthesis_brain/SynthesisBrain.ts index fde507c359..caf4a4c1ff 100644 --- a/fission/src/systems/simulation/synthesis_brain/SynthesisBrain.ts +++ b/fission/src/systems/simulation/synthesis_brain/SynthesisBrain.ts @@ -79,6 +79,10 @@ class SynthesisBrain extends Brain { return this._brainIndex } + public getWheelDrivers(): WheelDriver[] { + return this._simLayer.drivers.filter(driver => driver instanceof WheelDriver) + } + /** * Applies the requested drive type and returns the drive type actually in effect afterwards. * These can differ when the requested type is swerve but swerve detection fails. diff --git a/fission/src/ui/components/WheelAssignmentDebugPanel.tsx b/fission/src/ui/components/WheelAssignmentDebugPanel.tsx new file mode 100644 index 0000000000..7fd26a4a05 --- /dev/null +++ b/fission/src/ui/components/WheelAssignmentDebugPanel.tsx @@ -0,0 +1,73 @@ +import { Button, Stack } from "@mui/material" +import { useEffect, useState } from "react" +import EventSystem from "@/systems/EventSystem.ts" +import World from "@/systems/World" +import Label from "./Label" + +/** Throwaway dev-only panel for testing manual wheel-joint placement. */ +const WheelAssignmentDebugPanel: React.FC = () => { + const [enabled, setEnabled] = useState(false) + const [pendingCount, setPendingCount] = useState(0) + const [driveReversed, setDriveReversed] = useState(false) + + useEffect(() => { + const unsubToggle = EventSystem.listen("WheelAssignmentModeToggled", ({ enabled }) => setEnabled(enabled)) + const unsubCount = EventSystem.listen("WheelAssignmentPendingCountChanged", ({ count }) => + setPendingCount(count) + ) + const unsubReversed = EventSystem.listen("WheelAssignmentDriveReversedChanged", ({ reversed }) => + setDriveReversed(reversed) + ) + return () => { + unsubToggle() + unsubCount() + unsubReversed() + } + }, []) + + if (!import.meta.env.DEV) return null + + return ( + + + + + + + + + + + ) +} + +export default WheelAssignmentDebugPanel diff --git a/fission/src/urdf/URDFLoader.ts b/fission/src/urdf/URDFLoader.ts index 50a53c7a5a..6bf7428b25 100644 --- a/fission/src/urdf/URDFLoader.ts +++ b/fission/src/urdf/URDFLoader.ts @@ -1,9 +1,9 @@ import JSZip from "jszip" import type { mirabuf } from "@/proto/mirabuf" import { convertURDF } from "./URDFConverter" -import { detectAndTagWheels } from "@/systems/simulation/synthesis_brain/WheelDetector" import { type ProgressHandle, URDFImportProgressBar } from "@/components/ProgressNotificationData.ts" import { yieldToMain } from "@/util/Utility.ts" +import { detectAndTagWheels } from "@/systems/simulation/synthesis_brain/WheelDetector" const MESH_EXTENSIONS = new Set(["stl", "obj", "gltf", "bin"]) diff --git a/fission/src/util/DebugAssemblyDump.ts b/fission/src/util/DebugAssemblyDump.ts new file mode 100644 index 0000000000..254529cd55 --- /dev/null +++ b/fission/src/util/DebugAssemblyDump.ts @@ -0,0 +1,81 @@ +import { mirabuf } from "@/proto/mirabuf" +import { downloadBlob } from "@/util/Utility" + +const TO_OBJECT_OPTIONS = { longs: String, enums: String, bytes: String } + +/** Logs label + value as two separate console.log calls (value and its JSON form) to avoid truncation. */ +function logJson(label: string, value: unknown): void { + console.log(`${label}:`, value) + console.log(`${label} (JSON):`, JSON.stringify(value)) +} + +/** Logs an assembly's joints, design/joint hierarchy, and a lightweight part-instance summary. */ +// @ts-expect-error unused, kept for ad-hoc debugging +// biome-ignore lint/correctness/noUnusedVariables: kept for ad-hoc debugging +function dumpAssemblyStructure(assembly: mirabuf.Assembly, label: string): void { + if (assembly.data?.joints) { + const joints = mirabuf.joint.Joints.toObject(assembly.data.joints as mirabuf.joint.Joints, TO_OBJECT_OPTIONS) + logJson(`${label} -- assembly.data.joints`, joints) + } + + if (assembly.designHierarchy) { + const designHierarchy = mirabuf.GraphContainer.toObject( + assembly.designHierarchy as mirabuf.GraphContainer, + TO_OBJECT_OPTIONS + ) + logJson(`${label} -- assembly.designHierarchy`, designHierarchy) + } + + if (assembly.jointHierarchy) { + const jointHierarchy = mirabuf.GraphContainer.toObject( + assembly.jointHierarchy as mirabuf.GraphContainer, + TO_OBJECT_OPTIONS + ) + logJson(`${label} -- assembly.jointHierarchy`, jointHierarchy) + } + + const partInstances = Object.values(assembly.data?.parts?.partInstances ?? {}).map(inst => ({ + GUID: inst.info?.GUID, + name: inst.info?.name, + partDefinitionReference: inst.partDefinitionReference, + joints: inst.joints, + })) + logJson(`${label} -- part instances (GUID/name/partDefinitionReference/joints only)`, partInstances) +} + +/** Replaces each body's raw mesh vertex/normal/uv/index/color arrays with just their lengths, in place. */ +function stripMeshGeometry(assemblyObj: Record): void { + const defs = (assemblyObj.data as Record | undefined)?.parts as Record | undefined + const partDefinitions = defs?.partDefinitions as Record> | undefined + if (!partDefinitions) return + + for (const def of Object.values(partDefinitions)) { + const bodies = def.bodies as Record[] | undefined + for (const body of bodies ?? []) { + const mesh = (body.triangleMesh as Record | undefined)?.mesh as + | Record + | undefined + if (!mesh) continue + + for (const key of ["verts", "normals", "uv", "indices", "colors"]) { + const arr = mesh[key] + if (Array.isArray(arr)) mesh[key] = `` + } + } + } +} + +/** Serializes the whole assembly to a downloaded .json file, with mesh geometry stripped to array lengths. */ +// @ts-expect-error unused, kept for ad-hoc debugging +// biome-ignore lint/correctness/noUnusedVariables: kept for ad-hoc debugging +function downloadFullAssemblyJson(assembly: mirabuf.Assembly, filename: string): void { + const full = mirabuf.Assembly.toObject(assembly as mirabuf.Assembly, TO_OBJECT_OPTIONS) as Record + stripMeshGeometry(full) + + const resolvedFilename = filename.endsWith(".json") ? filename : `${filename}.json` + try { + downloadBlob(resolvedFilename, JSON.stringify(full, null, 2)) + } catch (error) { + console.error(`[DebugAssemblyDump] Failed to stringify assembly even after stripping mesh geometry:`, error) + } +} diff --git a/fission/src/util/geometry/WheelAxisFit.ts b/fission/src/util/geometry/WheelAxisFit.ts new file mode 100644 index 0000000000..9c1606b053 --- /dev/null +++ b/fission/src/util/geometry/WheelAxisFit.ts @@ -0,0 +1,186 @@ +import * as THREE from "three" + +/** A wheel's rotation axis, pivot origin, radius, and axle-direction width. */ +export interface WheelAxis { + center: THREE.Vector3 + axis: THREE.Vector3 + radius: number + width: number +} + +/** Any unit vector perpendicular to axis. */ +function anyPerpendicular(axis: THREE.Vector3): THREE.Vector3 { + const helper = Math.abs(axis.x) < 0.9 ? new THREE.Vector3(1, 0, 0) : new THREE.Vector3(0, 1, 0) + return helper.cross(axis).normalize() +} + +const LOCAL_AXES = [new THREE.Vector3(1, 0, 0), new THREE.Vector3(0, 1, 0), new THREE.Vector3(0, 0, 1)] + +/** Index of the AABB's smallest extent -- the axle direction. */ +function axleIndexFromAABB(points: THREE.Vector3[]): number { + const bbox = new THREE.Box3().setFromPoints(points) + const size = bbox.getSize(new THREE.Vector3()) + const extents = [size.x, size.y, size.z] + + let axleIndex = 0 + for (let i = 1; i < 3; i++) { + if (extents[i] < extents[axleIndex]) axleIndex = i + } + return axleIndex +} + +/** Derives a wheel's rotation axis and origin from its AABB. Naive baseline; see computeWheelAxisFromCircleFit. */ +export function computeWheelAxisFromAABB(points: THREE.Vector3[]): WheelAxis | undefined { + if (points.length === 0) return undefined + + const bbox = new THREE.Box3().setFromPoints(points) + const center = bbox.getCenter(new THREE.Vector3()) + const size = bbox.getSize(new THREE.Vector3()) + const axleIndex = axleIndexFromAABB(points) + const extents = [size.x, size.y, size.z] + const radialExtents = extents.filter((_, i) => i !== axleIndex) + + return { + center, + axis: LOCAL_AXES[axleIndex].clone(), + radius: Math.max(...radialExtents) / 2, + width: extents[axleIndex], + } +} + +// Minimum points to trust a circle fit. +const MIN_POINTS_FOR_CIRCLE_FIT = 12 + +// Percentile (not max) of radial distance used as outer-envelope radius. +const OUTER_RADIUS_PERCENTILE = 0.95 + +// Fraction of points (by fit residual) dropped before refitting. +const TRIM_FRACTION = 0.15 + +interface Circle2D { + cx: number + cy: number + radius: number +} + +/** Algebraic (Kasa) least-squares circle fit via one 3x3 linear solve. */ +function fitCircle2D(coords: { x: number; y: number }[]): Circle2D | undefined { + if (coords.length < 3) return undefined + + let sx = 0 + let sy = 0 + let sxx = 0 + let syy = 0 + let sxy = 0 + let sxz = 0 + let syz = 0 + let sz = 0 + for (const { x, y } of coords) { + const z = x * x + y * y + sx += x + sy += y + sxx += x * x + syy += y * y + sxy += x * y + sxz += x * z + syz += y * z + sz += z + } + const n = coords.length + + const det3 = ( + m00: number, + m01: number, + m02: number, + m10: number, + m11: number, + m12: number, + m20: number, + m21: number, + m22: number + ) => m00 * (m11 * m22 - m12 * m21) - m01 * (m10 * m22 - m12 * m20) + m02 * (m10 * m21 - m11 * m20) + + // Cramer's rule. + const D = det3(sxx, sxy, sx, sxy, syy, sy, sx, sy, n) + if (Math.abs(D) < 1e-9) return undefined // degenerate + + const Da = det3(sxz, sxy, sx, syz, syy, sy, sz, sy, n) + const Db = det3(sxx, sxz, sx, sxy, syz, sy, sx, sz, n) + const Dc = det3(sxx, sxy, sxz, sxy, syy, syz, sx, sy, sz) + + const a = Da / D / 2 + const b = Db / D / 2 + const c = Dc / D + + const radiusSq = c + a * a + b * b + if (radiusSq <= 0) return undefined + + return { cx: a, cy: b, radius: Math.sqrt(radiusSq) } +} + +/** Derives a wheel's rotation axis, hub origin, and outer radius from a local-space point cloud via a trimmed circle fit. */ +export function computeWheelAxisFromCircleFit(points: THREE.Vector3[]): WheelAxis | undefined { + if (points.length < MIN_POINTS_FOR_CIRCLE_FIT) return undefined + + const axleIndex = axleIndexFromAABB(points) + const axis = LOCAL_AXES[axleIndex].clone() + const [uIndex, vIndex] = [0, 1, 2].filter(i => i !== axleIndex) + const width = new THREE.Box3().setFromPoints(points).getSize(new THREE.Vector3()).getComponent(axleIndex) + + const centroid = points.reduce((sum, p) => sum.add(p), new THREE.Vector3()).divideScalar(points.length) + const toCoord = (p: THREE.Vector3) => ({ + x: p.getComponent(uIndex) - centroid.getComponent(uIndex), + y: p.getComponent(vIndex) - centroid.getComponent(vIndex), + }) + + const coords = points.map(toCoord) + let fit = fitCircle2D(coords) + if (!fit) return undefined + + const residual = (coord: { x: number; y: number }, f: Circle2D) => + Math.abs(Math.hypot(coord.x - f.cx, coord.y - f.cy) - f.radius) + + const keepCount = Math.floor(coords.length * (1 - TRIM_FRACTION)) + if (keepCount >= MIN_POINTS_FOR_CIRCLE_FIT && keepCount < coords.length) { + const trimmedCoords = coords + .map(coord => ({ coord, r: residual(coord, fit!) })) + .sort((a, b) => a.r - b.r) + .slice(0, keepCount) + .map(entry => entry.coord) + + const refit = fitCircle2D(trimmedCoords) + if (refit) fit = refit + } + + const center = centroid.clone() + center.setComponent(uIndex, centroid.getComponent(uIndex) + fit.cx) + center.setComponent(vIndex, centroid.getComponent(vIndex) + fit.cy) + + // Outer envelope of the untrimmed cloud, not the fit's own averaged radius. + const outerDistances = coords.map(coord => Math.hypot(coord.x - fit.cx, coord.y - fit.cy)).sort((a, b) => a - b) + const radius = outerDistances[Math.floor(outerDistances.length * OUTER_RADIUS_PERCENTILE)] + + return { center, axis, radius, width } +} + +/** Transforms a wheel axis into another space; radius/width carried through via offset points for non-uniform scale. */ +export function transformWheelAxis(local: WheelAxis, matrixWorld: THREE.Matrix4): WheelAxis { + const center = local.center.clone().applyMatrix4(matrixWorld) + + const normalMatrix = new THREE.Matrix3().getNormalMatrix(matrixWorld) + const axis = local.axis.clone().applyMatrix3(normalMatrix).normalize() + + const rim = local.center + .clone() + .addScaledVector(anyPerpendicular(local.axis), local.radius) + .applyMatrix4(matrixWorld) + const radius = rim.distanceTo(center) + + const edge = local.center + .clone() + .addScaledVector(local.axis, local.width / 2) + .applyMatrix4(matrixWorld) + const width = edge.distanceTo(center) * 2 + + return { center, axis, radius, width } +}