new property for soft bodies
authorJens Ole Wund <bjornmose@gmx.net>
Sat, 21 Nov 2009 22:45:25 +0000 (22:45 +0000)
committerJens Ole Wund <bjornmose@gmx.net>
Sat, 21 Nov 2009 22:45:25 +0000 (22:45 +0000)
sb->lcom   : Center Of Mass .. might be used to create loc IPO
sb_>lrot   : is a matrix[3] esitmates the roatation in world coordinates .. might be used to create rot IPO
sb_>lscale : is a matrix[3] esitmates the scaling   in world coordinates .. might be used to create scale IPO
(no python for that yet .. but may be matt has mercy on me )

can be cropped direclty  in soft body module by function
static void SB_estimate_transform(Object *ob,float lloc[3],float lrot[3][3],float lscale[3][3])

The targets lloc,lrot,lscale should work to be NULL, just in case you don't need it.

However i'd prefer if they were accessed via properties
which should be calculated automagically if
sb->solverflags & SBSO_ESTIMATEIPO
is set, like they do in draw_sb_motion(..) in drawobject.c

added static void draw_sb_motion(Scene *scene, Object *ob) to drawobject.c
for debuggering (had a hard time with destructive matrix operations )
if it causes any trouble with your build on any OS make sure to comment that away.
softbody.c and DNA should compile fine in any case.

source/blender/blenkernel/BKE_softbody.h
source/blender/blenkernel/intern/softbody.c
source/blender/editors/space_view3d/drawobject.c
source/blender/makesdna/DNA_object_force.h

index d8053281cebad639501f7c550d623514c3a92c55..92cb5542ad1418cc7a1edb64646c8ee30ef819df 100644 (file)
@@ -68,5 +68,8 @@ extern void                           sbObjectToSoftbody(struct Object *ob);
 /* pass NULL to unlink again */
 extern void             sbSetInterruptCallBack(int (*f)(void));
 
+extern void             SB_estimate_transform(Object *ob,float lloc[3],float lrot[3][3],float lscale[3][3]);
+
+
 #endif
 
index 34071b0034c7437fc2a8c55567b2322dd15eb720..37ca61d913c04131553f16e716d0c1d6d516ba78 100644 (file)
@@ -108,6 +108,16 @@ typedef struct BodyFace {
        short flag;
 } BodyFace;
 
+typedef struct ReferenceVert {
+       float pos[3]; /* position relative to com */
+       float mass;   /* node mass */
+} ReferenceVert;
+
+typedef struct ReferenceState {
+       float com[3]; /* center of mass*/
+       ReferenceVert *ivert; /* list of intial values */
+}ReferenceState ;
+
 
 /*private scratch pad for caching and other data only needed when alive*/
 typedef struct SBScratch {
@@ -117,6 +127,7 @@ typedef struct SBScratch {
        BodyFace *bodyface;
        int totface;
        float aabbmin[3],aabbmax[3];
+       ReferenceState Ref;
 }SBScratch;
 
 typedef struct  SB_thread_context {
@@ -882,6 +893,9 @@ static void free_scratch(SoftBody *sb)
                if (sb->scratch->bodyface){
                        MEM_freeN(sb->scratch->bodyface);
                }
+               if (sb->scratch->Ref.ivert){
+                       MEM_freeN(sb->scratch->Ref.ivert);
+               }
                MEM_freeN(sb->scratch);
                sb->scratch = NULL;
        }
@@ -3332,6 +3346,27 @@ static void mesh_faces_to_scratch(Object *ob)
        }
        sb->scratch->totface = me->totface;
 }
