adding function

vcloud_estimate_transform(..) to math library
comments there (@math_geom.c) should explain what it does
-- removing attached clutter from softbody.c  

1 files changed, 22 insertions, 152 deletions

diff --git a/source/blender/blenkernel/intern/softbody.c b/source/blender/blenkernel/intern/softbody.c
index 557f326e951..f53700976fd 100644
--- a/source/blender/blenkernel/intern/softbody.c
+++ b/source/blender/blenkernel/intern/softbody.c
@@ -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;