fix(simulation): hold ugv on sloped collision meshes

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

@@ -29,8 +29,8 @@ 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_BRISTLE_RELAXATION_LENGTH_METERS = 0.018;
+const TYRE_CONTACT_VELOCITY_RESPONSE_PER_SECOND = 10;
-const TYRE_BRISTLE_DAMPING_RATIO = 0.9;
+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;
@@ -90,7 +90,6 @@ interface NativeRaycastInfo extends NativeObject {
   get_m_contactNormalWS(): NativeVector3;
   get_m_contactPointWS(): NativeVector3;
   get_m_wheelAxleWS(): NativeVector3;
-  get_m_isInContact(): boolean;
 }
 
 interface NativeWheelInfo extends NativeObject {
@@ -171,8 +170,6 @@ interface WheelDefinition {
   left: boolean;
   front: boolean;
   anchor: Entity;
-  lateralDeflectionMeters: number;
-  longitudinalDeflectionMeters: number;
 }
 
 let ammoInstancePromise: Promise<AmmoApi> | null = null;
@@ -317,7 +314,6 @@ export class SimulationUgvController {
     rigidbody.linearVelocity = ZERO;
     rigidbody.angularVelocity = ZERO;
     this.vehicle?.resetSuspension();
-    this.resetParkingTyreDeflection();
     this.cameraInitialized = false;
     if (resetCameraOrbit) {
       this.cameraOrbitInitialized = false;
@@ -473,10 +469,7 @@ export class SimulationUgvController {
     if (!this.vehicle || !this.vehicleEntity || !this.tyreImpulseNative
       || !this.tyreRelativePositionNative) return;
 
-    if (!parkingBrakeEngaged) {
+    if (!parkingBrakeEngaged) return;
-      this.resetParkingTyreDeflection();
-      return;
-    }
 
     const timeStep = Math.min(Math.max(0, deltaSeconds), 1 / 30);
     if (timeStep === 0) return;
@@ -485,24 +478,16 @@ export class SimulationUgvController {
     const chassisPosition = this.vehicleEntity.getPosition();
 
     for (let index = 0; index < this.wheelDefinitions.length; index += 1) {
-      const definition = this.wheelDefinitions[index];
       const wheel = this.vehicle.getWheelInfo(index);
       const raycast = wheel.get_m_raycastInfo();
       const normalForce = wheel.get_m_wheelsSuspensionForce();
-      if (!raycast.get_m_isInContact() || !Number.isFinite(normalForce)
+      if (!Number.isFinite(normalForce) || normalForce < MIN_TYRE_NORMAL_FORCE_NEWTONS) {
-        || normalForce < MIN_TYRE_NORMAL_FORCE_NEWTONS) {
-        definition.lateralDeflectionMeters = 0;
-        definition.longitudinalDeflectionMeters = 0;
         continue;
       }
 
       const nativeNormal = raycast.get_m_contactNormalWS();
       this.tyreContactNormal.set(nativeNormal.x(), nativeNormal.y(), nativeNormal.z());
-      if (this.tyreContactNormal.lengthSq() < 0.001) {
+      if (this.tyreContactNormal.lengthSq() < 0.001) continue;
-        definition.lateralDeflectionMeters = 0;
-        definition.longitudinalDeflectionMeters = 0;
-        continue;
-      }
       this.tyreContactNormal.normalize();
 
       const nativeAxle = raycast.get_m_wheelAxleWS();
@@ -511,11 +496,7 @@ export class SimulationUgvController {
         this.tyreContactNormal,
         -this.tyreLateralDirection.dot(this.tyreContactNormal),
       );
-      if (this.tyreLateralDirection.lengthSq() < 0.001) {
+      if (this.tyreLateralDirection.lengthSq() < 0.001) continue;
-        definition.lateralDeflectionMeters = 0;
-        definition.longitudinalDeflectionMeters = 0;
-        continue;
-      }
       this.tyreLateralDirection.normalize();
       this.tyreLongitudinalDirection.cross(
         this.tyreContactNormal,
@@ -542,47 +523,38 @@ export class SimulationUgvController {
       const longitudinalSlipSpeed = this.tyreContactVelocity.dot(
         this.tyreLongitudinalDirection,
       );
-      // A bristle/contact-patch model carries static shear at zero slip and
+      // Static tyre friction is a contact constraint: it balances the component
-      // continuously transitions to kinetic friction when the Coulomb circle is exceeded.
+      // of gravity along the surface and damps slip at the contact patch. The
-      definition.lateralDeflectionMeters += lateralSlipSpeed * timeStep;
+      // force still passes through a Coulomb circle and is applied at the wheel,
-      definition.longitudinalDeflectionMeters += longitudinalSlipSpeed * timeStep;
+      // so the chassis remains a fully dynamic rigid body.
-      const bristleStiffness = normalForce / TYRE_BRISTLE_RELAXATION_LENGTH_METERS;
+      const lateralGravityAcceleration = -GRAVITY_METERS_PER_SECOND_SQUARED
-      const bristleDamping = 2 * TYRE_BRISTLE_DAMPING_RATIO
+        * this.tyreLateralDirection.y;
-        * Math.sqrt(bristleStiffness * wheelEffectiveMass);
+      const longitudinalGravityAcceleration = -GRAVITY_METERS_PER_SECOND_SQUARED
-      const trialLateralForce = -bristleStiffness * definition.lateralDeflectionMeters
+        * this.tyreLongitudinalDirection.y;
-        - bristleDamping * lateralSlipSpeed;
+      const trialLateralForce = -wheelEffectiveMass * (
-      const trialLongitudinalForce = -bristleStiffness
+        lateralGravityAcceleration
-        * definition.longitudinalDeflectionMeters
+        + TYRE_CONTACT_VELOCITY_RESPONSE_PER_SECOND * lateralSlipSpeed
-        - bristleDamping * longitudinalSlipSpeed;
+      );
+      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 deflection = Math.hypot(
-          definition.lateralDeflectionMeters,
-          definition.longitudinalDeflectionMeters,
-        );
-        const lateralDirection = slipSpeed > 0.0001
-          ? lateralSlipSpeed / slipSpeed
-          : deflection > 0
-            ? definition.lateralDeflectionMeters / deflection
-            : 0;
-        const longitudinalDirection = slipSpeed > 0.0001
-          ? longitudinalSlipSpeed / slipSpeed
-          : deflection > 0
-            ? definition.longitudinalDeflectionMeters / deflection
-            : 0;
         const kineticFrictionLimit = TYRE_KINETIC_FRICTION_COEFFICIENT * normalForce;
-        lateralForce = -lateralDirection * kineticFrictionLimit;
+        if (slipSpeed > 0.0001) {
-        longitudinalForce = -longitudinalDirection * kineticFrictionLimit;
+          lateralForce = -(lateralSlipSpeed / slipSpeed) * kineticFrictionLimit;
-        definition.lateralDeflectionMeters = -(
+          longitudinalForce = -(longitudinalSlipSpeed / slipSpeed) * kineticFrictionLimit;
-          lateralForce + bristleDamping * lateralSlipSpeed
+        } else {
-        ) / bristleStiffness;
+          const forceScale = staticFrictionLimit
-        definition.longitudinalDeflectionMeters = -(
+            / Math.hypot(trialLateralForce, trialLongitudinalForce);
-          longitudinalForce + bristleDamping * longitudinalSlipSpeed
+          lateralForce = trialLateralForce * forceScale;
-        ) / bristleStiffness;
+          longitudinalForce = trialLongitudinalForce * forceScale;
+        }
       }
 
       const lateralImpulse = lateralForce * timeStep;
@@ -604,13 +576,6 @@ export class SimulationUgvController {
     }
   }
 
-  private resetParkingTyreDeflection(): void {
-    for (const definition of this.wheelDefinitions) {
-      definition.lateralDeflectionMeters = 0;
-      definition.longitudinalDeflectionMeters = 0;
-    }
-  }
-
   private async createStaticCollisionBodies(collisionWorld: Entity): Promise<void> {
     const models = collisionWorld.findComponents("model") as ModelComponent[];
     if (models.length === 0) throw new Error("В слое коллизий нет геометрии для физики UGV.");
@@ -735,8 +700,6 @@ export class SimulationUgvController {
       this.wheelDefinitions.push({
         ...definition,
         anchor,
-        lateralDeflectionMeters: 0,
-        longitudinalDeflectionMeters: 0,
       });
     }
 

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

@@ -183,7 +183,9 @@ test("PlayCanvas owns the realtime scene graph without an iframe or React entity
   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_BRISTLE_RELAXATION_LENGTH_METERS = 0\.018/);
+  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/);
@@ -192,8 +194,12 @@ test("PlayCanvas owns the realtime scene graph without an iframe or React entity
   assert.match(ugv, /set_m_frictionSlip\(\s*parkingBrakeEngaged \? 0 : TYRE_FRICTION_SLIP/);
   assert.match(ugv, /applyParkingTyreContact/);
   assert.match(ugv, /wheel\.get_m_wheelsSuspensionForce\(\)/);
-  assert.match(ugv, /lateralSlipSpeed \* timeStep/);
+  assert.doesNotMatch(ugv, /get_m_isInContact/);
-  assert.match(ugv, /longitudinalSlipSpeed \* timeStep/);
+  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\)/);