/*
 * Rayforce(TM) - Patent pending
 *
 * Copyright (c) 2007 Alexis Naveros
 *
 * Portions developed under contract to the SURVICE Engineering Company.
 * (reference purchase order 06349-21 for U.S. Government contract 
 * GS23F0012M, W911QX-06-F-0137).
 * 
 * The U.S. Government has government purpose rights to this software and 
 * associated documentation per DFARS 252.227-7014 and DFARS 252.227-7013.
 *
 */
/**
 * @file
 *
 * Global and generic math functions.
*/

#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include <limits.h>
#include <float.h>
#include <time.h>
#include <stdarg.h>
#include <errno.h>
#include <assert.h>

#include <dirent.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/time.h>

#include "rfmath.h"



#define M_HPI 1.57079632679489661923
#define M_ANGSIN(x) sin((x*2*M_PI)/360.0)
#define M_ANGCOS(x) cos((x*2*M_PI)/360.0)


#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif


////


void mathMatrixIdentityf( float *a )
{
  memset( a, 0, sizeof(float)*16 );
  a[0*4+0] = a[1*4+1] = a[2*4+2] = a[3*4+3] = 1.0;
  return;
}

void mathMatrixTransposef( float *out, float *m )
{
  int a, b;
  for( a = 0 ; a < 4 ; a++ )
  {
    for( b = 0 ; b < 4; b++ )
      out[a*4+b] = m[b*4+a];
  }
  return;
}

void mathMatrixMulVectorf( float *out, float *v, float *m )
{
  out[0] = v[0] * m[0*4+0] + v[1] * m[1*4+0] + v[2] * m[2*4+0];
  out[1] = v[0] * m[0*4+1] + v[1] * m[1*4+1] + v[2] * m[2*4+1];
  out[2] = v[0] * m[0*4+2] + v[1] * m[1*4+2] + v[2] * m[2*4+2];
  return;
}

void mathMatrixMulPointf( float *out, float *v, float *m )
{
  out[0] = v[0] * m[0*4+0] + v[1] * m[1*4+0] + v[2] * m[2*4+0] + m[3*4+0];
  out[1] = v[0] * m[0*4+1] + v[1] * m[1*4+1] + v[2] * m[2*4+1] + m[3*4+1];
  out[2] = v[0] * m[0*4+2] + v[1] * m[1*4+2] + v[2] * m[2*4+2] + m[3*4+2];
  return;
}

void mathMatrixMulPoint4f( float *out, float *v, float *m )
{
  out[0] = v[0] * m[0*4+0] + v[1] * m[1*4+0] + v[2] * m[2*4+0] + v[3] * m[3*4+0];
  out[1] = v[0] * m[0*4+1] + v[1] * m[1*4+1] + v[2] * m[2*4+1] + v[3] * m[3*4+1];
  out[2] = v[0] * m[0*4+2] + v[1] * m[1*4+2] + v[2] * m[2*4+2] + v[3] * m[3*4+2];
  out[3] = v[0] * m[0*4+3] + v[1] * m[1*4+3] + v[2] * m[2*4+3] + v[3] * m[3*4+3];
  return;
}

void mathMatrixMultf( float *out, float *m1, float *m2 )
{
  int i, j;
  for( i = 0 ; i < 4 ; i++ )
  {
    for( j = 0 ; j < 4 ; j++ )
      out[i*4+j] = m1[i*4+0]*m2[0*4+j] + m1[i*4+1]*m2[1*4+j] + m1[i*4+2]*m2[2*4+j] + m1[i*4+3]*m2[3*4+j];
  }
  return;
}

void mathMatrixMultinvf( float *out, float *m1, float *m2 )
{
  int i, j;
  for( i = 0 ; i < 4 ; i++ )
  {
    for( j = 0 ; j < 4 ; j++ )
      out[i*4+j] = m1[i*4+0]*m2[j*4+0] + m1[i*4+1]*m2[j*4+1] + m1[i*4+2]*m2[j*4+2] + m1[i*4+3]*m2[j*4+3];
  }
  return;
}

void mathMatrixInvertf( float *mdst, float *m )
{
  float sx, sy, sz;

#define M(m,x,y) (m)[(x)+4*(y)]

  sx = 1.0 / ( m[0]*m[0] + m[1]*m[1] + m[2]*m[2] );
  sy = 1.0 / ( m[4]*m[4] + m[5]*m[5] + m[6]*m[6] );
  sz = 1.0 / ( m[8]*m[8] + m[9]*m[9] + m[10]*m[10] );
  M(mdst,0,0) = M(m,0,0) * sx;
  M(mdst,0,1) = M(m,1,0) * sx;
  M(mdst,0,2) = M(m,2,0) * sx;
  M(mdst,0,3) = -( M(m,0,3)*M(m,0,0) + M(m,1,3)*M(m,1,0) + M(m,2,3)*M(m,2,0) ) * sx;
  M(mdst,1,0) = M(m,0,1) * sy;
  M(mdst,1,1) = M(m,1,1) * sy;
  M(mdst,1,2) = M(m,2,1) * sy;
  M(mdst,1,3) = -( M(m,0,3)*M(m,0,1) + M(m,1,3)*M(m,1,1) + M(m,2,3)*M(m,2,1) ) * sy;
  M(mdst,2,0) = M(m,0,2) * sz;
  M(mdst,2,1) = M(m,1,2) * sz;
  M(mdst,2,2) = M(m,2,2) * sz;
  M(mdst,2,3) = -( M(m,0,3)*M(m,0,2) + M(m,1,3)*M(m,1,2) + M(m,2,3)*M(m,2,2) ) * sz;
  M(mdst,3,0) = 0.0;
  M(mdst,3,1) = 0.0;
  M(mdst,3,2) = 0.0;
  M(mdst,3,3) = 1.0;

#undef M

  return;
}

void mathMatrixRotf( float *mat, float angle, float x, float y, float z )
{
  float mag, s, c;
  float xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
  float m[16], n[16];

  angle *= ( M_PI / 180.0 );
  s = sin( angle );
  c = cos( angle );
  mag = sqrt( x*x + y*y + z*z );

  if( mag <= 1.0e-4 )
    return;
  x /= mag;
  y /= mag;
  z /= mag;

#define M(row,col)  m[col*4+row]
  xx = x * x;
  yy = y * y;
  zz = z * z;
  xy = x * y;
  yz = y * z;
  zx = z * x;
  xs = x * s;
  ys = y * s;
  zs = z * s;
  one_c = 1.0f - c;
  M(0,0) = (one_c * xx) + c;
  M(0,1) = (one_c * xy) - zs;
  M(0,2) = (one_c * zx) + ys;
  M(0,3) = 0.0f;
  M(1,0) = (one_c * xy) + zs;
  M(1,1) = (one_c * yy) + c;
  M(1,2) = (one_c * yz) - xs;
  M(1,3) = 0.0f;
  M(2,0) = (one_c * zx) - ys;
  M(2,1) = (one_c * yz) + xs;
  M(2,2) = (one_c * zz) + c;
  M(2,3) = 0.0F;
  M(3,0) = 0.0F;
  M(3,1) = 0.0F;
  M(3,2) = 0.0F;
  M(3,3) = 1.0F;
#undef M

  memcpy( n, mat, 16*sizeof(float) );
  mathMatrixMultf( mat, m, n );
  return;
}

void mathMatrixRotxf( float *m, float angle )
{
  float n[16];
  float o[16];
  angle *= ( M_PI / 180.0 );
  mathMatrixIdentityf( n );
  n[1*4+1] =  cos( angle );
  n[1*4+2] =  sin( angle );
  n[2*4+1] = -sin( angle );
  n[2*4+2] =  cos( angle );
  memcpy( o, m, 16*sizeof(float) );
  mathMatrixMultf( m, n, o );
  return;
}

void mathMatrixRotyf( float *m, float angle )
{
  float n[16];
  float o[16];
  angle *= ( M_PI / 180.0 );
  mathMatrixIdentityf( n );
  n[0*4+0] =  cos( angle );
  n[0*4+2] = -sin( angle );
  n[2*4+0] =  sin( angle );
  n[2*4+2] =  cos( angle );
  memcpy( o, m, 16*sizeof(float) );
  mathMatrixMultf( m, n, o );
  return;
}

void mathMatrixRotzf( float *m, float angle )
{
  float n[16];
  float o[16];
  angle *= ( M_PI / 180.0 );
  mathMatrixIdentityf( n );
  n[0*4+0] =  cos( angle );
  n[0*4+1] = -sin( angle );
  n[1*4+0] =  sin( angle );
  n[1*4+1] =  cos( angle );
  memcpy( o, m, 16*sizeof(float) );
  mathMatrixMultf( m, n, o );
  return;
}

