fix(simulation): model parked UGV tyre grip

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

@@ -27,6 +27,11 @@ const COAST_DECELERATION_MPS2 = 0.18;
 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_BRISTLE_RELAXATION_LENGTH_METERS = 0.018;
+const TYRE_BRISTLE_DAMPING_RATIO = 0.9;
+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;
@@ -81,12 +86,21 @@ interface NativeTransform extends NativeObject {
   getRotation(): NativeQuaternion;
 }
 
+interface NativeRaycastInfo extends NativeObject {
+  get_m_contactNormalWS(): NativeVector3;
+  get_m_contactPointWS(): NativeVector3;
+  get_m_wheelAxleWS(): NativeVector3;
+  get_m_isInContact(): boolean;
+}
+
 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 {
@@ -104,6 +118,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;
@@ -128,6 +143,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: {
@@ -139,7 +159,7 @@ interface PhysicsSystemAccess {
 
 interface NativeRigidBodyAccess {
   rigidbody?: {
-    body: NativeObject | null;
+    body: NativeRigidBody | null;
     linearVelocity: Vec3;
     angularVelocity: Vec3;
     teleport(position: Vec3, rotation?: Vec3 | Quat): void;
@@ -151,6 +171,8 @@ interface WheelDefinition {
   left: boolean;
   front: boolean;
   anchor: Entity;
+  lateralDeflectionMeters: number;
+  longitudinalDeflectionMeters: number;
 }
 
 let ammoInstancePromise: Promise<AmmoApi> | null = null;
@@ -194,6 +216,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;
@@ -208,6 +237,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;
@@ -286,6 +317,7 @@ export class SimulationUgvController {
     rigidbody.linearVelocity = ZERO;
     rigidbody.angularVelocity = ZERO;
     this.vehicle?.resetSuspension();
+    this.resetParkingTyreDeflection();
     this.cameraInitialized = false;
     if (resetCameraOrbit) {
       this.cameraOrbitInitialized = false;
@@ -310,10 +342,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);
@@ -331,7 +380,7 @@ export class SimulationUgvController {
     const brakeDeceleration = braking
       ? SERVICE_BRAKE_DECELERATION_MPS2
       : holding
-        ? Math.abs(speedMetersPerSecond) <= PARKING_BRAKE_ENGAGE_SPEED_MPS
+        ? parkingBrakeEngaged
           ? PARKING_BRAKE_HOLD_DECELERATION_MPS2
           : COAST_DECELERATION_MPS2
         : 0;
@@ -341,6 +390,11 @@ export class SimulationUgvController {
     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);
@@ -352,6 +406,16 @@ 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;
@@ -395,14 +459,158 @@ export class SimulationUgvController {
       }
     }
 
-    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) {
+      this.resetParkingTyreDeflection();
+      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 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)
+        || 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) {
+        definition.lateralDeflectionMeters = 0;
+        definition.longitudinalDeflectionMeters = 0;
+        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) {
+        definition.lateralDeflectionMeters = 0;
+        definition.longitudinalDeflectionMeters = 0;
+        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,
+      );
+      // A bristle/contact-patch model carries static shear at zero slip and
+      // continuously transitions to kinetic friction when the Coulomb circle is exceeded.
+      definition.lateralDeflectionMeters += lateralSlipSpeed * timeStep;
+      definition.longitudinalDeflectionMeters += longitudinalSlipSpeed * timeStep;
+      const bristleStiffness = normalForce / TYRE_BRISTLE_RELAXATION_LENGTH_METERS;
+      const bristleDamping = 2 * TYRE_BRISTLE_DAMPING_RATIO
+        * Math.sqrt(bristleStiffness * wheelEffectiveMass);
+      const trialLateralForce = -bristleStiffness * definition.lateralDeflectionMeters
+        - bristleDamping * lateralSlipSpeed;
+      const trialLongitudinalForce = -bristleStiffness
+        * definition.longitudinalDeflectionMeters
+        - bristleDamping * 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;
+        longitudinalForce = -longitudinalDirection * kineticFrictionLimit;
+        definition.lateralDeflectionMeters = -(
+          lateralForce + bristleDamping * lateralSlipSpeed
+        ) / bristleStiffness;
+        definition.longitudinalDeflectionMeters = -(
+          longitudinalForce + bristleDamping * longitudinalSlipSpeed
+        ) / bristleStiffness;
+      }
+
+      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 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.");
@@ -524,7 +732,12 @@ export class SimulationUgvController {
       applyMaterial(wheelMesh, this.wheelMaterial);
       anchor.addChild(wheelMesh);
       vehicle.addChild(anchor);
-      this.wheelDefinitions.push({ ...definition, anchor });
+      this.wheelDefinitions.push({
+        ...definition,
+        anchor,
+        lateralDeflectionMeters: 0,
+        longitudinalDeflectionMeters: 0,
+      });
     }
 
     vehicle.setLocalPosition(this.spawnPosition);
@@ -564,6 +777,8 @@ export class SimulationUgvController {
     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;
@@ -684,9 +899,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

@@ -182,9 +182,20 @@ test("PlayCanvas owns the realtime scene graph without an iframe or React entity
   assert.match(ugv, /PARKING_BRAKE_HOLD_DECELERATION_MPS2 = 6/);
   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, /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.match(ugv, /lateralSlipSpeed \* timeStep/);
+  assert.match(ugv, /longitudinalSlipSpeed \* timeStep/);
+  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/);