121 lines
2.9 KiB
C#
121 lines
2.9 KiB
C#
using System;
|
|
using System.Numerics;
|
|
|
|
namespace LLib.Movement.Unstuck;
|
|
|
|
public sealed class StallDetector(StallDetectorOptions options)
|
|
{
|
|
private bool _hasAnchor;
|
|
|
|
private long _anchorMs;
|
|
|
|
private Vector3 _anchorPosition;
|
|
|
|
private DriveMode _anchorMode;
|
|
|
|
private bool _anchorUsesProgress;
|
|
|
|
private float _referenceBestDistance;
|
|
|
|
private float _bestDistance;
|
|
|
|
private long _lastDrivenMs;
|
|
|
|
private long _cooldownUntilMs;
|
|
|
|
public StallVerdict? Observe(in StallSample sample)
|
|
{
|
|
if (!sample.Driving)
|
|
{
|
|
return null;
|
|
}
|
|
bool flag = sample.Mode != DriveMode.Ground && sample.DistanceToTarget.HasValue;
|
|
if (!_hasAnchor || sample.TimestampMs - _lastDrivenMs > options.ContinuityGapMs || sample.Mode != _anchorMode || flag != _anchorUsesProgress)
|
|
{
|
|
Anchor(in sample, flag);
|
|
return null;
|
|
}
|
|
_lastDrivenMs = sample.TimestampMs;
|
|
if (!flag)
|
|
{
|
|
return ObserveDisplacement(in sample);
|
|
}
|
|
return ObserveProgress(in sample);
|
|
}
|
|
|
|
public void Reset()
|
|
{
|
|
_hasAnchor = false;
|
|
_cooldownUntilMs = 0L;
|
|
}
|
|
|
|
private StallVerdict? ObserveProgress(in StallSample sample)
|
|
{
|
|
float value = sample.DistanceToTarget.Value;
|
|
if (value < _bestDistance)
|
|
{
|
|
_bestDistance = value;
|
|
}
|
|
float num = _referenceBestDistance - _bestDistance;
|
|
if (num >= options.ProgressImprovementThreshold)
|
|
{
|
|
_referenceBestDistance = _bestDistance;
|
|
_anchorMs = sample.TimestampMs;
|
|
_anchorPosition = sample.Position;
|
|
return null;
|
|
}
|
|
long num2 = sample.TimestampMs - _anchorMs;
|
|
if (num2 >= options.ProgressWindowMs)
|
|
{
|
|
return Trip(in sample, num2, num);
|
|
}
|
|
return null;
|
|
}
|
|
|
|
private StallVerdict? ObserveDisplacement(in StallSample sample)
|
|
{
|
|
float num = ((sample.Mode == DriveMode.Ground) ? DistanceXZ(_anchorPosition, sample.Position) : Vector3.Distance(_anchorPosition, sample.Position));
|
|
if (num >= options.GroundDistanceThreshold)
|
|
{
|
|
_anchorMs = sample.TimestampMs;
|
|
_anchorPosition = sample.Position;
|
|
return null;
|
|
}
|
|
long num2 = sample.TimestampMs - _anchorMs;
|
|
if (num2 >= options.GroundWindowMs)
|
|
{
|
|
return Trip(in sample, num2, num);
|
|
}
|
|
return null;
|
|
}
|
|
|
|
private StallVerdict? Trip(in StallSample sample, long elapsedMs, float measured)
|
|
{
|
|
bool num = sample.TimestampMs < _cooldownUntilMs;
|
|
Anchor(in sample, _anchorUsesProgress);
|
|
if (num)
|
|
{
|
|
return null;
|
|
}
|
|
_cooldownUntilMs = sample.TimestampMs + options.CooldownMs;
|
|
return new StallVerdict(elapsedMs, measured, sample.Mode);
|
|
}
|
|
|
|
private void Anchor(in StallSample sample, bool usesProgress)
|
|
{
|
|
_hasAnchor = true;
|
|
_anchorMs = sample.TimestampMs;
|
|
_anchorPosition = sample.Position;
|
|
_anchorMode = sample.Mode;
|
|
_anchorUsesProgress = usesProgress;
|
|
_lastDrivenMs = sample.TimestampMs;
|
|
_referenceBestDistance = (_bestDistance = sample.DistanceToTarget.GetValueOrDefault());
|
|
}
|
|
|
|
private static float DistanceXZ(Vector3 a, Vector3 b)
|
|
{
|
|
float num = a.X - b.X;
|
|
float num2 = a.Z - b.Z;
|
|
return MathF.Sqrt(num * num + num2 * num2);
|
|
}
|
|
}
|