void mathMatrixTranslatef( float *m, float x, float y, float z )
{
  float n[16];
  float o[16];
  mathMatrixIdentityf( n );
  n[3*4+0] = x;
  n[3*4+1] = y;
  n[3*4+2] = z;
  memcpy( o, m, 16*sizeof(float) );
  mathMatrixMultf( m, n, o );
  return;
}

void mathMatrixScalef( float *m, float x, float y, float z )
{
  float n[16];
  float o[16];
  mathMatrixIdentityf( n );
  n[0*4+0] = x;
  n[1*4+1] = y;
  n[2*4+2] = z;
  memcpy( o, m, 16*sizeof(float) );
  mathMatrixMultf( m, n, o );
  return;
}

void mathMatrixFrustumf( float *m, float left, float right, float bottom, float top, float nearval, float farval )
{
  float x, y, a, b, c, d;
  x = ( 2.0 * nearval ) / ( right - left );
  y = ( 2.0 * nearval ) / ( top - bottom );
  a = ( right + left ) / ( right - left );
  b = ( top + bottom ) / ( top - bottom );
  c = -( farval + nearval ) / ( farval - nearval );
  d = -( 2.0 * farval * nearval ) / ( farval - nearval );

#define M(row,col)  m[col*4+row]
  M(0,0) = x;
  M(0,1) = 0.0F;
  M(0,2) = a;
  M(0,3) = 0.0F;
  M(1,0) = 0.0F;
  M(1,1) = y;
  M(1,2) = b;
  M(1,3) = 0.0F;
  M(2,0) = 0.0F;
  M(2,1) = 0.0F;
  M(2,2) = c;
  M(2,3) = d;
  M(3,0) = 0.0F;
  M(3,1) = 0.0F;
  M(3,2) = -1.0F;
  M(3,3) = 0.0F;
#undef M

  return;
}

#define mathMatrix_FRINFINITY (0.00001)

void mathMatrixFrustumInfinityf( float *m, float left, float right, float bottom, float top, float nearval )
{
  float x, y, a, b, c, d;
  x = ( 2.0 * nearval ) / ( right - left );
  y = ( 2.0 * nearval ) / ( top - bottom );
  a = ( right + left ) / ( right - left );
  b = ( top + bottom ) / ( top - bottom );

  c = mathMatrix_FRINFINITY - 1;
  d = nearval * ( mathMatrix_FRINFINITY - 2 );

#define M(row,col)  m[col*4+row]
  M(0,0) = x;
  M(0,1) = 0.0F;
  M(0,2) = a;
  M(0,3) = 0.0F;
  M(1,0) = 0.0F;
  M(1,1) = y;
  M(1,2) = b;
  M(1,3) = 0.0F;
  M(2,0) = 0.0F;
  M(2,1) = 0.0F;
  M(2,2) = c;
  M(2,3) = d;
  M(3,0) = 0.0F;
  M(3,1) = 0.0F;
  M(3,2) = -1.0F;
  M(3,3) = 0.0F;
#undef M

  return;
}

void mathMatrixPerspectivef( float *m, float fovy, float aspect, float zNear, float zFar )
{
  float xmin, xmax, ymin, ymax;
  ymax = zNear * tan(fovy * M_PI / 360.0);
  ymin = -ymax;
  xmin = ymin * aspect;
  xmax = ymax * aspect;
  mathMatrixFrustumf( m, xmin, xmax, ymin, ymax, zNear, zFar );
  return;
}

void mathMatrixPerspectiveInfinityf( float *m, float fovy, float aspect, float zNear )
{
  float xmin, xmax, ymin, ymax;
  ymax = zNear * tan(fovy * M_PI / 360.0);
  ymin = -ymax;
  xmin = ymin * aspect;
  xmax = ymax * aspect;
  mathMatrixFrustumInfinityf( m, xmin, xmax, ymin, ymax, zNear );
  return;
}

void mathMatrixOrthof( float *m, float left, float right, float bottom, float top, float nearval, float farval )
{
  float x, y, z;
  float tx, ty, tz;
  x = 2.0 / ( right - left );
  y = 2.0 / ( top - bottom );
  z = -2.0 / ( farval - nearval );
  tx = -( right + left ) / ( right - left );
  ty = -( top + bottom ) / ( top - bottom );
  tz = -( farval + nearval ) / ( farval - nearval );

#define M(row,col)  m[col*4+row]
  M(0,0) = x;     M(0,1) = 0.0;  M(0,2) = 0.0;  M(0,3) = tx;
  M(1,0) = 0.0;  M(1,1) = y;     M(1,2) = 0.0;  M(1,3) = ty;                                                
  M(2,0) = 0.0;  M(2,1) = 0.0;  M(2,2) = z;     M(2,3) = tz; 
  M(3,0) = 0.0;  M(3,1) = 0.0;  M(3,2) = 0.0;  M(3,3) = 1.0;
#undef M

  return;
}



////



void mathVectorNormalizef( float *v )
{
  float f;
  f = 1.0 / sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] );
  v[0] *= f;
  v[1] *= f;
  v[2] *= f;
  return;
}

float mathVectorMagnitudef( float *v )
{
  return sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] );
}

void mathVectorNormalizeSafef( float *v )
{
  float f;
  f = sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] );
  if( f < 0.0000001 )
  {
    memset( v, 0, 3*sizeof(float) );
    return;
  }
  f = 1.0 / f;
  v[0] *= f;
  v[1] *= f;
  v[2] *= f;
  return;
}

void mathVectorCrossproductf( float *v, float *v1, float *v2 )
{
  v[0] = ( v1[1] * v2[2] ) - ( v1[2] * v2[1] );
  v[1] = ( v1[2] * v2[0] ) - ( v1[0] * v2[2] );
  v[2] = ( v1[0] * v2[1] ) - ( v1[1] * v2[0] );
  return;
}

float mathVectorDotProductf( float *v1, float *v2 )
{
  return ( v1[0] * v2[0] ) + ( v1[1] * v2[1] ) + ( v1[2] * v2[2] );
}

void mathVectorReflectf( float *v, float *rayvector, float *normal )
{
  float dotpr;
  dotpr = 2.0 * mathVectorDotProductf( rayvector, normal );
  mathVectorNormalizef( rayvector );
  v[0] = rayvector[0] - dotpr * normal[0];
  v[1] = rayvector[1] - dotpr * normal[1];
  v[2] = rayvector[2] - dotpr * normal[2];
  mathVectorNormalizef( v );
  return;
}



////



void mathPlaneNormf( float *p )
{
  float f = 1.0 / sqrt( p[0]*p[0] + p[1]*p[1] + p[2]*p[2] );
  p[0] *= f;
  p[1] *= f;
  p[2] *= f;
  p[3] *= f;
  return;
}

void mathPlaneEquationf( float *eq, float *v1, float *v2, float *v3 )
{
  float px, py, pz, qx, qy, qz;
  qx = v2[0] - v1[0];
  qy = v2[1] - v1[1];
  qz = v2[2] - v1[2];
  px = v3[0] - v1[0];
  py = v3[1] - v1[1];
  pz = v3[2] - v1[2];
  eq[0] = pz*qy - py*qz;
  eq[1] = px*qz - pz*qx;
  eq[2] = py*qx - px*qy;
  mathVectorNormalizef( eq );
  eq[3] = -( eq[0]*v1[0] + eq[1]*v1[1] + eq[2]*v1[2] );
  return;
}

void mathPlaneNormalf( float *n, float *v1, float *v2, float *v3 )
{
  float px, py, pz, qx, qy, qz;
  qx = v2[0] - v1[0];
  qy = v2[1] - v1[1];
  qz = v2[2] - v1[2];
  px = v3[0] - v1[0];
  py = v3[1] - v1[1];
  pz = v3[2] - v1[2];
  n[0] = pz*qy - py*qz;
  n[1] = px*qz - pz*qx;
  n[2] = py*qx - px*qy;
  return;
}

