%% Create navPath  Based on Multiple Waypoints
% Create a navPath object based on multiple waypoints in a Dubins space.

dubinsSpace = stateSpaceDubins([0 25; 0 25; -pi pi])
pathobj = navPath(dubinsSpace)
waypoints = [...
    8 10 pi/2;
    10 12 pi/4;
    12 17 pi/2;
    11 10 -pi];
append(pathobj, waypoints);

% Interpolate that path so that it contains exactly 250 points.

interpolate(pathobj, 250)

%  Visualize the interpolated path and the original waypoints.

figure;
grid on;
axis equal;
hold on;
plot(pathobj.States(:,1), pathobj.States(:,2), ".b");
plot(waypoints(:,1), waypoints(:,2), "*r", "MarkerSize", 10)

% Calculate length of path.

len = pathLength(pathobj);
disp("Path length = " + num2str(len))

% Copyright 2019 The MathWorks, Inc.