CgeMath/Mat4f.c

#include "CgeMath.h"
#include <string.h>
#include <math.h>

#define EPSILON 0.00001f
#define PI      3.14159265358979323846f
#define SET_ROW(row, val) row[0] = row[1] = row[2] = row[3] = val

void CgeMat4fIdentity(float out[16]) {
    const float ident[16] = {
        1.0f, 0.0f, 0.0f, 0.0f,
        0.0f, 1.0f, 0.0f, 0.0f,
        0.0f, 0.0f, 1.0f, 0.0f,
        0.0f, 0.0f, 0.0f, 1.0f
    };

    memcpy(out, ident, sizeof(ident));
}

void CgeMat4fAdd(const float a[16], const float b[16], float out[16]) {
    CgeVec4fAdd(&a[0], &b[0], &out[0]);
    CgeVec4fAdd(&a[4], &b[4], &out[4]);
    CgeVec4fAdd(&a[8], &b[8], &out[8]);
    CgeVec4fAdd(&a[12], &b[12], &out[12]);
}

void CgeMat4fSub(const float a[16], const float b[16],  float out[16]) {
    CgeVec4fSub(&a[0], &b[0], &out[0]);
    CgeVec4fSub(&a[4], &b[4], &out[4]);
    CgeVec4fSub(&a[8], &b[8], &out[8]);
    CgeVec4fSub(&a[12], &b[12], &out[12]);
}

void CgeMat4fMul(const float a[16], const float b[16], float out[16]) {
    float tmp[16], row[4];

    SET_ROW(row, b[0]); CgeVec4fMul(&a[0], row, &tmp[0]);
    SET_ROW(row, b[1]); CgeVec4fMulAdd(&a[4], row, &tmp[0], &tmp[0]);
    SET_ROW(row, b[2]); CgeVec4fMulAdd(&a[8], row, &tmp[0], &tmp[0]);
    SET_ROW(row, b[3]); CgeVec4fMulAdd(&a[12], row, &tmp[0], &tmp[0]);

    SET_ROW(row, b[4]); CgeVec4fMul(&a[0], row, &tmp[4]);
    SET_ROW(row, b[5]); CgeVec4fMulAdd(&a[4], row, &tmp[4], &tmp[4]);
    SET_ROW(row, b[6]); CgeVec4fMulAdd(&a[8], row, &tmp[4], &tmp[4]);
    SET_ROW(row, b[7]); CgeVec4fMulAdd(&a[12], row, &tmp[4], &tmp[4]);

    SET_ROW(row, b[8]); CgeVec4fMul(&a[0], row, &tmp[8]);
    SET_ROW(row, b[9]); CgeVec4fMulAdd(&a[4], row, &tmp[8], &tmp[8]);
    SET_ROW(row, b[10]); CgeVec4fMulAdd(&a[8], row, &tmp[8], &tmp[8]);
    SET_ROW(row, b[11]); CgeVec4fMulAdd(&a[12], row, &tmp[8], &tmp[8]);

    SET_ROW(row, b[12]); CgeVec4fMul(&a[0], row, &tmp[12]);
    SET_ROW(row, b[13]); CgeVec4fMulAdd(&a[4], row, &tmp[12], &tmp[12]);
    SET_ROW(row, b[14]); CgeVec4fMulAdd(&a[8], row, &tmp[12], &tmp[12]);
    SET_ROW(row, b[15]); CgeVec4fMulAdd(&a[12], row, &tmp[12], &tmp[12]);

    memcpy(out, tmp, sizeof(tmp));
}

void CgeMat4fScale(const float a[16], float b, float out[16]) {
    CgeVec4fScale(&a[0], b, &out[0]);
    CgeVec4fScale(&a[4], b, &out[4]);
    CgeVec4fScale(&a[8], b, &out[8]);
    CgeVec4fScale(&a[12], b, &out[12]);
}

void CgeMat4fTranspose(const float in[16], float out[16]) {
    float tmp[16];

    tmp[0] = in[0]; tmp[4] = in[1]; tmp[8] = in[2]; tmp[12] = in[3];
    tmp[1] = in[4]; tmp[5] = in[5]; tmp[9] = in[6]; tmp[13] = in[7];
    tmp[2] = in[8]; tmp[6] = in[9]; tmp[10] = in[10]; tmp[14] = in[11];
    tmp[3] = in[12]; tmp[7] = in[13]; tmp[11] = in[14]; tmp[15] = in[15];

    memcpy(out, tmp, sizeof(tmp));
}

float CgeMat4fTrace(const float in[16]) {
    return in[0] + in[5] + in[10] + in[15];
}

