%% Calculate Distance Between Two States in SE3
% Create an SE(3) state space.

space = stateSpaceSE3

% Calculate distance between two states.

dist = distance(space,[2 10 3 0.2 0 0 0.8],[0 -2.5 4 0.7 0.3 0 0])

% Calculate Euclidean distance between two states.

space.WeightQuaternion = 0;
distEuc = distance(space,[2 10 3 0.2 0 0 0.8; 4 5 2 1 2 4 2],[62 5 33 0.2 0 0 0.8; 9 9 3 3 1 3.1 7])

% Copyright 2020 The MathWorks, Inc.