diff --git a/apps/control-station/src/components/simulation/SimulationUgvController.ts b/apps/control-station/src/components/simulation/SimulationUgvController.ts index adaca6f..9ba310d 100644 --- a/apps/control-station/src/components/simulation/SimulationUgvController.ts +++ b/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_BRISTLE_DAMPING_RATIO = 0.9; +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; @@ -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 | 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) { - this.resetParkingTyreDeflection(); - return; - } + if (!parkingBrakeEngaged) 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) - || normalForce < MIN_TYRE_NORMAL_FORCE_NEWTONS) { - definition.lateralDeflectionMeters = 0; - definition.longitudinalDeflectionMeters = 0; + 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) { - definition.lateralDeflectionMeters = 0; - definition.longitudinalDeflectionMeters = 0; - continue; - } + if (this.tyreContactNormal.lengthSq() < 0.001) 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) { - definition.lateralDeflectionMeters = 0; - definition.longitudinalDeflectionMeters = 0; - continue; - } + if (this.tyreLateralDirection.lengthSq() < 0.001) 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 - // 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; + // 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 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; + 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; @@ -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 { 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, }); } diff --git a/apps/control-station/test/simulationWorkspace.test.mjs b/apps/control-station/test/simulationWorkspace.test.mjs index 9781f38..f4176cd 100644 --- a/apps/control-station/test/simulationWorkspace.test.mjs +++ b/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.match(ugv, /longitudinalSlipSpeed \* timeStep/); + 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\)/);