quat.c

#include <stdlib.h>
#include <math.h>

#include "gl-matrix.h"

quat_t quat_create(quat_t quat) {
    quat_t dest = calloc(sizeof(double), 4);

    if (quat) {
        dest[0] = quat[0];
        dest[1] = quat[1];
        dest[2] = quat[2];
        dest[3] = quat[3];
    }

    return dest;
}

quat_t quat_set(quat_t quat, quat_t dest) {
    dest[0] = quat[0];
    dest[1] = quat[1];
    dest[2] = quat[2];
    dest[3] = quat[3];

    return dest;
}

quat_t quat_calculateW(quat_t quat, quat_t dest) {
    double x = quat[0], y = quat[1], z = quat[2];

    if (!dest || quat == dest) {
        quat[3] = -sqrt(fabs(1.0 - x * x - y * y - z * z));
        return quat;
    }
    dest[0] = x;
    dest[1] = y;
    dest[2] = z;
    dest[3] = -sqrt(fabs(1.0 - x * x - y * y - z * z));
    return dest;
}

double quat_dot(quat_t quat, quat_t quat2) {
    return quat[0]*quat2[0] + quat[1]*quat2[1] + quat[2]*quat2[2] + quat[3]*quat2[3];
}

quat_t quat_inverse(quat_t quat, quat_t dest) {
    double dot = quat_dot(quat,quat),
        invDot = 1.0/dot;
    if(!dest || quat == dest) {
        quat[0] *= -invDot;
        quat[1] *= -invDot;
        quat[2] *= -invDot;
        quat[3] *= invDot;
        return quat;
    }
    dest[0] = -quat[0]*invDot;
    dest[1] = -quat[1]*invDot;
    dest[2] = -quat[2]*invDot;
    dest[3] = quat[3]*invDot;
    return dest;
}

quat_t quat_conjugate(quat_t quat, quat_t dest) {
    if (!dest || quat == dest) {
        quat[0] *= -1;
        quat[1] *= -1;
        quat[2] *= -1;
        return quat;
    }
    dest[0] = -quat[0];
    dest[1] = -quat[1];
    dest[2] = -quat[2];
    dest[3] = quat[3];
    return dest;
}

double quat_length(quat_t quat) {
    double x = quat[0], y = quat[1], z = quat[2], w = quat[3];
    return sqrt(x * x + y * y + z * z + w * w);
}

quat_t quat_normalize(quat_t quat, quat_t dest) {
    if (!dest) { dest = quat; }

    double x = quat[0], y = quat[1], z = quat[2], w = quat[3],
        len = sqrt(x * x + y * y + z * z + w * w);
    if (len == 0) {
        dest[0] = 0;
        dest[1] = 0;
        dest[2] = 0;
        dest[3] = 0;
        return dest;
    }
    len = 1 / len;
    dest[0] = x * len;
    dest[1] = y * len;
    dest[2] = z * len;
    dest[3] = w * len;

    return dest;
}

quat_t quat_multiply(quat_t quat, quat_t quat2, quat_t dest) {
    if (!dest) { dest = quat; }

    double qax = quat[0], qay = quat[1], qaz = quat[2], qaw = quat[3],
        qbx = quat2[0], qby = quat2[1], qbz = quat2[2], qbw = quat2[3];

    dest[0] = qax * qbw + qaw * qbx + qay * qbz - qaz * qby;
    dest[1] = qay * qbw + qaw * qby + qaz * qbx - qax * qbz;
    dest[2] = qaz * qbw + qaw * qbz + qax * qby - qay * qbx;
    dest[3] = qaw * qbw - qax * qbx - qay * qby - qaz * qbz;

    return dest;
}

