fix(lab): overlay full route semantic masks

apps/control-station/src/components/simulation/SimulationUgvController.ts

@@ -24,14 +24,7 @@ const PHYSICS_PROXY_ERROR = 0.0003;
 const PHYSICS_PROXY_MAX_TRIANGLES = 240_000;
 const SERVICE_BRAKE_DECELERATION_MPS2 = 1.8;
 const COAST_DECELERATION_MPS2 = 0.18;
-const PARKING_BRAKE_HOLD_DECELERATION_MPS2 = 6;
+const BRAKE_ATTITUDE_DAMPING = 6;
-const PARKING_BRAKE_ENGAGE_SPEED_MPS = 0.08;
-const TYRE_FRICTION_SLIP = 8.5;
-const TYRE_STATIC_FRICTION_COEFFICIENT = 0.95;
-const TYRE_KINETIC_FRICTION_COEFFICIENT = 0.78;
-const TYRE_CONTACT_VELOCITY_RESPONSE_PER_SECOND = 10;
-const GRAVITY_METERS_PER_SECOND_SQUARED = 9.81;
-const MIN_TYRE_NORMAL_FORCE_NEWTONS = 1;
 const DEFAULT_ORBIT_PITCH = 0.48;
 const CAMERA_RETURN_DELAY_SECONDS = 1.2;
 const CAMERA_RETURN_DURATION_SECONDS = 2;
@@ -86,20 +79,12 @@ interface NativeTransform extends NativeObject {
   getRotation(): NativeQuaternion;
 }
 
