【路径规划】基于前向动态规划算法在地形上找到最佳路径（Matlab代码实现）

function visualizePath(T, optimalPath)
% Visualize the path on given terrain T.
%
% -------------------------------------------------------------------------
[m, n] = size(T);
l = length(optimalPath);
% Convert back the node cardinal number to the corresponding xyz coordinate.
x = zeros(1, l);
y = zeros(1, l);
z = zeros(1, l);
for i = 1 : l
x(i) = mod(optimalPath(i) - 1, n) + 1;
y(i) = abs((optimalPath(i) - 1 - mod(optimalPath(i) - 1, n)) / n) + 1;
z(i) = 1.0 + T(y(i), x(i));
end
% Draw the optimal path as line.
plot3(x, y, z, 'r', 'LineWidth', 2)
% Draw asterisk symbol (*) at destination nodes.
plot3(x(l), y(l), z(l), '*m', 'LineWidth', 4)
function visualizeTerrain(T)
% Visualize the terrain.
%
% Don't forget to hold the figure when you want to visualize the optimal
% path using visualizePath function.
% -------------------------------------------------------------------------
[m, n] = size(T);
[X,Y] = meshgrid(1 : n, 1 : m);
surf(X, Y, T(1 : m, 1 : n));
end