// ---------------------------- VSALGEB.C -----------------------
// VSpace library
// Written by Javier Ar‚valo Baeza.
// Types and functions for the graphics algebra.

#if 0
#include "vsalgeb.h"
#include "vsisqrt.h"

// -------------------------------------------
// Useful constants.

    // [0,0,0].
extern const VSALG_T3DPoint VSALG_ZeroVector = {
    {0, 0, 0}
};

    /* [1,0,0
        0,1,0
        0,0,1] */
extern const VSALG_T3DMatrix VSALG_UnitMatrix = {
    {{1, 0, 0},
     {0, 1, 0},
     {0, 0, 1}}
};

// -------------------------------------------

extern VSALG_TCoord VSALG_Length2D(VSALG_P2DPoint v1) {
    return VSISQ_Sqrt(VSALG_Dot2D(v1, v1));
}

extern VSALG_TCoord VSALG_Length3D(VSALG_P3DPoint v1) {
    return VSISQ_Sqrt(VSALG_Dot3D(v1, v1));
}

    // Make a vector of length 1. Returns dest.
extern VSALG_P2DPoint VSALG_Normalize2D(VSALG_P2DPoint dest, VSALG_P2DPoint v1) {
    VSALG_TCoord len;

    len = VSISQ_ISqrt(VSALG_Dot2D(v1, v1));
/*
    if (len == 0) {
        if (dest != v1)
            VSALG_Copy2DVector(dest, v1);
    } else {
*/
        dest->x = v1->x*len;
        dest->y = v1->y*len;
//    }
    return dest;
}

extern VSALG_P3DPoint VSALG_Normalize3D(VSALG_P3DPoint dest, VSALG_P3DPoint v1) {
    VSALG_TCoord len;

    len = VSISQ_ISqrt(VSALG_Dot3D(v1, v1));
    dest->x = v1->x*len;
    dest->y = v1->y*len;
    dest->z = v1->z*len;
    return dest;
}

    // Distance between points
extern VSALG_TCoord VSALG_Distance2D(VSALG_P2DPoint v0, VSALG_P2DPoint v1) {
    VSALG_T2DPoint p;

    VSALG_Sub2D(&p, v0, v1);
    return VSALG_Length2D(&p);
}

extern VSALG_TCoord VSALG_Distance3D(VSALG_P3DPoint v0, VSALG_P3DPoint v1) {
    VSALG_T3DPoint p;

    VSALG_Sub3D(&p, v0, v1);
    return VSALG_Length3D(&p);
}

    // Cross product. Returns dest.
extern VSALG_P3DPoint VSALG_Cross3D(VSALG_P3DPoint dest, VSALG_P3DPoint v1, VSALG_P3DPoint v2) {
    VSALG_T3DPoint v;

    if (dest == v1) {
        VSALG_Copy3DVector(&v, v1);
        v1 = &v;
    }
    if (dest == v2) {
        VSALG_Copy3DVector(&v, v2);
        v2 = &v;
    }
    dest->x = v1->y * v2->z - v1->z * v2->y;
    dest->y = v1->z * v2->x - v1->x * v2->z;
    dest->z = v1->x * v2->y - v1->y * v2->x;
    return dest;
}

    // Transpose matrix. Remember, for orthogonal matrices, this is also
    // the inverse.
extern VSALG_P3DMatrix VSALG_Transpose(VSALG_P3DMatrix dest, VSALG_P3DMatrix m1) {
    if (dest == m1) {
        VSALG_TCoord t;
#define SWAPF(a,b) ((t=(a)),((a)=(b)),((b)=t))
        SWAPF(m1->m[0][1], m1->m[1][0]);
        SWAPF(m1->m[0][2], m1->m[2][0]);
        SWAPF(m1->m[1][2], m1->m[2][1]);
    } else {
        dest->m[0][0] = m1->m[0][0];
        dest->m[1][1] = m1->m[1][1];
        dest->m[2][2] = m1->m[2][2];
        dest->m[0][1] = m1->m[1][0];
        dest->m[0][2] = m1->m[2][0];
        dest->m[1][2] = m1->m[2][1];
        dest->m[1][0] = m1->m[0][1];
        dest->m[2][0] = m1->m[0][2];
        dest->m[2][1] = m1->m[1][2];
    }
    return dest;
}


    // Compose a rotation matrix 'm1' by the left with 'm2' into 'dest'.
    // Returns dest. This is a plain matrix multiplication.