// return 6 planes, 24 floats
void mathPlaneFrustumf( float *out, float *proj, float *modl )
{
  float m[16];

  mathMatrixMultf( m, modl, proj );

  // near
  out[0*4+0] = m[ 3] + m[ 2];
  out[0*4+1] = m[ 7] + m[ 6];
  out[0*4+2] = m[11] + m[10];
  out[0*4+3] = m[15] + m[14];
  mathPlaneNormf( &out[0*4] );

  // right
  out[1*4+0] = m[ 3] - m[ 0];
  out[1*4+1] = m[ 7] - m[ 4];
  out[1*4+2] = m[11] - m[ 8];
  out[1*4+3] = m[15] - m[12];
  mathPlaneNormf( &out[1*4] );

  // left
  out[2*4+0] = m[ 3] + m[ 0];
  out[2*4+1] = m[ 7] + m[ 4];
  out[2*4+2] = m[11] + m[ 8];
  out[2*4+3] = m[15] + m[12];
  mathPlaneNormf( &out[2*4] );

  // bottom
  out[3*4+0] = m[ 3] + m[ 1];
  out[3*4+1] = m[ 7] + m[ 5];
  out[3*4+2] = m[11] + m[ 9];
  out[3*4+3] = m[15] + m[13];
  mathPlaneNormf( &out[3*4] );

  // top
  out[4*4+0] = m[ 3] - m[ 1];
  out[4*4+1] = m[ 7] - m[ 5];
  out[4*4+2] = m[11] - m[ 9];
  out[4*4+3] = m[15] - m[13];
  mathPlaneNormf( &out[4*4] );

  // far
  out[5*4+0] = m[ 3] - m[ 2];
  out[5*4+1] = m[ 7] - m[ 6];
  out[5*4+2] = m[11] - m[10];
  out[5*4+3] = m[15] - m[14];
  mathPlaneNormf( &out[5*4] );

  return;
}

float mathPlanePointf( float *plane, float *pt )
{
  return pt[0]*plane[0] + pt[1]*plane[1] + pt[2]*plane[2] + plane[3];
}

float mathPlanePointInvf( float *plane, float *pt )
{
  return -pt[0]*plane[0] + -pt[1]*plane[1] + -pt[2]*plane[2] + plane[3];
}

void mathPlaneIntersect3f( float *pt, float *p0, float *p1, float *p2 )
{
  float d;
  float n0[3], n1[3], n2[3];
  _mathVectorCrossproduct( n0, &p1[0], &p2[0] );
  d = -1.0 / _mathVectorDotProduct( p0, n0 );
  _mathVectorCrossproduct( n1, &p2[0], &p0[0] );
  _mathVectorCrossproduct( n2, &p0[0], &p1[0] );
  _mathVectorMulScalar( n0, p0[3] );
  _mathVectorMulScalar( n1, p1[3] );
  _mathVectorMulScalar( n2, p2[3] );
  pt[0] = ( n0[0] + n1[0] + n2[0] ) * d;
  pt[1] = ( n0[1] + n1[1] + n2[1] ) * d;
  pt[2] = ( n0[2] + n1[2] + n2[2] ) * d;
  return;
}



////



float mathPointDistf( float *p0, float *p1 )
{
  float p[3];
  p[0] = p0[0] - p1[0];
  p[1] = p0[1] - p1[1];
  p[2] = p0[2] - p1[2];
  return sqrt( p[0]*p[0] + p[1]*p[1] + p[2]*p[2] );
}



////



void mathQuatIdentityf( float *quat )
{
  quat[0] = 1.0;
  quat[1] = 0.0;
  quat[2] = 0.0;
  quat[3] = 0.0;
  return;
}

void mathQuatInvertf( float *out, float *quat )
{
  out[0] = -quat[0];
  out[1] = -quat[1];
  out[2] = -quat[2];
  out[3] = quat[3];
  return;
}

void mathQuatNormalizef( float *quat )
{
  float f;
  float facteur = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3];
  f = 1.0 / sqrt( facteur );
  quat[0] = quat[0] * f;
  quat[1] = quat[1] * f;
  quat[2] = quat[2] * f;
  quat[3] = quat[3] * f;
  return;
}

void mathQuatVectorf( float *quat, float *v )
{
  float w = quat[0];
  float x = quat[1];
  float y = quat[2];
  float z = quat[3];
  mathQuatNormalizef( quat );
  v[0] = 2.0 * ( x * z - w * y );
  v[1] = 2.0 * ( y * z + w * x );
  v[2] = 1.0 - 2.0 * ( x * x + y * y );
  return;
}

void mathQuatMatrixf( float *quat, float *matrix )
{
  float w, x, y, z, xx, yy, zz;
  mathQuatNormalizef( quat );
  w = quat[0];
  x = quat[1];
  y = quat[2];
  z = quat[3];
  xx = x * x;
  yy = y * y;
  zz = z * z;
  matrix[0*4+0] = 1.0 - 2.0 * ( yy + zz );
  matrix[0*4+1] = 2.0 * ( x * y + w * z );
  matrix[0*4+2] = 2.0 * ( x * z - w * y );
  matrix[0*4+3] = 0.0;
  matrix[1*4+0] = 2.0 * ( x * y - w * z );
  matrix[1*4+1] = 1.0 - 2.0 * ( xx + zz );
  matrix[1*4+2] = 2.0 * ( y * z + w * x );
  matrix[1*4+3] = 0.0;
  matrix[2*4+0] = 2.0 * ( x * z + w * y );
  matrix[2*4+1] = 2.0 * ( y * z - w * x );
  matrix[2*4+2] = 1.0 - 2.0 * ( xx + yy );
  matrix[2*4+3] = 0.0;
  matrix[3*4+0] = 0.0;
  matrix[3*4+1] = 0.0;
  matrix[3*4+2] = 0.0;
  matrix[3*4+3] = 1.0;
  return;
}

void mathQuatTransMatrixf( float *quat, float *matrix )
{
  float w, x, y, z, xx, yy, zz;
  mathQuatNormalizef( quat );
  w = quat[0];
  x = quat[1];
  y = quat[2];
  z = quat[3];
  xx = x * x;
  yy = y * y;
  zz = z * z;
  matrix[0*4+0] = 1.0 - 2.0 * ( yy + zz );
  matrix[1*4+0] = 2.0 * ( x * y + w * z );
  matrix[2*4+0] = 2.0 * ( x * z - w * y );
  matrix[3*4+0] = 0.0;
  matrix[0*4+1] = 2.0 * ( x * y - w * z );
  matrix[1*4+1] = 1.0 - 2.0 * ( xx + zz );
  matrix[2*4+1] = 2.0 * ( y * z + w * x );
  matrix[3*4+1] = 0.0;
  matrix[0*4+2] = 2.0 * ( x * z + w * y );
  matrix[1*4+2] = 2.0 * ( y * z - w * x );
  matrix[2*4+2] = 1.0 - 2.0 * ( xx + yy );
  matrix[3*4+2] = 0.0;
  matrix[0*4+3] = 0.0;
  matrix[1*4+3] = 0.0;
  matrix[2*4+3] = 0.0;
  matrix[3*4+3] = 1.0;
  return;
}


void mathQuatLoadMatrixf( float *m, float *quat )
{
  float tr, s, q[4];
  int i, j, k;
  int nxt[3] = {1, 2, 0};
  tr = m[0*4+0] + m[1*4+1] + m[2*4+2];
  if( tr > 0.0 )
  {
    s = sqrt( tr + 1.0 );
    quat[0] = s / 2.0;
    s = 0.5 / s;
    quat[1] = ( m[1*4+2] - m[2*4+1] ) * s;
    quat[2] = ( m[2*4+0] - m[0*4+2] ) * s;
    quat[3] = ( m[0*4+1] - m[1*4+0] ) * s;
  }
  else {
    i = 0;
    if( m[1*4+1] > m[0*4+0] )
      i = 1;
    if( m[2*4+2] > m[i*4+i] )
      i = 2;
    j = nxt[i];
    k = nxt[j];
    s = sqrt( ( m[i*4+i] - ( m[j*4+j] + m[k*4+k] ) ) + 1.0 );
    q[i] = s * 0.5;
    if( s != 0.0 )
      s = 0.5 / s;
    q[3] = ( m[j*4+k] - m[k*4+j] ) * s;
    q[j] = ( m[i*4+j] + m[j*4+i] ) * s;
    q[k] = ( m[i*4+k] + m[k*4+i] ) * s;
    /// hmm... oh well
    quat[1] = q[0];
    quat[2] = q[1];
    quat[3] = q[2];
    quat[0] = q[3];
  }
  return;
}



void mathQuatMultResf( float *quat, float *quat1, float *quat2 )
{
  quat[0] = quat2[0]*quat1[0] - quat2[1]*quat1[1] - quat2[2]*quat1[2] - quat2[3]*quat1[3];
  quat[1] = quat2[0]*quat1[1] + quat2[1]*quat1[0] + quat2[2]*quat1[3] - quat2[3]*quat1[2];
  quat[2] = quat2[0]*quat1[2] - quat2[1]*quat1[3] + quat2[2]*quat1[0] + quat2[3]*quat1[1];
  quat[3] = quat2[0]*quat1[3] + quat2[1]*quat1[2] - quat2[2]*quat1[1] + quat2[3]*quat1[0];
  return;
}

