fix(simulation): hold ugv on sloped collision meshes
This commit is contained in:
DCCONSTRUCTIONS
@@ -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,
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user