#

vincentlu

2 天以前 7ce12b33107fb33941df7e1589bf1a6716539377

 zy-acs-manager/src/main/java/com/zy/acs/manager/common/utils/MapDataUtils.java

@@ -1,8 +1,10 @@
package com.zy.acs.manager.common.utils;

import com.alibaba.fastjson.JSON;
import com.zy.acs.common.utils.GsonUtils;
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.*;

@@ -10,6 +12,22 @@
 * 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<>();
@@ -20,9 +38,8 @@
    }

    public static List<Double> parseCdaNode(String cdaNodeStr) {
        List<Double> waveNodeList = new ArrayList<>();
        if (Cools.isEmpty(cdaNodeStr)) {
            return waveNodeList;
            return new ArrayList<>();
        }
        return JSON.parseArray(cdaNodeStr, Double.class);
    }
@@ -36,18 +53,47 @@
    }

    public static String generateWaveNode(String originStr, String waveNode) {
        List<String> originNode = GsonUtils.fromJsonToList(originStr, String.class);
        List<String> originNode = JSON.parseArray(originStr, String.class);
        Set<String> set = new HashSet<>(originNode);
        set.add(waveNode);
        return GsonUtils.toJson(set);
        return JSON.toJSONString(set);
    }

    public static Double getVehicleWaveSafeDistance(Integer diameter, Double factor) {
        if (Cools.isEmpty(diameter)) {
            return 0.0D;
    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);
        }
        factor = Optional.ofNullable(factor).orElse(1.0D);
        return diameter * factor;
        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);
    }

}