== Constraints System ==
[blender-staging.git] / source / blender / blenlib / intern / arithb.c
index f73005dcd9b2c48216d2f165359b9ade043951c4..125c24a7ada418525a0ebffa496e080ae7a5fa08 100644 (file)
@@ -4,9 +4,6 @@
  *
  * sort of cleaned up mar-01 nzc
  *
- * Functions here get counterparts with MTC prefixes. Basically, we phase
- * out the calls here in favour of fully prototyped versions.
- *
  * $Id$
  *
  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
 #include <unistd.h>
 #endif
 
-#ifdef WIN32
-#include "BLI_winstuff.h"
-#endif
-
 #include <stdio.h>
 #include "BLI_arithb.h"
 
@@ -86,18 +79,25 @@ float saacos(float fac)
        else return (float)acos(fac);
 }
 
+float saasin(float fac)
+{
+       if(fac<= -1.0f) return (float)-M_PI/2.0f;
+       else if(fac>=1.0f) return (float)M_PI/2.0f;
+       else return (float)asin(fac);
+}
+
 float sasqrt(float fac)
 {
        if(fac<=0.0) return 0.0;
        return (float)sqrt(fac);
 }
 
-float Normalise(float *n)
+float Normalize(float *n)
 {
        float d;
        
        d= n[0]*n[0]+n[1]*n[1]+n[2]*n[2];
-       /* A larger value causes normalise errors in a scaled down models with camera xtreme close */
+       /* A larger value causes normalize errors in a scaled down models with camera xtreme close */
        if(d>1.0e-35F) {
                d= (float)sqrt(d);
 
@@ -118,6 +118,7 @@ void Crossf(float *c, float *a, float *b)
        c[2] = a[0] * b[1] - a[1] * b[0];
 }
 
+/* Inpf returns the dot product, also called the scalar product and inner product */
 float Inpf( float *v1, float *v2)
 {
        return v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2];
@@ -759,7 +760,28 @@ void Mat4MulSerie(float answ[][4], float m1[][4],
        }
 }
 
+void Mat4BlendMat4(float out[][4], float dst[][4], float src[][4], float srcweight)
+{
+       float squat[4], dquat[4], fquat[4];
+       float ssize[3], dsize[3], fsize[4];
+       float sloc[3], dloc[3], floc[3];
+       
+       Mat4ToQuat(dst, dquat);
+       Mat4ToSize(dst, dsize);
+       VecCopyf(dloc, dst[3]);
+
+       Mat4ToQuat(src, squat);
+       Mat4ToSize(src, ssize);
+       VecCopyf(sloc, src[3]);
+       
+       /* do blending */
+       VecLerpf(floc, dloc, sloc, srcweight);
+       QuatInterpol(fquat, dquat, squat, srcweight);
+       VecLerpf(fsize, dsize, ssize, srcweight);
 
+       /* compose new matrix */
+       LocQuatSizeToMat4(out, floc, fquat, fsize);
+}
 
 void Mat4Clr(float *m)
 {
@@ -834,6 +856,18 @@ void Mat4Mul3Vecfl( float mat[][4], float *vec)
        vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
 }
 
+void Mat4MulVec3Project(float mat[][4], float *vec)
+{
+       float w;
+
+       w = vec[0]*mat[0][3] + vec[1]*mat[1][3] + vec[2]*mat[2][3] + mat[3][3];
+       Mat4MulVecfl(mat, vec);
+
+       vec[0] /= w;
+       vec[1] /= w;
+       vec[2] /= w;
+}
+
 void Mat4MulVec4fl( float mat[][4], float *vec)
 {
        float x,y,z;
@@ -953,10 +987,27 @@ int FloatCompare( float *v1,  float *v2, float limit)
        return 0;
 }
 
+float FloatLerpf( float target, float origin, float fac)
+{
+       return (fac*target) + (1.0f-fac)*origin;
+}
+
 void printvecf( char *str,  float v[3])
+{
+       printf("%s: %.3f %.3f %.3f\n", str, v[0], v[1], v[2]);
+
+}
+
+void printquat( char *str,  float q[4])
+{
+       printf("%s: %.3f %.3f %.3f %.3f\n", str, q[0], q[1], q[2], q[3]);
+
+}
+
+void printvec4f( char *str,  float v[4])
 {
        printf("%s\n", str);
-       printf("%f %f %f\n",v[0],v[1],v[2]);
+       printf("%f %f %f %f\n",v[0],v[1],v[2], v[3]);
        printf("\n");
 
 }
@@ -964,10 +1015,10 @@ void printvecf( char *str,  float v[3])
 void printmatrix4( char *str,  float m[][4])
 {
        printf("%s\n", str);
-       printf("%f %f %f %f\n",m[0][0],m[0][1],m[0][2],m[0][3]);
-       printf("%f %f %f %f\n",m[1][0],m[1][1],m[1][2],m[1][3]);
-       printf("%f %f %f %f\n",m[2][0],m[2][1],m[2][2],m[2][3]);
-       printf("%f %f %f %f\n",m[3][0],m[3][1],m[3][2],m[3][3]);
+       printf("%f %f %f %f\n",m[0][0],m[1][0],m[2][0],m[3][0]);
+       printf("%f %f %f %f\n",m[0][1],m[1][1],m[2][1],m[3][1]);
+       printf("%f %f %f %f\n",m[0][2],m[1][2],m[2][2],m[3][2]);
+       printf("%f %f %f %f\n",m[0][3],m[1][3],m[2][3],m[3][3]);
        printf("\n");
 
 }
@@ -975,9 +1026,9 @@ void printmatrix4( char *str,  float m[][4])
 void printmatrix3( char *str,  float m[][3])
 {
        printf("%s\n", str);
-       printf("%f %f %f\n",m[0][0],m[0][1],m[0][2]);
-       printf("%f %f %f\n",m[1][0],m[1][1],m[1][2]);
-       printf("%f %f %f\n",m[2][0],m[2][1],m[2][2]);
+       printf("%f %f %f\n",m[0][0],m[1][0],m[2][0]);
+       printf("%f %f %f\n",m[0][1],m[1][1],m[2][1]);
+       printf("%f %f %f\n",m[0][2],m[1][2],m[2][2]);
        printf("\n");
 
 }
@@ -998,6 +1049,56 @@ void QuatMul(float *q, float *q1, float *q2)
        q[2]=t2;
 }
 
