adding function

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

2 files changed, 164 insertions, 0 deletions

diff --git a/source/blender/blenlib/BLI_math_geom.h b/source/blender/blenlib/BLI_math_geom.h
index d54be9d5e03..c50d9caade0 100644
--- a/source/blender/blenlib/BLI_math_geom.h
+++ b/source/blender/blenlib/BLI_math_geom.h
@@ -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
diff --git a/source/blender/blenlib/intern/math_geom.c b/source/blender/blenlib/intern/math_geom.c
index 0b3ab2f0afc..aa43201fd46 100644
--- a/source/blender/blenlib/intern/math_geom.c
+++ b/source/blender/blenlib/intern/math_geom.c
@@ -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);
+
+			}
+		}
+	}
+}