Subversion Repositories Games.Carmageddon

#include "quat.h"

#include "matrix34.h"

#include "matrix4.h"

#include "harness/trace.h"

#include <math.h>

#define M(x, y) mat->m[x][y]

#define M_DIAG(x) (((br_scalar*)mat->m)[4*(x)])

#define Q_EL(e) (((br_scalar*)&(q->x))[(e)])

// IDA: br_quat* __cdecl BrQuatMul(br_quat *q, br_quat *l, br_quat *r)

br_quat* BrQuatMul(br_quat* q, br_quat* l, br_quat* r) {

    br_scalar x1;

    br_scalar x2;

    br_scalar x3;

    br_scalar x4;

    //br_scalar x5; // Pierre-Marie Baty -- unused variable

    //br_scalar x6; // Pierre-Marie Baty -- unused variable

    //br_scalar x7; // Pierre-Marie Baty -- unused variable

    //br_scalar x8; // Pierre-Marie Baty -- unused variable

    br_scalar s;

    //br_scalar t; // Pierre-Marie Baty -- unused variable

    LOG_TRACE("(%p, %p, %p)", q, l, r);

    x1 = (l->x + l->z) * (r->y + r->x);

    x2 = (l->w + l->y) * (r->w - r->z);

    x3 = (l->w - l->y) * (r->w + r->z);

    s = x1 + x2 + x3;

    x4 = ((l->z - l->x) * (r->x - r->y) + s) * 0.5f;

    q->x = x4 + (l->w + l->x) * (r->w + r->x) - s;

    q->y = x4 + (l->w - l->x) * (r->z + r->y) - x3;

    q->z = x4 + (l->y + l->z) * (r->w - r->x) - x2;

    q->w = x4 + (l->z - l->y) * (r->y - r->z) - x1;

    return q;

}

// IDA: br_quat* __cdecl BrQuatNormalise(br_quat *q, br_quat *qq)

br_quat* BrQuatNormalise(br_quat* q, br_quat* qq) {

    br_scalar s;

    LOG_TRACE("(%p, %p)", q, qq);

    s = 1.f / sqrtf(qq->x * qq->x + qq->y * qq->y + qq->z * qq->z + qq->w * qq->w);

    q->x = qq->x * s;

    q->y = qq->y * s;

    q->z = qq->z * s;

    q->w = qq->w * s;

    return q;

}

// IDA: br_quat* __cdecl BrQuatInvert(br_quat *q, br_quat *qq)

br_quat* BrQuatInvert(br_quat* q, br_quat* qq) {

    LOG_TRACE("(%p, %p)", q, qq);

    q->x = -qq->x;

    q->y = -qq->y;

    q->z = -qq->z;

    q->w = qq->w;

    return q;

}

// IDA: br_quat* __cdecl BrQuatSlerp(br_quat *q, br_quat *l, br_quat *r, br_scalar a, br_int_16 spins)

br_quat* BrQuatSlerp(br_quat* q, br_quat* l, br_quat* r, br_scalar a, br_int_16 spins) {

    int omega;

    int omega_spin;

    br_scalar s_omega;

    br_scalar c_omega;

    br_scalar scale_l;

    br_scalar scale_r;

    br_quat t;

    LOG_TRACE("(%p, %p, %p, %f, %d)", q, l, r, a, spins);

    // Animating rotation with quaternion curves, Ken Shoemake,SIGGRAPH Computer Graphics, 19(3):245–254, 1985. doi:10.1145/325165.325242.

    c_omega = l->x * r->x + l->y * r->y + l->z * r->z + l->w * r->w;

    if (c_omega >= 0.f) {

        t.x = r->x;

        t.y = r->y;

        t.z = r->z;

        t.w = r->w;

    } else {

        t.x = -r->x;

        t.y = -r->y;

        t.z = -r->z;

        t.w = -r->w;

        c_omega = -c_omega;

    }

    if ((1.f - c_omega) <= BR_SCALAR_EPSILON) {

        scale_l = 1.f - a;

        scale_r = a;

    } else {

        omega = BrRadianToAngle(acosf(c_omega));

        s_omega = sinf(BrAngleToRadian(omega));

        omega_spin = omega + spins * BR_ANGLE_RAD(PI);

        scale_l = sinf(BrAngleToRadian(omega - a * omega_spin)) / s_omega;

        scale_r = sinf(BrAngleToRadian(a * omega_spin)) / s_omega;

    }

    q->x = scale_l * l->x + scale_r * t.x;

    q->y = scale_l * l->y + scale_r * t.y;

    q->z = scale_l * l->z + scale_r * t.z;

    q->w = scale_l * l->w + scale_r * t.w;

    return q;

}

