/******************************************************************************
 *{@C
 *      Copyright:      2005-2010 Paul Obermeier (obermeier@tcl3d.org)
 *
 *                      See the file "Tcl3D_License.txt" for information on
 *                      usage and redistribution of this file, and for a
 *                      DISCLAIMER OF ALL WARRANTIES.
 *
 *      Module:         Tcl3D -> tcl3dOgl
 *      Filename:       tcl3dVecMath.c
 *
 *      Author:         Paul Obermeier
 *
 *      Description:    C functions for handling 3D vectors and transformation
 *                      matrices in OpenGL compatible style.
 *                      This module is intended to be wrapped with Swig for the
 *                      Tcl3D package. Vectors and matrices are represented
 *                      as standard Tcl lists.
 *
 *****************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <float.h>

#include "tcl3dVecMath.h"

/*
    MIN_NRM_FLOAT  : The smallest normalized single precision value greater than zero
    MIN_NRM_DOUBLE : The smallest normalized double precision value greater than zero

    The assertion for normalized numbers is:
    1.0f + MIN_NRM_FLOAT  != 1.0f
    1.0  + MIN_NRM_DOUBLE != 1.0
*/

#define MIN_NRM_FLOAT  (FLT_MIN * (1.0f / FLT_EPSILON))
#define MIN_NRM_DOUBLE (DBL_MIN * (1.0  / DBL_EPSILON))

#define DEGTORAD(degree) ((degree) * (3.1415926535897932384626433832795 / 180.0))
#define RADTODEG(radian) ((radian) * (180.0 / 3.1415926535897932384626433832795))
#define ABS(a) ((a)>0?(a):(-(a)))

#define X 0
#define Y 1
#define Z 2

#define VEC_COMPARE \
    return ABS(v2[0] - v1[0]) < delta && \
           ABS(v2[1] - v1[1]) < delta && \
           ABS(v2[2] - v1[2]) < delta;

/**
  * Check, if two 3D vectors in single precision are equal.
  * Each vector component of v1 must not differ by more than
  * delta units from the corresponding component of v2.
  */
int tcl3dVec3fCompare (float *v1, float *v2, float delta)
{
    VEC_COMPARE
}

/**
  * Check, if two 3D vectors in double precision are equal.
  * Each vector component of v1 must not differ by more than
  * delta units from the corresponding component of v2.
  */
int tcl3dVec3dCompare (double *v1, double *v2, double delta)
{
    VEC_COMPARE
}

#define VEC_IDENTITY \
    res[0] = 0.0; \
    res[1] = 0.0; \
    res[2] = 0.0;

/**
  * Initialize a single precision 3D vector with zeros.
  */
void tcl3dVec3fIdentity (float *res)
{
    VEC_IDENTITY
}

/**
  * Initialize a double precision 3D vector with zeros.
  */
void tcl3dVec3dIdentity (double *res)
{
    VEC_IDENTITY
}

#define VEC_COPY \
    res[0] = v[0]; \
    res[1] = v[1]; \
    res[2] = v[2];

/**
  * Copy a single precision 3D vector to a single precision 3D vector: res = v.
  */
void tcl3dVec3fCopy (float *v, float *res)
{
    VEC_COPY
}

/**
  * Copy a double precision 3D vector to a double precision 3D vector: res = v.
  */
void tcl3dVec3dCopy (double *v, double *res)
{
    VEC_COPY
}

/**
  * Copy a single precision 3D vector to a double precision 3D vector: res = v.
  */
void tcl3dVec3f2Vec3d (float *v, double *res)
{
    VEC_COPY
}

/**
  * Copy a double precision 3D vector to a single precision 3D vector: res = v.
  */
void tcl3dVec3d2Vec3f (double *v, float *res)
{
    VEC_COPY
}

#define VEC_LENGTH \
    return sqrt (v[X]*v[X] + v[Y]*v[Y] + v[Z]*v[Z]);

/**
  * Return the length of a single precision 3D vector.
  */
float tcl3dVec3fLength (float *v)
{
    VEC_LENGTH
}

/**
  * Return the length of a double precision 3D vector.
  */
double tcl3dVec3dLength (double *v)
{
    VEC_LENGTH
}

/**
  * Scale a single precision 3D vector so that its length becomes 1.0:
  *   v = v / max (MIN_NRM_FLOAT, length(v))
  * Return true (1), if length(v) was greater than MIN_NRM_FLOAT
  * before v was scaled, otherwise false (0).
  */
int tcl3dVec3fNormalize (float *v)
{
    float t;

    if ((t = sqrt (v[0] * v[0] + v[1] * v[1] + v[2] * v[2])) < MIN_NRM_FLOAT) {
        v[0] /= MIN_NRM_FLOAT;
        v[1] /= MIN_NRM_FLOAT;
        v[2] /= MIN_NRM_FLOAT;
        return 0;
    }
    v[0] /= t;
    v[1] /= t;
    v[2] /= t;
    return 1;
}

