139 lines
4.7 KiB
C#
139 lines
4.7 KiB
C#
using System;
|
|
|
|
namespace MyParking.Shared
|
|
{
|
|
/// <summary>
|
|
/// 提供与坐标系无关的角度归一化、角度差和单位转换功能。
|
|
/// </summary>
|
|
public static class AngleMath
|
|
{
|
|
public const double TwoPi = 2.0 * Math.PI;
|
|
|
|
/// <summary>
|
|
/// 将弧度归一化到[-π, π)区间。
|
|
/// -π包含在结果中，+π不包含在结果中，因此+π会返回-π。
|
|
/// </summary>
|
|
public static double NormalizeRadians(double angleRadians)
|
|
{
|
|
EnsureFinite(angleRadians, nameof(angleRadians));
|
|
|
|
var normalized = angleRadians % TwoPi;
|
|
|
|
if (normalized >= Math.PI)
|
|
{
|
|
normalized -= TwoPi;
|
|
}
|
|
else if (normalized < -Math.PI)
|
|
{
|
|
normalized += TwoPi;
|
|
}
|
|
|
|
return normalized == 0.0 ? 0.0 : normalized;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将角度归一化到[-180°, 180°)区间。
|
|
/// -180°包含在结果中，+180°不包含在结果中，因此+180°会返回-180°。
|
|
/// </summary>
|
|
public static double NormalizeDegrees(double angleDegrees)
|
|
{
|
|
EnsureFinite(angleDegrees, nameof(angleDegrees));
|
|
|
|
var normalized = angleDegrees % 360.0;
|
|
|
|
if (normalized >= 180.0)
|
|
{
|
|
normalized -= 360.0;
|
|
}
|
|
else if (normalized < -180.0)
|
|
{
|
|
normalized += 360.0;
|
|
}
|
|
|
|
return normalized == 0.0 ? 0.0 : normalized;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 计算从当前方向旋转到目标方向的最短有符号角度差，单位为弧度。
|
|
/// 返回值位于[-π, π)；正值表示逆时针，负值表示顺时针。
|
|
/// </summary>
|
|
public static double ShortestDifferenceRadians(
|
|
double targetRadians,
|
|
double currentRadians)
|
|
{
|
|
EnsureFinite(targetRadians, nameof(targetRadians));
|
|
EnsureFinite(currentRadians, nameof(currentRadians));
|
|
|
|
return NormalizeRadians(targetRadians - currentRadians);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 计算从当前方向旋转到目标方向的最短有符号角度差，单位为度。
|
|
/// 返回值位于[-180°, 180°)；正值表示逆时针，负值表示顺时针。
|
|
/// </summary>
|
|
public static double ShortestDifferenceDegrees(
|
|
double targetDegrees,
|
|
double currentDegrees)
|
|
{
|
|
EnsureFinite(targetDegrees, nameof(targetDegrees));
|
|
EnsureFinite(currentDegrees, nameof(currentDegrees));
|
|
|
|
return NormalizeDegrees(targetDegrees - currentDegrees);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 沿圆周最短方向在两个航向角之间插值，输入和结果单位均为弧度。
|
|
/// ratio为0时返回起始角，ratio为1时返回终止角；本方法不限制ratio，
|
|
/// 轨迹线段内插值时应先使用InterpolationMath.Clamp01进行限制。
|
|
/// 结果归一化到[-π, π)区间；角度差恰好为π时按负方向插值。
|
|
/// </summary>
|
|
public static double LerpRadians(
|
|
double startRadians,
|
|
double endRadians,
|
|
double ratio)
|
|
{
|
|
EnsureFinite(startRadians, nameof(startRadians));
|
|
EnsureFinite(endRadians, nameof(endRadians));
|
|
EnsureFinite(ratio, nameof(ratio));
|
|
|
|
var shortestDifference = ShortestDifferenceRadians(
|
|
endRadians,
|
|
startRadians);
|
|
|
|
return NormalizeRadians(
|
|
startRadians + ratio * shortestDifference);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将角度从度转换为弧度，不进行归一化。
|
|
/// </summary>
|
|
public static double DegreesToRadians(double angleDegrees)
|
|
{
|
|
EnsureFinite(angleDegrees, nameof(angleDegrees));
|
|
return angleDegrees * Math.PI / 180.0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将角度从弧度转换为度，不进行归一化。
|
|
/// </summary>
|
|
public static double RadiansToDegrees(double angleRadians)
|
|
{
|
|
EnsureFinite(angleRadians, nameof(angleRadians));
|
|
return angleRadians * 180.0 / Math.PI;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 验证角度是可用于计算的有限数值。
|
|
/// </summary>
|
|
private static void EnsureFinite(double angle, string parameterName)
|
|
{
|
|
if (double.IsNaN(angle) || double.IsInfinity(angle))
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
parameterName,
|
|
"角度必须是有限数值。");
|
|
}
|
|
}
|
|
}
|
|
}
|