diff options
author | Jens Ole Wund <bjornmose@gmx.net> | 2009-11-26 02:54:21 +0300 |
---|---|---|
committer | Jens Ole Wund <bjornmose@gmx.net> | 2009-11-26 02:54:21 +0300 |
commit | 12968cdd8a0141aa57e196256e7161ff11dd1ec3 (patch) | |
tree | e6dbd695a193057f1528274b10edc28dfedd671a /source/blender/blenlib | |
parent | a306759b7af05914a46eb9fec2dd3cc50aa6f20f (diff) |
adding function
vcloud_estimate_transform(..) to math library
comments there (@math_geom.c) should explain what it does
-- removing attached clutter from softbody.c
Diffstat (limited to 'source/blender/blenlib')
-rw-r--r-- | source/blender/blenlib/BLI_math_geom.h | 6 | ||||
-rw-r--r-- | source/blender/blenlib/intern/math_geom.c | 158 |
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); + + } + } + } +} |