AngleConstants.cs
5.32 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
namespace Rcs.Domain.ValueObjects
{
/// <summary>
/// 角度常量与工具方法 - 使用弧度（以 π 为单位）表示角度
/// </summary>
public static class AngleConstants
{
/// <summary>
/// 0 度（0 弧度）
/// </summary>
public const double Zero = 0.0;
/// <summary>
/// 15 度（π/12 弧度）- VDA5050 默认角度偏移
/// </summary>
public const double Degrees15 = Math.PI / 12;
/// <summary>
/// 允许的最大角度偏差（弧度）
/// </summary>
public const double AllowedDeviationTheta = Math.PI / 12;
/// <summary>
/// 取放货点位偏差弧度
/// </summary>
public const double EndDeviationTheta = Math.PI / 36;
/// <summary>
/// 30 度（π/6 弧度）
/// </summary>
public const double Degrees30 = Math.PI / 6;
/// <summary>
/// 45 度（π/4 弧度）
/// </summary>
public const double Degrees45 = Math.PI / 4;
/// <summary>
/// 60 度（π/3 弧度）
/// </summary>
public const double Degrees60 = Math.PI / 3;
/// <summary>
/// 90 度（π/2 弧度）
/// </summary>
public const double Degrees90 = Math.PI / 2;
/// <summary>
/// 120 度（2π/3 弧度）
/// </summary>
public const double Degrees120 = 2 * Math.PI / 3;
/// <summary>
/// 135 度（3π/4 弧度）
/// </summary>
public const double Degrees135 = 3 * Math.PI / 4;
/// <summary>
/// 150 度（5π/6 弧度）
/// </summary>
public const double Degrees150 = 5 * Math.PI / 6;
/// <summary>
/// 180 度（π 弧度）
/// </summary>
public const double Degrees180 = Math.PI;
/// <summary>
/// 360 度（2π 弧度）
/// </summary>
public const double Degrees360 = 2 * Math.PI;
/// <summary>
/// 整圆（2π 弧度）
/// </summary>
public const double FullCircle = 2 * Math.PI;
/// <summary>
/// 15 度（π/12 弧度）的常量（可用于组合负角）
/// </summary>
public const double MDegrees15 = Math.PI / 12;
/// <summary>
/// 30 度（π/6 弧度）的常量（可用于组合负角）
/// </summary>
public const double MDegrees30 = Math.PI / 6;
/// <summary>
/// 45 度（π/4 弧度）的常量（可用于组合负角）
/// </summary>
public const double MDegrees45 = Math.PI / 4;
/// <summary>
/// 60 度（π/3 弧度）的常量（可用于组合负角）
/// </summary>
public const double MDegrees60 = Math.PI / 3;
/// <summary>
/// 90 度（π/2 弧度）的常量（可用于组合负角）
/// </summary>
public const double MDegrees90 = Math.PI / 2;
/// <summary>
/// 120 度（2π/3 弧度）的常量（可用于组合负角）
/// </summary>
public const double MDegrees120 = 2 * Math.PI / 3;
/// <summary>
/// 135 度（3π/4 弧度）的常量（可用于组合负角）
/// </summary>
public const double MDegrees135 = 3 * Math.PI / 4;
/// <summary>
/// 150 度（5π/6 弧度）的常量（可用于组合负角）
/// </summary>
public const double MDegrees150 = 5 * Math.PI / 6;
public static double ToRadians(double degrees)
{
var radians = degrees * Math.PI / 180.0;
return NormalizeAngleSymmetric(radians);
}
/// <summary>
/// 将弧度转换为角度
/// </summary>
/// <param name="radians">弧度值</param>
/// <returns>角度</returns>
public static double ToDegrees(double radians)
{
return radians * 180.0 / Math.PI;
}
/// <summary>
/// 将角度标准化到 [0, 2π) 范围
/// </summary>
/// <param name="angle">角度（弧度）</param>
/// <returns>标准化后的角度</returns>
public static double NormalizeAngle(double angle)
{
while (angle < 0)
angle += FullCircle;
while (angle >= FullCircle)
angle -= FullCircle;
return angle;
}
/// <summary>
/// 将角度标准化到 [-π, π] 范围
/// </summary>
/// <param name="angle">角度（弧度）</param>
/// <returns>标准化后的角度</returns>
public static double NormalizeAngleSymmetric(double angle)
{
angle = NormalizeAngle(angle);
if (angle > Math.PI)
angle -= FullCircle;
return angle;
}
/// <summary>
/// 计算两个角度之间的最小差值（绝对值）
/// </summary>
/// <param name="angle1">第一个角度（弧度）</param>
/// <param name="angle2">第二个角度（弧度）</param>
/// <returns>最小角度差（弧度）</returns>
public static double AngleDifference(double angle1, double angle2)
{
double diff = NormalizeAngleSymmetric(angle2 - angle1);
return Math.Abs(diff);
}
}
}