-interface NativeRaycastInfo extends NativeObject {
-  get_m_contactNormalWS(): NativeVector3;
-  get_m_contactPointWS(): NativeVector3;
-  get_m_wheelAxleWS(): NativeVector3;
-}
-
 interface NativeWheelInfo extends NativeObject {
   set_m_suspensionStiffness(value: number): void;
   set_m_wheelsDampingRelaxation(value: number): void;
   set_m_wheelsDampingCompression(value: number): void;
   set_m_frictionSlip(value: number): void;
   set_m_rollInfluence(value: number): void;
-  get_m_wheelsSuspensionForce(): number;
-  get_m_raycastInfo(): NativeRaycastInfo;
 }
 
 interface NativeRaycastVehicle extends NativeObject {
@@ -117,7 +102,6 @@ interface NativeRaycastVehicle extends NativeObject {
   setBrake(force: number, wheel: number): void;
   setSteeringValue(value: number, wheel: number): void;
   getNumWheels(): number;
-  getWheelInfo(wheel: number): NativeWheelInfo;
   updateWheelTransform(wheel: number, interpolated: boolean): void;
   getWheelTransformWS(wheel: number): NativeTransform;
   getForwardVector(): NativeVector3;
@@ -142,11 +126,6 @@ interface NativeDynamicsWorld extends NativeObject {
   removeAction(action: NativeObject): void;
 }
 
-interface NativeRigidBody extends NativeObject {
-  applyImpulse(impulse: NativeVector3, relativePosition: NativeVector3): void;
-  setActivationState(state: number): void;
-}
-
 interface PhysicsSystemAccess {
   systems: {
     rigidbody: {
@@ -158,7 +137,7 @@ interface PhysicsSystemAccess {
 
 interface NativeRigidBodyAccess {
   rigidbody?: {
-    body: NativeRigidBody | null;
+    body: NativeObject | null;
     linearVelocity: Vec3;
     angularVelocity: Vec3;
     teleport(position: Vec3, rotation?: Vec3 | Quat): void;
@@ -213,13 +192,6 @@ export class SimulationUgvController {
   private readonly smoothedCamera = new Vec3();
   private readonly limitedLinearVelocity = new Vec3();
   private readonly limitedAngularVelocity = new Vec3();
-  private readonly tyreContactNormal = new Vec3();
-  private readonly tyreLateralDirection = new Vec3();
-  private readonly tyreLongitudinalDirection = new Vec3();
-  private readonly tyreContactPoint = new Vec3();
-  private readonly tyreRelativePosition = new Vec3();
-  private readonly tyreAngularContactVelocity = new Vec3();
-  private readonly tyreContactVelocity = new Vec3();
   private readonly spawnPosition = UGV_SPAWN_POSITION.clone();
   private orbitPointerId: number | null = null;
   private orbitPointerX = 0;
@@ -234,8 +206,6 @@ export class SimulationUgvController {
   private vehicle: NativeRaycastVehicle | null = null;
   private vehicleTuning: NativeObject | null = null;
   private vehicleRaycaster: NativeObject | null = null;
-  private tyreImpulseNative: NativeVector3 | null = null;
-  private tyreRelativePositionNative: NativeVector3 | null = null;
   private dynamicsWorld: NativeDynamicsWorld | null = null;
   private chassisMaterial: StandardMaterial | null = null;
   private wheelMaterial: StandardMaterial | null = null;
@@ -338,27 +308,9 @@ export class SimulationUgvController {
     const braking = this.pressed.has("Space");
     const rigidbody = (this.vehicleEntity as Entity & NativeRigidBodyAccess).rigidbody;
     const speedMetersPerSecond = this.vehicle.getCurrentSpeedKmHour() / 3.6;
-    const nativeForward = this.vehicle.getForwardVector();
-    const nativeForwardLength = Math.hypot(
-      nativeForward.x(),
-      nativeForward.y(),
-      nativeForward.z(),
-    );
-    const longitudinalSpeedMetersPerSecond = rigidbody && nativeForwardLength > 0.001
-      ? Math.abs(
-        (
-          rigidbody.linearVelocity.x * nativeForward.x()
-          + rigidbody.linearVelocity.y * nativeForward.y()
-          + rigidbody.linearVelocity.z * nativeForward.z()
-        ) / nativeForwardLength,
-      )
-      : Math.abs(speedMetersPerSecond);
     const maxSpeed = this.settings.maxSpeedMetersPerSecond;
     const maxTurnRate = this.settings.maxTurnRateDegrees * Math.PI / 180;
     const pureTurn = !braking && forwardInput === 0 && turnInput !== 0;
-    const holding = !braking && forwardInput === 0 && turnInput === 0;
-    const parkingBrakeEngaged = holding
-      && longitudinalSpeedMetersPerSecond <= PARKING_BRAKE_ENGAGE_SPEED_MPS;
     const desiredSpeed = forwardInput * maxSpeed;
     const speedError = desiredSpeed - speedMetersPerSecond;
     const speedResponseRange = Math.max(0.35, maxSpeed * 0.2);
@@ -373,27 +325,13 @@ export class SimulationUgvController {
     const leftCommand = clamp(forwardCommand - turnCommand, -1, 1);
     const rightCommand = clamp(forwardCommand + turnCommand, -1, 1);
     const engineForce = pureTurn ? pivotForcePerWheel : driveForcePerWheel;
-    const brakeDeceleration = braking
-      ? SERVICE_BRAKE_DECELERATION_MPS2
-      : holding
-        ? parkingBrakeEngaged
-          ? PARKING_BRAKE_HOLD_DECELERATION_MPS2
-          : COAST_DECELERATION_MPS2
-        : 0;
-    const wheelBrakeForce = this.settings.massKg * brakeDeceleration
-      / Math.max(1, this.wheelDefinitions.length);
 
     for (let index = 0; index < this.wheelDefinitions.length; index += 1) {
       const definition = this.wheelDefinitions[index];
       const command = definition.left ? leftCommand : rightCommand;
-      // Parking contact is solved below with one 2D Coulomb limit; disable the
-      // raycast vehicle's parallel friction impulse so grip is not counted twice.
-      this.vehicle.getWheelInfo(index).set_m_frictionSlip(
-        parkingBrakeEngaged ? 0 : TYRE_FRICTION_SLIP,
-      );
       this.vehicle.setSteeringValue(0, index);
       this.vehicle.applyEngineForce(command * engineForce, index);
-      this.vehicle.setBrake(wheelBrakeForce, index);
+      this.vehicle.setBrake(0, index);
       this.vehicle.updateWheelTransform(index, true);
       const transform = this.vehicle.getWheelTransformWS(index);
       const position = transform.getOrigin();
@@ -402,21 +340,22 @@ export class SimulationUgvController {
       definition.anchor.setRotation(rotation.x(), rotation.y(), rotation.z(), rotation.w());
     }
 
-    const body = rigidbody?.body ?? null;
-    if (rigidbody && body) {
-      this.applyParkingTyreContact(
-        deltaSeconds,
-        rigidbody,
-        body,
-        parkingBrakeEngaged,
-      );
-    }
-
     if (rigidbody) {
       const linearVelocity = rigidbody.linearVelocity;
       let nextLinearX = linearVelocity.x;
       let nextLinearZ = linearVelocity.z;
       let horizontalSpeed = Math.hypot(nextLinearX, nextLinearZ);
+      const coasting = !braking && forwardInput === 0 && turnInput === 0;
+      if ((braking || coasting) && horizontalSpeed > 0.001) {
+        const deceleration = braking
+          ? SERVICE_BRAKE_DECELERATION_MPS2
+          : COAST_DECELERATION_MPS2;
+        const nextSpeed = Math.max(0, horizontalSpeed - deceleration * Math.max(0, deltaSeconds));
+        const scale = nextSpeed / horizontalSpeed;
+        nextLinearX *= scale;
+        nextLinearZ *= scale;
+        horizontalSpeed = nextSpeed;
+      }
       if (pureTurn && horizontalSpeed > 0.001) {
         const pivotDamping = Math.exp(-Math.max(0, deltaSeconds) * 8);
         nextLinearX *= pivotDamping;
@@ -433,6 +372,9 @@ export class SimulationUgvController {
         rigidbody.linearVelocity = this.limitedLinearVelocity;
       }
       const angularVelocity = rigidbody.angularVelocity;
+      const attitudeDamping = braking
+        ? Math.exp(-Math.max(0, deltaSeconds) * BRAKE_ATTITUDE_DAMPING)
+        : 1;
       let nextAngularY = angularVelocity.y;
       if (pureTurn) {
         const desiredYawRate = -turnInput * maxTurnRate;
@@ -445,137 +387,24 @@ export class SimulationUgvController {
       } else if (Math.abs(angularVelocity.y) > maxTurnRate) {
         nextAngularY = Math.sign(angularVelocity.y) * maxTurnRate;
       }
-      if (nextAngularY !== angularVelocity.y) {
+      if (attitudeDamping !== 1 || nextAngularY !== angularVelocity.y) {
         this.limitedAngularVelocity.set(
-          angularVelocity.x,
+          angularVelocity.x * attitudeDamping,
           nextAngularY,
-          angularVelocity.z,
+          angularVelocity.z * attitudeDamping,
         );
         rigidbody.angularVelocity = this.limitedAngularVelocity;
       }
     }
 
-    body?.setActivationState(DISABLE_DEACTIVATION);
+    const body = (this.vehicleEntity as Entity & NativeRigidBodyAccess).rigidbody?.body as {
+      setActivationState?: (state: number) => void;
+    } | null;
+    body?.setActivationState?.(DISABLE_DEACTIVATION);
     if (this.vehicleEntity.getPosition().y < -8) this.reset();
     this.updateCamera(deltaSeconds);
   }
 
-  private applyParkingTyreContact(
-    deltaSeconds: number,
-    rigidbody: NonNullable<NativeRigidBodyAccess["rigidbody"]>,
-    body: NativeRigidBody,
-    parkingBrakeEngaged: boolean,
-  ): void {
-    if (!this.vehicle || !this.vehicleEntity || !this.tyreImpulseNative
-      || !this.tyreRelativePositionNative) return;
-
-    if (!parkingBrakeEngaged) return;
-
-    const timeStep = Math.min(Math.max(0, deltaSeconds), 1 / 30);
-    if (timeStep === 0) return;
-    const wheelEffectiveMass = this.settings.massKg
-      / Math.max(1, this.wheelDefinitions.length);
-    const chassisPosition = this.vehicleEntity.getPosition();
-
-    for (let index = 0; index < this.wheelDefinitions.length; index += 1) {
-      const wheel = this.vehicle.getWheelInfo(index);
-      const raycast = wheel.get_m_raycastInfo();
-      const normalForce = wheel.get_m_wheelsSuspensionForce();
-      if (!Number.isFinite(normalForce) || normalForce < MIN_TYRE_NORMAL_FORCE_NEWTONS) {
-        continue;
-      }
-
-      const nativeNormal = raycast.get_m_contactNormalWS();
-      this.tyreContactNormal.set(nativeNormal.x(), nativeNormal.y(), nativeNormal.z());
-      if (this.tyreContactNormal.lengthSq() < 0.001) continue;
-      this.tyreContactNormal.normalize();
-
-      const nativeAxle = raycast.get_m_wheelAxleWS();
-      this.tyreLateralDirection.set(nativeAxle.x(), nativeAxle.y(), nativeAxle.z());
-      this.tyreLateralDirection.addScaled(
-        this.tyreContactNormal,
-        -this.tyreLateralDirection.dot(this.tyreContactNormal),
-      );
-      if (this.tyreLateralDirection.lengthSq() < 0.001) continue;
-      this.tyreLateralDirection.normalize();
-      this.tyreLongitudinalDirection.cross(
-        this.tyreContactNormal,
-        this.tyreLateralDirection,
-      ).normalize();
-
-      const nativeContactPoint = raycast.get_m_contactPointWS();
-      this.tyreContactPoint.set(
-        nativeContactPoint.x(),
-        nativeContactPoint.y(),
-        nativeContactPoint.z(),
-      );
-      this.tyreRelativePosition.sub2(this.tyreContactPoint, chassisPosition);
-      this.tyreAngularContactVelocity.cross(
-        rigidbody.angularVelocity,
-        this.tyreRelativePosition,
-      );
-      this.tyreContactVelocity.add2(
-        rigidbody.linearVelocity,
-        this.tyreAngularContactVelocity,
-      );
-
-      const lateralSlipSpeed = this.tyreContactVelocity.dot(this.tyreLateralDirection);
-      const longitudinalSlipSpeed = this.tyreContactVelocity.dot(
-        this.tyreLongitudinalDirection,
-      );
-      // Static tyre friction is a contact constraint: it balances the component
-      // of gravity along the surface and damps slip at the contact patch. The
-      // force still passes through a Coulomb circle and is applied at the wheel,
-      // so the chassis remains a fully dynamic rigid body.
-      const lateralGravityAcceleration = -GRAVITY_METERS_PER_SECOND_SQUARED
-        * this.tyreLateralDirection.y;
-      const longitudinalGravityAcceleration = -GRAVITY_METERS_PER_SECOND_SQUARED
-        * this.tyreLongitudinalDirection.y;
-      const trialLateralForce = -wheelEffectiveMass * (
-        lateralGravityAcceleration
-        + TYRE_CONTACT_VELOCITY_RESPONSE_PER_SECOND * lateralSlipSpeed
-      );
-      const trialLongitudinalForce = -wheelEffectiveMass * (
-        longitudinalGravityAcceleration
-        + TYRE_CONTACT_VELOCITY_RESPONSE_PER_SECOND * longitudinalSlipSpeed
-      );
-      const staticFrictionLimit = TYRE_STATIC_FRICTION_COEFFICIENT * normalForce;
-      let lateralForce = trialLateralForce;
-      let longitudinalForce = trialLongitudinalForce;
-
-      if (Math.hypot(trialLateralForce, trialLongitudinalForce) > staticFrictionLimit) {
-        const slipSpeed = Math.hypot(lateralSlipSpeed, longitudinalSlipSpeed);
-        const kineticFrictionLimit = TYRE_KINETIC_FRICTION_COEFFICIENT * normalForce;
-        if (slipSpeed > 0.0001) {
-          lateralForce = -(lateralSlipSpeed / slipSpeed) * kineticFrictionLimit;
-          longitudinalForce = -(longitudinalSlipSpeed / slipSpeed) * kineticFrictionLimit;
-        } else {
-          const forceScale = staticFrictionLimit
-            / Math.hypot(trialLateralForce, trialLongitudinalForce);
-          lateralForce = trialLateralForce * forceScale;
-          longitudinalForce = trialLongitudinalForce * forceScale;
-        }
-      }
-
-      const lateralImpulse = lateralForce * timeStep;
-      const longitudinalImpulse = longitudinalForce * timeStep;
-      this.tyreImpulseNative.setValue(
-        this.tyreLateralDirection.x * lateralImpulse
-          + this.tyreLongitudinalDirection.x * longitudinalImpulse,
-        this.tyreLateralDirection.y * lateralImpulse
-          + this.tyreLongitudinalDirection.y * longitudinalImpulse,
-        this.tyreLateralDirection.z * lateralImpulse
-          + this.tyreLongitudinalDirection.z * longitudinalImpulse,
-      );
-      this.tyreRelativePositionNative.setValue(
-        this.tyreRelativePosition.x,
-        this.tyreRelativePosition.y,
-        this.tyreRelativePosition.z,
-      );
-      body.applyImpulse(this.tyreImpulseNative, this.tyreRelativePositionNative);
-    }
-  }
-
   private async createStaticCollisionBodies(collisionWorld: Entity): Promise<void> {
     const models = collisionWorld.findComponents("model") as ModelComponent[];
     if (models.length === 0) throw new Error("В слое коллизий нет геометрии для физики UGV.");
@@ -635,9 +464,8 @@ export class SimulationUgvController {
       type: "dynamic",
       mass: this.settings.massKg,
       friction: 0.85,
-      rollingFriction: 0.12,
+      linearDamping: 0.08,
-      linearDamping: 0.12,
+      angularDamping: 0.45,
-      angularDamping: 0.6,
     });
 
     this.chassisMaterial = createMaterial(readThemeAccent(), new Color(0.03, 0.04, 0.05));
@@ -697,10 +525,7 @@ export class SimulationUgvController {
       applyMaterial(wheelMesh, this.wheelMaterial);
       anchor.addChild(wheelMesh);
       vehicle.addChild(anchor);
-      this.wheelDefinitions.push({
+      this.wheelDefinitions.push({ ...definition, anchor });
-        ...definition,
-        anchor,
-      });
     }
 
     vehicle.setLocalPosition(this.spawnPosition);
@@ -734,14 +559,12 @@ export class SimulationUgvController {
       wheel.set_m_suspensionStiffness(24);
       wheel.set_m_wheelsDampingRelaxation(3.2);
       wheel.set_m_wheelsDampingCompression(4.8);
-      wheel.set_m_frictionSlip(TYRE_FRICTION_SLIP);
+      wheel.set_m_frictionSlip(5.5);
       wheel.set_m_rollInfluence(0.08);
     }
     this.ammo.destroy(axle);
     this.ammo.destroy(direction);
     this.ammo.destroy(connection);
-    this.tyreImpulseNative = new this.ammo.btVector3(0, 0, 0);
-    this.tyreRelativePositionNative = new this.ammo.btVector3(0, 0, 0);
 
     dynamicsWorld.addAction(nativeVehicle);
     this.vehicleEntity = vehicle;
@@ -862,13 +685,9 @@ export class SimulationUgvController {
     if (this.vehicle) runCleanup("destroy vehicle", () => this.ammo.destroy(this.vehicle as NativeObject));
     if (this.vehicleRaycaster) runCleanup("destroy vehicle raycaster", () => this.ammo.destroy(this.vehicleRaycaster as NativeObject));
     if (this.vehicleTuning) runCleanup("destroy vehicle tuning", () => this.ammo.destroy(this.vehicleTuning as NativeObject));
-    if (this.tyreImpulseNative) runCleanup("destroy tyre impulse vector", () => this.ammo.destroy(this.tyreImpulseNative as NativeObject));
-    if (this.tyreRelativePositionNative) runCleanup("destroy tyre relative-position vector", () => this.ammo.destroy(this.tyreRelativePositionNative as NativeObject));
     this.vehicle = null;
     this.vehicleRaycaster = null;
     this.vehicleTuning = null;
-    this.tyreImpulseNative = null;
-    this.tyreRelativePositionNative = null;
     this.dynamicsWorld = null;
 
     if (this.vehicleEntity) runCleanup("destroy vehicle entity", () => this.vehicleEntity?.destroy());

apps/control-station/test/simulationWorkspace.test.mjs

@@ -179,33 +179,8 @@ test("PlayCanvas owns the realtime scene graph without an iframe or React entity
   assert.match(ugv, /desiredSpeed = forwardInput \* maxSpeed/);
   assert.match(ugv, /maximumAcceleration = clamp\(1\.4 \+ maxSpeed \* 0\.35, 1\.8, 4\.2\)/);
   assert.match(ugv, /SERVICE_BRAKE_DECELERATION_MPS2 = 1\.8/);
-  assert.match(ugv, /PARKING_BRAKE_HOLD_DECELERATION_MPS2 = 6/);
+  assert.match(ugv, /horizontalSpeed - deceleration \* Math\.max\(0, deltaSeconds\)/);
-  assert.match(ugv, /PARKING_BRAKE_ENGAGE_SPEED_MPS = 0\.08/);
+  assert.match(ugv, /this\.vehicle\.setBrake\(0, index\)/);
-  assert.match(ugv, /TYRE_FRICTION_SLIP = 8\.5/);
-  assert.match(ugv, /TYRE_STATIC_FRICTION_COEFFICIENT = 0\.95/);
-  assert.match(ugv, /TYRE_KINETIC_FRICTION_COEFFICIENT = 0\.78/);
-  assert.match(ugv, /TYRE_CONTACT_VELOCITY_RESPONSE_PER_SECOND = 10/);
-  assert.match(ugv, /GRAVITY_METERS_PER_SECOND_SQUARED = 9\.81/);
-  assert.match(ugv, /holding = !braking && forwardInput === 0 && turnInput === 0/);
-  assert.match(ugv, /longitudinalSpeedMetersPerSecond/);
-  assert.match(ugv, /parkingBrakeEngaged = holding/);
-  assert.match(ugv, /this\.settings\.massKg \* brakeDeceleration/);
-  assert.match(ugv, /this\.vehicle\.setBrake\(wheelBrakeForce, index\)/);
-  assert.match(ugv, /set_m_frictionSlip\(\s*parkingBrakeEngaged \? 0 : TYRE_FRICTION_SLIP/);
-  assert.match(ugv, /applyParkingTyreContact/);
-  assert.match(ugv, /wheel\.get_m_wheelsSuspensionForce\(\)/);
-  assert.doesNotMatch(ugv, /get_m_isInContact/);
-  assert.match(ugv, /normalForce < MIN_TYRE_NORMAL_FORCE_NEWTONS/);
-  assert.match(ugv, /lateralGravityAcceleration/);
-  assert.match(ugv, /longitudinalGravityAcceleration/);
-  assert.match(ugv, /TYRE_CONTACT_VELOCITY_RESPONSE_PER_SECOND \* lateralSlipSpeed/);
-  assert.match(ugv, /TYRE_CONTACT_VELOCITY_RESPONSE_PER_SECOND \* longitudinalSlipSpeed/);
-  assert.match(ugv, /Math\.hypot\(trialLateralForce, trialLongitudinalForce\)/);
-  assert.match(ugv, /body\.applyImpulse\(this\.tyreImpulseNative, this\.tyreRelativePositionNative\)/);
-  assert.doesNotMatch(ugv, /horizontalSpeed - deceleration \* Math\.max\(0, deltaSeconds\)/);
-  assert.match(ugv, /rollingFriction: 0\.12/);
-  assert.match(ugv, /angularDamping: 0\.6/);
-  assert.match(ugv, /wheel\.set_m_frictionSlip\(TYRE_FRICTION_SLIP\)/);
   assert.doesNotMatch(ugv, /massKg \* 3/);
   assert.match(ugv, /pureTurn = !braking && forwardInput === 0 && turnInput !== 0/);
   assert.match(ugv, /desiredYawRate = -turnInput \* maxTurnRate/);