Теорема об отделении осей сводит меня с ума!

Я работаю над реализацией теоремы о разделении осей для использования в 2D-играх. Это вроде работает, но вроде как.

Я использую это так:

bool penetration = sat(c1, c2) && sat(c2, c1);

Где c1 и c2 относятся к типу Convex , определенному как :

class Convex
{
public:
    float tx, ty;
public:
    std::vector p;
    void translate(float x, float y) {
        tx = x;
        ty = y;
    }
};

( Точка представляет собой структуру float x , float y )

Точки вводятся по часовой стрелке.

Мой текущий код (игнорировать отладку Qt):

bool sat(Convex c1, Convex c2, QPainter *debug)
{
    //Debug
    QColor col[] = {QColor(255, 0, 0), QColor(0, 255, 0), QColor(0, 0, 255), QColor(0, 0, 0)};
    bool ret = true;

    int c1_faces = c1.p.size();
    int c2_faces = c2.p.size();

    //For every face in c1
    for(int i = 0; i < c1_faces; i++)
    {
        //Grab a face (face x, face y)
        float fx = c1.p[i].x - c1.p[(i + 1) % c1_faces].x;
        float fy = c1.p[i].y - c1.p[(i + 1) % c1_faces].y;

        //Create a perpendicular axis to project on (axis x, axis y)
        float ax = -fy, ay = fx;

        //Normalize the axis
        float len_v = sqrt(ax * ax + ay * ay);
        ax /= len_v;
        ay /= len_v;

        //Debug graphics (ignore)
        debug->setPen(col[i]);
        //Draw the face
        debug->drawLine(QLineF(c1.tx + c1.p[i].x, c1.ty + c1.p[i].y, c1.p[(i + 1) % c1_faces].x + c1.tx, c1.p[(i + 1) % c1_faces].y + c1.ty));
        //Draw the axis
        debug->save();
        debug->translate(c1.p[i].x, c1.p[i].y);
        debug->drawLine(QLineF(c1.tx, c1.ty, ax * 100 + c1.tx, ay * 100 + c1.ty));
        debug->drawEllipse(QPointF(ax * 100 + c1.tx, ay * 100 + c1.ty), 10, 10);
        debug->restore();

        //Carve out the min and max values
        float c1_min = FLT_MAX, c1_max = FLT_MIN;
        float c2_min = FLT_MAX, c2_max = FLT_MIN;

        //Project every point in c1 on the axis and store min and max
        for(int j = 0; j < c1_faces; j++)
        {
            float c1_proj = (ax * (c1.p[j].x + c1.tx) + ay * (c1.p[j].y + c1.ty)) / (ax * ax + ay * ay);
            c1_min = min(c1_proj, c1_min);
            c1_max = max(c1_proj, c1_max);
        }

        //Project every point in c2 on the axis and store min and max
        for(int j = 0; j < c2_faces; j++)
        {
            float c2_proj = (ax * (c2.p[j].x + c2.tx) + ay * (c2.p[j].y + c2.ty)) / (ax * ax + ay * ay);
            c2_min = min(c2_proj, c2_min);
            c2_max = max(c2_proj, c2_max);
        }

        //Return if the projections do not overlap
        if(!(c1_max >= c2_min && c1_min <= c2_max))
            ret = false; //return false;
    }
    return ret; //return true;
}

Что я делаю не так? Он отлично регистрирует столкновение, но слишком чувствителен к одному краю (в моем тесте с использованием треугольника и ромба):

//Triangle
push_back(Point(0, -150));
push_back(Point(0, 50));
push_back(Point(-100, 100));

//Diamond
push_back(Point(0, -100));
push_back(Point(100, 0));
push_back(Point(0, 100));
push_back(Point(-100, 0));

Я получаю этот мега-adhd, пожалуйста, помогите мне :)

http: // u8999827 .fsdata.se / sat.png