Update mesh logic

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();
  });

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<void> {
@@ -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);
  }
 }