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; atotpoint; 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; atotpoint; 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; atotpoint; 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]; + 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); - 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 && ilrot,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; */ + + 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; 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; aPolardecompose*/ + /* 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; amass); 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