diff options
author | Jens Ole Wund <bjornmose@gmx.net> | 2008-01-09 03:25:51 +0300 |
---|---|---|
committer | Jens Ole Wund <bjornmose@gmx.net> | 2008-01-09 03:25:51 +0300 |
commit | 048170bc6f8e20beec8aedb4753991b027c65f4f (patch) | |
tree | b6b0302a8f02fded423655c65899417b0ddd7e25 /source/blender/blenkernel | |
parent | 489d8144153c2432a3e200579e877853d1790ec2 (diff) |
quite a load is still hidden behind the define #ifdef _work_on_sb_solver
a glance to view is the "STU PID semi implicit euler"
most of the work to implement a semi implicit euler was done ..
now i am dealing with the tradeoffs between 'calculation time' which is quite expensive .. inverting a 0(n*n) sparse matrix /* once agian thanks to brecht for his work on making sparse matrices LU decomposition and evaluating inverses that easy*/ putting it into and cropping works pretty nice .. argh off topic again
...
while i spent a little time on reading recent papers i found :
1. control on springs needs to be split in pushing and pulling /* fabric pushes easy but pulls hard */
2. diagonals on 4-gons (in the current SB model) can be seen as shear .. thus need a contol to modify .. this commit wil add it
3. 2 nd order springs /*aka rigidity */ can focus on bending .. thus renaming 'em
i have no idea how i would provide backward compatiblity, but the spots i marked in code :)
Diffstat (limited to 'source/blender/blenkernel')
-rw-r--r-- | source/blender/blenkernel/BKE_softbody.h | 1 | ||||
-rw-r--r-- | source/blender/blenkernel/SConscript | 1 | ||||
-rw-r--r-- | source/blender/blenkernel/intern/softbody.c | 843 |
3 files changed, 769 insertions, 76 deletions
diff --git a/source/blender/blenkernel/BKE_softbody.h b/source/blender/blenkernel/BKE_softbody.h index 0f58b93730a..512ee67b36f 100644 --- a/source/blender/blenkernel/BKE_softbody.h +++ b/source/blender/blenkernel/BKE_softbody.h @@ -41,6 +41,7 @@ typedef struct BodyPoint { float origS[3], origE[3], origT[3], pos[3], vec[3], force[3]; float goal; float prevpos[3], prevvec[3], prevdx[3], prevdv[3]; /* used for Heun integration */ + float impdv[3],impdx[3]; int nofsprings; int *springs; float choke; float colball; diff --git a/source/blender/blenkernel/SConscript b/source/blender/blenkernel/SConscript index 9ecc76046c7..744ccf27491 100644 --- a/source/blender/blenkernel/SConscript +++ b/source/blender/blenkernel/SConscript @@ -8,6 +8,7 @@ incs += ' ../python ../render/extern/include #/intern/decimation/extern' incs += ' ../imbuf ../avi #/intern/elbeem/extern ../nodes' incs += ' #/intern/iksolver/extern ../blenloader ../quicktime' incs += ' #/intern/bmfont' +incs += ' #/intern/opennl/extern' incs += ' ' + env['BF_OPENGL_INC'] incs += ' ' + env['BF_ZLIB_INC'] diff --git a/source/blender/blenkernel/intern/softbody.c b/source/blender/blenkernel/intern/softbody.c index fdc03a88230..3e76d5a8cc0 100644 --- a/source/blender/blenkernel/intern/softbody.c +++ b/source/blender/blenkernel/intern/softbody.c @@ -87,6 +87,7 @@ variables on the UI for now #include "BIF_editdeform.h" #include "BIF_graphics.h" #include "PIL_time.h" +#include "ONL_opennl.h" /* callbacks for errors and interrupts and some goo */ static int (*SB_localInterruptCallBack)(void) = NULL; @@ -97,7 +98,7 @@ static int (*SB_localInterruptCallBack)(void) = NULL; typedef struct BodySpring { int v1, v2; - float len, strength, cf; + float len, strength, cf,load; float ext_force[3]; /* edges colliding and sailing */ short order; short flag; @@ -120,6 +121,11 @@ typedef struct SBScratch { float aabbmin[3],aabbmax[3]; }SBScratch; +#define NLF_BUILD 1 +#define NLF_SOLVE 2 + +#define MID_PRESERVE 1 + #define SOFTGOALSNAP 0.999f /* if bp-> goal is above make it a *forced follow original* and skip all ODE stuff for this bp removes *unnecessary* stiffnes from ODE system @@ -585,6 +591,7 @@ static void add_mesh_quad_diag_springs(Object *ob) if (ob->soft){ int nofquads; + float s_shear = ob->soft->shearstiff*ob->soft->shearstiff; nofquads = count_mesh_quads(me); if (nofquads) { @@ -605,12 +612,12 @@ static void add_mesh_quad_diag_springs(Object *ob) if(mface->v4) { bs->v1= mface->v1; bs->v2= mface->v3; - bs->strength= 1.0; + bs->strength= s_shear; bs->order =2; bs++; bs->v1= mface->v2; bs->v2= mface->v4; - bs->strength= 1.0; + bs->strength= s_shear; bs->order =2; bs++; @@ -692,6 +699,7 @@ static void add_2nd_order_springs(Object *ob,float stiffness) { int counter = 0; BodySpring *bs_new; + stiffness *=stiffness; add_2nd_order_roller(ob,stiffness,&counter,0); /* counting */ if (counter) { @@ -1450,6 +1458,7 @@ int sb_detect_edge_collisionCached(float edge_v1[3],float edge_v2[3],float *damp } + void scan_for_ext_spring_forces(Object *ob,float timenow) { SoftBody *sb = ob->soft; @@ -2095,8 +2104,110 @@ static int sb_deflect_face(Object *ob,float *actpos,float *facenormal,float *for return(deflected); } +static void dfdx_spring(int ia, int ic, int op, float dir[3],float L,float len,float factor) +{ + float m,delta_ij; + int i ,j; + if (L < len){ + for(i=0;i<3;i++) + for(j=0;j<3;j++){ + delta_ij = (i==j ? (1.0f): (0.0f)); + m=factor*(dir[i]*dir[j] + (1-L/len)*(delta_ij - dir[i]*dir[j])); + nlMatrixAdd(ia+i,op+ic+j,m); + } + } + else{ + for(i=0;i<3;i++) + for(j=0;j<3;j++){ + m=factor*dir[i]*dir[j]; + nlMatrixAdd(ia+i,op+ic+j,m); + } + } +} -static void softbody_calc_forces(Object *ob, float forcetime, float timenow) + +static void dfdx_goal(int ia, int ic, int op, float factor) +{ + int i; + for(i=0;i<3;i++) nlMatrixAdd(ia+i,op+ic+i,factor); +} + +static void dfdv_goal(int ia, int ic,float factor) +{ + int i; + for(i=0;i<3;i++) nlMatrixAdd(ia+i,ic+i,factor); +} +static void sb_spring_force(Object *ob,int bpi,BodySpring *bs,float iks,float forcetime,int nl_flags) +{ + SoftBody *sb= ob->soft; /* is supposed to be there */ + BodyPoint *bp1,*bp2; + + float dir[3],dvel[3]; + float distance,forcefactor,kd,absvel,projvel; + int ia,ic; + /* prepare depending on which side of the spring we are on */ + if (bpi == bs->v1){ + bp1 = &sb->bpoint[bs->v1]; + bp2 = &sb->bpoint[bs->v2]; + ia =3*bs->v1; + ic =3*bs->v2; + } + else if (bpi == bs->v2){ + bp1 = &sb->bpoint[bs->v2]; + bp2 = &sb->bpoint[bs->v1]; + ia =3*bs->v2; + ic =3*bs->v1; + } + else{ + /* TODO make this debug option */ + /**/ + printf("bodypoint <bpi> is not attached to spring <*bs> --> sb_spring_force()\n"); + return; + } + + /* do bp1 <--> bp2 elastic */ + VecSubf(dir,bp1->pos,bp2->pos); + distance = Normalize(dir); + if (bs->len < distance) + iks = 1.0f/(1.0f-sb->inspring)-1.0f ;/* inner spring constants function */ + else + iks = 1.0f/(1.0f-sb->inpush)-1.0f ;/* inner spring constants function */ + + if(bs->len > 0.0f) /* check for degenerated springs */ + forcefactor = iks/bs->len; + else + forcefactor = iks; + forcefactor *= bs->strength; + Vec3PlusStVec(bp1->force,(bs->len - distance)*forcefactor,dir); + + /* do bp1 <--> bp2 viscous */ + VecSubf(dvel,bp1->vec,bp2->vec); + kd = sb->infrict * sb_fric_force_scale(ob); + absvel = Normalize(dvel); + projvel = Inpf(dir,dvel); + kd *= absvel * projvel; + Vec3PlusStVec(bp1->force,-kd,dir); + + /* do jacobian stuff if needed */ + if(nl_flags & NLF_BUILD){ + int op =3*sb->totpoint; + float mvel = -forcetime*kd; + float mpos = -forcetime*forcefactor; + /* depending on my pos */ + dfdx_spring(ia,ia,op,dir,bs->len,distance,-mpos); + /* depending on my vel */ + dfdv_goal(ia,ia,mvel); // well that ignores geometie + if(bp2->goal < SOFTGOALSNAP){ /* ommit this bp when it snaps */ + /* depending on other pos */ + dfdx_spring(ia,ic,op,dir,bs->len,distance,mpos); + /* depending on other vel */ + dfdv_goal(ia,ia,-mvel); // well that ignores geometie + } + } +} + + +static void softbody_calc_forces(Object *ob, float forcetime, float timenow, int nl_flags) { /* rule we never alter free variables :bp->vec bp->pos in here ! * this will ruin adaptive stepsize AKA heun! (BM) @@ -2106,10 +2217,20 @@ static void softbody_calc_forces(Object *ob, float forcetime, float timenow) BodyPoint *bproot; BodySpring *bs; ListBase *do_effector; - float iks, ks, kd, gravity, actspringlen, forcefactor, sd[3]; + float iks, ks, kd, gravity; float fieldfactor = 1000.0f, windfactor = 250.0f; float tune = sb->ballstiff; int a, b, do_deflector,do_selfcollision,do_springcollision,do_aero; + + + + NLboolean success; + + if(nl_flags){ + nlBegin(NL_SYSTEM); + nlBegin(NL_MATRIX); + } + gravity = sb->grav * sb_grav_force_scale(ob); @@ -2131,7 +2252,7 @@ static void softbody_calc_forces(Object *ob, float forcetime, float timenow) float defforce[3]; do_deflector = sb_detect_aabb_collisionCached(defforce,ob->lay,ob,timenow); } - +/* if (do_selfcollision ){ float ce[3]; VecMidf(ce,sb->scratch->aabbmax,sb->scratch->aabbmin); @@ -2139,10 +2260,33 @@ static void softbody_calc_forces(Object *ob, float forcetime, float timenow) bp->octantflag = set_octant_flags(ce,bp->pos,bp->colball); } } - +*/ for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) { /* clear forces accumulator */ bp->force[0]= bp->force[1]= bp->force[2]= 0.0; + if(nl_flags & NLF_BUILD){ + int ia =3*(sb->totpoint-a); + int op =3*sb->totpoint; + /* dF/dV = v */ + + nlMatrixAdd(op+ia,ia,-forcetime); + nlMatrixAdd(op+ia+1,ia+1,-forcetime); + nlMatrixAdd(op+ia+2,ia+2,-forcetime); + + + /* we need [unit - h* dF/dy]^-1 */ + + nlMatrixAdd(ia,ia,1); + nlMatrixAdd(ia+1,ia+1,1); + nlMatrixAdd(ia+2,ia+2,1); + + nlMatrixAdd(op+ia,op+ia,1); + nlMatrixAdd(op+ia+1,op+ia+1,1); + nlMatrixAdd(op+ia+2,op+ia+2,1); + + + + } /* naive ball self collision */ /* needs to be done if goal snaps or not */ @@ -2156,7 +2300,7 @@ static void softbody_calc_forces(Object *ob, float forcetime, float timenow) for(c=sb->totpoint, obp= sb->bpoint; c>=a; c--, obp++) { - if ((bp->octantflag & obp->octantflag) == 0) continue; + //if ((bp->octantflag & obp->octantflag) == 0) continue; compare = (obp->colball + bp->colball); VecSubf(def, bp->pos, obp->pos); @@ -2182,8 +2326,33 @@ static void softbody_calc_forces(Object *ob, float forcetime, float timenow) VecSubf(dvel,velcenter,bp->vec); VecMulf(dvel,sb->nodemass); - Vec3PlusStVec(bp->force,sb->balldamp,dvel); Vec3PlusStVec(bp->force,f*(1.0f-sb->balldamp),def); + Vec3PlusStVec(bp->force,sb->balldamp,dvel); + + if(nl_flags & NLF_BUILD){ + int ia =3*(sb->totpoint-a); + int ic =3*(sb->totpoint-c); + int op =3*sb->totpoint; + float mvel = forcetime*sb->nodemass*sb->balldamp; + float mpos = forcetime*tune*(1.0f-sb->balldamp); + /*some quick and dirty entries to the jacobian*/ + dfdx_goal(ia,ia,op,mpos); + dfdv_goal(ia,ia,mvel); + /* exploit force(a,b) == -force(b,a) part1/2 */ + dfdx_goal(ic,ic,op,mpos); + dfdv_goal(ic,ic,mvel); + + + /*TODO sit down an X-out the true jacobian entries*/ + /*well does not make to much sense because the eigenvalues + of the jacobian go negative; and negative eigenvalues + on a complex iterative system z(n+1)=A * z(n) + give imaginary roots in the charcateristic polynom + --> solutions that to z(t)=u(t)* exp ( i omega t) --> oscilations we don't want here + where u(t) is a unknown amplitude function (worst case rising fast) + */ + } + /* exploit force(a,b) == -force(b,a) part2/2 */ VecSubf(dvel,velcenter,obp->vec); VecMulf(dvel,sb->nodemass); @@ -2191,6 +2360,7 @@ static void softbody_calc_forces(Object *ob, float forcetime, float timenow) Vec3PlusStVec(obp->force,sb->balldamp,dvel); Vec3PlusStVec(obp->force,-f*(1.0f-sb->balldamp),def); + } } } @@ -2201,23 +2371,39 @@ static void softbody_calc_forces(Object *ob, float forcetime, float timenow) float auxvect[3]; float velgoal[3]; float absvel =0, projvel= 0; - /* do goal stuff */ if(ob->softflag & OB_SB_GOAL) { /* true elastic goal */ - VecSubf(auxvect,bp->origT,bp->pos); + VecSubf(auxvect,bp->pos,bp->origT); ks = 1.0f/(1.0f- bp->goal*sb->goalspring)-1.0f ; - bp->force[0]+= ks*(auxvect[0]); - bp->force[1]+= ks*(auxvect[1]); - bp->force[2]+= ks*(auxvect[2]); + bp->force[0]+= -ks*(auxvect[0]); + bp->force[1]+= -ks*(auxvect[1]); + bp->force[2]+= -ks*(auxvect[2]); + + if(nl_flags & NLF_BUILD){ + int ia =3*(sb->totpoint-a); + int op =3*(sb->totpoint); + /* depending on my pos */ + dfdx_goal(ia,ia,op,ks*forcetime); + } + + /* calulate damping forces generated by goals*/ VecSubf(velgoal,bp->origS, bp->origE); kd = sb->goalfrict * sb_fric_force_scale(ob) ; + VecAddf(auxvect,velgoal,bp->vec); if (forcetime > 0.0 ) { /* make sure friction does not become rocket motor on time reversal */ - bp->force[0]-= kd * (velgoal[0] + bp->vec[0]); - bp->force[1]-= kd * (velgoal[1] + bp->vec[1]); - bp->force[2]-= kd * (velgoal[2] + bp->vec[2]); + bp->force[0]-= kd * (auxvect[0]); + bp->force[1]-= kd * (auxvect[1]); + bp->force[2]-= kd * (auxvect[2]); + if(nl_flags & NLF_BUILD){ + int ia =3*(sb->totpoint-a); + Normalize(auxvect); + /* depending on my vel */ + dfdv_goal(ia,ia,kd*forcetime); + } + } else { bp->force[0]-= kd * (velgoal[0] - bp->vec[0]); @@ -2230,6 +2416,7 @@ static void softbody_calc_forces(Object *ob, float forcetime, float timenow) /* gravitation */ bp->force[2]-= gravity*sb->nodemass; /* individual mass of node here */ + //bp->force[1]-= gravity*sb->nodemass; /* individual mass of node here */ /* particle field & vortex */ @@ -2260,6 +2447,16 @@ static void softbody_calc_forces(Object *ob, float forcetime, float timenow) bp->force[1]-= bp->vec[1]*kd; bp->force[2]-= bp->vec[2]*kd; /* friction in media done */ + if(nl_flags & NLF_BUILD){ + int ia =3*(sb->totpoint-a); + int op =3*sb->totpoint; + /* da/dv = */ + + nlMatrixAdd(ia,ia,forcetime*kd); + nlMatrixAdd(ia+1,ia+1,forcetime*kd); + nlMatrixAdd(ia+2,ia+2,forcetime*kd); + } + } /* +++cached collision targets */ bp->choke = 0.0f; @@ -2269,7 +2466,7 @@ static void softbody_calc_forces(Object *ob, float forcetime, float timenow) kd = 1.0f; if (sb_deflect_face(ob,bp->pos,facenormal,defforce,&cf,timenow,vel,&intrusion)){ - if (intrusion < 0.0f){ + if ((!nl_flags)&&(intrusion < 0.0f)){ /*bjornmose: uugh.. what an evil hack violation of the 'don't touch bp->pos in here' rule but works nice, like this--> @@ -2287,8 +2484,15 @@ static void softbody_calc_forces(Object *ob, float forcetime, float timenow) VECSUB(cfforce,bp->vec,vel); Vec3PlusStVec(bp->force,-cf*50.0f,cfforce); } - Vec3PlusStVec(bp->force,kd,defforce); + if (nl_flags & NLF_BUILD){ + int ia =3*(sb->totpoint-a); + int op =3*sb->totpoint; + //dfdx_goal(ia,ia,op,mpos); // don't do unless you know + //dfdv_goal(ia,ia,-cf); + + } + } } @@ -2305,48 +2509,8 @@ static void softbody_calc_forces(Object *ob, float forcetime, float timenow) bp->choke = bs->cf; } - - if (( (sb->totpoint-a) == bs->v1) ){ - VecSubf(sd,(bproot+bs->v2)->pos, bp->pos); - actspringlen=Normalize(sd); - - /* friction stuff V1 */ - VecSubf(velgoal,bp->vec,(bproot+bs->v2)->vec); - kd = sb->infrict * sb_fric_force_scale(ob); - absvel = Normalize(velgoal); - projvel = ABS(Inpf(sd,velgoal)); - kd *= absvel * projvel; - Vec3PlusStVec(bp->force,-kd,velgoal); - - if(bs->len > 0.0f) /* check for degenerated springs */ - forcefactor = (bs->len - actspringlen)/bs->len * iks; - else - forcefactor = actspringlen * iks; - forcefactor *= bs->strength; - - Vec3PlusStVec(bp->force,-forcefactor,sd); - - } - - if (( (sb->totpoint-a) == bs->v2) ){ - VecSubf(sd,bp->pos,(bproot+bs->v1)->pos); - actspringlen=Normalize(sd); - - /* friction stuff V2 */ - VecSubf(velgoal,bp->vec,(bproot+bs->v1)->vec); - kd = sb->infrict * sb_fric_force_scale(ob); - absvel = Normalize(velgoal); - projvel = ABS(Inpf(sd,velgoal)); - kd *= absvel * projvel; - Vec3PlusStVec(bp->force,-kd,velgoal); - - if(bs->len > 0.0) - forcefactor = (bs->len - actspringlen)/bs->len * iks; - else - forcefactor = actspringlen * iks; - forcefactor *= bs->strength; - Vec3PlusStVec(bp->force,+forcefactor,sd); - } + // sb_spring_force(Object *ob,int bpi,BodySpring *bs,float iks,float forcetime,int nl_flags) + sb_spring_force(ob,sb->totpoint-a,bs,iks,forcetime,nl_flags); }/* loop springs */ }/* existing spring list */ }/*any edges*/ @@ -2356,14 +2520,206 @@ static void softbody_calc_forces(Object *ob, float forcetime, float timenow) /* finally add forces caused by face collision */ - if (ob->softflag & OB_SB_FACECOLL) scan_for_ext_face_forces(ob,timenow); + if (ob->softflag & OB_SB_FACECOLL) scan_for_ext_face_forces(ob,timenow); + + /* finish matrix and solve */ + if(nl_flags & NLF_SOLVE){ + //double sct,sst=PIL_check_seconds_timer(); + for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) { + int iv =3*(sb->totpoint-a); + int ip =3*(2*sb->totpoint-a); + int n; + for (n=0;n<3;n++) {nlRightHandSideSet(0, iv+n, bp->force[0+n]);} + for (n=0;n<3;n++) {nlRightHandSideSet(0, ip+n, bp->vec[0+n]);} + } + nlEnd(NL_MATRIX); + nlEnd(NL_SYSTEM); + + if ((G.rt >0) && (nl_flags & NLF_BUILD)) + { + printf("####MEE#####\n"); + nlPrintMatrix(); + } + + success= nlSolveAdvanced(NULL, 1); + + // nlPrintMatrix(); /* for debug purpose .. anyhow cropping B vector looks like working */ + if(success){ + float f; + int index =0; + /* for debug purpose .. anyhow cropping B vector looks like working */ + if (G.rt >0) + for(a=2*sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) { + f=nlGetVariable(0,index); + printf("(%f ",f);index++; + f=nlGetVariable(0,index); + printf("%f ",f);index++; + f=nlGetVariable(0,index); + printf("%f)",f);index++; + } + + index =0; + for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) { + f=nlGetVariable(0,index); + bp->impdv[0] = f; index++; + f=nlGetVariable(0,index); + bp->impdv[1] = f; index++; + f=nlGetVariable(0,index); + bp->impdv[2] = f; index++; + } + /* + for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) { + f=nlGetVariable(0,index); + bp->impdx[0] = f; index++; + f=nlGetVariable(0,index); + bp->impdx[1] = f; index++; + f=nlGetVariable(0,index); + bp->impdx[2] = f; index++; + } + */ + } + else{ + printf("Matrix inversion failed \n"); + for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) { + VECCOPY(bp->impdv,bp->force); + } + + } + + //sct=PIL_check_seconds_timer(); + //if (sct-sst > 0.01f) printf(" implicit solver time %f %s \r",sct-sst,ob->id.name); + } /* cleanup */ + if(do_effector) pdEndEffectors(do_effector); } +static void softbody_apply_fake_implicit_forces(Object *ob, float forcetime, float *err, float *err_ref_pos,float *err_ref_vel) +{ + /* time evolution */ + /* do semi implicit euler */ + SoftBody *sb= ob->soft; /* is supposed to be there */ + BodyPoint *bp; + float *perp,*perv; + float erp[3],erv[3],dx[3],dv[3],aabbmin[3],aabbmax[3],cm[3]={0.0f,0.0f,0.0f}; + float timeovermass; + float maxerrpos= 0.0f,maxerrvel = 0.0f,maxerrvel2 = 0.0f; + int a,fuzzy=0; + + forcetime *= sb_time_scale(ob); + + aabbmin[0]=aabbmin[1]=aabbmin[2] = 1e20f; + aabbmax[0]=aabbmax[1]=aabbmax[2] = -1e20f; + + /* claim a minimum mass for vertex */ + if (sb->nodemass > 0.009999f) timeovermass = forcetime/sb->nodemass; + else timeovermass = forcetime/0.009999f; + /* step along error reference vector */ + perp =err_ref_pos; + perv =err_ref_vel; -static void softbody_apply_forces(Object *ob, float forcetime, int mode, float *err) + for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) { + /* step along error reference vector */ + if(perp) {VECCOPY(erp,perp);perp +=3;} + if(perv) {VECCOPY(erv,perv);perv +=3;} + if(bp->goal < SOFTGOALSNAP){ + + /* so here is (v)' = a(cceleration) = sum(F_springs)/m + gravitation + some friction forces + more forces*/ + /* the ( ... )' operator denotes derivate respective time */ + /* the "implicit" euler step for velocity then becomes */ + /* v(t + dt) = v(t) + O * X(t) * dt */ + /* O = [1-dt*dA/d(X)]^1 */ + /* with X = (a[n],v[n]) */ + /* da/dv | da/dx */ + /* dA/d(X) = ( ------------- )*/ + /* dv/dv | dv/dx */ + /* note!! we did not solve any implicit system but looking forward more or less smart + what a implicit solution may look like by means of taylor expansion */ + VECCOPY(dx,bp->vec); + + bp->force[0]= timeovermass * bp->impdv[0]; /* individual mass of node here */ + bp->force[1]= timeovermass * bp->impdv[1]; + bp->force[2]= timeovermass * bp->impdv[2]; + VECCOPY(dv,bp->force); + if(perv){ + maxerrvel = MAX2(maxerrvel,ABS(dv[0] - erv[0])); + maxerrvel = MAX2(maxerrvel,ABS(dv[1] - erv[1])); + maxerrvel = MAX2(maxerrvel,ABS(dv[2] - erv[2])); + } + + maxerrvel2 = MAX2(maxerrvel2,ABS(dv[0])); + maxerrvel2 = MAX2(maxerrvel2,ABS(dv[1])); + maxerrvel2 = MAX2(maxerrvel2,ABS(dv[2])); + + VECADD(bp->vec, bp->vec, dv); + + /* so here is (x)'= v(elocity) */ + /* the euler step for location then becomes */ + /* x(t + dt) = x(t) + v(t) * dt */ + +// VECCOPY(dx,bp->vec); + + dx[0]*=forcetime; + dx[1]*=forcetime; + dx[2]*=forcetime; + + /* bp->choke is set when we need to pull a vertex or edge out of the collider. + the collider object signals to get out by pushing hard. on the other hand + we don't want to end up in deep space so we add some <viscosity> + to balance that out */ + if (bp->choke > 0.0f){ + dx[0]*=(1.0f - bp->choke); + dx[1]*=(1.0f - bp->choke); + dx[2]*=(1.0f - bp->choke); + } + + + /* should be possible to get more out of the matrix inversion + but not verified yet + dx[0]*=forcetime*forcetime*bp->impdx[0]; + dx[1]*=forcetime*forcetime*bp->impdx[1]; + dx[2]*=forcetime*forcetime*bp->impdx[2]; + */ + VECADD(bp->pos, bp->pos, dx); + if (perp){ + maxerrpos = MAX2(maxerrpos,ABS(bp->pos[0]-erp[0])); + maxerrpos = MAX2(maxerrpos,ABS(bp->pos[1]-erp[1])); + maxerrpos = MAX2(maxerrpos,ABS(bp->pos[2]-erp[2])); + } + + + }/*snap*/ + /* so while we are looping BPs anyway do statistics on the fly */ + aabbmin[0] = MIN2(aabbmin[0],bp->pos[0]); + aabbmin[1] = MIN2(aabbmin[1],bp->pos[1]); + aabbmin[2] = MIN2(aabbmin[2],bp->pos[2]); + aabbmax[0] = MAX2(aabbmax[0],bp->pos[0]); + aabbmax[1] = MAX2(aabbmax[1],bp->pos[1]); + aabbmax[2] = MAX2(aabbmax[2],bp->pos[2]); + if (bp->flag & SBF_DOFUZZY) fuzzy =1; + } /*for*/ + + if (sb->totpoint) VecMulf(cm,1.0f/sb->totpoint); + if (sb->scratch){ + VECCOPY(sb->scratch->aabbmin,aabbmin); + VECCOPY(sb->scratch->aabbmax,aabbmax); + } + + if (err){ /* so step size will be controlled by biggest difference in slope */ + if (sb->solverflags & SBSO_OLDERR) + *err = MAX2(maxerrpos,maxerrvel); + else + if (perp) *err = maxerrpos; + else *err = maxerrvel2; + //printf("EP %f EV %f \n",maxerrpos,maxerrvel); + if (fuzzy){ + *err /= sb->fuzzyness; + } + } +} + +static void softbody_apply_forces(Object *ob, float forcetime, int mode, float *err, int mid_flags) { /* time evolution */ /* actually does an explicit euler step mode == 0 */ @@ -2386,6 +2742,7 @@ static void softbody_apply_forces(Object *ob, float forcetime, int mode, float * for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) { if(bp->goal < SOFTGOALSNAP){ + if(mid_flags & MID_PRESERVE) VECCOPY(dx,bp->vec); /* so here is (v)' = a(cceleration) = sum(F_springs)/m + gravitation + some friction forces + more forces*/ /* the ( ... )' operator denotes derivate respective time */ @@ -2417,8 +2774,7 @@ static void softbody_apply_forces(Object *ob, float forcetime, int mode, float * /* so here is (x)'= v(elocity) */ /* the euler step for location then becomes */ /* x(t + dt) = x(t) + v(t) * dt */ - - VECCOPY(dx,bp->vec); + if(!(mid_flags & MID_PRESERVE)) VECCOPY(dx,bp->vec); dx[0]*=forcetime; dx[1]*=forcetime; dx[2]*=forcetime; @@ -2490,6 +2846,79 @@ static void softbody_restore_prev_step(Object *ob) } } +static void softbody_store_step(Object *ob) +{ + SoftBody *sb= ob->soft; /* is supposed to be there*/ + BodyPoint *bp; + int a; + + for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) { + VECCOPY(bp->prevvec,bp->vec); + VECCOPY(bp->prevpos,bp->pos); + } +} + + +/* used by predictors and correctors */ +static void softbody_store_state(Object *ob,float *ppos,float *pvel) +{ + SoftBody *sb= ob->soft; /* is supposed to be there*/ + BodyPoint *bp; + int a; + float *pp=ppos,*pv=pvel; + + for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) { + + VECCOPY(pv, bp->vec); + pv+=3; + + VECCOPY(pp, bp->pos); + pp+=3; + } +} + +/* used by predictors and correctors */ +static void softbody_retrieve_state(Object *ob,float *ppos,float *pvel) +{ + SoftBody *sb= ob->soft; /* is supposed to be there*/ + BodyPoint *bp; + int a; + float *pp=ppos,*pv=pvel; + + for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) { + + VECCOPY(bp->vec,pv); + pv+=3; + + VECCOPY(bp->pos,pp); + pp+=3; + } +} + +/* used by predictors and correctors */ +static void softbody_swap_state(Object *ob,float *ppos,float *pvel) +{ + SoftBody *sb= ob->soft; /* is supposed to be there*/ + BodyPoint *bp; + int a; + float *pp=ppos,*pv=pvel; + float temp[3]; + + for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) { + + VECCOPY(temp, bp->vec); + VECCOPY(bp->vec,pv); + VECCOPY(pv,temp); + pv+=3; + + VECCOPY(temp, bp->pos); + VECCOPY(bp->pos,pp); + VECCOPY(pp,temp); + pp+=3; + } +} + + /* care for bodypoints taken out of the 'ordinary' solver step ** because they are screwed to goal by bolts ** they just need to move along with the goal in time @@ -3193,6 +3622,8 @@ SoftBody *sbNew(void) sb->inspring= 0.5f; sb->infrict= 0.5f; + /*todo backward file compat should copy infrict to inpush while reading old files*/ + sb->inpush = 0.5f; sb->interval= 10; sb->sfra= G.scene->r.sfra; @@ -3205,9 +3636,13 @@ SoftBody *sbNew(void) sb->minloops = 10; + sb->maxloops = 300; sb->choke = 3; sb_new_scratch(sb); + /*todo backward file compat should set sb->shearstiff = 1.0f while reading old files*/ + sb->shearstiff = 1.0f; + sb->solverflags |= SBSO_OLDERR; return sb; } @@ -3256,7 +3691,7 @@ void sbObjectStep(Object *ob, float framenr, float (*vertexCos)[3], int numVerts HairKey *key= NULL; BodyPoint *bp; int a; - float dtime,ctime,forcetime,err; + float dtime,ctime,forcetime; float hairmat[4][4]; /* This part only sets goals and springs, based on original mesh/curve/lattice data. @@ -3446,8 +3881,10 @@ void sbObjectStep(Object *ob, float framenr, float (*vertexCos)[3], int numVerts } - if (TRUE) { /* */ + if (sb->solver_ID < 2) { /* special case of 2nd order Runge-Kutta type AKA Heun */ + int mid_flags=0; + float err = 0; float forcetimemax = 1.0f; float forcetimemin = 0.001f; float timedone =0.0; /* how far did we get without violating error condition */ @@ -3459,7 +3896,8 @@ void sbObjectStep(Object *ob, float framenr, float (*vertexCos)[3], int numVerts if (sb->minloops > 0) forcetimemax = 1.0f / sb->minloops; if (sb->maxloops > 0) forcetimemin = 1.0f / sb->maxloops; - + + if(sb->solver_ID>0) mid_flags |= MID_PRESERVE; //forcetime = dtime; /* hope for integrating in one step */ forcetime =forcetimemax; /* hope for integrating in one step */ @@ -3470,13 +3908,13 @@ void sbObjectStep(Object *ob, float framenr, float (*vertexCos)[3], int numVerts sb->scratch->flag &= ~SBF_DOFUZZY; /* do predictive euler step */ - softbody_calc_forces(ob, forcetime,timedone/dtime); - softbody_apply_forces(ob, forcetime, 1, NULL); + softbody_calc_forces(ob, forcetime,timedone/dtime,0); + softbody_apply_forces(ob, forcetime, 1, NULL,mid_flags); /* crop new slope values to do averaged slope step */ - softbody_calc_forces(ob, forcetime,timedone/dtime); - softbody_apply_forces(ob, forcetime, 2, &err); + softbody_calc_forces(ob, forcetime,timedone/dtime,0); + softbody_apply_forces(ob, forcetime, 2, &err,mid_flags); softbody_apply_goalsnap(ob); @@ -3517,6 +3955,7 @@ void sbObjectStep(Object *ob, float framenr, float (*vertexCos)[3], int numVerts sct=PIL_check_seconds_timer(); if (sct-sst > 0.5f) printf("%3.0f%% \r",100.0f*timedone); } + /* ask for user break */ if (SB_localInterruptCallBack && SB_localInterruptCallBack()) break; } @@ -3531,11 +3970,263 @@ void sbObjectStep(Object *ob, float framenr, float (*vertexCos)[3], int numVerts } } + else if (sb->solver_ID == 2) + { + /* do semi "fake" implicit euler */ + float *predict_vel=NULL,*predict_pos=NULL; /* for BDF style stepping */ + NLContext *sb_nlc = NULL; + int npredict=0,predict_mem_size; + int nvar = 3*2*sb->totpoint; + int loops =0 ; + float forcetimemax = 1.0f; // this one needs 5 steps as a minimum + float forcetimemin = 0.001f; + float timedone =0.0; /* how far did we get without violating error condition */ + /* loops = counter for emergency brake + * we don't want to lock up the system if physics fail + */ + SoftHeunTol = sb->rklimit; /* humm .. this should be calculated from sb parameters and sizes */ + if (sb->minloops > 0) forcetimemax = 1.0f / sb->minloops; + if (sb->maxloops > 0) forcetimemin = 1.0f / sb->maxloops; + + + forcetime =forcetimemax; /* hope for integrating as fast as possible */ + + //allocate predictor buffers + npredict =1; + predict_mem_size =3*sizeof(float)*npredict*sb->totpoint; + predict_pos = MEM_mallocN(predict_mem_size,"SB_predict_pos"); + predict_vel = MEM_mallocN(predict_mem_size,"SB_predict_vel"); + + + while ( (ABS(timedone) < ABS(dtime)) && (loops < 2000) ){ + float loss_of_accuracy=0; + // create new solver context for this loop + sb_nlc = (NLContext*)nlNewContext(); + /* hum it smells like threading trouble */ + nlSolverParameteri(NL_NB_VARIABLES, nvar); + nlSolverParameteri(NL_LEAST_SQUARES, NL_FALSE); + + interpolate_exciter(ob,200,(int)(200.0*(timedone/dtime))); + softbody_store_step(ob); /* used for rolling back step guessing */ + softbody_store_state(ob,predict_pos,predict_vel); + softbody_calc_forces(ob, forcetime,timedone/dtime,NLF_BUILD|NLF_SOLVE); + // go full step + softbody_apply_fake_implicit_forces(ob, forcetime, NULL,NULL,NULL); + + // restore old situation and store 1rst solution to predictors + softbody_swap_state(ob,predict_pos,predict_vel); + // the following is to find out how good we were + // may be we can do smarter + // so now using the forces and jacobian we calculated before + // go only 1/2 + softbody_apply_fake_implicit_forces(ob, forcetime/2.0f, NULL,NULL,NULL); + // explore situation here without redoing the jacobian + softbody_calc_forces(ob, forcetime,timedone/dtime,NLF_SOLVE); + // go next 1/2 + softbody_apply_fake_implicit_forces(ob, forcetime/2.0f,&loss_of_accuracy,predict_pos,predict_vel ); + // now we have "loss_of_accuracy" + + softbody_apply_goalsnap(ob); + + if (loss_of_accuracy > SoftHeunTol) { /* error needs to be scaled to some quantity */ + + if (forcetime > forcetimemin){ + forcetime = MAX2(forcetime / 2.0f,forcetimemin); + softbody_restore_prev_step(ob); + //printf("down,"); + } + else { + timedone += forcetime; + } + } + else { + float newtime = forcetime * 1.5f; /* hope for 1.1 times better conditions in next step */ + // all that 1/2 stepping was useless ... hum we know now .. + softbody_swap_state(ob,predict_pos,predict_vel); + if (0){//(sb->scratch->flag & SBF_DOFUZZY){ + //if (err > SoftHeunTol/(2.0f*sb->fuzzyness)) { /* stay with this stepsize unless err really small */ + newtime = forcetime; + //} + } + else { + if (loss_of_accuracy > SoftHeunTol/1.1f) { /* stay with this stepsize unless err really small */ + newtime = forcetime; + } + } + + timedone += forcetime; + newtime=MIN2(forcetimemax,MAX2(newtime,forcetimemin)); + //if (newtime > forcetime) printf("up,"); + if (forcetime > 0.0) + forcetime = MIN2(dtime - timedone,newtime); + else + forcetime = MAX2(dtime - timedone,newtime); + } + loops++; + // give away solver context within loop + if (sb_nlc) + { + if (sb_nlc != nlGetCurrent())printf("Aye NL context mismatch! in softbody.c !\n"); + nlDeleteContext(sb_nlc); + sb_nlc = NULL; + } + + if(sb->solverflags & SBSO_MONITOR ){ + sct=PIL_check_seconds_timer(); + if (sct-sst > 0.5f) printf("%3.0f%% \r",100.0f*timedone); + } + + /* ask for user break */ + if (SB_localInterruptCallBack && SB_localInterruptCallBack()) break; + } + + // give away buffers + if (predict_pos) MEM_freeN(predict_pos); + if (predict_vel) MEM_freeN(predict_vel); + + if(sb->solverflags & SBSO_MONITOR ){ + if (loops > HEUNWARNLIMIT) /* monitor high loop counts */ + printf("\r needed %d steps/frame ",loops); + } + + + }/*SOLVER SELECT*/ + else if (sb->solver_ID == 4) + { + /* do semi "fake" implicit euler */ + NLContext *sb_nlc = NULL; + int nvar = 3*2*sb->totpoint; + int loops =0 ; + float forcetimemax = 1.0f; // this one needs 5 steps as a minimum + float forcetimemin = 0.001f; + float timedone =0.0; /* how far did we get without violating error condition */ + /* loops = counter for emergency brake + * we don't want to lock up the system if physics fail + */ + SoftHeunTol = sb->rklimit; /* humm .. this should be calculated from sb parameters and sizes */ + if (sb->minloops > 0) forcetimemax = 1.0f / sb->minloops; + if (sb->maxloops > 0) forcetimemin = 1.0f / sb->maxloops; + + + forcetime =forcetimemax; /* hope for integrating as fast as possible */ + while ( (ABS(timedone) < ABS(dtime)) && (loops < 2000) ){ + float loss_of_accuracy=0; + // create new solver context for this loop + sb_nlc = (NLContext*)nlNewContext(); + /* hum it smells like threading trouble */ + nlSolverParameteri(NL_NB_VARIABLES, nvar); + nlSolverParameteri(NL_LEAST_SQUARES, NL_FALSE); + + + interpolate_exciter(ob,200,(int)(200.0*(timedone/dtime))); + softbody_store_step(ob); /* used for rolling back step guessing */ + softbody_calc_forces(ob, forcetime,timedone/dtime,NLF_BUILD|NLF_SOLVE); + softbody_apply_fake_implicit_forces(ob, forcetime,&loss_of_accuracy,NULL,NULL); + softbody_apply_goalsnap(ob); + + if (loss_of_accuracy > SoftHeunTol) { /* error needs to be scaled to some quantity */ + + if (forcetime > forcetimemin){ + forcetime = MAX2(forcetime / 2.0f,forcetimemin); + softbody_restore_prev_step(ob); + //printf("down,"); + } + else { + timedone += forcetime; + } + } + else { + float newtime = forcetime * 1.1f; /* hope for 1.1 times better conditions in next step */ + if (0){//(sb->scratch->flag & SBF_DOFUZZY){ + //if (err > SoftHeunTol/(2.0f*sb->fuzzyness)) { /* stay with this stepsize unless err really small */ + newtime = forcetime; + //} + } + else { + if (loss_of_accuracy > SoftHeunTol/2.0f) { /* stay with this stepsize unless err really small */ + newtime = forcetime; + } + } + + timedone += forcetime; + newtime=MIN2(forcetimemax,MAX2(newtime,forcetimemin)); + //if (newtime > forcetime) printf("up,"); + if (forcetime > 0.0) + forcetime = MIN2(dtime - timedone,newtime); + else + forcetime = MAX2(dtime - timedone,newtime); + } + loops++; + // give away solver context within loop + if (sb_nlc) + { + if (sb_nlc != nlGetCurrent())printf("Aye NL context mismatch! in softbody.c !\n"); + nlDeleteContext(sb_nlc); + sb_nlc = NULL; + } + + if(sb->solverflags & SBSO_MONITOR ){ + sct=PIL_check_seconds_timer(); + if (sct-sst > 0.5f) printf("%3.0f%% \r",100.0f*timedone); + } + + /* ask for user break */ + if (SB_localInterruptCallBack && SB_localInterruptCallBack()) break; + } + + + if(sb->solverflags & SBSO_MONITOR ){ + if (loops > HEUNWARNLIMIT) /* monitor high loop counts */ + printf("\r needed %d steps/frame ",loops); + } + + + }/*SOLVER SELECT*/ + else if (sb->solver_ID == 3){ + /* do "stupid" semi "fake" implicit euler */ + NLContext *sb_nlc = NULL; + int nvar = 3*2*sb->totpoint; + int loops =0 ; + float forcetimemax = 1.0f; // this one needs 5 steps as a minimum + float timedone =0.0; /* how far did we get without violating error condition */ + /* loops = counter for emergency brake + * we don't want to lock up the system if physics fail + */ + if (sb->minloops > 0) forcetimemax = 1.0f / sb->minloops; + + + forcetime =forcetimemax; /* hope for integrating as fast as possible */ + + while ( (ABS(timedone) < ABS(dtime)) && (loops < 2000) ){ + + sb_nlc = (NLContext*)nlNewContext(); + /* hum it smells like threading trouble */ + nlSolverParameteri(NL_NB_VARIABLES, nvar); + nlSolverParameteri(NL_LEAST_SQUARES, NL_FALSE); + + interpolate_exciter(ob,200,(int)(200.0*(timedone/dtime))); + softbody_calc_forces(ob, forcetime,timedone/dtime,NLF_BUILD|NLF_SOLVE); + softbody_apply_fake_implicit_forces(ob, forcetime, NULL,NULL,NULL); + softbody_apply_goalsnap(ob); + timedone += forcetime; + loops++; + if (sb_nlc) + { + if (sb_nlc != nlGetCurrent())printf("Aye NL context mismatch! in softbody.c !\n"); + nlDeleteContext(sb_nlc); + sb_nlc = NULL; + } + /* ask for user break */ + if (SB_localInterruptCallBack && SB_localInterruptCallBack()) break; + } + + + + }/*SOLVER SELECT*/ else{ - /* do brute force explicit euler */ - /* removed but left this branch for better integrators / solvers (BM) */ - /* yah! Nicholas Guttenberg (NichG) here is the place to plug in */ - } + printf("softbody no valid solver ID!"); + }/*SOLVER SELECT*/ + if(sb->solverflags & SBSO_MONITOR ){ sct=PIL_check_seconds_timer(); if (sct-sst > 0.5f) printf(" solver time %f %s \r",sct-sst,ob->id.name); |