float CgeMat4fDet(const float in[16]) {
    float a, b, c, d, e, f, result;

    a = in[2]  * in[7]  - in[3]  * in[6];
    b = in[2]  * in[11] - in[3]  * in[10];
    c = in[2]  * in[15] - in[3]  * in[14];
    d = in[6]  * in[11] - in[7]  * in[10];
    e = in[6]  * in[15] - in[7]  * in[14];
    f = in[10] * in[15] - in[11] * in[14];

    result = 0.0f;
    result += in[0]  * (in[5] * f - in[9] * e + in[13] * d);
    result -= in[4]  * (in[1] * f - in[9] * c + in[13] * b);
    result += in[8]  * (in[1] * e - in[5] * c + in[13] * a);
    result -= in[12] * (in[1] * d - in[5] * b + in[9]  * a);

    return result;
}

int CgeMat4fInverse(const float in[16], float out[16]) {
    float a, b, c, d, e, f, det;
    float tmp[16];

    a = in[2]  * in[7]  - in[3]  * in[6];
    b = in[2]  * in[11] - in[3]  * in[10];
    c = in[2]  * in[15] - in[3]  * in[14];
    d = in[6]  * in[11] - in[7]  * in[10];
    e = in[6]  * in[15] - in[7]  * in[14];
    f = in[10] * in[15] - in[11] * in[14];

    tmp[0] =  (in[5] * f - in[9] * e + in[13] * d);
    tmp[1] = -(in[1] * f - in[9] * c + in[13] * b);
    tmp[2] =  (in[1] * e - in[5] * c + in[13] * a);
    tmp[3] = -(in[1] * d - in[5] * b + in[9]  * a);

    det = 0.0f;
    det += in[0]  * tmp[0];
    det += in[4]  * tmp[1];
    det += in[8]  * tmp[2];
    det += in[12] * tmp[3];

    if (fabsf(det) < EPSILON)
        return 0;

    tmp[4] = -(in[4] * f - in[8] * e + in[12] * d);
    tmp[5] =  (in[0] * f - in[8] * c + in[12] * b);
    tmp[6] = -(in[0] * e - in[4] * c + in[12] * a);
    tmp[7] =  (in[0] * d - in[4] * b + in[8]  * a);

    a = in[1] * in[7]  - in[3]  * in[5];
    b = in[1] * in[11] - in[3]  * in[9];
    c = in[1] * in[15] - in[3]  * in[13];
    d = in[5] * in[11] - in[7]  * in[9];
    e = in[5] * in[15] - in[7]  * in[13];
    f = in[9] * in[15] - in[11] * in[13];

    tmp[8] =   (in[4] * f - in[8] * e + in[12] * d);
    tmp[9] =  -(in[0] * f - in[8] * c + in[12] * b);
    tmp[10] =  (in[0] * e - in[4] * c + in[12] * a);
    tmp[11] = -(in[0] * d - in[4] * b + in[8]  * a);

    a = in[1] * in[6]  - in[2]  * in[5];
    b = in[1] * in[10] - in[2]  * in[9];
    c = in[1] * in[14] - in[2]  * in[13];
    d = in[5] * in[10] - in[6]  * in[9];
    e = in[5] * in[14] - in[6]  * in[13];
    f = in[9] * in[14] - in[10] * in[13];

    tmp[12] = -(in[4] * f - in[8] * e + in[12] * d);
    tmp[13] =  (in[0] * f - in[8] * c + in[12] * b);
    tmp[14] = -(in[0] * e - in[4] * c + in[12] * a);
    tmp[15] =  (in[0] * d - in[4] * b + in[8]  * a);

    CgeMat4fScale(tmp, 1.0f / det, out);
    return 1;
}

void CgeMat4fFromScale(float x, float y, float z, float out[16]) {
    CgeMat4fIdentity(out);
    out[0] = x;
    out[5] = y;
    out[10] = z;
}

void CgeMat4fFromTranslation(float x, float y, float z, float out[16]) {
    CgeMat4fIdentity(out);
    out[12] = x;
    out[13] = y;
    out[14] = z;
}

void CgeMat4fFromRotationX(float angle, float out[16]) {
    float c, s;

    c = cosf(angle);
    s = sinf(angle);

    CgeMat4fIdentity(out);
    out[5] = c;
    out[6] = s;
    out[9] = -s;
    out[10] = c;
}

void CgeMat4fFromRotationY(float angle, float out[16]) {
    float c, s;

    c = cosf(angle);
    s = sinf(angle);

    CgeMat4fIdentity(out);
    out[0] = c;
    out[2] = -s;
    out[8] = s;
    out[10] = c;
}

void CgeMat4fFromRotationZ(float angle, float out[16]) {
    float c, s;

    c = cosf(angle);
    s = sinf(angle);

    CgeMat4fIdentity(out);
    out[0] = c;
    out[1] = s;
    out[4] = -s;
    out[5] = c;
}

