== Constraints System ==
[blender-staging.git] / source / blender / blenlib / intern / arithb.c
index fd91a4231f67c456bbe1f8699196efa402244bff..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 *****
@@ -95,12 +92,12 @@ float sasqrt(float fac)
        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);
 
@@ -763,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)
 {
@@ -838,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;
@@ -957,6 +987,11 @@ 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]);
@@ -1203,7 +1238,7 @@ 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];
        angle= 0.5f*saacos(co);
@@ -1324,7 +1359,7 @@ float *vectoquat( float *vec, short axis, short upflag)
        }
        co/= len1;
 
-       Normalise(nor);
+       Normalize(nor);
        
        angle= 0.5f*saacos(co);
        si= (float)sin(angle);
@@ -1397,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]);
        
@@ -1443,14 +1478,14 @@ 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]);
        
@@ -1734,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];
@@ -1859,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)
@@ -1875,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)
@@ -1891,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)
@@ -1911,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);
 }
 
 
@@ -2005,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);
 }
@@ -2022,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);
 }
@@ -2122,9 +2156,14 @@ void MinMax3(float *min, float *max, float *vec)
        if(max[2]<vec[2]) max[2]= vec[2];
 }
 
-static void BarycentricWeights(float *v1, float *v2, float *v3, float *co, float *n, float *w)
+static float TriSignedArea(float *v1, float *v2, float *v3, int i, int j)
 {
-       float t00, t01, t10, t11, det, detinv, xn, yn, zn;
+       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
@@ -2136,28 +2175,30 @@ static void BarycentricWeights(float *v1, float *v2, float *v3, float *co, float
        else if(yn>=xn && yn>=zn) {i= 0; j= 2;}
        else {i= 1; j= 2;} 
 
-       /* compute determinant */
-       t00= v3[i]-v1[i]; t01= v3[j]-v1[j];
-       t10= v3[i]-v2[i]; t11= v3[j]-v2[j];
+       a1= TriSignedArea(v2, v3, co, i, j);
+       a2= TriSignedArea(v3, v1, co, i, j);
+       a3= TriSignedArea(v1, v2, co, i, j);
 
-       det= t00*t11 - t10*t01;
+       asum= a1 + a2 + a3;
 
-       if(det == 0.0f) {
+       if (fabs(asum) < FLT_EPSILON) {
                /* zero area triangle */
                w[0]= w[1]= w[2]= 1.0f/3.0f;
-               return;
+               return 1;
        }
 
-       /* compute weights */
-       detinv= 1.0/det;
+       asum= 1.0f/asum;
+       w[0]= a1*asum;
+       w[1]= a2*asum;
+       w[2]= a3*asum;
 
-       w[0]= ((co[j]-v3[j])*t10 - (co[i]-v3[i])*t11)*detinv;
-       w[1]= ((co[i]-v3[i])*t01 - (co[j]-v3[j])*t00)*detinv; 
-       w[2]= 1.0f - w[0] - w[1];
+       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 */
@@ -2172,6 +2213,7 @@ void InterpWeightsQ3Dfl(float *v1, float *v2, float *v3, float *v4, float *co, f
        else {
                /* otherwise compute barycentric interpolation weights */
                float n1[3], n2[3], n[3];
+               int degenerate;
 
                VecSubf(n1, v1, v3);
                if (v4) {
@@ -2184,16 +2226,20 @@ void InterpWeightsQ3Dfl(float *v1, float *v2, float *v3, float *v4, float *co, f
 
                /* OpenGL seems to split this way, so we do too */
                if (v4) {
-                       BarycentricWeights(v1, v2, v4, co, n, w);
+                       degenerate= BarycentricWeights(v1, v2, v4, co, n, w);
+                       SWAP(float, w[2], w[3]);
 
-                       if(w[0] < 0.0f) {
+                       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 */
-                               w[0]= 0.0f;
-                               BarycentricWeights(v2, v3, v4, co, n, w+1);
-                       }
-                       else {
-                               SWAP(float, w[2], w[3]);
+                               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
@@ -2365,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 */
@@ -2373,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 {
@@ -2395,8 +2450,8 @@ float VecAngle3(float *v1, float *v2, float *v3)
 
        VecSubf(vec1, v2, v1);
        VecSubf(vec2, v2, v3);
-       Normalise(vec1);
-       Normalise(vec2);
+       Normalize(vec1);
+       Normalize(vec2);
 
        return NormalizedVecAngle2(vec1, vec2) * 180.0/M_PI;
 }
@@ -2408,8 +2463,8 @@ float VecAngle2(float *v1, float *v2)
 
        VecCopyf(vec1, v1);
        VecCopyf(vec2, v2);
-       Normalise(vec1);
-       Normalise(vec2);
+       Normalize(vec1);
+       Normalize(vec2);
 
        return NormalizedVecAngle2(vec1, vec2)* 180.0/M_PI;
 }
@@ -2551,16 +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);
 
 }
 
@@ -2570,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 ******************* */
@@ -2590,7 +2654,7 @@ 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;
        
@@ -2610,7 +2674,7 @@ 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);
 
        angle= (float)(0.5*atan2(vec[1], vec[0]));
        co= (float)cos(angle);
@@ -2671,7 +2735,7 @@ float Inp2f(float *v1, float *v2)
        return v1[0]*v2[0]+v1[1]*v2[1];
 }
 
-float Normalise2(float *n)
+float Normalize2(float *n)
 {
        float d;
        
@@ -3223,28 +3287,46 @@ int point_in_tri_prism(float p[3], float v1[3], float v2[3], float v3[3])
 /********************************************************/
 
 /* 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 tmat[3][3];
+       float rmat[3][3], smat[3][3], tmat[3][3];
        
-       /* make base matrix */
-       EulToMat3(eul, tmat);
-
-       /* make new matrix */
+       /* initialise new matrix */
        Mat4One(mat);
        
-       mat[0][0] = tmat[0][0] * size[0];
-       mat[0][1] = tmat[0][1] * size[1];
-       mat[0][2] = tmat[0][2] * size[2];
+       /* 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);
        
-       mat[1][0] = tmat[1][0] * size[0];
-       mat[1][1] = tmat[1][1] * size[1];
-       mat[1][2] = tmat[1][2] * size[2];
+       /* make rotation + scaling part */
+       QuatToMat3(quat, rmat);
+       SizeToMat3(size, smat);
+       Mat3MulMat3(tmat, rmat, smat);
        
-       mat[2][0] = tmat[2][0] * size[0];
-       mat[2][1] = tmat[2][1] * size[1];
-       mat[2][2] = tmat[2][2] * size[2];
+       /* 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];