Переписал трение на пасейку (говно)
This commit is contained in:
Valera
11
Code/Base/Wheel/Friction.cs
Normal file
11
Code/Base/Wheel/Friction.cs
Normal file
@@ -0,0 +1,11 @@
|
||||
namespace VeloX;
|
||||
|
||||
public struct Friction
|
||||
{
|
||||
public float SlipCoef { get; set; }
|
||||
public float ForceCoef { get; set; }
|
||||
|
||||
public float Force { get; set; }
|
||||
public float Slip { get; set; }
|
||||
public float Speed { get; set; }
|
||||
}
|
||||
16
Code/Base/Wheel/FrictionPreset.cs
Normal file
16
Code/Base/Wheel/FrictionPreset.cs
Normal file
@@ -0,0 +1,16 @@
|
||||
using System;
|
||||
|
||||
namespace VeloX;
|
||||
public class FrictionPreset
|
||||
{
|
||||
public float B { get; set; } = 10.86f;
|
||||
public float C { get; set; } = 2.15f;
|
||||
public float D { get; set; } = 0.933f;
|
||||
public float E { get; set; } = 0.992f;
|
||||
public float Evaluate( float slip )
|
||||
{
|
||||
var t = Math.Abs( slip );
|
||||
|
||||
return D * MathF.Sin( C * MathF.Atan( B * t - E * (B * t - MathF.Atan( B * t )) ) );
|
||||
}
|
||||
}
|
||||
@@ -1,18 +0,0 @@
|
||||
using Sandbox;
|
||||
|
||||
namespace VeloX;
|
||||
|
||||
public partial class VeloXWheel
|
||||
{
|
||||
[Property] float BrakePowerMax { get; set; } = 3000;
|
||||
[Property, Group( "Suspension" )] float SuspensionLength { get; set; } = 10;
|
||||
[Property, Group( "Suspension" )] float SpringStrength { get; set; } = 800;
|
||||
[Property, Group( "Suspension" )] float SpringDamper { get; set; } = 3000;
|
||||
|
||||
[Property, Group( "Traction" )] float ForwardTractionMax { get; set; } = 2600;
|
||||
[Property, Group( "Traction" )] float SideTractionMultiplier { get; set; } = 20;
|
||||
[Property, Group( "Traction" )] float SideTractionMaxAng { get; set; } = 25;
|
||||
[Property, Group( "Traction" )] float SideTractionMax { get; set; } = 2400;
|
||||
[Property, Group( "Traction" )] float SideTractionMin { get; set; } = 800;
|
||||
|
||||
}
|
||||
@@ -1,7 +1,11 @@
|
||||
using Sandbox;
|
||||
using Sandbox.UI;
|
||||
using System;
|
||||
using System.Buffers.Text;
|
||||
using System.Numerics;
|
||||
using System.Runtime.Intrinsics.Arm;
|
||||
using System.Text.RegularExpressions;
|
||||
using System.Threading;
|
||||
|
||||
namespace VeloX;
|
||||
|
||||
@@ -11,14 +15,34 @@ public partial class VeloXWheel : Component
|
||||
{
|
||||
|
||||
[Property] public float Radius { get; set; } = 15;
|
||||
[Property] public float Mass { get; set; } = 20;
|
||||
[Property] public float RollingResistance { get; set; } = 20;
|
||||
[Property] public float SlipCircleShape { get; set; } = 1.05f;
|
||||
|
||||
public FrictionPreset LongitudinalFrictionPreset => WheelFriction.Longitudinal;
|
||||
public FrictionPreset LateralFrictionPreset => WheelFriction.Lateral;
|
||||
public FrictionPreset AligningFrictionPreset => WheelFriction.Aligning;
|
||||
|
||||
[Property] public WheelFriction WheelFriction { get; set; }
|
||||
|
||||
|
||||
[Property] public float Width { get; set; } = 6;
|
||||
[Sync] public float SideSlip { get; private set; }
|
||||
[Sync] public float ForwardSlip { get; private set; }
|
||||
[Sync, Range( 0, 1 )] public float BrakePower { get; set; }
|
||||
[Sync] public float Torque { get; set; }
|
||||
[Sync, Range( 0, 1 )] public float Brake { get; set; }
|
||||
[Property] float BrakePowerMax { get; set; } = 3000;
|
||||
[Property] public bool IsFront { get; protected set; }
|
||||
[Property] public float SteerMultiplier { get; set; }
|
||||
[Property] public float CasterAngle { get; set; } = 7; // todo
|
||||
[Property] public float CamberAngle { get; set; } = -3;
|
||||
[Property] public float ToeAngle { get; set; } = 0.5f;
|
||||
|
||||
[Property, Group( "Suspension" )] float SuspensionLength { get; set; } = 10;
|
||||
[Property, Group( "Suspension" )] float SpringStrength { get; set; } = 800;
|
||||
[Property, Group( "Suspension" )] float SpringDamper { get; set; } = 3000;
|
||||
|
||||
public float Spin { get; private set; }
|
||||
|
||||
public float RPM { get => angularVelocity * 60f / MathF.Tau; set => angularVelocity = value / (60 / MathF.Tau); }
|
||||
@@ -31,12 +55,23 @@ public partial class VeloXWheel : Component
|
||||
public bool IsOnGround => Trace.Hit;
|
||||
|
||||
private float lastSpringOffset;
|
||||
private Vector2 tractionCycle;
|
||||
private float angularVelocity;
|
||||
private float load;
|
||||
private float lastFraction;
|
||||
private RealTimeUntil expandSoundCD;
|
||||
private RealTimeUntil contractSoundCD;
|
||||
|
||||
private Vector3 contactPos;
|
||||
private Vector3 forward;
|
||||
private Vector3 right;
|
||||
private Vector3 up;
|
||||
|
||||
private Friction forwardFriction;
|
||||
private Friction sideFriction;
|
||||
private Vector3 force;
|
||||
|
||||
private float BaseInertia => 0.5f * Mass * MathF.Pow( Radius.InchToMeter(), 2 );
|
||||
private float Inertia => BaseInertia;
|
||||
|
||||
protected override void OnAwake()
|
||||
{
|
||||
@@ -45,15 +80,6 @@ public partial class VeloXWheel : Component
|
||||
StartPos = LocalPosition;
|
||||
}
|
||||
|
||||
private static float TractionRamp( float slipAngle, float sideTractionMaxAng, float sideTractionMax, float sideTractionMin )
|
||||
{
|
||||
sideTractionMaxAng /= 90; // Convert max slip angle to the 0 - 1 range
|
||||
var x = (slipAngle - sideTractionMaxAng) / (1 - sideTractionMaxAng);
|
||||
|
||||
|
||||
return slipAngle < sideTractionMaxAng ? sideTractionMax : (sideTractionMax * (1 - x)) + (sideTractionMin * x);
|
||||
}
|
||||
|
||||
private void DoSuspensionSounds( VeloXBase vehicle, float change )
|
||||
{
|
||||
if ( change > 0.1f && expandSoundCD )
|
||||
@@ -75,41 +101,145 @@ public partial class VeloXWheel : Component
|
||||
internal void Update( VeloXBase vehicle, in float dt )
|
||||
{
|
||||
UpdateVisuals( vehicle, dt );
|
||||
|
||||
if ( !IsOnGround )
|
||||
{
|
||||
angularVelocity += (Torque / 20) * dt;
|
||||
angularVelocity = MathX.Approach( angularVelocity, 0, dt * 4 );
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
private void UpdateVisuals( VeloXBase vehicle, in float dt )
|
||||
{
|
||||
//Rotate the wheel around the axle axis
|
||||
Spin = (Spin - MathX.RadianToDegree( angularVelocity ) * dt) % 360;
|
||||
var entityAngles = vehicle.WorldRotation;
|
||||
|
||||
WorldRotation = vehicle.WorldTransform.RotationToWorld( vehicle.SteerAngle * SteerMultiplier ).RotateAroundAxis( Vector3.Right, Spin );
|
||||
Spin -= angularVelocity.MeterToInch() * dt;
|
||||
|
||||
|
||||
var steerRotated = entityAngles.RotateAroundAxis( Vector3.Up, vehicle.SteerAngle.yaw * SteerMultiplier + ToeAngle );
|
||||
var camberRotated = steerRotated.RotateAroundAxis( Vector3.Forward, -CamberAngle );
|
||||
var angularVelocityRotated = camberRotated.RotateAroundAxis( Vector3.Right, Spin );
|
||||
|
||||
WorldRotation = angularVelocityRotated;
|
||||
|
||||
}
|
||||
|
||||
public void DoPhysics( VeloXBase vehicle, ref Vector3 vehVel, ref Vector3 vehAngVel, ref Vector3 outLin, ref Vector3 outAng, in float dt )
|
||||
private (float, float, float, float) StepLongitudinal( float Vx, float Lc, float kFx, float kSx, float dt )
|
||||
{
|
||||
float Tm = Torque;
|
||||
float Tb = Brake * BrakePowerMax + RollingResistance;
|
||||
float R = Radius.InchToMeter();
|
||||
float I = Inertia;
|
||||
|
||||
float Winit = angularVelocity;
|
||||
float W = angularVelocity;
|
||||
|
||||
float VxAbs = MathF.Abs( Vx );
|
||||
float Sx;
|
||||
if ( VxAbs >= 0.1f )
|
||||
Sx = (Vx - W * R) / VxAbs;
|
||||
|
||||
else
|
||||
Sx = (Vx - W * R) * 0.6f;
|
||||
|
||||
Sx = Math.Clamp( Sx * kSx, -1, 1 );
|
||||
|
||||
W += Tm / I * dt;
|
||||
|
||||
Tb *= W > 0 ? -1 : 1;
|
||||
|
||||
float TbCap = MathF.Abs( W ) * I / dt;
|
||||
float Tbr = MathF.Abs( Tb ) - MathF.Abs( TbCap );
|
||||
Tbr = MathF.Max( Tbr, 0 );
|
||||
Tb = Math.Clamp( Tb, -TbCap, TbCap );
|
||||
W += Tb / I * dt;
|
||||
|
||||
float maxTorque = LongitudinalFrictionPreset.Evaluate( Sx ) * Lc * kFx;
|
||||
|
||||
float errorTorque = (W - Vx / R) * I / dt;
|
||||
|
||||
float surfaceTorque = MathX.Clamp( errorTorque, -maxTorque, maxTorque );
|
||||
|
||||
W -= surfaceTorque / I * dt;
|
||||
|
||||
float Fx = surfaceTorque / R;
|
||||
|
||||
|
||||
Tbr *= W > 0 ? -1 : 1;
|
||||
float TbCap2 = MathF.Abs( W ) * I / dt;
|
||||
|
||||
float Tb2 = Math.Clamp( Tbr, -TbCap2, TbCap2 );
|
||||
|
||||
W += Tb2 / I * dt;
|
||||
|
||||
float deltaOmegaTorque = (W - Winit) * I / dt;
|
||||
|
||||
float Tcnt = -surfaceTorque + Tb + Tb2 - deltaOmegaTorque;
|
||||
if ( Lc < 0.001f )
|
||||
Sx = 0;
|
||||
|
||||
return (W, Sx, Fx, Tcnt);
|
||||
}
|
||||
|
||||
private (float, float) StepLateral( float Vx, float Vy, float Lc, float kFy, float kSy, float dt )
|
||||
{
|
||||
float VxAbs = MathF.Abs( Vx );
|
||||
float Sy;
|
||||
|
||||
if ( VxAbs > 0.1f )
|
||||
Sy = MathF.Atan( Vy / VxAbs ).RadianToDegree() * 0.01111f;
|
||||
else
|
||||
Sy = Vy * (0.003f / dt);
|
||||
|
||||
|
||||
Sy *= kSy * 0.95f;
|
||||
Sy = Math.Clamp( Sy * kSy, -1, 1 );
|
||||
float Fy = -MathF.Sign( Sy ) * LateralFrictionPreset.Evaluate( Sy ) * Lc * kFy;
|
||||
if ( Lc < 0.0001f )
|
||||
Sy = 0;
|
||||
|
||||
return (Sy, Fy);
|
||||
}
|
||||
|
||||
private void SlipCircle( float Sx, float Sy, float Fx, ref float Fy )
|
||||
{
|
||||
float SxAbs = Math.Abs( Sx );
|
||||
if ( SxAbs > 0.01f )
|
||||
{
|
||||
|
||||
float SxClamped = Math.Clamp( Sx, -1, 1 );
|
||||
|
||||
float SyClamped = Math.Clamp( Sy, -1, 1 );
|
||||
|
||||
Vector2 combinedSlip = new(
|
||||
SxClamped * SlipCircleShape,
|
||||
SyClamped
|
||||
);
|
||||
|
||||
Vector2 slipDir = combinedSlip.Normal;
|
||||
|
||||
float F = MathF.Sqrt( Fx * Fx + Fy * Fy );
|
||||
|
||||
float absSlipDirY = MathF.Abs( slipDir.y );
|
||||
|
||||
Fy = F * absSlipDirY * MathF.Sign( Fy );
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
private static float GetLongitudinalLoadCoefficient( float load ) => 11000 * (1 - MathF.Exp( -0.00014f * load ));
|
||||
private static float GetLateralLoadCoefficient( float load ) => 18000 * (1 - MathF.Exp( -0.0001f * load ));
|
||||
|
||||
public void DoPhysics( VeloXBase vehicle, in float dt )
|
||||
{
|
||||
var pos = vehicle.WorldTransform.PointToWorld( StartPos );
|
||||
|
||||
var ang = vehicle.WorldTransform.RotationToWorld( vehicle.SteerAngle * SteerMultiplier );
|
||||
|
||||
var fw = ang.Forward;
|
||||
var rt = ang.Right;
|
||||
var up = ang.Up;
|
||||
forward = ang.Forward;
|
||||
right = ang.Right;
|
||||
up = ang.Up;
|
||||
|
||||
var maxLen = SuspensionLength;
|
||||
|
||||
var endPos = pos + ang.Down * maxLen;
|
||||
Trace = Scene.Trace.IgnoreGameObjectHierarchy( vehicle.GameObject )
|
||||
.FromTo( pos, endPos )
|
||||
.Cylinder( Width, Radius )
|
||||
Trace = Scene.Trace
|
||||
.IgnoreGameObjectHierarchy( vehicle.GameObject )
|
||||
.Cylinder( Width, Radius, pos, endPos )
|
||||
.Rotated( vehicle.WorldTransform.Rotation * CylinderOffset )
|
||||
.UseRenderMeshes( false )
|
||||
.UseHitPosition( false )
|
||||
@@ -118,123 +248,107 @@ public partial class VeloXWheel : Component
|
||||
|
||||
var fraction = Trace.Fraction;
|
||||
|
||||
var contactPos = pos - maxLen * fraction * up;
|
||||
contactPos = pos - maxLen * fraction * up;
|
||||
|
||||
LocalPosition = vehicle.WorldTransform.PointToLocal( contactPos );
|
||||
|
||||
DoSuspensionSounds( vehicle, fraction - lastFraction );
|
||||
lastFraction = fraction;
|
||||
|
||||
var normal = Trace.Normal;
|
||||
|
||||
var vel = vehicle.Body.GetVelocityAtPoint( pos );
|
||||
|
||||
vel.x = vel.x.InchToMeter();
|
||||
vel.y = vel.y.InchToMeter();
|
||||
vel.z = vel.z.InchToMeter();
|
||||
|
||||
if ( !IsOnGround )
|
||||
{
|
||||
SideSlip = 0;
|
||||
ForwardSlip = 0;
|
||||
return;
|
||||
}
|
||||
var normal = Trace.Normal;
|
||||
|
||||
var vel = vehicle.Body.GetVelocityAtPoint( pos );
|
||||
// Split that velocity among our local directions
|
||||
|
||||
var velF = fw.Dot( vel );
|
||||
|
||||
var velR = rt.Dot( vel );
|
||||
|
||||
var velU = normal.Dot( vel );
|
||||
|
||||
var absVelR = Math.Abs( velR );
|
||||
|
||||
|
||||
//Make forward forces be perpendicular to the surface normal
|
||||
|
||||
fw = normal.Cross( rt );
|
||||
|
||||
//Suspension spring force &damping
|
||||
|
||||
var offset = maxLen - (fraction * maxLen);
|
||||
|
||||
var springForce = (offset * SpringStrength);
|
||||
|
||||
var damperForce = (lastSpringOffset - offset) * SpringDamper;
|
||||
lastSpringOffset = offset;
|
||||
var force = (springForce - damperForce) * up.Dot( normal ) * normal;
|
||||
force = (springForce - damperForce) * MathF.Max( 0, up.Dot( normal ) ) * normal / dt;
|
||||
|
||||
//If the suspension spring is going to be fully compressed on the next frame...
|
||||
// Vector3.CalculateVelocityOffset is broken (need fix)
|
||||
//var velU = normal.Dot( vel ).MeterToInch();
|
||||
//if ( velU < 0 && offset + Math.Abs( velU * dt ) > SuspensionLength )
|
||||
//{
|
||||
// var (linearVel, angularVel) = vehicle.Body.PhysicsBody.CalculateVelocityOffset( (-velU.InchToMeter() / dt) * normal, pos );
|
||||
// vehicle.Body.Velocity += linearVel;
|
||||
// vehicle.Body.AngularVelocity += angularVel;
|
||||
//}
|
||||
|
||||
if ( velU < 0 && offset + Math.Abs( velU * dt ) > SuspensionLength )
|
||||
|
||||
load = springForce - damperForce;
|
||||
load = Math.Max( load, 0 );
|
||||
|
||||
var longitudinalLoadCoefficient = GetLongitudinalLoadCoefficient( load );
|
||||
var lateralLoadCoefficient = GetLateralLoadCoefficient( load );
|
||||
|
||||
float forwardSpeed = 0;
|
||||
float sideSpeed = 0;
|
||||
|
||||
|
||||
if ( IsOnGround )
|
||||
{
|
||||
var (linearVel, angularVel) = vehicle.Body.PhysicsBody.CalculateVelocityOffset( (-velU / dt) * normal, pos );
|
||||
vehVel += linearVel;
|
||||
vehAngVel += angularVel;
|
||||
forwardSpeed = vel.Dot( forward );
|
||||
sideSpeed = vel.Dot( right );
|
||||
}
|
||||
|
||||
//Rolling resistance
|
||||
force += 0.05f * -velF * fw;
|
||||
(float W, float Sx, float Fx, float _) = StepLongitudinal(
|
||||
forwardSpeed,
|
||||
longitudinalLoadCoefficient,
|
||||
0.95f,
|
||||
0.9f,
|
||||
dt
|
||||
);
|
||||
|
||||
//Brake and torque forces
|
||||
var surfaceGrip = 1;
|
||||
(float Sy, float Fy) = StepLateral(
|
||||
forwardSpeed,
|
||||
sideSpeed,
|
||||
lateralLoadCoefficient,
|
||||
0.95f,
|
||||
0.9f,
|
||||
dt
|
||||
);
|
||||
|
||||
var maxTraction = ForwardTractionMax * surfaceGrip * 1;
|
||||
//Grip loss logic
|
||||
SlipCircle( Sx, Sy, Fx, ref Fy );
|
||||
angularVelocity = W;
|
||||
|
||||
var brakeForce = MathX.Clamp( -velF, -Brake, Brake ) * BrakePowerMax * surfaceGrip;
|
||||
forwardFriction = new Friction()
|
||||
{
|
||||
Slip = Sx,
|
||||
Force = Fx.MeterToInch(),
|
||||
Speed = forwardSpeed
|
||||
};
|
||||
|
||||
var forwardForce = Torque + brakeForce;
|
||||
|
||||
var signForwardForce = forwardForce > 0 ? 1 : (forwardForce < 0 ? -1 : 0);
|
||||
|
||||
// Given an amount of sideways slippage( up to the max.traction )
|
||||
// and the forward force, calculate how much grip we are losing.
|
||||
|
||||
tractionCycle.x = Math.Min( absVelR, maxTraction );
|
||||
tractionCycle.y = forwardForce;
|
||||
|
||||
var gripLoss = Math.Max( tractionCycle.Length - maxTraction, 0 );
|
||||
sideFriction = new Friction()
|
||||
{
|
||||
Slip = Sy,
|
||||
Force = Fy.MeterToInch(),
|
||||
Speed = sideSpeed
|
||||
};
|
||||
|
||||
|
||||
// Reduce the forward force by the amount of grip we lost,
|
||||
|
||||
// but still allow some amount of brake force to apply regardless.
|
||||
|
||||
forwardForce += -(gripLoss * signForwardForce) + MathX.Clamp( brakeForce * 0.5f, -maxTraction, maxTraction );
|
||||
|
||||
force += fw * forwardForce;
|
||||
|
||||
// Get how fast the wheel would be spinning if it had never lost grip
|
||||
|
||||
var groundAngularVelocity = MathF.Tau * (velF / (Radius * MathF.Tau));
|
||||
|
||||
// Add our grip loss to our spin velocity
|
||||
var _angvel = groundAngularVelocity + gripLoss * (Torque > 0 ? 1 : (Torque < 0 ? -1 : 0));
|
||||
|
||||
// Smoothly match our current angular velocity to the angular velocity affected by grip loss
|
||||
|
||||
angularVelocity = MathX.Approach( angularVelocity, _angvel, dt * 200 );
|
||||
ForwardSlip = groundAngularVelocity - angularVelocity;
|
||||
|
||||
// Calculate side slip angle
|
||||
var slipAngle = MathF.Atan2( velR, MathF.Abs( velF ) ) / MathF.PI * 2;
|
||||
SideSlip = slipAngle * MathX.Clamp( vehicle.TotalSpeed * 0.005f, 0, 1 ) * 2;
|
||||
|
||||
//Sideways traction ramp
|
||||
|
||||
slipAngle = MathF.Abs( slipAngle * slipAngle );
|
||||
|
||||
maxTraction = TractionRamp( slipAngle, SideTractionMaxAng, SideTractionMax, SideTractionMin );
|
||||
|
||||
var sideForce = -rt.Dot( vel * SideTractionMultiplier );
|
||||
|
||||
// Reduce sideways traction force as the wheel slips forward
|
||||
sideForce *= 1 - Math.Clamp( MathF.Abs( gripLoss ) * 0.1f, 0, 1 ) * 0.9f;
|
||||
|
||||
// Apply sideways traction force
|
||||
force += Math.Clamp( sideForce, -maxTraction, maxTraction ) * surfaceGrip * rt;
|
||||
|
||||
force += velR * SideTractionMultiplier * -0.1f * rt;
|
||||
//Apply the forces at the axle / ground contact position
|
||||
|
||||
vehicle.Body.ApplyForceAt( pos, force / dt );
|
||||
var frictionforce = right * sideFriction.Force + forward * forwardFriction.Force;
|
||||
|
||||
vehicle.Body.ApplyForceAt( contactPos, force + frictionforce );
|
||||
|
||||
}
|
||||
|
||||
|
||||
// debug
|
||||
protected override void OnUpdate()
|
||||
{
|
||||
DebugOverlay.Normal( contactPos, forward * forwardFriction.Force / 10000f, Color.Red, overlay: true );
|
||||
DebugOverlay.Normal( contactPos, right * sideFriction.Force / 10000f, Color.Green, overlay: true );
|
||||
DebugOverlay.Normal( contactPos, up * force / 50000f, Color.Blue, overlay: true );
|
||||
}
|
||||
}
|
||||
|
||||
12
Code/Base/Wheel/WheelFriction.cs
Normal file
12
Code/Base/Wheel/WheelFriction.cs
Normal file
@@ -0,0 +1,12 @@
|
||||
using Sandbox;
|
||||
|
||||
namespace VeloX;
|
||||
|
||||
|
||||
[GameResource( "Wheel Friction", "whfric", "Wheel Friction", Category = "VeloX", Icon = "radio_button_checked" )]
|
||||
public class WheelFriction : GameResource
|
||||
{
|
||||
public FrictionPreset Longitudinal { get; set; }
|
||||
public FrictionPreset Lateral { get; set; }
|
||||
public FrictionPreset Aligning { get; set; }
|
||||
}
|
||||
Reference in New Issue
Block a user