%% Check Geometry Collision Status
% This example shows how to check the collision status of two collision geometries.
% Create a box collision geometry.

bx = collisionBox(1,2,3);

%
% Create a cylinder collision geometry.

cy = collisionCylinder(3,1);

%
% Translate the cylinder along the x-axis by 2.

T = trvec2tform([2 0 0]);
cy.Pose = T;

%
% Plot the two geometries.

show(cy)
hold on
show(bx)
xlim([-5 5])
ylim([-5 5])
zlim([-5 5])
hold off

%
% Check the collision status. Confirm the status is consistent with the plot.

[areIntersecting,dist,witnessPoints] = checkCollision(bx,cy)

%
% Translate the box along the x-axis by 3 and down the z-axis by 4. Confirm the box and cylinder are not colliding.

T = trvec2tform([0 3 -4]);
bx.Pose = T;
[areIntersecting,dist,witnessPoints] = checkCollision(bx,cy)

%
% Plot the box, cylinder, and the line segment representing the minimum distance between the two geometries.

show(cy)
hold on
show(bx)
wp = witnessPoints;
plot3([wp(1,1) wp(1,2)], [wp(2,1) wp(2,2)], [wp(3,1) wp(3,2)], 'bo-')
xlim([-5 5])
ylim([-5 5])
zlim([-5 5])
hold off

% Copyright 2019 The MathWorks, Inc.