%% Build 3-D Ego Body Paths and Check for Collisions with 3-D Obstacles
% Build an ego body path and maintain obstacle states using the dynamicCapsuleList3D object. Visualize the states of all objects in the environment at different timestamps. Validate the path of the ego body by checking for collisions with obstacles in the environment.
% Create the dynamicCapsuleList3D object. Extract the maximum number of steps to use as the number of time stamps for your object paths.

obsList = dynamicCapsuleList3D;
numSteps = obsList.MaxNumSteps;

%% Add Ego Body
% Define an ego body by specifying the ID, geometry, and state together in a structure. The capsule geometry has a length of 3 m and radius of 1 m. Specify the state as a linear path from x = 0 m to x = 100 m.

egoID1 = 1;
geom = struct("Length",3,"Radius",1,"FixedTransform",eye(4));
states = linspace(0,1,obsList.MaxNumSteps)'.*[100 0 0];
states = [states ones(numSteps,2) zeros(numSteps,2)];

egoCapsule1 = struct('ID',egoID1,'States',states,'Geometry',geom);
addEgo(obsList,egoCapsule1);

show(obsList,"TimeStep",1:numSteps);
ylim([-20 20])
zlim([-5 20])
view(-45,25)
hold on

%% Add Obstacles
% Specify states for two obstacles that are separated from the ego body by 5 m in opposite directions on the y-axis. Assume the obstacles have the same geometry geom as the ego body.

obsState1 = states + [0 5 0 0 0 0 0];
obsState2 = states + [0 -5 0 0 0 0 0];

obsCapsule1 = struct('ID',1,'States',obsState1,'Geometry',geom);
obsCapsule2 = struct('ID',2,'States',obsState2,'Geometry',geom);

addObstacle(obsList,obsCapsule1);
addObstacle(obsList,obsCapsule2);

cla
show(obsList,"TimeStep",1:numSteps);

%
%% Update Obstacles
% Alter your obstacle locations and geometry dimensions over time. Use the previously generated structure, modify the fields, and update the obstacles using the updateObstacleGeometry and updateObstaclePose object functions. Reduce the radius of the first obstacle to 0.5 m, and change the path to move it towards the ego body.

obsCapsule1.Geometry.Radius = 0.5;

obsCapsule1.States = ...
    [linspace(0,100,numSteps)' ... % x
     linspace(5,-4,numSteps)' ... % y
     zeros(numSteps,1) ... % z
     ones(numSteps,2) zeros(numSteps,2)]; % quaternion                               % quaternion

updateObstacleGeometry(obsList,1,obsCapsule1);
updateObstaclePose(obsList,1,obsCapsule1);

%
%% Check for Collisions
% Visualize the new paths. Show where collisions between the ego body and an obstacle occur, which the display highlights in red. Notice that collisions between the obstacles are not checked.

cla
show(obsList,"TimeStep",1:numSteps,"ShowCollisions",1);

% Programmatically check for collisions bu using the checkCollision object function. The function returns a vector of logical values that indicates the collision status at each time step. The vector is transposed for display purposes.

collisions = checkCollision(obsList)'

% To validate paths with a large number of steps, use the any function on the vector of collision values.

if any(collisions)
    disp("Collision detected.")
end

%
%% Update Ego Path
% Specify a new path for the ego body. Visualize the paths again, displaying collisions.

egoCapsule1.States = ...
    [linspace(0,100,numSteps)' ... % x
    3*sin(linspace(0,2*pi,numSteps))' ... % y
    zeros(numSteps,1)... % z
    ones(numSteps,2) zeros(numSteps,2)]; %quaternion                                  % quaternion

updateEgoPose(obsList,1,egoCapsule1);

cla
show(obsList,"TimeStep",1:numSteps,"ShowCollisions",1);

% Copyright 2020 The MathWorks, Inc.
%