Calculate contour by using Gaham Scan

Question

In my code I get the coordinates of points stored in a vector called matrix. The points aren't sorted, so I have to sort them. Since I do not know whether the contour forms a convex hull I have to modify the Graham scan. I decided to use the following procedure. First, I use to find the Graham scan to the convex hull.The hull I save in a new vector. Then I delete the points of the convex hull from the vector matrix. Then I have to sort the remaining points only in the new vector, for this I would use the distance and the dot product.

For this I have started the program Graham scan. I oriented myself at this link.

So here is what I have till now:

int ccw(const std::array<double, 3>& s1, const std::array<double, 3>& s2,const std::array<double, 3>& origin)
{
  std::array<double, 3> v1, v2;
  int cp_z;

  v1[0] = (s1[0] - origin[0]); 
  v1[1] = (s1[1] - origin[1]);
  v2[0] = (s2[0] - origin[0]);
  v2[1] = (s2[1] - origin[1]);

  cp_z = v1[0] * v2[1] - v1[1] * v2[0];
  if(cp_z > 0)
       return CCW_LEFT_TURN;
  else if(cp_z < 0)
       return CCW_RIGHT_TURN;

  return CCW_COLINEAR;
}

bool compare_angle(const std::array<double, 3>& s1, const std::array<double, 3>& s2)
{
  int res;
  std::array<double, 3> p0;
  p0[0]=0;
  p0[1]=0;

  res = ccw(s1, s2, p0);

  if(res == CCW_LEFT_TURN)
       return true;
  else if(res == CCW_COLINEAR) {
       double da, db;
       da = Distance(p0, s1);
       db = Distance(p0, s2);
       if(da < db)
          return true;
  }

  return false;
}

void set_p0(std::vector<std::array<double, 3>> matrix)
{
  int i;
  for(i=1; i<matrix.size(); i++)
  {
    if(matrix[i][1] < matrix[0][1])
    {
        swap(matrix[0], matrix[1]);
    }
    else if(matrix[i][1] == matrix[0][1] && matrix[i][0] < matrix[0][0])
    {
        swap(matrix[0], matrix[i]);
    }
  }
}

void GrahamScan(std::vector<std::array<double, 3>> matrix, std::vector<std::array<double, 3>> chull)
{
  int i;
  bool done;
  std::array<double, 3> next2last, last;

  set_p0(matrix);

  sort(matrix.begin()+1, matrix.end(), compare_angle);

  chull.push_back(matrix[0]);
  chull.push_back(matrix[1]);

  for(i=2; i<matrix.size(); i++)
  {
    done = false;

    while(!done && chull.size() > 1) {
        last = chull.back();
        next2last = chull[chull.size()-2];

        if(ccw(last, matrix[i], next2last) != CCW_LEFT_TURN)
            chull.pop_back();
        else
            done = true;
    }

    chull.push_back(matrix[i]);
  }
}

void GeneratPath(const std::vector<std::vector<LineSegment>> &slicesWithLineSegments, const v3 &aabbSize, const double infill) {

  ofstream file ("data.csv");

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

  .
  .
  .

  if(matrix.size()>2)
    {
        GrahamScan(matrix, chull);
    }

 }

So in the function GeneratPath I want to sort the points. For this, I'll call the GrahamScan function which I pass the two vectors matrix and chull.In GrahamScan I first search the point with the lowest y-value using set_p0. Next I have to sort the points. Therefore I use the sort-function C++ provides. As argument I use the function compare_angle. The original function I found looks like this:

struct point_angle_comp_p {
point_t p0;

point_angle_comp_p () : p0 (0, 0) { } 
point_angle_comp_p (const point_t &p) : p0 (p) { } 

bool operator() (const point_t &a, const point_t &b) 
{
    int res;

    res = ccw (a, b, p0);

    if (res == CCW_LEFT_TURN)
        return true;
    else if (res == CCW_COLINEAR) {
        double da, db;

        da = dist (p0, a);
        db = dist (p0, b);

        if (da < db)
            return true;
    }

    return false;
 }
};

Since the function in my template to a struct-based I had to rewrite the function for my purposes. But if I now send forth program I get the error message: Expression: vector iterator + offset out of range.

I can not see where I made the mistake but unfortunately. Can someone help me here?