/**
  * Scale a double precision 3D vector so that its length becomes 1.0:
  *   v = v / max (MIN_NRM_DOUBLE, length(v))
  * Return true (1), if length(v) was greater than MIN_NRM_DOUBLE
  * before v was scaled, otherwise false (0).
  */
int tcl3dVec3dNormalize (double *v)
{
    double t;

    if ((t = sqrt (v[0] * v[0] + v[1] * v[1] + v[2] * v[2])) < MIN_NRM_DOUBLE) {
        v[0] /= MIN_NRM_DOUBLE;
        v[1] /= MIN_NRM_DOUBLE;
        v[2] /= MIN_NRM_DOUBLE;
        return 0;
    }
    v[0] /= t;
    v[1] /= t;
    v[2] /= t;
    return 1;
}

#define VEC_DISTANCE(TYPE) \
    TYPE t0, t1, t2;    \
    t0 = v1[0] - v2[0]; \
    t1 = v1[1] - v2[1]; \
    t2 = v1[2] - v2[2]; \
    return sqrt (t0 * t0 + t1 * t1 + t2 * t2);

/**
  * Return the distance between two single precision 3D points.
  */
float tcl3dVec3fDistance (float *v1,  float *v2)
{
    VEC_DISTANCE(float)
}

/**
  * Return the distance between two double precision 3D points.
  */
double tcl3dVec3dDistance (double *v1,  double *v2)
{
    VEC_DISTANCE(double)
}

#define VEC_DOTPRODUCT \
    return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];

/**
  * Return the dot product of two single precision 3D vectors.
  */
float tcl3dVec3fDotProduct (float *v1,  float *v2)
{
    VEC_DOTPRODUCT
}

/**
  * Return the dot product of two double precision 3D vectors.
  */
double tcl3dVec3dDotProduct (double *v1,  double *v2)
{
    VEC_DOTPRODUCT
}

#define VEC_CROSSPRODUCT \
    res[0] = v1[1] * v2[2] - v2[1] * v1[2]; \
    res[1] = v1[2] * v2[0] - v2[2] * v1[0]; \
    res[2] = v1[0] * v2[1] - v2[0] * v1[1]; \

/**
  * Calculate the cross product of two single precision 3D vectors: res = v1 x v2.
  * "res" must not refer to the same memory location as "v1" or "v2".
  */
void tcl3dVec3fCrossProduct (float *v1, float *v2, float *res)
{
    VEC_CROSSPRODUCT
}

/**
  * Calculate the cross product of two double precision 3D vectors: res = v1 x v2.
  * "res" must not refer to the same memory location as "v1" or "v2".
  */
void tcl3dVec3dCrossProduct (double *v1, double *v2, double *res)
{
    VEC_CROSSPRODUCT
}

#define VEC_ADD \
    res[0] = v1[0] + v2[0]; \
    res[1] = v1[1] + v2[1]; \
    res[2] = v1[2] + v2[2];

/**
  * Add two single precision 3D vectors: res = v1 + v2.
  */
void tcl3dVec3fAdd (float *v1, float *v2, float *res)
{
    VEC_ADD
}

/**
  * Add two double precision 3D vectors: res = v1 + v2.
  */
void tcl3dVec3dAdd (double *v1, double *v2, double *res)
{
    VEC_ADD
}

#define VEC_SUB \
    res[0] = v1[0] - v2[0]; \
    res[1] = v1[1] - v2[1]; \
    res[2] = v1[2] - v2[2];

/**
  * Subtract two single precision 3D vectors: res = v1 - v2.
  */
void tcl3dVec3fSubtract (float *v1, float *v2, float *res)
{
    VEC_SUB
}

/**
  * Subtract two double precision 3D vectors: res = v1 - v2.
  */
void tcl3dVec3dSubtract (double *v1, double *v2, double *res)
{
    VEC_SUB
}

#define VEC_SCALE \
    res[0] = v[0] * scalar; \
    res[1] = v[1] * scalar; \
    res[2] = v[2] * scalar;

/**
  * Multiply a single precision 3D vector with a scalar: res = v * scalar.
  */
void tcl3dVec3fScale (float scalar, float *v, float *res)
{
    VEC_SCALE
}

/**
  * Multiply a double precision 3D vector with a scalar: res = v * scalar.
  */
void tcl3dVec3dScale (double scalar, double *v, double *res)
{
    VEC_SCALE
}

/**
  * Create the normal of the plane defined by three single precision 3D points.
  */
