Convert computer vision camera matrices to OpenGL

VisionGL.java

/*
 * Copyright (c) 2011 Michael Nischt
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 *
 * Neither the name of the project's author nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
 * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

/**
 * matrices are column major
 */
final class VisionGL {

    public static float[] modelView(float[] R, float[] t) {
        float[] mV = new float[4 * 4];

        for (int row = 0; row < 3; row++) {
            for (int col = 0; col < 3; col++) {
                set4x4(mV, row, col, get3x3(R, row, col));
            }
        }
        for (int i = 0; i < 3; i++) {
            set4x4(mV, 3, i, 0);
            set4x4(mV, i, 3, t[i]);
        }
        set4x4(mV, 3, 3, 1);

        return mV;
    }

    public static float[] projection(float[] K, float nearZ, float farZ, int width, int height) {
        float[] K_inverse = inverse3x3(K);
        if (K_inverse == null) {
            throw new IllegalArgumentException("K is not ivertible");
        }

        float left, bottom;
        {
            float[] p_image = {0, 0, 1};
            float[] p_camera = transform3x3(K_inverse, p_image);
            float s = nearZ / p_camera[2];
            left = p_camera[0] * s;
            bottom = p_camera[1] * s;
        }

        float right, top;
        {
            float[] p_image = {width, height, 1};
            float[] p_camera = transform3x3(K_inverse, p_image);
            float s = nearZ / p_camera[2];
            right = p_camera[0] * s;
            top = p_camera[1] * s;
        }

        float[] P = frustum(left, right, bottom, top, nearZ, farZ);

        changeBase(P);
        return P;
    }

    private static void changeBase(float[] P) {
        // efficient version of: P * ScaleMatrix(1,-1,-1)
        for (int col = 1; col <= 2; col++) {
            for (int row = 0; row < 4; row++) {
                set4x4(P, row, col, -set4x4(P, row, col));
            }
        }
    }

    private static float[] frustum(float left, float right, float bottom, float top, float near, float far) {
        // same as glFrustum:
        // RH = right: +x, up: +y, front: -z
        // (0,0,-zNear) -> -1
        // (0,0, -zFar) -> +1

        if (near == far || left == right || bottom == top) {
            throw new IllegalArgumentException("Not a viewing volume");
        }

        float width = right - left;
        float height = top - bottom;
        float depth = far - near;
        float near2 = 2 * near;

        float a = (right + left) / width;
        float b = (top + bottom) / height;
        float c = -(far + near) / depth;
        float d = -near2 * far / depth;

        float[] P = new float[4 * 4];

        set4x4(P, 0, 0, near2 / width);
        set4x4(P, 1, 0, 0);
        set4x4(P, 2, 0, 0);
        set4x4(P, 3, 0, 0);

        set4x4(P, 0, 1, 0);
        set4x4(P, 1, 1, near2 / height);
        set4x4(P, 2, 1, 0);
        set4x4(P, 3, 1, 0);

        set4x4(P, 0, 2, a);
        set4x4(P, 1, 2, b);
        set4x4(P, 2, 2, c);
        set4x4(P, 3, 2, -1);

        set4x4(P, 0, 3, 0);
        set4x4(P, 1, 3, 0);
        set4x4(P, 2, 3, d);
        set4x4(P, 3, 3, 0);

        return P;
    }

    private static float det3x3(float[] M) {
        return get3x3(M, 0, 0) * get3x3(M, 1, 1) * get3x3(M, 2, 2)
                + get3x3(M, 0, 1) * get3x3(M, 1, 2) * get3x3(M, 2, 0)
                + get3x3(M, 0, 2) * get3x3(M, 1, 0) * get3x3(M, 2, 1)
                - get3x3(M, 0, 2) * get3x3(M, 1, 1) * get3x3(M, 2, 0)
                - get3x3(M, 0, 1) * get3x3(M, 1, 0) * get3x3(M, 2, 2)
                - get3x3(M, 0, 0) * get3x3(M, 1, 2) * get3x3(M, 2, 1);
    }

    private static float[] inverse3x3(float[] M) {
        //    | A B C |
        //M = | D E F |
        //    | G H I |

        // -1     1     |   EI-FH  -(BI-HC)   BF-EC  |
        //M   = ----- . | -(DI-FG)   AI-GC  -(AF-DC) |
        //      det M   |   DH-GE  -(AH-GB)   AE-BD  |

        float EPSILON = 1e-9f;
        float det = det3x3(M);
        if (Math.abs(det) < EPSILON) {
            return null;
        }

        float[] M_inverse = new float[3 * 3];
        set3x3(M_inverse, 0, 0, +(get3x3(M, 1, 1) * get3x3(M, 2, 2) - get3x3(M, 1, 2) * get3x3(M, 2, 1)) / det);
        set3x3(M_inverse, 0, 1, -(get3x3(M, 0, 1) * get3x3(M, 2, 2) - get3x3(M, 0, 2) * get3x3(M, 2, 1)) / det);
        set3x3(M_inverse, 0, 2, +(get3x3(M, 0, 1) * get3x3(M, 1, 2) - get3x3(M, 0, 2) * get3x3(M, 1, 1)) / det);

        set3x3(M_inverse, 1, 0, -(get3x3(M, 1, 0) * get3x3(M, 2, 2) - get3x3(M, 1, 2) * get3x3(M, 2, 1)) / det);
        set3x3(M_inverse, 1, 1, +(get3x3(M, 0, 0) * get3x3(M, 2, 2) - get3x3(M, 0, 2) * get3x3(M, 2, 1)) / det);
        set3x3(M_inverse, 1, 2, -(get3x3(M, 0, 0) * get3x3(M, 1, 2) - get3x3(M, 0, 2) * get3x3(M, 1, 0)) / det);

        set3x3(M_inverse, 2, 1, +(get3x3(M, 1, 0) * get3x3(M, 2, 1) - get3x3(M, 2, 1) * get3x3(M, 1, 1)) / det);
        set3x3(M_inverse, 2, 1, -(get3x3(M, 0, 0) * get3x3(M, 2, 1) - get3x3(M, 2, 1) * get3x3(M, 0, 1)) / det);
        set3x3(M_inverse, 2, 2, +(get3x3(M, 0, 0) * get3x3(M, 1, 1) - get3x3(M, 1, 0) * get3x3(M, 0, 1)) / det);

        return M_inverse;
    }

    private static float[] transform3x3(float[] A, float[] x) {
        float[] y = {
            get3x3(A, 0, 0) * x[0] + get3x3(A, 0, 1) * x[1] + get3x3(A, 0, 2) * x[2],
            get3x3(A, 1, 0) * x[0] + get3x3(A, 1, 1) * x[1] + get3x3(A, 1, 2) * x[2],
            get3x3(A, 2, 0) * x[0] + get3x3(A, 2, 1) * x[1] + get3x3(A, 2, 2) * x[2]
        };
        return y;
    }

    private static float get3x3(float[] m, int row, int col) {
        return m[3 * col + row];
    }

    private static float set4x4(float[] m, int row, int col) {
        return m[4 * col + row];
    }

    private static void set3x3(float[] m, int row, int col, float value) {
        m[3 * col + row] = value;
    }

    private static void set4x4(float[] m, int row, int col, float value) {
        m[4 * col + row] = value;
    }

    private VisionGL() { /*
         * static class => no instancing
         */ }
}