Триангуляция Делоне в C ++ от lua

Я пытаюсь реализовать простую триангуляцию Делоне, от файла lua в zip-архиве до C ++.

Вот оригинальный файл lua

#include "delaunay.h"
#include <cmath>
#include <algorithm>
#include <assert.h>

std::vector<Triangle> Delaunay::triangulate(std::vector<Vector2<float>> points)
{
std::size_t npoints = points.size();
int trmax = npoints * 4;

float minX = points[0].getX();
float minY = points[0].getY();
float maxX = minX;
float maxY = minY;

for(std::size_t i = 0; i < points.size(); ++i) {
points[i].id = i;
if (points[i].getX() < minX)
minX = points[i].getX();
if (points[i].getY() < minY)
minY = points[i].getY();
if (points[i].getX() > maxX)
maxX = points[i].getX();
if (points[i].getY() > maxY)
maxY = points[i].getY();
}

float dx = maxX - minX;
float dy = maxY - minY;
float deltaMax = std::max(dx, dy);
float midx = (minX + maxX) * 0.5;
float midy = (minY + maxY) * 0.5;Vector2<float> p1(midx - 2 * deltaMax, midy - deltaMax);
Vector2<float> p2(midx, midy + 2 * deltaMax);
Vector2<float> p3(midx + 2 * deltaMax, midy - deltaMax);

p1.id = npoints + 1;
p2.id = npoints + 2;
p3.id = npoints + 3;

points.push_back(p1);
points.push_back(p2);
points.push_back(p3);

std::vector<Triangle> triangles;
triangles.push_back(Triangle(points[points.size() - 1], points[points.size() - 2], points[points.size() - 3]));

for(std::size_t i = 0; i < npoints; ++i) {
std::vector<Edge> edges;

for(std::size_t j = triangles.size(); j-- > 0; ) {
Triangle curTriangle = triangles[j];

if(curTriangle.inCircumCircle(points[i])) {
edges.push_back(curTriangle.getE1());
edges.push_back(curTriangle.getE2());
edges.push_back(curTriangle.getE3());
triangles.erase(triangles.begin() + j);
}
}

for(std::size_t j = edges.size() - 1; --j > 0; ) {
for(std::size_t k = edges.size(); --k > j + 1; ) {
if(edges[j].equals(edges[k])) {
edges.erase(edges.begin() + j);
edges.erase(edges.begin() + k - 1);
}
}
}

for(std::size_t j = 0; j < edges.size(); ++j) {
int n = triangles.size();
std::cout << n << " " << trmax << std::endl;
assert(n <= trmax && "Generated more than needed triangles");
triangles.push_back(Triangle(edges[j].getP1(), edges[j].getP2(), points[i]));
}
}

for(std::size_t i = triangles.size(); --i > 0; ) {
Triangle triangle = triangles[i];
if(triangle.getP1().id > static_cast<int>(npoints) ||
triangle.getP2().id > static_cast<int>(npoints) ||
triangle.getP3().id > static_cast<int>(npoints)) {
triangles.erase(triangles.begin() + i);
}
}

return triangles;
}

И вот что я получил

/usr/bin/../lib64/gcc/x86_64-unknown-linux-gnu/5.2.0/../../../../include/c++/5.2.0/debug/vector:406:
error: attempt to subscript container with out-of-bounds index 29, but
container only holds 29 elements.

Objects involved in the operation:
sequence "this" @ 0x0x7ffd71c01260 {
}
[1]    31657 abort (core dumped)  ./pudding

Проблема в том, что я продолжаю получать нагрузку от связанных индексов, и
когда это работает, утверждение в строке 77 в hastebin приводит к сбою.

Эта линия приводит к краху

if(edges[j].equals(edges[k]))

У кого-нибудь есть идея, почему она не работает? Все файлы здесь если хочешь посмотреть.

Большое спасибо.