adding function
authorJens Ole Wund <bjornmose@gmx.net>
Wed, 25 Nov 2009 23:54:21 +0000 (23:54 +0000)
committerJens Ole Wund <bjornmose@gmx.net>
Wed, 25 Nov 2009 23:54:21 +0000 (23:54 +0000)
vcloud_estimate_transform(..) to math library
comments there (@math_geom.c) should explain what it does
-- removing attached clutter from softbody.c

source/blender/blenkernel/intern/softbody.c
source/blender/blenlib/BLI_math_geom.h
source/blender/blenlib/intern/math_geom.c

index 557f326e951b49553aeba268c42fe8a0c5418e8a..f53700976fd35fa26b10c587c485287c07f00e10 100644 (file)
@@ -3752,6 +3752,7 @@ static void softbody_update_positions(Object *ob, SoftBody *sb, float (*vertexCo
        }
 }
 
+
 /* void SB_estimate_transform */
 /* input   Object *ob out (says any object that can do SB like mesh,lattice,curve )
    output  float lloc[3],float lrot[3][3],float lscale[3][3]
@@ -3764,164 +3765,40 @@ static void softbody_update_positions(Object *ob, SoftBody *sb, float (*vertexCo
    lloc,lrot,lscale are allowed to be NULL, just in case you don't need it. 
    should be pretty useful for pythoneers :)
    not! velocity .. 2nd order stuff
+   vcloud_estimate_transform see
    */
 
