Что я делаю не так с алгоритмом Сазерленда Ходжмана?

Question

Извините за стену кода ниже ... но я не на 100% уверен, где именно проблема. Я использую алгоритм отсечения Сазерленда Ходжмана, чтобы найти точки соприкосновения в моей системе обнаружения столкновений (физический движок для моей дипломной работы). Вершины, которые он возвращает, ТОЧНО верны, однако я часто получаю много ложных срабатываний в функции InsideEdge или неправильных пересечений. Если кто-то может помочь, я был бы так благодарен! Я пробовал с разными входными вершинами, и результат варьируется, однако ось X кажется наиболее правильной.

Пример ввода-вывода с визуалом:

Ввод: обтравочные вершины [(2.0, 2.0), (2.0, -2.0), (-2.0, -2.0), (-2.0, 2.0)] , многовариантные [(2,5, 3,0), (2,5, 1,0), (0,5, 1,0), (0,5, 3,0)]

Выход: (2,0, 1,0), (0,5, 1,0), (0,5 , 3,0 ( неправильно ))

 bool SAT::InsideEdge(double px, double py, double edgeMaxX, double edgeMaxY, double edgeMinX, double edgeMinY) 
    {
        double one = (edgeMaxX - edgeMinX) * (py - edgeMinY);
        double two = (edgeMaxY - edgeMinY) * (px - edgeMinX);
        return (one - two) < 0; 
    }

void SAT::SutherlandHodgman(std::vector<Vector3>& _clipped, const Vector3& normal, const Vector3* polyVertices, const Vector3* clippingVertices)
{
    const unsigned maxPoints = 16;
    Vector3 newPoints[maxPoints];
    for (unsigned i = 0; i < numVertices; i++)
    {
        newPoints[i] = polyVertices[i];
    }
    unsigned newSize = 0;

    for (unsigned edge = 0; edge < numEdges; edge++) //for each clipping edge
    {
        //NOTE: axes is a 2D array to define which two axes to consider out of x, y, z
        Vector3 edgeMin = clippingVertices[edge];
        Vector3 edgeMax = clippingVertices[(edge + 1) % numEdges];

        for (unsigned v = 0; v < numVertices; v++) //for each input vertex
        {
            Vector3 v1 = newPoints[v];
            Vector3 v2 = newPoints[(v + 1) % numVertices];

            bool insideEdge1 = InsideEdge(v1[axes[0]], v1[axes[1]], edgeMax[axes[0]], edgeMax[axes[1]], edgeMin[axes[0]], edgeMin[axes[1]]);
            bool insideEdge2 = InsideEdge(v2[axes[0]], v2[axes[1]], edgeMax[axes[0]], edgeMax[axes[1]], edgeMin[axes[0]], edgeMin[axes[1]]);

            if (insideEdge1 && insideEdge2)
            {
                newPoints[newSize] = v2;
                newSize++;
            }
            else if (!insideEdge1 && insideEdge2)
            {
                newPoints[newSize] = CalculateIntersection(v1, v2, axes, edgeMin, edgeMax);
                newSize++;
                newPoints[newSize] = v2;
                newSize++;
            }
            else if (insideEdge1 && !insideEdge2)
            {
                newPoints[newSize] = CalculateIntersection(v1, v2, axes, edgeMin, edgeMax);
                newSize++;
            }
        }
        numVertices = newSize;
        if (numVertices >= maxPoints)
            break;
    }
    for (unsigned i = 0; i < numVertices; i++)
    {
        //Removes duplicates before adding to the final list
        VerifyVertex(_clipped, newPoints[i]);
    }
}

И вычислить код пересечения ...

//x intersect
    double num = (x1 * y2 - y1 * x2) * (x3 - x4) - (x1 - x2) * (x3 * y4 - y3 * x4);
    double den = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
    double X = num / den;

    //y intersect
    num = (x1 * y2 - y1 * x2) * (y3 - y4) - (y1 - y2) * (x3 * y4 - y3 * x4);
    den = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
    double Y = num / den;

    Vector3 intersection;
    intersection[axes[0]] = X;
    intersection[axes[1]] = Y;

    //Get the value of the remaining axis
    double percAcrossLine = 1;
    if (x2 != 0)
        percAcrossLine = (x1 + X) / x2;
    else if (y2 != 0)
        percAcrossLine = (y1 + Y) / y2;

    unsigned i = 0;
    if (axes[0] == 0)
    {
        if (axes[1] == 1)
            i = 2;
        else if (axes[1] == 2)
            i = 1;
    }

    intersection[i] = v1[i] + (v2[i] - v1[i]) * percAcrossLine;

    return intersection;