fix(lab): overlay full route semantic masks

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

@@ -24,7 +24,14 @@ 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 BRAKE_ATTITUDE_DAMPING = 6;
+const PARKING_BRAKE_HOLD_DECELERATION_MPS2 = 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;
@@ -79,12 +86,20 @@ 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 {
@@ -102,6 +117,7 @@ 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;
@@ -126,6 +142,11 @@ 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: {
@@ -137,7 +158,7 @@ interface PhysicsSystemAccess {
 
 interface NativeRigidBodyAccess {
   rigidbody?: {
-    body: NativeObject | null;
+    body: NativeRigidBody | null;
     linearVelocity: Vec3;
     angularVelocity: Vec3;
     teleport(position: Vec3, rotation?: Vec3 | Quat): void;
@@ -192,6 +213,13 @@ 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;
@@ -206,6 +234,8 @@ 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;
@@ -308,9 +338,27 @@ 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);
@@ -325,13 +373,27 @@ 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(0, index);
+      this.vehicle.setBrake(wheelBrakeForce, index);
       this.vehicle.updateWheelTransform(index, true);
       const transform = this.vehicle.getWheelTransformWS(index);
       const position = transform.getOrigin();
@@ -340,22 +402,21 @@ 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;
@@ -372,9 +433,6 @@ 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;
@@ -387,24 +445,137 @@ export class SimulationUgvController {
       } else if (Math.abs(angularVelocity.y) > maxTurnRate) {
         nextAngularY = Math.sign(angularVelocity.y) * maxTurnRate;
       }
-      if (attitudeDamping !== 1 || nextAngularY !== angularVelocity.y) {
+      if (nextAngularY !== angularVelocity.y) {
         this.limitedAngularVelocity.set(
-          angularVelocity.x * attitudeDamping,
+          angularVelocity.x,
           nextAngularY,
-          angularVelocity.z * attitudeDamping,
+          angularVelocity.z,
         );
         rigidbody.angularVelocity = this.limitedAngularVelocity;
       }
     }
 
-    const body = (this.vehicleEntity as Entity & NativeRigidBodyAccess).rigidbody?.body as {
+    body?.setActivationState(DISABLE_DEACTIVATION);
-      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.");
@@ -464,8 +635,9 @@ export class SimulationUgvController {
       type: "dynamic",
       mass: this.settings.massKg,
       friction: 0.85,
-      linearDamping: 0.08,
+      rollingFriction: 0.12,
-      angularDamping: 0.45,
+      linearDamping: 0.12,
+      angularDamping: 0.6,
     });
 
     this.chassisMaterial = createMaterial(readThemeAccent(), new Color(0.03, 0.04, 0.05));
@@ -525,7 +697,10 @@ export class SimulationUgvController {
       applyMaterial(wheelMesh, this.wheelMaterial);
       anchor.addChild(wheelMesh);
       vehicle.addChild(anchor);
-      this.wheelDefinitions.push({ ...definition, anchor });
+      this.wheelDefinitions.push({
+        ...definition,
+        anchor,
+      });
     }
 
     vehicle.setLocalPosition(this.spawnPosition);
@@ -559,12 +734,14 @@ export class SimulationUgvController {
       wheel.set_m_suspensionStiffness(24);
       wheel.set_m_wheelsDampingRelaxation(3.2);
       wheel.set_m_wheelsDampingCompression(4.8);
-      wheel.set_m_frictionSlip(5.5);
+      wheel.set_m_frictionSlip(TYRE_FRICTION_SLIP);
       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;
@@ -685,9 +862,13 @@ 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,8 +179,33 @@ 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, /horizontalSpeed - deceleration \* Math\.max\(0, deltaSeconds\)/);
+  assert.match(ugv, /PARKING_BRAKE_HOLD_DECELERATION_MPS2 = 6/);
-  assert.match(ugv, /this\.vehicle\.setBrake\(0, index\)/);
+  assert.match(ugv, /PARKING_BRAKE_ENGAGE_SPEED_MPS = 0\.08/);
+  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/);