#

vincentlu

2 天以前 7ce12b33107fb33941df7e1589bf1a6716539377

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
package com.zy.acs.manager.common.utils;
 
import com.alibaba.fastjson.JSON;
import com.zy.acs.framework.common.Cools;
import com.zy.acs.manager.core.constant.MapDataConstant;
import com.zy.acs.manager.core.domain.VehicleFootprint;
import com.zy.acs.manager.manager.entity.AgvModel;
 
import java.util.*;
 
/**
 * Created by vincent on 8/7/2024
 */
public class MapDataUtils {
 
    public static Double[][][] preComputeCdaMatrix(String[][] cdaStrMatrix) {
        int rows = cdaStrMatrix.length;
        int cols = cdaStrMatrix[0].length;
 
        Double[][][] cdaMatrix = new Double[rows][cols][2];
        for (int i = 0; i < rows; i++) {
            for (int j = 0; j < cols; j++) {
                List<Double> cda = MapDataUtils.parseCdaNode(cdaStrMatrix[i][j]);
                cdaMatrix[i][j][0] = cda.get(0);
                cdaMatrix[i][j][1] = cda.get(1);
            }
        }
 
        return cdaMatrix;
    }
 
    public static List<String> parseWaveNode(String waveNodeStr) {
        List<String> waveNodeList = new ArrayList<>();
        if (Cools.isEmpty(waveNodeStr)) {
            return waveNodeList;
        }
        return JSON.parseArray(waveNodeStr, String.class);
    }
 
    public static List<Double> parseCdaNode(String cdaNodeStr) {
        if (Cools.isEmpty(cdaNodeStr)) {
            return new ArrayList<>();
        }
        return JSON.parseArray(cdaNodeStr, Double.class);
    }
 
    public static List<String> hasOtherWave(List<String> list, String vehicle) {
        if (Cools.isEmpty(list)) {
            return list;
        }
        list.removeIf(next -> next.equals(vehicle));
        return list;
    }
 
    public static String generateWaveNode(String originStr, String waveNode) {
        List<String> originNode = JSON.parseArray(originStr, String.class);
        Set<String> set = new HashSet<>(originNode);
        set.add(waveNode);
        return JSON.toJSONString(set);
    }
 
    public static Double getVehicleWaveSafeDistance(Number mm) {
        return getVehicleWaveSafeDistance(mm, null);
    }
 
    public static Double getVehicleWaveSafeDistance(Number mm, Double factor) {
        if (mm == null) {
            throw new IllegalArgumentException("Invalid map length: " + mm);
        }
        double val = mm.doubleValue();
        if (val <= 0) {
            throw new IllegalArgumentException("Invalid map length: " + mm);
        }
        factor = Optional.ofNullable(factor).orElse(MapDataConstant.MAX_DISTANCE_BETWEEN_ADJACENT_AGV_FACTOR);
        return val * factor;
    }
 
    public static VehicleFootprint buildFootprint(AgvModel agvModel) {
        if (null == agvModel) {
            throw new IllegalArgumentException("AgvModel is null");
        }
        if (agvModel.getHeadOffset() == null || agvModel.getHeadOffset() <= 0) {
            throw new IllegalArgumentException("Invalid head offset: " + agvModel.getHeadOffset());
        }
        if (agvModel.getTailOffset() == null || agvModel.getTailOffset() <= 0) {
            throw new IllegalArgumentException("Invalid tail offset: " + agvModel.getTailOffset());
        }
        if (agvModel.getWidth() == null || agvModel.getWidth() <= 0) {
            throw new IllegalArgumentException("Invalid width: " + agvModel.getWidth());
        }
 
        double head = agvModel.getHeadOffset();
        double tail = agvModel.getTailOffset();
        double halfWidth = (double) agvModel.getWidth() / 2;
 
        return new VehicleFootprint(head, tail, halfWidth);
    }
 
}