+/* Assumes a unit quaternion */
+void QuatMulVecf(float *q, float *v)
+{
+       float t0, t1, t2;
+
+       t0=  -q[1]*v[0]-q[2]*v[1]-q[3]*v[2];
+       t1=   q[0]*v[0]+q[2]*v[2]-q[3]*v[1];
+       t2=   q[0]*v[1]+q[3]*v[0]-q[1]*v[2];
+       v[2]= q[0]*v[2]+q[1]*v[1]-q[2]*v[0];
+       v[0]=t1; 
+       v[1]=t2;
+
+       t1=   t0*-q[1]+v[0]*q[0]-v[1]*q[3]+v[2]*q[2];
+       t2=   t0*-q[2]+v[1]*q[0]-v[2]*q[1]+v[0]*q[3];
+       v[2]= t0*-q[3]+v[2]*q[0]-v[0]*q[2]+v[1]*q[1];
+       v[0]=t1; 
+       v[1]=t2;
+}
+
+void QuatConj(float *q)
+{
+       q[1] = -q[1];
+       q[2] = -q[2];
+       q[3] = -q[3];
+}
+
+float QuatDot(float *q1, float *q2)
+{
+       return q1[0]*q2[0] + q1[1]*q2[1] + q1[2]*q2[2] + q1[3]*q2[3];
+}
+
+void QuatInv(float *q)
+{
+       float f = QuatDot(q, q);
+
+       if (f == 0.0f)
+               return;
+
+       QuatConj(q);
+       QuatMulf(q, 1.0f/f);
+}
+
+void QuatMulf(float *q, float f)
+{
+       q[0] *= f;
+       q[1] *= f;
+       q[2] *= f;
+       q[3] *= f;
+}
+
 void QuatSub(float *q, float *q1, float *q2)
 {
        q2[0]= -q2[0];
@@ -1126,7 +1227,7 @@ void Mat3ToQuat( float wmat[][3], float *q)               /* from Sig.Proc.85 pag 253 */
 
 void Mat3ToQuat_is_ok( float wmat[][3], float *q)
 {
-       float mat[3][3], matr[3][3], matn[3][3], q1[4], q2[4], hoek, si, co, nor[3];
+       float mat[3][3], matr[3][3], matn[3][3], q1[4], q2[4], angle, si, co, nor[3];
 
        /* work on a copy */
        Mat3CpyMat3(mat, wmat);
@@ -1137,13 +1238,13 @@ void Mat3ToQuat_is_ok( float wmat[][3], float *q)
        nor[0] = mat[2][1];             /* cross product with (0,0,1) */
        nor[1] =  -mat[2][0];
        nor[2] = 0.0;
-       Normalise(nor);
+       Normalize(nor);
        
        co= mat[2][2];
-       hoek= 0.5f*saacos(co);
+       angle= 0.5f*saacos(co);
        
-       co= (float)cos(hoek);
-       si= (float)sin(hoek);
+       co= (float)cos(angle);
+       si= (float)sin(angle);
        q1[0]= co;
        q1[1]= -nor[0]*si;              /* negative here, but why? */
        q1[2]= -nor[1]*si;
@@ -1155,10 +1256,10 @@ void Mat3ToQuat_is_ok( float wmat[][3], float *q)
        Mat3MulVecfl(matn, mat[0]);
        
        /* and align x-axes */
-       hoek= (float)(0.5*atan2(mat[0][1], mat[0][0]));
+       angle= (float)(0.5*atan2(mat[0][1], mat[0][0]));
        
-       co= (float)cos(hoek);
-       si= (float)sin(hoek);
+       co= (float)cos(angle);
+       si= (float)sin(angle);
        q2[0]= co;
        q2[1]= 0.0f;
        q2[2]= 0.0f;
@@ -1202,7 +1303,7 @@ void NormalQuat(float *q)
 float *vectoquat( float *vec, short axis, short upflag)
 {
        static float q1[4];
-       float q2[4], nor[3], *fp, mat[3][3], hoek, si, co, x2, y2, z2, len1;
+       float q2[4], nor[3], *fp, mat[3][3], angle, si, co, x2, y2, z2, len1;
        
        /* first rotate to axis */
        if(axis>2) {    
@@ -1258,11 +1359,11 @@ float *vectoquat( float *vec, short axis, short upflag)
        }
        co/= len1;
 
-       Normalise(nor);
+       Normalize(nor);
        
-       hoek= 0.5f*saacos(co);
-       si= (float)sin(hoek);
-       q1[0]= (float)cos(hoek);
+       angle= 0.5f*saacos(co);
+       si= (float)sin(angle);
+       q1[0]= (float)cos(angle);
        q1[1]= nor[0]*si;
        q1[2]= nor[1]*si;
        q1[3]= nor[2]*si;
@@ -1272,20 +1373,20 @@ float *vectoquat( float *vec, short axis, short upflag)
 
                fp= mat[2];
                if(axis==0) {
-                       if(upflag==1) hoek= (float)(0.5*atan2(fp[2], fp[1]));
-                       else hoek= (float)(-0.5*atan2(fp[1], fp[2]));
+                       if(upflag==1) angle= (float)(0.5*atan2(fp[2], fp[1]));
+                       else angle= (float)(-0.5*atan2(fp[1], fp[2]));
                }
                else if(axis==1) {
-                       if(upflag==0) hoek= (float)(-0.5*atan2(fp[2], fp[0]));
-                       else hoek= (float)(0.5*atan2(fp[0], fp[2]));
+                       if(upflag==0) angle= (float)(-0.5*atan2(fp[2], fp[0]));
+                       else angle= (float)(0.5*atan2(fp[0], fp[2]));
                }
                else {
-                       if(upflag==0) hoek= (float)(0.5*atan2(-fp[1], -fp[0]));
-                       else hoek= (float)(-0.5*atan2(-fp[0], -fp[1]));
+                       if(upflag==0) angle= (float)(0.5*atan2(-fp[1], -fp[0]));
+                       else angle= (float)(-0.5*atan2(-fp[0], -fp[1]));
                }
                                
-               co= (float)cos(hoek);
-               si= (float)(sin(hoek)/len1);
+               co= (float)cos(angle);
+               si= (float)(sin(angle)/len1);
                q2[0]= co;
                q2[1]= x2*si;
                q2[2]= y2*si;
@@ -1331,14 +1432,14 @@ void VecUpMat3old( float *vec, float mat[][3], short axis)
        mat[coz][0]= vec[0];
        mat[coz][1]= vec[1];
        mat[coz][2]= vec[2];
-       Normalise((float *)mat[coz]);
+       Normalize((float *)mat[coz]);
        
        inp= mat[coz][0]*up[0] + mat[coz][1]*up[1] + mat[coz][2]*up[2];
        mat[coy][0]= up[0] - inp*mat[coz][0];
        mat[coy][1]= up[1] - inp*mat[coz][1];
        mat[coy][2]= up[2] - inp*mat[coz][2];
 
-       Normalise((float *)mat[coy]);
+       Normalize((float *)mat[coy]);
        
        Crossf(mat[cox], mat[coy], mat[coz]);
        
@@ -1377,20 +1478,21 @@ void VecUpMat3(float *vec, float mat[][3], short axis)
        mat[coz][0]= vec[0];
        mat[coz][1]= vec[1];
        mat[coz][2]= vec[2];
-       Normalise((float *)mat[coz]);
+       Normalize((float *)mat[coz]);
        
        inp= mat[coz][2];
        mat[coy][0]= - inp*mat[coz][0];
        mat[coy][1]= - inp*mat[coz][1];
        mat[coy][2]= 1.0f - inp*mat[coz][2];
 
-       Normalise((float *)mat[coy]);
+       Normalize((float *)mat[coy]);
        
        Crossf(mat[cox], mat[coy], mat[coz]);
        
 }
 
 /* A & M Watt, Advanced animation and rendering techniques, 1992 ACM press */
+void QuatInterpolW(float *, float *, float *, float );
 
 void QuatInterpolW(float *result, float *quat1, float *quat2, float t)
 {
@@ -1667,26 +1769,25 @@ void i_lookat(float vx, float vy, float vz, float px, float py, float pz, float
 
 void Mat3Ortho(float mat[][3])
 {      
-       Normalise(mat[0]);
-       Normalise(mat[1]);
-       Normalise(mat[2]);
+       Normalize(mat[0]);
+       Normalize(mat[1]);
+       Normalize(mat[2]);
 }
 
 void Mat4Ortho(float mat[][4])
 {
        float len;
        
-       len= Normalise(mat[0]);
+       len= Normalize(mat[0]);
        if(len!=0.0) mat[0][3]/= len;
-       len= Normalise(mat[1]);
+       len= Normalize(mat[1]);
        if(len!=0.0) mat[1][3]/= len;
-       len= Normalise(mat[2]);
+       len= Normalize(mat[2]);
        if(len!=0.0) mat[2][3]/= len;
 }
 
 void VecCopyf(float *v1, float *v2)
 {
-
        v1[0]= v2[0];
        v1[1]= v2[1];
        v1[2]= v2[2];
@@ -1712,6 +1813,11 @@ float VecLenf( float *v1, float *v2)
        return (float)sqrt(x*x+y*y+z*z);
 }
 
+float VecLength(float *v)
+{
+       return (float) sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
+}
+
 void VecAddf(float *v, float *v1, float *v2)
 {
        v[0]= v1[0]+ v2[0];
@@ -1726,6 +1832,15 @@ void VecSubf(float *v, float *v1, float *v2)
        v[2]= v1[2]- v2[2];
 }
 
+void VecLerpf(float *target, float *a, float *b, float t)
+{
+       float s = 1.0f-t;
+
+       target[0]= s*a[0] + t*b[0];
+       target[1]= s*a[1] + t*b[1];
+       target[2]= s*a[2] + t*b[2];
+}
+
 void VecMidf(float *v, float *v1, float *v2)
 {
        v[0]= 0.5f*(v1[0]+ v2[0]);
@@ -1741,6 +1856,17 @@ void VecMulf(float *v1, float f)
        v1[2]*= f;
 }
 
+int VecLenCompare(float *v1, float *v2, float limit)
+{
+    float x,y,z;
+
+       x=v1[0]-v2[0];
+       y=v1[1]-v2[1];
+       z=v1[2]-v2[2];
+
+       return ((x*x + y*y + z*z) < (limit*limit));
+}
+
 int VecCompare( float *v1, float *v2, float limit)
 {
        if( fabs(v1[0]-v2[0])<limit )
@@ -1749,6 +1875,11 @@ int VecCompare( float *v1, float *v2, float limit)
        return 0;
 }
 
+int VecEqual(float *v1, float *v2)
+{
+       return ((v1[0]==v2[0]) && (v1[1]==v2[1]) && (v1[2]==v2[2]));
+}
+
 void CalcNormShort( short *v1, short *v2, short *v3, float *n) /* is also cross product */
 {
        float n1[3],n2[3];
@@ -1762,7 +1893,7 @@ void CalcNormShort( short *v1, short *v2, short *v3, float *n) /* is also cross
        n[0]= n1[1]*n2[2]-n1[2]*n2[1];
        n[1]= n1[2]*n2[0]-n1[0]*n2[2];
        n[2]= n1[0]*n2[1]-n1[1]*n2[0];
-       Normalise(n);
+       Normalize(n);
 }
 
 void CalcNormLong( int* v1, int*v2, int*v3, float *n)
@@ -1778,7 +1909,7 @@ void CalcNormLong( int* v1, int*v2, int*v3, float *n)
        n[0]= n1[1]*n2[2]-n1[2]*n2[1];
        n[1]= n1[2]*n2[0]-n1[0]*n2[2];
        n[2]= n1[0]*n2[1]-n1[1]*n2[0];
-       Normalise(n);
+       Normalize(n);
 }
 
 float CalcNormFloat( float *v1, float *v2, float *v3, float *n)
@@ -1794,7 +1925,7 @@ float CalcNormFloat( float *v1, float *v2, float *v3, float *n)
        n[0]= n1[1]*n2[2]-n1[2]*n2[1];
        n[1]= n1[2]*n2[0]-n1[0]*n2[2];
        n[2]= n1[0]*n2[1]-n1[1]*n2[0];
-       return Normalise(n);
+       return Normalize(n);
 }
 
 float CalcNormFloat4( float *v1, float *v2, float *v3, float *v4, float *n)
@@ -1814,7 +1945,7 @@ float CalcNormFloat4( float *v1, float *v2, float *v3, float *v4, float *n)
        n[1]= n1[2]*n2[0]-n1[0]*n2[2];
        n[2]= n1[0]*n2[1]-n1[1]*n2[0];
 
-       return Normalise(n);
+       return Normalize(n);
 }
 
 
@@ -1908,12 +2039,12 @@ float AreaQ3Dfl( float *v1, float *v2, float *v3,  float *v4)  /* only convex Qu
        VecSubf(vec1, v2, v1);
        VecSubf(vec2, v4, v1);
        Crossf(n, vec1, vec2);
-       len= Normalise(n);
+       len= Normalize(n);
 
        VecSubf(vec1, v4, v3);
        VecSubf(vec2, v2, v3);
        Crossf(n, vec1, vec2);
-       len+= Normalise(n);
+       len+= Normalize(n);
 
        return (len/2.0f);
 }
@@ -1925,7 +2056,7 @@ float AreaT3Dfl( float *v1, float *v2, float *v3)  /* Triangles */
        VecSubf(vec1, v3, v2);
        VecSubf(vec2, v1, v2);
        Crossf(n, vec1, vec2);
-       len= Normalise(n);
+       len= Normalize(n);
 
        return (len/2.0f);
 }
@@ -2025,6 +2156,97 @@ void MinMax3(float *min, float *max, float *vec)
        if(max[2]<vec[2]) max[2]= vec[2];
 }
 
+static float TriSignedArea(float *v1, float *v2, float *v3, int i, int j)
+{
+       return 0.5f*((v1[i]-v2[i])*(v2[j]-v3[j]) + (v1[j]-v2[j])*(v3[i]-v2[i]));
+}
+
+static int BarycentricWeights(float *v1, float *v2, float *v3, float *co, float *n, float *w)
+{
+       float xn, yn, zn, a1, a2, a3, asum;
+       short i, j;
+
+       /* find best projection of face XY, XZ or YZ: barycentric weights of
+          the 2d projected coords are the same and faster to compute */
+       xn= fabs(n[0]);
+       yn= fabs(n[1]);
+       zn= fabs(n[2]);
+       if(zn>=xn && zn>=yn) {i= 0; j= 1;}
+       else if(yn>=xn && yn>=zn) {i= 0; j= 2;}
+       else {i= 1; j= 2;} 
+
+       a1= TriSignedArea(v2, v3, co, i, j);
+       a2= TriSignedArea(v3, v1, co, i, j);
+       a3= TriSignedArea(v1, v2, co, i, j);
+
+       asum= a1 + a2 + a3;
+
+       if (fabs(asum) < FLT_EPSILON) {
+               /* zero area triangle */
+               w[0]= w[1]= w[2]= 1.0f/3.0f;
+               return 1;
+       }
+
+       asum= 1.0f/asum;
+       w[0]= a1*asum;
+       w[1]= a2*asum;
+       w[2]= a3*asum;
+
+       return 0;
+}
+
+void InterpWeightsQ3Dfl(float *v1, float *v2, float *v3, float *v4, float *co, float *w)
+{
+       float w2[3];
+
+       w[0]= w[1]= w[2]= w[3]= 0.0f;
+
+       /* first check for exact match */
+       if(VecEqual(co, v1))
+               w[0]= 1.0f;
+       else if(VecEqual(co, v2))
+               w[1]= 1.0f;
+       else if(VecEqual(co, v3))
+               w[2]= 1.0f;
+       else if(v4 && VecEqual(co, v4))
+               w[3]= 1.0f;
+       else {
+               /* otherwise compute barycentric interpolation weights */
+               float n1[3], n2[3], n[3];
+               int degenerate;
+
+               VecSubf(n1, v1, v3);
+               if (v4) {
+                       VecSubf(n2, v2, v4);
+               }
+               else {
+                       VecSubf(n2, v2, v3);
+               }
+               Crossf(n, n1, n2);
+
+               /* OpenGL seems to split this way, so we do too */
+               if (v4) {
+                       degenerate= BarycentricWeights(v1, v2, v4, co, n, w);
+                       SWAP(float, w[2], w[3]);
+
+                       if(degenerate || (w[0] < 0.0f)) {
+                               /* if w[1] is negative, co is on the other side of the v1-v3 edge,
+                                  so we interpolate using the other triangle */
+                               degenerate= BarycentricWeights(v2, v3, v4, co, n, w2);
+
+                               if(!degenerate) {
+                                       w[0]= 0.0f;
+                                       w[1]= w2[0];
+                                       w[2]= w2[1];
+                                       w[3]= w2[2];
+                               }
+                       }
+               }
+               else
+                       BarycentricWeights(v1, v2, v3, co, n, w);
+       }
+} 
+
 /* ************ EULER *************** */
 
 void EulToMat3( float *eul, float mat[][3])
@@ -2084,8 +2306,8 @@ void EulToMat4( float *eul,float mat[][4])
        mat[3][3]= 1.0f;
 }
 
-
-void Mat3ToEul(float tmat[][3], float *eul)
+/* returns two euler calculation methods, so we can pick the best */
+static void mat3_to_eul2(float tmat[][3], float *eul1, float *eul2)
 {
        float cy, quat[4], mat[3][3];
        
@@ -2095,44 +2317,49 @@ void Mat3ToEul(float tmat[][3], float *eul)
        Mat3Ortho(mat);
        
        cy = (float)sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]);
-
+       
        if (cy > 16.0*FLT_EPSILON) {
-               eul[0] = (float)atan2(mat[1][2], mat[2][2]);
-               eul[1] = (float)atan2(-mat[0][2], cy);
-               eul[2] = (float)atan2(mat[0][1], mat[0][0]);
+               
+               eul1[0] = (float)atan2(mat[1][2], mat[2][2]);
+               eul1[1] = (float)atan2(-mat[0][2], cy);
+               eul1[2] = (float)atan2(mat[0][1], mat[0][0]);
+               
+               eul2[0] = (float)atan2(-mat[1][2], -mat[2][2]);
+               eul2[1] = (float)atan2(-mat[0][2], -cy);
+               eul2[2] = (float)atan2(-mat[0][1], -mat[0][0]);
+               
        } else {
-               eul[0] = (float)atan2(-mat[2][1], mat[1][1]);
-               eul[1] = (float)atan2(-mat[0][2], cy);
-               eul[2] = 0.0f;
+               eul1[0] = (float)atan2(-mat[2][1], mat[1][1]);
+               eul1[1] = (float)atan2(-mat[0][2], cy);
+               eul1[2] = 0.0f;
+               
+               VecCopyf(eul2, eul1);
        }
 }
 