void tcl3dVec3fPlaneNormal (float *p1, float *p2, float *p3, float *res)
{
    float v1[3], v2[3];

    /* Calculate the vector from point 2 to point 1 */
    tcl3dVec3fSubtract (p1, p2, v1);

    /* Calculate the vector from point 3 to point 2 */
    tcl3dVec3fSubtract (p2, p3, v2);

    /* Compute the cross product to give us the surface normal */
    tcl3dVec3fCrossProduct (v1, v2, res);

    /* Normalize the surface normal vector */
    tcl3dVec3fNormalize (res);
}

/**
  * Create the normal of the plane defined by three double precision 3D points.
  */
void tcl3dVec3dPlaneNormal (double *p1, double *p2, double *p3, double *res)
{
    double v1[3], v2[3];

    /* Calculate the vector from point 2 to point 1 */
    tcl3dVec3dSubtract (p1, p2, v1);

    /* Calculate the vector from point 3 to point 2 */
    tcl3dVec3dSubtract (p2, p3, v2);

    /* Compute the cross product to give us the surface normal */
    tcl3dVec3dCrossProduct (v1, v2, res);

    /* Normalize the surface normal vector */
    tcl3dVec3dNormalize (res);
}

#define MAT_COMPARE \
    return ABS(m2[0]  - m1[0])  < delta && \
           ABS(m2[1]  - m1[1])  < delta && \
           ABS(m2[2]  - m1[2])  < delta && \
           ABS(m2[3]  - m1[3])  < delta && \
           ABS(m2[4]  - m1[4])  < delta && \
           ABS(m2[5]  - m1[5])  < delta && \
           ABS(m2[6]  - m1[6])  < delta && \
           ABS(m2[7]  - m1[7])  < delta && \
           ABS(m2[8]  - m1[8])  < delta && \
           ABS(m2[9]  - m1[9])  < delta && \
           ABS(m2[10] - m1[10]) < delta && \
           ABS(m2[11] - m1[11]) < delta && \
           ABS(m2[12] - m1[12]) < delta && \
           ABS(m2[13] - m1[13]) < delta && \
           ABS(m2[14] - m1[14]) < delta && \
           ABS(m2[15] - m1[15]) < delta;

/**
  * Check, if two single precision matrices are equal.
  * Each matrix component of m1 must not differ by more than
  * delta units from the corresponding component of m2.
  */
int tcl3dMatfCompare (float *m1, float *m2, float delta)
{
    MAT_COMPARE
}

/**
  * Check, if two double precision matrices are equal.
  * Each matrix component of m1 must not differ by more than
  * delta units from the corresponding component of m2.
  */
int tcl3dMatdCompare (double *m1, double *m2, double delta)
{
    MAT_COMPARE
}

#define MAT_IDENTITY \
    res[0]=1.0; res[4]=0.0; res[8] =0.0; res[12]=0.0; \
    res[1]=0.0; res[5]=1.0; res[9] =0.0; res[13]=0.0; \
    res[2]=0.0; res[6]=0.0; res[10]=1.0; res[14]=0.0; \
    res[3]=0.0; res[7]=0.0; res[11]=0.0; res[15]=1.0;

/**
  * Build an identity transformation matrix in single precision.
  */
void tcl3dMatfIdentity (float *res)
{
    MAT_IDENTITY
}

/**
  * Build an identity transformation matrix in double precision.
  */
void tcl3dMatdIdentity (double *res)
{
    MAT_IDENTITY
}

#define MAT_COPY(m,res) \
    res[0]  = m[0];  res[1]  = m[1];  res[2]  = m[2];  res[3]  = m[3];  \
    res[4]  = m[4];  res[5]  = m[5];  res[6]  = m[6];  res[7]  = m[7];  \
    res[8]  = m[8];  res[9]  = m[9];  res[10] = m[10]; res[11] = m[11]; \
    res[12] = m[12]; res[13] = m[13]; res[14] = m[14]; res[15] = m[15];

/**
  * Copy a single precision transformation matrix to a single precision matrix: res = m.
  */
void tcl3dMatfCopy (float *m, float *res)
{
    MAT_COPY(m,res)
}

/**
  * Copy a double precision transformation matrix to a double precision matrix: res = m.
  */
void tcl3dMatdCopy (double *m, double *res)
{
    MAT_COPY(m,res)
}

/**
  * Copy a single precision transformation matrix to a double precision matrix: res = m.
  */
void tcl3dMatf2Matd (float *m, double *res)
{
    MAT_COPY(m,res)
}

/**
  * Copy a double precision transformation matrix to a single precision matrix: res = m.
  */
void tcl3dMatd2Matf (double *m, float *res)
{
    MAT_COPY(m,res)
}

#define MAT_TRANSLATEV \
    MAT_IDENTITY    \
    res[12] = v[0]; \
    res[13] = v[1]; \
    res[14] = v[2];

/**
  *  Build a single precision translation matrix based on a single precision 3D vector.
  */
void tcl3dMatfTranslatev (float *v, float *res)
{
    MAT_TRANSLATEV
}