+static void reference_to_scratch(Object *ob)
+{
+       SoftBody *sb= ob->soft; 
+       ReferenceVert *rp;
+       BodyPoint     *bp;
+       float accu_pos[3] ={0.f,0.f,0.f};
+       float accu_mass = 0.f;
+       int a;
+
+       sb->scratch->Ref.ivert = MEM_mallocN(sizeof(ReferenceVert)*sb->totpoint,"SB_Reference");
+       bp= ob->soft->bpoint;
+       rp= sb->scratch->Ref.ivert;
+       for(a=0; a<sb->totpoint; a++, rp++, bp++) {
+               VECCOPY(rp->pos,bp->pos);
+               VECADD(accu_pos,accu_pos,bp->pos);
+               accu_mass += bp-> mass;
+       }
+       mul_v3_fl(accu_pos,1.0f/accu_mass);
+       VECCOPY(sb->scratch->Ref.com,accu_pos);
+       /* printf("reference_to_scratch \n"); */
+}
 
 /*
 helper function to get proper spring length 
@@ -3569,16 +3604,19 @@ static void curve_surf_to_softbody(Scene *scene, Object *ob)
 /* copies softbody result back in object */
 static void softbody_to_object(Object *ob, float (*vertexCos)[3], int numVerts, int local)
 {
-       BodyPoint *bp= ob->soft->bpoint;
-       int a;
-
-       /* inverse matrix is not uptodate... */
-       invert_m4_m4(ob->imat, ob->obmat);
-       
-       for(a=0; a<numVerts; a++, bp++) {
-               VECCOPY(vertexCos[a], bp->pos);
-               if(local==0) 
-                       mul_m4_v3(ob->imat, vertexCos[a]);      /* softbody is in global coords, baked optionally not */
+       SoftBody *sb= ob->soft;
+       if(sb){
+               BodyPoint *bp= sb->bpoint;
+               int a;
+               if(sb->solverflags & SBSO_MONITOR ||sb->solverflags & SBSO_ESTIMATEIPO){SB_estimate_transform(ob,sb->lcom,sb->lrot,sb->lscale);}
+               /* inverse matrix is not uptodate... */
+               invert_m4_m4(ob->imat, ob->obmat);
+
+               for(a=0; a<numVerts; a++, bp++) {
+                       VECCOPY(vertexCos[a], bp->pos);
+                       if(local==0) 
+                               mul_m4_v3(ob->imat, vertexCos[a]);      /* softbody is in global coords, baked optionally not */
+               }
        }
 }
 
@@ -3592,6 +3630,7 @@ static void sb_new_scratch(SoftBody *sb)
        sb->scratch->totface = 0;
        sb->scratch->aabbmax[0]=sb->scratch->aabbmax[1]=sb->scratch->aabbmax[2] = 1.0e30f;
        sb->scratch->aabbmin[0]=sb->scratch->aabbmin[1]=sb->scratch->aabbmin[2] = -1.0e30f;
+       sb->scratch->Ref.ivert = NULL;
        
 }
 /* --- ************ maintaining scratch *************** */