void mathQuatMultf( float *quat, float *quatMult )
{
  float quat_temp[4];
  memcpy( quat_temp, quat, 4 * sizeof(float) );
  mathQuatMultResf( quat, quat_temp, quatMult );
  return;
}

void mathQuatBuildf( float *quat, float angle, float x, float y, float z )
{
  float a, b, c;
  angle *= ( M_PI / 180.0 );
  b = x*x + y*y + z*z;
  if( b != 0 )
    a = ( 1.0 / sqrt( b ) );
  else
    a = 32768.0;
  x = x * a;
  y = y * a;
  z = z * a;
  c = sin( angle / 2.0 );
  quat[0] = cos( angle / 2.0 );
  quat[1] = x * c;
  quat[2] = y * c;
  quat[3] = z * c;
  return;
}

void mathQuatDefinef( float *quat, float x, float y, float z )
{
  float quat_x[4];
  float quat_y[4];
  float quat_z[4];
  mathQuatBuildf( quat_x, x, 1.0, 0.0, 0.0 );
  mathQuatBuildf( quat_y, y, 0.0, 1.0, 0.0 );
  mathQuatBuildf( quat_z, z, 0.0, 0.0, 1.0 );
  memcpy( quat, quat_x, 4 * sizeof(float) );
  mathQuatMultf( quat, quat_y );
  mathQuatMultf( quat, quat_z );
  return;
}

#define DEF_DELTA 0.00001

void mathQuatInterf( float *out, float slerp, float *quat1, float *quat2 )
{
  int a;
  float quat1t[4], quat2t[4], omega, cosom, sinom, scale0, scale1;
  cosom = quat1[0] * quat2[0] + quat1[1] * quat2[1] + quat1[2] * quat2[2] + quat1[3] * quat2[3];
  memcpy( quat1t, quat1, 4 * sizeof(float) );
  memcpy( quat2t, quat2, 4 * sizeof(float) );
  if( cosom < 0.0 )
  {
    cosom = -cosom;
    for( a = 0 ; a < 4 ; a++ )
      quat1t[a] = -quat1t[a];
  }
  if( ( 1.0 - cosom ) > DEF_DELTA )
  {
    omega = acos( cosom );
    sinom = sin( omega );
    scale0 = sin( ( 1 - slerp ) * omega ) / sinom;
    scale1 = sin( slerp * omega ) / sinom;
  }
  else {
    scale0 = 1.0 - slerp;
    scale1 = slerp;
  }
  for( a = 0 ; a < 4 ; a++ )
    out[a] = scale0 * quat1t[a] + scale1 * quat2t[a];
  return;
}



////



void mathBezier3f( float *pt, float *p0, float *p1, float *p2, float step, int dims )
{
  int a;
  float stepinv;
  float stepf0, stepf1, stepf2;
  stepinv = 1 - step;
  stepf0 = stepinv * stepinv;
  stepf1 = 2.0 * stepinv * step;
  stepf2 = step * step;
  for( a = 0 ; a < dims ; a++ )
    pt[a] = p0[a]*stepf0  +  p1[a]*stepf1  +  p2[a]*stepf2;
  return;
}


void mathBezier4f( float *pt, float *p0, float *p1, float *p2, float *p3, float step, int dims )
{
  int a;
  float stepinv;
  float stepf0, stepf1, stepf2, stepf3;
  stepinv = 1 - step;
  stepf0 = stepinv * stepinv * stepinv;
  stepf1 = 3.0 * step * stepinv * stepinv;
  stepf2 = 3.0 * step * step * stepinv;
  stepf3 = step * step * step;
  for( a = 0 ; a < dims ; a++ )
    pt[a] = stepf0*p0[a]  +  stepf1*p1[a]  +  stepf2*p2[a]  +  stepf3*p3[a];
  return;
}


void mathBezier5f( float *pt, float *p0, float *p1, float *p2, float *p3, float *p4, float step, int dims )
{
  int a;
  float stepinv;
  float stepf0, stepf1, stepf2, stepf3, stepf4;
  stepinv = 1 - step;
  stepf0 = stepinv * stepinv * stepinv * stepinv;
  stepf1 = 4.0 * step * stepinv * stepinv * stepinv;
  stepf2 = 5.0 * step * step * stepinv * stepinv;
  stepf3 = 4.0 * step * step * step * stepinv;
  stepf4 = step * step * step * step;
  for( a = 0 ; a < dims ; a++ )
    pt[a] = stepf0*p0[a]  +  stepf1*p1[a]  +  stepf2*p2[a]  +  stepf3*p3[a]  +  stepf4*p4[a];
  return;
}


void mathBezier6f( float *pt, float *p0, float *p1, float *p2, float *p3, float *p4, float *p5, float step, int dims )
{
  int a;
  float stepinv;
  float stepf0, stepf1, stepf2, stepf3, stepf4, stepf5;
  stepinv = 1 - step;
  stepf0 = stepinv * stepinv * stepinv * stepinv * stepinv;;
  stepf1 = 5.0 * step * stepinv * stepinv * stepinv * stepinv;;
  stepf2 = 9.0 * step * step * stepinv * stepinv * stepinv;
  stepf3 = 9.0 * step * step * step * stepinv * stepinv;
  stepf4 = 5.0 * step * step * step * step * stepinv;
  stepf5 = step * step * step * step * step;
  for( a = 0 ; a < dims ; a++ )
    pt[a] = stepf0*p0[a]  +  stepf1*p1[a]  +  stepf2*p2[a]  +  stepf3*p3[a]  +  stepf4*p4[a]  +  stepf5*p5[a];
  return;
}



////



float mathBezier3x1f( float *p, float step )
{
  float stepinv;
  float stepf0, stepf1, stepf2;
  stepinv = 1 - step;
  stepf0 = stepinv * stepinv;
  stepf1 = 2.0 * stepinv * step;
  stepf2 = step * step;
  return stepf0*p[0]  +  stepf1*p[1]  +  stepf2*p[2];
}

float mathBezier4x1f( float *p, float step )
{
  float stepinv;
  float stepf0, stepf1, stepf2, stepf3;
  stepinv = 1 - step;
  stepf0 = stepinv * stepinv * stepinv;
  stepf1 = 3.0 * step * stepinv * stepinv;
  stepf2 = 3.0 * step*step*stepinv;
  stepf3 = step * step * step;
  return stepf0*p[0]  +  stepf1*p[1]  +  stepf2*p[2]  +  stepf3*p[3];
}

void mathBezier3x2f( float *pt, float *p, float step )
{
  float stepinv;
  float stepf0, stepf1, stepf2;
  stepinv = 1 - step;
  stepf0 = stepinv * stepinv;
  stepf1 = 2.0 * stepinv * step;
  stepf2 = step * step;
  pt[0] = stepf0*p[0]  +  stepf1*p[2]  +  stepf2*p[4];
  pt[1] = stepf0*p[1]  +  stepf1*p[3]  +  stepf2*p[5];
  return;
}

void mathBezier4x2f( float *pt, float *p, float step )
{
  float stepinv;
  float stepf0, stepf1, stepf2, stepf3;
  stepinv = 1 - step;
  stepf0 = stepinv * stepinv * stepinv;
  stepf1 = 3.0 * step * stepinv * stepinv;
  stepf2 = 3.0 * step*step*stepinv;
  stepf3 = step * step * step;
  pt[0] = stepf0*p[0]  +  stepf1*p[2]  +  stepf2*p[4]  +  stepf3*p[5];
  pt[1] = stepf0*p[1]  +  stepf1*p[3]  +  stepf2*p[5]  +  stepf3*p[7];
  return;
}



////



float mathBezier3x3x1f( float *p, float u, float v )
{
  float ct[3];
  ct[0] = mathBezier3x1f( &p[0], u );
  ct[1] = mathBezier3x1f( &p[3], u );
  ct[2] = mathBezier3x1f( &p[6], u );
  return mathBezier3x1f( ct, v );
}

float mathBezier4x4x1f( float *p, float u, float v )
{
  float ct[4];
  ct[0] = mathBezier4x1f( &p[0], u );
  ct[1] = mathBezier4x1f( &p[4], u );
  ct[2] = mathBezier4x1f( &p[8], u );
  ct[3] = mathBezier4x1f( &p[12], u );
  return mathBezier4x1f( ct, v );
}

void mathBezier3x3x2f( float *pt, float *p, float u, float v )
{
  float ct[6];
  mathBezier3x2f( &ct[0], &p[0], u );
  mathBezier3x2f( &ct[2], &p[6], u );
  mathBezier3x2f( &ct[4], &p[12], u );
  mathBezier3x2f( pt, ct, v );
  return;
}