/**
  *  Build a double precision translation matrix based on a double precision 3D vector.
  */
void tcl3dMatdTranslatev (double *v, double *res)
{
    MAT_TRANSLATEV
}

#define MAT_TRANSLATE \
    MAT_IDENTITY  \
    res[12] = tx; \
    res[13] = ty; \
    res[14] = tz;

/**
  * Build a single precision translation matrix based on 3 scalar values.
  */
void tcl3dMatfTranslate (float tx, float ty, float tz, float *res)
{
    MAT_TRANSLATE
}

/**
  * Build a double precision translation matrix based on 3 scalar values.
  */
void tcl3dMatdTranslate (double tx, double ty, double tz, double *res)
{
    MAT_TRANSLATE
}

/**
  * Build a single precision rotation matrix based on angle and axis.
  * Angle is given in degrees.
  */
void tcl3dMatfRotate (float angle, float *axis, float *res)
{
    float s, c, v;
    float ux, uy, uz;

    s = sin(DEGTORAD(angle));
    c = cos(DEGTORAD(angle));
    v = 1-c;

    tcl3dVec3fNormalize (axis);

    ux = axis[0];
    uy = axis[1];
    uz = axis[2];
    
    res[0]  = ux*ux*v + c;
    res[1]  = ux*uy*v + uz*s;
    res[2]  = uz*ux*v - uy*s;
    res[3]  = 0.0f;
    
    res[4]  = ux*uy*v - uz*s;
    res[5]  = uy*uy*v + c;
    res[6]  = uz*uy*v + ux*s;
    res[7]  = 0.0f;
    
    res[8]  = ux*uz*v + uy*s;
    res[9]  = uz*uy*v - ux*s;
    res[10] = uz*uz*v + c;
    res[11] = 0.0f;
    
    res[12] = 0.0f;
    res[13] = 0.0f;
    res[14] = 0.0f;
    res[15] = 1.0f;
}

/**
  * Build a double precision rotation matrix based on angle and axis.
  * Angle is given in degrees.
  */
void tcl3dMatdRotate (double angle, double *axis, double *res)
{
    double s, c, v;
    double ux, uy, uz;

    s = sin(DEGTORAD(angle));
    c = cos(DEGTORAD(angle));
    v = 1-c;

    tcl3dVec3dNormalize (axis);

    ux = axis[0];
    uy = axis[1];
    uz = axis[2];
    
    res[0]  = ux*ux*v + c;
    res[1]  = ux*uy*v + uz*s;
    res[2]  = uz*ux*v - uy*s;
    res[3]  = 0.0;
    
    res[4]  = ux*uy*v - uz*s;
    res[5]  = uy*uy*v + c;
    res[6]  = uz*uy*v + ux*s;
    res[7]  = 0.0;
    
    res[8]  = ux*uz*v + uy*s;
    res[9]  = uz*uy*v - ux*s;
    res[10] = uz*uz*v + c;
    res[11] = 0.0;
    
    res[12] = 0.0;
    res[13] = 0.0;
    res[14] = 0.0;
    res[15] = 1.0;
}

/**
  * Build a single precision rotation matrix based on angle around x axis.
  * Angle is given in degrees.
  */
void tcl3dMatfRotateX (float angle, float *res)
{
    float s, c;

    s = sin(DEGTORAD(angle));
    c = cos(DEGTORAD(angle));
    tcl3dMatfIdentity (res);
    res[5]  =  c;
    res[6]  =  s;
    res[9]  = -s;
    res[10] =  c;
}

/**
  * Build a double precision rotation matrix based on angle around x axis.
  * Angle is given in degrees.
  */
void tcl3dMatdRotateX (double angle, double *res)
{
    double s, c;

    s = sin(DEGTORAD(angle));
    c = cos(DEGTORAD(angle));
    tcl3dMatdIdentity (res);
    res[5]  =  c;
    res[6]  =  s;
    res[9]  = -s;
    res[10] =  c;
}

/**
  * Build a single precision rotation matrix based on angle around y axis.
  * Angle is given in degrees.
  */
void tcl3dMatfRotateY (float angle, float *res)
{
    float s, c;

    s = sin(DEGTORAD(angle));
    c = cos(DEGTORAD(angle));
    tcl3dMatfIdentity (res);
    res[0]  =  c;
    res[2]  = -s;
    res[8]  =  s;
    res[10] =  c;
}

/**
  * Build a doubel precision rotation matrix based on angle around y axis.
  * Angle is given in degrees.
  */
void tcl3dMatdRotateY (double angle, double *res)
{
    double s, c;

    s = sin(DEGTORAD(angle));
    c = cos(DEGTORAD(angle));
    tcl3dMatdIdentity (res);
    res[0]  =  c;
    res[2]  = -s;
    res[8]  =  s;
    res[10] =  c;
}

