_camera.md

camera.c

/* ============================================================================
 *
 *                              CAMERA
 *
 * ============================================================================
This is a right-handed camera implementation composed of:
    - Free FPS camera (no camera offset, only camera orientation and camera position)
    - 3rd person cameras (camera offset + camera position is model position)
    - Arcball like camera (camera offset with fixed camera position)

with some more advanced features usually not found like:
    - Selectable camera output Z range
    - Infinite far Z plane (with epsilon to prevent floating point errors)
    - Directly modifyable position, offset and orientation
    - Orientation either as rotation matrix or unit quaternion
    - Provides camera ray for 3D object picking
    - No camera specific math datastructures or functions only floats arrays
    - Pitch, yaw and roll euler rotations
    - No direct dependencies
    - Implementation notes if you don't like my code but want some feature(s)

References:
    [1] http://www.terathon.com/gdc07_lengyel.pdf By Eric Lengyel GDC slides on projection matrix tricks
    [2] https://www.youtube.com/watch?v=9KaIB7PFWeE By Casey Muratori Matrix introduction
    [3] https://www.youtube.com/watch?v=kvSbiHrFKNk By Casey Muratori Matrix inversion
    [4] https://www.youtube.com/watch?v=3ZmqJb7J5wE By Jorge Rodriguez The Camera View Transform Matrix
    [5] http://bookofhook.com/mousepick.pdf By Brian Hook Mouse Ray Picking Explained
    [6] https://unspecified.wordpress.com/2012/06/21/calculating-the-gluperspective-matrix-and-other-opengl-matrix-maths/ By Matt Giuca Calculating the gluPerspective matrix and other OpenGL matrix maths
    [7] http://in2gpu.com/2016/03/14/opengl-fps-camera-quaternion/ By Sergiu Craitoiu OPENGL FPS CAMERA QUATERNION
    https://gamedev.stackexchange.com/questions/30644/how-to-keep-my-quaternion-using-fps-camera-from-tilting-and-messing-up/30654
    https://gamedev.stackexchange.com/questions/13436/glm-euler-angles-to-quaternion

License:
    public domain: no warrenty implied; use at your own risk.
*/
#include <math.h> /* sqrt, sin, cos, tan */
#include <assert.h> /* assert macro */
#include <string.h> /* memcpy */

#define PI_CONSTANT (3.141592654f)
#define TO_RAD  ((PI_CONSTANT/180.0f))

#define CAM_INF (-1.0f)
enum cam_orient {CAM_ORIENT_QUAT, CAM_ORIENT_MAT};
enum cam_output_z_range {
    CAM_OUT_Z_NEGATIVE_ONE_TO_ONE,
    CAM_OUT_Z_NEGATIVE_ONE_TO_ZERO,
    CAM_OUT_Z_ZERO_TO_ONE
};
struct cam {
    /*--- [input] ---*/
    int zout, orient_struct;

    /* proj */
    float fov;
    float near, far;
    float aspect_ratio;
    float z_range_epsilon;

    /* transform */
    float pos[3];
    float off[3];
    float ear[3];
    float quat[4];
    float mat[3][3];

    /*--- [output] ---*/
    float view[4][4];
    float view_inv[4][4];
    float proj[4][4];
    float proj_inv[4][4];