void mathBezier4x4x2f( float *pt, float *p, float u, float v )
{
  float ct[8];
  mathBezier4x2f( &ct[0], &p[0], u );
  mathBezier4x2f( &ct[2], &p[8], u );
  mathBezier4x2f( &ct[4], &p[16], u );
  mathBezier4x2f( &ct[6], &p[24], u );
  mathBezier4x2f( pt, ct, v );
  return;
}




////////////////////////////////////////////////////////////////////////////////







void mathMatrixIdentityd( double *a )
{
  memset( a, 0, sizeof(double)*16 );
  a[0*4+0] = a[1*4+1] = a[2*4+2] = a[3*4+3] = 1.0;
  return;
}

void mathMatrixTransposed( double *out, double *m )
{
  int a, b;
  for( a = 0 ; a < 4 ; a++ )
  {
    for( b = 0 ; b < 4; b++ )
      out[a*4+b] = m[b*4+a];
  }
  return;
}

void mathMatrixMulVectord( double *out, double *v, double *m )
{
  out[0] = v[0] * m[0*4+0] + v[1] * m[1*4+0] + v[2] * m[2*4+0];
  out[1] = v[0] * m[0*4+1] + v[1] * m[1*4+1] + v[2] * m[2*4+1];
  out[2] = v[0] * m[0*4+2] + v[1] * m[1*4+2] + v[2] * m[2*4+2];
  return;
}

void mathMatrixMulPointd( double *out, double *v, double *m )
{
  out[0] = v[0] * m[0*4+0] + v[1] * m[1*4+0] + v[2] * m[2*4+0] + m[3*4+0];
  out[1] = v[0] * m[0*4+1] + v[1] * m[1*4+1] + v[2] * m[2*4+1] + m[3*4+1];
  out[2] = v[0] * m[0*4+2] + v[1] * m[1*4+2] + v[2] * m[2*4+2] + m[3*4+2];
  return;
}

void mathMatrixMulPoint4d( double *out, double *v, double *m )
{
  out[0] = v[0] * m[0*4+0] + v[1] * m[1*4+0] + v[2] * m[2*4+0] + v[3] * m[3*4+0];
  out[1] = v[0] * m[0*4+1] + v[1] * m[1*4+1] + v[2] * m[2*4+1] + v[3] * m[3*4+1];
  out[2] = v[0] * m[0*4+2] + v[1] * m[1*4+2] + v[2] * m[2*4+2] + v[3] * m[3*4+2];
  out[3] = v[0] * m[0*4+3] + v[1] * m[1*4+3] + v[2] * m[2*4+3] + v[3] * m[3*4+3];
  return;
}

void mathMatrixMultd( double *out, double *m1, double *m2 )
{
  int i, j;
  for( i = 0 ; i < 4 ; i++ )
  {
    for( j = 0 ; j < 4 ; j++ )
      out[i*4+j] = m1[i*4+0]*m2[0*4+j] + m1[i*4+1]*m2[1*4+j] + m1[i*4+2]*m2[2*4+j] + m1[i*4+3]*m2[3*4+j];
  }
  return;
}

void mathMatrixMultinvd( double *out, double *m1, double *m2 )
{
  int i, j;
  for( i = 0 ; i < 4 ; i++ )
  {
    for( j = 0 ; j < 4 ; j++ )
      out[i*4+j] = m1[i*4+0]*m2[j*4+0] + m1[i*4+1]*m2[j*4+1] + m1[i*4+2]*m2[j*4+2] + m1[i*4+3]*m2[j*4+3];
  }
  return;
}

void mathMatrixInvertd( double *mdst, double *m )
{
  double sx, sy, sz;

#define M(m,x,y) (m)[(x)+4*(y)]

  sx = 1.0 / ( m[0]*m[0] + m[1]*m[1] + m[2]*m[2] );
  sy = 1.0 / ( m[4]*m[4] + m[5]*m[5] + m[6]*m[6] );
  sz = 1.0 / ( m[8]*m[8] + m[9]*m[9] + m[10]*m[10] );
  M(mdst,0,0) = M(m,0,0) * sx;
  M(mdst,0,1) = M(m,1,0) * sx;
  M(mdst,0,2) = M(m,2,0) * sx;
  M(mdst,0,3) = -( M(m,0,3)*M(m,0,0) + M(m,1,3)*M(m,1,0) + M(m,2,3)*M(m,2,0) ) * sx;
  M(mdst,1,0) = M(m,0,1) * sy;
  M(mdst,1,1) = M(m,1,1) * sy;
  M(mdst,1,2) = M(m,2,1) * sy;
  M(mdst,1,3) = -( M(m,0,3)*M(m,0,1) + M(m,1,3)*M(m,1,1) + M(m,2,3)*M(m,2,1) ) * sy;
  M(mdst,2,0) = M(m,0,2) * sz;
  M(mdst,2,1) = M(m,1,2) * sz;
  M(mdst,2,2) = M(m,2,2) * sz;
  M(mdst,2,3) = -( M(m,0,3)*M(m,0,2) + M(m,1,3)*M(m,1,2) + M(m,2,3)*M(m,2,2) ) * sz;
  M(mdst,3,0) = 0.0;
  M(mdst,3,1) = 0.0;
  M(mdst,3,2) = 0.0;
  M(mdst,3,3) = 1.0;

#undef M

  return;
}

void mathMatrixRotd( double *mat, double angle, double x, double y, double z )
{
  double mag, s, c;
  double xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
  double m[16], n[16];

  angle *= ( M_PI / 180.0 );
  s = sin( angle );
  c = cos( angle );
  mag = sqrt( x*x + y*y + z*z );

  if( mag <= 1.0e-4 )
    return;
  x /= mag;
  y /= mag;
  z /= mag;

#define M(row,col)  m[col*4+row]
  xx = x * x;
  yy = y * y;
  zz = z * z;
  xy = x * y;
  yz = y * z;
  zx = z * x;
  xs = x * s;
  ys = y * s;
  zs = z * s;
  one_c = 1.0f - c;
  M(0,0) = (one_c * xx) + c;
  M(0,1) = (one_c * xy) - zs;
  M(0,2) = (one_c * zx) + ys;
  M(0,3) = 0.0f;
  M(1,0) = (one_c * xy) + zs;
  M(1,1) = (one_c * yy) + c;
  M(1,2) = (one_c * yz) - xs;
  M(1,3) = 0.0f;
  M(2,0) = (one_c * zx) - ys;
  M(2,1) = (one_c * yz) + xs;
  M(2,2) = (one_c * zz) + c;
  M(2,3) = 0.0F;
  M(3,0) = 0.0F;
  M(3,1) = 0.0F;
  M(3,2) = 0.0F;
  M(3,3) = 1.0F;
#undef M

  memcpy( n, mat, 16*sizeof(double) );
  mathMatrixMultd( mat, m, n );
  return;
}

void mathMatrixRotxd( double *m, double angle )
{
  double n[16];
  double o[16];
  angle *= ( M_PI / 180.0 );
  mathMatrixIdentityd( n );
  n[1*4+1] =  cos( angle );
  n[1*4+2] =  sin( angle );
  n[2*4+1] = -sin( angle );
  n[2*4+2] =  cos( angle );
  memcpy( o, m, 16*sizeof(double) );
  mathMatrixMultd( m, n, o );
  return;
}

void mathMatrixRotyd( double *m, double angle )
{
  double n[16];
  double o[16];
  angle *= ( M_PI / 180.0 );
  mathMatrixIdentityd( n );
  n[0*4+0] =  cos( angle );
  n[0*4+2] = -sin( angle );
  n[2*4+0] =  sin( angle );
  n[2*4+2] =  cos( angle );
  memcpy( o, m, 16*sizeof(double) );
  mathMatrixMultd( m, n, o );
  return;
}

void mathMatrixRotzd( double *m, double angle )
{
  double n[16];
  double o[16];
  angle *= ( M_PI / 180.0 );
  mathMatrixIdentityd( n );
  n[0*4+0] =  cos( angle );
  n[0*4+1] = -sin( angle );
  n[1*4+0] =  sin( angle );
  n[1*4+1] =  cos( angle );
  memcpy( o, m, 16*sizeof(double) );
  mathMatrixMultd( m, n, o );
  return;
}

void mathMatrixTranslated( double *m, double x, double y, double z )
{
  double n[16];
  double o[16];
  mathMatrixIdentityd( n );
  n[3*4+0] = x;
  n[3*4+1] = y;
  n[3*4+2] = z;
  memcpy( o, m, 16*sizeof(double) );
  mathMatrixMultd( m, n, o );
  return;
}

