818 lines
32 KiB
C#
818 lines
32 KiB
C#
// using ClumsyCore;
|
||
// using ClumsyCore.DTools;
|
||
// using ClumsyCore.Interfaces;
|
||
// using ClumsyCore.Pilot;
|
||
// using CommonUsage.Chassis;
|
||
// using MyParking.Shared;
|
||
// using System;
|
||
// using System.Collections.Generic;
|
||
// using System.Numerics;
|
||
|
||
// namespace MultiWheelC
|
||
// {
|
||
// // C层单车测试：在可配置的运动坐标系中统一跟踪直线、圆弧或S型曲线。
|
||
// public sealed class CrabMotionFrameTracker : MovementDefinition
|
||
// {
|
||
// public enum ReferencePathKind
|
||
// {
|
||
// Straight = 0,
|
||
// LeftArc = 1,
|
||
// SCurve = 2
|
||
// }
|
||
|
||
// public enum ChassisCommandBackend
|
||
// {
|
||
// SendXYThSpeed = 0,
|
||
// SendMotion = 1
|
||
// }
|
||
|
||
// public ReferencePathKind PathKind;
|
||
// public ChassisCommandBackend CommandBackend =
|
||
// ChassisCommandBackend.SendMotion;
|
||
// public Vector2 StartPosition;
|
||
// public double InitialBodyYawRadians;
|
||
// public float LengthMillimeters = 4000f;
|
||
// public float RadiusMillimeters = 2000f;
|
||
// public float SCurveLateralOffsetMillimeters = 400f;
|
||
// public double ArcSweepRadians = Math.PI / 2.0;
|
||
// public float CruiseSpeed = 0.2f;
|
||
// public float SlowDistanceMillimeters = 600f;
|
||
// public float FinishDistanceMillimeters = 30f;
|
||
// public float MinimumSpeed = 0.04f;
|
||
// public double LateralGainPerSecond = 0.8;
|
||
// public double MaximumLateralCorrection = 0.12;
|
||
// public double HeadingGainPerSecond = 1.5;
|
||
// public double MaximumAngularSpeedRadiansPerSecond =
|
||
// AngleMath.DegreesToRadians(30.0);
|
||
// public double MaximumVirtualSteeringRadians =
|
||
// AngleMath.DegreesToRadians(30.0);
|
||
// public float WheelAlignmentToleranceDegrees = 2f;
|
||
// public float WheelAlignmentStableSeconds = 0.3f;
|
||
// public float WheelAlignmentTimeoutSeconds = 10f;
|
||
// public float TrackingTimeoutSeconds = 60f;
|
||
// public Action<float, float, float> CommandObserver;
|
||
|
||
// // 运动坐标系相对车体坐标系的朝向：普通模式为0，蟹行为π/2。
|
||
// public double MotionFrameYawInBodyRadians = Math.PI / 2.0;
|
||
// private double _lastSCurveProgress;
|
||
|
||
// public override IEnumerable<bool> Get()
|
||
// {
|
||
// ValidateParameters();
|
||
|
||
// var chassis =
|
||
// PilotDefinition.Chassis as MultiWheelChassis;
|
||
// if (chassis == null)
|
||
// throw new InvalidOperationException(
|
||
// "当前底盘不是MultiWheelChassis，无法执行运动坐标系轨迹测试。");
|
||
|
||
// var adapter = new MultiWheelChassisAdapter(
|
||
// chassis,
|
||
// PilotDefinition.Self.CarNum);
|
||
// adapter.ResetToBodyFrame();
|
||
|
||
// var lastCommandTime = DateTime.Now;
|
||
|
||
// try
|
||
// {
|
||
// // 模式切换阶段只转舵轮，驱动速度始终保持为零。
|
||
// var alignmentStarted = DateTime.Now;
|
||
// DateTime? stableSince = null;
|
||
// while (true)
|
||
// {
|
||
// if (!adapter.PrepareParallelDirection(
|
||
// MotionFrameYawInBodyRadians))
|
||
// throw new InvalidOperationException(
|
||
// "无法生成运动坐标系对应的舵轮准备姿态。");
|
||
|
||
// var aligned =
|
||
// adapter.AreParallelWheelsAligned(
|
||
// MotionFrameYawInBodyRadians,
|
||
// AngleMath.DegreesToRadians(
|
||
// WheelAlignmentToleranceDegrees));
|
||
|
||
// if (aligned)
|
||
// {
|
||
// if (stableSince == null)
|
||
// stableSince = DateTime.Now;
|
||
|
||
// if ((DateTime.Now - stableSince.Value)
|
||
// .TotalSeconds >=
|
||
// WheelAlignmentStableSeconds)
|
||
// break;
|
||
// }
|
||
// else
|
||
// {
|
||
// stableSince = null;
|
||
// }
|
||
|
||
// if ((DateTime.Now - alignmentStarted)
|
||
// .TotalSeconds >
|
||
// WheelAlignmentTimeoutSeconds)
|
||
// throw new TimeoutException(
|
||
// "舵轮在限定时间内未稳定到达运动坐标系初始方向。");
|
||
|
||
// yield return true;
|
||
// }
|
||
|
||
// if (CommandBackend ==
|
||
// ChassisCommandBackend.SendMotion)
|
||
// {
|
||
// // 舵轮已按真实机械角度完成预对齐；
|
||
// // 现在由Shared适配层激活SendMotion虚拟运动坐标系。
|
||
// adapter.ActivateMotionFrame(
|
||
// MotionFrameYawInBodyRadians);
|
||
// }
|
||
|
||
// var trackingStarted = DateTime.Now;
|
||
// while (true)
|
||
// {
|
||
// if ((DateTime.Now - trackingStarted)
|
||
// .TotalSeconds >
|
||
// TrackingTimeoutSeconds)
|
||
// throw new TimeoutException(
|
||
// "蟹行轨迹在限定时间内未完成。");
|
||
|
||
// var location =
|
||
// DetourInterface.getCartLocation();
|
||
// if (!IsFinite(location.x) ||
|
||
// !IsFinite(location.y) ||
|
||
// !IsFinite(location.th))
|
||
// throw new InvalidOperationException(
|
||
// "蟹行轨迹测试期间Detour位姿无效。");
|
||
|
||
// var currentPosition = new Vector2(
|
||
// (float)location.x,
|
||
// (float)location.y);
|
||
// var currentBodyYaw =
|
||
// AngleMath.DegreesToRadians(location.th);
|
||
|
||
// CalculateReference(
|
||
// currentPosition,
|
||
// out var tangentYaw,
|
||
// out var referencePoint,
|
||
// out var remainingMillimeters,
|
||
// out var referenceCurvature);
|
||
|
||
// if (remainingMillimeters <=
|
||
// FinishDistanceMillimeters)
|
||
// break;
|
||
|
||
// var speed =
|
||
// CalculateSpeed(remainingMillimeters);
|
||
// var tangent = new Vector2(
|
||
// (float)Math.Cos(tangentYaw),
|
||
// (float)Math.Sin(tangentYaw));
|
||
// var leftNormal = new Vector2(
|
||
// -tangent.Y,
|
||
// tangent.X);
|
||
// var positionError =
|
||
// currentPosition - referencePoint;
|
||
// var lateralErrorMeters =
|
||
// Vector2.Dot(
|
||
// positionError,
|
||
// leftNormal) / 1000.0;
|
||
// var normalCorrection =
|
||
// Limit(
|
||
// -LateralGainPerSecond *
|
||
// lateralErrorMeters,
|
||
// MaximumLateralCorrection);
|
||
|
||
// // 先在世界坐标中组合切向速度与横向纠偏速度。
|
||
// var worldVx =
|
||
// tangent.X * speed +
|
||
// leftNormal.X * (float)normalCorrection;
|
||
// var worldVy =
|
||
// tangent.Y * speed +
|
||
// leftNormal.Y * (float)normalCorrection;
|
||
|
||
// // 将世界速度表达为当前蟹行运动坐标系速度。
|
||
// var motionYaw =
|
||
// currentBodyYaw +
|
||
// MotionFrameYawInBodyRadians;
|
||
// var motionCos = Math.Cos(motionYaw);
|
||
// var motionSin = Math.Sin(motionYaw);
|
||
// var vxInMotion =
|
||
// motionCos * worldVx +
|
||
// motionSin * worldVy;
|
||
// var vyInMotion =
|
||
// -motionSin * worldVx +
|
||
// motionCos * worldVy;
|
||
|
||
// var desiredBodyYaw =
|
||
// tangentYaw -
|
||
// MotionFrameYawInBodyRadians;
|
||
// var headingError =
|
||
// AngleMath.ShortestDifferenceRadians(
|
||
// desiredBodyYaw,
|
||
// currentBodyYaw);
|
||
// var omega =
|
||
// speed * referenceCurvature +
|
||
// HeadingGainPerSecond * headingError;
|
||
// omega = Limit(
|
||
// omega,
|
||
// MaximumAngularSpeedRadiansPerSecond);
|
||
|
||
// var now = DateTime.Now;
|
||
// var interval = now - lastCommandTime;
|
||
// lastCommandTime = now;
|
||
|
||
// bool commandAccepted;
|
||
// Twist2D bodyTwist;
|
||
// if (CommandBackend ==
|
||
// ChassisCommandBackend.SendMotion)
|
||
// {
|
||
// // 运动坐标系相对车体系旋转+90°：
|
||
// // 运动系正向速度会转换成车体系+Y速度。
|
||
// bodyTwist =
|
||
// FrameTransform2D
|
||
// .TransformTwistAtSamePoint(
|
||
// new Pose2D(
|
||
// 0.0,
|
||
// 0.0,
|
||
// MotionFrameYawInBodyRadians),
|
||
// new Twist2D(
|
||
// vxInMotion,
|
||
// vyInMotion,
|
||
// omega));
|
||
|
||
// // 将运动坐标系原点和前后几何控制点处的速度，
|
||
// // 转换为SendMotion需要的前后轴方向。
|
||
// var controlPointRadiusMeters =
|
||
// Math.Max(
|
||
// chassis.ControlPointRadius /
|
||
// 1000.0,
|
||
// 0.001);
|
||
// var frontVelocityY =
|
||
// vyInMotion +
|
||
// omega *
|
||
// controlPointRadiusMeters;
|
||
// var rearVelocityY =
|
||
// vyInMotion -
|
||
// omega *
|
||
// controlPointRadiusMeters;
|
||
// var frontSteeringRadians =
|
||
// Math.Atan2(
|
||
// frontVelocityY,
|
||
// vxInMotion);
|
||
// var rearSteeringRadians =
|
||
// Math.Atan2(
|
||
// rearVelocityY,
|
||
// vxInMotion);
|
||
|
||
// // 蟹行测试绕过M层ManualControl并直接调用SendMotion，
|
||
// // 因此需要在C层同步应用蟹行虚拟几何比例和转向符号。
|
||
// if (IsCrabMotionFrame())
|
||
// {
|
||
// var geometryRatio =
|
||
// adapter.HalfTrackWidthMeters /
|
||
// adapter.HalfWheelBaseMeters;
|
||
|
||
// frontSteeringRadians =
|
||
// ConvertToCrabSteering(
|
||
// frontSteeringRadians,
|
||
// geometryRatio);
|
||
// rearSteeringRadians =
|
||
// ConvertToCrabSteering(
|
||
// rearSteeringRadians,
|
||
// geometryRatio);
|
||
// }
|
||
|
||
// var frontThetaDegrees =
|
||
// (float)AngleMath.RadiansToDegrees(
|
||
// frontSteeringRadians);
|
||
// var rearThetaDegrees =
|
||
// (float)AngleMath.RadiansToDegrees(
|
||
// rearSteeringRadians);
|
||
// var motionSpeed =
|
||
// (float)Math.Sqrt(
|
||
// vxInMotion * vxInMotion +
|
||
// vyInMotion * vyInMotion);
|
||
|
||
// commandAccepted =
|
||
// chassis.SendMotion(
|
||
// motionSpeed,
|
||
// frontThetaDegrees,
|
||
// rearThetaDegrees,
|
||
// interval);
|
||
// }
|
||
// else if (CommandBackend ==
|
||
// ChassisCommandBackend
|
||
// .SendXYThSpeed)
|
||
// {
|
||
// // 安全XYTh后端根据舵角误差统一压低驱动轮速。
|
||
// bodyTwist =
|
||
// FrameTransform2D
|
||
// .TransformTwistAtSamePoint(
|
||
// new Pose2D(
|
||
// 0.0,
|
||
// 0.0,
|
||
// MotionFrameYawInBodyRadians),
|
||
// new Twist2D(
|
||
// vxInMotion,
|
||
// vyInMotion,
|
||
// omega));
|
||
// var command = new ChassisCommand(
|
||
// PilotDefinition.Self.CarNum,
|
||
// bodyTwist);
|
||
// commandAccepted =
|
||
// adapter.Send(
|
||
// command,
|
||
// interval);
|
||
// }
|
||
// else
|
||
// {
|
||
// throw new InvalidOperationException(
|
||
// $"不支持的底盘命令后端：{CommandBackend}。");
|
||
// }
|
||
|
||
// if (!commandAccepted)
|
||
// throw new InvalidOperationException(
|
||
// "运动坐标系轨迹底盘解算失败：" +
|
||
// chassis
|
||
// .LastMotionDecomposeFailureReason);
|
||
|
||
// CommandObserver?.Invoke(
|
||
// (float)bodyTwist.VxMetersPerSecond,
|
||
// (float)bodyTwist.VyMetersPerSecond,
|
||
// (float)bodyTwist
|
||
// .OmegaRadiansPerSecond);
|
||
|
||
// yield return true;
|
||
// }
|
||
// }
|
||
// finally
|
||
// {
|
||
// adapter.StopImmediately();
|
||
// if (CommandBackend ==
|
||
// ChassisCommandBackend.SendMotion)
|
||
// {
|
||
// // 测试退出后恢复真实车体坐标系，避免影响后续测试。
|
||
// adapter.ResetToBodyFrame();
|
||
// }
|
||
// CommandObserver?.Invoke(0f, 0f, 0f);
|
||
// }
|
||
|
||
// yield return false;
|
||
// }
|
||
|
||
// // 判断当前运动坐标系是否为车体左侧朝前的蟹行坐标系。
|
||
// private bool IsCrabMotionFrame()
|
||
// {
|
||
// return Math.Abs(
|
||
// AngleMath.ShortestDifferenceRadians(
|
||
// Math.PI / 2.0,
|
||
// MotionFrameYawInBodyRadians)) <
|
||
// 1e-6;
|
||
// }
|
||
|
||
// // 按车体几何比例缩小蟹行转角。
|
||
// // +90°运动坐标系已经完成方向映射，此处不能再次反号。
|
||
// private double ConvertToCrabSteering(
|
||
// double normalSteeringRadians,
|
||
// double geometryRatio)
|
||
// {
|
||
// var crabSteeringRadians =
|
||
// Math.Atan(
|
||
// geometryRatio *
|
||
// Math.Tan(
|
||
// normalSteeringRadians));
|
||
|
||
// return Limit(
|
||
// crabSteeringRadians,
|
||
// MaximumVirtualSteeringRadians);
|
||
// }
|
||
|
||
// // 计算当前点在直线或圆弧上的参考点、切线和剩余距离。
|
||
// private void CalculateReference(
|
||
// Vector2 currentPosition,
|
||
// out double tangentYaw,
|
||
// out Vector2 referencePoint,
|
||
// out float remainingMillimeters,
|
||
// out double curvaturePerMeter)
|
||
// {
|
||
// var initialMotionYaw =
|
||
// InitialBodyYawRadians +
|
||
// MotionFrameYawInBodyRadians;
|
||
|
||
// if (PathKind == ReferencePathKind.Straight)
|
||
// {
|
||
// var tangent = new Vector2(
|
||
// (float)Math.Cos(initialMotionYaw),
|
||
// (float)Math.Sin(initialMotionYaw));
|
||
// var relative = currentPosition - StartPosition;
|
||
// var progress =
|
||
// Vector2.Dot(relative, tangent);
|
||
// var clampedProgress =
|
||
// Math.Max(
|
||
// 0f,
|
||
// Math.Min(progress, LengthMillimeters));
|
||
|
||
// tangentYaw = initialMotionYaw;
|
||
// referencePoint =
|
||
// StartPosition +
|
||
// tangent * clampedProgress;
|
||
// remainingMillimeters =
|
||
// Math.Max(
|
||
// 0f,
|
||
// LengthMillimeters - progress);
|
||
// curvaturePerMeter = 0.0;
|
||
// return;
|
||
// }
|
||
|
||
// if (PathKind == ReferencePathKind.SCurve)
|
||
// {
|
||
// CalculateSCurveReference(
|
||
// currentPosition,
|
||
// initialMotionYaw,
|
||
// out tangentYaw,
|
||
// out referencePoint,
|
||
// out remainingMillimeters,
|
||
// out curvaturePerMeter);
|
||
// return;
|
||
// }
|
||
|
||
// var center = GetArcCenter();
|
||
// var startRadialYaw =
|
||
// initialMotionYaw - Math.PI / 2.0;
|
||
// var radial = currentPosition - center;
|
||
// var currentRadialYaw =
|
||
// Math.Atan2(radial.Y, radial.X);
|
||
// var progressRadians =
|
||
// AngleMath.NormalizeRadians(
|
||
// currentRadialYaw - startRadialYaw);
|
||
|
||
// // 测试圆弧只有+90°，起点附近的轻微负噪声按0处理。
|
||
// if (progressRadians < 0.0)
|
||
// progressRadians = 0.0;
|
||
|
||
// var clampedProgressRadians =
|
||
// Math.Min(
|
||
// progressRadians,
|
||
// ArcSweepRadians);
|
||
// var referenceRadialYaw =
|
||
// startRadialYaw +
|
||
// clampedProgressRadians;
|
||
// referencePoint = center + new Vector2(
|
||
// RadiusMillimeters *
|
||
// (float)Math.Cos(referenceRadialYaw),
|
||
// RadiusMillimeters *
|
||
// (float)Math.Sin(referenceRadialYaw));
|
||
// tangentYaw =
|
||
// referenceRadialYaw + Math.PI / 2.0;
|
||
// remainingMillimeters =
|
||
// (float)Math.Max(
|
||
// 0.0,
|
||
// (ArcSweepRadians - progressRadians) *
|
||
// RadiusMillimeters);
|
||
// curvaturePerMeter =
|
||
// 1000.0 / RadiusMillimeters;
|
||
// }
|
||
|
||
// // 通过离散最近点和解析导数计算两段三次贝塞尔S曲线的参考状态。
|
||
// private void CalculateSCurveReference(
|
||
// Vector2 currentPosition,
|
||
// double initialMotionYaw,
|
||
// out double tangentYaw,
|
||
// out Vector2 referencePoint,
|
||
// out float remainingMillimeters,
|
||
// out double curvaturePerMeter)
|
||
// {
|
||
// const int nearestPointSamples = 200;
|
||
// var searchStart =
|
||
// Math.Max(
|
||
// 0.0,
|
||
// _lastSCurveProgress - 0.02);
|
||
// var bestProgress = _lastSCurveProgress;
|
||
// var bestDistanceSquared = double.MaxValue;
|
||
|
||
// for (var i = 0;
|
||
// i <= nearestPointSamples;
|
||
// i++)
|
||
// {
|
||
// var progress =
|
||
// searchStart +
|
||
// (1.0 - searchStart) *
|
||
// i / nearestPointSamples;
|
||
// EvaluateSCurve(
|
||
// progress,
|
||
// out var localPoint,
|
||
// out _,
|
||
// out _);
|
||
// var worldPoint =
|
||
// LocalPathPointToWorld(
|
||
// localPoint,
|
||
// initialMotionYaw);
|
||
// var distanceSquared =
|
||
// Vector2.DistanceSquared(
|
||
// currentPosition,
|
||
// worldPoint);
|
||
|
||
// if (distanceSquared <
|
||
// bestDistanceSquared)
|
||
// {
|
||
// bestDistanceSquared =
|
||
// distanceSquared;
|
||
// bestProgress = progress;
|
||
// }
|
||
// }
|
||
|
||
// // 轨迹进度不允许因定位噪声倒退，防止控制目标跳回上一段曲线。
|
||
// _lastSCurveProgress =
|
||
// Math.Max(
|
||
// _lastSCurveProgress,
|
||
// bestProgress);
|
||
// EvaluateSCurve(
|
||
// _lastSCurveProgress,
|
||
// out var bestLocalPoint,
|
||
// out var firstDerivative,
|
||
// out var secondDerivative);
|
||
// referencePoint =
|
||
// LocalPathPointToWorld(
|
||
// bestLocalPoint,
|
||
// initialMotionYaw);
|
||
// tangentYaw =
|
||
// initialMotionYaw +
|
||
// Math.Atan2(
|
||
// firstDerivative.Y,
|
||
// firstDerivative.X);
|
||
|
||
// var derivativeMagnitude =
|
||
// Math.Sqrt(
|
||
// firstDerivative.X *
|
||
// firstDerivative.X +
|
||
// firstDerivative.Y *
|
||
// firstDerivative.Y);
|
||
// if (derivativeMagnitude < 1e-6)
|
||
// {
|
||
// curvaturePerMeter = 0.0;
|
||
// }
|
||
// else
|
||
// {
|
||
// // 导数单位为mm，乘1000后将曲率从1/mm转换成1/m。
|
||
// curvaturePerMeter =
|
||
// (firstDerivative.X *
|
||
// secondDerivative.Y -
|
||
// firstDerivative.Y *
|
||
// secondDerivative.X) *
|
||
// 1000.0 /
|
||
// Math.Pow(
|
||
// derivativeMagnitude,
|
||
// 3.0);
|
||
// }
|
||
|
||
// remainingMillimeters =
|
||
// ApproximateSCurveRemainingLength(
|
||
// _lastSCurveProgress);
|
||
// }
|
||
|
||
// // 计算与普通4m S型测试完全一致的三段三次贝塞尔完整S曲线。
|
||
// private void EvaluateSCurve(
|
||
// double progress,
|
||
// out Vector2 point,
|
||
// out Vector2 firstDerivative,
|
||
// out Vector2 secondDerivative)
|
||
// {
|
||
// progress =
|
||
// Math.Max(
|
||
// 0.0,
|
||
// Math.Min(progress, 1.0));
|
||
|
||
// Vector2 p0;
|
||
// Vector2 p1;
|
||
// Vector2 p2;
|
||
// Vector2 p3;
|
||
// double t;
|
||
|
||
// if (progress <= 0.25)
|
||
// {
|
||
// t = progress * 4.0;
|
||
// p0 = new Vector2(0f, 0f);
|
||
// p1 = new Vector2(
|
||
// LengthMillimeters / 12f,
|
||
// 0f);
|
||
// p2 = new Vector2(
|
||
// LengthMillimeters / 6f,
|
||
// SCurveLateralOffsetMillimeters);
|
||
// p3 = new Vector2(
|
||
// LengthMillimeters * 0.25f,
|
||
// SCurveLateralOffsetMillimeters);
|
||
// }
|
||
// else if (progress <= 0.75)
|
||
// {
|
||
// t = (progress - 0.25) * 2.0;
|
||
// p0 = new Vector2(
|
||
// LengthMillimeters * 0.25f,
|
||
// SCurveLateralOffsetMillimeters);
|
||
// p1 = new Vector2(
|
||
// LengthMillimeters / 3f,
|
||
// SCurveLateralOffsetMillimeters);
|
||
// p2 = new Vector2(
|
||
// LengthMillimeters * 2f / 3f,
|
||
// -SCurveLateralOffsetMillimeters);
|
||
// p3 = new Vector2(
|
||
// LengthMillimeters * 0.75f,
|
||
// -SCurveLateralOffsetMillimeters);
|
||
// }
|
||
// else
|
||
// {
|
||
// t = (progress - 0.75) * 4.0;
|
||
// p0 = new Vector2(
|
||
// LengthMillimeters * 0.75f,
|
||
// -SCurveLateralOffsetMillimeters);
|
||
// p1 = new Vector2(
|
||
// LengthMillimeters * 5f / 6f,
|
||
// -SCurveLateralOffsetMillimeters);
|
||
// p2 = new Vector2(
|
||
// LengthMillimeters * 11f / 12f,
|
||
// 0f);
|
||
// p3 = new Vector2(
|
||
// LengthMillimeters,
|
||
// 0f);
|
||
// }
|
||
|
||
// var oneMinusT = 1.0 - t;
|
||
// point =
|
||
// p0 * (float)(
|
||
// oneMinusT *
|
||
// oneMinusT *
|
||
// oneMinusT) +
|
||
// p1 * (float)(
|
||
// 3.0 *
|
||
// oneMinusT *
|
||
// oneMinusT *
|
||
// t) +
|
||
// p2 * (float)(
|
||
// 3.0 *
|
||
// oneMinusT *
|
||
// t *
|
||
// t) +
|
||
// p3 * (float)(t * t * t);
|
||
// firstDerivative =
|
||
// (p1 - p0) *
|
||
// (float)(
|
||
// 3.0 *
|
||
// oneMinusT *
|
||
// oneMinusT) +
|
||
// (p2 - p1) *
|
||
// (float)(
|
||
// 6.0 *
|
||
// oneMinusT *
|
||
// t) +
|
||
// (p3 - p2) *
|
||
// (float)(3.0 * t * t);
|
||
// secondDerivative =
|
||
// (p2 - 2f * p1 + p0) *
|
||
// (float)(6.0 * oneMinusT) +
|
||
// (p3 - 2f * p2 + p1) *
|
||
// (float)(6.0 * t);
|
||
// }
|
||
|
||
// // 通过分段采样估算从当前S曲线进度到终点的实际弧长。
|
||
// private float ApproximateSCurveRemainingLength(
|
||
// double startProgress)
|
||
// {
|
||
// const int lengthSamples = 100;
|
||
// EvaluateSCurve(
|
||
// startProgress,
|
||
// out var previousPoint,
|
||
// out _,
|
||
// out _);
|
||
// var length = 0f;
|
||
|
||
// for (var i = 1;
|
||
// i <= lengthSamples;
|
||
// i++)
|
||
// {
|
||
// var progress =
|
||
// startProgress +
|
||
// (1.0 - startProgress) *
|
||
// i / lengthSamples;
|
||
// EvaluateSCurve(
|
||
// progress,
|
||
// out var point,
|
||
// out _,
|
||
// out _);
|
||
// length +=
|
||
// Vector2.Distance(
|
||
// previousPoint,
|
||
// point);
|
||
// previousPoint = point;
|
||
// }
|
||
|
||
// return length;
|
||
// }
|
||
|
||
// // 将以初始蟹行方向为X轴的局部路径点转换到Detour世界坐标。
|
||
// private Vector2 LocalPathPointToWorld(
|
||
// Vector2 localPoint,
|
||
// double initialMotionYaw)
|
||
// {
|
||
// var cos =
|
||
// (float)Math.Cos(initialMotionYaw);
|
||
// var sin =
|
||
// (float)Math.Sin(initialMotionYaw);
|
||
|
||
// return StartPosition + new Vector2(
|
||
// localPoint.X * cos -
|
||
// localPoint.Y * sin,
|
||
// localPoint.X * sin +
|
||
// localPoint.Y * cos);
|
||
// }
|
||
|
||
// // 获取蟹行左转圆弧圆心；它位于初始运动方向的左侧。
|
||
// public Vector2 GetArcCenter()
|
||
// {
|
||
// var initialMotionYaw =
|
||
// InitialBodyYawRadians +
|
||
// MotionFrameYawInBodyRadians;
|
||
// return StartPosition + new Vector2(
|
||
// -RadiusMillimeters *
|
||
// (float)Math.Sin(initialMotionYaw),
|
||
// RadiusMillimeters *
|
||
// (float)Math.Cos(initialMotionYaw));
|
||
// }
|
||
|
||
// // 获取圆弧测试的理论终点。
|
||
// public Vector2 GetArcDestination()
|
||
// {
|
||
// var initialMotionYaw =
|
||
// InitialBodyYawRadians +
|
||
// MotionFrameYawInBodyRadians;
|
||
// var startRadialYaw =
|
||
// initialMotionYaw - Math.PI / 2.0;
|
||
// var endRadialYaw =
|
||
// startRadialYaw + ArcSweepRadians;
|
||
// var center = GetArcCenter();
|
||
|
||
// return center + new Vector2(
|
||
// RadiusMillimeters *
|
||
// (float)Math.Cos(endRadialYaw),
|
||
// RadiusMillimeters *
|
||
// (float)Math.Sin(endRadialYaw));
|
||
// }
|
||
|
||
// // 根据剩余路径长度生成终点减速速度。
|
||
// private float CalculateSpeed(
|
||
// float remainingMillimeters)
|
||
// {
|
||
// if (remainingMillimeters >=
|
||
// SlowDistanceMillimeters)
|
||
// return CruiseSpeed;
|
||
|
||
// var ratio =
|
||
// remainingMillimeters /
|
||
// Math.Max(
|
||
// SlowDistanceMillimeters,
|
||
// 1f);
|
||
// return Math.Max(
|
||
// MinimumSpeed,
|
||
// CruiseSpeed * ratio);
|
||
// }
|
||
|
||
// private void ValidateParameters()
|
||
// {
|
||
// if (CruiseSpeed <= 0f ||
|
||
// !IsFinite(CruiseSpeed) ||
|
||
// LengthMillimeters <= 0f ||
|
||
// !IsFinite(LengthMillimeters) ||
|
||
// RadiusMillimeters <= 0f ||
|
||
// !IsFinite(RadiusMillimeters) ||
|
||
// SCurveLateralOffsetMillimeters <= 0f ||
|
||
// !IsFinite(
|
||
// SCurveLateralOffsetMillimeters) ||
|
||
// ArcSweepRadians <= 0.0 ||
|
||
// !IsFinite(ArcSweepRadians) ||
|
||
// SlowDistanceMillimeters <= 0f ||
|
||
// !IsFinite(SlowDistanceMillimeters) ||
|
||
// FinishDistanceMillimeters < 0f ||
|
||
// !IsFinite(FinishDistanceMillimeters) ||
|
||
// TrackingTimeoutSeconds <= 0f ||
|
||
// !IsFinite(TrackingTimeoutSeconds) ||
|
||
// MaximumVirtualSteeringRadians <= 0.0 ||
|
||
// MaximumVirtualSteeringRadians >=
|
||
// Math.PI / 2.0 ||
|
||
// !IsFinite(
|
||
// MaximumVirtualSteeringRadians))
|
||
// throw new ArgumentOutOfRangeException(
|
||
// "蟹行轨迹测试参数无效。");
|
||
// }
|
||
|
||
// private static double Limit(
|
||
// double value,
|
||
// double absoluteLimit)
|
||
// {
|
||
// return Math.Max(
|
||
// -absoluteLimit,
|
||
// Math.Min(value, absoluteLimit));
|
||
// }
|
||
|
||
// private static bool IsFinite(double value)
|
||
// {
|
||
// return
|
||
// !double.IsNaN(value) &&
|
||
// !double.IsInfinity(value);
|
||
// }
|
||
// }
|
||
// }
|