    float loc[3];
    float forward[3];
    float backward[3];
    float right[3];
    float down[3];
    float left[3];
    float up[3];
};
static void
cam_init(struct cam *c)
{
    static const float qid[] = {0,0,0,1};
    static const float m3id[] = {1,0,0,0,1,0,0,0,1};
    static const float m4id[] = {1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1};

    memset(c, 0, sizeof(*c));
    c->aspect_ratio = 3.0f/2.0f;
    c->fov = PI_CONSTANT / 4.0f;
    c->near = 0.01f;
    c->far = 10000;

    memcpy(c->quat, qid, sizeof(qid));
    memcpy(c->mat, m3id, sizeof(m3id));
    memcpy(c->view, m4id, sizeof(m4id));
    memcpy(c->view_inv, m4id, sizeof(m4id));
    memcpy(c->proj, m4id, sizeof(m4id));
    memcpy(c->proj_inv, m4id, sizeof(m4id));
}
static void
cam_build(struct cam *c)
{
    assert(c);
    if (!c) return;

    /* convert orientation matrix into quaternion */
    if (c->orient_struct == CAM_ORIENT_MAT) {
        float s,t,
        trace = c->mat[0][0];
        trace += c->mat[1][1];
        trace += c->mat[2][2];
        if (trace > 0.0f) {
            t = trace + 1.0f;
            s = (float)sqrt((double)(1.0f/t)) * 0.5f;

            c->quat[3] = s * t;
            c->quat[0] = (c->mat[2][1] - c->mat[1][2]) * s;
            c->quat[1] = (c->mat[0][2] - c->mat[2][0]) * s;
            c->quat[2] = (c->mat[1][0] - c->mat[0][1]) * s;
        } else {
            int i = 0, j, k;
            static const int next[] = {1,2,0};
            if (c->mat[1][1] > c->mat[0][0] ) i = 1;
            if (c->mat[2][2] > c->mat[i][i] ) i = 2;

            j = next[i]; k = next[j];
            t = (c->mat[i][i] - (c->mat[j][j] - c->mat[k][k])) + 1.0f;
            s = (float)sqrt((double)(1.0f/t)) * 0.5f;

            c->quat[i] = s*t;
            c->quat[3] = (c->mat[k][j] - c->mat[j][k]) * s;
            c->quat[j] = (c->mat[j][i] + c->mat[i][j]) * s;
            c->quat[k] = (c->mat[k][i] + c->mat[i][k]) * s;
        }

        /* normalize quaternion */
        {float len2 = c->quat[0]*c->quat[0] + c->quat[1]*c->quat[1];
        len2 += c->quat[2]*c->quat[2] + c->quat[3]*c->quat[3];
        if (len2 != 0.0f) {
            float len = (float)sqrt((double)len2);
            float inv_len = 1.0f/len;
            c->quat[0] *= inv_len; c->quat[1] *= inv_len;
            c->quat[2] *= inv_len; c->quat[3] *= inv_len;
        }}
    }

    /* Camera euler orientation
    It is not feasible to multiply euler angles directly together to represent the camera
    orientation because of gimbal lock (Even quaternions do not save you against
    gimbal lock under all circumstances). While it is true that it is not a problem for
    FPS style cameras, it is a problem for free cameras. To fix that issue this camera only
    takes in the relative angle rotation and does not store the absolute angle values [7].*/
    {float sx =(float)sin((double)(c->ear[0] * 0.5f));
    float sy = (float)sin((double)(c->ear[1] * 0.5f));
    float sz = (float)sin((double)(c->ear[2] * 0.5f));

    float cx = (float)cos((double)(c->ear[0] * 0.5f));
    float cy = (float)cos((double)(c->ear[1] * 0.5f));
    float cz = (float)cos((double)(c->ear[2] * 0.5f));

    float a[4], b[4];
    a[0] = cz*sx; a[1] = sz*sx; a[2] = sz*cx; a[3] = cz*cx;
    b[0] = c->quat[0]*cy - c->quat[2]*sy;
    b[1] = c->quat[3]*sy + c->quat[1]*cy;
    b[2] = c->quat[2]*cy + c->quat[0]*sy;
    b[3] = c->quat[3]*cy - c->quat[1]*sy;

    c->quat[0] = a[3]*b[0] + a[0]*b[3] + a[1]*b[2] - a[2]*b[1];
    c->quat[1] = a[3]*b[1] + a[1]*b[3] + a[2]*b[0] - a[0]*b[2];
    c->quat[2] = a[3]*b[2] + a[2]*b[3] + a[0]*b[1] - a[1]*b[0];
    c->quat[3] = a[3]*b[3] - a[0]*b[0] - a[1]*b[1] - a[2]*b[2];
    memset(c->ear, 0, sizeof(c->ear));}

    /* Convert quaternion to matrix
    Next up we want to convert our camera quaternion orientation into a 3x3 matrix
    to generate our view matrix. So to convert from quaternion to rotation
    matrix we first look at how to transform a vector quaternion by and how by matrix.
    To transform a vector by a unit quaternion you turn the vector into a zero w
    quaternion and left multiply by quaternion and right multiply by quaternion inverse:
        p2  = q * q(P1) * pi
            = q(qx,qy,qz,qw) * q(x,y,z,0) * q(-qx,-qy,-qz,qw)

    To get the same result with a rotation matrix you just multiply the vector by matrix:
        p2 = M * p1

    So to get the matrix M you first multiply out the quaternion transformation and
    group each x,y and z term into a column. The end result is: */
    {float x2 = c->quat[0] + c->quat[0];
    float y2 = c->quat[1] + c->quat[1];
    float z2 = c->quat[2] + c->quat[2];

    float xx = c->quat[0] * x2;
    float xy = c->quat[0] * y2;
    float xz = c->quat[0] * z2;

    float yy = c->quat[1] * y2;
    float yz = c->quat[1] * z2;
    float zz = c->quat[2] * z2;

    float wx = c->quat[3] * x2;
    float wy = c->quat[3] * y2;
    float wz = c->quat[3] * z2;

    c->mat[0][0] = 1.0f - (yy + zz);
    c->mat[0][1] = xy - wz;
    c->mat[0][2] = xz + wy;

    c->mat[1][0] = xy + wz;
    c->mat[1][1] = 1.0f - (xx + zz);
    c->mat[1][2] = yz - wx;

    c->mat[2][0] = xz - wy;
    c->mat[2][1] = yz + wx;
    c->mat[2][2] = 1.0f - (xx + yy);}

    /* View matrix
    The general transform pipeline is object space to world space to local camera
    space to screenspace. While this particular matrix, the view matrix,
    transforms from world space to local camera space.

    So we start by trying to find the camera world transform a 4x3 matrix which
    can and will be in this particular implementation extended to a 4x4 matrix
    composed of camera rotation, camera translation and camera offset.
    While pure camera position and orientation is usefull for free FPS style cameras,
    allows the camera offset 3rd person cameras or tracking ball behavior.

        T.......camera position
        F.......camera offset
        R.......camera orientation 3x3 matrix
        C.......camera world transform 4x4 matrix

            |1 T|   |R 0|   |1 F|
        C = |0 1| * |0 1| * |0 1|

            |R   Rf+T|
        C = |0      1|
    */
    /* 1.) copy orientation matrix */
    c->view_inv[0][0] = c->mat[0][0]; c->view_inv[0][1] = c->mat[0][1];
    c->view_inv[0][2] = c->mat[0][2]; c->view_inv[1][0] = c->mat[1][0];
    c->view_inv[1][1] = c->mat[1][1]; c->view_inv[1][2] = c->mat[1][2];
    c->view_inv[2][0] = c->mat[2][0]; c->view_inv[2][1] = c->mat[2][1];
    c->view_inv[2][2] = c->mat[2][2];

    /* 2.) transform offset by camera orientation and add translation */
    c->view_inv[3][0] = c->view_inv[0][0] * c->off[0];
    c->view_inv[3][1] = c->view_inv[0][1] * c->off[0];
    c->view_inv[3][2] = c->view_inv[0][2] * c->off[0];

    c->view_inv[3][0] += c->view_inv[1][0] * c->off[1];
    c->view_inv[3][1] += c->view_inv[1][1] * c->off[1];
    c->view_inv[3][2] += c->view_inv[1][2] * c->off[1];

    c->view_inv[3][0] += c->view_inv[2][0] * c->off[2];
    c->view_inv[3][1] += c->view_inv[2][1] * c->off[2];
    c->view_inv[3][2] += c->view_inv[2][2] * c->off[2];

    c->view_inv[3][0] += c->pos[0];
    c->view_inv[3][1] += c->pos[1];
    c->view_inv[3][2] += c->pos[2];

    /* 3.) fill last empty 4x4 matrix row */
    c->view_inv[0][3] = 0;
    c->view_inv[1][3] = 0;
    c->view_inv[2][3] = 0;
    c->view_inv[3][3] = 1.0f;

    /* Now we have a matrix to transform from local camera space into world
    camera space. But remember we are looking for the opposite transform since we
    want to transform the world around the camera. So to get the other way
    around we have to invert our world camera transformation to transform to
    local camera space.

    Usually inverting matricies is quite a complex endeavour both in needed complexity
    as well as number of calculations required. Luckily we can use a nice property
    of orthonormal matrices (matrices with each column being unit length)
    on the more complicated matrix the rotation matrix.
    The inverse of orthonormal matrices is the same as the transpose of the same
    matrix, which is just a matrix column to row swap.

    So with inverse rotation matrix covered the only thing left is the inverted
    camera translation which just needs to be negated plus and this is the more
    important part tranformed by the inverted rotation matrix.
    Why? simply because the inverse of a matrix multiplication M = A * B
    is NOT Mi = Ai * Bi (i for inverse) but rather Mi = Bi * Ai.
    So if we put everything together we get the following view matrix:

        R.......camera orientation matrix3x3
        T.......camera translation
        F.......camera offset
        Ti......camera inverse translation
        Fi......camera inverse offset
        Ri......camera inverse orientation matrix3x3
        V.......view matrix

               (|R   Rf+T|)
        V = inv(|0      1|)

            |Ri -Ri*Ti-Fi|
        V = |0          1|

    Now we finally have our matrix composition and can fill out the view matrix:

    1.) Inverse camera orientation by transpose */
    c->view[0][0] = c->mat[0][0];
    c->view[0][1] = c->mat[1][0];
    c->view[0][2] = c->mat[2][0];

    c->view[1][0] = c->mat[0][1];
    c->view[1][1] = c->mat[1][1];
    c->view[1][2] = c->mat[2][1];

    c->view[2][0] = c->mat[0][2];
    c->view[2][1] = c->mat[1][2];
    c->view[2][2] = c->mat[2][2];

    /* 2.) Transform inverted position vector by transposed orientation and subtract offset */
    {float pos_inv[3];
    pos_inv[0] = -c->pos[0];
    pos_inv[1] = -c->pos[1];
    pos_inv[2] = -c->pos[2];

    c->view[3][0] = c->view[0][0] * pos_inv[0];
    c->view[3][1] = c->view[0][1] * pos_inv[0];
    c->view[3][2] = c->view[0][2] * pos_inv[0];

    c->view[3][0] += c->view[1][0] * pos_inv[1];
    c->view[3][1] += c->view[1][1] * pos_inv[1];
    c->view[3][2] += c->view[1][2] * pos_inv[1];

    c->view[3][0] += c->view[2][0] * pos_inv[2];
    c->view[3][1] += c->view[2][1] * pos_inv[2];
    c->view[3][2] += c->view[2][2] * pos_inv[2];

    c->view[3][0] -= c->off[0];
    c->view[3][1] -= c->off[1];
    c->view[3][2] -= c->off[2];}

    /* 3.) fill last empty 4x4 matrix row */
    c->view[0][3] = 0; c->view[1][3] = 0;
    c->view[2][3] = 0; c->view[3][3] = 1.0f;

    /*  Projection matrix
    While the view matrix transforms from world space to local camera space,
    tranforms the perspective projection matrix from camera space to screen space.

    The actual work for the transformation is from eye coordinates camera
    frustum far plane to a cube with coordinates (-1,1), (0,1), (-1,0) depending on
    argument `out_z_range` in this particual implementation.

    To actually build the projection matrix we need:
        - Vertical field of view angle
        - Screen aspect ratio which controls the horizontal view angle in
            contrast to the field of view.
        - Z coordinate of the frustum near clipping plane
        - Z coordinate of the frustum far clipping plane

    While I will explain how to incooperate the near,far z clipping
    plane I would recommend reading [7] for the other values since it is quite
    hard to do understand without visual aid and he is probably better in
    explaining than I would be.*/
    {float hfov = (float)tan((double)(c->fov*0.5f));
    c->proj[0][0] = 1.0f/(c->aspect_ratio * hfov);
    c->proj[0][1] = 0;
    c->proj[0][2] = 0;
    c->proj[0][3] = 0;

    c->proj[1][0] = 0;
    c->proj[1][1] = 1.0f/hfov;
    c->proj[1][2] = 0;
    c->proj[1][3] = 0;

    c->proj[2][0] = 0;
    c->proj[2][1] = 0;
    c->proj[2][3] = -1.0f;

    c->proj[3][0] = 0;
    c->proj[3][1] = 0;
    c->proj[3][3] = 0;

    if (c->far <= CAM_INF) {
        /* Up to this point we got perspective matrix:

            |1/aspect*hfov  0       0  0|
            |0              1/hfov  0  0|
            |0              0       A  B|
            |0              0       -1 0|

        but we are still missing A and B to map between the frustum near/far
        and the clipping cube near/far value. So we take the lower right part
        of the matrix and multiply by a vector containing the missing
        z and w value, which gives us the resulting clipping cube z;

            |A  B|   |z|    |Az + B |           B
            |-1 0| * |1| =  | -z    | = -A + ------
                                               -z

        So far so good but now we need to map from the frustum near,
        far clipping plane (n,f) to the clipping cube near and far plane (cn,cf).
        So we plugin the frustum near/far values into z and the resulting
        cube near/far plane we want to end up with.

                   B                    B
            -A + ------ = cn and -A + ------ = cf
                   n                    f

        We now have two equations with two unkown A and B since n,f as well
        as cn,cf are provided, so we can solved them by subtitution either by
        hand or, if you are like me prone to easily make small mistakes,
        with WolframAlpha which solved for:*/
        switch (c->zout) {
            default:
            case CAM_OUT_Z_NEGATIVE_ONE_TO_ONE: {
                /* cn = -1 and cf = 1: */
                c->proj[2][2] = -(c->far + c->near) / (c->far - c->near);
                c->proj[3][2] = -(2.0f * c->far * c->near) / (c->far - c->near);
            } break;
            case CAM_OUT_Z_NEGATIVE_ONE_TO_ZERO: {
                /* cn = -1 and cf = 0: */
                c->proj[2][2] = (c->near) / (c->near - c->far);
                c->proj[3][2] = (c->far * c->near) / (c->near - c->far);
            } break;
            case CAM_OUT_Z_ZERO_TO_ONE: {
                /* cn = 0 and cf = 1: */
                c->proj[2][2] = -(c->far) / (c->far - c->near);
                c->proj[3][2] = -(c->far * c->near) / (c->far - c->near);
            } break;
        }
    } else {
    /*  Infinite projection [1]:
        In general infinite projection matrices map direction to points on the
        infinite distant far plane, which is mainly useful for rendering:
            - skyboxes, sun, moon, stars
            - stencil shadow volume caps

        To actually calculate the infinite perspective matrix you let the
        far clip plane go to infinity. Once again I would recommend using and
        checking WolframAlpha to make sure all values are correct, while still
        doing the calculation at least once by hand.

        While it is mathematically correct to go to infinity, floating point errors
        result in a depth smaller or bigger. This in term results in
        fragment culling since the hardware thinks fragments are beyond the
        far clipping plane. The general solution is to introduce an epsilon
        to fix the calculation error and map it to the infinite far plane.

        Important:
        For 32-bit floating point epsilon should be greater than 2.4*10^-7,
        to account for floating point precision problems. */
        switch (c->zout) {
            default:
            case CAM_OUT_Z_NEGATIVE_ONE_TO_ONE: {
                /*lim f->inf -((f+n)/(f-n)) => -((inf+n)/(inf-n)) => -(inf)/(inf) => -1.0*/
                c->proj[2][2] = c->z_range_epsilon - 1.0f;
                /* lim f->inf -(2*f*n)/(f-n) => -(2*inf*n)/(inf-n) => -(2*inf*n)/inf => -2n*/
                c->proj[3][2] = (c->z_range_epsilon - 2.0f) * c->near;
            } break;
            case CAM_OUT_Z_NEGATIVE_ONE_TO_ZERO: {
                /* lim f->inf  n/(n-f) => n/(n-inf) => n/(n-inf) => -0 */
                c->proj[2][2] = c->z_range_epsilon;
                /* lim f->inf (f*n)/(n-f) => (inf*n)/(n-inf) => (inf*n)/-inf = -n */
                c->proj[3][2] = (c->z_range_epsilon - 1.0f) * c->near;
            } break;
            case CAM_OUT_Z_ZERO_TO_ONE: {
                /* lim f->inf (-f)/(f-n) => (-inf)/(inf-n) => -inf/inf = -1 */
                c->proj[2][2] = c->z_range_epsilon - 1.0f;
                /* lim f->inf (-f*n)/(f-n) => (-inf*n)/(inf-n) => (-inf*n)/(inf) => -n */
                c->proj[3][2] = (c->z_range_epsilon - 1.0f) * c->near;
            } break;
        }
    }}

    /* Invert projection [2][3]:
    Since perspective matrices have a fixed layout, it makes sense
    to calculate the specific perspective inverse instead of relying on a default
    matrix inverse function. Actually calculating the matrix for any perspective
    matrix is quite straight forward:

        I.......identity matrix
        p.......perspective matrix
        I(p)....inverse perspective matrix

    1.) Fill a variable inversion matrix and perspective layout matrix into the
        inversion formula: I(p) * p = I

        |x0  x1  x2  x3 |   |a 0 0 0|   |1 0 0 0|
        |x4  x5  x6  x7 | * |0 b 0 0| = |0 1 0 0|
        |x8  x9  x10 x11|   |0 0 c d|   |0 0 1 0|
        |x12 x13 x14 x15|   |0 0 e 0|   |0 0 0 1|

    2.) Multiply inversion matrix times perspective matrix

        |x0*a x1*b x2*c+x3*e x2*d|      |1 0 0 0|
        |x4*a x5*b x6*c+x7*e x6*d|    = |0 1 0 0|
        |x8*a x9*b x10*c+x11*e x10*d|   |0 0 1 0|
        |x12*a x13*b x14*c+x15*e x14*d| |0 0 0 1|

    3.) Finally substitute each x value: e.g: x0*a = 1 => x0 = 1/a so I(p) at column 0, row 0 is 1/a.

                |1/a 0 0 0|
        I(p) =  |0 1/b 0 0|
                |0 0 0 1/e|
                |0 0 1/d -c/de|

    These steps basically work for any invertable matrices, but I would recommend
    using WolframAlpha for these specific kinds of matrices, since it can
    automatically generate inversion matrices without any fuss or possible
    human calculation errors. */
    memset(c->proj_inv, 0, sizeof(c->proj_inv));
    c->proj_inv[0][0] = 1.0f/c->proj[0][0];
    c->proj_inv[1][1] = 1.0f/c->proj[1][1];
    c->proj_inv[2][3] = 1.0f/c->proj[3][2];
    c->proj_inv[3][2] = 1.0f/c->proj[2][3];
    c->proj_inv[3][3] = -c->proj[2][2];
    c->proj_inv[3][3] /= (c->proj[3][2] * c->proj[2][3]);

    /* fill vectors with data */
    c->loc[0] = c->view_inv[3][0];
    c->loc[1] = c->view_inv[3][1];
    c->loc[2] = c->view_inv[3][2];

    c->forward[0] = c->view_inv[2][0];
    c->forward[1] = c->view_inv[2][1];
    c->forward[2] = c->view_inv[2][2];

    c->backward[0] = -c->view_inv[2][0];
    c->backward[1] = -c->view_inv[2][1];
    c->backward[2] = -c->view_inv[2][2];

    c->right[0] = c->view_inv[0][0];
    c->right[1] = c->view_inv[0][1];
    c->right[2] = c->view_inv[0][2];

    c->left[0] = -c->view_inv[0][0];
    c->left[1] = -c->view_inv[0][1];
    c->left[2] = -c->view_inv[0][2];

    c->up[0] = c->view_inv[1][0];
    c->up[1] = c->view_inv[1][1];
    c->up[2] = c->view_inv[1][2];

    c->down[0] = -c->view_inv[1][0];
    c->down[1] = -c->view_inv[1][1];
    c->down[2] = -c->view_inv[1][2];
}
static void
cam_move(struct cam *c, float x, float y, float z)
{
    c->pos[0] += c->view_inv[0][0]*x;
    c->pos[1] += c->view_inv[0][1]*x;
    c->pos[2] += c->view_inv[0][2]*x;

    c->pos[0] += c->view_inv[1][0]*y;
    c->pos[1] += c->view_inv[1][1]*y;
    c->pos[2] += c->view_inv[1][2]*y;

    c->pos[0] += c->view_inv[2][0]*z;
    c->pos[1] += c->view_inv[2][1]*z;
    c->pos[2] += c->view_inv[2][2]*z;
}
static void
cam_screen_to_world(const struct cam *c, float width, float height,
    float *res, float screen_x, float screen_y, float camera_z)
{
    /* Screen space to world space coordinates
    To convert from screen space coordinates to world coordinates we
    basically have to revert all transformations typically done to
    convert from world space to screen space:
        Viewport => NDC => Clip => View => World

    Viewport => NDC => Clip
    -----------------------
    First up is the transform from viewport to clipping space.
    To get from clipping space to viewport we calculate:

                    |((x+1)/2)*w|
        Vn = v =    |((1-y)/2)*h|
                    |((z+1)/2)  |

    Now we need to the the inverse process by solvinging for n:
                |(2*x)/w - 1|
        n =     |(2*y)/h    |
                |(2*z)-1)   |
                | 1         |
    */
    float x = (screen_x / width * 2.0f) - 1.0f;
    float y = (screen_y / height) * 2.0f - 1.0f;
    float z = 2.0f * camera_z - 1.0f;

    /* Clip => View
    -----------------------
    A vector v or position p in view space is tranform to clip
    coordinates c by being transformed by a projection matrix P:

        c = P * v

    To convert from clipping coordinates c to view coordinates we
    just have to transfrom c by the inverse projection matrix Pi:

        v = Pi * c

    The inverse projection matrix for all common projection matrices
    can be calculated by (see Camera_Build for more information):

                |1/a 0 0 0|
        Pi  =   |0 1/b 0 0|
                |0 0 0 1/e|
                |0 0 1/d -c/de|

    View => World
    -----------------------
    Finally we just need to convert from view coordinates to world
    coordinates w by transforming our view coordinates by the inverse view
    matrix Vi which in this context is just the camera translation and
    rotation.

        w = Vi * v

    Now we reached our goal and have our world coordinates. This implementation
    combines both the inverse projection as well as inverse view transformation
    into one since the projection layout is known we can do some optimization:*/
    float ax = c->proj_inv[0][0]*x;
    float by = c->proj_inv[1][1]*y;
    float dz = c->proj_inv[2][3]*z;
    float w = c->proj_inv[3][3] + dz;

    res[0] = c->proj_inv[3][2] * c->view_inv[2][0];
    res[0] += c->proj_inv[3][3] * c->view_inv[3][0];
    res[0] += ax * c->view_inv[0][0];
    res[0] += by * c->view_inv[1][0];
    res[0] += dz * c->view_inv[3][0];

    res[1] = c->proj_inv[3][2] * c->view_inv[2][1];
    res[1] += c->proj_inv[3][3] * c->view_inv[3][1];
    res[1] += ax * c->view_inv[0][1];
    res[1] += by * c->view_inv[1][1];
    res[1] += dz * c->view_inv[3][1];

    res[2] = c->proj_inv[3][2] * c->view_inv[2][2];
    res[2] += c->proj_inv[3][3] * c->view_inv[3][2];
    res[2] += ax * c->view_inv[0][2];
    res[2] += by * c->view_inv[1][2];
    res[2] += dz * c->view_inv[3][2];
    res[0] /= w; res[1] /= w; res[2] /= w;
}
static void
cam_picking_ray(const struct cam *c, float w, float h,
    float mouse_x, float mouse_y, float *orig, float *dir)
{
    float world[3];
    cam_screen_to_world(c, w, h, world, mouse_x, mouse_y, 0);

    /* calculate direction
    We generate the ray normal vector by first transforming the mouse cursor position
    from screen coordinates into world coordinates. After that we only have to
    subtract our camera position from our calculated mouse world position and
    normalize the result to make sure we have a unit vector as direction. */
    orig[0] = c->loc[0];
    orig[1] = c->loc[1];
    orig[2] = c->loc[2];

    dir[0] = world[0] - orig[0];
    dir[1] = world[1] - orig[1];
    dir[2] = world[2] - orig[2];

    /* normalize */
    {float dot = dir[0]*dir[0] + dir[1]*dir[1] + dir[2]*dir[2];
    if (dot != 0.0f) {
        float len = (float)sqrt((double)dot);
        dir[0] /= len; dir[1] /= len; dir[2] /= len;
    }}
}