/**
  * Build a single precision rotation matrix based on angle around z axis.
  * Angle is given in degrees.
  */
void tcl3dMatfRotateZ (float angle, float *res)
{
    float s, c;

    s = sin(DEGTORAD(angle));
    c = cos(DEGTORAD(angle));
    tcl3dMatfIdentity (res);
    res[0] =  c;
    res[1] =  s;
    res[4] = -s;
    res[5] =  c;
}

/**
  * Build a double precision rotation matrix based on angle around z axis.
  * Angle is given in degrees.
  */
void tcl3dMatdRotateZ (double angle, double *res)
{
    double s, c;

    s = sin(DEGTORAD(angle));
    c = cos(DEGTORAD(angle));
    tcl3dMatdIdentity (res);
    res[0] =  c;
    res[1] =  s;
    res[4] = -s;
    res[5] =  c;
}

#define MAT_SCALEV  \
    MAT_IDENTITY    \
    res[0]  = v[0]; \
    res[5]  = v[1]; \
    res[10] = v[2];

/**
  * Build a single precision scale matrix based on a single precision 3D vector.
  */
void tcl3dMatfScalev (float *v, float *res)
{
    MAT_SCALEV
}

/**
  * Build a double precision scale matrix based on a double precision 3D vector.
  */
void tcl3dMatdScalev (double *v, double *res)
{
    MAT_SCALEV
}

#define MAT_SCALE \
    MAT_IDENTITY  \
    res[0]  = sx; \
    res[5]  = sy; \
    res[10] = sz;

/**
  * Build a single precision scale matrix based on 3 scalar values.
  */
void tcl3dMatfScale (float sx, float sy, float sz, float *res)
{
    MAT_SCALE
}

/**
  * Build a double precision scale matrix based on 3 scalar values.
  */
void tcl3dMatdScale (double sx, double sy, double sz, double *res)
{
    MAT_SCALE
}

#define MAT_TFM(offx, offy, offz) \
    res[0] = v[0] * m[0] +        \
             v[1] * m[4] +        \
             v[2] * m[8] + offx;  \
    res[1] = v[0] * m[1] +        \
             v[1] * m[5] +        \
             v[2] * m[9] + offy;  \
    res[2] = v[0] * m[2] +        \
             v[1] * m[6] +        \
             v[2] * m[10]+ offz;

/**
  * Transform a single precision 3D point by a single precision matrix.
  */
void tcl3dMatfTransformPoint (float *v, float *m, float *res)
{
    MAT_TFM (m[12], m[13], m[14]);
}

/**
  * Transform a double precision 3D point by a double precision matrix.
  */
void tcl3dMatdTransformPoint (double *v, double *m, double *res)
{
    MAT_TFM (m[12], m[13], m[14]);
}

/**
  * Transform a single precision 3D vector by a single precision matrix.
  */
void tcl3dMatfTransformVector (float *v, float *m, float *res)
{
    MAT_TFM (0.0f, 0.0f, 0.0f);
}

/**
  * Transform a double precision 3D vector by a double precision matrix.
  */
void tcl3dMatdTransformVector (double *v, double *m, double *res)
{
    MAT_TFM (0.0, 0.0, 0.0);
}

#define MAT_MULT(TYPE) \
    TYPE tmp[16];   \
    tmp[0]  = (m1[0]*m2[ 0])+(m1[4]*m2[ 1])+(m1[ 8]*m2[ 2])+(m1[12]*m2[ 3]); \
    tmp[1]  = (m1[1]*m2[ 0])+(m1[5]*m2[ 1])+(m1[ 9]*m2[ 2])+(m1[13]*m2[ 3]); \
    tmp[2]  = (m1[2]*m2[ 0])+(m1[6]*m2[ 1])+(m1[10]*m2[ 2])+(m1[14]*m2[ 3]); \
    tmp[3]  = (m1[3]*m2[ 0])+(m1[7]*m2[ 1])+(m1[11]*m2[ 2])+(m1[15]*m2[ 3]); \
    tmp[4]  = (m1[0]*m2[ 4])+(m1[4]*m2[ 5])+(m1[ 8]*m2[ 6])+(m1[12]*m2[ 7]); \
    tmp[5]  = (m1[1]*m2[ 4])+(m1[5]*m2[ 5])+(m1[ 9]*m2[ 6])+(m1[13]*m2[ 7]); \
    tmp[6]  = (m1[2]*m2[ 4])+(m1[6]*m2[ 5])+(m1[10]*m2[ 6])+(m1[14]*m2[ 7]); \
    tmp[7]  = (m1[3]*m2[ 4])+(m1[7]*m2[ 5])+(m1[11]*m2[ 6])+(m1[15]*m2[ 7]); \
    tmp[8]  = (m1[0]*m2[ 8])+(m1[4]*m2[ 9])+(m1[ 8]*m2[10])+(m1[12]*m2[11]); \
    tmp[9]  = (m1[1]*m2[ 8])+(m1[5]*m2[ 9])+(m1[ 9]*m2[10])+(m1[13]*m2[11]); \
    tmp[10] = (m1[2]*m2[ 8])+(m1[6]*m2[ 9])+(m1[10]*m2[10])+(m1[14]*m2[11]); \
    tmp[11] = (m1[3]*m2[ 8])+(m1[7]*m2[ 9])+(m1[11]*m2[10])+(m1[15]*m2[11]); \
    tmp[12] = (m1[0]*m2[12])+(m1[4]*m2[13])+(m1[ 8]*m2[14])+(m1[12]*m2[15]); \
    tmp[13] = (m1[1]*m2[12])+(m1[5]*m2[13])+(m1[ 9]*m2[14])+(m1[13]*m2[15]); \
    tmp[14] = (m1[2]*m2[12])+(m1[6]*m2[13])+(m1[10]*m2[14])+(m1[14]*m2[15]); \
    tmp[15] = (m1[3]*m2[12])+(m1[7]*m2[13])+(m1[11]*m2[14])+(m1[15]*m2[15]); \
    MAT_COPY(tmp,res)

