35 lines
776 B
C
35 lines
776 B
C
/*
|
|
* SPDX-FileCopyrightText: 2024-2025 Espressif Systems (Shanghai) CO LTD
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
|
|
#include "domain_internal.h"
|
|
|
|
/* 角度统一归一化到 (-180, 180]，为目录匹配和最短旋转提供唯一表示。 */
|
|
float domain_angle_normalize(float angle)
|
|
{
|
|
while (angle > 180.0f) {
|
|
angle -= 360.0f;
|
|
}
|
|
while (angle <= -180.0f) {
|
|
angle += 360.0f;
|
|
}
|
|
return angle;
|
|
}
|
|
|
|
float domain_angle_shortest_rotation(float current, float target)
|
|
{
|
|
/* 结果限制到 [-180, 180]，正负号决定方向，避免不必要的整圈运动。 */
|
|
float diff = target - current;
|
|
|
|
while (diff > 180.0f) {
|
|
diff -= 360.0f;
|
|
}
|
|
while (diff < -180.0f) {
|
|
diff += 360.0f;
|
|
}
|
|
|
|
return diff;
|
|
}
|