-void Mat3ToEuln( float tmat[][3], float *eul)
+void Mat3ToEul(float tmat[][3], float *eul)
 {
-       float sin1, cos1, sin2, cos2, sin3, cos3;
+       float eul1[3], eul2[3];
        
-       sin1 = -tmat[2][0];
-       cos1 = (float)sqrt(1 - sin1*sin1);
-
-       if ( fabs(cos1) > FLT_EPSILON ) {
-               sin2 = tmat[2][1] / cos1;
-               cos2 = tmat[2][2] / cos1;
-               sin3 = tmat[1][0] / cos1;
-               cos3 = tmat[0][0] / cos1;
-    } 
+       mat3_to_eul2(tmat, eul1, eul2);
+               
+       /* return best, which is just the one with lowest values it in */
+       if( fabs(eul1[0])+fabs(eul1[1])+fabs(eul1[2]) > fabs(eul2[0])+fabs(eul2[1])+fabs(eul2[2])) {
+               VecCopyf(eul, eul2);
+       }
        else {
-               sin2 = -tmat[1][2];
-               cos2 = tmat[1][1];
-               sin3 = 0.0;
-               cos3 = 1.0;
-    }
-       
-       eul[0] = (float)atan2(sin3, cos3);
-       eul[1] = (float)atan2(sin1, cos1);
-       eul[2] = (float)atan2(sin2, cos2);
+               VecCopyf(eul, eul1);
+       }
+}
 