extern VSALG_P3DMatrix VSALG_Compose(VSALG_P3DMatrix dest, VSALG_P3DMatrix m1, VSALG_P3DMatrix m2) {
    int i;
    VSALG_T3DMatrix m;

    if (dest == m1) {
        VSALG_Copy3DMatrix(&m, m1);
        m1 = &m;
    }
    if (dest == m2) {
        VSALG_Copy3DMatrix(&m, m2);
        m2 = &m;
    }
        // Should all three matrices be the same, it still works.
    for (i = 0; i < 3; i++) {
/*
        int j;
        for (j = 0; j < 3; j++)
            dest->m[i][j] = m1->m[i][0] * m2->m[0][j]
                          + m1->m[i][1] * m2->m[1][j]
                          + m1->m[i][2] * m2->m[2][j];
*/
        dest->m[i][0] = m1->m[i][0] * m2->m[0][0]
                      + m1->m[i][1] * m2->m[1][0]
                      + m1->m[i][2] * m2->m[2][0];
        dest->m[i][1] = m1->m[i][0] * m2->m[0][1]
                      + m1->m[i][1] * m2->m[1][1]
                      + m1->m[i][2] * m2->m[2][1];
        dest->m[i][2] = m1->m[i][0] * m2->m[0][2]
                      + m1->m[i][1] * m2->m[1][2]
                      + m1->m[i][2] * m2->m[2][2];
    }
    return dest;
}


    // Compose the inverse of a rotation matrix 'm1' by the left with 'm2' into
    // 'dest'. Returns dest. This is a plain matrix multiplication.
extern VSALG_P3DMatrix VSALG_PreComposeInv(VSALG_P3DMatrix dest, VSALG_P3DMatrix m1, VSALG_P3DMatrix m2) {
    int i;
    VSALG_T3DMatrix m;

    if (dest == m1) {
        VSALG_Copy3DMatrix(&m, m1);
        m1 = &m;
    }
    if (dest == m2) {
        VSALG_Copy3DMatrix(&m, m2);
        m2 = &m;
    }
        // Should all three matrices be the same, it still works.
    for (i = 0; i < 3; i++) {
        dest->m[i][0] = m1->m[0][i] * m2->m[0][0]
                      + m1->m[1][i] * m2->m[1][0]
                      + m1->m[2][i] * m2->m[2][0];
        dest->m[i][1] = m1->m[0][i] * m2->m[0][1]
                      + m1->m[1][i] * m2->m[1][1]
                      + m1->m[2][i] * m2->m[2][1];
        dest->m[i][2] = m1->m[0][i] * m2->m[0][2]
                      + m1->m[1][i] * m2->m[1][2]
                      + m1->m[2][i] * m2->m[2][2];
    }
    return dest;
}

    // Compose a rotation matrix 'm1' by the left with the inverse of 'm2' into
    // 'dest'. Returns dest. This is a plain matrix multiplication.
extern VSALG_P3DMatrix VSALG_PostComposeInv(VSALG_P3DMatrix dest, VSALG_P3DMatrix m1, VSALG_P3DMatrix m2) {
    int i;
    VSALG_T3DMatrix m;

    if (dest == m1) {
        VSALG_Copy3DMatrix(&m, m1);
        m1 = &m;
    }
    if (dest == m2) {
        VSALG_Copy3DMatrix(&m, m2);
        m2 = &m;
    }
        // Should all three matrices be the same, it still works.
    for (i = 0; i < 3; i++) {
        dest->m[i][0] = m1->m[0][i] * m2->m[0][0]
                      + m1->m[1][i] * m2->m[0][1]
                      + m1->m[2][i] * m2->m[0][2];
        dest->m[i][1] = m1->m[0][i] * m2->m[1][0]
                      + m1->m[1][i] * m2->m[1][1]
                      + m1->m[2][i] * m2->m[1][2];
        dest->m[i][2] = m1->m[0][i] * m2->m[2][0]
                      + m1->m[1][i] * m2->m[2][1]
                      + m1->m[2][i] * m2->m[2][2];
    }
    return dest;
}


    // Build a rotation matrix around the X axis.
