diff --git a/VR/client/app.js b/VR/client/app.js index 5ded000..8223ad1 100644 --- a/VR/client/app.js +++ b/VR/client/app.js @@ -1,175 +1,4 @@ -import * as THREE from "three"; -import { VRButton } from "three/addons/webxr/VRButton.js"; -import { CHARACTER_MODES_REGISTRY } from "./modes/modes_registry.js"; +import { createApp } from "./app/create_app.js"; -const statusEl = document.getElementById("status"); -const startButton = document.getElementById("start-btn"); -const joinPanel = document.getElementById("join-panel"); -const joinButton = document.getElementById("join-btn"); -const characterModeSelect = document.getElementById("character-mode"); -let selectedCharacterMode = "demo-spline"; - -const searchParams = new URLSearchParams(window.location.search); -const defaultWsScheme = window.location.protocol === "https:" ? "wss" : "ws"; -const defaultWsHost = window.location.hostname || "127.0.0.1"; -const defaultWsPort = searchParams.get("ws_port") ?? "8765"; -const serverHost = - searchParams.get("ws") ?? - `${defaultWsScheme}://${defaultWsHost}:${defaultWsPort}`; - -const calibrationOffset = new THREE.Vector3( - Number(searchParams.get("ox") ?? 0.0), - Number(searchParams.get("oy") ?? 0.0), - Number(searchParams.get("oz") ?? 0.0) -); - -let socket = null; -let userId = null; -let sessionRole = "vr_client"; -let sessionMode = "vr_client"; - -const scene = new THREE.Scene(); -scene.background = new THREE.Color(0x1a1f2b); -const worldRoot = new THREE.Group(); -worldRoot.position.copy(calibrationOffset); -scene.add(worldRoot); - -const camera = new THREE.PerspectiveCamera( - 70, - window.innerWidth / window.innerHeight, - 0.01, - 50 -); -camera.position.set(0, 1.4, 2.0); - -const renderer = new THREE.WebGLRenderer({ antialias: true }); -renderer.setSize(window.innerWidth, window.innerHeight); -renderer.xr.enabled = true; -document.body.appendChild(renderer.domElement); - -const hemi = new THREE.HemisphereLight(0xffffff, 0x223344, 1.2); -scene.add(hemi); - -const grid = new THREE.GridHelper(8, 16, 0x6c757d, 0x495057); -grid.position.y = 0; -worldRoot.add(grid); -const worldAxes = new THREE.AxesHelper(0.3); -worldRoot.add(worldAxes); - -function setStatus(text) { - statusEl.textContent = text; -} - -function applyCharacterMode(modeKey) { - // TODO: Implement character mode selection -} - -function connect(joinConfig) { - socket = new WebSocket(serverHost); - sessionRole = joinConfig.serverRole ?? joinConfig.role; - sessionMode = joinConfig.role; - - socket.onopen = () => { - setStatus(`connected: ${serverHost}`); - socket.send( - JSON.stringify({ - version: 1, - type: "hello", - payload: { - client: "sparc-webxr", - role: sessionRole, - }, - }) - ); - }; - - socket.onclose = (event) => { - setStatus(`disconnected: ${serverHost} (code=${event.code})`); - }; - socket.onerror = () => { - setStatus(`error: failed to connect ${serverHost}`); - }; - - socket.onmessage = (event) => { - // TODO: Implement message handling - - const message = JSON.parse(event.data); - - if (message.type === "error") { - setStatus(`server error: ${message.payload?.reason ?? "unknown"}`); - } - - }; -} - -function sendXRInput(frame) { - // TODO: Implement XR input sending -} - -function animate() { - // TODO: Implement animation loop - let lastTime = performance.now() / 1000; - renderer.setAnimationLoop((_, frame) => { - const now = performance.now() / 1000; - const dt = Math.min(0.05, now - lastTime); - lastTime = now; - - if (frame) sendXRInput(frame); - - renderer.render(scene, camera); - }); -} - -startButton.addEventListener("click", () => { - document.body.appendChild(VRButton.createButton(renderer)); - startButton.remove(); -}); - -window.addEventListener("resize", () => { - camera.aspect = window.innerWidth / window.innerHeight; - camera.updateProjectionMatrix(); - renderer.setSize(window.innerWidth, window.innerHeight); -}); - -/* TODO: Implement key handling for desktop mode */ -window.addEventListener("keydown", (event) => { - const key = event.key.toLowerCase(); - // TODO: Implement key handling -}); - -window.addEventListener("keyup", (event) => { - const key = event.key.toLowerCase(); - // TODO: Implement key handling -}); -/* TODO */ - -renderer.domElement.addEventListener("click", () => { - if (!renderer.xr.isPresenting && document.pointerLockElement !== renderer.domElement) { - renderer.domElement.requestPointerLock(); - } -}); - -document.addEventListener("mousemove", (event) => { - if (document.pointerLockElement !== renderer.domElement) return; - if (renderer.xr.isPresenting) return; - - // TODO: Implement desktop mode mouse movement handling -}); - -function setupJoinPanel() { - // TODO: Implement join panel setup -} - -function populateCharacterModeSelect() { - characterModeSelect.replaceChildren(); - for (const { id, label } of CHARACTER_MODES_REGISTRY) { - const opt = document.createElement("option"); - opt.value = id; - opt.textContent = label; - characterModeSelect.appendChild(opt); - } -} - -populateCharacterModeSelect(); -setupJoinPanel(); -animate(); +const app = createApp({ document, window }); +app.start(); diff --git a/VR/client/app/config.js b/VR/client/app/config.js new file mode 100644 index 0000000..8a75b1a --- /dev/null +++ b/VR/client/app/config.js @@ -0,0 +1,94 @@ +import * as THREE from "three"; + + +export function createWindowConfig({ window }) { + const searchParams = new URLSearchParams(window.location.search); + const defaultWsScheme = window.location.protocol === "https:" ? "wss" : "ws"; + const defaultWsHost = window.location.hostname || "127.0.0.1"; + const defaultWsPort = searchParams.get("ws_port") ?? "8765"; + const serverHost = + searchParams.get("ws") ?? + `${defaultWsScheme}://${defaultWsHost}:${defaultWsPort}`; + + return { + searchParams, + serverHost, + }; +} + +export function createRunConfig({ windowConfig }) { + const calibrationOffset = new THREE.Vector3( + Number(windowConfig.searchParams.get("ox") ?? 0.0), + Number(windowConfig.searchParams.get("oy") ?? 0.0), + Number(windowConfig.searchParams.get("oz") ?? 0.0) + ); + + const demoArmIds = [ + "left_arm", + "right_arm" + ]; + + const armConfig = { + left_arm: { + color: "#ff6b6b", + base: new THREE.Vector3(-0.25, 0.95, -0.35), + hand: "left", + }, + right_arm: { + color: "#74c0fc", + base: new THREE.Vector3(0.25, 0.95, -0.35), + hand: "right", + } + }; + + const characterModes = { + "demo-spline": { + allowsBaseControl: true, + leftBase: new THREE.Vector3(-0.25, 0.95, -0.35), + rightBase: new THREE.Vector3(0.25, 0.95, -0.35), + }, + "two-cr": { + allowsBaseControl: false, + leftBase: new THREE.Vector3(-0.15, 1.0, -0.15), + rightBase: new THREE.Vector3(0.15, 1.0, -0.15), + } + }; + + // Initial director for controller forward direction + const initialControllerForward = new THREE.Vector3(0.0, -1.0, 0.0); + + return { + calibrationOffset, + initialControllerForward, + demoArmIds, + armIds: demoArmIds, + armConfig, + characterModes, + curveSamples: 21, + tipDefaultRadius: 0.045, + maxContactPoints: 6000, + + // Zoom configuration + zoomConfig: { + scale: 1.0, + min: 0.35, + max: 3.0, + step: 0.06, + }, + baseControl: { + deadband: 0.12, + step: 0.012, + minX: -1.5, + maxX: 1.5, + minZ: -2.0, + maxZ: 1.5, + minY: 0.1, + maxY: 2.0, + heightStep: 0.02, + }, + inputSendHz: Math.max(1.0, Number(windowConfig.searchParams.get("input_hz") ?? 30.0)), + defaultDemoArmColors: Object.fromEntries( + Object.entries(armConfig).map(([armId, arm]) => [armId, arm.color]) + ), + }; +} diff --git a/VR/client/app/create_app.js b/VR/client/app/create_app.js new file mode 100644 index 0000000..fb9e0ba --- /dev/null +++ b/VR/client/app/create_app.js @@ -0,0 +1,366 @@ +import * as THREE from "three"; +import { VRButton } from "three/addons/webxr/VRButton.js"; + +import { + createArms, + updateArmPosture, + applyArmSceneState, +} from "../modes/arms.js"; + +import { CHARACTER_MODES_REGISTRY } from "../modes/modes_registry.js"; + +import { createWindowConfig, createRunConfig } from "./config.js"; +import { createAppState } from "./state.js"; + +export function createApp({ document, window }) { + const configWindow = createWindowConfig({ window }); + const configRun = createRunConfig({ windowConfig: configWindow }); + const state = createAppState(); + + const dom = { + statusEl: document.getElementById("status"), + startButton: document.getElementById("start-btn"), + joinPanel: document.getElementById("join-panel"), + joinButton: document.getElementById("join-btn"), + characterModeSelect: document.getElementById("character-mode"), + }; + + /* XR Scene Configuration */ + const scene = new THREE.Scene(); + scene.background = new THREE.Color(0x1a1f2b); + const worldRoot = new THREE.Group(); + worldRoot.position.copy(configRun.calibrationOffset); + scene.add(worldRoot); + + const camera = new THREE.PerspectiveCamera( + 70, + window.innerWidth / window.innerHeight, + 0.01, + 50 + ); + camera.position.set(0, 1.4, 2.0); + + const renderer = new THREE.WebGLRenderer({ antialias: true }); + renderer.setSize(window.innerWidth, window.innerHeight); + renderer.xr.enabled = true; + document.body.appendChild(renderer.domElement); + + const hemi = new THREE.HemisphereLight(0xffffff, 0x223344, 1.2); + scene.add(hemi); + + const grid = new THREE.GridHelper(8, 16, 0x6c757d, 0x495057); + grid.position.y = 0; + worldRoot.add(grid); + const worldAxes = new THREE.AxesHelper(0.3); + worldRoot.add(worldAxes); + /* XR Configuration */ + + const demoArms = createArms({ + armIds: configRun.armIds, + armConfig: configRun.armConfig, + worldRoot, + curveSamples: configRun.curveSamples, + tipDefaultRadius: configRun.tipDefaultRadius, + maxContactPoints: configRun.maxContactPoints, + }); + + function setStatus(text) { + dom.statusEl.textContent = text; + } + + function updateDesktopCamera(dt) { + // Ignore if XR is presenting + if (renderer.xr.isPresenting) return; + + // Keyboard control + const direction = new THREE.Vector3(); + if (state.desktopControls.keys.w) direction.z += 1; + if (state.desktopControls.keys.s) direction.z -= 1; + if (state.desktopControls.keys.a) direction.x -= 1; + if (state.desktopControls.keys.d) direction.x += 1; + if (state.desktopControls.keys.e) direction.y += 1; + if (state.desktopControls.keys.q) direction.y -= 1; + + if (direction.lengthSq() > 0) { + + direction.normalize(); + const forward = new THREE.Vector3(0, 0, -1).applyQuaternion(camera.quaternion); + if (forward.lengthSq() > 0) { + forward.normalize(); + } + const up = new THREE.Vector3(0, 1, 0).applyQuaternion(camera.quaternion).normalize(); + const right = new THREE.Vector3().crossVectors(forward, up).normalize(); + const delta = new THREE.Vector3() + .addScaledVector(forward, direction.z * state.desktopControls.moveSpeed * dt) + .addScaledVector(right, direction.x * state.desktopControls.moveSpeed * dt) + .addScaledVector(up, direction.y * state.desktopControls.moveSpeed * dt); + camera.position.add(delta); + } + } + + function clearRenderedArms() { + for (const demoArm of demoArms.values()) { + demoArm.group.visible = false; + } + } + + function applyCharacterMode(modeKey) { + const mode = configRun.characterModes[modeKey] ?? configRun.characterModes["demo-spline"]; + state.selectedCharacterMode = + modeKey in configRun.characterModes ? modeKey : "demo-spline"; + demoArms.get("left_arm").base.copy(mode.leftBase); + demoArms.get("right_arm").base.copy(mode.rightBase); + clearRenderedArms(); + } + + function connect(joinConfig) { + // Connect to server via WebSocket + state.socket = new WebSocket(configWindow.serverHost); + state.sessionRole = joinConfig.serverRole ?? joinConfig.role; + state.sessionMode = joinConfig.role; + + // Prepare connection + clearRenderedArms(); + + /* Connection event handlers */ + state.socket.onopen = () => { + setStatus(`connected: ${configWindow.serverHost}`); + state.socket.send( + JSON.stringify({ + version: 1, + type: "hello", + payload: { + client: "sparc-webxr", + role: joinConfig.serverRole ?? joinConfig.role, + requested_arm_count: joinConfig.requestedArmCount, + character_mode: joinConfig.characterMode, + }, + }) + ); + }; + + state.socket.onclose = (event) => { + setStatus(`disconnected: ${configWindow.serverHost} (code=${event.code})`); + clearRenderedArms(); + }; + + state.socket.onerror = () => { + setStatus(`error: failed to connect ${configWindow.serverHost}`); + clearRenderedArms(); + }; + + state.socket.onmessage = (event) => { + const message = JSON.parse(event.data); + + // Hello acknowledgement + // User ID, session role, character mode, controlled arms, arm IDs + if (message.type === "hello_ack") { + state.userId = message.payload.user_id ?? null; + state.sessionRole = message.payload.role ?? state.sessionRole; + const resolvedMode = message.payload.character_mode; + if (typeof resolvedMode === "string" && resolvedMode in configRun.characterModes) { + state.selectedCharacterMode = resolvedMode; + dom.characterModeSelect.value = resolvedMode; + applyCharacterMode(resolvedMode); + } + const controlled = message.payload.controlled_arm_ids ?? []; + if (state.sessionMode === "vr_client" && controlled.length >= 2) { + state.controlledArmByHand.left = controlled[0]; + state.controlledArmByHand.right = controlled[1]; + } + state.currentUserRenderArmIds = message.payload.arm_ids ?? []; + state.currentUserArmIds = (state.currentUserRenderArmIds || []).filter( + (armId) => typeof armId !== "string" || !armId.toLowerCase().includes("head") + ); + setStatus( + `connected: ${configWindow.serverHost} | role=${state.sessionRole} | user=${state.userId ?? "unknown"} | arms=${( + message.payload.arm_ids ?? [] + ).length}` + ); + } + + if (message.type === "asset_manifest") { + state.armManifest = message.payload?.arms || {}; + } + + if (message.type === "error") { + setStatus(`server error: ${message.payload?.reason ?? "unknown"}`); + } + + if (message.type === "scene_state") { + applyArmSceneState({ + demoArms, + controlledArmByHand: state.controlledArmByHand, + armIdByDemoKey: null, + armStates: message.payload.arms || {}, + selectedCharacterMode: state.selectedCharacterMode, + forceGenericRod: false, + }); + } + }; + /* Connection event handlers */ + } + + function sendXRInput(frame) { + // Ignore if not in VR client mode + if (state.sessionMode !== "vr_client") return; + // Ignore if not connected to server + if (!state.socket || state.socket.readyState !== WebSocket.OPEN) return; + + // Fetch XR session and reference space + const session = renderer.xr.getSession(); + const referenceSpace = renderer.xr.getReferenceSpace(); + if (!session || !referenceSpace) return; + + // TODO: Implement XR input sending (controller, head pose, action, etc.) + return; + } + + function animate() { + // Heartbeat + let lastTime = performance.now() / 1000; + renderer.setAnimationLoop((_, frame) => { + // Update time + const now = performance.now() / 1000; + const dt = Math.min(0.05, now - lastTime); + lastTime = now; + + // Update desktop camera + updateDesktopCamera(dt); + + // Send XR input + if (frame) sendXRInput(frame); + + // Update arm posture + updateArmPosture({ + demoArms, + armIds: configRun.armIds, + selectedCharacterMode: state.selectedCharacterMode, + forceGenericRod: false, + curveSamples: configRun.curveSamples, + tipDefaultRadius: configRun.tipDefaultRadius, + maxContactPoints: configRun.maxContactPoints, + }); + + // Render scene + renderer.render(scene, camera); + }); + } + + function populateCharacterModeSelect() { + dom.characterModeSelect.replaceChildren(); + for (const { id, label } of CHARACTER_MODES_REGISTRY) { + if (!(id in configRun.characterModes)) { + console.warn( + `modes_registry: "${id}" is listed in CHARACTER_MODES_REGISTRY but missing from characterModes` + ); + continue; + } + const opt = document.createElement("option"); + opt.value = id; + opt.textContent = label; + dom.characterModeSelect.appendChild(opt); + } + } + + function setupJoinPanel() { + const modeFromQuery = configWindow.searchParams.get("mode"); + if (modeFromQuery && modeFromQuery in configRun.characterModes) { + dom.characterModeSelect.value = modeFromQuery; + } + + const refreshModeUI = () => { + const mode = dom.characterModeSelect.value; + applyCharacterMode(mode); + }; + + dom.characterModeSelect.addEventListener("change", refreshModeUI); + refreshModeUI(); + + dom.joinButton.addEventListener("click", () => { + const joinConfig = { + role: "vr_client", + characterMode: dom.characterModeSelect.value, + requestedArmCount: + dom.characterModeSelect.value === "cathy-foraging" ? 8 : 2, + }; + connect(joinConfig); + dom.joinButton.disabled = true; + dom.characterModeSelect.disabled = true; + dom.joinPanel.style.display = "none"; + }); + } + + function setupEvents() { + dom.startButton.addEventListener("click", () => { + document.body.appendChild(VRButton.createButton(renderer)); + dom.startButton.remove(); + }); + + renderer.xr.addEventListener("sessionstart", () => { + connectionIndicator.visible = false; + }); + + window.addEventListener("resize", () => { + camera.aspect = window.innerWidth / window.innerHeight; + camera.updateProjectionMatrix(); + renderer.setSize(window.innerWidth, window.innerHeight); + }); + + window.addEventListener("keydown", (event) => { + const key = event.key.toLowerCase(); + if (key in state.desktopControls.keys) { + state.desktopControls.keys[key] = true; + } + }); + + window.addEventListener("keyup", (event) => { + const key = event.key.toLowerCase(); + if (key in state.desktopControls.keys) { + state.desktopControls.keys[key] = false; + } + }); + + renderer.domElement.addEventListener("click", () => { + if (!renderer.xr.isPresenting && document.pointerLockElement !== renderer.domElement) { + renderer.domElement.requestPointerLock(); + } + }); + + document.addEventListener("mousemove", (event) => { + // Ignore if not in desktop mode + if (document.pointerLockElement !== renderer.domElement) return; + + // Ignore if in XR session + if (renderer.xr.isPresenting) return; + + // Update desktop camera controls based on mouse movement + // ref: https://github.com/mrdoob/three.js/blob/dev/examples/webxr_xr_ballshooter.html + state.desktopControls.yaw -= event.movementX * state.desktopControls.lookSensitivity; + state.desktopControls.pitch -= event.movementY * state.desktopControls.lookSensitivity; + const maxPitch = Math.PI * 0.49; + state.desktopControls.pitch = Math.max( + -maxPitch, + Math.min(maxPitch, state.desktopControls.pitch) + ); + camera.quaternion.setFromEuler( + new THREE.Euler( + state.desktopControls.pitch, + state.desktopControls.yaw, + 0, + "YXZ" + ) + ); + }); + } + + return { + start() { + populateCharacterModeSelect(); + setupJoinPanel(); + setupEvents(); + clearRenderedArms(); + animate(); + }, + }; +} diff --git a/VR/client/app/state.js b/VR/client/app/state.js new file mode 100644 index 0000000..98ecff6 --- /dev/null +++ b/VR/client/app/state.js @@ -0,0 +1,30 @@ +export function createAppState() { + return { + socket: null, + userId: null, + sessionRole: "vr_client", + sessionMode: "vr_client", + + controlledArmByHand: { left: "left_arm", right: "right_arm" }, + selectedCharacterMode: "demo-spline", + armManifest: {}, + currentUserArmIds: [], + currentUserRenderArmIds: [], + + // Desktop camera control + desktopControls: { + moveSpeed: 1.8, + lookSensitivity: 0.0024, + keys: { + w: false, + a: false, + s: false, + d: false, + q: false, + e: false, + }, + yaw: 0.0, + pitch: 0.0, + }, + }; +} diff --git a/VR/client/entities/rod/arm_geometry.js b/VR/client/entities/rod/arm_geometry.js new file mode 100644 index 0000000..63683f7 --- /dev/null +++ b/VR/client/entities/rod/arm_geometry.js @@ -0,0 +1,157 @@ +import * as THREE from "three"; + +const SUPPORTED_REPRESENTATIONS = new Set([ + "segmented-pipe", + "torus-stack", +]); + + +export function renderArmGeometry(demoArm, options = {}) { + const representation = options.builder ?? options.representation; + clearArmBodyGroup(demoArm.armBodyGroup); + + switch (representation) { + case "segmented-pipe": + buildSegmentedPipe(demoArm); + break; + case "torus-stack": + buildArmWithTorusStack(demoArm); + break; + default: + buildSegmentedPipe(demoArm); + } +} + +function clearArmBodyGroup(group) { + while (group.children.length > 0) { + const child = group.children[group.children.length - 1]; + group.remove(child); + disposeObject3D(child); + } +} + +function disposeObject3D(object3d) { + object3d.traverse((child) => { + if (child.geometry) { + child.geometry.dispose(); + } + if (child.material) { + if (Array.isArray(child.material)) { + for (const material of child.material) material.dispose(); + } else { + child.material.dispose(); + } + } + }); +} + +function buildSegmentedPipe(demoArm) { + const points = demoArm.centerline; + const radii = demoArm.radii; + if (!Array.isArray(points) || points.length < 2) return; + if (!Array.isArray(radii) || radii.length < 1) return; + + const material = new THREE.MeshStandardMaterial({ + color: demoArm.color, + roughness: 0.45, + metalness: 0.05, + }); + const yAxis = new THREE.Vector3(0, 1, 0); + + for (let i = 0; i < points.length - 1; i += 1) { + const p0 = points[i]; + const p1 = points[i + 1]; + const segment = p1.clone().sub(p0); + const length = segment.length(); + if (length <= 1e-6) continue; + + const r0 = Math.max(0.001, radii[Math.min(i, radii.length - 1)]); + const r1 = Math.max(0.001, radii[Math.min(i + 1, radii.length - 1)]); + const geom = new THREE.CylinderGeometry(r1, r0, length, 14, 1, true); + const mesh = new THREE.Mesh(geom, material.clone()); + mesh.position.copy(p0.clone().add(p1).multiplyScalar(0.5)); + mesh.quaternion.setFromUnitVectors(yAxis, segment.normalize()); + demoArm.armBodyGroup.add(mesh); + } + + for (let i = 0; i < points.length; i += 1) { + let radius; + if (i === 0) { + radius = radii[0]; + } else if (i === points.length - 1) { + radius = radii[radii.length - 1]; + } else { + const left = radii[Math.max(0, i - 1)]; + const right = radii[Math.min(radii.length - 1, i)]; + radius = 0.5 * (left + right); + } + const sphere = new THREE.Mesh( + new THREE.SphereGeometry(Math.max(0.001, radius), 14, 10), + material.clone() + ); + sphere.position.copy(points[i]); + demoArm.armBodyGroup.add(sphere); + } +} + +function buildArmWithTorusStack(demoArm) { + const points = demoArm.centerline; + const radii = demoArm.radii; + const elementLengths = demoArm.elementLengths; + const directors = demoArm.directors; + if (!Array.isArray(points) || points.length < 2) return; + if (!Array.isArray(radii) || radii.length < 1) return; + + const zAxis = new THREE.Vector3(0, 0, 1); + + for (let i = 0; i < points.length - 1; i += 1) { + const p0 = points[i]; + const p1 = points[i + 1]; + const segment = p1.clone().sub(p0); + const segmentLength = segment.length(); + if (segmentLength <= 1e-6) continue; + + const majorRadius = Math.max(0.001, radii[Math.min(i, radii.length - 1)]); + const rawElementLength = + Array.isArray(elementLengths) && i < elementLengths.length + ? elementLengths[i] + : segmentLength; + const minorRadius = Math.max( + 0.001, + Math.min(majorRadius * 0.9, 0.2 * Math.max(0.0, rawElementLength)) + ); + + const torus = new THREE.Mesh( + new THREE.TorusGeometry(majorRadius, minorRadius, 10, 20), + new THREE.MeshStandardMaterial({ + color: demoArm.color, + roughness: 0.42, + metalness: 0.06, + }) + ); + torus.position.copy(p0.clone().add(p1).multiplyScalar(0.5)); + + const hasDirector = + Array.isArray(directors) && + directors.length === points.length - 1 && + Array.isArray(directors[i]) && + directors[i].length === 3; + if (hasDirector) { + const m = new THREE.Matrix4(); + const rows = directors[i]; + // directors are row-wise [normal, binormal, tangent] in world coords. + // For object rotation, columns must be basis vectors in world => D^T. + m.set( + rows[0][0], rows[1][0], rows[2][0], 0.0, + rows[0][1], rows[1][1], rows[2][1], 0.0, + rows[0][2], rows[1][2], rows[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + torus.quaternion.setFromRotationMatrix(m); + } else { + torus.quaternion.setFromUnitVectors(zAxis, segment.normalize()); + } + + demoArm.armBodyGroup.add(torus); + } +}