@@ -3713,6 +3752,178 @@ 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]
+   that is:
+   a precise position vector denoting the motion of the center of mass
+   give a rotation/scale matrix using averaging method, that's why estimate and not calculate
+   see: this is kind of reverse engeneering: having to states of a point cloud and recover what happend
+   our advantage here we know the identity of the vertex
+   there are others methods giving other results.
+   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
+   */
+
+/* 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;
+}
+
+static 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);
+
+
+       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;
+       }
+       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; */
+}
+
 static void softbody_reset(Object *ob, SoftBody *sb, float (*vertexCos)[3], int numVerts)
 {
        BodyPoint *bp;
@@ -3750,6 +3961,7 @@ static void softbody_reset(Object *ob, SoftBody *sb, float (*vertexCos)[3], int
        sb->scratch->needstobuildcollider=1; 
 
        /* copy some info to scratch */
+       if (1) reference_to_scratch(ob); /* wa only need that if we want to reconstruct IPO */
        switch(ob->type) {
        case OB_MESH:
                if (ob->softflag & OB_SB_FACECOLL) mesh_faces_to_scratch(ob);
@@ -3932,7 +4144,6 @@ void sbObjectStep(Scene *scene, Object *ob, float cfra, float (*vertexCos)[3], i
                cache->flag &= ~PTCACHE_SIMULATION_VALID;
                cache->simframe= 0;
                //cache->last_exact= 0;
-
                return;
        }
        else if(framenr > endframe) {
@@ -3967,22 +4178,23 @@ void sbObjectStep(Scene *scene, Object *ob, float cfra, float (*vertexCos)[3], i
        if(BKE_ptcache_get_continue_physics()) {
                cache->flag &= ~PTCACHE_SIMULATION_VALID;
                cache->simframe= 0;
-
                /* do simulation */
                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;
        }
 
        /* still no points? go away */
-       if(sb->totpoint==0) return;
-
-       if(framenr == startframe) {
+       if(sb->totpoint==0) {
+               return;
+       }
+    if(framenr == startframe) {
                BKE_ptcache_id_reset(scene, &pid, PTCACHE_RESET_OUTDATED);
 
                /* first frame, no simulation to do, just set the positions */
@@ -3998,6 +4210,9 @@ 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;
@@ -4035,7 +4250,6 @@ void sbObjectStep(Scene *scene, Object *ob, float cfra, float (*vertexCos)[3], i
 
        cache->simframe= framenr;
        cache->flag |= PTCACHE_SIMULATION_VALID;
-
        BKE_ptcache_write_cache(&pid, framenr);
 }
 
index a0c13b6d6eebd0ce908575a7f48f865990063582..496af5ef1b846f66467b783db56c3a8bf09eac3c 100644 (file)
@@ -91,6 +91,7 @@
 #include "BKE_particle.h"
 #include "BKE_pointcache.h"
 #include "BKE_property.h"
+#include "BKE_softbody.h"
 #include "BKE_smoke.h"
 #include "BKE_unit.h"
 #include "BKE_utildefines.h"
@@ -4325,7 +4326,65 @@ static void draw_ptcache_edit(Scene *scene, View3D *v3d, RegionView3D *rv3d, Obj
 
        glPointSize(1.0);
 }
+static void draw_sb_motion(Scene *scene, Object *ob)
+{
+       SoftBody *sb = 0;
+       if (sb= ob->soft){
+               if(sb->solverflags & SBSO_MONITOR ||sb->solverflags & SBSO_ESTIMATEIPO){
+                       /* draw com */ 
+               float rt[3][3],sc[3][3],tr[3][3]; 
+                       /* looks like to swap a b in reverse */
+                       copy_m3_m3(sc,sb->lscale);
+                       copy_m3_m3(rt,sb->lrot);
+                       mul_m3_m3m3(tr,rt,sc); 
+                       if(1){
+                               float root[3],tip[3];
+
+                               glBegin(GL_LINES);
+                               root[1] = root[2] = 0.0f;
+                               root[0] = -1.0f;
+                               mul_m3_v3(tr,root);
+                               VECADD(root,root,sb->lcom);
+                               glVertex3fv(root); 
+                               tip[1] = tip[2] = 0.0f;
+                               tip[0] = 1.0f;
+                               mul_m3_v3(tr,tip);
+                               VECADD(tip,tip,sb->lcom);
+                               glVertex3fv(tip); 
+                               glEnd();
+
+                               glBegin(GL_LINES);
+                               root[0] = root[2] = 0.0f;
+                               root[1] = -1.0f;
+                               mul_m3_v3(tr,root);
+                               VECADD(root,root,sb->lcom);
+                               glVertex3fv(root); 
+                               tip[0] = tip[2] = 0.0f;
+                               tip[1] = 1.0f;
+                               mul_m3_v3(tr,tip);
+                               VECADD(tip,tip,sb->lcom);
+                               glVertex3fv(tip); 
+                               glEnd();
+
+                               glBegin(GL_LINES);
+                               root[0] = root[1] = 0.0f;
+                               root[2] = -1.0f;
+                               mul_m3_v3(tr,root);
+                               VECADD(root,root,sb->lcom);
+                               glVertex3fv(root); 
+                               tip[0] = tip[1] = 0.0f;
+                               tip[2] = 1.0f;
+                               mul_m3_v3(tr,tip);
+                               VECADD(tip,tip,sb->lcom);
+                               glVertex3fv(tip); 
+                               glEnd();
+                       }
+
+               }
+       }
+};
 
+/*place to add drawers */
 unsigned int nurbcol[8]= {
        0, 0x9090, 0x409030, 0x603080, 0, 0x40fff0, 0x40c033, 0xA090F0 };
 
@@ -5720,6 +5779,8 @@ void draw_object(Scene *scene, ARegion *ar, View3D *v3d, Base *base, int flag)
                default:
                        drawaxes(1.0, flag, OB_ARROWS);
        }
+    if(ob->soft /*&& flag & OB_SBMOTION*/) draw_sb_motion(scene, ob);
+
        if(ob->pd && ob->pd->forcefield) draw_forcefield(scene, ob);
 
        /* particle mode has to be drawn first so that possible child particles get cached in edit mode */
index 1376a08eb3e554d9c153968261c3009452d27890..adccf8893d4aa4097cfba6c2184b590e6d5d699b 100644 (file)
@@ -301,6 +301,11 @@ typedef struct SoftBody {
        struct ListBase ptcaches;
 
        struct EffectorWeights *effector_weights;
+    /* reverse esimated obmatrix .. no need to store in blend file .. how ever who cares */ 
+       float lcom[3];
+       float lrot[3][3];
+       float lscale[3][3];
+    char  pad4[4];
 
 } SoftBody;
 
@@ -379,8 +384,9 @@ typedef struct SoftBody {
 #define OB_SB_AERO_ANGLE       16384
 
 /* sb->solverflags */
-#define SBSO_MONITOR    1 
-#define SBSO_OLDERR     2 
+#define SBSO_MONITOR           1 
+#define SBSO_OLDERR                    2 
+#define SBSO_ESTIMATEIPO    4 
 
 /* sb->sbc_mode */
 #define SBC_MODE_MANUAL                0