-} 
+void Mat4ToEul(float tmat[][4], float *eul)
+{
+       float tempMat[3][3];
 
+       Mat3CpyMat4 (tempMat, tmat);
+       Mat3Ortho(tempMat);
+       Mat3ToEul(tempMat, eul);
+}
 
 void QuatToEul( float *quat, float *eul)
 {
@@ -2184,6 +2411,15 @@ void VecRotToMat3( float *vec, float phi, float mat[][3])
        
 }
 
+void VecRotToMat4( float *vec, float phi, float mat[][4])
+{
+       float tmat[3][3];
+       
+       VecRotToMat3(vec, phi, tmat);
+       Mat4One(mat);
+       Mat4CpyMat3(mat, tmat);
+}
+
 void VecRotToQuat( float *vec, float phi, float *quat)
 {
        /* rotation of phi radials around vec */
@@ -2192,8 +2428,8 @@ void VecRotToQuat( float *vec, float phi, float *quat)
        quat[1]= vec[0];
        quat[2]= vec[1];
        quat[3]= vec[2];
-                                                                                                          
-       if( Normalise(quat+1) == 0.0) {
+       
+       if( Normalize(quat+1) == 0.0) {
                QuatOne(quat);
        }
        else {
@@ -2205,6 +2441,50 @@ void VecRotToQuat( float *vec, float phi, float *quat)
        }
 }
 
+/* Return the angle in degrees between vecs 1-2 and 2-3 in degrees
+   If v1 is a shoulder, v2 is the elbow and v3 is the hand,
+   this would return the angle at the elbow */
+float VecAngle3(float *v1, float *v2, float *v3)
+{
+       float vec1[3], vec2[3];
+
+       VecSubf(vec1, v2, v1);
+       VecSubf(vec2, v2, v3);
+       Normalize(vec1);
+       Normalize(vec2);
+
+       return NormalizedVecAngle2(vec1, vec2) * 180.0/M_PI;
+}
+
+/* Return the shortest angle in degrees between the 2 vectors */
+float VecAngle2(float *v1, float *v2)
+{
+       float vec1[3], vec2[3];
+
+       VecCopyf(vec1, v1);
+       VecCopyf(vec2, v2);
+       Normalize(vec1);
+       Normalize(vec2);
+
+       return NormalizedVecAngle2(vec1, vec2)* 180.0/M_PI;
+}
+
+float NormalizedVecAngle2(float *v1, float *v2)
+{
+       /* this is the same as acos(Inpf(v1, v2)), but more accurate */
+       if (Inpf(v1, v2) < 0.0f) {
+               float vec[3];
+               
+               vec[0]= -v2[0];
+               vec[1]= -v2[1];
+               vec[2]= -v2[2];
+
+               return (float)M_PI - 2.0f*saasin(VecLenf(vec, v1)/2.0f);
+       }
+       else
+               return 2.0f*saasin(VecLenf(v2, v1)/2.0);
+}
+
 void euler_rot(float *beul, float ang, char axis)
 {
        float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
@@ -2223,7 +2503,95 @@ void euler_rot(float *beul, float ang, char axis)
        
 }
 
+/* exported to transform.c */
+void compatible_eul(float *eul, float *oldrot)
+{
+       float dx, dy, dz;
+       
+       /* correct differences of about 360 degrees first */
+       
+       dx= eul[0] - oldrot[0];
+       dy= eul[1] - oldrot[1];
+       dz= eul[2] - oldrot[2];
+       
+       while( fabs(dx) > 5.1) {
+               if(dx > 0.0) eul[0] -= 2.0*M_PI; else eul[0]+= 2.0*M_PI;
+               dx= eul[0] - oldrot[0];
+       }
+       while( fabs(dy) > 5.1) {
+               if(dy > 0.0) eul[1] -= 2.0*M_PI; else eul[1]+= 2.0*M_PI;
+               dy= eul[1] - oldrot[1];
+       }
+       while( fabs(dz) > 5.1 ) {
+               if(dz > 0.0) eul[2] -= 2.0*M_PI; else eul[2]+= 2.0*M_PI;
+               dz= eul[2] - oldrot[2];
+       }
+       
+       /* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */      
+       if( fabs(dx) > 3.2 && fabs(dy)<1.6 && fabs(dz)<1.6 ) {
+               if(dx > 0.0) eul[0] -= 2.0*M_PI; else eul[0]+= 2.0*M_PI;
+       }
+       if( fabs(dy) > 3.2 && fabs(dz)<1.6 && fabs(dx)<1.6 ) {
+               if(dy > 0.0) eul[1] -= 2.0*M_PI; else eul[1]+= 2.0*M_PI;
+       }
+       if( fabs(dz) > 3.2 && fabs(dx)<1.6 && fabs(dy)<1.6 ) {
+               if(dz > 0.0) eul[2] -= 2.0*M_PI; else eul[2]+= 2.0*M_PI;
+       }
+       
+       /* the method below was there from ancient days... but why! probably because the code sucks :)
+               */
+#if 0  
+       /* calc again */
+       dx= eul[0] - oldrot[0];
+       dy= eul[1] - oldrot[1];
+       dz= eul[2] - oldrot[2];
+       
+       /* special case, tested for x-z  */
+       
+       if( (fabs(dx) > 3.1 && fabs(dz) > 1.5 ) || ( fabs(dx) > 1.5 && fabs(dz) > 3.1 ) ) {
+               if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI;
+               if(eul[1] > 0.0) eul[1]= M_PI - eul[1]; else eul[1]= -M_PI - eul[1];
+               if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI;
+               
+       }
+       else if( (fabs(dx) > 3.1 && fabs(dy) > 1.5 ) || ( fabs(dx) > 1.5 && fabs(dy) > 3.1 ) ) {
+               if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI;
+               if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI;
+               if(eul[2] > 0.0) eul[2]= M_PI - eul[2]; else eul[2]= -M_PI - eul[2];
+       }
+       else if( (fabs(dy) > 3.1 && fabs(dz) > 1.5 ) || ( fabs(dy) > 1.5 && fabs(dz) > 3.1 ) ) {
+               if(eul[0] > 0.0) eul[0]= M_PI - eul[0]; else eul[0]= -M_PI - eul[0];
+               if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI;
+               if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI;
+       }
+#endif 
+}
+
+/* uses 2 methods to retrieve eulers, and picks the closest */
+void Mat3ToCompatibleEul(float mat[][3], float *eul, float *oldrot)
+{
+       float eul1[3], eul2[3];
+       float d1, d2;
+       
+       mat3_to_eul2(mat, eul1, eul2);
+       
+       compatible_eul(eul1, oldrot);
+       compatible_eul(eul2, oldrot);
+       
+       d1= fabs(eul1[0]-oldrot[0]) + fabs(eul1[1]-oldrot[1]) + fabs(eul1[2]-oldrot[2]);
+       d2= fabs(eul2[0]-oldrot[0]) + fabs(eul2[1]-oldrot[1]) + fabs(eul2[2]-oldrot[2]);
+       
+       /* return best, which is just the one with lowest difference */
+       if( d1 > d2) {
+               VecCopyf(eul, eul2);
+       }
+       else {
+               VecCopyf(eul, eul1);
+       }
+       
+}
 
+/* ******************************************** */
 
 void SizeToMat3( float *size, float mat[][3])
 {
@@ -2238,17 +2606,25 @@ void SizeToMat3( float *size, float mat[][3])
        mat[2][0]= 0.0;
 }
 
+void SizeToMat4( float *size, float mat[][4])
+{
+       float tmat[3][3];
+       
+       SizeToMat3(size, tmat);
+       Mat4One(mat);
+       Mat4CpyMat3(mat, tmat);
+}
+
 void Mat3ToSize( float mat[][3], float *size)
 {
        float vec[3];
 
-
        VecCopyf(vec, mat[0]);
-       size[0]= Normalise(vec);
+       size[0]= Normalize(vec);
        VecCopyf(vec, mat[1]);
-       size[1]= Normalise(vec);
+       size[1]= Normalize(vec);
        VecCopyf(vec, mat[2]);
-       size[2]= Normalise(vec);
+       size[2]= Normalize(vec);
 
 }
 
@@ -2258,11 +2634,11 @@ void Mat4ToSize( float mat[][4], float *size)
        
 
        VecCopyf(vec, mat[0]);
-       size[0]= Normalise(vec);
+       size[0]= Normalize(vec);
        VecCopyf(vec, mat[1]);
-       size[1]= Normalise(vec);
+       size[1]= Normalize(vec);
        VecCopyf(vec, mat[2]);
-       size[2]= Normalise(vec);
+       size[2]= Normalize(vec);
 }
 
 /* ************* SPECIALS ******************* */
