#

vincentlu

2026-04-07 cc69250f7766581dbfd38f500021d03b29f0a4e0

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
package com.zy.acs.manager.core.utils;
 
import com.zy.acs.common.enums.ClockwiseType;
import com.zy.acs.manager.manager.entity.AgvModel;
import com.zy.acs.manager.manager.entity.Code;
 
/**
 * 车型角度工具：
 * 将地图物理方向（默认顺时针）转换为车型自身角度体系。
 */
public final class AgvAngleUtils {
 
    private AgvAngleUtils() {
    }
 
    public static Double calculateDirection(Code startCode, Code endCode, AgvModel agvModel) {
        if (startCode == null || endCode == null) {
            return null;
        }
        double deltaX = endCode.getX() - startCode.getX();
        double deltaY = endCode.getY() - startCode.getY();
        double mapAngle = Math.toDegrees(Math.atan2(deltaX, deltaY));
        return toModelAngle(normalizeAngle(mapAngle), agvModel);
    }
 
    public static Double toModelAngle(Double mapAngleDegrees, AgvModel agvModel) {
        if (mapAngleDegrees == null) {
            return null;
        }
        double factor = isClockwise(agvModel) ? 1D : -1D;
        double offset = getAngleOffsetDeg(agvModel);
        return normalizeAngle(factor * normalizeAngle(mapAngleDegrees) + offset);
    }
 
    public static Double fromModelAngle(Double modelAngleDegrees, AgvModel agvModel) {
        if (modelAngleDegrees == null) {
            return null;
        }
        double factor = isClockwise(agvModel) ? 1D : -1D;
        double offset = getAngleOffsetDeg(agvModel);
        return normalizeAngle(factor * (normalizeAngle(modelAngleDegrees) - offset));
    }
 
    public static Double toRadians(Double angleDegrees) {
        if (angleDegrees == null) {
            return null;
        }
        return Math.toRadians(normalizeAngle(angleDegrees));
    }
 
    public static Double fromRadians(Double angleRadians) {
        if (angleRadians == null) {
            return null;
        }
        return normalizeAngle(Math.toDegrees(angleRadians));
    }
 
    public static Double normalizeAngle(Double angleDegrees) {
        if (angleDegrees == null) {
            return null;
        }
        double normalized = angleDegrees % 360D;
        return normalized >= 0 ? normalized : normalized + 360D;
    }
 
    private static Double getAngleOffsetDeg(AgvModel agvModel) {
        return agvModel == null || agvModel.getAngleOffsetDeg() == null ? 0D : agvModel.getAngleOffsetDeg();
    }
 
    private static boolean isClockwise(AgvModel agvModel) {
        return agvModel == null
                || agvModel.getAngleClockwise() == null
                || agvModel.getAngleClockwise().equals(ClockwiseType.CW.value);
    }
}