feat: add quintic Hermite curve geometry
This commit is contained in:
梁薄云
@@ -0,0 +1,124 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>满足两个二维端点二阶边界条件的参数五次 Hermite 曲线。</summary>
|
||||
internal sealed class QuinticHermiteCurve2D
|
||||
{
|
||||
private readonly double _x0;
|
||||
private readonly double _x1;
|
||||
private readonly double _x2;
|
||||
private readonly double _x3;
|
||||
private readonly double _x4;
|
||||
private readonly double _x5;
|
||||
private readonly double _y0;
|
||||
private readonly double _y1;
|
||||
private readonly double _y2;
|
||||
private readonly double _y3;
|
||||
private readonly double _y4;
|
||||
private readonly double _y5;
|
||||
|
||||
private QuinticHermiteCurve2D(
|
||||
double x0, double x1, double x2, double x3, double x4, double x5,
|
||||
double y0, double y1, double y2, double y3, double y4, double y5)
|
||||
{
|
||||
_x0 = x0;
|
||||
_x1 = x1;
|
||||
_x2 = x2;
|
||||
_x3 = x3;
|
||||
_x4 = x4;
|
||||
_x5 = x5;
|
||||
_y0 = y0;
|
||||
_y1 = y1;
|
||||
_y2 = y2;
|
||||
_y3 = y3;
|
||||
_y4 = y4;
|
||||
_y5 = y5;
|
||||
}
|
||||
|
||||
internal static bool TryCreate(
|
||||
double x0, double y0, double dx0, double dy0, double ddx0, double ddy0,
|
||||
double x1, double y1, double dx1, double dy1, double ddx1, double ddy1,
|
||||
out QuinticHermiteCurve2D curve,
|
||||
out string reason)
|
||||
{
|
||||
curve = null;
|
||||
reason = string.Empty;
|
||||
if (!AreFinite(x0, y0, dx0, dy0, ddx0, ddy0, x1, y1, dx1, dy1, ddx1, ddy1))
|
||||
{
|
||||
reason = "五次 Hermite 曲线需要有限的边界条件。";
|
||||
return false;
|
||||
}
|
||||
if (IsZeroVector(dx0, dy0) || IsZeroVector(dx1, dy1))
|
||||
{
|
||||
reason = "五次 Hermite 曲线端点一阶导数不能为零。";
|
||||
return false;
|
||||
}
|
||||
|
||||
SolveCoordinate(x0, dx0, ddx0, x1, dx1, ddx1,
|
||||
out double ax0, out double ax1, out double ax2, out double ax3, out double ax4, out double ax5);
|
||||
SolveCoordinate(y0, dy0, ddy0, y1, dy1, ddy1,
|
||||
out double ay0, out double ay1, out double ay2, out double ay3, out double ay4, out double ay5);
|
||||
if (!AreFinite(ax0, ax1, ax2, ax3, ax4, ax5, ay0, ay1, ay2, ay3, ay4, ay5))
|
||||
{
|
||||
reason = "五次 Hermite 曲线系数溢出。";
|
||||
return false;
|
||||
}
|
||||
|
||||
curve = new QuinticHermiteCurve2D(ax0, ax1, ax2, ax3, ax4, ax5, ay0, ay1, ay2, ay3, ay4, ay5);
|
||||
return true;
|
||||
}
|
||||
|
||||
internal void Evaluate(
|
||||
double u,
|
||||
out double x, out double y,
|
||||
out double dx, out double dy,
|
||||
out double ddx, out double ddy)
|
||||
{
|
||||
if (!IsFinite(u) || u < 0d || u > 1d)
|
||||
throw new ArgumentOutOfRangeException(nameof(u), "The curve parameter must be finite and within [0, 1].");
|
||||
|
||||
x = EvaluateValue(_x0, _x1, _x2, _x3, _x4, _x5, u);
|
||||
y = EvaluateValue(_y0, _y1, _y2, _y3, _y4, _y5, u);
|
||||
dx = EvaluateFirstDerivative(_x1, _x2, _x3, _x4, _x5, u);
|
||||
dy = EvaluateFirstDerivative(_y1, _y2, _y3, _y4, _y5, u);
|
||||
ddx = EvaluateSecondDerivative(_x2, _x3, _x4, _x5, u);
|
||||
ddy = EvaluateSecondDerivative(_y2, _y3, _y4, _y5, u);
|
||||
}
|
||||
|
||||
private static void SolveCoordinate(double p0, double v0, double acceleration0, double p1, double v1, double acceleration1,
|
||||
out double a0, out double a1, out double a2, out double a3, out double a4, out double a5)
|
||||
{
|
||||
a0 = p0;
|
||||
a1 = v0;
|
||||
a2 = acceleration0 / 2d;
|
||||
double c0 = p1 - (a0 + a1 + a2);
|
||||
double c1 = v1 - (a1 + 2d * a2);
|
||||
double c2 = acceleration1 - 2d * a2;
|
||||
a3 = 10d * c0 - 4d * c1 + 0.5d * c2;
|
||||
a4 = -15d * c0 + 7d * c1 - c2;
|
||||
a5 = 6d * c0 - 3d * c1 + 0.5d * c2;
|
||||
}
|
||||
|
||||
private static double EvaluateValue(double a0, double a1, double a2, double a3, double a4, double a5, double u) =>
|
||||
(((((a5 * u + a4) * u + a3) * u + a2) * u + a1) * u) + a0;
|
||||
|
||||
private static double EvaluateFirstDerivative(double a1, double a2, double a3, double a4, double a5, double u) =>
|
||||
((((5d * a5 * u + 4d * a4) * u + 3d * a3) * u + 2d * a2) * u) + a1;
|
||||
|
||||
private static double EvaluateSecondDerivative(double a2, double a3, double a4, double a5, double u) =>
|
||||
(((20d * a5 * u + 12d * a4) * u + 6d * a3) * u) + 2d * a2;
|
||||
|
||||
private static bool IsZeroVector(double x, double y) => x == 0d && y == 0d;
|
||||
|
||||
private static bool AreFinite(params double[] values)
|
||||
{
|
||||
for (int index = 0; index < values.Length; index++)
|
||||
{
|
||||
if (!IsFinite(values[index])) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
@@ -0,0 +1,118 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
|
||||
}
|
||||
}
|
||||
|
||||
function Assert-Rejected($Result, [string]$Message) {
|
||||
if ($Result.Accepted) { throw $Message }
|
||||
Assert-True (-not [string]::IsNullOrWhiteSpace($Result.Reason)) 'Rejected curves must provide a stable reason.'
|
||||
}
|
||||
|
||||
function Assert-Throws([scriptblock]$Action, [string]$Message) {
|
||||
try {
|
||||
& $Action
|
||||
}
|
||||
catch [ArgumentOutOfRangeException] {
|
||||
return
|
||||
}
|
||||
catch [System.Reflection.TargetInvocationException] {
|
||||
Assert-True ($_.Exception.InnerException -is [ArgumentOutOfRangeException]) $Message
|
||||
return
|
||||
}
|
||||
throw $Message
|
||||
}
|
||||
|
||||
$curveType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.QuinticHermiteCurve2D', $true)
|
||||
$tryCreate = $curveType.GetMethod('TryCreate', [Reflection.BindingFlags]'Static,NonPublic')
|
||||
$evaluate = $curveType.GetMethod('Evaluate', [Reflection.BindingFlags]'Instance,NonPublic')
|
||||
Assert-True ($null -ne $tryCreate) 'QuinticHermiteCurve2D must expose its internal TryCreate factory.'
|
||||
Assert-True ($null -ne $evaluate) 'QuinticHermiteCurve2D must expose its internal Evaluate method.'
|
||||
|
||||
function Invoke-Curve(
|
||||
[double]$X0, [double]$Y0, [double]$Dx0, [double]$Dy0, [double]$Ddx0, [double]$Ddy0,
|
||||
[double]$X1, [double]$Y1, [double]$Dx1, [double]$Dy1, [double]$Ddx1, [double]$Ddy1) {
|
||||
$arguments = [object[]]@($X0, $Y0, $Dx0, $Dy0, $Ddx0, $Ddy0, $X1, $Y1, $Dx1, $Dy1, $Ddx1, $Ddy1, $null, $null)
|
||||
$accepted = $tryCreate.Invoke($null, $arguments)
|
||||
Assert-True $accepted "Quintic Hermite curve creation must succeed. Reason=$($arguments[13])"
|
||||
return $arguments[12]
|
||||
}
|
||||
|
||||
function Invoke-InvalidCurve([string]$CaseName) {
|
||||
$arguments = switch ($CaseName) {
|
||||
'ZeroFirstDerivative' { [object[]]@(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0, 0.0, 1.0, 0.0, 0.0, 0.0, $null, $null) }
|
||||
'NonFiniteInput' { [object[]]@(0.0, 0.0, [double]::NaN, 0.0, 0.0, 0.0, 2.0, 0.0, 1.0, 0.0, 0.0, 0.0, $null, $null) }
|
||||
default { throw "Unknown invalid curve case: $CaseName" }
|
||||
}
|
||||
return [PSCustomObject]@{
|
||||
Accepted = $tryCreate.Invoke($null, $arguments)
|
||||
Reason = [string]$arguments[13]
|
||||
}
|
||||
}
|
||||
|
||||
function Invoke-Evaluate($Curve, [double]$U) {
|
||||
$arguments = [object[]]@($U, $null, $null, $null, $null, $null, $null)
|
||||
$evaluate.Invoke($Curve, $arguments)
|
||||
return [PSCustomObject]@{
|
||||
X = [double]$arguments[1]; Y = [double]$arguments[2]
|
||||
Dx = [double]$arguments[3]; Dy = [double]$arguments[4]
|
||||
Ddx = [double]$arguments[5]; Ddy = [double]$arguments[6]
|
||||
}
|
||||
}
|
||||
|
||||
function Get-Curvature($Sample) {
|
||||
return ($Sample.Dx * $Sample.Ddy - $Sample.Dy * $Sample.Ddx) /
|
||||
[Math]::Pow($Sample.Dx * $Sample.Dx + $Sample.Dy * $Sample.Dy, 1.5)
|
||||
}
|
||||
|
||||
function Assert-Boundary($Expected, $Actual, [string]$Name) {
|
||||
Assert-Near $Expected.X $Actual.X 1e-10 "$Name X must match."
|
||||
Assert-Near $Expected.Y $Actual.Y 1e-10 "$Name Y must match."
|
||||
Assert-Near $Expected.Dx $Actual.Dx 1e-10 "$Name dX must match."
|
||||
Assert-Near $Expected.Dy $Actual.Dy 1e-10 "$Name dY must match."
|
||||
Assert-Near $Expected.Ddx $Actual.Ddx 1e-10 "$Name ddX must match."
|
||||
Assert-Near $Expected.Ddy $Actual.Ddy 1e-10 "$Name ddY must match."
|
||||
}
|
||||
|
||||
$straight = Invoke-Curve 0 0 1 0 0 0 2 0 1 0 0 0
|
||||
$straightStart = Invoke-Evaluate $straight 0.0
|
||||
$straightEnd = Invoke-Evaluate $straight 1.0
|
||||
$straightMid = Invoke-Evaluate $straight 0.5
|
||||
Assert-Boundary ([PSCustomObject]@{ X = 0.0; Y = 0.0; Dx = 1.0; Dy = 0.0; Ddx = 0.0; Ddy = 0.0 }) $straightStart 'Straight start'
|
||||
Assert-Boundary ([PSCustomObject]@{ X = 2.0; Y = 0.0; Dx = 1.0; Dy = 0.0; Ddx = 0.0; Ddy = 0.0 }) $straightEnd 'Straight end'
|
||||
Assert-Near 0.0 $straightMid.Y 1e-12 'A straight Hermite curve must remain on the axis.'
|
||||
|
||||
$curved = Invoke-Curve 0 0 1 0 0 0.4 1 1 0 1 -0.4 0
|
||||
$curvedStart = Invoke-Evaluate $curved 0.0
|
||||
$curvedEnd = Invoke-Evaluate $curved 1.0
|
||||
Assert-Boundary ([PSCustomObject]@{ X = 0.0; Y = 0.0; Dx = 1.0; Dy = 0.0; Ddx = 0.0; Ddy = 0.4 }) $curvedStart 'Curved start'
|
||||
Assert-Boundary ([PSCustomObject]@{ X = 1.0; Y = 1.0; Dx = 0.0; Dy = 1.0; Ddx = -0.4; Ddy = 0.0 }) $curvedEnd 'Curved end'
|
||||
Assert-Near 0.4 (Get-Curvature $curvedStart) 1e-10 'Start curvature must match boundary derivatives.'
|
||||
Assert-Near 0.4 (Get-Curvature $curvedEnd) 1e-10 'End curvature must match boundary derivatives.'
|
||||
|
||||
$allFinite = $true
|
||||
foreach ($u in @(0.0, 0.25, 0.5, 0.75, 1.0)) {
|
||||
$sample = Invoke-Evaluate $curved $u
|
||||
foreach ($value in @($sample.X, $sample.Y, $sample.Dx, $sample.Dy, $sample.Ddx, $sample.Ddy)) {
|
||||
$allFinite = $allFinite -and (-not [double]::IsNaN($value)) -and (-not [double]::IsInfinity($value))
|
||||
}
|
||||
}
|
||||
Assert-True $allFinite 'All sampled values must be finite.'
|
||||
|
||||
Assert-Rejected (Invoke-InvalidCurve 'ZeroFirstDerivative') `
|
||||
'A curve endpoint with zero first derivative must be rejected.'
|
||||
Assert-Rejected (Invoke-InvalidCurve 'NonFiniteInput') `
|
||||
'A curve with a non-finite boundary value must be rejected.'
|
||||
Assert-Throws { Invoke-Evaluate $curved -0.001 } 'Evaluate must reject parameters below zero.'
|
||||
Assert-Throws { Invoke-Evaluate $curved 1.001 } 'Evaluate must reject parameters above one.'
|
||||
|
||||
Write-Output 'Local G2 quintic curve checks passed.'
|
||||
Reference in New Issue
Block a user