@@ -2270,7 +2646,7 @@ void Mat4ToSize( float mat[][4], float *size)
 void triatoquat( float *v1,  float *v2,  float *v3, float *quat)
 {
        /* imaginary x-axis, y-axis triangle is being rotated */
-       float vec[3], q1[4], q2[4], n[3], si, co, hoek, mat[3][3], imat[3][3];
+       float vec[3], q1[4], q2[4], n[3], si, co, angle, mat[3][3], imat[3][3];
        
        /* move z-axis to face-normal */
        CalcNormFloat(v1, v2, v3, vec);
@@ -2278,13 +2654,13 @@ void triatoquat( float *v1,  float *v2,  float *v3, float *quat)
        n[0]= vec[1];
        n[1]= -vec[0];
        n[2]= 0.0;
-       Normalise(n);
+       Normalize(n);
        
        if(n[0]==0.0 && n[1]==0.0) n[0]= 1.0;
        
-       hoek= -0.5f*saacos(vec[2]);
-       co= (float)cos(hoek);
-       si= (float)sin(hoek);
+       angle= -0.5f*saacos(vec[2]);
+       co= (float)cos(angle);
+       si= (float)sin(angle);
        q1[0]= co;
        q1[1]= n[0]*si;
        q1[2]= n[1]*si;
@@ -2298,11 +2674,11 @@ void triatoquat( float *v1,  float *v2,  float *v3, float *quat)
 
        /* what angle has this line with x-axis? */
        vec[2]= 0.0;
-       Normalise(vec);
+       Normalize(vec);
 
-       hoek= (float)(0.5*atan2(vec[1], vec[0]));
-       co= (float)cos(hoek);
-       si= (float)sin(hoek);
+       angle= (float)(0.5*atan2(vec[1], vec[0]));
+       co= (float)cos(angle);
+       si= (float)sin(angle);
        q2[0]= co;
        q2[1]= 0.0f;
        q2[2]= 0.0f;
@@ -2342,6 +2718,41 @@ void Vec2Addf(float *v, float *v1, float *v2)
        v[1]= v1[1]+ v2[1];
 }
 