extern VSALG_P3DMatrix VSALG_CreateXRotation(VSALG_P3DMatrix m, VSALG_TAngle angle) {
    VSALG_TCoord ca;
    VSALG_TCoord sa;

    ca = cos(angle);
    sa = sin(angle);
    m->m[0][0] =   1; m->m[0][1] =   0; m->m[0][2] =   0;
    m->m[1][0] =   0; m->m[1][1] =  ca; m->m[1][2] = -sa;
    m->m[2][0] =   0; m->m[2][1] =  sa; m->m[2][2] =  ca;
    return m;
}

    // Build a rotation matrix around the Y axis.
extern VSALG_P3DMatrix VSALG_CreateYRotation(VSALG_P3DMatrix m, VSALG_TAngle angle) {
    VSALG_TCoord ca;
    VSALG_TCoord sa;

    ca = cos(angle);
    sa = sin(angle);
    m->m[0][0] =  ca; m->m[0][1] =   0; m->m[0][2] =  sa;
    m->m[1][0] =   0; m->m[1][1] =   1; m->m[1][2] =   0;
    m->m[2][0] = -sa; m->m[2][1] =   0; m->m[2][2] =  ca;
    return m;
}

    // Build a rotation matrix around the Z axis.
extern VSALG_P3DMatrix VSALG_CreateZRotation(VSALG_P3DMatrix m, VSALG_TAngle angle) {
    VSALG_TCoord ca;
    VSALG_TCoord sa;

    ca = cos(angle);
    sa = sin(angle);
    m->m[0][0] =  ca; m->m[0][1] = -sa; m->m[0][2] =   0;
    m->m[1][0] =  sa; m->m[1][1] =  ca; m->m[1][2] =   0;
    m->m[2][0] =   0; m->m[2][1] =   0; m->m[2][2] =   1;
    return m;
}

    // Build a rotation matrix around an arbitrary axis defined by a unit vector.
extern VSALG_P3DMatrix VSALG_CreateAxisRotation(VSALG_P3DMatrix m, VSALG_P3DPoint v, VSALG_TAngle angle) {
    VSALG_TCoord c, s, t, x, y, z;

    c = cos(angle);
    s = sin(angle);
    t = 1 - c;
    x = v->x;
    y = v->y;
    z = v->z;
    m->m[0][0] =  t*x*x + c;
      m->m[0][1] = t*y*x - s*z;
        m->m[0][2] = t*z*x + s*y;
    m->m[1][0] = t*x*y + s*z;
      m->m[1][1] = t*y*y + c;
        m->m[1][2] = t*z*y - s*x;
    m->m[2][0] = t*x*z - s*y;
      m->m[2][1] = t*y*z + s*x;
        m->m[2][2] = t*z*z + c;
    return m;
}

    // Compose a rotation matrix around the X axis by the left into m.
extern VSALG_P3DMatrix VSALG_PreComposeXRotation(VSALG_P3DMatrix dest, VSALG_P3DMatrix m, VSALG_TAngle angle) {
    if (angle != 0) {
        VSALG_T3DMatrix m1;
        VSALG_CreateXRotation(&m1, angle);
        VSALG_Compose(dest, &m1, m);
    } else {
        VSALG_Copy3DMatrix(dest, m);
    }
    return dest;
}

    // Compose a rotation matrix around the Y axis by the left into m.
