Compute the distance from the cube and a point

Compute the distance from the point and the cube. Here, we use the method from:

https://math.stackexchange.com/questions/2133217/minimal-distance-to-a-cube-in-2d-and-3d-from-a-point-lying-outside

Followings are the MATLAB implementation of this method. 

Results:

Codes:

mian.m

% Initialize the cube information

t = rand(1,3);

xyz_min = 1.0+rand(1,3);

xyz_len = 1.0+rand(1,3);

R = eye(3,3);

 

% Get costs

n = 5e3;

xyzs = zeros(n,3);

costs = zeros(n,1);

for i = 1:n

    curr_pos = 5.0*rand(1,3);

    ws_cost = get_ws_dist(curr_pos,t+xyz_min,t+xyz_min+xyz_len);

    xyzs(i,:) = curr_pos;

    costs(i) = ws_cost;

end

 

% Plot the points whose distances are between 0~1

figure(1); hold on;

opt = struct('fig_idx',1,'subfig_idx',1,'color','k','alpha',0.1);

plot_cube(t,xyz_min,xyz_len,R,opt);

for i = 1:n

    curr_cost = costs(i);

    curr_color = curr_cost/max(costs)*[0,1,1];

    if curr_cost == 0

        plot3(xyzs(i,1),xyzs(i,2),xyzs(i,3),'.','Color','k');

    elseif curr_cost < 1.0

        plot3(xyzs(i,1),xyzs(i,2),xyzs(i,3),'.','Color','b');

    elseif curr_cost < 2.0

        plot3(xyzs(i,1),xyzs(i,2),xyzs(i,3),'.','Color','g');

    end

end

view(60,16); grid on;

xlabel('X'); ylabel('Y'); zlabel('Z');

axis equal; axis([0,5,0,5,0,5]); 

get_ws_dist.m

function [max_dist,dists] = get_ws_dist(traj,xyz_min,xyz_max)

%

% Get the workspace cost using the following link: 

% https://math.stackexchange.com/questions/2133217/minimal-distance-to-a-cube-in-2d-and-3d-from-a-point-lying-outside

%

 

xyz_min = reshape(xyz_min,[1,3]);

xyz_max = reshape(xyz_max,[1,3]);

xyz_rate = 2.0./(xyz_max-xyz_min);

 

len_traj = size(traj,1);

dists = zeros(len_traj,1);

for tick = 1:len_traj

    curr_pos = traj(tick,:); % [1x3]

    curr_pos = abs(curr_pos - xyz_mid);

    curr_pos = max(0,curr_pos-xyz_len/2);

    dist = norm(curr_pos);

    % compute the distances

    dists(tick) = dist;

end

max_dist = max(dists);

plot_cube.m

function handler = plot_cube(t,xyz_min,xyz_len,R,opt)

%

% Plot 3-d cube

%

persistent h

 

% Make enough handlers at the first

if isempty(h)

    for i = 1:10

        for j = 1:200

            h{i,j}.first_flag = true;

        end

    end

end

 

% Parse options

color = getfield_safe(opt,'color','b','plot_cube');

alpha = getfield_safe(opt,'alpha',0.5,'plot_cube');

fig_idx = getfield_safe(opt,'fig_idx',1,'plot_cube');

subfig_idx = getfield_safe(opt,'subfig_idx',1,'plot_cube');

 

% Reshape

xyz_min = reshape(xyz_min,[1,3]);

xyz_len = reshape(xyz_len,[1,3]);

 

vertex_matrix = [0 0 0;1 0 0;1 1 0;0 1 0;0 0 1;1 0 1;1 1 1;0 1 1];

faces_matrix = [1 2 6 5;2 3 7 6;3 4 8 7;4 1 5 8;1 2 3 4;5 6 7 8];

 

vertex_matrix = vertex_matrix.*xyz_len;

vertex_matrix = vertex_matrix + xyz_min; % do basic translation 

vertex_matrix = vertex_matrix*R'; % do rotation

vertex_matrix = vertex_matrix + t; % do final translation 

 

if h{fig_idx,subfig_idx}.first_flag

    h{fig_idx,subfig_idx}.first_flag = false;

    h{fig_idx,subfig_idx}.fig = figure(fig_idx);

    h{fig_idx,subfig_idx}.patch = patch('Vertices',vertex_matrix,...

        'Faces',faces_matrix,...

        'FaceColor',color,'FaceAlpha',alpha,...

        'EdgeColor',color);

        % 'EdgeColor','none');

        

else

    h{fig_idx,subfig_idx}.patch.Vertices = vertex_matrix;

end

handler = h{fig_idx,subfig_idx}.patch;