/**
  * Multiply two single precision matrices: res = m1 * m2.
  */
void tcl3dMatfMult (float *m1, float *m2, float *res)
{
    MAT_MULT(float)
}

/**
  * Multiply two double precision matrices: res = m1 * m2.
  */
void tcl3dMatdMult (double *m1, double *m2, double *res)
{
    MAT_MULT(double)
}

/*
  OpenGL matrices are column major and can be quite confusing to access 
  when stored in the typical, one-dimensional array often used by the API.
  Here are some shorthand conversion macros, which convert a row/column 
  combination into an array index.
*/
    
#define MAT(r,c) m[c*4+r]

#define m11 MAT(0,0)
#define m12 MAT(0,1)
#define m13 MAT(0,2)
#define m14 MAT(0,3)
#define m21 MAT(1,0)
#define m22 MAT(1,1)
#define m23 MAT(1,2)
#define m24 MAT(1,3)
#define m31 MAT(2,0)
#define m32 MAT(2,1)
#define m33 MAT(2,2)
#define m34 MAT(2,3)
#define m41 MAT(3,0)
#define m42 MAT(3,1)
#define m43 MAT(3,2)
#define m44 MAT(3,3)

/**
  * Invert a single precision matrix.
  */
void tcl3dMatfInvert (float *m, float *res)
{
    /* Inverse = adjoint / det.
       Pre-compute 2x2 dets for last two rows when computing cofactors of first two rows.
     */
    float d12, d13, d23, d24, d34, d41;
    float det, invDet;
    float tmp[16];

    d12 = (m31 * m42 - m41 * m32);
    d13 = (m31 * m43 - m41 * m33);
    d23 = (m32 * m43 - m42 * m33);
    d24 = (m32 * m44 - m42 * m34);
    d34 = (m33 * m44 - m43 * m34);
    d41 = (m34 * m41 - m44 * m31);
    
    tmp[0] =  (m22 * d34 - m23 * d24 + m24 * d23);
    tmp[1] = -(m21 * d34 + m23 * d41 + m24 * d13);
    tmp[2] =  (m21 * d24 + m22 * d41 + m24 * d12);
    tmp[3] = -(m21 * d23 - m22 * d13 + m23 * d12);

    /* Compute determinant as early as possible using these cofactors. */
    det = m11 * tmp[0] + m12 * tmp[1] + m13 * tmp[2] + m14 * tmp[3];

    /* Run singularity test. */
    if ( det == 0.0 ) {
        printf ("Warning: Call to invertMatrix produced a Singular matrix.\n");
        MAT_IDENTITY
    } else {
       invDet = 1.0 / det;
       
       /* Compute rest of inverse. */
       tmp[0] *= invDet;
       tmp[1] *= invDet;
       tmp[2] *= invDet;
       tmp[3] *= invDet;

       tmp[4] = -(m12 * d34 - m13 * d24 + m14 * d23) * invDet;
       tmp[5] =  (m11 * d34 + m13 * d41 + m14 * d13) * invDet;
       tmp[6] = -(m11 * d24 + m12 * d41 + m14 * d12) * invDet;
       tmp[7] =  (m11 * d23 - m12 * d13 + m13 * d12) * invDet;

       /* Pre-compute 2x2 dets for first two rows when computing cofactors 
          of last two rows. */
       d12 = m11 * m22 - m21 * m12;
       d13 = m11 * m23 - m21 * m13;
       d23 = m12 * m23 - m22 * m13;
       d24 = m12 * m24 - m22 * m14;
       d34 = m13 * m24 - m23 * m14;
       d41 = m14 * m21 - m24 * m11;

       tmp[8]  =  (m42 * d34 - m43 * d24 + m44 * d23) * invDet;
       tmp[9]  = -(m41 * d34 + m43 * d41 + m44 * d13) * invDet;
       tmp[10] =  (m41 * d24 + m42 * d41 + m44 * d12) * invDet;
       tmp[11] = -(m41 * d23 - m42 * d13 + m43 * d12) * invDet;
       tmp[12] = -(m32 * d34 - m33 * d24 + m34 * d23) * invDet;
       tmp[13] =  (m31 * d34 + m33 * d41 + m34 * d13) * invDet;
       tmp[14] = -(m31 * d24 + m32 * d41 + m34 * d12) * invDet;
       tmp[15] =  (m31 * d23 - m32 * d13 + m33 * d12) * invDet;

       MAT_COPY(tmp,res)
    }
}

