diff --git a/src/bundles/robot_simulation/controllers/__tests__/ev3/components/Mesh.ts b/src/bundles/robot_simulation/controllers/__tests__/ev3/components/Mesh.ts index ee523870f..082e14986 100644 --- a/src/bundles/robot_simulation/controllers/__tests__/ev3/components/Mesh.ts +++ b/src/bundles/robot_simulation/controllers/__tests__/ev3/components/Mesh.ts @@ -55,6 +55,21 @@ describe('Mesh', () => { getTranslation: jest.fn().mockReturnValue(new THREE.Vector3()), getRotation: jest.fn().mockReturnValue(new THREE.Quaternion()), }), + config: { + orientation: { + position: { + x: 0, + y: 0.0775, + z: 0, + }, + rotation: { + x: 0, + y: 0, + z: 0, + w: 1, + }, + }, + } } as unknown as ChassisWrapper; mockConfig = { url: 'path/to/mesh', @@ -82,11 +97,8 @@ describe('Mesh', () => { it('should update mesh position and orientation according to chassis', async () => { await mesh.start(); - mesh.update(); + mesh.update({residualFactor: 0.5}); - const chassisEntity = mockChassisWrapper.getEntity(); - expect(chassisEntity.getTranslation).toHaveBeenCalled(); - expect(chassisEntity.getRotation).toHaveBeenCalled(); expect(mesh.mesh.scene.position.copy).toHaveBeenCalled(); expect(mesh.mesh.scene.quaternion.copy).toHaveBeenCalled(); }); diff --git a/src/bundles/robot_simulation/controllers/ev3/components/Mesh.ts b/src/bundles/robot_simulation/controllers/ev3/components/Mesh.ts index 25993d4a5..57a193ec8 100644 --- a/src/bundles/robot_simulation/controllers/ev3/components/Mesh.ts +++ b/src/bundles/robot_simulation/controllers/ev3/components/Mesh.ts @@ -1,7 +1,10 @@ +import * as THREE from 'three'; + import { type GLTF } from 'three/examples/jsm/loaders/GLTFLoader.js'; import { type Controller, type Renderer } from '../../../engine'; // eslint-disable-next-line import/extensions -import type { Dimension, SimpleVector } from '../../../engine/Math/Vector'; +import type { Dimension, SimpleQuaternion, SimpleVector } from '../../../engine/Math/Vector'; +import type { PhysicsTimingInfo } from '../../../engine/Physics'; import { loadGLTF } from '../../../engine/Render/helpers/GLTF'; import { type ChassisWrapper } from './Chassis'; @@ -23,6 +26,11 @@ export class Mesh implements Controller { mesh: GLTF | null = null; + previousTranslation: SimpleVector | null= null; + previousRotation: SimpleQuaternion | null = null; + currentTranslation: SimpleVector; + currentRotation: SimpleQuaternion; + constructor( chassisWrapper: ChassisWrapper, render: Renderer, @@ -36,6 +44,8 @@ export class Mesh implements Controller { y: this.config?.offset?.y || 0, z: this.config?.offset?.z || 0, }; + this.currentTranslation = this.chassisWrapper.config.orientation.position; + this.currentRotation = new THREE.Quaternion(0,0,0,1); } async start(): Promise { @@ -44,15 +54,27 @@ export class Mesh implements Controller { this.render.add(this.mesh.scene); } - update() { - const chassisEntity = this.chassisWrapper.getEntity(); - const chassisPosition = chassisEntity.getTranslation(); + fixedUpdate(): void { + this.previousTranslation = this.currentTranslation; + this.previousRotation = this.currentRotation; + this.currentRotation = this.chassisWrapper.getEntity().getRotation(); + this.currentTranslation = this.chassisWrapper.getEntity().getTranslation(); + } + + update(timingInfo: PhysicsTimingInfo) { + const vecCurrentTranslation = new THREE.Vector3().copy(this.currentTranslation); + const vecPreviousTranslation = new THREE.Vector3().copy(this.previousTranslation || this.currentTranslation); + const quatCurrentRotation = new THREE.Quaternion().copy(this.currentRotation); + const quatPreviousRotation = new THREE.Quaternion().copy(this.previousRotation || this.currentRotation); + + const estimatedTranslation = vecPreviousTranslation.lerp(vecCurrentTranslation, timingInfo.residualFactor); + const estimatedRotation = quatPreviousRotation.slerp(quatCurrentRotation, timingInfo.residualFactor); - chassisPosition.x -= this.offset.x / 2; - chassisPosition.y -= this.offset.y / 2; - chassisPosition.z -= this.offset.z / 2; + estimatedTranslation.x -= this.offset.x / 2; + estimatedTranslation.y -= this.offset.y / 2; + estimatedTranslation.z -= this.offset.z / 2; - this.mesh?.scene.position.copy(chassisPosition); - this.mesh?.scene.quaternion.copy(chassisEntity.getRotation()); + this.mesh?.scene.position.copy(estimatedTranslation); + this.mesh?.scene.quaternion.copy(estimatedRotation); } }