SILVER/NODEDC_MISSION_CORE
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

fix(simulation): hold ugv on sloped collision meshes

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-08-29 12:41:28 +03:00
parent 57dacc5a04
commit 24b65926fe
2 changed files with 40 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/control-station/src/components/simulation/SimulationUgvController.ts
+31 -68
View File
@@ -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<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) {
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<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
+9 -3
View File
@@ -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\)/);