extern VSALG_P3DMatrix VSALG_PreComposeYRotation(VSALG_P3DMatrix dest, VSALG_P3DMatrix m, VSALG_TAngle angle) {
    if (angle != 0) {
        VSALG_T3DMatrix m1;
        VSALG_CreateYRotation(&m1, angle);
        VSALG_Compose(dest, &m1, m);
    } else {
        VSALG_Copy3DMatrix(dest, m);
    }
    return dest;
}

    // Compose a rotation matrix around the Z axis by the left into m.
extern VSALG_P3DMatrix VSALG_PreComposeZRotation(VSALG_P3DMatrix dest, VSALG_P3DMatrix m, VSALG_TAngle angle) {
    if (angle != 0) {
        VSALG_T3DMatrix m1;
        VSALG_CreateZRotation(&m1, angle);
        VSALG_Compose(dest, &m1, m);
    } else {
        VSALG_Copy3DMatrix(dest, m);
    }
    return dest;
}

    // Post Compose a rotation matrix around the X axis by the right into m.
extern VSALG_P3DMatrix VSALG_PostComposeXRotation(VSALG_P3DMatrix dest, VSALG_P3DMatrix m, VSALG_TAngle angle) {
    if (angle != 0) {
        VSALG_T3DMatrix m1;
        VSALG_CreateXRotation(&m1, angle);
        VSALG_Compose(dest, m, &m1);
    } else {
        VSALG_Copy3DMatrix(dest, m);
    }
    return dest;
}

    // Post Compose a rotation matrix around the Y axis by the right into m.
extern VSALG_P3DMatrix VSALG_PostComposeYRotation(VSALG_P3DMatrix dest, VSALG_P3DMatrix m, VSALG_TAngle angle) {
    if (angle != 0) {
        VSALG_T3DMatrix m1;
        VSALG_CreateYRotation(&m1, angle);
        VSALG_Compose(dest, m, &m1);
    } else {
        VSALG_Copy3DMatrix(dest, m);
    }
    return dest;
}

    // Post Compose a rotation matrix around the Z axis by the right into m.
extern VSALG_P3DMatrix VSALG_PostComposeZRotation(VSALG_P3DMatrix dest, VSALG_P3DMatrix m, VSALG_TAngle angle) {
    if (angle != 0) {
        VSALG_T3DMatrix m1;
        VSALG_CreateZRotation(&m1, angle);
        VSALG_Compose(dest, m, &m1);
    } else {
        VSALG_Copy3DMatrix(dest, m);
    }
    return dest;
}

    // Compose a rotation matrix around the axis by the left into m.
extern VSALG_P3DMatrix VSALG_PreComposeAxisRotation(VSALG_P3DMatrix dest, VSALG_P3DMatrix m, VSALG_P3DPoint v, VSALG_TAngle angle) {
    if (angle != 0) {
        VSALG_T3DMatrix m1;
        VSALG_CreateAxisRotation(&m1, v, angle);
        VSALG_Compose(dest, &m1, m);
    } else {
        VSALG_Copy3DMatrix(dest, m);
    }
    return dest;
}

    // Post Compose a rotation matrix around the axis by the left into m.
extern VSALG_P3DMatrix VSALG_PostComposeAxisRotation(VSALG_P3DMatrix dest, VSALG_P3DMatrix m, VSALG_P3DPoint v, VSALG_TAngle angle) {
    if (angle != 0) {
        VSALG_T3DMatrix m1;
        VSALG_CreateAxisRotation(&m1, v, angle);
        VSALG_Compose(dest, m, &m1);
    } else {
        VSALG_Copy3DMatrix(dest, m);
    }
    return dest;
}

    // Advance a vector by some other vector rotated by a matrix.
    // dest = v + d*M
extern VSALG_P3DPoint VSALG_AdvanceRotated(VSALG_P3DPoint dest, VSALG_P3DPoint v, VSALG_P3DPoint d, VSALG_P3DMatrix m) {
    VSALG_T3DPoint p;

    VSALG_Rotate(&p, d, m);
    VSALG_Add3D(dest, v, &p);
    return dest;
}

