Рассчитать контур с помощью Gaham Scan

В моем коде я получаю координаты точек, хранящихся в векторе с именем matrix,
Точки не отсортированы, поэтому я должен их отсортировать. Поскольку я не знаю, образует ли контур выпуклый корпус, я должен изменить скан Грэма. Я решил использовать следующую процедуру. Во-первых, я использую, чтобы найти сканирование Грэма для выпуклой оболочки. Корпус я сохраняю в новом векторе. Затем я удаляю точки выпуклой оболочки из вектора matrix, Затем мне нужно отсортировать оставшиеся точки только по новому вектору, для этого я бы использовал расстояние и произведение точек.

Для этого я запустил программу сканирования Грэма. Я ориентировался на это ссылка на сайт.

Итак, вот что я имею до сих пор:

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);
}

}

Так в функции GeneratPath Я хочу отсортировать баллы. Для этого я буду вызывать функцию GrahamScan, которую я передаю двум векторам. matrix а также chull.В GrahamScan Сначала я ищу точку с самым низким значением y, используя set_p0, Далее я должен отсортировать точки. Поэтому я использую функцию сортировки, предоставляемую C ++. В качестве аргумента я использую функцию compare_angle,
Исходная функция, которую я нашел, выглядит следующим образом:

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;
}
};

Поскольку функция в моем шаблоне была основана на структуре, мне пришлось переписать функцию для моих целей. Но если я сейчас отправлю программу, я получу сообщение об ошибке: Выражение: векторный итератор + смещение вне диапазона.

Я не вижу, где я сделал ошибку, но, к сожалению. Может ли кто-нибудь помочь мне здесь?