void mathMatrixScaled( double *m, double x, double y, double z )
{
  double n[16];
  double o[16];
  mathMatrixIdentityd( n );
  n[0*4+0] = x;
  n[1*4+1] = y;
  n[2*4+2] = z;
  memcpy( o, m, 16*sizeof(double) );
  mathMatrixMultd( m, n, o );
  return;
}

void mathMatrixFrustumd( double *m, double left, double right, double bottom, double top, double nearval, double farval )
{
  double x, y, a, b, c, d;
  x = ( 2.0 * nearval ) / ( right - left );
  y = ( 2.0 * nearval ) / ( top - bottom );
  a = ( right + left ) / ( right - left );
  b = ( top + bottom ) / ( top - bottom );
  c = -( farval + nearval ) / ( farval - nearval );
  d = -( 2.0 * farval * nearval ) / ( farval - nearval );

#define M(row,col)  m[col*4+row]
  M(0,0) = x;
  M(0,1) = 0.0F;
  M(0,2) = a;
  M(0,3) = 0.0F;
  M(1,0) = 0.0F;
  M(1,1) = y;
  M(1,2) = b;
  M(1,3) = 0.0F;
  M(2,0) = 0.0F;
  M(2,1) = 0.0F;
  M(2,2) = c;
  M(2,3) = d;
  M(3,0) = 0.0F;
  M(3,1) = 0.0F;
  M(3,2) = -1.0F;
  M(3,3) = 0.0F;
#undef M

  return;
}

#define mathMatrix_FRINFINITY (0.00001)

void mathMatrixFrustumInfinityd( double *m, double left, double right, double bottom, double top, double nearval )
{
  double x, y, a, b, c, d;
  x = ( 2.0 * nearval ) / ( right - left );
  y = ( 2.0 * nearval ) / ( top - bottom );
  a = ( right + left ) / ( right - left );
  b = ( top + bottom ) / ( top - bottom );

  c = mathMatrix_FRINFINITY - 1;
  d = nearval * ( mathMatrix_FRINFINITY - 2 );

#define M(row,col)  m[col*4+row]
  M(0,0) = x;
  M(0,1) = 0.0F;
  M(0,2) = a;
  M(0,3) = 0.0F;
  M(1,0) = 0.0F;
  M(1,1) = y;
  M(1,2) = b;
  M(1,3) = 0.0F;
  M(2,0) = 0.0F;
  M(2,1) = 0.0F;
  M(2,2) = c;
  M(2,3) = d;
  M(3,0) = 0.0F;
  M(3,1) = 0.0F;
  M(3,2) = -1.0F;
  M(3,3) = 0.0F;
#undef M

  return;
}

void mathMatrixPerspectived( double *m, double fovy, double aspect, double zNear, double zFar )
{
  double xmin, xmax, ymin, ymax;
  ymax = zNear * tan(fovy * M_PI / 360.0);
  ymin = -ymax;
  xmin = ymin * aspect;
  xmax = ymax * aspect;
  mathMatrixFrustumd( m, xmin, xmax, ymin, ymax, zNear, zFar );
  return;
}

void mathMatrixPerspectiveInfinityd( double *m, double fovy, double aspect, double zNear )
{
  double xmin, xmax, ymin, ymax;
  ymax = zNear * tan(fovy * M_PI / 360.0);
  ymin = -ymax;
  xmin = ymin * aspect;
  xmax = ymax * aspect;
  mathMatrixFrustumInfinityd( m, xmin, xmax, ymin, ymax, zNear );
  return;
}

void mathMatrixOrthod( double *m, double left, double right, double bottom, double top, double nearval, double farval )
{
  double x, y, z;
  double tx, ty, tz;
  x = 2.0 / ( right - left );
  y = 2.0 / ( top - bottom );
  z = -2.0 / ( farval - nearval );
  tx = -( right + left ) / ( right - left );
  ty = -( top + bottom ) / ( top - bottom );
  tz = -( farval + nearval ) / ( farval - nearval );

#define M(row,col)  m[col*4+row]
  M(0,0) = x;     M(0,1) = 0.0;  M(0,2) = 0.0;  M(0,3) = tx;
  M(1,0) = 0.0;  M(1,1) = y;     M(1,2) = 0.0;  M(1,3) = ty;                                                
  M(2,0) = 0.0;  M(2,1) = 0.0;  M(2,2) = z;     M(2,3) = tz; 
  M(3,0) = 0.0;  M(3,1) = 0.0;  M(3,2) = 0.0;  M(3,3) = 1.0;
#undef M

  return;
}



////



void mathVectorNormalized( double *v )
{
  double f;
  f = 1.0 / sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] );
  v[0] *= f;
  v[1] *= f;
  v[2] *= f;
  return;
}

double mathVectorMagnituded( double *v )
{
  return sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] );
}

void mathVectorNormalizeSafed( double *v )
{
  double f;
  f = sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] );
  if( f < 0.0000001 )
  {
    memset( v, 0, 3*sizeof(double) );
    return;
  }
  f = 1.0 / f;
  v[0] *= f;
  v[1] *= f;
  v[2] *= f;
  return;
}

void mathVectorCrossproductd( double *v, double *v1, double *v2 )
{
  v[0] = ( v1[1] * v2[2] ) - ( v1[2] * v2[1] );
  v[1] = ( v1[2] * v2[0] ) - ( v1[0] * v2[2] );
  v[2] = ( v1[0] * v2[1] ) - ( v1[1] * v2[0] );
  return;
}

double mathVectorDotProductd( double *v1, double *v2 )
{
  return ( v1[0] * v2[0] ) + ( v1[1] * v2[1] ) + ( v1[2] * v2[2] );
}

void mathVectorReflectd( double *v, double *rayvector, double *normal )
{
  double dotpr;
  dotpr = 2.0 * mathVectorDotProductd( rayvector, normal );
  mathVectorNormalized( rayvector );
  v[0] = rayvector[0] - dotpr * normal[0];
  v[1] = rayvector[1] - dotpr * normal[1];
  v[2] = rayvector[2] - dotpr * normal[2];
  mathVectorNormalized( v );
  return;
}



////



void mathPlaneNormd( double *p )
{
  double f = 1.0 / sqrt( p[0]*p[0] + p[1]*p[1] + p[2]*p[2] );
  p[0] *= f;
  p[1] *= f;
  p[2] *= f;
  p[3] *= f;
  return;
}

void mathPlaneEquationd( double *eq, double *v1, double *v2, double *v3 )
{
  double px, py, pz, qx, qy, qz;
  qx = v2[0] - v1[0];
  qy = v2[1] - v1[1];
  qz = v2[2] - v1[2];
  px = v3[0] - v1[0];
  py = v3[1] - v1[1];
  pz = v3[2] - v1[2];
  eq[0] = pz*qy - py*qz;
  eq[1] = px*qz - pz*qx;
  eq[2] = py*qx - px*qy;
  mathVectorNormalized( eq );
  eq[3] = -( eq[0]*v1[0] + eq[1]*v1[1] + eq[2]*v1[2] );
  return;
}

void mathPlaneNormald( double *n, double *v1, double *v2, double *v3 )
{
  double px, py, pz, qx, qy, qz;
  qx = v2[0] - v1[0];
  qy = v2[1] - v1[1];
  qz = v2[2] - v1[2];
  px = v3[0] - v1[0];
  py = v3[1] - v1[1];
  pz = v3[2] - v1[2];
  n[0] = pz*qy - py*qz;
  n[1] = px*qz - pz*qx;
  n[2] = py*qx - px*qy;
  return;
}

// return 6 planes, 24 doubles
void mathPlaneFrustumd( double *out, double *proj, double *modl )
{
  double m[16];

  mathMatrixMultd( m, modl, proj );

  // near
  out[0*4+0] = m[ 3] + m[ 2];
  out[0*4+1] = m[ 7] + m[ 6];
  out[0*4+2] = m[11] + m[10];
  out[0*4+3] = m[15] + m[14];
  mathPlaneNormd( &out[0*4] );

  // right
  out[1*4+0] = m[ 3] - m[ 0];
  out[1*4+1] = m[ 7] - m[ 4];
  out[1*4+2] = m[11] - m[ 8];
  out[1*4+3] = m[15] - m[12];
  mathPlaneNormd( &out[1*4] );

  // left
  out[2*4+0] = m[ 3] + m[ 0];
  out[2*4+1] = m[ 7] + m[ 4];
  out[2*4+2] = m[11] + m[ 8];
  out[2*4+3] = m[15] + m[12];
  mathPlaneNormd( &out[2*4] );

  // bottom
  out[3*4+0] = m[ 3] + m[ 1];
  out[3*4+1] = m[ 7] + m[ 5];
  out[3*4+2] = m[11] + m[ 9];
  out[3*4+3] = m[15] + m[13];
  mathPlaneNormd( &out[3*4] );

  // top
  out[4*4+0] = m[ 3] - m[ 1];
  out[4*4+1] = m[ 7] - m[ 5];
  out[4*4+2] = m[11] - m[ 9];
  out[4*4+3] = m[15] - m[13];
  mathPlaneNormd( &out[4*4] );

  // far
  out[5*4+0] = m[ 3] - m[ 2];
  out[5*4+1] = m[ 7] - m[ 6];
  out[5*4+2] = m[11] - m[10];
  out[5*4+3] = m[15] - m[14];
  mathPlaneNormd( &out[5*4] );

  return;
}

