diff --git a/src/controls.ts b/src/controls.ts index 0260a24..eb6d6c2 100644 --- a/src/controls.ts +++ b/src/controls.ts @@ -17,6 +17,7 @@ export class URDFControls extends GUI { private _sceneFolder: any; private _jointsFolder: any; private _jointsEditorFolder: any; + private _linksFolder: any; private _workingPath = ''; controls: any = { @@ -28,6 +29,7 @@ export class URDFControls extends GUI { }, joints: {}, lights: {}, + links: {}, editor: {} }; @@ -53,6 +55,9 @@ export class URDFControls extends GUI { this._jointsFolder = this.addFolder('Joints'); this._jointsFolder.domElement.setAttribute('class', 'dg joints-folder'); + this._linksFolder = this.addFolder('Links'); + this._linksFolder.domElement.setAttribute('class', 'dg links-folder'); + this._jointsEditorFolder = this.addFolder('Joints Editor'); this._jointsEditorFolder.domElement.setAttribute( 'class', @@ -97,6 +102,13 @@ export class URDFControls extends GUI { return this._jointsEditorFolder; } + /** + * Retrieves the folder with links settings + */ + get linksFolder() { + return this._linksFolder; + } + /** * Checks if a given object is empty {} * @@ -556,4 +568,66 @@ export class URDFControls extends GUI { return this.controls.editor; } + + /** + * Creates controls for link visualization (frames, opacity) + * + * @param linkNames - Array of available link names + * @returns - The controls to trigger callbacks when link settings change + */ + createLinkControls(linkNames: string[] = []) { + if (this._isEmpty(this.controls.links)) { + const globalLinkSettings = { + 'Axis Indicator': false, + 'Frame Size': 1 + }; + + this.controls.links.axisIndicator = this._linksFolder.add( + globalLinkSettings, + 'Axis Indicator' + ); + + this.controls.links.frameSize = this._linksFolder.add( + globalLinkSettings, + 'Frame Size', + 0.1, + 10, + 0.05 + ); + + this._enforceNumericInput(this.controls.links.frameSize); + + // Individual link controls subfolder + const individualLinksFolder = + this._linksFolder.addFolder('Individual Links'); + this.controls.links.individual = {}; + + // Create controls for each link + linkNames.forEach(linkName => { + const linkFolder = individualLinksFolder.addFolder(linkName); + + const linkSettings = { + 'Show Frame': false, + Opacity: 1.0 + }; + + this.controls.links.individual[linkName] = { + showFrame: linkFolder.add(linkSettings, 'Show Frame'), + opacity: linkFolder.add(linkSettings, 'Opacity', 0.0, 1.0, 0.01) + }; + + // Enforce numeric input for opacity slider + this._enforceNumericInput( + this.controls.links.individual[linkName].opacity + ); + + linkFolder.close(); + }); + + individualLinksFolder.close(); + this._linksFolder.open(); + } + + return this.controls.links; + } } diff --git a/src/layout.ts b/src/layout.ts index 9c022a7..4463a92 100644 --- a/src/layout.ts +++ b/src/layout.ts @@ -185,6 +185,7 @@ export class URDFLayout extends PanelLayout { this._setSceneControls(); this._setJointControls(); this._setLightControls(); + this._setLinkControls(); this._setEditorControls(); } @@ -235,6 +236,53 @@ export class URDFLayout extends PanelLayout { }); } + /** + * Set callbacks for the link controls in the panel. + */ + private _setLinkControls(): void { + // Add link controls with link names + const linkNames = Object.keys(this._loader.robotModel.links); + const linkControls = this._controlsPanel.createLinkControls(linkNames); + + // Axis indicator control + linkControls.axisIndicator.onChange((show: boolean) => { + this._renderer.setAxisIndicatorVisibility(show); + }); + + linkControls.frameSize.onChange((size: number) => { + // Update individual frames with new size + if (linkControls.individual) { + Object.keys(linkControls.individual).forEach(linkName => { + const showIndividual = + linkControls.individual[linkName].showFrame.getValue(); + if (showIndividual) { + this._renderer.setIndividualFrameVisibility(linkName, true, size); + } + }); + } + }); + + // Individual link controls + if (linkControls.individual) { + Object.keys(linkControls.individual).forEach(linkName => { + // Frame visibility + linkControls.individual[linkName].showFrame.onChange( + (show: boolean) => { + const size = linkControls.frameSize.getValue(); + this._renderer.setIndividualFrameVisibility(linkName, show, size); + } + ); + + // Link opacity control + linkControls.individual[linkName].opacity.onChange( + (opacity: number) => { + this._renderer.setLinkOpacity(linkName, opacity); + } + ); + }); + } + } + /** * Set callback for changing directional light position in the controls panel. */ @@ -596,11 +644,11 @@ export class URDFLayout extends PanelLayout { this._interactionEditor.unHighlightLink('parent'); this._selectedLinks.parent = { name: null, obj: null }; } else { - const link = this._loader.robotModel.links[linkName]; - const linkObject = link.children.find((c: any) => c.isURDFVisual) - ?.children[0]; + const linkObject = this._renderer.getLinkObject(linkName); this._selectedLinks.parent = { name: linkName, obj: linkObject }; - this._interactionEditor.highlightLink(linkObject, 'parent'); + if (linkObject) { + this._interactionEditor.highlightLink(linkObject, 'parent'); + } } updateJointName(); }); @@ -615,11 +663,11 @@ export class URDFLayout extends PanelLayout { this._interactionEditor.unHighlightLink('child'); this._selectedLinks.child = { name: null, obj: null }; } else { - const link = this._loader.robotModel.links[linkName]; - const linkObject = link.children.find((c: any) => c.isURDFVisual) - ?.children[0]; + const linkObject = this._renderer.getLinkObject(linkName); this._selectedLinks.child = { name: linkName, obj: linkObject }; - this._interactionEditor.highlightLink(linkObject, 'child'); + if (linkObject) { + this._interactionEditor.highlightLink(linkObject, 'child'); + } } updateJointName(); }); @@ -655,24 +703,25 @@ export class URDFLayout extends PanelLayout { parentLink: string, childLink: string ): void { + this._interactionEditor.clearHighlights(); if (parentLink !== 'none') { - const link = this._loader.robotModel.links[parentLink]; - const linkObject = link?.children.find((c: any) => c.isURDFVisual) - ?.children[0]; + const linkObject = this._renderer.getLinkObject(parentLink); this._selectedLinks.parent = { name: parentLink, obj: linkObject }; if (linkObject) { this._interactionEditor.highlightLink(linkObject, 'parent'); } + } else { + this._selectedLinks.parent = { name: null, obj: null }; } if (childLink !== 'none') { - const link = this._loader.robotModel.links[childLink]; - const linkObject = link?.children.find((c: any) => c.isURDFVisual) - ?.children[0]; + const linkObject = this._renderer.getLinkObject(childLink); this._selectedLinks.child = { name: childLink, obj: linkObject }; if (linkObject) { this._interactionEditor.highlightLink(linkObject, 'child'); } + } else { + this._selectedLinks.child = { name: null, obj: null }; } } diff --git a/src/links/axisIndicator.ts b/src/links/axisIndicator.ts new file mode 100644 index 0000000..2d07e14 --- /dev/null +++ b/src/links/axisIndicator.ts @@ -0,0 +1,49 @@ +import * as THREE from 'three'; +import { ViewHelper } from 'three/examples/jsm/helpers/ViewHelper.js'; + +/** + * A helper class to manage and render an axis indicator using THREE.ViewHelper. + */ +export class AxisIndicatorHelper { + private _viewHelper: ViewHelper; + private _proxyCamera: THREE.Camera; + public visible = false; + + /** + * @param camera The main scene camera to derive orientation from. + * @param domElement The renderer's DOM element for ViewHelper instantiation. + */ + constructor(camera: THREE.Camera, domElement: HTMLElement) { + // Create a dedicated camera for the ViewHelper. + // It will be synchronized with the main camera before rendering. + this._proxyCamera = camera.clone(); + this._viewHelper = new ViewHelper(this._proxyCamera, domElement); + } + + /** + * Renders the axis indicator. + * This should be called in the main render loop, after the main scene has been rendered. + * @param renderer The main WebGLRenderer instance. + * @param mainCamera The main scene camera to sync with. + */ + public render(renderer: THREE.WebGLRenderer, mainCamera: THREE.Camera): void { + if (!this.visible) { + return; + } + + this._proxyCamera.quaternion.copy(mainCamera.quaternion); + const rotationX = new THREE.Quaternion().setFromAxisAngle( + new THREE.Vector3(1, 0, 0), + Math.PI / 2 + ); + this._proxyCamera.quaternion.premultiply(rotationX); + this._viewHelper.render(renderer); + } + + /** + * Disposes of the internal ViewHelper. + */ + public dispose(): void { + this._viewHelper.dispose(); + } +} diff --git a/src/links/linkManager.ts b/src/links/linkManager.ts new file mode 100644 index 0000000..b0e4dba --- /dev/null +++ b/src/links/linkManager.ts @@ -0,0 +1,263 @@ +import * as THREE from 'three'; +import { URDFRobot, URDFVisual } from 'urdf-loader'; + +/** + * Manages the visual representation of links + */ +export class LinkManager { + private _robot: URDFRobot | null = null; + private _frameHelpers: THREE.Group; + private _redrawCallback: () => void; + private _linkToMeshes: Map = new Map(); + private _correctLinkMap: Map = new Map(); + + constructor(scene: THREE.Scene, redrawCallback: () => void) { + this._frameHelpers = new THREE.Group(); + scene.add(this._frameHelpers); + this._redrawCallback = redrawCallback; + } + + /** + * Sets the current robot model, builds a correct map of all links, + * and then maps those links to their meshes. + */ + public setRobot(robot: URDFRobot | null): void { + // If there's an old robot, remove it from the scene completely. + if (this._robot && this._robot.parent) { + this._robot.parent.remove(this._robot); + } + + this._robot = robot; + this._frameHelpers.clear(); + this._linkToMeshes.clear(); + this._correctLinkMap.clear(); + + if (robot) { + this._buildLinkAndMeshMaps(robot); + this.updateAllFramePositions(); + } + } + + /** + * Updates positions of all visible frames to match the current robot pose. + */ + public updateAllFramePositions(): void { + if (!this._robot) { + return; + } + + this._frameHelpers.children.forEach((frameGroup: any) => { + const linkName = frameGroup.userData.linkName; + const link = this._correctLinkMap.get(linkName); + + if (link) { + const worldPosition = new THREE.Vector3(); + const worldQuaternion = new THREE.Quaternion(); + link.matrixWorld.decompose( + worldPosition, + worldQuaternion, + new THREE.Vector3() + ); + frameGroup.position.copy(worldPosition); + frameGroup.quaternion.copy(worldQuaternion); + } + }); + } + + /** + * Shows or hides a coordinate frame for a specific link. + */ + public setIndividualFrameVisibility( + linkName: string, + visible: boolean, + size = 0.3 + ): void { + const existingFrame = this._frameHelpers.children.find( + (child: any) => child.userData.linkName === linkName + ); + if (existingFrame) { + this._frameHelpers.remove(existingFrame); + } + + if (!visible || !this._robot) { + this._redrawCallback(); + return; + } + + const link = this._correctLinkMap.get(linkName); + + if (link) { + const axes = this._createCustomAxesHelper(size); + axes.userData.linkName = linkName; + + const worldPosition = new THREE.Vector3(); + const worldQuaternion = new THREE.Quaternion(); + link.matrixWorld.decompose( + worldPosition, + worldQuaternion, + new THREE.Vector3() + ); + + axes.position.copy(worldPosition); + axes.quaternion.copy(worldQuaternion); + + this._frameHelpers.add(axes); + } + + this._redrawCallback(); + } + + /** + * Sets the opacity of a specific link using our custom mesh mapping. + */ + public setLinkOpacity(linkName: string, opacity: number): void { + const meshes = this._linkToMeshes.get(linkName); + if (!meshes || meshes.length === 0) { + return; + } + + meshes.forEach(mesh => { + const materials = Array.isArray(mesh.material) + ? mesh.material + : [mesh.material]; + + materials.forEach(material => { + if (material) { + if (opacity < 1.0) { + material.transparent = true; + material.depthWrite = false; + } else { + material.transparent = false; + material.depthWrite = true; + } + material.opacity = opacity; + material.needsUpdate = true; + } + }); + }); + + this._redrawCallback(); + } + + /** + * Retrieves the visual object for a given link name. + * @param linkName - The name of the link. + * @returns The THREE.Object3D associated with the link's visual, or null. + */ + public getLinkObject(linkName: string): THREE.Object3D | null { + const meshes = this._linkToMeshes.get(linkName); + // Return the first mesh if it exists, otherwise null. + return meshes && meshes.length > 0 ? meshes[0] : null; + } + + /** + * Disposes of managed resources. + */ + public dispose(): void { + this._frameHelpers.clear(); + this._frameHelpers.parent?.remove(this._frameHelpers); + this._linkToMeshes.clear(); + this._correctLinkMap.clear(); + } + + /** + * This builds the link and mesh maps by using the + * URDF's XML structure as the absolute ground truth. + */ + private _buildLinkAndMeshMaps(robot: URDFRobot): void { + this._correctLinkMap.clear(); + this._linkToMeshes.clear(); + + const rootXml = robot.urdfRobotNode; + if (!rootXml) { + return; + } + + // Step 1: Build a map from the XML Element to the THREE.URDFVisual object + const linkXmlToVisualMap = new Map(); + robot.traverse(node => { + if ((node as any).isURDFVisual) { + const visual = node as URDFVisual; + const visualXml = visual.urdfNode; + // The parent of a tag is its tag. + if (visualXml && visualXml.parentElement) { + linkXmlToVisualMap.set(visualXml.parentElement, visual); + } + } + }); + + // Step 2: Get all tags from the XML + const allLinkElements = rootXml.querySelectorAll('link'); + + // Step 3: Iterate through the list and populate our maps + allLinkElements.forEach(linkElement => { + const linkName = linkElement.getAttribute('name'); + if (!linkName) { + return; + } + + const visual = linkXmlToVisualMap.get(linkElement); + + if (visual) { + const meshes: THREE.Mesh[] = []; + visual.traverse(child => { + if (child instanceof THREE.Mesh) { + meshes.push(child); + } + }); + this._linkToMeshes.set(linkName, meshes); + + // Map the transform object. + if (visual.parent && visual.parent !== robot) { + // Jointed link: the parent is the distinct URDFLink object + this._correctLinkMap.set(linkName, visual.parent); + } else { + // Jointless link: the visual itself is the best object representing the transform + this._correctLinkMap.set(linkName, visual); + } + } else { + // This link has no visual component (like 'world') + this._linkToMeshes.set(linkName, []); + // The root URDFRobot object itself acts as the 'world' link + if (linkName === 'world') { + this._correctLinkMap.set(linkName, robot); + } + } + }); + + // Step 4: Clone materials for all found meshes to ensure uniqueness + for (const meshes of this._linkToMeshes.values()) { + meshes.forEach(mesh => { + if (Array.isArray(mesh.material)) { + mesh.material = mesh.material.map(mat => (mat ? mat.clone() : mat)); + } else if (mesh.material) { + mesh.material = mesh.material.clone(); + } + }); + } + } + + /** + * Creates a custom axes helper + */ + private _createCustomAxesHelper(size = 0.3): THREE.Group { + const axesGroup = new THREE.Group(); + const createArrow = (direction: THREE.Vector3, color: number) => { + return new THREE.ArrowHelper( + direction.normalize(), + new THREE.Vector3(0, 0, 0), + size, + color, + size * 0.2, + size * 0.1 + ); + }; + + const xAxis = createArrow(new THREE.Vector3(1, 0, 0), 0xff0000); + const yAxis = createArrow(new THREE.Vector3(0, 1, 0), 0x00ff00); + const zAxis = createArrow(new THREE.Vector3(0, 0, 1), 0x0000ff); + + axesGroup.add(xAxis, yAxis, zAxis); + return axesGroup; + } +} diff --git a/src/renderer.ts b/src/renderer.ts index 2b4eb43..af11939 100644 --- a/src/renderer.ts +++ b/src/renderer.ts @@ -2,6 +2,9 @@ import * as THREE from 'three'; import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls'; import { CSS2DRenderer } from 'three/examples/jsm/renderers/CSS2DRenderer.js'; +import { AxisIndicatorHelper } from './links/axisIndicator'; +import { LinkManager } from './links/linkManager'; +import { SceneManager } from './scene/sceneManager'; import { URDFRobot } from 'urdf-loader'; @@ -19,16 +22,12 @@ import { URDFRobot } from 'urdf-loader'; * URDFRenderer: a renderer to manage the view of a scene with a robot */ export class URDFRenderer extends THREE.WebGLRenderer { - private _scene: THREE.Scene; + private _sceneManager: SceneManager; private _camera: THREE.PerspectiveCamera; private _controls: OrbitControls; private _css2dRenderer: CSS2DRenderer; - private _colorSky = new THREE.Color(); - private _colorGround = new THREE.Color(); - private _gridHeight = 0; - private _robotIndex = -1; - private _directionalLightHelper: THREE.DirectionalLightHelper | null = null; - private _hemisphereLightHelper: THREE.HemisphereLightHelper | null = null; + private _axisIndicator: AxisIndicatorHelper; + private _linkManager: LinkManager; /** * Creates a renderer to manage the scene elements @@ -42,8 +41,8 @@ export class URDFRenderer extends THREE.WebGLRenderer { ) { super({ antialias: true }); - this._colorSky = colorSky; - this._colorGround = colorGround; + // This is needed to render the axis indicator correctly + this.autoClear = false; this.setClearColor(0xffffff); this.setClearAlpha(0); @@ -51,8 +50,9 @@ export class URDFRenderer extends THREE.WebGLRenderer { this.shadowMap.enabled = true; this.shadowMap.type = THREE.PCFSoftShadowMap; - this._scene = new THREE.Scene(); - this._initScene(); + this._sceneManager = new SceneManager(colorSky, colorGround, () => + this.redraw() + ); this._camera = new THREE.PerspectiveCamera(); this._initCamera(); @@ -65,6 +65,15 @@ export class URDFRenderer extends THREE.WebGLRenderer { this._css2dRenderer.domElement.style.position = 'absolute'; this._css2dRenderer.domElement.style.top = '0px'; this._css2dRenderer.domElement.style.pointerEvents = 'none'; + + // Instantiate the other managers + this._axisIndicator = new AxisIndicatorHelper( + this._camera, + this.domElement + ); + this._linkManager = new LinkManager(this._sceneManager.scene, () => + this.redraw() + ); } /** @@ -89,120 +98,13 @@ export class URDFRenderer extends THREE.WebGLRenderer { this._controls.addEventListener('change', () => this.redraw()); } - /** - * Initializes the scene - */ - private _initScene(): void { - this._scene.background = this._colorSky; - this._scene.up = new THREE.Vector3(0, 0, 1); // Z is up - this._addGround(); - this._addGrid(); - this._addLights(); - } - - /** - * Adds a plane representing the ground to the scene - */ - private _addGround(): void { - // TODO: fix shadows - const ground = new THREE.Mesh( - new THREE.PlaneGeometry(40, 40), - new THREE.ShadowMaterial({ opacity: 0.5 }) - ); - ground.rotation.x = -Math.PI / 2; - ground.scale.setScalar(30); - ground.receiveShadow = true; - this._scene.add(ground); - } - - /** - * Adds a grid to the scene with ground color given to the constructor() - */ - private _addGrid(): void { - const grid = new THREE.GridHelper( - 50, - 50, - this._colorGround, - this._colorGround - ); - grid.receiveShadow = true; - this._scene.add(grid); - } - - /** - * Adds three lights to the scene - */ - private _addLights(): void { - // Directional light - const directionalLight = new THREE.DirectionalLight(0xfff2cc, 1.8); - directionalLight.castShadow = true; - directionalLight.position.set(3, 3, 3); - directionalLight.shadow.camera.top = 5; - directionalLight.shadow.camera.bottom = -5; - directionalLight.shadow.camera.left = -5; - directionalLight.shadow.camera.right = 5; - directionalLight.shadow.camera.near = 0.5; - directionalLight.shadow.camera.far = 50; - this._scene.add(directionalLight); - - // Directional light helper - this._directionalLightHelper = new THREE.DirectionalLightHelper( - directionalLight, - 2, - new THREE.Color(0x000000) - ); - this._directionalLightHelper.visible = false; - this._scene.add(this._directionalLightHelper); - - // Ambient light - const ambientLight = new THREE.AmbientLight(0x404040); - ambientLight.intensity = 0.1; - ambientLight.position.set(0, 5, 0); - this._scene.add(ambientLight); - - // Hemisphere light - const hemisphereLight = new THREE.HemisphereLight( - 0x8888ff, // cool sky - 0x442200, // warm ground - 0.4 - ); - this._scene.add(hemisphereLight); - - // Hemisphere light helper - this._hemisphereLightHelper = new THREE.HemisphereLightHelper( - hemisphereLight, - 2 - ); - this._hemisphereLightHelper.material.color.set(0x000000); - this._hemisphereLightHelper.visible = false; // Set to hidden by default - this._scene.add(this._hemisphereLightHelper); - } - - /** - * Updates the hemisphere light to reflect the sky and ground colors - */ - private _updateLights(): void { - const hemisphereLight = new THREE.HemisphereLight( - this._colorSky, - this._colorGround - ); - hemisphereLight.intensity = 1; - const hemisphereIndex = this._scene.children - .map(i => i.type) - .indexOf('HemisphereLight'); - this._scene.children[hemisphereIndex] = hemisphereLight; - } - /** * Toggle the visibility of the directional light helper * * @param visible - Whether the helper should be visible */ setDirectionalLightHelperVisibility(visible: boolean): void { - if (this._directionalLightHelper) { - this._directionalLightHelper.visible = visible; - this.redraw(); - } + this._sceneManager.setDirectionalLightHelperVisibility(visible); } /** @@ -211,10 +113,7 @@ export class URDFRenderer extends THREE.WebGLRenderer { * @param visible - Whether the helper should be visible */ setHemisphereLightHelperVisibility(visible: boolean): void { - if (this._hemisphereLightHelper) { - this._hemisphereLightHelper.visible = visible; - this.redraw(); - } + this._sceneManager.setHemisphereLightHelperVisibility(visible); } /** @@ -227,22 +126,7 @@ export class URDFRenderer extends THREE.WebGLRenderer { altitude: number, azimuth: number ): void { - const directionalLight = this._scene.children.find( - obj => obj.type === 'DirectionalLight' - ) as THREE.DirectionalLight; - - if (directionalLight) { - const distance = 3; - const x = distance * Math.cos(altitude) * Math.cos(azimuth); - const z = distance * Math.cos(altitude) * Math.sin(azimuth); - const y = distance * Math.sin(altitude); - - directionalLight.position.set(x, y, z); - if (this._directionalLightHelper) { - this._directionalLightHelper.update(); - } - this.redraw(); - } + this._sceneManager.setDirectionalLightPositionSpherical(altitude, azimuth); } /** @@ -251,10 +135,7 @@ export class URDFRenderer extends THREE.WebGLRenderer { * @param newColor - The new background color as [R, G, B] array 0-255 */ setSkyColor(newColor: number[]): void { - this._colorSky = new THREE.Color(...newColor.map(x => x / 255)); - this._scene.background = this._colorSky; - this._updateLights(); - this.redraw(); + this._sceneManager.setSkyColor(newColor); } /** @@ -263,19 +144,7 @@ export class URDFRenderer extends THREE.WebGLRenderer { * @param newColor - The new background color as [R, G, B] array 0-255 */ setGroundColor(newColor: number[]): void { - this._colorGround = new THREE.Color(...newColor.map(x => x / 255)); - const gridIndex = this._scene.children - .map(i => i.type) - .indexOf('GridHelper'); - this._scene.children[gridIndex] = new THREE.GridHelper( - 50, - 50, - this._colorGround, - this._colorGround - ); - this._updateLights(); - this.setGridHeight(this._gridHeight); - this.redraw(); + this._sceneManager.setGroundColor(newColor); } /** @@ -284,12 +153,7 @@ export class URDFRenderer extends THREE.WebGLRenderer { * @param height - The height to shift the grid to */ setGridHeight(height = 0): void { - const gridIndex = this._scene.children - .map(i => i.type) - .indexOf('GridHelper'); - this._scene.children[gridIndex].position.y = height; - this._gridHeight = height; - this.redraw(); + this._sceneManager.setGridHeight(height); } /** @@ -298,19 +162,8 @@ export class URDFRenderer extends THREE.WebGLRenderer { * @param robot */ setRobot(robot: URDFRobot): void { - if (this._robotIndex !== -1) { - this._scene.children[this._robotIndex].traverse(child => { - if (child instanceof THREE.Mesh) { - child.geometry.dispose(); - child.material.dispose(); - } - }); - this._scene.children.splice(this._robotIndex, 1); - } - - this._robotIndex = this._scene.children.length; - this._scene.add(robot); - this.redraw(); + this._sceneManager.setRobot(robot); + this._linkManager.setRobot(robot); } /** @@ -321,17 +174,7 @@ export class URDFRenderer extends THREE.WebGLRenderer { * @param z - The new z position */ setDirectionalLightPosition(x: number, y: number, z: number): void { - const directionalLight = this._scene.children.find( - obj => obj.type === 'DirectionalLight' - ) as THREE.DirectionalLight; - - if (directionalLight) { - directionalLight.position.set(x, y, z); - if (this._directionalLightHelper) { - this._directionalLightHelper.update(); - } - this.redraw(); - } + this._sceneManager.setDirectionalLightPosition(x, y, z); } /** @@ -340,14 +183,7 @@ export class URDFRenderer extends THREE.WebGLRenderer { * @param newColor - The new color as [R, G, B] array 0-255 */ setDirectionalLightColor(newColor: number[]): void { - const directionalLight = this._scene.children.find( - obj => obj.type === 'DirectionalLight' - ) as THREE.DirectionalLight; - - if (directionalLight) { - directionalLight.color = new THREE.Color(...newColor.map(x => x / 255)); - this.redraw(); - } + this._sceneManager.setDirectionalLightColor(newColor); } /** @@ -356,14 +192,7 @@ export class URDFRenderer extends THREE.WebGLRenderer { * @param intensity - The new intensity value */ setDirectionalLightIntensity(intensity: number): void { - const directionalLight = this._scene.children.find( - obj => obj.type === 'DirectionalLight' - ) as THREE.DirectionalLight; - - if (directionalLight) { - directionalLight.intensity = intensity; - this.redraw(); - } + this._sceneManager.setDirectionalLightIntensity(intensity); } /** @@ -372,14 +201,7 @@ export class URDFRenderer extends THREE.WebGLRenderer { * @param newColor - The new color as [R, G, B] array 0-255 */ setAmbientLightColor(newColor: number[]): void { - const ambientLight = this._scene.children.find( - obj => obj.type === 'AmbientLight' - ) as THREE.AmbientLight; - - if (ambientLight) { - ambientLight.color = new THREE.Color(...newColor.map(x => x / 255)); - this.redraw(); - } + this._sceneManager.setAmbientLightColor(newColor); } /** @@ -388,14 +210,7 @@ export class URDFRenderer extends THREE.WebGLRenderer { * @param intensity - The new intensity value */ setAmbientLightIntensity(intensity: number): void { - const ambientLight = this._scene.children.find( - obj => obj.type === 'AmbientLight' - ) as THREE.AmbientLight; - - if (ambientLight) { - ambientLight.intensity = intensity; - this.redraw(); - } + this._sceneManager.setAmbientLightIntensity(intensity); } /** @@ -404,14 +219,7 @@ export class URDFRenderer extends THREE.WebGLRenderer { * @param newColor - The new color as [R, G, B] array 0-255 */ setHemisphereLightSkyColor(newColor: number[]): void { - const hemisphereLight = this._scene.children.find( - obj => obj.type === 'HemisphereLight' - ) as THREE.HemisphereLight; - - if (hemisphereLight) { - hemisphereLight.color = new THREE.Color(...newColor.map(x => x / 255)); - this.redraw(); - } + this._sceneManager.setHemisphereLightSkyColor(newColor); } /** @@ -420,16 +228,7 @@ export class URDFRenderer extends THREE.WebGLRenderer { * @param newColor - The new color as [R, G, B] array 0-255 */ setHemisphereLightGroundColor(newColor: number[]): void { - const hemisphereLight = this._scene.children.find( - obj => obj.type === 'HemisphereLight' - ) as THREE.HemisphereLight; - - if (hemisphereLight) { - hemisphereLight.groundColor = new THREE.Color( - ...newColor.map(x => x / 255) - ); - this.redraw(); - } + this._sceneManager.setHemisphereLightGroundColor(newColor); } /** @@ -438,14 +237,45 @@ export class URDFRenderer extends THREE.WebGLRenderer { * @param intensity - The new intensity value */ setHemisphereLightIntensity(intensity: number): void { - const hemisphereLight = this._scene.children.find( - obj => obj.type === 'HemisphereLight' - ) as THREE.HemisphereLight; + this._sceneManager.setHemisphereLightIntensity(intensity); + } - if (hemisphereLight) { - hemisphereLight.intensity = intensity; - this.redraw(); - } + /** + * Toggle the axis indicator visibility + */ + setAxisIndicatorVisibility(visible: boolean): void { + this._axisIndicator.visible = visible; + this.redraw(); + } + + /** + * Shows or hides coordinate frame for a specific link + */ + setIndividualFrameVisibility( + linkName: string, + visible: boolean, + size = 0.3 + ): void { + this._linkManager.setIndividualFrameVisibility(linkName, visible, size); + } + + /** + * Sets the opacity of a specific link + * + * @param linkName - The name of the link + * @param opacity - Opacity value from 0 (invisible) to 1 (fully opaque) + */ + setLinkOpacity(linkName: string, opacity: number): void { + this._linkManager.setLinkOpacity(linkName, opacity); + } + + /** + * Retrieves the visual object for a given link name. + * @param linkName - The name of the link. + * @returns The THREE.Object3D associated with the link's visual, or null. + */ + getLinkObject(linkName: string): THREE.Object3D | null { + return this._linkManager.getLinkObject(linkName); } /** @@ -455,10 +285,15 @@ export class URDFRenderer extends THREE.WebGLRenderer { const renderSize = this.getSize(new THREE.Vector2()); this._camera.aspect = renderSize.width / renderSize.height; this._camera.updateProjectionMatrix(); - this.render(this._scene, this._camera); + this.clear(); + + this.render(this._sceneManager.scene, this._camera); + if (this._css2dRenderer) { - this._css2dRenderer.render(this._scene, this._camera); + this._css2dRenderer.render(this._sceneManager.scene, this._camera); } + + this._axisIndicator.render(this, this._camera); } /** @@ -482,8 +317,10 @@ export class URDFRenderer extends THREE.WebGLRenderer { } getRobot(): URDFRobot | null { - return this._robotIndex !== -1 - ? (this._scene.children[this._robotIndex] as URDFRobot) - : null; + return this._sceneManager.getRobot(); + } + + dispose(): void { + this._axisIndicator.dispose(); } } diff --git a/src/scene/sceneManager.ts b/src/scene/sceneManager.ts new file mode 100644 index 0000000..fb76334 --- /dev/null +++ b/src/scene/sceneManager.ts @@ -0,0 +1,372 @@ +import * as THREE from 'three'; +import { URDFRobot } from 'urdf-loader'; + +/** + * Manages the THREE.js scene, including lights, ground, grid, and robot model. + */ +export class SceneManager { + public readonly scene: THREE.Scene; + private _colorSky: THREE.Color; + private _colorGround: THREE.Color; + private _gridHeight = 0; + private _robotIndex = -1; + private _directionalLightHelper: THREE.DirectionalLightHelper | null = null; + private _hemisphereLightHelper: THREE.HemisphereLightHelper | null = null; + private _redrawCallback: () => void; + + constructor( + colorSky: THREE.Color, + colorGround: THREE.Color, + redrawCallback: () => void + ) { + this.scene = new THREE.Scene(); + this._colorSky = colorSky; + this._colorGround = colorGround; + this._redrawCallback = redrawCallback; + + this._initScene(); + } + + /** + * Initializes the scene + */ + private _initScene(): void { + this.scene.background = this._colorSky; + this.scene.up = new THREE.Vector3(0, 0, 1); // Z is up + this._addGround(); + this._addGrid(); + this._addLights(); + } + + /** + * Adds a plane representing the ground to the scene + */ + private _addGround(): void { + // TODO: fix shadows + const ground = new THREE.Mesh( + new THREE.PlaneGeometry(40, 40), + new THREE.ShadowMaterial({ opacity: 0.5 }) + ); + ground.rotation.x = -Math.PI / 2; + ground.scale.setScalar(30); + ground.receiveShadow = true; + this.scene.add(ground); + } + + /** + * Adds a grid to the scene with ground color given to the constructor() + */ + private _addGrid(): void { + const grid = new THREE.GridHelper( + 50, + 50, + this._colorGround, + this._colorGround + ); + grid.receiveShadow = true; + this.scene.add(grid); + } + + /** + * Adds three lights to the scene + */ + private _addLights(): void { + // Directional light + const directionalLight = new THREE.DirectionalLight(0xfff2cc, 1.8); + directionalLight.castShadow = true; + directionalLight.position.set(3, 3, 3); + directionalLight.shadow.camera.top = 5; + directionalLight.shadow.camera.bottom = -5; + directionalLight.shadow.camera.left = -5; + directionalLight.shadow.camera.right = 5; + directionalLight.shadow.camera.near = 0.5; + directionalLight.shadow.camera.far = 50; + this.scene.add(directionalLight); + + // Directional light helper + this._directionalLightHelper = new THREE.DirectionalLightHelper( + directionalLight, + 2, + new THREE.Color(0x000000) + ); + this._directionalLightHelper.visible = false; + this.scene.add(this._directionalLightHelper); + + // Ambient light + const ambientLight = new THREE.AmbientLight(0x404040); + ambientLight.intensity = 0.1; + ambientLight.position.set(0, 5, 0); + this.scene.add(ambientLight); + + // Hemisphere light + const hemisphereLight = new THREE.HemisphereLight(0x8888ff, 0x442200, 0.4); + this.scene.add(hemisphereLight); + + // Hemisphere light helper + this._hemisphereLightHelper = new THREE.HemisphereLightHelper( + hemisphereLight, + 2 + ); + this._hemisphereLightHelper.material.color.set(0x000000); + this._hemisphereLightHelper.visible = false; + this.scene.add(this._hemisphereLightHelper); + } + + /** + * Updates the hemisphere light to reflect the sky and ground colors + */ + private _updateLights(): void { + const hemisphereLight = new THREE.HemisphereLight( + this._colorSky, + this._colorGround + ); + hemisphereLight.intensity = 1; + const hemisphereIndex = this.scene.children + .map(i => i.type) + .indexOf('HemisphereLight'); + this.scene.children[hemisphereIndex] = hemisphereLight; + } + + /** + * Toggle the visibility of the directional light helper + * + * @param visible - Whether the helper should be visible + */ + public setDirectionalLightHelperVisibility(visible: boolean): void { + if (this._directionalLightHelper) { + this._directionalLightHelper.visible = visible; + this._redrawCallback(); + } + } + + /** + * Toggle the visibility of the hemisphere light helper + * + * @param visible - Whether the helper should be visible + */ + public setHemisphereLightHelperVisibility(visible: boolean): void { + if (this._hemisphereLightHelper) { + this._hemisphereLightHelper.visible = visible; + this._redrawCallback(); + } + } + + /** + * Updates the position of the directional light using spherical coordinates + * + * @param altitude - Angle in radians from the horizontal plane (elevation) + * @param azimuth - Angle in radians around the vertical axis + */ + public setDirectionalLightPositionSpherical( + altitude: number, + azimuth: number + ): void { + const directionalLight = this.scene.children.find( + obj => obj.type === 'DirectionalLight' + ) as THREE.DirectionalLight; + if (directionalLight) { + const distance = 3; + const x = distance * Math.cos(altitude) * Math.cos(azimuth); + const z = distance * Math.cos(altitude) * Math.sin(azimuth); + const y = distance * Math.sin(altitude); + directionalLight.position.set(x, y, z); + if (this._directionalLightHelper) { + this._directionalLightHelper.update(); + } + this._redrawCallback(); + } + } + + /** + * Change the background color of the scene + * + * @param newColor - The new background color as [R, G, B] array 0-255 + */ + public setSkyColor(newColor: number[]): void { + this._colorSky = new THREE.Color(...newColor.map(x => x / 255)); + this.scene.background = this._colorSky; + this._updateLights(); + this._redrawCallback(); + } + + /** + * Change the grid color of the ground + * + * @param newColor - The new background color as [R, G, B] array 0-255 + */ + public setGroundColor(newColor: number[]): void { + this._colorGround = new THREE.Color(...newColor.map(x => x / 255)); + const gridIndex = this.scene.children + .map(i => i.type) + .indexOf('GridHelper'); + this.scene.children[gridIndex] = new THREE.GridHelper( + 50, + 50, + this._colorGround, + this._colorGround + ); + this._updateLights(); + this.setGridHeight(this._gridHeight); + this._redrawCallback(); + } + + /** + * Changes the height of the grid in the vertical axis (y-axis for three.js) + * + * @param height - The height to shift the grid to + */ + public setGridHeight(height = 0): void { + const gridIndex = this.scene.children + .map(i => i.type) + .indexOf('GridHelper'); + this.scene.children[gridIndex].position.y = height; + this._gridHeight = height; + this._redrawCallback(); + } + + /** + * Adds a robot to the scene or updates the existing robot + * + * @param robot + */ + public setRobot(robot: URDFRobot): void { + if (this._robotIndex !== -1) { + this.scene.children[this._robotIndex].traverse(child => { + if (child instanceof THREE.Mesh) { + child.geometry.dispose(); + child.material.dispose(); + } + }); + this.scene.children.splice(this._robotIndex, 1); + } + this._robotIndex = this.scene.children.length; + this.scene.add(robot); + this._redrawCallback(); + } + + public getRobot(): URDFRobot | null { + return this._robotIndex !== -1 + ? (this.scene.children[this._robotIndex] as URDFRobot) + : null; + } + + public setDirectionalLightPosition(x: number, y: number, z: number): void { + const directionalLight = this.scene.children.find( + obj => obj.type === 'DirectionalLight' + ) as THREE.DirectionalLight; + if (directionalLight) { + directionalLight.position.set(x, y, z); + if (this._directionalLightHelper) { + this._directionalLightHelper.update(); + } + this._redrawCallback(); + } + } + + /** + * Updates the color of the directional light + * + * @param newColor - The new color as [R, G, B] array 0-255 + */ + public setDirectionalLightColor(newColor: number[]): void { + const directionalLight = this.scene.children.find( + obj => obj.type === 'DirectionalLight' + ) as THREE.DirectionalLight; + if (directionalLight) { + directionalLight.color = new THREE.Color(...newColor.map(x => x / 255)); + this._redrawCallback(); + } + } + + /** + * Updates the intensity of the directional light + * + * @param intensity - The new intensity value + */ + public setDirectionalLightIntensity(intensity: number): void { + const directionalLight = this.scene.children.find( + obj => obj.type === 'DirectionalLight' + ) as THREE.DirectionalLight; + if (directionalLight) { + directionalLight.intensity = intensity; + this._redrawCallback(); + } + } + + /** + * Updates the color of the ambient light + * + * @param newColor - The new color as [R, G, B] array 0-255 + */ + public setAmbientLightColor(newColor: number[]): void { + const ambientLight = this.scene.children.find( + obj => obj.type === 'AmbientLight' + ) as THREE.AmbientLight; + if (ambientLight) { + ambientLight.color = new THREE.Color(...newColor.map(x => x / 255)); + this._redrawCallback(); + } + } + + /** + * Updates the intensity of the ambient light + * + * @param intensity - The new intensity value + */ + public setAmbientLightIntensity(intensity: number): void { + const ambientLight = this.scene.children.find( + obj => obj.type === 'AmbientLight' + ) as THREE.AmbientLight; + if (ambientLight) { + ambientLight.intensity = intensity; + this._redrawCallback(); + } + } + + /** + * Updates the hemisphere light sky color + * + * @param newColor - The new color as [R, G, B] array 0-255 + */ + public setHemisphereLightSkyColor(newColor: number[]): void { + const hemisphereLight = this.scene.children.find( + obj => obj.type === 'HemisphereLight' + ) as THREE.HemisphereLight; + if (hemisphereLight) { + hemisphereLight.color = new THREE.Color(...newColor.map(x => x / 255)); + this._redrawCallback(); + } + } + + /** + * Updates the hemisphere light ground color + * + * @param newColor - The new color as [R, G, B] array 0-255 + */ + public setHemisphereLightGroundColor(newColor: number[]): void { + const hemisphereLight = this.scene.children.find( + obj => obj.type === 'HemisphereLight' + ) as THREE.HemisphereLight; + if (hemisphereLight) { + hemisphereLight.groundColor = new THREE.Color( + ...newColor.map(x => x / 255) + ); + this._redrawCallback(); + } + } + + /** + * Updates the hemisphere light intensity + * + * @param intensity - The new intensity value + */ + public setHemisphereLightIntensity(intensity: number): void { + const hemisphereLight = this.scene.children.find( + obj => obj.type === 'HemisphereLight' + ) as THREE.HemisphereLight; + if (hemisphereLight) { + hemisphereLight.intensity = intensity; + this._redrawCallback(); + } + } +}