/**
  * Invert a double precision matrix.
  */
void tcl3dMatdInvert (double *m, double *res)
{
    /* Inverse = adjoint / det.
       Pre-compute 2x2 dets for last two rows when computing cofactors of first two rows.
     */
    double d12, d13, d23, d24, d34, d41;
    double det, invDet;
    double tmp[16];

    d12 = (m31 * m42 - m41 * m32);
    d13 = (m31 * m43 - m41 * m33);
    d23 = (m32 * m43 - m42 * m33);
    d24 = (m32 * m44 - m42 * m34);
    d34 = (m33 * m44 - m43 * m34);
    d41 = (m34 * m41 - m44 * m31);
    
    tmp[0] =  (m22 * d34 - m23 * d24 + m24 * d23);
    tmp[1] = -(m21 * d34 + m23 * d41 + m24 * d13);
    tmp[2] =  (m21 * d24 + m22 * d41 + m24 * d12);
    tmp[3] = -(m21 * d23 - m22 * d13 + m23 * d12);

    /* Compute determinant as early as possible using these cofactors. */
    det = m11 * tmp[0] + m12 * tmp[1] + m13 * tmp[2] + m14 * tmp[3];

    /* Run singularity test. */
    if ( det == 0.0 ) {
        printf ("Warning: Call to invertMatrix produced a Singular matrix.\n");
        MAT_IDENTITY
    } else {
       invDet = 1.0 / det;
       
       /* Compute rest of inverse. */
       tmp[0] *= invDet;
       tmp[1] *= invDet;
       tmp[2] *= invDet;
       tmp[3] *= invDet;

       tmp[4] = -(m12 * d34 - m13 * d24 + m14 * d23) * invDet;
       tmp[5] =  (m11 * d34 + m13 * d41 + m14 * d13) * invDet;
       tmp[6] = -(m11 * d24 + m12 * d41 + m14 * d12) * invDet;
       tmp[7] =  (m11 * d23 - m12 * d13 + m13 * d12) * invDet;

       /* Pre-compute 2x2 dets for first two rows when computing cofactors 
          of last two rows. */
       d12 = m11 * m22 - m21 * m12;
       d13 = m11 * m23 - m21 * m13;
       d23 = m12 * m23 - m22 * m13;
       d24 = m12 * m24 - m22 * m14;
       d34 = m13 * m24 - m23 * m14;
       d41 = m14 * m21 - m24 * m11;

       tmp[8]  =  (m42 * d34 - m43 * d24 + m44 * d23) * invDet;
       tmp[9]  = -(m41 * d34 + m43 * d41 + m44 * d13) * invDet;
       tmp[10] =  (m41 * d24 + m42 * d41 + m44 * d12) * invDet;
       tmp[11] = -(m41 * d23 - m42 * d13 + m43 * d12) * invDet;
       tmp[12] = -(m32 * d34 - m33 * d24 + m34 * d23) * invDet;
       tmp[13] =  (m31 * d34 + m33 * d41 + m34 * d13) * invDet;
       tmp[14] = -(m31 * d24 + m32 * d41 + m34 * d12) * invDet;
       tmp[15] =  (m31 * d23 - m32 * d13 + m33 * d12) * invDet;

       MAT_COPY(tmp,res)
    }
}

#undef m11
#undef m12
#undef m13
#undef m14
#undef m21
#undef m22
#undef m23
#undef m24
#undef m31
#undef m32
#undef m33
#undef m34
#undef m41
#undef m42
#undef m43
#undef m44
#undef MAT

#define MAT_TRANSPOSE \
    res[0]  = m[0];   \
    res[1]  = m[4];   \
    res[2]  = m[8];   \
    res[3]  = m[12];  \
    res[4]  = m[1];   \
    res[5]  = m[5];   \
    res[6]  = m[9];   \
    res[7]  = m[13];  \
    res[8]  = m[2];   \
    res[9]  = m[6];   \
    res[10] = m[10];  \
    res[11] = m[14];  \
    res[12] = m[3];   \
    res[13] = m[7];   \
    res[14] = m[11];  \
    res[15] = m[15];

