function [path, totalCost, totalDistance, totalTime, totalRE]totalRE, nodeId] = AStarPathTD(nodes, adjacencyMatrix3D, heuristicMatrix, start, goal, Kd, Kt, Ke, cost_calc, buildingPositions, buildingSizes, r, smooth)
% Find index of start and goal nodes
[~, startIndex] = min(pdist2(nodes, start));
[~, goalIndex] = min(pdist2(nodes, goal));
if ~smooth
connectedToStart = getConnectedNodesfind(adjacencyMatrix3D(startIndex,:,1) < inf & adjacencyMatrix3D(startIndex,:,1) > 0); %getConnectedNodes(startIndex, nodes, adjacencyMatrix3D, r, buildingPositions, buildingSizes);
connectedToEnd = getConnectedNodesfind(adjacencyMatrix3D(goalIndex,:,1) < inf & adjacencyMatrix3D(goalIndex,:,1) > 0); %getConnectedNodes(goalIndex, nodes, adjacencyMatrix3D, r, buildingPositions, buildingSizes);
if isempty(connectedToStart) || isempty(connectedToEnd)
[path,if isempty(connectedToEnd) && isempty(connectedToStart)
nodeId = [startIndex; goalIndex];
elseif isempty(connectedToEnd) && ~isempty(connectedToStart)
nodeId = goalIndex;
elseif isempty(connectedToStart) && ~isempty(connectedToEnd)
nodeId = startIndex;
end
path = [];
totalCost, = [];
totalDistance, = [];
totalTime, totalRE]= [];
totalRE = deal([]);[];
return;
end
end
% Bidirectional search setup
openSetF = MinHeap(); % From start
openSetB = MinHeap(); % From goal
openSetF = insert(openSetF, startIndex, 0);
openSetB = insert(openSetB, goalIndex, 0);
numNodes = size(nodes, 1);
LARGENUMBER = 10e10;
gScoreF = infLARGENUMBER * ones(numNodes, 1); % Future cost from start
gScoreB = infLARGENUMBER * ones(numNodes, 1); % Future cost from goal
fScoreF = infLARGENUMBER * ones(numNodes, 1); % Total cost from start
fScoreB = infLARGENUMBER * ones(numNodes, 1); % Total cost from goal
gScoreF(startIndex) = 0;
gScoreB(goalIndex) = 0;
fScoreF(startIndex) = calculateCost(heuristicMatrix(startIndex, 1), heuristicMatrix(startIndex, 2), heuristicMatrix(startIndex, 3), Kd, Kt, Ke, cost_calc);
fScoreB(goalIndex) = calculateCost(heuristicMatrix(goalIndex, 1), heuristicMatrix(goalIndex, 2), heuristicMatrix(goalIndex, 3), Kd, Kt, Ke, cost_calc);
cameFromF = cell(numNodes, 1); % Path tracking from start
cameFromB = cell(numNodes, 1); % Path tracking from goal
% Early exit flag
isPathFound = false;
meetingPoint = -1;
%pre pre computing costs
heuristicCosts = calculateCost(heuristicMatrix(:,1), heuristicMatrix(:,2), heuristicMatrix(:,3), Kd, Kt, Ke, cost_calc);
costMatrix = inf(numNodes, numNodes);
for i = 1:numNodes
for j = i +1: numNodes
if adjacencyMatrix3D(i,j,1) < inf
costMatrix(i,j) = calculateCost(adjacencyMatrix3D(i,j,1), adjacencyMatrix3D(i,j,2), adjacencyMatrix3D(i,j,3), Kd, Kt, Ke, cost_calc);
costMatrix(j,i) = costMatrix(i,j);
end
end
end
costMatrix = sparse(costMatrix);
%initial costs
fScoreF(startIndex) = heuristicCosts(startIndex);
fScoreB(goalIndex) = heuristicCosts(goalIndex);
%KD Tree
kdtree = KDTreeSearcher(nodes);
% Main loop
while ~isEmpty(openSetF) && ~isEmpty(openSetB)
% Forward search
[openSetF, currentF] = extractMin(openSetF);
if isfinite(fScoreF(currentF)) && isfinite(fScoreB(currentF))
if fScoreF(currentF) + fScoreB(currentF) < infLARGENUMBER % Possible meeting point
isPathFound = true;
meetingPoint = currentF;
break;
end
end
% Process neighbors in parallel
neighborsF = find(adjacencyMatrix3D(currentF, :, 1) < inf & adjacencyMatrix3D(currentF, :, 1) > 0);
tentative_gScoresF = inf(1, numel(neighborsF));
tentativeFScoreF = inf(1, numel(neighborsF));
validNeighborsFgScoreFCurrent = false(1, numelgScoreF(neighborsF)currentF);
parfor icostF = 1:numelcostMatrix(neighborsF)
neighbor =currentF, neighborsF(i);
tentative_gScoreFtentative_gScoresF = gScoreF(currentF)gScoreFCurrent + calculateCost(adjacencyMatrix3D(currentF,costF;
neighbor, 1), adjacencyMatrix3D(currentF, neighbor,validMask 2),= adjacencyMatrix3D~isinf(currentF, neighbor, 3), Kd, Kt, Ke, cost_calctentative_gScoresF);
ifvalidNeighborsF = ~isinf(tentative_gScoresFneighborsF(i)validMask);
validHScoresF = validNeighborsFheuristicCosts(ivalidNeighborsF) = true; ;
tentativeFScoreF(ivalidMask) = tentative_gScoresF(ivalidMask) + calculateCost(heuristicMatrix(neighbor, 1),validHScoresF';
heuristicMatrix(neighbor, 2), heuristicMatrix(neighbor, 3), Kd, Kt,%validNeighborsF Ke,= cost_calcneighborsF(validMask);
validTentative_gScoresF = tentative_gScoresF(validMask);
end
validTentativeFScoreF = endtentativeFScoreF(validMask);
for i = findnumel(validNeighborsF)
neighbor = neighborsFvalidNeighborsF(i);
tentative_gScore = tentative_gScoresFvalidTentative_gScoresF(i);
if tentative_gScore < gScoreF(neighbor)
cameFromF{neighbor} = currentF;
gScoreF(neighbor) = tentative_gScore;
fScoreF(neighbor) = tentativeFScoreFvalidTentativeFScoreF(i);
openSetF = insert(openSetF, neighbor, fScoreF(neighbor));
end
end
% Backward search
[openSetB, currentB] = extractMin(openSetB);
if isfinite(fScoreF(currentB)) && isfinite(fScoreB(currentB))
if fScoreF(currentB) + fScoreB(currentB) < infLARGENUMBER % Possible meeting point
isPathFound = true;
meetingPoint = currentB;
break;
end
end
% Process neighbors in parallel
neighborsB = find(adjacencyMatrix3D(currentB, :, 1) < inf & adjacencyMatrix3D(currentB, :, 1) > 0);
tentative_gScoresB = inf(1, numel(neighborsB));
tentativeFScoreB = inf(1, numel(neighborsB));
validNeighborsBgScoreBCurrent = falsegScoreB(1currentB);
tentative_gScoresB = gScoreBCurrent + costMatrix(currentB, numelneighborsB);
validMask = ~isinf(tentative_gScoresB);
validNeighborsB = neighborsB(validMask);
validHScoreB = heuristicCosts(validNeighborsB);
tentativeFScoreB(validMask) = tentative_gScoresB(validMask) + validHScoreB';
%validNeighborsB = neighborsB(validMask);
validTentative_gScoresB = tentative_gScoresB(validMask);
validTentativeFScoresB = tentativeFScoreB(validMask);
parfor i = 1:numel(neighborsB)
neighbor = neighborsB(i);
tentative_gScoreB(i) = gScoreB(currentB) + calculateCost(adjacencyMatrix3D(currentB, neighbor, 1), adjacencyMatrix3D(currentB, neighbor, 2), adjacencyMatrix3D(currentB, neighbor, 3), Kd, Kt, Ke, cost_calc);
if ~isinf(tentative_gScoresB(i))
validNeighborsB(i) = true;
tentativeFScoreB(i) = tentative_gScoreB(i) + calculateCost(heuristicMatrix(neighbor, 1), heuristicMatrix(neighbor, 2), heuristicMatrix(neighbor, 3), Kd, Kt, Ke, cost_calc);
end
end
for i = findnumel(validNeighborsB)
neighbor = neighborsBvalidNeighborsB(i);
tentative_gScore = tentative_gScoresBvalidTentative_gScoresB(i);
if tentative_gScore < gScoreB(neighbor)
cameFromB{neighbor} = currentB;
gScoreB(neighbor) = tentative_gScore;
fScoreB(neighbor) = tentativeFScoreBvalidTentativeFScoresB(i);
openSetB = insert(openSetB, neighbor, fScoreB(neighbor));
end
end
end
if isPathFound
pathF = reconstructPath(cameFromF, meetingPoint, nodes);
pathB = reconstructPath(cameFromB, meetingPoint, nodes);
pathB = flipud(pathB);
path = [pathF; pathB(2:end, :)]; % Concatenate paths
totalCost = fScoreF(meetingPoint) + fScoreB(meetingPoint);
totalDistance
pathIndices = sum(adjacencyMatrix3D(sub2ind(sizeknnsearch(adjacencyMatrix3D)kdtree, path(1:end-1,1), path(2:end,1)'K', ones(size(path,1)-1,1))));
totalDistance = 0;
totalTime = sum(adjacencyMatrix3D(sub2ind0;
totalRE = 0;
for i = 1:(sizenumel(adjacencyMatrix3DpathIndices), path(1:end-1, 1),
path idx1 = pathIndices(2:end,1i),;
2*ones idx2 = pathIndices(sizei+1);
totalDistance = totalDistance + adjacencyMatrix3D(pathidx1,1)-1 idx2, 1))));
totalRE totalTime = sum(totalTime + adjacencyMatrix3D(sub2ind(size(adjacencyMatrix3D)idx1, path(1:end-1,1)idx2, path(2:end,1),;
3*ones(size totalRE = totalRE + adjacencyMatrix3D(pathidx1,1)-1 idx2,1))) 3);
end
nodeId = [];
else
[path,path = [];
totalCost, = [];
totalDistance, = [];
totalTime, totalRE]= [];
totalRE = deal([]);[];
nodeId = [currentF; currentB];
end
end
`function path = reconstructPath(cameFrom, current, nodes)
path = current;
while ~isempty(cameFrom{current})
current = cameFrom{current};
path = [current; path];
end
path = nodes(path, :);
end
function [cost] = calculateCost(RD,RT,RE, Kt,Kd,Ke,cost_calc)
% Time distance and energy cost equation constants can be modified on needs
if cost_calc == 1
cost = RD/Kd; % weighted cost function
elseif cost_calc == 2
cost = RT/Kt;
elseif cost_calc == 3
cost = RE/Ke;
elseif cost_calc == 4
cost = RD/Kd + RT/Kt;
elseif cost_calc == 5
cost = RD/Kd + RE/Ke;
elseif cost_calc == 6
cost = RT/Kt + RE/Ke;
elseif cost_calc == 7
cost = RD/Kd + RT/Kt + RE/Ke;
elseif cost_calc == 8
cost = 3*(RD/Kd) + 4*(RT/Kt) ;
elseif cost_calc == 9
cost = 3*(RD/Kd) + 5*(RE/Ke);
elseif cost_calc == 10
cost = 4*(RT/Kt) + 5*(RE/Ke);
elseif cost_calc == 11
cost = 3*(RD/Kd) + 4*(RT/Kt) + 5*(RE/Ke);
elseif cost_calc == 12
cost = 4*(RD/Kd) + 5*(RT/Kt) ;
elseif cost_calc == 13
cost = 4*(RD/Kd) + 3*(RE/Ke);
elseif cost_calc == 14
cost = 5*(RT/Kt) + 3*(RE/Ke);
elseif cost_calc == 15
cost = 4*(RD/Kd) + 5*(RT/Kt) + 3*(RE/Ke);
elseif cost_calc == 16
cost = 5*(RD/Kd) + 3*(RT/Kt) ;
elseif cost_calc == 17
cost = 5*(RD/Kd) + 4*(RE/Ke);
elseif cost_calc == 18
cost = 3*(RT/Kt) + 4*(RE/Ke);
elseif cost_calc == 19
cost = 5*(RD/Kd) + 3*(RT/Kt) + 4*(RE/Ke);
end
end
I am currently modifying the code to precompute the heuristic costs and gscore (adjacency matrix cost) to hopefully speed up the function.
Update 1: I have added the updated A* path with the relavent helper functions as well as the main loop that calls the A* function below, I will add in a screen shot of the profiler once I get a valid path.
disp('Calculating Adjacency and Heuristic Matricies...');
adjacencyMatrix = calculateLocalAdjacencyMatrix(nodes, n,buildingPositions, buildingSizes, mapSize,u,v,w,false,gridResolution);
disp('Adjacency Matrix Calculated...');
processPairMatrix = processPairGeneration(adjacencyMatrix, nodes, buildingSizes, mapSize);
%% Heuristic Matrix h cost per node
heuristicMatrix = calculateHeuristicMatrix(nodes, endPoint, u, v, w,true,gridResolution);
disp('Heuristic Matrix Calculated...');
%% Perform A* pathfinding Modify for parellel processing
for cost_idx = 1:cost_calcs
disp('Calculating A* Path');
[path, totalCost, totalDistance, totalTime, totalRE, nodeId] = AStarPathTD(nodes, adjacencyMatrix, heuristicMatrix, startPoint, endPoint, Kd,Kt,Ke, cost_idx, buildingPositions, buildingSizes, r_n(size(nodes,1)), false);
disp(['Original A* Path Calculated, Combination ' num2str(cost_idx) ' Calculated'] );
batchadded = 0;
while isempty(path) || totalCost == inf
disp('No valid path found between the start and end points. Adding more nodes...');
nodeadded = round(0.5*numNodes);
newnumNodes = size(nodes,1)+nodeadded;
PointAddNodes = [];
for z = 1:size(nodeId,1)
PointNodes= generateNodesSP(startPoint, buildingPositions,buildingSizes, 0.25*r_n(size(nodes,1)), r_n(size(nodes,1)), nodes,mapSize,adjacencyMatrix);
disp(['Nodes Added Around Node ' num2str(nodes(nodeId(z),:))]);
PointAddNodes = [PointAddNodes;PointNodes];
end
[~,ia,~] = intersect(PointAddNodes, nodes, 'rows');
PointAddNodes(ia,:) = [];
MoreNodes = [nodes; PointAddNodes];
disp('Updating Adjacency Matrix');
adjacencyMatrix = calculateLocalAdjacencyMatrixNewNodes(n, buildingPositions, buildingSizes, mapSize, u, v, w, false, gridResolution, adjacencyMatrix, nodes, PointAddNodes);
disp('Adjacency Matrix Updated');
%% possible path check
[~, startNodeIdx] = min(sum((nodes - startPoint).^2, 2));
startNodeIdx = max(1, min(startNodeIdx, size(nodes,1)));
% Initialize the list of nodes connected to start and end points
connectedToStart = getConnectedNodes(startNodeIdx, MoreNodes, adjacencyMatrix, r_n(size(MoreNodes,1)), buildingPositions, buildingSizes);
[~, EndNodeIdx] = min(sum((nodes - endPoint).^2, 2));
EndNodeIdx = max(1, min(EndNodeIdx, size(nodes,1)));
% Initialize the list of nodes connected to start and end points
connectedToEnd = getConnectedNodes(EndNodeIdx, MoreNodes, adjacencyMatrix, r_n(size(MoreNodes,1)), buildingPositions, buildingSizes);
if isNodeConnectable(startPoint, startPoint, endPoint, connectedToStart, connectedToEnd)
disp('Possible Path Found calculating new Heruistic Matrix');
heuristicMatrix = calculateHeuristicMatrixAddedNodes(nodes, endPoint, u, v, w, true, gridResolution, heuristicMatrix, StartEndNodes);
disp('New Heuristic Matrix has been made');
nodes= [nodes; StartEndNodes];
% Attempt A* pathfinding again
disp('Calculating A* Path With New Nodes');
[path, totalCost, totalDistance, totalTime, totalRE, nodeId] = AStarPathTD(nodes, adjacencyMatrix, heuristicMatrix, startPoint, endPoint, Kd, Kt, Ke, cost_idx, buildingPositions, buildingSizes, r_n(size(nodes,1)), false);
end
processPairMatrix = processPairGeneration(adjacencyMatrix, MoreNodes, buildingSizes, mapSize);
[newNodes, processPairMatrix] = generateNodesAdjacencyMatrix(MoreNodes, buildingPositions, buildingSizes, 0.25*r_n(size(MoreNodes,1)), r_n(size(MoreNodes,1)), nodeadded, mapSize, adjacencyMatrix, gridResolution, processPairMatrix, prismCenter, prismSize, startPoint, endPoint);
batchadded = batchadded +1;
% If batchadded exceeds 10, restart the current MT iteration
if batchadded > 15
disp('Exceeded maximum number of node batches. Restarting Monte Carlo Trial...');
restartTrial = true;
break;
end
additionalNodes = [startNodes;endNodes;newNodes];
disp('More Nodes Have Been Distributed In The Environment.');
% Recalculate adjacency and heuristic matrices
disp('Calculating New Adjacency and Heuristic Matrices');
adjacencyMatrix = calculateLocalAdjacencyMatrixNewNodes(n, buildingPositions, buildingSizes, mapSize, u, v, w, false, gridResolution, adjacencyMatrix, MoreNodes, newNodes);
disp('New Adjacency Matrix has been made');
heuristicMatrix = calculateHeuristicMatrixAddedNodes(nodes, endPoint, u, v, w, true, gridResolution, heuristicMatrix, additionalNodes);
disp('New Heuristic Matrix has been made');
nodes= [MoreNodes; newNodes];
% Attempt A* pathfinding again
disp('Calculating A* Path With New Nodes');
[path, totalCost, totalDistance, totalTime, totalRE, nodeId] = AStarPathTD(nodes, adjacencyMatrix, heuristicMatrix, startPoint, endPoint, Kd, Kt, Ke, cost_idx,buildingPositions,buildingSizes, r_n(size(nodes,1)), false);
if isempty(path) == true
processPairMatrix = processPairGeneration(adjacencyMatrix, nodes, buildingSizes, mapSize);
end
end
if restartTrial == true
break;
end
parameter_comparison_matrix(1,cost_idx,MT) = totalDistance;
parameter_comparison_matrix(2,cost_idx,MT) = totalTime;
parameter_comparison_matrix(3,cost_idx,MT) = totalRE;
parameter_comparison_matrix(4,cost_idx,MT) = totalCost;
Original_paths{cost_idx,MT} = path;
nodes_stored{MT,1} = nodes;
u_nodes = zeros(size(nodes, 1), 1);
v_nodes = zeros(size(nodes, 1), 1);
w_nodes = zeros(size(nodes, 1), 1);
% Loop through each node
for i = 1:size(nodes, 1)
% Round node coordinates
rounded_node = round(nodes(i,:)/gridResolution);
% Check for boundary conditions
if rounded_node(1) <= 0
rounded_node(1) = 1;
end
if rounded_node(2) <= 0
rounded_node(2) = 1;
end
if rounded_node(3) <= 0
rounded_node(3) = 1;
end
% Assign the wind values at the nearest grid point to the current node
u_nodes(i) = u(rounded_node(1), rounded_node(2), rounded_node(3));
v_nodes(i) = v(rounded_node(1), rounded_node(2), rounded_node(3));
w_nodes(i) = w(rounded_node(1), rounded_node(2), rounded_node(3));
end
u_saved{MT} = u_nodes;
v_saved{MT} = v_nodes;
w_saved{MT} = w_nodes;
%% Path Refinement and smoothing
refine = 1; % How many times we go through the path refinement process
smoothnodes = nodes;
smoothAdjMatrix = adjacencyMatrix;
smoothHeurMatrix = heuristicMatrix;
for a = 1:refine
% Define parameters
segmentLength = 10; % Number of points in each segment
addednodes = [];
% Assuming segmentLength is defined somewhere in your code
num_points = size(path, 1);
t = 1:num_points;
c = 3; % Determines cubic spline smoothness
tqq = linspace(1, num_points, c * size(path, 1)); % Adjust the number of points for finer spline resolution if needed
splinedPath = zeros(size(tqq, 2), 3);
for dim = 1:3 % Iterate over x, y, z dimensions
x = spline(t, path(:, dim), tqq);
splinedPath(:, dim) = x;
end
splinedPathNodes = splinedPath;
disp('Path Smoothed By Utilizing A Cubic Spline');
for k = 1:size(splinedPath, 1)-1
step_size = k; % Define step size based on k
numSegments = floor((size(splinedPath, 1) - 1) / step_size); % Calculate number of segments based on step size
% Iterate over segments
for i = 1:numSegments
% Get segment endpoints
segmentStart = splinedPathNodes((i-1) * step_size + 1, :);
segmentEnd = splinedPathNodes(i * step_size + 1, :);
% Divide segment into smaller sections using linear interpolation
numPoints = segmentLength - 1; % Number of points excluding start and end
sectionPoints = zeros(numPoints, size(segmentStart, 2)); % Initialize array to store section points
% Linear interpolation for each dimension separately
for j = 1:size(segmentStart, 2) % Iterate over dimensions
sectionPoints(:, j) = linspace(segmentStart(j), segmentEnd(j), numPoints)';
end
sectionPoints = sectionPoints(2:end-1, :); % Exclude start and end points
% Check each point in sectionPoints for proximity to buildings
validPoints = [];
for point = sectionPoints'
% Check if point is too close to any building
tooClose = false;
for e = 1:numBuildings
buildingPos = buildingPositions(e, :);
if norm(point(1:2) - buildingPos(1:2)) < obstacleRadius && abs(point(3) - buildingPos(3)) < buildingSizes(e, 3)/2
tooClose = true;
break;
end
end
if ~tooClose
validPoints = [validPoints; point'];
end
end
% Add valid points to addednodes
addednodes = [addednodes; validPoints];
end
end
preexistingNodes = smoothnodes;
nearbyNodes = [];
r = r_n(size(addednodes, 1) + size(nodes, 1));
nearbyIndices = []; % Initialize array to store indices of nearby nodes
for i = 1:size(preexistingNodes, 1)
for j = 1:size(addednodes, 1)
if norm(preexistingNodes(i, :) - addednodes(j, :)) <= r
nearbyNodes = [nearbyNodes; preexistingNodes(i, :)];
nearbyIndices = [nearbyIndices; i]; % Store the index of the nearby node
break;
end
end
end
% Remove duplicate indices
nearbyIndices = unique(nearbyIndices);
% Extract the reduced adjacency and heuristic matrices
% Adjacency matrix has 3 dimensions: (node, node, coordinate)
reducedAdjacencyMatrix = smoothAdjMatrix(nearbyIndices, nearbyIndices, :);
% Heuristic matrix has 2 dimensions: (node, coordinate)
reducedHeuristicMatrix = smoothHeurMatrix(nearbyIndices, :);
% Combine valid points and nearby preexisting nodes
localNodes = unique([addednodes; nearbyNodes], 'rows');
disp('Calculating Local Adjacency and Heuristic Matricies for Smoothed Path...');
localAdjacencyMatrix = calculateLocalAdjacencyMatrixNewNodes(n, buildingPositions, buildingSizes, mapSize, u, v, w, false, gridResolution, reducedAdjacencyMatrix, nearbyNodes, addednodes);
smoothAdjMatrix = localAdjacencyMatrix;
disp('Local Adjacency Matrix Calculated...');
localheuristicMatrix = calculateHeuristicMatrixAddedNodes(nearbyNodes, endPoint, u, v, w, true, gridResolution, reducedHeuristicMatrix, addednodes);
smoothHeurMatrix = localheuristicMatrix;
disp('Local Heuristic Matrix Calculated...');
[RefinedPath,SmoothtotalCost, SmoothtotalDistance, SmoothtotalTime, SmoothtotalRE, ~] = AStarPathTD(localNodes, localAdjacencyMatrix, localheuristicMatrix, path(1,:), path(end,:), Kd,Kt,Ke,cost_idx,buildingPositions,buildingSizes, r, true);
disp(['Original Path Combination ' num2str(cost_idx) ' Refined ' num2str(cost_idx) ' Times']);
path = RefinedPath;
smoothnodes = [nodes; addednodes];
end
Elaboration 1: The A* function is called for a set of nodes, adjacency matrix, and heuristic matrix, and the idea with using the parfor loop initially was to spread out the cost calculations for the forward and backward search --> this was replaced with vectorization which has sped up the function. I will post an screen shot of the A* function as I am aware that calculating the adjacency matrix and heuristic matrix are the other two functions in my code that take a significant amount of time.
