30 lines
1.1 KiB
C#
30 lines
1.1 KiB
C#
using System;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
|
|
|
internal static class CurvatureLimitPolicy
|
|
{
|
|
internal static bool TryGetAllowedMaximumVehicleCurvaturePerMeter(
|
|
VehicleParameters vehicle,
|
|
double radiusToleranceMeters,
|
|
out double allowedMaximumCurvaturePerMeter)
|
|
{
|
|
allowedMaximumCurvaturePerMeter = 0d;
|
|
if (!NumericGuard.IsFinite(radiusToleranceMeters) || radiusToleranceMeters < 0d ||
|
|
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double nominalMaximumCurvature))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
double nominalMinimumRadius = 1d / nominalMaximumCurvature;
|
|
double toleratedMinimumRadius = nominalMinimumRadius - radiusToleranceMeters;
|
|
if (!NumericGuard.IsPositiveFinite(toleratedMinimumRadius)) return false;
|
|
|
|
allowedMaximumCurvaturePerMeter = 1d / toleratedMinimumRadius;
|
|
return NumericGuard.IsPositiveFinite(allowedMaximumCurvaturePerMeter);
|
|
}
|
|
}
|