How can i sort coordinate points based on their short-distance between them using one of the optimization techniques?

I'm working on path plan for mobile robot, with multiple goals. coordiantes of their goals are provided, how can i re-arrange these coordinates based on closeness on ground, by using optimization techniques like, GA, PSO and others.

[KSSV]

How about knnsearch?

Answer 1

[Suresh Maddina]

Hi, It is my understanding that you are trying to sort the states based on the distance to goal. The Navigation Toolbox has a method stateSpaceSE2.distance which can be used to compute the distances which can then be sorted.

Example:

% Create a state space
ss = stateSpaceSE2([0,10; 0,10; -pi,pi]);

% Generate random states from state space
states = ss.sampleUniform(100); 

% Define goal
goal = [3,4,0];

% Sort the distances from states to goal
dist = distance(ss, states, goal)
[~, ind] = sort(dist);

% Sorted states
states(ind,:)