// IDA: br_matrix34* __cdecl BrQuatToMatrix34(br_matrix34 *mat, br_quat *q)

br_matrix34* BrQuatToMatrix34(br_matrix34* mat, br_quat* q) {

    br_scalar xs;

    br_scalar ys;

    br_scalar zs;

    br_scalar wx;

    br_scalar wy;

    br_scalar wz;

    br_scalar xx;

    br_scalar xy;

    br_scalar xz;

    br_scalar yy;

    br_scalar yz;

    br_scalar zz;

    LOG_TRACE("(%p, %p)", mat, q);

    zz = q->z + q->z;

    yy = q->y + q->y;

    zs = q->z * zz;

    xx = q->x + q->x;

    wx = q->w * xx;

    wy = q->w * yy;

    wz = q->w * zz;

    xy = q->x * yy;

    xz = q->x * zz;

    ys = q->y * yy;

    xs = q->x * xx;

    yz = q->y * zz;

    M(0, 0) = 1.f - (ys + zs);

    M(1, 0) = xy - wz;

    M(2, 0) = xz + wy;

    M(0, 1) = xy + wz;

    M(1, 1) = 1.f - (zs + xs);

    M(2, 1) = yz - wx;

    M(0, 2) = xz - wy;

    M(1, 2) = yz + wx;

    M(2, 2) = 1.f - (xs + ys);

    M(3, 0) = 0.f;

    M(3, 1) = 0.f;

    M(3, 2) = 0.f;

    return mat;

}

// IDA: br_quat* __cdecl BrMatrix34ToQuat(br_quat *q, br_matrix34 *mat)

br_quat* BrMatrix34ToQuat(br_quat* q, br_matrix34* mat) {

    br_scalar tr;

    br_scalar s;

    int i;

    int j;

    int k;

    LOG_TRACE("(%p, %p)", q, mat);

    tr = M(0, 0) + M(1, 1) + M(2, 2);

    if (tr >= 0.f) {

        tr = sqrtf(tr + 1.f);

        s = 0.5f / tr;

        q->w = tr * 0.5f;

        q->x = (M(1, 2) - M(2, 1)) * s;

        q->y = (M(2, 0) - M(0, 2)) * s;

        q->z = (M(0, 1) - M(1, 0)) * s;

        return q;

    }

    i = M_DIAG(1) >= M_DIAG(0) ? 1 : 0;

    i = M_DIAG(2) >= M_DIAG(i) ? 2 : i;

    j = (i + 1) % 3;

    k = (j + 1) % 3;

    tr = sqrtf(M_DIAG(i) - (M_DIAG(j) + M_DIAG(k)) + 1.f);

    s = 0.5f / tr;

    Q_EL(i) = 0.5f * tr;

    Q_EL(j) = (M(i, j) + M(j, i)) * s;

    Q_EL(k) = (M(i, k) + M(k, i)) * s;

    q->w = (M(k, j) - M(j, k)) * s;

    return q;

}

// IDA: br_matrix4* __cdecl BrQuatToMatrix4(br_matrix4 *mat, br_quat *q)

br_matrix4* BrQuatToMatrix4(br_matrix4* mat, br_quat* q) {

    br_matrix34 tmp;

    LOG_TRACE("(%p, %p)", mat, q);

    BrQuatToMatrix34(&tmp, q);

    BrMatrix4Copy34(mat, &tmp);

    return mat;

}

// IDA: br_quat* __cdecl BrMatrix4ToQuat(br_quat *q, br_matrix4 *mat)

br_quat* BrMatrix4ToQuat(br_quat* q, br_matrix4* mat) {

    br_matrix34 tmp;

    LOG_TRACE("(%p, %p)", q, mat);

    BrMatrix34Copy4(&tmp, mat);

    return BrMatrix34ToQuat(q, &tmp);

}