/**
  * Transpose a single precision matrix.
  */
void tcl3dMatfTranspose (float *m, float *res)
{
    MAT_TRANSPOSE
}

/**
  * Transpose a double precision matrix.
  */
void tcl3dMatdTranspose (double *m, double *res)
{
    MAT_TRANSPOSE
}

/**
  * Decompose a rotation matrix into the 3 Euler angles.
  */
int tcl3dMatfGetAngles1 (float *m, float *res)
{
    res[1] = asin(m[0*4+2]);
    if (fabs(cos(res[1])) > 0.0000001) {
        res[0] = atan2 (m[1*4+2], m[2*4+2]);
        res[2] = atan2 (m[0*4+1], m[0*4+0]);
        return 1;
    } else {
        res[0] = atan2 (m[1*4+0], m[1*4+1]);
        res[0] = 0.0;
        return 0;
    }
}

int tcl3dMatfGetAngles (float *m, float *res)
{
    #define HALF_PI 1.5707963267948966192313216916398
    if ( m[2] < 1.0f ) {
        if ( m[2] > -1.0f ) {
            res[0] = atan2(-m[6],m[10]);
            res[1] = asin(m[2]);
            res[2] = atan2(-m[1],m[0]);
            return 1;
        } else {
            // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
            res[0] = -atan2(m[4],m[5]);
            res[1] = -HALF_PI;
            res[2] = 0.0f;
            return 0;
        }
    } else {
        // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
        res[0] = atan2(m[4],m[5]);
        res[1] = HALF_PI;
        res[2] = 0.0f;
        return 0;
    }
}

/** Replacement function for glMultMatf: res = res * m.
  * It has the same signature except the last parameter, where the accumulated
  * matrix is stored.
  */
void tcl3dMultMatrixf (float *m, float *res) 
{
    tcl3dMatfMult (res, m, res);
}

/** Replacement function for glMultMatd: res = res * m.
  * It has the same signature except the last parameter, where the accumulated
  * matrix is stored.
  */
void tcl3dMultMatrixd (double *m, double *res) 
{
    tcl3dMatdMult (res, m, res);
}

/** Replacement function for glRotatef.
  * It has the same signature except the last parameter, where the accumulated
  * matrix is stored.
  */
void tcl3dRotatef (float angle, float x, float y, float z, float *res) 
{
    float axis[3];
    float tmp[16];

    axis[0] = x;
    axis[1] = y;
    axis[2] = z;
    tcl3dMatfRotate (angle, axis, tmp);
    tcl3dMatfMult (res, tmp, res);
}

/** Replacement function for glRotated.
  * It has the same signature except the last parameter, where the accumulated
  * matrix is stored.
  */
void tcl3dRotated (double angle, double x, double y, double z, double *res) 
{
    double axis[3];
    double tmp[16];

    axis[0] = x;
    axis[1] = y;
    axis[2] = z;
    tcl3dMatdRotate (angle, axis, tmp);
    tcl3dMatdMult (res, tmp, res);
}

/** Replacement function for glScalef.
  * It has the same signature except the last parameter, where the accumulated
  * matrix is stored.
  */
void tcl3dScalef (float x, float y, float z, float *res)
{
    float tmp[16];
    tcl3dMatfScale (x, y, z, tmp);
    tcl3dMatfMult (res, tmp, res);
}

/** Replacement function for glScaled.
  * It has the same signature except the last parameter, where the accumulated
  * matrix is stored.
  */
void tcl3dScaled (double x, double y, double z, double *res)
{
    double tmp[16];
    tcl3dMatdScale (x, y, z, tmp);
    tcl3dMatdMult (res, tmp, res);
}

/** Replacement function for glTranslatef.
  * It has the same signature except the last parameter, where the accumulated
  * matrix is stored.
  */
void tcl3dTranslatef (float x, float y, float z, float *res)
{
    float tmp[16];
    tcl3dMatfTranslate (x, y, z, tmp);
    tcl3dMatfMult (res, tmp, res);
}

/** Replacement function for glTranslated.
  * It has the same signature except the last parameter, where the accumulated
  * matrix is stored.
  */
void tcl3dTranslated (double x, double y, double z, double *res)
{
    double tmp[16];
    tcl3dMatdTranslate (x, y, z, tmp);
    tcl3dMatdMult (res, tmp, res);
}

unsigned int tcl3dRoundUpPow2 (unsigned int val)
{
    val--;
    val |= val >>  1;
    val |= val >>  2;
    val |= val >>  4;
    val |= val >>  8;
    val |= val >> 16;
    return val+1;
}

unsigned int tcl3dIsPow2 (unsigned int val) {
    unsigned int i = 1;
    while (i <= val) {
        if (val == i) {
            return 1;
        }
        i *= 2;
    }
    return 0;
}