void CgeMat4fFromAxis(const float axis[3], float angle, float out[16]) {
    float x, y, z, length;
    float c, s, moc, xx, xy, xz, yy, yz, zz;

    length = CgeVec3fLength(axis);
    CgeMat4fIdentity(out);

    if (fabsf(length) < EPSILON)
        return;

    x = axis[0] / length;
    y = axis[1] / length;
    z = axis[2] / length;

    c = cosf(angle);
    s = sinf(angle);
    moc = 1.0f - c;

    xx = x * x;
    xy = x * y;
    xz = x * z;
    yy = y * y;
    yz = y * z;
    zz = z * z;

    out[0] = c + xx * moc;
    out[1] = xy * moc + z * s;
    out[2] = xz * moc - y * s;

    out[4] = xy * moc - z * s;
    out[5] = c + yy * moc;
    out[6] = yz * moc + x * s;

    out[8] = xz * moc + y * s;
    out[9] = yz * moc - x * s;
    out[10] = c + zz * moc;
}

void CgeMat4fFromEuler(float roll, float pitch, float yaw, float out[16]) {
    float rs, rc, ys, yc, ps, pc;

    rs = sinf(roll);
    rc = cosf(roll);
    ps = sinf(pitch);
    pc = cosf(pitch);
    ys = sinf(yaw);
    yc = cosf(yaw);

    CgeMat4fIdentity(out);
    out[0] = pc * yc;
    out[1] = pc * ys;
    out[2] = -ps;
    out[4] = ps * rs * yc - rc * ys;
    out[5] = ps * rs * ys + rc * yc;
    out[6] = pc * rs;
    out[8] = rs * ys + ps * rc * yc;
    out[9] = ps * rc * ys - rs * yc;
    out[10] = pc * rc;
}

void CgeMat4fFromQuat4f(const float in[4], float out[16]) {
    CgeQuat4fToMat4f(in, out);
}

void CgeMat4fFromOrtho(float xMin, float xMax, float yMin, float yMax,
                       float zMin, float zMax, float out[16]) {
    float dx, dy, dz;

    dx = xMax - xMin;
    dy = yMax - yMin;
    dz = zMax - zMin;

    CgeMat4fIdentity(out);

    out[0] = 2.0f / dx;
    out[5] = 2.0f / dy;
    out[10] = -2.0f / dz;
    out[12] = -(xMax + xMin) / dx;
    out[13] = -(yMax + yMin) / dy;
    out[14] = -(zMax + zMin) / dz;
}

void CgeMat4fFromFrustum(float fov, float aspect, float zMin, float zMax,
                         float out[16]) {
    float t, dz;

    dz = zMax - zMin;
    t = tanf(fov / 2.0f);

    CgeMat4fIdentity(out);

    out[0] = 1.0f / (aspect * t);
    out[5] = 1.0f / t;
    out[10] = -(zMax + zMin) / dz;
    out[11] = -1.0f;
    out[14] = -(2.0f * zMax * zMin) / dz;
    out[15] = 0.0f;
}

void CgeMat4fFromLookAt(const float position[3], const float at[3],
                        const float up[3], float out[16]) {
    float cameraDir[3], cameraRight[3], cameraUp[3];

    CgeVec3fSub(position, at, cameraDir);
    CgeVec3fNormal(cameraDir, cameraDir);
    CgeVec3fCross(up, cameraDir, cameraRight);
    CgeVec3fNormal(cameraRight, cameraRight);
    CgeVec3fCross(cameraDir, cameraRight, cameraUp);

    out[0] = cameraRight[0];
    out[1] = cameraUp[0];
    out[2] = cameraDir[0];
    out[3] = 0.0f;

    out[4] = cameraRight[1];
    out[5] = cameraUp[1];
    out[6] = cameraDir[1];
    out[7] = 0.0f;

    out[8] = cameraRight[2];
    out[9] = cameraUp[2];
    out[10] = cameraDir[2];
    out[11] = 0.0f;

    out[12] = -CgeVec3fDot(cameraRight, position);
    out[13] = -CgeVec3fDot(cameraUp, position);
    out[14] = -CgeVec3fDot(cameraDir, position);
    out[15] = 1.0f;
}

void CgeMat4fApplyVec4f(const float a[16], const float b[4], float out[4]) {
    float tmp[4], row[4];

    SET_ROW(row, b[0]); CgeVec4fMul(&a[0], row, tmp);
    SET_ROW(row, b[1]); CgeVec4fMulAdd(&a[4], row, tmp, tmp);
    SET_ROW(row, b[2]); CgeVec4fMulAdd(&a[8], row, tmp, tmp);
    SET_ROW(row, b[3]); CgeVec4fMulAdd(&a[12], row, tmp, tmp);

    memcpy(out, tmp, sizeof(tmp));
}

void CgeMat4fApplyVec3f(const float a[16], const float b[3], float out[3]) {
    float tmp[4], row[4];

    SET_ROW(row, b[0]); CgeVec4fMul(&a[0], row, tmp);
    SET_ROW(row, b[1]); CgeVec4fMulAdd(&a[4], row, tmp, tmp);
    SET_ROW(row, b[2]); CgeVec4fMulAdd(&a[8], row, tmp, tmp);
    SET_ROW(row, 1.0f); CgeVec4fMulAdd(&a[12], row, tmp, tmp);

    memcpy(out, tmp, sizeof(float) * 3);
}