package com.zy.acs.manager.core.service.astart;
|
|
import com.fasterxml.jackson.databind.ObjectMapper;
|
import com.zy.acs.common.utils.RedisSupport;
|
import com.zy.acs.framework.common.Cools;
|
import com.zy.acs.manager.common.utils.MapDataUtils;
|
import com.zy.acs.manager.core.service.LaneBuilder;
|
import com.zy.acs.manager.core.service.astart.domain.AStarNavigateNode;
|
import com.zy.acs.manager.core.service.astart.domain.DynamicNode;
|
import com.zy.acs.manager.core.utils.RouteGenerator;
|
import com.zy.acs.manager.manager.entity.Segment;
|
import com.zy.acs.manager.manager.service.JamService;
|
import com.zy.acs.manager.system.service.ConfigService;
|
import lombok.extern.slf4j.Slf4j;
|
import org.springframework.beans.factory.annotation.Autowired;
|
import org.springframework.core.io.ClassPathResource;
|
import org.springframework.stereotype.Service;
|
|
import javax.annotation.PostConstruct;
|
import java.io.InputStream;
|
import java.util.*;
|
|
|
@Slf4j
|
@Service
|
public class AStarNavigateService {
|
|
private final RedisSupport redis = RedisSupport.defaultRedisSupport;
|
|
public static final boolean OPEN_TURN_COST_WEIGHT = Boolean.TRUE;
|
public static final int WEIGHT_CALC_FACTOR = 1;
|
|
// 四方向
|
private final static int[][] DIRECTIONS = {{0,1},{0,-1},{-1,0},{1,0}};
|
|
@Autowired
|
private MapDataDispatcher mapDataDispatcher;
|
@Autowired
|
private JamService jamService;
|
@Autowired
|
private LaneBuilder laneService;
|
@Autowired
|
private ConfigService configService;
|
|
/**
|
* 节点坐标缓存：codeData -> Coordinate
|
* 从json加载
|
*/
|
private Map<String, NodeCoordinate> nodeCoordinateMap = new HashMap<>();
|
|
public static class NodeCoordinate {
|
private double x;
|
private double y;
|
|
public NodeCoordinate() {}
|
|
public NodeCoordinate(double x, double y) {
|
this.x = x;
|
this.y = y;
|
}
|
|
public double getX() { return x; }
|
public void setX(double x) { this.x = x; }
|
public double getY() { return y; }
|
public void setY(double y) { this.y = y; }
|
|
/**
|
* 计算与另一个节点的距离（米）
|
*/
|
public double distanceTo(NodeCoordinate other) {
|
if (other == null) {
|
return 0.0;
|
}
|
double dx = (this.x - other.x) / 1000.0; // 毫米转米
|
double dy = (this.y - other.y) / 1000.0;
|
return Math.sqrt(dx * dx + dy * dy);
|
}
|
}
|
|
private static class MotionParams {
|
final double vMax; // 最大速度 (m/s)
|
final double aAccel; // 加速度 (m/s^2)
|
final double aDecel; // 减速度 (m/s^2)
|
final double controlPeriod; // 控制周期 (s)
|
final double fallbackGridLength; // 后备网格距离 (m)
|
|
MotionParams(double vMax, double aAccel, double aDecel, double controlPeriod, double fallbackGridLength) {
|
this.vMax = vMax;
|
this.aAccel = aAccel;
|
this.aDecel = aDecel;
|
this.controlPeriod = controlPeriod;
|
this.fallbackGridLength = fallbackGridLength;
|
}
|
}
|
|
@PostConstruct
|
public void loadNodeCoordinates() {
|
try {
|
// 文件路径
|
String coordinateFile = getStringConfig("nodeCoordinateFile", "man_code.json");
|
|
ClassPathResource resource = new ClassPathResource(coordinateFile);
|
InputStream inputStream = resource.getInputStream();
|
|
ObjectMapper objectMapper = new ObjectMapper();
|
Map<String, Object> jsonData = objectMapper.readValue(inputStream, Map.class);
|
|
if (jsonData.containsKey("path_id_to_coordinates")) {
|
Map<String, List<Map<String, Object>>> pathData =
|
(Map<String, List<Map<String, Object>>>) jsonData.get("path_id_to_coordinates");
|
|
for (Map.Entry<String, List<Map<String, Object>>> entry : pathData.entrySet()) {
|
String nodeCode = entry.getKey();
|
List<Map<String, Object>> coordinates = entry.getValue();
|
|
if (coordinates != null && !coordinates.isEmpty()) {
|
Map<String, Object> coord = coordinates.get(0);
|
double x = ((Number) coord.get("x")).doubleValue();
|
double y = ((Number) coord.get("y")).doubleValue();
|
nodeCoordinateMap.put(nodeCode, new NodeCoordinate(x, y));
|
}
|
}
|
|
log.info("success load node coordinates, total {} nodes", nodeCoordinateMap.size());
|
} else {
|
log.warn("path_id_to_coordinates not found");
|
}
|
|
} catch (Exception e) {
|
log.error("load node coordinates failed", e);
|
}
|
}
|
|
private String getStringConfig(String key, String defaultVal) {
|
try {
|
String v = configService.getVal(key, String.class);
|
if (v != null && !v.isEmpty()) {
|
return v;
|
}
|
} catch (Exception e) {
|
log.debug("config {} not available, use default value {}", key, defaultVal);
|
}
|
return defaultVal;
|
}
|
|
public synchronized AStarNavigateNode execute(String agvNo,
|
AStarNavigateNode start,
|
AStarNavigateNode end,
|
Boolean lock,
|
List<String> blackList,
|
Segment segment) {
|
if (start.getX() == end.getX() && start.getY() == end.getY()) {
|
return end;
|
}
|
Integer maxAgvCountInLane = configService.getVal("maxAgvCountInLane", Integer.class);
|
|
// ===== 1. 基于物理参数计算时间步相关的参数 =====
|
// 网格中心距（米）- 默认后备值，当无法获取实际坐标时使用
|
double gridLength = getDoubleConfig("navigateGridLength", 0.5);
|
// 控制周期（秒/时间步）
|
double controlPeriod = getDoubleConfig("navigateControlPeriod", 0.5);
|
|
// AGV 物理特性
|
double vMax = getDoubleConfig("agvMaxSpeed", 1.0); // m/s
|
double aAccel = getDoubleConfig("agvAccel", 0.4); // m/s^2
|
double aDecel = getDoubleConfig("agvDecel", 0.4); // m/s^2
|
double tTurn = getDoubleConfig("agvTurn90TimeSec", 4.0); // s
|
|
// 安全时间间隔（秒）→ 时间步
|
double safetyGapTime = getDoubleConfig("navigateSafetyGapTimeSec", 3.0);
|
|
// 转 90° 的时间（秒）直接用配置
|
double turnTimeSec = tTurn;
|
// 转弯时间步成本
|
final int turnStepCost = Math.max(1, (int) Math.ceil(turnTimeSec / controlPeriod));
|
final int safetyGapStep = Math.max(1, (int) Math.ceil(safetyGapTime / controlPeriod));
|
|
// 运动学参数打包
|
final MotionParams motionParams = new MotionParams(vMax, aAccel, aDecel, controlPeriod, gridLength);
|
|
if (log.isDebugEnabled()) {
|
log.debug("A* phys params init: gridLength(fallback)={}, controlPeriod={}, vMax={}, aAccel={}, aDecel={}, " +
|
"tTurn={}, turnStepCost={}, safetyGapStep={}, loaded node coordinates={}",
|
gridLength, controlPeriod, vMax, aAccel, aDecel,
|
tTurn, turnStepCost, safetyGapStep, nodeCoordinateMap.size());
|
}
|
|
// ===== 2. A* 初始化 =====
|
PriorityQueue<AStarNavigateNode> openQueue = new PriorityQueue<>();
|
Set<AStarNavigateNode> existNodes = new HashSet<>();
|
Map<String, Integer> bestGMap = new HashMap<>();
|
|
start.setG(0);
|
start.setH(calcNodeCost(start, end));
|
start.setF(start.getG() + start.getH());
|
// 时间步从 0 开始
|
start.setStep(0);
|
String startKey = start.getX() + "_" + start.getY();
|
bestGMap.put(startKey, start.getG());
|
|
openQueue.add(start);
|
existNodes.add(start);
|
|
int[][] mapMatrix = mapDataDispatcher.getMapMatrix(null, null);
|
String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(null);
|
DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(null);
|
String[][] waveMatrix = mapDataDispatcher.getWaveMatrix(null);
|
|
// ===== 3. 基于 dynamicMatrix 估算各 AGV 当前进度 =====
|
Map<String, Integer> currentSerialMap = buildCurrentSerialMap(dynamicMatrix, agvNo);
|
|
long getNeighborNodesTime = 0;
|
int getNeighborNodesCount = 0;
|
|
while (!openQueue.isEmpty()) {
|
// 取 F 值最小的节点
|
AStarNavigateNode currentNode = openQueue.poll();
|
|
// 已到终点
|
if (currentNode.getX() == end.getX() && currentNode.getY() == end.getY()) {
|
return currentNode;
|
}
|
|
long currentTime = System.currentTimeMillis();
|
List<AStarNavigateNode> neighbourNodes =
|
this.getNeighborNodes(currentNode, mapMatrix, codeMatrix, existNodes,
|
motionParams, turnStepCost);
|
getNeighborNodesTime += System.currentTimeMillis() - currentTime;
|
getNeighborNodesCount++;
|
|
for (AStarNavigateNode node : neighbourNodes) {
|
node.setCodeData(codeMatrix[node.getX()][node.getY()]);
|
|
boolean isEndNode = node.getX() == end.getX() && node.getY() == end.getY();
|
|
int weight = 0;
|
|
if (!Cools.isEmpty(blackList) && blackList.contains(node.getCodeData())) {
|
continue;
|
}
|
// 特殊：blackList 只有 start 节点，避免走直接相邻最短路
|
if (blackList.size() == 1 && blackList.get(0).equals(start.getCodeData())) {
|
if (isEndNode && currentNode.getCodeData().equals(start.getCodeData())) {
|
continue;
|
}
|
}
|
|
// ===== 3.1 动态避障：基于时间步的时空冲突判断 =====
|
DynamicNode dynamicNode = dynamicMatrix[node.getX()][node.getY()];
|
String vehicle = dynamicNode.getVehicle();
|
assert !vehicle.equals(DynamicNodeType.BLOCK.val);
|
|
if (!vehicle.equals(DynamicNodeType.ACCESS.val)) {
|
if (!vehicle.equals(agvNo)) {
|
boolean conflict = true;
|
|
Integer otherCurrentSerial = currentSerialMap.get(vehicle);
|
int otherSerial = dynamicNode.getSerial();
|
|
if (otherCurrentSerial != null) {
|
int remainStep = otherSerial - otherCurrentSerial;
|
if (remainStep < 0) {
|
remainStep = 0;
|
}
|
// 我从起点到该节点需要的时间步
|
int myStep = node.getStep();
|
|
// 如果我到时对方已经离开，并且预留安全间隔，则允许共享该节点
|
if (myStep > remainStep + safetyGapStep) {
|
conflict = false;
|
}
|
}
|
|
if (conflict) {
|
if (lock) {
|
// 锁定模式下，直接视为不可走
|
continue;
|
} else {
|
// 非锁模式：允许通过，但增加代价
|
weight += (WEIGHT_CALC_FACTOR * 2);
|
}
|
}
|
|
// 拥堵历史加权（与时间冲突判断正交）
|
if (null != segment) {
|
if (!Cools.isEmpty(jamService.getJamFromSegmentByAvo(segment, vehicle))) {
|
weight += (WEIGHT_CALC_FACTOR * 3);
|
} else {
|
weight += (WEIGHT_CALC_FACTOR * 2);
|
}
|
}
|
}
|
}
|
|
// ===== 3.2 避障波 waveMatrix 判断 =====
|
String waveNode = waveMatrix[node.getX()][node.getY()];
|
assert !waveNode.equals(WaveNodeType.DISABLE.val);
|
if (!waveNode.equals(WaveNodeType.ENABLE.val)) {
|
List<String> waveNodeList = MapDataUtils.parseWaveNode(waveNode);
|
List<String> otherWaveList = MapDataUtils.hasOtherWave(waveNodeList, agvNo);
|
if (!Cools.isEmpty(otherWaveList)) {
|
if (lock) {
|
continue;
|
}
|
}
|
}
|
|
// ===== 3.3 单巷道容量控制 =====
|
List<int[]> laneCodeIdxList = laneService.getLaneCodeIdxList(node.getCodeData());
|
if (!Cools.isEmpty(laneCodeIdxList)) {
|
Set<String> lanVehicleSet = new HashSet<>();
|
|
for (int[] codeMatrixIdx : laneCodeIdxList) {
|
DynamicNode laneDynamicNode = dynamicMatrix[codeMatrixIdx[0]][codeMatrixIdx[1]];
|
String laneVehicle = laneDynamicNode.getVehicle();
|
assert !laneVehicle.equals(DynamicNodeType.BLOCK.val);
|
if (!laneVehicle.equals(DynamicNodeType.ACCESS.val)) {
|
if (!laneVehicle.equals(agvNo)) {
|
lanVehicleSet.add(laneVehicle);
|
}
|
}
|
}
|
if (lanVehicleSet.size() + 1 > maxAgvCountInLane) {
|
continue;
|
}
|
}
|
|
// ===== 3.4 转弯额外代价（在步长基础上加权） =====
|
if (OPEN_TURN_COST_WEIGHT) {
|
if (this.isTurning(currentNode, node)) {
|
weight += WEIGHT_CALC_FACTOR;
|
node.setTurnCount(currentNode.getTurnCount() + 1);
|
} else {
|
node.setTurnCount(currentNode.getTurnCount());
|
}
|
}
|
|
// ===== 3.5 计算 G/F/H 等 =====
|
node.setWeight(weight);
|
node.setLastDistance(calcNodeCost(currentNode, node));
|
node.initNode(currentNode, end);
|
node.setH(calcNodeCost(node, end));
|
node.setF(node.getG() + node.getH());
|
|
String key = node.getX() + "_" + node.getY();
|
Integer recordedG = bestGMap.get(key);
|
if (recordedG == null || node.getG() <= recordedG) {
|
bestGMap.put(key, node.getG());
|
openQueue.add(node);
|
}
|
}
|
}
|
return null;
|
}
|
|
/**
|
* 获取当前节点的四周邻居节点
|
*
|
* @param currentNode 当前节点
|
* @param mapMatrix 地图矩阵
|
* @param codeMatrix 节点编码矩阵
|
* @param existNodes 已存在节点集合
|
* @param motionParams AGV运动学参数
|
* @param turnStepCost 转弯时间步成本
|
* @return 邻居节点列表
|
*/
|
private List<AStarNavigateNode> getNeighborNodes(AStarNavigateNode currentNode,
|
int[][] mapMatrix,
|
String[][] codeMatrix,
|
Set<AStarNavigateNode> existNodes,
|
MotionParams motionParams,
|
int turnStepCost) {
|
int x = currentNode.getX();
|
int y = currentNode.getY();
|
AStarNavigateNode parent = currentNode.getParent();
|
|
List<AStarNavigateNode> neighbourNodes = new ArrayList<>();
|
List<AStarNavigateNode> possibleNodes = new ArrayList<>();
|
|
// 遍历四个方向：右、左、上、下
|
for (int[] d: DIRECTIONS) {
|
int nx = x + d[0];
|
int ny = y + d[1];
|
// 不回到父节点
|
if (parent != null && nx == parent.getX() && ny == parent.getY()) {
|
continue;
|
}
|
possibleNodes.add(new AStarNavigateNode(nx, ny));
|
}
|
|
// 扩展每个可能的邻居节点
|
for (AStarNavigateNode pn : possibleNodes) {
|
AStarNavigateNode next = extendNeighborNodes(currentNode, pn, mapMatrix, codeMatrix,
|
existNodes, null, null, motionParams, turnStepCost);
|
if (next != null) {
|
neighbourNodes.add(next);
|
}
|
}
|
|
return neighbourNodes;
|
}
|
|
/**
|
* 扩展邻居节点，计算基于实际距离的时间步成本
|
*
|
* @param currentNode 当前节点
|
* @param extendNode 待扩展节点
|
* @param mapMatrix 地图矩阵
|
* @param codeMatrix 节点编码矩阵
|
* @param existNodes 已存在节点集合
|
* @param dx x方向增量（递归调用时使用）
|
* @param dy y方向增量（递归调用时使用）
|
* @param motionParams AGV运动学参数
|
* @param turnStepCost 转弯时间步成本
|
* @return 扩展后的节点，若不可达则返回null
|
*/
|
private AStarNavigateNode extendNeighborNodes(AStarNavigateNode currentNode,
|
AStarNavigateNode extendNode,
|
int[][] mapMatrix,
|
String[][] codeMatrix,
|
Set<AStarNavigateNode> existNodes,
|
Integer dx,
|
Integer dy,
|
MotionParams motionParams,
|
int turnStepCost) {
|
AStarNavigateNode nextNode;
|
|
if (dx == null || dy == null) {
|
dx = extendNode.getX() - currentNode.getX();
|
dy = extendNode.getY() - currentNode.getY();
|
nextNode = extendNode;
|
} else {
|
nextNode = new AStarNavigateNode(extendNode.getX() + dx, extendNode.getY() + dy);
|
}
|
|
int x = nextNode.getX();
|
int y = nextNode.getY();
|
|
// 数组越界检查
|
if (x < 0 || x >= mapMatrix.length || y < 0 || y >= mapMatrix[0].length) {
|
return null;
|
}
|
|
// 遇到不可通行节点，继续沿当前方向扩展
|
if (mapMatrix[x][y] == MapNodeType.DISABLE.val) {
|
return extendNeighborNodes(currentNode, nextNode, mapMatrix, codeMatrix,
|
existNodes, dx, dy, motionParams, turnStepCost);
|
}
|
|
assert mapMatrix[x][y] == MapNodeType.ENABLE.val;
|
|
// 判断通道通过性
|
String routeCdaKey = RouteGenerator.generateRouteCdaKey(
|
new int[]{currentNode.getX(), currentNode.getY()},
|
new int[]{nextNode.getX(), nextNode.getY()});
|
if (!mapDataDispatcher.validRouteCdaKey(routeCdaKey)) {
|
return null;
|
}
|
|
// ===== 基于实际坐标计算节点间距离和时间步 =====
|
boolean turning = isTurning(currentNode, nextNode);
|
int deltaStep;
|
|
if (turning) {
|
// 转弯：使用固定的转弯时间步
|
deltaStep = turnStepCost;
|
} else {
|
// 直行：根据实际距离计算时间步
|
String currentCode = codeMatrix[currentNode.getX()][currentNode.getY()];
|
String nextCode = codeMatrix[x][y];
|
|
// 从坐标映射获取实际距离
|
double actualDistance = calculateActualDistance(currentCode, nextCode, motionParams.fallbackGridLength);
|
|
// 基于实际距离和运动学模型计算所需时间
|
double travelTimeSec = calcStraightTravelTimeSec(actualDistance,
|
motionParams.vMax, motionParams.aAccel, motionParams.aDecel);
|
|
// 转换为时间步（至少为1）
|
deltaStep = Math.max(1, (int) Math.ceil(travelTimeSec / motionParams.controlPeriod));
|
|
if (log.isTraceEnabled()) {
|
log.trace("节点 {} -> {} 距离={}m, 时间={}s, 时间步={}",
|
currentCode, nextCode, actualDistance, travelTimeSec, deltaStep);
|
}
|
}
|
|
// 设置累计时间步
|
nextNode.setStep(currentNode.getStep() + deltaStep);
|
|
return nextNode;
|
}
|
|
/**
|
* 计算两个节点间的实际距离
|
* 优先使用坐标映射计算欧几里得距离，若无坐标数据则使用后备网格距离
|
*
|
* @param fromCode 起始节点编码
|
* @param toCode 目标节点编码
|
* @param fallbackDistance 后备距离（米）
|
* @return 实际距离（米）
|
*/
|
private double calculateActualDistance(String fromCode, String toCode, double fallbackDistance) {
|
NodeCoordinate fromCoord = nodeCoordinateMap.get(fromCode);
|
NodeCoordinate toCoord = nodeCoordinateMap.get(toCode);
|
|
if (fromCoord != null && toCoord != null) {
|
return fromCoord.distanceTo(toCoord);
|
} else {
|
if (log.isTraceEnabled() && (fromCoord == null || toCoord == null)) {
|
log.trace("节点 {} 或 {} 缺少坐标数据，使用后备距离 {}m",
|
fromCode, toCode, fallbackDistance);
|
}
|
return fallbackDistance;
|
}
|
}
|
|
//------------------A* 启发函数------------------//
|
|
// 曼哈顿距离
|
private int calcNodeCost(AStarNavigateNode node1, AStarNavigateNode node2) {
|
return Math.abs(node2.getX() - node1.getX()) + Math.abs(node2.getY() - node1.getY());
|
}
|
|
/**
|
* 根据当前 dynamicMatrix 预估每辆车的“当前时间步”（使用最小 serial 作为近似）。
|
*/
|
private Map<String, Integer> buildCurrentSerialMap(DynamicNode[][] dynamicMatrix, String selfAgvNo) {
|
Map<String, Integer> result = new HashMap<>();
|
if (dynamicMatrix == null) {
|
return result;
|
}
|
for (int i = 0; i < dynamicMatrix.length; i++) {
|
DynamicNode[] row = dynamicMatrix[i];
|
if (row == null) {
|
continue;
|
}
|
for (int j = 0; j < row.length; j++) {
|
DynamicNode node = row[j];
|
if (node == null) {
|
continue;
|
}
|
String vehicle = node.getVehicle();
|
if (vehicle == null
|
|| DynamicNodeType.ACCESS.val.equals(vehicle)
|
|| DynamicNodeType.BLOCK.val.equals(vehicle)
|
|| vehicle.equals(selfAgvNo)) {
|
continue;
|
}
|
int serial = node.getSerial();
|
Integer recorded = result.get(vehicle);
|
if (recorded == null || serial < recorded) {
|
result.put(vehicle, serial);
|
}
|
}
|
}
|
return result;
|
}
|
|
// 转弯判断：是否改变了方向
|
private boolean isTurning(AStarNavigateNode currNode, AStarNavigateNode nextNode) {
|
if (currNode.getParent() == null) {
|
return false;
|
}
|
AStarNavigateNode parent = currNode.getParent();
|
// 如果下一点与 parent 在同一行或同一列 => 没有转弯
|
if (nextNode.getX() == parent.getX() || nextNode.getY() == parent.getY()) {
|
return false;
|
}
|
return true;
|
}
|
|
// --------- 物理参数 → 时间步 的换算辅助方法 --------- //
|
|
/**
|
* 从配置中心读取 double，失败则返回默认值
|
*/
|
private double getDoubleConfig(String key, double defaultVal) {
|
try {
|
Double v = configService.getVal(key, Double.class);
|
if (v != null && v > 0) {
|
return v;
|
}
|
} catch (Exception e) {
|
log.warn("config {} not available, use default {}", key, defaultVal);
|
}
|
return defaultVal;
|
}
|
|
/**
|
* 计算一格直线移动的物理时间（秒）
|
* 使用标准的加速-匀速-减速 或 三角速度曲线模型：
|
*
|
* - 先算达到 vmax 所需的加速距离 s_acc = vmax^2 / (2 a_acc)
|
* - 减速距离 s_dec = vmax^2 / (2 a_dec)
|
* - 若 dist >= s_acc + s_dec：能跑到 vmax，则 t = t_acc + t_dec + t_cruise
|
* - 否则：达不到 vmax，采用三角形速度曲线，求峰值速度 v_peak，再算 t_acc + t_dec
|
*/
|
private double calcStraightTravelTimeSec(double dist,
|
double vMax,
|
double aAcc,
|
double aDec) {
|
if (dist <= 0) {
|
return 0.0;
|
}
|
// 避免参数异常
|
vMax = Math.max(vMax, 0.01);
|
aAcc = Math.max(aAcc, 0.01);
|
aDec = Math.max(aDec, 0.01);
|
|
double sAcc = vMax * vMax / (2.0 * aAcc);
|
double sDec = vMax * vMax / (2.0 * aDec);
|
|
// 情况1：距离足够长，可以加速到 vmax 再减速
|
if (dist >= sAcc + sDec) {
|
double tAcc = vMax / aAcc;
|
double tDec = vMax / aDec;
|
double sCruise = dist - sAcc - sDec;
|
double tCruise = sCruise / vMax;
|
return tAcc + tCruise + tDec;
|
} else {
|
// 情况2：距离较短，达不到 vmax，采用三角速度曲线
|
// dist = v_peak^2 / (2 aAcc) + v_peak^2 / (2 aDec)
|
double denom = (1.0 / aAcc + 1.0 / aDec);
|
double vPeak = Math.sqrt(2.0 * dist / denom);
|
double tAcc = vPeak / aAcc;
|
double tDec = vPeak / aDec;
|
return tAcc + tDec;
|
}
|
}
|
|
}
|