example.c

#include <GL/gl.h>
#include <GL/glu.h>

#include <SDL2/SDL.h>
#include <SDL2/SDL_opengl.h>

#define countof(a) (sizeof(a)/sizeof((a)[0]))
#define max(a,b) ((a) > (b)?(a):(b))
#define max3(a,b,c) max(max(a,b),c)

typedef struct sys_int2 {int x,y;} sys_int2;
typedef int sys_bool;

enum {
    SYS_FALSE = 0,
    SYS_TRUE = 1,
    SYS_MAX_KEYS = 256
};
struct sys_button {
    sys_bool down;
    sys_bool pressed;
    sys_bool released;
};
enum sys_mouse_mode {
    SYSTEM_MOUSE_ABSOLUTE,
    SYSTEM_MOUSE_RELATIVE
};
struct sys_mouse {
    enum sys_mouse_mode mode;
    sys_bool visible;
    sys_int2 position;
    sys_int2 position_last;
    sys_int2 position_delta;
    float scroll_delta;
    struct sys_button left_button;
    struct sys_button right_button;
};
struct sys_time {
    float refresh_rate;
    float last;
    float delta;
    unsigned int start;
};
struct sys_window {
    const char *title;
    SDL_Window *handle;
    SDL_GLContext glContext;
    sys_int2 position;
    sys_int2 size;
    sys_bool resized;
    sys_bool moved;
};
struct sys {
    sys_bool quit;
    sys_bool initialized;
    struct sys_time time;
    struct sys_window window;
    struct sys_button key[SYS_MAX_KEYS];
    struct sys_mouse mouse;
};
static void
sys_init(struct sys *s)
{
    int monitor_refresh_rate;
    int win_x, win_y, win_w, win_h;
    const char *title;

    SDL_Init(SDL_INIT_VIDEO);
    title = s->window.title ? s->window.title: "SDL";
    win_x = s->window.position.x ? s->window.position.x: SDL_WINDOWPOS_CENTERED;
    win_y = s->window.position.y ? s->window.position.y: SDL_WINDOWPOS_CENTERED;
    win_w = s->window.size.x ? s->window.size.x: 800;
    win_h = s->window.size.y ? s->window.size.y: 600;
    s->window.handle = SDL_CreateWindow(title, win_x, win_y, win_w, win_h, SDL_WINDOW_OPENGL|SDL_WINDOW_SHOWN);

    SDL_GL_SetAttribute(SDL_GL_CONTEXT_PROFILE_MASK, SDL_GL_CONTEXT_PROFILE_COMPATIBILITY);
    s->window.glContext = SDL_GL_CreateContext(s->window.handle);
    s->mouse.visible = SYS_TRUE;

    {SDL_DisplayMode mode;
    if (SDL_GetDisplayMode(0, 0, &mode))
        monitor_refresh_rate = mode.refresh_rate;
    else monitor_refresh_rate = 60;
    s->time.refresh_rate = 1.0f/(float)monitor_refresh_rate;}
    s->initialized = SYS_TRUE;
}
static void
sys_shutdown(struct sys *s)
{
    SDL_GL_DeleteContext(s->window.glContext);
    SDL_DestroyWindow(s->window.handle);
    SDL_Quit();
}
static void
sys_update_button(struct sys_button *b, sys_bool down)
{
    sys_bool was_down = b->down;
    b->down = down;
    b->pressed = !was_down && down;
    b->released = was_down && !down;
}
static void
sys_pull(struct sys *s)
{
    int i = 0;
    SDL_PumpEvents();
    s->time.start = SDL_GetTicks();
    s->mouse.left_button.pressed = 0;
    s->mouse.left_button.released = 0;
    s->mouse.right_button.pressed = 0;
    s->mouse.right_button.released = 0;
    s->mouse.scroll_delta = 0;

    /* poll window */
    s->window.moved = 0;
    s->window.resized = 0;
    SDL_GetWindowSize(s->window.handle, &s->window.size.x, &s->window.size.y);
    SDL_GetWindowPosition(s->window.handle, &s->window.position.x, &s->window.position.y);

    /* poll keyboard */
    {const unsigned char* keys = SDL_GetKeyboardState(0);
    for (i = 0; i < SYS_MAX_KEYS; ++i) {
        SDL_Keycode sym = SDL_GetKeyFromScancode((SDL_Scancode)i);
        if (sym < SYS_MAX_KEYS)
            sys_update_button(s->key + sym, keys[i]);
    }}

    /* poll mouse */
    s->mouse.position_delta.x = 0;
    s->mouse.position_delta.y = 0;
    SDL_SetRelativeMouseMode(s->mouse.mode == SYSTEM_MOUSE_RELATIVE);
    if (s->mouse.mode == SYSTEM_MOUSE_ABSOLUTE) {
        s->mouse.position_last.x = s->mouse.position.x;
        s->mouse.position_last.y = s->mouse.position.y;
        SDL_GetMouseState(&s->mouse.position.x, &s->mouse.position.y);
        s->mouse.position_delta.x = s->mouse.position.x - s->mouse.position_last.x;
        s->mouse.position_delta.y = s->mouse.position.y - s->mouse.position_last.y;
    } else {
        s->mouse.position_last.x = s->window.size.x/2;
        s->mouse.position_last.y = s->window.size.y/2;
        s->mouse.position.x = s->window.size.x/2;
        s->mouse.position.y = s->window.size.y/2;
    }

    /* handle events */
    {SDL_Event evt;
    while (SDL_PollEvent(&evt)) {
        if (evt.type == SDL_QUIT)
            s->quit = SYS_TRUE;
        else if (evt.type == SDL_MOUSEBUTTONDOWN ||  evt.type == SDL_MOUSEBUTTONUP) {
            sys_bool down = evt.type == SDL_MOUSEBUTTONDOWN;
            if (evt.button.button == SDL_BUTTON_LEFT)
                sys_update_button(&s->mouse.left_button, down);
            else if (evt.button.button == SDL_BUTTON_RIGHT)
                sys_update_button(&s->mouse.right_button, down);
        } else if (evt.type == SDL_WINDOWEVENT) {
            if (evt.window.event == SDL_WINDOWEVENT_MOVED)
                s->window.moved = SYS_TRUE;
            else if (evt.window.event == SDL_WINDOWEVENT_RESIZED)
                s->window.resized = SYS_TRUE;
        } else if (evt.type == SDL_MOUSEMOTION) {
            s->mouse.position_delta.x += evt.motion.xrel;
            s->mouse.position_delta.y += evt.motion.yrel;
        } else if (evt.type == SDL_MOUSEWHEEL) {
            s->mouse.scroll_delta = -evt.wheel.y;
        }
    }}
}
static void
sys_push(struct sys *s)
{
    SDL_GL_SwapWindow(s->window.handle);
    s->time.delta = (SDL_GetTicks() - s->time.start) / 1000.0f;
    if (s->time.delta < s->time.refresh_rate)
        SDL_Delay((Uint32)((s->time.refresh_rate - s->time.delta) * 1000.0f));
}
#define glBoxf(b) glBox(b[0], b[1], b[2], b[3], b[4], b[5]);
static void
glBox(float min_x, float min_y, float min_z, float max_x, float max_y, float max_z)
{
    float forward = max_x - min_x;
    float up = max_y - min_y;
    float right = max_z - min_z;

    glBegin(GL_TRIANGLES);
        /* Back face */
        glNormal3f(-1,0,0);
        glTexCoord2f(0,0);
        glVertex3f(min_x, min_y, min_z);
        glTexCoord2f(1,0);
        glVertex3f(min_x, min_y, min_z + right);
        glTexCoord2f(1,1);
        glVertex3f(min_x, min_y + up, min_z + right);
        glTexCoord2f(0,0);
        glVertex3f(min_x, min_y, min_z);
        glTexCoord2f(1,1);
        glVertex3f(min_x, min_y + up, min_z + right);
        glTexCoord2f(0,1);
        glVertex3f(min_x, min_y + up, min_z);

        /* Left face */
        glNormal3f(0,0,-1);
        glTexCoord2f(0,0);
        glVertex3f(min_x, min_y, min_z);
        glTexCoord2f(0,1);
        glVertex3f(min_x, min_y+up, min_z);
        glTexCoord2f(1,1);
        glVertex3f(min_x+forward, min_y+up, min_z);
        glTexCoord2f(0,0);
        glVertex3f(min_x, min_y, min_z);
        glTexCoord2f(1,1);
        glVertex3f(min_x+forward, min_y+up, min_z);
        glTexCoord2f(1,0);
        glVertex3f(min_x+forward, min_y, min_z);

        /* Bottom face */
        glNormal3f(0,-1,0);
        glTexCoord2f(0,0);
        glVertex3f(min_x, min_y, min_z);
        glTexCoord2f(1,0);
        glVertex3f(min_x+forward, min_y, min_z);
        glTexCoord2f(1,1);
        glVertex3f(min_x+forward, min_y, min_z+right);
        glTexCoord2f(0,0);
        glVertex3f(min_x, min_y, min_z);
        glTexCoord2f(1,1);
        glVertex3f(min_x+forward, min_y, min_z+right);
        glTexCoord2f(0,1);
        glVertex3f(min_x, min_y, min_z+right);

        /* Top face */
        glNormal3f(0,1,0);
        glTexCoord2f(0,1);
        glVertex3f(min_x+forward, min_y+up, min_z+right);
        glTexCoord2f(0,0);
        glVertex3f(min_x+forward, min_y+up, min_z);
        glTexCoord2f(1,0);
        glVertex3f(min_x, min_y+up, min_z);
        glTexCoord2f(0,1);
        glVertex3f(min_x+forward, min_y+up, min_z+right);
        glTexCoord2f(1,0);
        glVertex3f(min_x, min_y+up, min_z);
        glTexCoord2f(1,1);
        glVertex3f(min_x, min_y+up, min_z+right);

        /* Right face */
        glNormal3f(0,0,1);
        glTexCoord2f(0,1);
        glVertex3f(min_x+forward, min_y+up, min_z+right);
        glTexCoord2f(1,1);
        glVertex3f(min_x, min_y+up, min_z+right);
        glTexCoord2f(1,0);
        glVertex3f(min_x, min_y, min_z+right);
        glTexCoord2f(0,1);
        glVertex3f(min_x+forward, min_y+up, min_z+right);
        glTexCoord2f(1,0);
        glVertex3f(min_x, min_y, min_z+right);
        glTexCoord2f(0,0);
        glVertex3f(min_x+forward, min_y, min_z+right);

        /* Front face */
        glNormal3f(0,0,1);
        glTexCoord2f(0,1);
        glVertex3f(min_x+forward, min_y+up, min_z+right);
        glTexCoord2f(0,0);
        glVertex3f(min_x+forward, min_y, min_z+right);
        glTexCoord2f(1,0);
        glVertex3f(min_x+forward, min_y, min_z);
        glTexCoord2f(0,1);
        glVertex3f(min_x+forward, min_y+up, min_z+right);
        glTexCoord2f(1,0);
        glVertex3f(min_x+forward, min_y, min_z);
        glTexCoord2f(1,1);
        glVertex3f(min_x+forward, min_y+up, min_z);
    glEnd();
}
static float
v3dot(const float *a, const float *b)
{
    return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}