double mathPlanePointd( double *plane, double *pt )
{
  return pt[0]*plane[0] + pt[1]*plane[1] + pt[2]*plane[2] + plane[3];
}

double mathPlanePointInvd( double *plane, double *pt )
{
  return -pt[0]*plane[0] + -pt[1]*plane[1] + -pt[2]*plane[2] + plane[3];
}

void mathPlaneIntersect3d( double *pt, double *p0, double *p1, double *p2 )
{
  double d;
  double n0[3], n1[3], n2[3];
  _mathVectorCrossproduct( n0, &p1[0], &p2[0] );
  d = -1.0 / _mathVectorDotProduct( p0, n0 );
  _mathVectorCrossproduct( n1, &p2[0], &p0[0] );
  _mathVectorCrossproduct( n2, &p0[0], &p1[0] );
  _mathVectorMulScalar( n0, p0[3] );
  _mathVectorMulScalar( n1, p1[3] );
  _mathVectorMulScalar( n2, p2[3] );
  pt[0] = ( n0[0] + n1[0] + n2[0] ) * d;
  pt[1] = ( n0[1] + n1[1] + n2[1] ) * d;
  pt[2] = ( n0[2] + n1[2] + n2[2] ) * d;
  return;
}



////



double mathPointDistd( double *p0, double *p1 )
{
  double p[3];
  p[0] = p0[0] - p1[0];
  p[1] = p0[1] - p1[1];
  p[2] = p0[2] - p1[2];
  return sqrt( p[0]*p[0] + p[1]*p[1] + p[2]*p[2] );
}



////



void mathQuatIdentityd( double *quat )
{
  quat[0] = 1.0;
  quat[1] = 0.0;
  quat[2] = 0.0;
  quat[3] = 0.0;
  return;
}

void mathQuatInvertd( double *out, double *quat )
{
  out[0] = -quat[0];
  out[1] = -quat[1];
  out[2] = -quat[2];
  out[3] = quat[3];
  return;
}

void mathQuatNormalized( double *quat )
{
  double f;
  double facteur = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3];
  f = 1.0 / sqrt( facteur );
  quat[0] = quat[0] * f;
  quat[1] = quat[1] * f;
  quat[2] = quat[2] * f;
  quat[3] = quat[3] * f;
  return;
}

void mathQuatVectord( double *quat, double *v )
{
  double w = quat[0];
  double x = quat[1];
  double y = quat[2];
  double z = quat[3];
  mathQuatNormalized( quat );
  v[0] = 2.0 * ( x * z - w * y );
  v[1] = 2.0 * ( y * z + w * x );
  v[2] = 1.0 - 2.0 * ( x * x + y * y );
  return;
}

void mathQuatMatrixd( double *quat, double *matrix )
{
  double w, x, y, z, xx, yy, zz;
  mathQuatNormalized( quat );
  w = quat[0];
  x = quat[1];
  y = quat[2];
  z = quat[3];
  xx = x * x;
  yy = y * y;
  zz = z * z;
  matrix[0*4+0] = 1.0 - 2.0 * ( yy + zz );
  matrix[0*4+1] = 2.0 * ( x * y + w * z );
  matrix[0*4+2] = 2.0 * ( x * z - w * y );
  matrix[0*4+3] = 0.0;
  matrix[1*4+0] = 2.0 * ( x * y - w * z );
  matrix[1*4+1] = 1.0 - 2.0 * ( xx + zz );
  matrix[1*4+2] = 2.0 * ( y * z + w * x );
  matrix[1*4+3] = 0.0;
  matrix[2*4+0] = 2.0 * ( x * z + w * y );
  matrix[2*4+1] = 2.0 * ( y * z - w * x );
  matrix[2*4+2] = 1.0 - 2.0 * ( xx + yy );
  matrix[2*4+3] = 0.0;
  matrix[3*4+0] = 0.0;
  matrix[3*4+1] = 0.0;
  matrix[3*4+2] = 0.0;
  matrix[3*4+3] = 1.0;
  return;
}

void mathQuatTransMatrixd( double *quat, double *matrix )
{
  double w, x, y, z, xx, yy, zz;
  mathQuatNormalized( quat );
  w = quat[0];
  x = quat[1];
  y = quat[2];
  z = quat[3];
  xx = x * x;
  yy = y * y;
  zz = z * z;
  matrix[0*4+0] = 1.0 - 2.0 * ( yy + zz );
  matrix[1*4+0] = 2.0 * ( x * y + w * z );
  matrix[2*4+0] = 2.0 * ( x * z - w * y );
  matrix[3*4+0] = 0.0;
  matrix[0*4+1] = 2.0 * ( x * y - w * z );
  matrix[1*4+1] = 1.0 - 2.0 * ( xx + zz );
  matrix[2*4+1] = 2.0 * ( y * z + w * x );
  matrix[3*4+1] = 0.0;
  matrix[0*4+2] = 2.0 * ( x * z + w * y );
  matrix[1*4+2] = 2.0 * ( y * z - w * x );
  matrix[2*4+2] = 1.0 - 2.0 * ( xx + yy );
  matrix[3*4+2] = 0.0;
  matrix[0*4+3] = 0.0;
  matrix[1*4+3] = 0.0;
  matrix[2*4+3] = 0.0;
  matrix[3*4+3] = 1.0;
  return;
}


void mathQuatLoadMatrixd( double *m, double *quat )
{
  double tr, s, q[4];
  int i, j, k;
  int nxt[3] = {1, 2, 0};
  tr = m[0*4+0] + m[1*4+1] + m[2*4+2];
  if( tr > 0.0 )
  {
    s = sqrt( tr + 1.0 );
    quat[0] = s / 2.0;
    s = 0.5 / s;
    quat[1] = ( m[1*4+2] - m[2*4+1] ) * s;
    quat[2] = ( m[2*4+0] - m[0*4+2] ) * s;
    quat[3] = ( m[0*4+1] - m[1*4+0] ) * s;
  }
  else {
    i = 0;
    if( m[1*4+1] > m[0*4+0] )
      i = 1;
    if( m[2*4+2] > m[i*4+i] )
      i = 2;
    j = nxt[i];
    k = nxt[j];
    s = sqrt( ( m[i*4+i] - ( m[j*4+j] + m[k*4+k] ) ) + 1.0 );
    q[i] = s * 0.5;
    if( s != 0.0 )
      s = 0.5 / s;
    q[3] = ( m[j*4+k] - m[k*4+j] ) * s;
    q[j] = ( m[i*4+j] + m[j*4+i] ) * s;
    q[k] = ( m[i*4+k] + m[k*4+i] ) * s;
    /// hmm... oh well
    quat[1] = q[0];
    quat[2] = q[1];
    quat[3] = q[2];
    quat[0] = q[3];
  }
  return;
}



void mathQuatMultResd( double *quat, double *quat1, double *quat2 )
{
  quat[0] = quat2[0]*quat1[0] - quat2[1]*quat1[1] - quat2[2]*quat1[2] - quat2[3]*quat1[3];
  quat[1] = quat2[0]*quat1[1] + quat2[1]*quat1[0] + quat2[2]*quat1[3] - quat2[3]*quat1[2];
  quat[2] = quat2[0]*quat1[2] - quat2[1]*quat1[3] + quat2[2]*quat1[0] + quat2[3]*quat1[1];
  quat[3] = quat2[0]*quat1[3] + quat2[1]*quat1[2] - quat2[2]*quat1[1] + quat2[3]*quat1[0];
  return;
}

void mathQuatMultd( double *quat, double *quatMult )
{
  double quat_temp[4];
  memcpy( quat_temp, quat, 4 * sizeof(double) );
  mathQuatMultResd( quat, quat_temp, quatMult );
  return;
}