extern VSALG_P3DMatrix VSALG_LookAt(VSALG_P3DMatrix m, VSALG_P3DPoint from, VSALG_P3DPoint to) {
    VSALG_T3DPoint dir;
    VSALG_TCoord len;

    VSALG_Sub3D(&dir, to, from);
//    VSALG_Normalize3D(&dir, &dir);

    len = sqrt(dir.x*dir.x + dir.z*dir.z);

    VSALG_CreateYRotation(m, HALFPI-atan2(dir.z, dir.x));
    VSALG_PostComposeXRotation(m, m, -atan2(dir.y, len));
    return m;
}

    // --------------------------------------

    // Gram-Schmidt orthonormalization of a 3D matrix.
extern VSALG_P3DMatrix VSALG_Ortho3DMatrix(VSALG_P3DMatrix dest, VSALG_P3DMatrix m) {
    VSALG_P3DPoint v1, v2, v3, w1, w2, w3;
    VSALG_TCoord d;

    v1 = &m->v[0];
    v2 = &m->v[1];
    v3 = &m->v[2];
    w1 = &dest->v[0];
    w2 = &dest->v[1];
    w3 = &dest->v[2];

    VSALG_Normalize3D(w1, v1);
    d = VSALG_Dot3D(v2, w1);
    w2->x = v2->x - d*w1->x;
    w2->y = v2->y - d*w1->y;
    w2->z = v2->z - d*w1->y;
    VSALG_Normalize3D(w2, w2);
    VSALG_Cross3D(w3, w1, w2);
    return dest;
}

    // Arvo's (Gems1) method for finding a transformed bounding box.
    // Box is (min,max), not (center,radius). v' = Mv + t
extern VSALG_P3DBox VSALG_RotateAddBounds(VSALG_P3DBox dest, VSALG_P3DBox box, VSALG_P3DMatrix m, VSALG_P3DPoint t) {
    int i, j;

    dest->v0.x = dest->v1.x = t->x;
    dest->v0.y = dest->v1.y = t->y;
    dest->v0.z = dest->v1.z = t->z;
    for (i = 0; i < 3; i++) {
        for (j = 0; j < 3; j++) {
            VSALG_TCoord a, b;
            a = m->m[i][j]*box->v0.m[j];
            b = m->m[i][j]*box->v1.m[j];
            if (a < b) {
                dest->v0.m[i] += a;
                dest->v1.m[i] += b;
            } else {    // a > b
                dest->v0.m[i] += b;
                dest->v1.m[i] += a;
            }
        }
    }
    return dest;
}

    // --------------------------
    // Quaternion stuff (maybe we'll find a use for it).
    // Based on code by Gavin Bell & Doug Moore.

    // Generate a quaternion for a given axis & angle around it.
    // Remember to give the axis normalized!
extern VSALG_PQuaternion VSALG_Axis2Quat(VSALG_PQuaternion dest, VSALG_TAngle s, VSALG_P3DPoint v) {
    float ssin;

    ssin = sin(s/2);
    VSALG_Scale3D(&dest->v, v, ssin);
    dest->s = cos(s/2);
    return dest;
}

    // Normalize a quaternion.
extern VSALG_PQuaternion VSALG_NormalizeQuat(VSALG_PQuaternion dest, VSALG_PQuaternion q) {
/*
        // Weird way by Mr. Bell.
    int which, i;
    float gr;

    VSALG_CopyQuat(dest, q);
        // Choose maximum value to minimise roundoff error near 0.
    which = 0;
    gr = q->m[which];
    for (i = 1; i < 4; i++)
        if (fabs(q->m[i]) > fabs(gr)) {
            which = i;
            gr = q->m[i];
        }
        // Clear this so it doesn't affect next operation.
    q->m[which] = 0;
    dest->m[which] = sqrt(1.0 - (VSALG_Pow2(q->s) + VSALG_Dot3D(&q->v, &q->v)));
        // Was the original value negative?
    if (gr < 0)
        dest->m[which] = -dest->m[which];
*/
        // Usual "divide" way to do it.
    float l;

    l = VSISQ_ISqrt(VSALG_Pow2(q->s) + VSALG_Dot3D(&q->v, &q->v));
    dest->s = q->s*l;
    VSALG_Scale3D(&dest->v, &q->v, l);
    return dest;
}

    // Add a quaternion to another.