+void Vec2Subf(float *v, float *v1, float *v2)
+{
+       v[0]= v1[0]- v2[0];
+       v[1]= v1[1]- v2[1];
+}
+
+void Vec2Copyf(float *v1, float *v2)
+{
+       v1[0]= v2[0];
+       v1[1]= v2[1];
+}
+
+float Inp2f(float *v1, float *v2)
+{
+       return v1[0]*v2[0]+v1[1]*v2[1];
+}
+
+float Normalize2(float *n)
+{
+       float d;
+       
+       d= n[0]*n[0]+n[1]*n[1];
+
+       if(d>1.0e-35F) {
+               d= (float)sqrt(d);
+
+               n[0]/=d; 
+               n[1]/=d; 
+       } else {
+               n[0]=n[1]= 0.0;
+               d= 0.0;
+       }
+       return d;
+}
+
 void hsv_to_rgb(float h, float s, float v, float *r, float *g, float *b)
 {
        int i;
@@ -2399,6 +2810,75 @@ void hsv_to_rgb(float h, float s, float v, float *r, float *g, float *b)
        }
 }
 
+void rgb_to_yuv(float r, float g, float b, float *ly, float *lu, float *lv)
+{
+       float y, u, v;
+       y= 0.299*r + 0.587*g + 0.114*b;
+       u=-0.147*r - 0.289*g + 0.436*b;
+       v= 0.615*r - 0.515*g - 0.100*b;
+       
+       *ly=y;
+       *lu=u;
+       *lv=v;
+}
+
+void yuv_to_rgb(float y, float u, float v, float *lr, float *lg, float *lb)
+{
+       float r, g, b;
+       r=y+1.140*v;
+       g=y-0.394*u - 0.581*v;
+       b=y+2.032*u;
+       
+       *lr=r;
+       *lg=g;
+       *lb=b;
+}
+
+void rgb_to_ycc(float r, float g, float b, float *ly, float *lcb, float *lcr)
+{
+       float sr,sg, sb;
+       float y, cr, cb;
+       
+       sr=255.0*r;
+       sg=255.0*g;
+       sb=255.0*b;
+       
+       
+       y=(0.257*sr)+(0.504*sg)+(0.098*sb)+16.0;
+       cb=(-0.148*sr)-(0.291*sg)+(0.439*sb)+128.0;
+       cr=(0.439*sr)-(0.368*sg)-(0.071*sb)+128.0;
+       
+       *ly=y;
+       *lcb=cb;
+       *lcr=cr;
+}
+
+void ycc_to_rgb(float y, float cb, float cr, float *lr, float *lg, float *lb)
+{
+       float r,g,b;
+       
+       r=1.164*(y-16)+1.596*(cr-128);
+       g=1.164*(y-16)-0.813*(cr-128)-0.392*(cb-128);
+       b=1.164*(y-16)+2.017*(cb-128);
+       
+       *lr=r/255.0;
+       *lg=g/255.0;
+       *lb=b/255.0;
+}
+
+void hex_to_rgb(char *hexcol, float *r, float *g, float *b)
+{
+       unsigned int ri, gi, bi;
+       
+       if (hexcol[0] == '#') hexcol++;
+       
+       if (sscanf(hexcol, "%02x%02x%02x", &ri, &gi, &bi)) {
+               *r = ri / 255.0;
+               *g = gi / 255.0;                
+               *b = bi / 255.0;
+       }
+}
+
 void rgb_to_hsv(float r, float g, float b, float *lh, float *ls, float *lv)
 {
        float h, s, v;
@@ -2492,3 +2972,362 @@ void cpack_to_rgb(unsigned int col, float *r, float *g, float *b)
        *b= (float)(((col)>>16)&0xFF);
        *b /= 255.0f;
 }