void mathQuatBuildd( double *quat, double angle, double x, double y, double z )
{
  double a, b, c;
  angle *= ( M_PI / 180.0 );
  b = x*x + y*y + z*z;
  if( b != 0 )
    a = ( 1.0 / sqrt( b ) );
  else
    a = 32768.0;
  x = x * a;
  y = y * a;
  z = z * a;
  c = sin( angle / 2.0 );
  quat[0] = cos( angle / 2.0 );
  quat[1] = x * c;
  quat[2] = y * c;
  quat[3] = z * c;
  return;
}

void mathQuatDefined( double *quat, double x, double y, double z )
{
  double quat_x[4];
  double quat_y[4];
  double quat_z[4];
  mathQuatBuildd( quat_x, x, 1.0, 0.0, 0.0 );
  mathQuatBuildd( quat_y, y, 0.0, 1.0, 0.0 );
  mathQuatBuildd( quat_z, z, 0.0, 0.0, 1.0 );
  memcpy( quat, quat_x, 4 * sizeof(double) );
  mathQuatMultd( quat, quat_y );
  mathQuatMultd( quat, quat_z );
  return;
}

#define DEF_DELTA 0.00001

void mathQuatInterd( double *out, double slerp, double *quat1, double *quat2 )
{
  int a;
  double quat1t[4], quat2t[4], omega, cosom, sinom, scale0, scale1;
  cosom = quat1[0] * quat2[0] + quat1[1] * quat2[1] + quat1[2] * quat2[2] + quat1[3] * quat2[3];
  memcpy( quat1t, quat1, 4 * sizeof(double) );
  memcpy( quat2t, quat2, 4 * sizeof(double) );
  if( cosom < 0.0 )
  {
    cosom = -cosom;
    for( a = 0 ; a < 4 ; a++ )
      quat1t[a] = -quat1t[a];
  }
  if( ( 1.0 - cosom ) > DEF_DELTA )
  {
    omega = acos( cosom );
    sinom = sin( omega );
    scale0 = sin( ( 1 - slerp ) * omega ) / sinom;
    scale1 = sin( slerp * omega ) / sinom;
  }
  else {
    scale0 = 1.0 - slerp;
    scale1 = slerp;
  }
  for( a = 0 ; a < 4 ; a++ )
    out[a] = scale0 * quat1t[a] + scale1 * quat2t[a];
  return;
}



////



void mathBezier3d( double *pt, double *p0, double *p1, double *p2, double step, int dims )
{
  int a;
  double stepinv;
  double stepf0, stepf1, stepf2;
  stepinv = 1 - step;
  stepf0 = stepinv * stepinv;
  stepf1 = 2.0 * stepinv * step;
  stepf2 = step * step;
  for( a = 0 ; a < dims ; a++ )
    pt[a] = p0[a]*stepf0  +  p1[a]*stepf1  +  p2[a]*stepf2;
  return;
}


void mathBezier4d( double *pt, double *p0, double *p1, double *p2, double *p3, double step, int dims )
{
  int a;
  double stepinv;
  double stepf0, stepf1, stepf2, stepf3;
  stepinv = 1 - step;
  stepf0 = stepinv * stepinv * stepinv;
  stepf1 = 3.0 * step * stepinv * stepinv;
  stepf2 = 3.0 * step * step * stepinv;
  stepf3 = step * step * step;
  for( a = 0 ; a < dims ; a++ )
    pt[a] = stepf0*p0[a]  +  stepf1*p1[a]  +  stepf2*p2[a]  +  stepf3*p3[a];
  return;
}


void mathBezier5d( double *pt, double *p0, double *p1, double *p2, double *p3, double *p4, double step, int dims )
{
  int a;
  double stepinv;
  double stepf0, stepf1, stepf2, stepf3, stepf4;
  stepinv = 1 - step;
  stepf0 = stepinv * stepinv * stepinv * stepinv;
  stepf1 = 4.0 * step * stepinv * stepinv * stepinv;
  stepf2 = 5.0 * step * step * stepinv * stepinv;
  stepf3 = 4.0 * step * step * step * stepinv;
  stepf4 = step * step * step * step;
  for( a = 0 ; a < dims ; a++ )
    pt[a] = stepf0*p0[a]  +  stepf1*p1[a]  +  stepf2*p2[a]  +  stepf3*p3[a]  +  stepf4*p4[a];
  return;
}


void mathBezier6d( double *pt, double *p0, double *p1, double *p2, double *p3, double *p4, double *p5, double step, int dims )
{
  int a;
  double stepinv;
  double stepf0, stepf1, stepf2, stepf3, stepf4, stepf5;
  stepinv = 1 - step;
  stepf0 = stepinv * stepinv * stepinv * stepinv * stepinv;;
  stepf1 = 5.0 * step * stepinv * stepinv * stepinv * stepinv;;
  stepf2 = 9.0 * step * step * stepinv * stepinv * stepinv;
  stepf3 = 9.0 * step * step * step * stepinv * stepinv;
  stepf4 = 5.0 * step * step * step * step * stepinv;
  stepf5 = step * step * step * step * step;
  for( a = 0 ; a < dims ; a++ )
    pt[a] = stepf0*p0[a]  +  stepf1*p1[a]  +  stepf2*p2[a]  +  stepf3*p3[a]  +  stepf4*p4[a]  +  stepf5*p5[a];
  return;
}



////



double mathBezier3x1d( double *p, double step )
{
  double stepinv;
  double stepf0, stepf1, stepf2;
  stepinv = 1 - step;
  stepf0 = stepinv * stepinv;
  stepf1 = 2.0 * stepinv * step;
  stepf2 = step * step;
  return stepf0*p[0]  +  stepf1*p[1]  +  stepf2*p[2];
}

double mathBezier4x1d( double *p, double step )
{
  double stepinv;
  double stepf0, stepf1, stepf2, stepf3;
  stepinv = 1 - step;
  stepf0 = stepinv * stepinv * stepinv;
  stepf1 = 3.0 * step * stepinv * stepinv;
  stepf2 = 3.0 * step*step*stepinv;
  stepf3 = step * step * step;
  return stepf0*p[0]  +  stepf1*p[1]  +  stepf2*p[2]  +  stepf3*p[3];
}

void mathBezier3x2d( double *pt, double *p, double step )
{
  double stepinv;
  double stepf0, stepf1, stepf2;
  stepinv = 1 - step;
  stepf0 = stepinv * stepinv;
  stepf1 = 2.0 * stepinv * step;
  stepf2 = step * step;
  pt[0] = stepf0*p[0]  +  stepf1*p[2]  +  stepf2*p[4];
  pt[1] = stepf0*p[1]  +  stepf1*p[3]  +  stepf2*p[5];
  return;
}

void mathBezier4x2d( double *pt, double *p, double step )
{
  double stepinv;
  double stepf0, stepf1, stepf2, stepf3;
  stepinv = 1 - step;
  stepf0 = stepinv * stepinv * stepinv;
  stepf1 = 3.0 * step * stepinv * stepinv;
  stepf2 = 3.0 * step*step*stepinv;
  stepf3 = step * step * step;
  pt[0] = stepf0*p[0]  +  stepf1*p[2]  +  stepf2*p[4]  +  stepf3*p[5];
  pt[1] = stepf0*p[1]  +  stepf1*p[3]  +  stepf2*p[5]  +  stepf3*p[7];
  return;
}



////



double mathBezier3x3x1d( double *p, double u, double v )
{
  double ct[3];
  ct[0] = mathBezier3x1d( &p[0], u );
  ct[1] = mathBezier3x1d( &p[3], u );
  ct[2] = mathBezier3x1d( &p[6], u );
  return mathBezier3x1d( ct, v );
}

double mathBezier4x4x1d( double *p, double u, double v )
{
  double ct[4];
  ct[0] = mathBezier4x1d( &p[0], u );
  ct[1] = mathBezier4x1d( &p[4], u );
  ct[2] = mathBezier4x1d( &p[8], u );
  ct[3] = mathBezier4x1d( &p[12], u );
  return mathBezier4x1d( ct, v );
}

void mathBezier3x3x2d( double *pt, double *p, double u, double v )
{
  double ct[6];
  mathBezier3x2d( &ct[0], &p[0], u );
  mathBezier3x2d( &ct[2], &p[6], u );
  mathBezier3x2d( &ct[4], &p[12], u );
  mathBezier3x2d( pt, ct, v );
  return;
}

void mathBezier4x4x2d( double *pt, double *p, double u, double v )
{
  double ct[8];
  mathBezier4x2d( &ct[0], &p[0], u );
  mathBezier4x2d( &ct[2], &p[8], u );
  mathBezier4x2d( &ct[4], &p[16], u );
  mathBezier4x2d( &ct[6], &p[24], u );
  mathBezier4x2d( pt, ct, v );
  return;
}