extern VSALG_PQuaternion VSALG_AddQuat(VSALG_PQuaternion dest, VSALG_PQuaternion q1, VSALG_PQuaternion q2) {
    VSALG_T3DPoint t1, t2, t3;
    VSALG_TQuaternion tf;

    if (q1 == dest) {
        VSALG_CopyQuat(&tf, q1);
        q1 = &tf;
    }
    if (q2 == dest) {
        VSALG_CopyQuat(&tf, q2);
        q2 = &tf;
    }

        // < s1*s2 - v1.v2 , s1*v2 + s2*v1 + v1xv2 >
    VSALG_Scale3D(&t1, &q1->v, q2->s);
    VSALG_Scale3D(&t2, &q2->v, q1->s);
    VSALG_Cross3D(&t3, &q1->v, &q2->v);     // ?? q2 x q1
    VSALG_Add3D(&dest->v, &t1, &t2);
    VSALG_Add3D(&dest->v, &tf.v, &t3);
    dest->s = q1->s*q2->s - VSALG_Dot3D(&q1->v, &q2->v);
    return dest;
}

    // Build a rotation matrix for a unit quaternion.
extern VSALG_P3DMatrix VSALG_Quat2Matrix(VSALG_P3DMatrix dest, VSALG_PQuaternion q) {
    VSALG_TCoord xx, xy, xz, yy, yz, zz, sx, sy, sz;
    xx = q->v.x*q->v.x;
    xy = q->v.x*q->v.y;
    xz = q->v.x*q->v.z;
    yy = q->v.y*q->v.y;
    yz = q->v.y*q->v.z;
    zz = q->v.z*q->v.z;
    sx = q->s*q->v.x;
    sy = q->s*q->v.y;
    sz = q->s*q->v.z;

    dest->m[0][0] = 1.0 - 2.0*(yy + zz);
    dest->m[0][1] = 2.0*(xy - sz);
    dest->m[0][2] = 2.0*(xz + sy);
    dest->m[1][0] = 2.0*(xy + sz);
    dest->m[1][1] = 1.0 - 2.0*(xx + zz);
    dest->m[1][2] = 2.0*(yz - sx);
    dest->m[2][0] = 2.0*(xz - sy);
    dest->m[2][1] = 2.0*(yz + sx);
    dest->m[2][2] = 1.0 - 2.0*(xx + yy);

    return dest;
}

    // Spherical interpolation of unit quaternions. 0 <= t <= 1
    // Adapted from code by Xanthome/DZ.
extern VSALG_PQuaternion VSALG_QuatSlerp(VSALG_PQuaternion dest,
                                         VSALG_PQuaternion q1,
                                         VSALG_PQuaternion q2,
                                         VSALG_TCoord t) {
    float omega, cosom, sinom, sclp, sclq;

    cosom = q1->s*q2->s + VSALG_Dot3D(&q1->v, &q2->v);

    if ((1.0 + cosom) > 1e-6) {
        if ((1.0 - cosom) > 1e-6) {
            omega = acos(cosom);
            sinom = 1.0/sin(omega);
            sclp  = sin((1.0-t)*omega)*sinom;
            sclq  = sin(t*omega)*sinom;
        } else {
            sclp = 1.0 - t;
            sclq = t;
        }
        dest->s = sclp*q1->s + sclq*q2->s;
        VSALG_Combine3D(&dest->v, &q1->v, &q2->v, sclp, sclq);
    } else {
//        qt[X] = -p[Y]; qt[Y] = p[X]; qt[Z] = -p[W]; ??
        dest->s = q1->v.z;
        sclp = sin((1.0-t)*HALFPI);
        sclq = sin(t*HALFPI);
        VSALG_Combine3D(&dest->v, &q1->v, &q2->v, sclp, sclq);
    }
    return dest;
}

// ---------------------------- VSALGEB.C -----------------------
#endif