static void
v3sub(float *dst, const float *a, const float *b)
{
    dst[0] = a[0] - b[0];
    dst[1] = a[1] - b[1];
    dst[2] = a[2] - b[2];
}
static int
ray_intersects_sphere(const float *ray_origin, const float *ray_dir,
    const float *center, float radius)
{
    float p[3], a,b,c,d;
    v3sub(p, ray_origin, center);
    a = v3dot(ray_dir, ray_dir);
    b = v3dot(ray_dir, p);
    c = v3dot(p, p) - (radius* radius);
    d = b * b - c * a;
    return (d >= 0.0f);
}
int main(void)
{
    const float cube_colors[][3] = {{1,0,0}, {0,1,0}, {0,0,1}, {1,1,0}};
    const float cube_bounds[][6] = {{-1,-1,-1, 1,1,1}, {6,-1,-3, 4,1,1},
        {-6,-1,-5, -4,1,-3}, {-1,-1,-10, 1,1,-8}};

    /* Platform */
    struct sys sys;
    memset(&sys, 0, sizeof(sys));
    sys.window.title = "Demo";
    sys.window.size.x = 800;
    sys.window.size.y = 600;
    sys_init(&sys);

    /* Camera */
    {struct cam cam;
    cam_init(&cam);
    cam.pos[2] = 5;
    cam.aspect_ratio = (float)sys.window.size.x/(float)sys.window.size.y;
    cam_build(&cam);

    while (!sys.quit)
    {
        sys_pull(&sys);

        /* Camera control */
        #define CAMERA_SPEED (4.0f)
        {const float frame_movement = CAMERA_SPEED * sys.time.delta;
        const float forward = (sys.key['w'].down ? -1 : 0.0f) + (sys.key['s'].down ? 1 : 0.0f);
        const float right = (sys.key['a'].down ? -1 : 0.0f) + (sys.key['d'].down ? 1 : 0.0f);
        cam_move(&cam, right * frame_movement, 0, forward * frame_movement);
        cam.off[2] += sys.mouse.scroll_delta;
        if (sys.mouse.right_button.down) {
            sys.mouse.mode = SYSTEM_MOUSE_RELATIVE;
            cam.ear[0] += (float)sys.mouse.position_delta.y * TO_RAD;
            cam.ear[1] += (float)sys.mouse.position_delta.x * TO_RAD;
        } else sys.mouse.mode = SYSTEM_MOUSE_ABSOLUTE;
        cam.aspect_ratio = (float)sys.window.size.x/(float)sys.window.size.y;
        cam_build(&cam);}

        /* 3D Picking */
        if (sys.mouse.left_button.pressed) {
            /* Picking ray */
            float ray_origin[3], ray_dir[3];
            cam_picking_ray(&cam, sys.window.size.x, sys.window.size.y,
                sys.mouse.position.x, (sys.window.size.y-1) - sys.mouse.position.y,
                ray_origin, ray_dir);

            /* Sphere intersection  */
            {size_t i = 0;
            for (i = 0; i < countof(cube_colors); ++i) {
                float size[3];
                const float *bounds = &cube_bounds[i][0];
                size[0] = (bounds[3] - bounds[0]);
                size[1] = (bounds[4] - bounds[1]);
                size[2] = (bounds[5] - bounds[2]);

                {float center[3];
                float radius = max3(size[0], size[1], size[2]);
                center[0] = bounds[0] + (size[0] * 0.5f);
                center[1] = bounds[1] + (size[1] * 0.5f);
                center[2] = bounds[2] + (size[2] * 0.5f);

                if (ray_intersects_sphere(ray_origin, ray_dir,  center, radius))
                    fprintf(stdout, "Sphere Intersected: %lu!\n", i);
                else fprintf(stdout, "Sphere Not Intersected!\n");}
            }}
        }

        /* Draw */
        {size_t i = 0;
        glEnable(GL_DEPTH_TEST);
        glViewport(0, 0, sys.window.size.x, sys.window.size.y);
        glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
        glMatrixMode(GL_PROJECTION);
        glLoadIdentity();
        glLoadMatrixf((float*)cam.proj);
        glMatrixMode(GL_MODELVIEW);
        glLoadIdentity();
        glLoadMatrixf((float*)cam.view);
        for (i = 0; i < countof(cube_colors); ++i) {
            glColor3fv(cube_colors[i]);
            glBoxf(cube_bounds[i]);
        }}
        sys_push(&sys);
    }}
    sys_shutdown(&sys);
    return 0;
}

Hi - do you have any source for the derivation of calculations at line 146? Initially I thought this is simply euler to quaternion conversion, followed by a quaternion multiplication. Problem is that writing it out explicitly as these two operations does not seem to produce same results...