quat_t quat_multiplyVec3(quat_t quat, vec3_t vec, vec3_t dest) {
    if (!dest) { dest = vec; }

    double x = vec[0], y = vec[1], z = vec[2],
        qx = quat[0], qy = quat[1], qz = quat[2], qw = quat[3],

        // calculate quat * vec
        ix = qw * x + qy * z - qz * y,
        iy = qw * y + qz * x - qx * z,
        iz = qw * z + qx * y - qy * x,
        iw = -qx * x - qy * y - qz * z;

    // calculate result * inverse quat
    dest[0] = ix * qw + iw * -qx + iy * -qz - iz * -qy;
    dest[1] = iy * qw + iw * -qy + iz * -qx - ix * -qz;
    dest[2] = iz * qw + iw * -qz + ix * -qy - iy * -qx;

    return dest;
}

mat3_t quat_toMat3(quat_t quat, mat3_t dest) {
    if (!dest) { dest = mat3_create(NULL); }

    double x = quat[0], y = quat[1], z = quat[2], w = quat[3],
        x2 = x + x,
        y2 = y + y,
        z2 = z + z,

        xx = x * x2,
        xy = x * y2,
        xz = x * z2,
        yy = y * y2,
        yz = y * z2,
        zz = z * z2,
        wx = w * x2,
        wy = w * y2,
        wz = w * z2;

    dest[0] = 1 - (yy + zz);
    dest[1] = xy + wz;
    dest[2] = xz - wy;

    dest[3] = xy - wz;
    dest[4] = 1 - (xx + zz);
    dest[5] = yz + wx;

    dest[6] = xz + wy;
    dest[7] = yz - wx;
    dest[8] = 1 - (xx + yy);

    return dest;
}

quat_t quat_toMat4(quat_t quat, mat4_t dest) {
    if (!dest) { dest = mat4_create(NULL); }

    double x = quat[0], y = quat[1], z = quat[2], w = quat[3],
        x2 = x + x,
        y2 = y + y,
        z2 = z + z,

        xx = x * x2,
        xy = x * y2,
        xz = x * z2,
        yy = y * y2,
        yz = y * z2,
        zz = z * z2,
        wx = w * x2,
        wy = w * y2,
        wz = w * z2;

    dest[0] = 1 - (yy + zz);
    dest[1] = xy + wz;
    dest[2] = xz - wy;
    dest[3] = 0;

    dest[4] = xy - wz;
    dest[5] = 1 - (xx + zz);
    dest[6] = yz + wx;
    dest[7] = 0;

    dest[8] = xz + wy;
    dest[9] = yz - wx;
    dest[10] = 1 - (xx + yy);
    dest[11] = 0;

    dest[12] = 0;
    dest[13] = 0;
    dest[14] = 0;
    dest[15] = 1;

    return dest;
}

quat_t quat_slerp(quat_t quat, quat_t quat2, double slerp, quat_t dest) {
    if (!dest) { dest = quat; }

    double cosHalfTheta = quat[0] * quat2[0] + quat[1] * quat2[1] + quat[2] * quat2[2] + quat[3] * quat2[3],
        halfTheta,
        sinHalfTheta,
        ratioA,
        ratioB;

    if (fabs(cosHalfTheta) >= 1.0) {
        if (dest != quat) {
            dest[0] = quat[0];
            dest[1] = quat[1];
            dest[2] = quat[2];
            dest[3] = quat[3];
        }
        return dest;
    }

    halfTheta = acos(cosHalfTheta);
    sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);

    if (fabs(sinHalfTheta) < 0.001) {
        dest[0] = (quat[0] * 0.5 + quat2[0] * 0.5);
        dest[1] = (quat[1] * 0.5 + quat2[1] * 0.5);
        dest[2] = (quat[2] * 0.5 + quat2[2] * 0.5);
        dest[3] = (quat[3] * 0.5 + quat2[3] * 0.5);
        return dest;
    }

    ratioA = sin((1 - slerp) * halfTheta) / sinHalfTheta;
    ratioB = sin(slerp * halfTheta) / sinHalfTheta;

    dest[0] = (quat[0] * ratioA + quat2[0] * ratioB);
    dest[1] = (quat[1] * ratioA + quat2[1] * ratioB);
    dest[2] = (quat[2] * ratioA + quat2[2] * ratioB);
    dest[3] = (quat[3] * ratioA + quat2[3] * ratioB);

    return dest;
}