+
+
+/* *************** PROJECTIONS ******************* */
+
+void tubemap(float x, float y, float z, float *u, float *v)
+{
+       float len;
+       
+       *v = (z + 1.0) / 2.0;
+       
+       len= sqrt(x*x+y*y);
+       if(len>0) {
+               *u = (1.0 - (atan2(x/len,y/len) / M_PI)) / 2.0;
+       }
+}
+
+/* ------------------------------------------------------------------------- */
+
+void spheremap(float x, float y, float z, float *u, float *v)
+{
+       float len;
+       
+       len= sqrt(x*x+y*y+z*z);
+       if(len>0.0) {
+               
+               if(x==0.0 && y==0.0) *u= 0.0;   /* othwise domain error */
+               else *u = (1.0 - atan2(x,y)/M_PI )/2.0;
+               
+               z/=len;
+               *v = 1.0- saacos(z)/M_PI;
+       }
+}
+
+/* ------------------------------------------------------------------------- */
+
+/* *****************  m1 = m2 *****************  */
+void cpy_m3_m3(float m1[][3], float m2[][3]) 
+{      
+       memcpy(m1[0], m2[0], 9*sizeof(float));
+}
+
+/* *****************  m1 = m2 *****************  */
+void cpy_m4_m4(float m1[][4], float m2[][4]) 
+{      
+       memcpy(m1[0], m2[0], 16*sizeof(float));
+}
+
+/* ***************** identity matrix *****************  */
+void ident_m4(float m[][4])
+{
+       
+       m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0;
+       m[0][1]= m[0][2]= m[0][3]= 0.0;
+       m[1][0]= m[1][2]= m[1][3]= 0.0;
+       m[2][0]= m[2][1]= m[2][3]= 0.0;
+       m[3][0]= m[3][1]= m[3][2]= 0.0;
+}
+
+
+/* *****************  m1 = m2 (pre) * m3 (post) ***************** */
+void mul_m3_m3m3(float m1[][3], float m2[][3], float m3[][3])
+{
+       float m[3][3];
+       
+       m[0][0]= m2[0][0]*m3[0][0] + m2[1][0]*m3[0][1] + m2[2][0]*m3[0][2]; 
+       m[0][1]= m2[0][1]*m3[0][0] + m2[1][1]*m3[0][1] + m2[2][1]*m3[0][2]; 
+       m[0][2]= m2[0][2]*m3[0][0] + m2[1][2]*m3[0][1] + m2[2][2]*m3[0][2]; 
+
+       m[1][0]= m2[0][0]*m3[1][0] + m2[1][0]*m3[1][1] + m2[2][0]*m3[1][2]; 
+       m[1][1]= m2[0][1]*m3[1][0] + m2[1][1]*m3[1][1] + m2[2][1]*m3[1][2]; 
+       m[1][2]= m2[0][2]*m3[1][0] + m2[1][2]*m3[1][1] + m2[2][2]*m3[1][2]; 
+
+       m[2][0]= m2[0][0]*m3[2][0] + m2[1][0]*m3[2][1] + m2[2][0]*m3[2][2]; 
+       m[2][1]= m2[0][1]*m3[2][0] + m2[1][1]*m3[2][1] + m2[2][1]*m3[2][2]; 
+       m[2][2]= m2[0][2]*m3[2][0] + m2[1][2]*m3[2][1] + m2[2][2]*m3[2][2]; 
+
+       cpy_m3_m3(m1, m2);
+}
+
+/*  ***************** m1 = m2 (pre) * m3 (post) ***************** */
+void mul_m4_m4m4(float m1[][4], float m2[][4], float m3[][4])
+{
+       float m[4][4];
+       
+       m[0][0]= m2[0][0]*m3[0][0] + m2[1][0]*m3[0][1] + m2[2][0]*m3[0][2] + m2[3][0]*m3[0][3]; 
+       m[0][1]= m2[0][1]*m3[0][0] + m2[1][1]*m3[0][1] + m2[2][1]*m3[0][2] + m2[3][1]*m3[0][3]; 
+       m[0][2]= m2[0][2]*m3[0][0] + m2[1][2]*m3[0][1] + m2[2][2]*m3[0][2] + m2[3][2]*m3[0][3]; 
+       m[0][3]= m2[0][3]*m3[0][0] + m2[1][3]*m3[0][1] + m2[2][3]*m3[0][2] + m2[3][3]*m3[0][3]; 
+
+       m[1][0]= m2[0][0]*m3[1][0] + m2[1][0]*m3[1][1] + m2[2][0]*m3[1][2] + m2[3][0]*m3[1][3]; 
+       m[1][1]= m2[0][1]*m3[1][0] + m2[1][1]*m3[1][1] + m2[2][1]*m3[1][2] + m2[3][1]*m3[1][3]; 
+       m[1][2]= m2[0][2]*m3[1][0] + m2[1][2]*m3[1][1] + m2[2][2]*m3[1][2] + m2[3][2]*m3[1][3]; 
+       m[1][3]= m2[0][3]*m3[1][0] + m2[1][3]*m3[1][1] + m2[2][3]*m3[1][2] + m2[3][3]*m3[1][3]; 
+
+       m[2][0]= m2[0][0]*m3[2][0] + m2[1][0]*m3[2][1] + m2[2][0]*m3[2][2] + m2[3][0]*m3[2][3]; 
+       m[2][1]= m2[0][1]*m3[2][0] + m2[1][1]*m3[2][1] + m2[2][1]*m3[2][2] + m2[3][1]*m3[2][3]; 
+       m[2][2]= m2[0][2]*m3[2][0] + m2[1][2]*m3[2][1] + m2[2][2]*m3[2][2] + m2[3][2]*m3[2][3]; 
+       m[2][3]= m2[0][3]*m3[2][0] + m2[1][3]*m3[2][1] + m2[2][3]*m3[2][2] + m2[3][3]*m3[2][3]; 
+       
+       m[3][0]= m2[0][0]*m3[3][0] + m2[1][0]*m3[3][1] + m2[2][0]*m3[3][2] + m2[3][0]*m3[3][3]; 
+       m[3][1]= m2[0][1]*m3[3][0] + m2[1][1]*m3[3][1] + m2[2][1]*m3[3][2] + m2[3][1]*m3[3][3]; 
+       m[3][2]= m2[0][2]*m3[3][0] + m2[1][2]*m3[3][1] + m2[2][2]*m3[3][2] + m2[3][2]*m3[3][3]; 
+       m[3][3]= m2[0][3]*m3[3][0] + m2[1][3]*m3[3][1] + m2[2][3]*m3[3][2] + m2[3][3]*m3[3][3]; 
+       
+       cpy_m4_m4(m1, m2);
+}
+
+/*  ***************** m1 = inverse(m2)  *****************  */
+void inv_m3_m3(float m1[][3], float m2[][3])
+{
+       short a,b;
+       float det;
+       
+       /* calc adjoint */
+       Mat3Adj(m1, m2);
+       
+       /* then determinant old matrix! */
+       det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1])
+           -m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1])
+           +m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]);
+       
+       if(det==0.0f) det=1.0f;
+       det= 1.0f/det;
+       for(a=0;a<3;a++) {
+               for(b=0;b<3;b++) {
+                       m1[a][b]*=det;
+               }
+       }
+}
+
+/*  ***************** m1 = inverse(m2)  *****************  */
+int inv_m4_m4(float inverse[][4], float mat[][4])
+{
+       int i, j, k;
+       double temp;
+       float tempmat[4][4];
+       float max;
+       int maxj;
+       
+       /* Set inverse to identity */
+       ident_m4(inverse);
+       
+       /* Copy original matrix so we don't mess it up */
+       cpy_m4_m4(tempmat, mat);
+       
+       for(i = 0; i < 4; i++) {
+               /* Look for row with max pivot */
+               max = ABS(tempmat[i][i]);
+               maxj = i;
+               for(j = i + 1; j < 4; j++) {
+                       if(ABS(tempmat[j][i]) > max) {
+                               max = ABS(tempmat[j][i]);
+                               maxj = j;
+                       }
+               }
+               /* Swap rows if necessary */
+               if (maxj != i) {
+                       for( k = 0; k < 4; k++) {
+                               SWAP(float, tempmat[i][k], tempmat[maxj][k]);
+                               SWAP(float, inverse[i][k], inverse[maxj][k]);
+                       }
+               }
+               
+               temp = tempmat[i][i];
+               if (temp == 0)
+                       return 0;  /* No non-zero pivot */
+               for(k = 0; k < 4; k++) {
+                       tempmat[i][k] = (float)(tempmat[i][k]/temp);
+                       inverse[i][k] = (float)(inverse[i][k]/temp);
+               }
+               for(j = 0; j < 4; j++) {
+                       if(j != i) {
+                               temp = tempmat[j][i];
+                               for(k = 0; k < 4; k++) {
+                                       tempmat[j][k] -= (float)(tempmat[i][k]*temp);
+                                       inverse[j][k] -= (float)(inverse[i][k]*temp);
+                               }
+                       }
+               }
+       }
+       return 1;
+}
+
+/*  ***************** v1 = v2 * mat  ***************** */
+void mul_v3_v3m4(float *v1, float *v2, float mat[][4])
+{
+       float x, y;
+       
+       x= v2[0];       /* work with a copy, v1 can be same as v2 */
+       y= v2[1];
+       v1[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*v2[2] + mat[3][0];
+       v1[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*v2[2] + mat[3][1];
+       v1[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*v2[2] + mat[3][2];
+       
+}
+
+/* moved from effect.c
+   test if the line starting at p1 ending at p2 intersects the triangle v0..v2
+   return non zero if it does 
+*/
+int LineIntersectsTriangle(float p1[3], float p2[3], float v0[3], float v1[3], float v2[3], float *lambda)
+{
+
+       float p[3], s[3], d[3], e1[3], e2[3], q[3];
+       float a, f, u, v;
+       
+       VecSubf(e1, v1, v0);
+       VecSubf(e2, v2, v0);
+       VecSubf(d, p2, p1);
+       
+       Crossf(p, d, e2);
+       a = Inpf(e1, p);
+       if ((a > -0.000001) && (a < 0.000001)) return 0;
+       f = 1.0f/a;
+       
+       VecSubf(s, p1, v0);
+       
+       Crossf(q, s, e1);
+       *lambda = f * Inpf(e2, q);
+       if ((*lambda < 0.0)||(*lambda > 1.0)) return 0;
+       
+       u = f * Inpf(s, p);
+       if ((u < 0.0)||(u > 1.0)) return 0;
+       
+       v = f * Inpf(d, q);
+       if ((v < 0.0)||((u + v) > 1.0)) return 0;
+       
+       return 1;
+}
+
+
+/*
+find closest point to p on line through l1,l2
+and return lambda, where (0 <= lambda <= 1) when cp is in the line segement l1,l2
+*/
+float lambda_cp_line_ex(float p[3], float l1[3], float l2[3], float cp[3])
+{
+       float h[3],u[3],lambda;
+       VecSubf(u, l2, l1);
+       VecSubf(h, p, l1);
+       lambda =Inpf(u,h)/Inpf(u,u);
+       cp[0] = l1[0] + u[0] * lambda;
+       cp[1] = l1[1] + u[1] * lambda;
+       cp[2] = l1[2] + u[2] * lambda;
+       return lambda;
+}
+/* little sister we only need to know lambda */
+float lambda_cp_line(float p[3], float l1[3], float l2[3])
+{
+       float h[3],u[3];
+       VecSubf(u, l2, l1);
+       VecSubf(h, p, l1);
+       return(Inpf(u,h)/Inpf(u,u));
+}
+
+
+
+int point_in_slice(float p[3], float v1[3], float l1[3], float l2[3])
+{
+/* 
+what is a slice ?
+some maths:
+a line including l1,l2 and a point not on the line 
+define a subset of R3 delimeted by planes parallel to the line and orthogonal 
+to the (point --> line) distance vector,one plane on the line one on the point, 
+the room inside usually is rather small compared to R3 though still infinte
+useful for restricting (speeding up) searches 
+e.g. all points of triangular prism are within the intersection of 3 'slices'
+onother trivial case : cube 
+but see a 'spat' which is a deformed cube with paired parallel planes needs only 3 slices too
+*/
+       float h,rp[3],cp[3],q[3];
+
+       lambda_cp_line_ex(v1,l1,l2,cp);
+       VecSubf(q,cp,v1);
+
+       VecSubf(rp,p,v1);
+       h=Inpf(q,rp)/Inpf(q,q);
+       if (h < 0.0f || h > 1.0f) return 0;
+       return 1;
+}
+
+/*adult sister defining the slice planes by the origin and the normal  
+NOTE |normal| may not be 1 but defining the thickness of the slice*/
+int point_in_slice_as(float p[3],float origin[3],float normal[3])
+{
+       float h,rp[3];
+       VecSubf(rp,p,origin);
+       h=Inpf(normal,rp)/Inpf(normal,normal);
+       if (h < 0.0f || h > 1.0f) return 0;
+       return 1;
+}
+
+/*mama (knowing the squared lenght of the normal)*/
+int point_in_slice_m(float p[3],float origin[3],float normal[3],float lns)
+{
+       float h,rp[3];
+       VecSubf(rp,p,origin);
+       h=Inpf(normal,rp)/lns;
+       if (h < 0.0f || h > 1.0f) return 0;
+       return 1;
+}
+
+
+int point_in_tri_prism(float p[3], float v1[3], float v2[3], float v3[3])
+{
+       if(!point_in_slice(p,v1,v2,v3)) return 0;
+       if(!point_in_slice(p,v2,v3,v1)) return 0;
+       if(!point_in_slice(p,v3,v1,v2)) return 0;
+       return 1;
+}
+
+/********************************************************/
+
+/* make a 4x4 matrix out of 3 transform components */
+/* matrices are made in the order: scale * rot * loc */
+void LocEulSizeToMat4(float mat[][4], float loc[3], float eul[3], float size[3])
+{
+       float rmat[3][3], smat[3][3], tmat[3][3];
+       
+       /* initialise new matrix */
+       Mat4One(mat);
+       
+       /* make rotation + scaling part */
+       EulToMat3(eul, rmat);
+       SizeToMat3(size, smat);
+       Mat3MulMat3(tmat, rmat, smat);
+       
+       /* copy rot/scale part to output matrix*/
+       Mat4CpyMat3(mat, tmat);
+       
+       /* copy location to matrix */
+       mat[3][0] = loc[0];
+       mat[3][1] = loc[1];
+       mat[3][2] = loc[2];
+}
+
+/* make a 4x4 matrix out of 3 transform components */
+/* matrices are made in the order: scale * rot * loc */
+void LocQuatSizeToMat4(float mat[][4], float loc[3], float quat[4], float size[3])
+{
+       float rmat[3][3], smat[3][3], tmat[3][3];
+       
+       /* initialise new matrix */
+       Mat4One(mat);
+       
+       /* make rotation + scaling part */
+       QuatToMat3(quat, rmat);
+       SizeToMat3(size, smat);
+       Mat3MulMat3(tmat, rmat, smat);
+       
+       /* copy rot/scale part to output matrix*/
+       Mat4CpyMat3(mat, tmat);
+       
+       /* copy location to matrix */
+       mat[3][0] = loc[0];
+       mat[3][1] = loc[1];
+       mat[3][2] = loc[2];
+}