ParkingRobot/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/JerkLimitedStoppingMath.cs

using System;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

public sealed class JerkLimitedStoppingProfile
{
    internal JerkLimitedStoppingProfile(double distanceMeters, double durationSeconds,
        double finalSpeedMetersPerSecond, double finalAccelerationMetersPerSecondSquared)
    {
        DistanceMeters = distanceMeters;
        DurationSeconds = durationSeconds;
        FinalSpeedMetersPerSecond = finalSpeedMetersPerSecond;
        FinalAccelerationMetersPerSecondSquared = finalAccelerationMetersPerSecondSquared;
    }

    public double DistanceMeters { get; }
    public double DurationSeconds { get; }
    public double FinalSpeedMetersPerSecond { get; }
    public double FinalAccelerationMetersPerSecondSquared { get; }
}

public static class JerkLimitedStoppingMath
{
    private const double NumericTolerance = 1e-12d;

    public static bool TryCalculate(double speed, double acceleration,
        double maximumDeceleration, double maximumJerk,
        out JerkLimitedStoppingProfile profile, out string failureReason)
    {
        profile = null;
        failureReason = string.Empty;
        if (!IsFinite(speed) || speed < 0d || !IsFinite(acceleration) ||
            !IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) ||
            acceleration < -maximumDeceleration - NumericTolerance)
        {
            failureReason = "Stopping inputs are outside finite longitudinal bounds.";
            return false;
        }
        if (speed <= NumericTolerance && Math.Abs(acceleration) <= NumericTolerance)
        {
            profile = new JerkLimitedStoppingProfile(0d, 0d, 0d, 0d);
            return true;
        }

        double unavoidableReleaseLoss = acceleration < 0d
            ? acceleration * acceleration / (2d * maximumJerk)
            : 0d;
        if (speed + NumericTolerance < unavoidableReleaseLoss)
        {
            failureReason = "The current negative acceleration cannot be released before speed crosses zero.";
            return false;
        }

        double peakDeceleration = Math.Sqrt(maximumJerk * speed +
            0.5d * acceleration * acceleration);
        double downDuration;
        double plateauDuration;
        double upDuration;
        if (peakDeceleration <= maximumDeceleration + NumericTolerance)
        {
            peakDeceleration = Math.Min(peakDeceleration, maximumDeceleration);
            downDuration = (acceleration + peakDeceleration) / maximumJerk;
            plateauDuration = 0d;
            upDuration = peakDeceleration / maximumJerk;
        }
        else
        {
            peakDeceleration = maximumDeceleration;
            downDuration = (acceleration + peakDeceleration) / maximumJerk;
            upDuration = peakDeceleration / maximumJerk;
            double speedAfterDown = speed + acceleration * downDuration -
                0.5d * maximumJerk * downDuration * downDuration;
            double releaseLoss = peakDeceleration * peakDeceleration /
                (2d * maximumJerk);
            plateauDuration = (speedAfterDown - releaseLoss) / peakDeceleration;
        }
        if (downDuration < -NumericTolerance || plateauDuration < -NumericTolerance)
        {
            failureReason = "No monotone three-phase jerk-limited stop exists for the current state.";
            return false;
        }

        downDuration = Math.Max(0d, downDuration);
        plateauDuration = Math.Max(0d, plateauDuration);
        double s = 0d;
        double u = speed;
        double a = acceleration;
        Integrate(ref s, ref u, ref a, -maximumJerk, downDuration);
        Integrate(ref s, ref u, ref a, 0d, plateauDuration);
        Integrate(ref s, ref u, ref a, maximumJerk, upDuration);
        if (Math.Abs(u) > 1e-9d || Math.Abs(a) > 1e-9d || s < -NumericTolerance)
        {
            failureReason = "The jerk-limited stop did not end at zero speed and acceleration.";
            return false;
        }