-/* can't believe there is none in math utils */
-float _det_m3(float m2[3][3])
-{
-       float det = 0.f;
-       if (m2){
-       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]);
-       }
-       return det;
-}
-
 void SB_estimate_transform(Object *ob,float lloc[3],float lrot[3][3],float lscale[3][3])
 {
        BodyPoint *bp;
        ReferenceVert *rp;
-       float accu_pos[3] = {0.0f,0.0f,0.0f};
-       float com[3],va[3],vb[3],rcom[3];
-       float accu_mass = 0.0f,la = 0.0f,lb = 0.0f,eps = 0.000001f;
        SoftBody *sb = 0;
-       int a,i=0,imax=16;
-    int _localdebug;
-       
-       if (lloc) zero_v3(lloc);
-       if (lrot) zero_m3(lrot);
-       if (lscale) zero_m3(lscale);
-
+       float (*opos)[3];
+       float (*rpos)[3];
+       float com[3],rcom[3];
+       int a;
 
        if(!ob ||!ob->soft) return; /* why did we get here ? */
        sb= ob->soft;
-       /*for threading there should be a lock */
-       /* sb-> lock; */
-       /* calculate center of mass */
        if(!sb || !sb->bpoint) return;
-       _localdebug=sb->solverflags & SBSO_MONITOR; /* turn this on/off if you (don't) want to see progress on console */ 
-       for(a=0,bp=sb->bpoint; a<sb->totpoint; a++, bp++) {
-               VECADD(accu_pos,accu_pos,bp->pos);
-               accu_mass += bp->mass;
+       opos= MEM_callocN( (sb->totpoint)*3*sizeof(float), "SB_OPOS");
+       rpos= MEM_callocN( (sb->totpoint)*3*sizeof(float), "SB_RPOS");
+       /* might filter vertex selection with a vertex group */  
+       for(a=0, bp=sb->bpoint, rp=sb->scratch->Ref.ivert; a<sb->totpoint; a++, bp++, rp++) {
+               VECCOPY(rpos[a],rp->pos); 
+               VECCOPY(opos[a],bp->pos); 
        }
-       VECCOPY(com,accu_pos);
-       mul_v3_fl(com,1.0f/accu_mass);
-       /* center of mass done*/
-       if (sb->scratch){
-               float dcom[3],stunt[3];
-               float m[3][3],mr[3][3],q[3][3],qi[3][3];
-               float odet,ndet;
-               zero_m3(m);
-               zero_m3(mr);
-               VECSUB(dcom,com,sb->scratch->Ref.com);
-               VECCOPY(rcom,sb->scratch->Ref.com);
-               if (_localdebug) {
-                       printf("DCOM %f %f %f\n",dcom[0],dcom[1],dcom[2]);  
-               }
-               if (lloc) VECCOPY(lloc,dcom);
-        VECCOPY(sb->lcom,dcom);
-               /* build 'projection' matrix */
-               for(a=0, bp=sb->bpoint, rp=sb->scratch->Ref.ivert; a<sb->totpoint; a++, bp++, rp++) {
-                       VECSUB(va,rp->pos,rcom); 
-                       la += len_v3(va);
-                       /* mul_v3_fl(va,bp->mass);  mass needs renormalzation here ?? */ 
-                       VECSUB(vb,bp->pos,com); 
-                       lb += len_v3(vb);
-                       /* mul_v3_fl(va,rp->mass); */
-                       m[0][0] += va[0] * vb[0];
-                       m[0][1] += va[0] * vb[1];
-                       m[0][2] += va[0] * vb[2];
-
-                       m[1][0] += va[1] * vb[0];
-                       m[1][1] += va[1] * vb[1];
-                       m[1][2] += va[1] * vb[2];
-
-                       m[2][0] += va[2] * vb[0];
-                       m[2][1] += va[2] * vb[1];
-                       m[2][2] += va[2] * vb[2];
-
-                       /* building the referenc matrix on the fly
-                       needed to scale properly later*/
-          
-                       mr[0][0] += va[0] * va[0];
-                       mr[0][1] += va[0] * va[1];
-                       mr[0][2] += va[0] * va[2];
-
-                       mr[1][0] += va[1] * va[0];
-                       mr[1][1] += va[1] * va[1];
-                       mr[1][2] += va[1] * va[2];
-
-                       mr[2][0] += va[2] * va[0];
-                       mr[2][1] += va[2] * va[1];
-                       mr[2][2] += va[2] * va[2];
-               }
-               /* we are pretty much set up here and we could return that raw mess containing essential information
-               but being nice fellows we proceed:
-               knowing we did split off the tanslational part to the center of mass (com) part
-               however let's do some more reverse engeneering and see if we can split 
-               rotation from scale ->Polardecompose
-               */
-               copy_m3_m3(q,m);
-               stunt[0] = q[0][0]; stunt[1] = q[1][1]; stunt[2] = q[2][2];
-               /* nothing to see here but renormalizing works nicely
-               printf("lenght stunt %5.3f a %5.3f b %5.3f %5.3f\n",len_v3(stunt),la,lb,sqrt(la*lb));
-               */
-               mul_m3_fl(q,1.f/len_v3(stunt)); 
-               /* not too much to see here
-                       if(_localdebug){
-                               printf("q0 %5.3f %5.3f %5.3f\n",q[0][0],q[0][1],q[0][2]);
-                               printf("q1 %5.3f %5.3f %5.3f\n",q[1][0],q[1][1],q[1][2]);
-                               printf("q2 %5.3f %5.3f %5.3f\n",q[2][0],q[2][1],q[2][2]);
-                       }
-                       */
-               /* this is pretty much Polardecompose 'inline' the algo based on Higham's thesis */
-               /* without the far case !!! but seems to work here pretty neat                   */
-               odet = 0.f;
-               ndet = _det_m3(q);
-               while((odet-ndet)*(odet-ndet) > eps && i<imax){
-                       invert_m3_m3(qi,q);
-                       transpose_m3(qi);
-                       add_m3_m3m3(q,q,qi);
-                       mul_m3_fl(q,0.5f);
-                       odet =ndet;
-                       ndet =_det_m3(q);
-                       i++;
-               }
-               if (i){
-                       float scale[3][3];
-                       float irot[3][3];
-                       if(lrot) copy_m3_m3(lrot,q);
-                       copy_m3_m3(sb->lrot,q);
-                       if(_localdebug){
-                               printf("Rot .. i %d\n",i);
-                               printf("!q0 %5.3f %5.3f %5.3f\n",q[0][0],q[0][1],q[0][2]);
-                               printf("!q1 %5.3f %5.3f %5.3f\n",q[1][0],q[1][1],q[1][2]);
-                               printf("!q2 %5.3f %5.3f %5.3f\n",q[2][0],q[2][1],q[2][2]);
-                       }
-                       invert_m3_m3(irot,q);
-                       /* now that's where we need mr to get scaling right */
-                       invert_m3_m3(qi,mr);
-                       mul_m3_m3m3(q,m,qi); 
-
-                       //mul_m3_m3m3(scale,q,irot);
-                       mul_m3_m3m3(scale,irot,q); /*  i always have a problem with this C functions left/right operator applies first*/
-                   mul_m3_fl(scale,lb/la); /* 0 order scale was normalized away so put it back here  dunno if that is needed here ???*/
-
-                       if(lscale) copy_m3_m3(lscale,scale);
-                       copy_m3_m3(sb->lscale,scale);
-                       if(_localdebug){
-                               printf("Scale .. \n");
-                               printf("!s0 %5.3f %5.3f %5.3f\n",scale[0][0],scale[0][1],scale[0][2]);
-                               printf("!s1 %5.3f %5.3f %5.3f\n",scale[1][0],scale[1][1],scale[1][2]);
-                               printf("!s2 %5.3f %5.3f %5.3f\n",scale[2][0],scale[2][1],scale[2][2]);
-                       }
 
-                       
-               }
-       }
-       /*for threading there should be a unlock */
-       /* sb-> unlock; */
+       vcloud_estimate_transform(sb->totpoint, opos, NULL, rpos, NULL, com, rcom,lrot,lscale);
+       //VECSUB(com,com,rcom);
+       if (lloc) VECCOPY(lloc,com);
+       VECCOPY(sb->lcom,com);
+       if (lscale) copy_m3_m3(sb->lscale,lscale);
+       if (lrot)   copy_m3_m3(sb->lrot,lrot);
+
+  
+       MEM_freeN(opos);
+       MEM_freeN(rpos);
 }
 
 static void softbody_reset(Object *ob, SoftBody *sb, float (*vertexCos)[3], int numVerts)
@@ -4182,10 +4059,6 @@ void sbObjectStep(Scene *scene, Object *ob, float cfra, float (*vertexCos)[3], i
                dtime = timescale;
                softbody_update_positions(ob, sb, vertexCos, numVerts);
                softbody_step(scene, ob, sb, dtime);
-               if(sb->solverflags & SBSO_MONITOR ){
-                       printf("Picked from cache continue_physics %d\n",framenr);
-               }
-
                softbody_to_object(ob, vertexCos, numVerts, 0);
                return;
        }
@@ -4210,9 +4083,6 @@ void sbObjectStep(Scene *scene, Object *ob, float cfra, float (*vertexCos)[3], i
        cache_result = BKE_ptcache_read_cache(&pid, framenr, scene->r.frs_sec);
 
        if(cache_result == PTCACHE_READ_EXACT || cache_result == PTCACHE_READ_INTERPOLATED) {
-               if(sb->solverflags & SBSO_MONITOR ){
-                       printf("Picked from cache at frame %d\n",framenr);
-               }
                softbody_to_object(ob, vertexCos, numVerts, sb->local);
 
                cache->simframe= framenr;
index d54be9d5e0308ec0d4e37aec6e141f40270b450e..c50d9caade077979ff479221a945ee590baff058 100644 (file)
@@ -148,6 +148,12 @@ void sum_or_add_vertex_tangent(void *arena, VertexTangent **vtang,
 void tangent_from_uv(float *uv1, float *uv2, float *uv3,
        float *co1, float *co2, float *co3, float *n, float *tang);
 
+/********************************* vector clouds******************************/
+
+
+void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight,float (*rpos)[3], float *rweight,
+                                                       float lloc[3],float rloc[3],float lrot[3][3],float lscale[3][3]);
+
 #ifdef __cplusplus
 }
 #endif
index 0b3ab2f0afc04343f0051c6f11015495212b2de3..aa43201fd461941c7a6f6d2bd4ba5a28564e7fa5 100644 (file)
@@ -1597,3 +1597,161 @@ void tangent_from_uv(float *uv1, float *uv2, float *uv3, float *co1, float *co2,
                negate_v3(tang);
 }
 
+/********************************************************/
+
+/* vector clouds */
+/* void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight,float (*rpos)[3], float *rweight,
+                                                          float lloc[3],float rloc[3],float lrot[3][3],float lscale[3][3])
+/*
+input
+(
+int list_size
+4 lists as pointer to array[list_size]
+1. current pos array of 'new' positions 
+2. current weight array of 'new'weights (may be NULL pointer if you have no weights )
+3. reference rpos array of 'old' positions
+4. reference rweight array of 'old'weights (may be NULL pointer if you have no weights )
+)
+output  
+(
+float lloc[3] center of mass pos
+float rloc[3] center of mass rpos
+float lrot[3][3] rotation matrix
+float lscale[3][3] scale matrix
+pointers may be NULL if not needed
+)
+
+*/
+/* can't believe there is none in math utils */
+float _det_m3(float m2[3][3])
+{
+    float det = 0.f;
+    if (m2){
+    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]);
+    }
+    return det;
+}
+
+
+void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight,float (*rpos)[3], float *rweight,
+                                                       float lloc[3],float rloc[3],float lrot[3][3],float lscale[3][3])
+{
+       float accu_com[3]= {0.0f,0.0f,0.0f}, accu_rcom[3]= {0.0f,0.0f,0.0f};
+       float accu_weight = 0.0f,accu_rweight = 0.0f,eps = 0.000001f;
+
+       int a;
+       /* first set up a nice default response */
+       if (lloc) zero_v3(lloc);
+       if (rloc) zero_v3(rloc);
+       if (lrot) unit_m3(lrot);
+       if (lscale) unit_m3(lscale);
+       /* do com for both clouds */
+       if (pos && rpos && (list_size > 0)) /* paranoya check */
+       {
+               /* do com for both clouds */
+               for(a=0; a<list_size; a++){
+                       if (weight){
+                               float v[3];
+                               copy_v3_v3(v,pos[a]);
+                               mul_v3_fl(v,weight[a]);
+                               add_v3_v3v3(accu_com,accu_com,v);
+                               accu_weight +=weight[a]; 
+                       }
+                       else add_v3_v3v3(accu_com,accu_com,pos[a]);
+
+                       if (rweight){
+                               float v[3];
+                               copy_v3_v3(v,rpos[a]);
+                               mul_v3_fl(v,rweight[a]);
+                               add_v3_v3v3(accu_rcom,accu_rcom,v);
+                               accu_rweight +=rweight[a]; 
+                       }
+                       else add_v3_v3v3(accu_rcom,accu_rcom,rpos[a]);
+
+               }
+               if (!weight || !rweight){
+                       accu_weight = accu_rweight = list_size;
+               }
+
+               mul_v3_fl(accu_com,1.0f/accu_weight);
+               mul_v3_fl(accu_rcom,1.0f/accu_rweight);
+               if (lloc) copy_v3_v3(lloc,accu_com);
+               if (rloc) copy_v3_v3(rloc,accu_rcom);
+               if (lrot || lscale){ /* caller does not want rot nor scale, strange but legal */
+                       /*so now do some reverse engeneering and see if we can split rotation from scale ->Polardecompose*/
+                       /* build 'projection' matrix */
+                       float m[3][3],mr[3][3],q[3][3],qi[3][3];
+                       float va[3],vb[3],stunt[3];
+                       float odet,ndet;
+                       int i=0,imax=15;
+                       zero_m3(m);
+                       zero_m3(mr);
+
+                       /* build 'projection' matrix */
+                       for(a=0; a<list_size; a++){
+                               sub_v3_v3v3(va,rpos[a],accu_rcom);
+                               /* mul_v3_fl(va,bp->mass);  mass needs renormalzation here ?? */
+                               sub_v3_v3v3(vb,pos[a],accu_com);
+                               /* mul_v3_fl(va,rp->mass); */
+                               m[0][0] += va[0] * vb[0];
+                               m[0][1] += va[0] * vb[1];
+                               m[0][2] += va[0] * vb[2];
+
+                               m[1][0] += va[1] * vb[0];
+                               m[1][1] += va[1] * vb[1];
+                               m[1][2] += va[1] * vb[2];
+
+                               m[2][0] += va[2] * vb[0];
+                               m[2][1] += va[2] * vb[1];
+                               m[2][2] += va[2] * vb[2];
+
+                               /* building the referenc matrix on the fly
+                               needed to scale properly later*/
+
+                               mr[0][0] += va[0] * va[0];
+                               mr[0][1] += va[0] * va[1];
+                               mr[0][2] += va[0] * va[2];
+
+                               mr[1][0] += va[1] * va[0];
+                               mr[1][1] += va[1] * va[1];
+                               mr[1][2] += va[1] * va[2];
+
+                               mr[2][0] += va[2] * va[0];
+                               mr[2][1] += va[2] * va[1];
+                               mr[2][2] += va[2] * va[2];
+                       }
+                       copy_m3_m3(q,m);
+                       stunt[0] = q[0][0]; stunt[1] = q[1][1]; stunt[2] = q[2][2];
+                       /* renormalizing for numeric stability */
+                       mul_m3_fl(q,1.f/len_v3(stunt)); 
+
+                       /* this is pretty much Polardecompose 'inline' the algo based on Higham's thesis */
+                       /* without the far case ... but seems to work here pretty neat                   */
+                       odet = 0.f;
+                       ndet = _det_m3(q);
+                       while((odet-ndet)*(odet-ndet) > eps && i<imax){
+                               invert_m3_m3(qi,q);
+                               transpose_m3(qi);
+                               add_m3_m3m3(q,q,qi);
+                               mul_m3_fl(q,0.5f);
+                               odet =ndet;
+                               ndet =_det_m3(q);
+                               i++;
+                       }
+
+                       if (i){
+                               float scale[3][3];
+                               float irot[3][3];
+                               if(lrot) copy_m3_m3(lrot,q);
+                               invert_m3_m3(irot,q);
+                               invert_m3_m3(qi,mr);
+                               mul_m3_m3m3(q,m,qi); 
+                               mul_m3_m3m3(scale,irot,q); 
+                               if(lscale) copy_m3_m3(lscale,scale);
+
+                       }
+               }
+       }
+}