Transform quadrilateral into rectangle/square (in C#)

Question

I have a quadrilateral with corners P1, P2, P3 and P4, all of which I know. I also have the output coordinates I want to warp these to (Q1, Q2, Q3 and Q4), which are a square of size 1.

How can I make a function that "warps" an input point P to an output point Q?

It's kind of like scanning documents where the user can mark 4 points that are then "warped" into a rectangle, although I only need it mathematically.

I have found other posts where people had the same problem, with the solution using matrices. I tried to implement this, but due to my lack of understanding of matrices, it seems I did something wrong, although I can't figure out what went wrong.

Here's another StackOverflow post with one of those solutions: Transform quadrilateral into a rectangle?

And here's the function I wrote (MatrixMath is a class for changing matrices that I downloaded; I don't think that's where the error is, since I checked some specific examples and they were never wrong):

public static Vector2 ProjectiveTransform(Vector2 input, Vector2 p1, Vector2 p2, Vector2 p3, Vector2 p4, Vector2 q1, Vector2 q2, Vector2 q3, Vector2 q4)
    {
        // 8x8 Matrix
        float[][] A = new float[8][]
        { 
            new float[8] { p1.x, p1.y, 1, 0, 0, 0, -p1.x * q1.x, -p1.y * q1.x },
            new float[8] { 0, 0, 0, p1.x, p1.y, 1, -p1.x * q1.y, -p1.y * q1.y },
            new float[8] { p2.x, p2.y, 1, 0, 0, 0, -p2.x * q2.x, -p2.y * q2.x },
            new float[8] { 0, 0, 0, p2.x, p2.y, 1, -p2.x * q2.y, -p2.y * q2.y },
            new float[8] { p3.x, p3.y, 1, 0, 0, 0, -p3.x * q3.x, -p3.y * q3.x },
            new float[8] { 0, 0, 0, p3.x, p3.y, 1, -p3.x * q3.y, -p3.y * q3.y },
            new float[8] { p4.x, p4.y, 1, 0, 0, 0, -p4.x * q4.x, -p4.y * q4.x },
            new float[8] { 0, 0, 0, p4.x, p4.y, 1, -p4.x * q4.y, -p4.y * q4.y }
        };

        // Vector 8
        float[][] B = new float[8][]
        {
            new float[1] { q1.x },
            new float[1] { q1.y },
            new float[1] { q2.x },
            new float[1] { q2.y },
            new float[1] { q3.x },
            new float[1] { q3.y },
            new float[1] { q4.x },
            new float[1] { q4.y }
        };

        // Vector 8
        float[][] x = MatrixMath.MatrixProduct(MatrixMath.MatrixInverse(A), B);

        // 3x3 Matrix
        float[][] T = new float[3][]
        {
            new float[3] {x[0][0], x[1][0], x[2][0]},
            new float[3] {x[3][0], x[4][0], x[5][0]},
            new float[3] {x[6][0], x[7][0], 1}
        };

        // Vector 3
        float[][] inputMatrix = new float[3][]
        {
            new float[1] { input.x },
            new float[1] { input.y },
            new float[1] { 1 }
        };

        // q = T*p
        // Vector 3
        float[][] outputMatrix = MatrixMath.MatrixProduct(T, inputMatrix);

        return new Vector2(outputMatrix[0][0], outputMatrix[1][0]);
    }

Answer 1

Thanks to @ardget it works now, so I'll post the working code here in case someone needs it too.

The problem was that I didn't divide the result by the third vector component.

public static Vector2 ProjectiveTransform(Vector2 input, Vector2 p1, Vector2 p2, Vector2 p3, Vector2 p4, Vector2 q1, Vector2 q2, Vector2 q3, Vector2 q4)
{
    // 8x8 Matrix
    float[][] A = new float[8][]
    { 
        new float[8] { p1.x, p1.y, 1, 0, 0, 0, -p1.x * q1.x, -p1.y * q1.x },
        new float[8] { 0, 0, 0, p1.x, p1.y, 1, -p1.x * q1.y, -p1.y * q1.y },
        new float[8] { p2.x, p2.y, 1, 0, 0, 0, -p2.x * q2.x, -p2.y * q2.x },
        new float[8] { 0, 0, 0, p2.x, p2.y, 1, -p2.x * q2.y, -p2.y * q2.y },
        new float[8] { p3.x, p3.y, 1, 0, 0, 0, -p3.x * q3.x, -p3.y * q3.x },
        new float[8] { 0, 0, 0, p3.x, p3.y, 1, -p3.x * q3.y, -p3.y * q3.y },
        new float[8] { p4.x, p4.y, 1, 0, 0, 0, -p4.x * q4.x, -p4.y * q4.x },
        new float[8] { 0, 0, 0, p4.x, p4.y, 1, -p4.x * q4.y, -p4.y * q4.y }
    };

    // Vector 8
    float[][] B = new float[8][]
    {
        new float[1] { q1.x },
        new float[1] { q1.y },
        new float[1] { q2.x },
        new float[1] { q2.y },
        new float[1] { q3.x },
        new float[1] { q3.y },
        new float[1] { q4.x },
        new float[1] { q4.y }
    };

    // Vector 8
    float[][] x = MatrixMath.MatrixProduct(MatrixMath.MatrixInverse(A), B);

    // 3x3 Matrix
    float[][] T = new float[3][]
    {
        new float[3] {x[0][0], x[1][0], x[2][0]},
        new float[3] {x[3][0], x[4][0], x[5][0]},
        new float[3] {x[6][0], x[7][0], 1}
    };

    // Vector 3
    float[][] inputMatrix = new float[3][]
    {
        new float[1] { input.x },
        new float[1] { input.y },
        new float[1] { 1 }
    };

    // q = T*p
    // Vector 3
    float[][] outputMatrix = MatrixMath.MatrixProduct(T, inputMatrix);

    float W = outputMatrix[2][0];
    return new Vector2(outputMatrix[0][0] / W, outputMatrix[1][0] / W);
}