        profile = new JerkLimitedStoppingProfile(Math.Max(0d, s),
            downDuration + plateauDuration + upDuration, 0d, 0d);
        return true;
    }

    public static double MaximumInitialSpeedForDistance(double availableDistance,
        double conservativeInitialAcceleration, double maximumDeceleration,
        double maximumJerk, double directionMaximumSpeed)
    {
        if (!IsFinite(availableDistance) || availableDistance < 0d ||
            !IsFinite(conservativeInitialAcceleration) ||
            !IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) ||
            !IsPositiveFinite(directionMaximumSpeed))
            throw new ArgumentOutOfRangeException(nameof(availableDistance));

        double lower = 0d;
        double upper = directionMaximumSpeed;
        for (int iteration = 0; iteration < 64; iteration++)
        {
            double candidate = 0.5d * (lower + upper);
            bool fits = TryCalculate(candidate, conservativeInitialAcceleration,
                maximumDeceleration, maximumJerk,
                out JerkLimitedStoppingProfile stop, out _) &&
                stop.DistanceMeters <= availableDistance + NumericTolerance;
            if (fits)
                lower = candidate;
            else
                upper = candidate;
        }
        return lower;
    }

    public static double CalculateMaximumStoppedDistance(double initialSpeed,
        double initialAcceleration, double maximumSpeed, double maximumAcceleration,
        double maximumDeceleration, double maximumJerk, double timeHorizon)
    {
        if (!IsFinite(initialSpeed) || initialSpeed < 0d ||
            !IsFinite(initialAcceleration) || !IsPositiveFinite(maximumSpeed) ||
            !IsPositiveFinite(maximumAcceleration) || !IsPositiveFinite(maximumDeceleration) ||
            !IsPositiveFinite(maximumJerk) || !IsPositiveFinite(timeHorizon))
            throw new ArgumentOutOfRangeException(nameof(timeHorizon));

        double lower = 0d;
        double upper = timeHorizon;
        double bestDistance = 0d;
        for (int iteration = 0; iteration < 64; iteration++)
        {
            double driveDuration = 0.5d * (lower + upper);
            AdvanceTowardMaximumSpeed(initialSpeed, initialAcceleration,
                maximumSpeed, maximumAcceleration, maximumJerk, driveDuration,
                out double driveDistance, out double speed, out double acceleration);
            bool fits = TryCalculate(speed, acceleration, maximumDeceleration,
                maximumJerk, out JerkLimitedStoppingProfile stop, out _) &&
                driveDuration + stop.DurationSeconds <= timeHorizon + NumericTolerance;
            if (fits)
            {
                lower = driveDuration;
                bestDistance = Math.Max(bestDistance, driveDistance + stop.DistanceMeters);
            }
            else
            {
                upper = driveDuration;
            }
        }
        return bestDistance;
    }

    private static void AdvanceTowardMaximumSpeed(double initialSpeed, double initialAcceleration,
        double maximumSpeed, double maximumAcceleration, double maximumJerk, double duration,
        out double distance, out double speed, out double acceleration)
    {
        distance = 0d;
        speed = initialSpeed;
        acceleration = initialAcceleration;
        double remaining = duration;
        while (remaining > NumericTolerance)
        {
            double dt = Math.Min(0.001d, remaining);
            double speedNeededToReleaseAcceleration = acceleration > 0d
                ? acceleration * acceleration / (2d * maximumJerk)
                : 0d;
            double jerk = speed + speedNeededToReleaseAcceleration >= maximumSpeed
                ? (acceleration > 0d ? -maximumJerk : 0d)
                : (acceleration < maximumAcceleration ? maximumJerk : 0d);
            Integrate(ref distance, ref speed, ref acceleration, jerk, dt);
            if (speed > maximumSpeed && speed - maximumSpeed <= 1e-6d)
            {
                speed = maximumSpeed;
                acceleration = 0d;
            }
            remaining -= dt;
        }
    }

    private static void Integrate(ref double s, ref double u, ref double a,
        double jerk, double duration)
    {
        s += u * duration + 0.5d * a * duration * duration +
            jerk * duration * duration * duration / 6d;
        u += a * duration + 0.5d * jerk * duration * duration;
        a += jerk * duration;
    }

    private static bool IsPositiveFinite(double value) => IsFinite(value) && value > 0d;

    private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);
}