From 57dacc5a0416c913f7be7a399a05a4bcda71b04f Mon Sep 17 00:00:00 2001 From: DCCONSTRUCTIONS Date: Sat, 29 Aug 2026 12:03:33 +0300 Subject: [PATCH] fix(simulation): model parked UGV tyre grip --- .../simulation/SimulationUgvController.ts | 233 +++++++++++++++++- .../test/simulationWorkspace.test.mjs | 11 + 2 files changed, 237 insertions(+), 7 deletions(-) diff --git a/apps/control-station/src/components/simulation/SimulationUgvController.ts b/apps/control-station/src/components/simulation/SimulationUgvController.ts index e3cf811..adaca6f 100644 --- a/apps/control-station/src/components/simulation/SimulationUgvController.ts +++ b/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 | 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 { - setActivationState?: (state: number) => void; - } | null; - body?.setActivationState?.(DISABLE_DEACTIVATION); + body?.setActivationState(DISABLE_DEACTIVATION); if (this.vehicleEntity.getPosition().y < -8) this.reset(); this.updateCamera(deltaSeconds); } + private applyParkingTyreContact( + deltaSeconds: number, + rigidbody: NonNullable, + 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 { 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()); diff --git a/apps/control-station/test/simulationWorkspace.test.mjs b/apps/control-station/test/simulationWorkspace.test.mjs index a671589..9781f38 100644 --- a/apps/control-station/test/simulationWorkspace.test.mjs +++ b/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/);