Sorting points according to their coordinates

Question

In my software I get points of a 2D contour stored in a vector matrix My task now is to sort this points so I get the contour. First I tried the atan2 function, witch worked good for regular cases. But in cases for a non convex contour this do not work. So after a look up in google and after some replies here, I now try to calculate the nearest points. So therfore I have a function that calculates the distance between two points.

double distancepoints(vector<double> const& s1, vector<double> const& s2)
{
    return sqrt( (s1[0] - s2[0])*(s1[0] - s2[0]) + (s1[1] - s2[1])*(s1[1] - s2[1]) );
}

To find the nearest point I would define in a function the index of the point, which is closest to a predetermined starting point.

int closestpoint(vector<double> const& p, begin, end )
{
   double d=0;
   result = begin;
   while(begin != end)
   {
      double d2 = distancepoints(p, begin);
      if(d2 < d)
      {
        d = d2;
        result = begin;
      }
    }
    return result;
}

Here I do not know how I to pass the beginn and the end of the vector.

If I have the index of the next point, I would save this point in the vector Hull and delete it from the vector matrix. This should happen as long, until the matrix completely erased.

vector<vector<double> > matrix;
vector<vector<double> > hull;

int columns = 3;

const std::vector<LineSegment> &lss = slicesWithLineSegments[i];
rows = 2*lss.size();

matrix.resize(rows);

for(size_t i=0; i<matrix.size(); i++) {
    matrix[i].resize(columns);
}

for(size_t i=0; i<hull.size(); i++) {
    hull[i].resize(columns);
}

vector<vector<double> > startpoint;

for(size_t i=0; i<startpoint.size(); i++) {
    startpoint[i].resize(columns);
}

startpoint[0][0]=matrix[0][0];
startpoint[0][1]=matrix[0][1];
startpoint[0][2]=matrix[0][2];

matrix.erase(matrix.begin() );

while (matrix.size())
{
// Find next point (the one closest to p) and return index
int it = closestpoint( startpoint, matrix.begin(), matrix.end() );

// Store nearest point 
hull.push_back(std::vector<double>(3, 0));
r = hull.size()-1;
hull[r][0] = matrix[it][0];
hull[r][1] = matrix[it][1];
hull[r][2] = matrix[it][2];
// Our new p is the point we just found
startpoint = matrix[it];
// Remove the point we just found from the vector of points
matrix.erase(matrix[it]);
}

But somehow I manage not just to program the function. Maybe someone has an idea what I'm doing wrong?

Answer 1

Note: I am writing for a C++11 compatible compiler. So >> is perfectly valid for templates ;)

First

A vector of vectors is a very inefficient way of storing threedimensional data. You should consider using

std::vector<std::array<double, 3>>

instead of

std::vector<std::vector<double>>

I am using std::array for the examples. If you need std::vector for a single point just use this type instead of std::array

Second

You do not need to calculate the square root if you search for the nearst point. Comparing the square distance will work.

double squareDistancePoints(const std::array<double, 3>& a, const std::array<double, 3>& b)
{
    return pow(a[0]-b[0], 2) + pow(a[1]-b[1], 2) + pow(a[2]-b[2], 2);
}

For vector-points this would be

double squareDistancePoints(const std::vector<double>& a, const std::vector<double>& b)
{
    assert(a.size() == b.size());
    double sum = 0;
    for(size_t i = 0; i < a.size(); ++i)
        sum += pow(a[i]-b[i], 2);
    return sum;
}

Third

If you delete used points from your 'matrix', why will you supply begin and end iterators to your 'closestpoint' function?

std::vector<std::array<double, 3>>::const_iterator closestPoint(const std::array<double, 3>& point, const std::vector<std::array<double, 3>>& matrix)
{
    return std::min_element(matrix.begin(), matrix.end(), [=](const auto& a, const auto& b) { 
        return squareDistancePoints(point, a) < squareDistancePoints(point, b); 
    });
}

So you do not need the closestPoint() function at all. What you need is std::min_element from the standard library. It's a little bit slower this way because the distance for the best point is calculated multiple times, but if your sqrt() was fast enough, this code will also be fast enough.

A faster, but longer version is here:

std::vector<std::array<double, 3>>::const_iterator closestPoint(const std::array<double, 3>& point, const std::vector<std::array<double, 3>>& matrix)
{
    double bestDistance = DBL_MAX;
    auto bestIt = matrix.end();

    for(auto it = matrix.begin(); it != matrix.end(); ++it)
    {
        const auto distance = squareDistancePoints(point, *it);
        if (distance < bestDistance)
        {
            bestDistance = distance;
            bestIt = it;
        }  
    }

    return bestIt;
}

Example

std::vector<std::array<double, 3>> matrix;
std::vector<std::array<double, 3>> hull;

... // populate matrix

// Add first point to hull
hull.push_back(matrix.back());
matrix.pop_back();

// Add additional points to hull
while(!matrix.empty())
{
    auto it = closestPoint(hull.back(), matrix);
    hull.push_back(*it);
    matrix.erase(it);
}

Inline Example without using closestPoint() function because this would require a closestPoint() function which takes begin and end iterator.

std::vector<std::array<double, 3>> points;

... // populate points with matrix data

for(auto it = points.begin(); it != points.end(); ++it)
{
    auto bestIt = points.end();
    double bestSquareDistance = DBL_MAX;
    for(auto nextIt = it + 1; nextIt != points.end(); ++nextIt)
    {
        const auto squareDistance = squareDistancePoints(*it, *nextIt);
        if (squareDistance < bestSquareDistance)
        {
             bestSquareDistance = squareDistance;
             bestIt = nextIt;
        }
    }
    if (bestIt != points.end())
        std::swap(*(it+1), *bestIt);
}

Short Inline Example (Short, but very inefficient; Useable for small sets of points)

std::vector<std::array<double, 3>> points;

... // populate points with matrix data

for(auto it = points.begin(); it != points.end() && it+1 != points.end(); ++it)
    std::sort(it+1, points.end(), [=](const auto& a, const auto& b) {
        return squareDistancePoints(*it, a) < squareDistancePoints(*it, b);
    });

// vector 'points' now contains all hull points in correct order

Full Example I created a small sample program which sorts the points of a 2D rectangle. You can find it at http://ideone.com/OeCpdG

Answer 2

So let us add some typedefs to simplify the code:

typedef std::vector<double> Point;   // I still think std::array<double,3>
                                     // would be better here.
typedef std::vector<Point> PointList;

size_t closestpoint( const Point& p, const PointList& plist)
{
    if (plist.empty())
    {
        throw ???;  // Call is meaningless if plist is empty.
    }
    size_t result = 0;
    double best = distancepoints( p, plist[result] );
    for (size_t i = 1; i < plist.size(); i++)
    {
         const double dist = distancepoints( p, plist[i] );
         if (dist < best)
         {
             result = i;
             best = dist;
         }
    }
    return result;
 }

The call is:

    size_t it = closestpoint(startpoint, matrix);

Answer 3

I propose (caution: not tested)

int closestpoint (std::vector<std::vector<double> > const & p,
                  std::vector<std::vector<double> >::const_iterator & it, 
                  std::vector<std::vector<double> >::const_iterator & end )
{
   double d=DBL_MAX;
   int  result=0;
   for (int i = 0 ; it != end ; ++it, ++i)
   {
      double d2 = distancepoints(p, *it);
      if(d2 < d)
      {
        d      = d2;
        result = i;
      }
    }
    return result;
}