Orange branch: Revived hidden treasure, the Groups!
[blender.git] / source / blender / blenkernel / intern / softbody.c
1 /*  softbody.c      
2  * 
3  * $Id$
4  *
5  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
6  *
7  * This program is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU General Public License
9  * as published by the Free Software Foundation; either version 2
10  * of the License, or (at your option) any later version. The Blender
11  * Foundation also sells licenses for use in proprietary software under
12  * the Blender License.  See http://www.blender.org/BL/ for information
13  * about this.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
18  * GNU General Public License for more details.
19  *
20  * You should have received a copy of the GNU General Public License
21  * along with this program; if not, write to the Free Software Foundation,
22  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
23  *
24  * The Original Code is Copyright (C) Blender Foundation
25  * All rights reserved.
26  *
27  * The Original Code is: all of this file.
28  *
29  * Contributor(s): none yet.
30  *
31  * ***** END GPL/BL DUAL LICENSE BLOCK *****
32  */
33
34 /*
35 ******
36 variables on the UI for now
37
38         float mediafrict;  friction to env 
39         float nodemass;   softbody mass of *vertex* 
40         float grav;        softbody amount of gravitaion to apply 
41         
42         float goalspring;  softbody goal springs 
43         float goalfrict;   softbody goal springs friction 
44         float mingoal;     quick limits for goal 
45         float maxgoal;
46
47         float inspring;   softbody inner springs 
48         float infrict;     softbody inner springs friction 
49
50 *****
51 */
52
53
54 #include <math.h>
55 #include <stdlib.h>
56 #include <string.h>
57
58 #include "MEM_guardedalloc.h"
59
60 /* types */
61 #include "DNA_curve_types.h"
62 #include "DNA_object_types.h"
63 #include "DNA_object_force.h"   /* here is the softbody struct */
64 #include "DNA_key_types.h"
65 #include "DNA_mesh_types.h"
66 #include "DNA_meshdata_types.h"
67 #include "DNA_lattice_types.h"
68 #include "DNA_scene_types.h"
69
70 #include "BLI_blenlib.h"
71 #include "BLI_arithb.h"
72
73 #include "BKE_curve.h"
74 #include "BKE_displist.h"
75 #include "BKE_effect.h"
76 #include "BKE_global.h"
77 #include "BKE_key.h"
78 #include "BKE_object.h"
79 #include "BKE_softbody.h"
80 #include "BKE_utildefines.h"
81 #include "BKE_DerivedMesh.h"
82
83 #include  "BIF_editdeform.h"
84
85
86 /* ********** soft body engine ******* */
87
88 typedef struct BodyPoint {
89         float origS[3], origE[3], origT[3], pos[3], vec[3], force[3];
90         float weight, goal;
91         float prevpos[3], prevvec[3], prevdx[3], prevdv[3]; /* used for Heun integration */
92     int nofsprings; int *springs;
93         float contactfrict;
94 } BodyPoint;
95
96 typedef struct BodySpring {
97         int v1, v2;
98         float len, strength;
99 } BodySpring;
100
101
102 #define SOFTGOALSNAP  0.999f 
103 /* if bp-> goal is above make it a *forced follow original* and skip all ODE stuff for this bp
104    removes *unnecessary* stiffnes from ODE system
105 */
106 #define HEUNWARNLIMIT 1 /* 50 would be fine i think for detecting severe *stiff* stuff */
107
108
109 float SoftHeunTol = 1.0f; /* humm .. this should be calculated from sb parameters and sizes */
110
111 /* local prototypes */
112 static void free_softbody_intern(SoftBody *sb);
113 /* aye this belongs to arith.c */
114 static void Vec3PlusStVec(float *v, float s, float *v1);
115
116
117 /*+++ frame based timing +++*/
118
119 /*physical unit of force is [kg * m / sec^2]*/
120
121 static float sb_grav_force_scale(Object *ob)
122 /* since unit of g is [m/sec^2] and F = mass * g we rescale unit mass of node to 1 gramm
123   put it to a function here, so we can add user options later without touching simulation code
124 */
125 {
126         return (0.001f);
127 }
128
129 static float sb_fric_force_scale(Object *ob)
130 /* rescaling unit of drag [1 / sec] to somehow reasonable
131   put it to a function here, so we can add user options later without touching simulation code
132 */
133 {
134         return (0.01f);
135 }
136
137 static float sb_time_scale(Object *ob)
138 /* defining the frames to *real* time relation */
139 {
140         SoftBody *sb= ob->soft; /* is supposed to be there */
141         if (sb){
142                 return(sb->physics_speed); 
143                 /*hrms .. this could be IPO as well :) 
144                  estimated range [0.001 sluggish slug - 100.0 very fast (i hope ODE solver can handle that)]
145                  1 approx = a unit 1 pendulum at g = 9.8 [earth conditions]  has period 65 frames
146          theory would give a 50 frames period .. so there must be something inaccurate .. looking for that (BM) 
147                  */
148         }
149         return (1.0f);
150         /* 
151         this would be frames/sec independant timing assuming 25 fps is default
152         but does not work very well with NLA
153                 return (25.0f/G.scene->r.frs_sec)
154         */
155 }
156 /*--- frame based timing ---*/
157
158
159 static int count_mesh_quads(Mesh *me)
160 {
161         int a,result = 0;
162         MFace *mface= me->mface;
163         
164         if(mface) {
165                 for(a=me->totface; a>0; a--, mface++) {
166                         if(mface->v4) result++;
167                 }
168         }       
169         return result;
170 }
171
172 static void add_mesh_quad_diag_springs(Object *ob)
173 {
174         Mesh *me= ob->data;
175         MFace *mface= me->mface;
176         BodyPoint *bp;
177         BodySpring *bs, *bs_new;
178         int a ;
179         
180         if (ob->soft){
181                 int nofquads;
182                 
183                 nofquads = count_mesh_quads(me);
184                 if (nofquads) {
185                         /* resize spring-array to hold additional quad springs */
186                         bs_new= MEM_callocN( (ob->soft->totspring + nofquads *2 )*sizeof(BodySpring), "bodyspring");
187                         memcpy(bs_new,ob->soft->bspring,(ob->soft->totspring )*sizeof(BodySpring));
188                         
189                         if(ob->soft->bspring)
190                                 MEM_freeN(ob->soft->bspring); /* do this before reassigning the pointer  or have a 1st class memory leak */
191                         ob->soft->bspring = bs_new; 
192                         
193                         /* fill the tail */
194                         a = 0;
195                         bs = bs_new+ob->soft->totspring;
196                         bp= ob->soft->bpoint;
197                         if(mface ) {
198                                 for(a=me->totface; a>0; a--, mface++) {
199                                         if(mface->v4) {
200                                                 bs->v1= mface->v1;
201                                                 bs->v2= mface->v3;
202                                                 bs->strength= 1.0;
203                                                 bs++;
204                                                 bs->v1= mface->v2;
205                                                 bs->v2= mface->v4;
206                                                 bs->strength= 1.0;
207                                                 bs++;
208                                                 
209                                         }
210                                 }       
211                         }
212                         
213             /* now we can announce new springs */
214                         ob->soft->totspring += nofquads *2;
215                 }
216         }
217 }
218
219
220 static void add_bp_springlist(BodyPoint *bp,int springID)
221 {
222         int *newlist;
223         
224         if (bp->springs == NULL) {
225                 bp->springs = MEM_callocN( sizeof(int), "bpsprings");
226                 bp->springs[0] = springID;
227                 bp->nofsprings = 1;
228         }
229         else {
230                 bp->nofsprings++;
231                 newlist = MEM_callocN(bp->nofsprings * sizeof(int), "bpsprings");
232                 memcpy(newlist,bp->springs,(bp->nofsprings-1)* sizeof(int));
233                 MEM_freeN(bp->springs);
234                 bp->springs = newlist;
235                 bp->springs[bp->nofsprings-1] = springID;
236         }
237 }
238
239 /* do this once when sb is build
240 it is O(N^2) so scanning for springs every iteration is too expensive
241 */
242 static void build_bps_springlist(Object *ob)
243 {
244         SoftBody *sb= ob->soft; /* is supposed to be there */
245         BodyPoint *bp;  
246         BodySpring *bs; 
247         int a,b;
248         
249         if (sb==NULL) return; /* paranoya check */
250         
251         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
252                 /* scan for attached inner springs */   
253                 for(b=sb->totspring, bs= sb->bspring; b>0; b--, bs++) {
254                         if (( (sb->totpoint-a) == bs->v1) ){ 
255                                 add_bp_springlist(bp,sb->totspring -b);
256                         }
257                         if (( (sb->totpoint-a) == bs->v2) ){ 
258                                 add_bp_springlist(bp,sb->totspring -b);
259                         }
260                 }/*for springs*/
261                 /* if (bp->nofsprings) printf(" node %d has %d spring links\n",a,bp->nofsprings);*/
262         }/*for bp*/             
263 }
264
265
266 /* creates new softbody if didn't exist yet, makes new points and springs arrays */
267 static void renew_softbody(Object *ob, int totpoint, int totspring)  
268 {
269         SoftBody *sb;
270         int i;
271         
272         if(ob->soft==NULL) ob->soft= sbNew();
273         else free_softbody_intern(ob->soft);
274         sb= ob->soft;
275            
276         if(totpoint) {
277                 sb->totpoint= totpoint;
278                 sb->totspring= totspring;
279                 
280                 sb->bpoint= MEM_mallocN( totpoint*sizeof(BodyPoint), "bodypoint");
281                 if(totspring) 
282                         sb->bspring= MEM_mallocN( totspring*sizeof(BodySpring), "bodyspring");
283
284                         /* initialise BodyPoint array */
285                 for (i=0; i<totpoint; i++) {
286                         BodyPoint *bp = &sb->bpoint[i];
287
288                         bp->weight= 1.0;
289                         if(ob->softflag & OB_SB_GOAL) {
290                                 bp->goal= ob->soft->defgoal;
291                         }
292                         else { 
293                                 bp->goal= 0.0f; 
294                                 /* so this will definily be below SOFTGOALSNAP */
295                         }
296                         
297                         bp->nofsprings= 0;
298                         bp->springs= NULL;
299                         bp->contactfrict = 0.0f;
300                 }
301         }
302 }
303
304 static void free_softbody_baked(SoftBody *sb)
305 {
306         SBVertex *key;
307         int k;
308
309         for(k=0; k<sb->totkey; k++) {
310                 key= *(sb->keys + k);
311                 if(key) MEM_freeN(key);
312         }
313         if(sb->keys) MEM_freeN(sb->keys);
314         
315         sb->keys= NULL;
316         sb->totkey= 0;
317         
318 }
319
320 /* only frees internal data */
321 static void free_softbody_intern(SoftBody *sb)
322 {
323         if(sb) {
324                 int a;
325                 BodyPoint *bp;
326                 
327                 if(sb->bpoint){
328                         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
329                                 /* free spring list */ 
330                                 if (bp->springs != NULL) {
331                                         MEM_freeN(bp->springs);
332                                 }
333                         }
334                         MEM_freeN(sb->bpoint);
335                 }
336                 
337                 if(sb->bspring) MEM_freeN(sb->bspring);
338                 
339                 sb->totpoint= sb->totspring= 0;
340                 sb->bpoint= NULL;
341                 sb->bspring= NULL;
342                 
343                 free_softbody_baked(sb);
344         }
345 }
346
347
348 /* ************ dynamics ********** */
349 int sb_detect_collision(float opco[3], float facenormal[3], float *damp,
350                                                 float force[3], unsigned int par_layer,struct Object *vertexowner)
351 {
352         Base *base;
353         Object *ob;
354         float nv1[3], nv2[3], nv3[3], nv4[3], edge1[3], edge2[3],d_nvect[3], dv1[3], dv2[3],
355                 facedist,n_mag,t,force_mag_norm,
356                 innerfacethickness = -0.5f, outerfacethickness = 0.2f,
357                 ee = 5.0f, ff = 0.1f, fa;
358         int a, deflected=0;
359                 
360         base= G.scene->base.first;
361         while (base) {
362                 /*Only proceed for mesh object in same layer */
363                 if(base->object->type==OB_MESH && (base->lay & par_layer)) {
364                         ob= base->object;
365                         if((vertexowner) && (ob == vertexowner)){ 
366                                 /* if vertexowner is given  we don't want to check collision with owner object */ 
367                                 base = base->next;
368                                 continue;                               
369                         }
370
371                         /* only with deflecting set */
372                         if(ob->pd && ob->pd->deflect) {
373                                 DerivedMesh *dm= NULL;                          
374                                 DispListMesh *disp_mesh= NULL;
375                                 MFace *mface= NULL;
376                                 Object *copyob;
377                                 int dmNeedsFree;                                
378                                 
379                                 /* do object level stuff */
380                                 /* need to have user control for that since it depends on model scale */
381                                 innerfacethickness =-ob->pd->pdef_sbift;
382                                 outerfacethickness =ob->pd->pdef_sboft;
383                                 fa = (ff*outerfacethickness-outerfacethickness);
384                                 fa *= fa;
385                                 fa = 1.0f/fa;
386                                 copyob = ob;
387                                 
388                                 if(1) { /* so maybe someone wants overkill to collide with subsurfed */
389                                         dm = mesh_get_derived_deform(copyob, &dmNeedsFree);
390                                 } else {
391                                         dm = mesh_get_derived_final(copyob, &dmNeedsFree);
392                                 }
393                                 
394                                 if (dm) {
395                                         disp_mesh = dm->convertToDispListMesh(dm, 1);
396                                         mface= disp_mesh->mface;
397                                         a = disp_mesh->totface;
398                                 }
399                                 else {
400                                         a = 0 ;
401                                 }
402                                 
403                                 /* use mesh*/
404                                 while (a--) {
405                                         /* Calculate the global co-ordinates of the vertices*/
406                                         if (dm){
407                                                 dm->getVertCo(dm,mface->v1,nv1);
408                                                 Mat4MulVecfl(ob->obmat, nv1);
409                                                 
410                                                 dm->getVertCo(dm,mface->v2,nv2);
411                                                 Mat4MulVecfl(ob->obmat, nv2);
412                                                 
413                                                 dm->getVertCo(dm,mface->v3,nv3);
414                                                 Mat4MulVecfl(ob->obmat, nv3);
415                                                 
416                                                 
417                                                 if (mface->v4){
418                                                         dm->getVertCo(dm,mface->v4,nv4);
419                                                         Mat4MulVecfl(ob->obmat, nv4);
420                                                 }
421                                         }
422                                         
423                                         
424                                         
425                                         /* switch origin to be nv2*/
426                                         VECSUB(edge1, nv1, nv2);
427                                         VECSUB(edge2, nv3, nv2);
428                                         VECSUB(dv1,opco,nv2); /* abuse dv1 to have vertex in question at *origin* of triangle */
429                                         
430                                         Crossf(d_nvect, edge2, edge1);
431                                         n_mag = Normalise(d_nvect);
432                                         facedist = Inpf(dv1,d_nvect);
433                                         
434                                         if ((facedist > innerfacethickness) && (facedist < outerfacethickness)){                
435                                                 dv2[0] = opco[0] - 2.0f*facedist*d_nvect[0];
436                                                 dv2[1] = opco[1] - 2.0f*facedist*d_nvect[1];
437                                                 dv2[2] = opco[2] - 2.0f*facedist*d_nvect[2];
438                                                 if ( LineIntersectsTriangle( opco, dv2, nv1, nv2, nv3, &t)){
439                                                         force_mag_norm =(float)exp(-ee*facedist);
440                                                         if (facedist > outerfacethickness*ff)
441                                                                 force_mag_norm =(float)force_mag_norm*fa*(facedist - outerfacethickness)*(facedist - outerfacethickness);
442                                                         Vec3PlusStVec(force,force_mag_norm,d_nvect);
443                                                         *damp=ob->pd->pdef_sbdamp;
444                                                         deflected = 2;
445                                                 }
446                                         }               
447                                         if (mface->v4){ /* quad */
448                                                 /* switch origin to be nv4 */
449                                                 VECSUB(edge1, nv3, nv4);
450                                                 VECSUB(edge2, nv1, nv4);
451                                                 VECSUB(dv1,opco,nv4); /* abuse dv1 to have vertex in question at *origin* of triangle */
452                                                 
453                                                 Crossf(d_nvect, edge2, edge1);
454                                                 n_mag = Normalise(d_nvect);
455                                                 facedist = Inpf(dv1,d_nvect);
456                                                 
457                                                 if ((facedist > innerfacethickness) && (facedist < outerfacethickness)){
458                                                         dv2[0] = opco[0] - 2.0f*facedist*d_nvect[0];
459                                                         dv2[1] = opco[1] - 2.0f*facedist*d_nvect[1];
460                                                         dv2[2] = opco[2] - 2.0f*facedist*d_nvect[2];
461                                                         if (LineIntersectsTriangle( opco, dv2, nv1, nv3, nv4, &t)){
462                                                                 force_mag_norm =(float)exp(-ee*facedist);
463                                                                 if (facedist > outerfacethickness*ff)
464                                                                         force_mag_norm =(float)force_mag_norm*fa*(facedist - outerfacethickness)*(facedist - outerfacethickness);
465                                                                 Vec3PlusStVec(force,force_mag_norm,d_nvect);
466                                                                 *damp=ob->pd->pdef_sbdamp;
467                                                                 deflected = 2;
468                                                         }
469                                                         
470                                                 }
471                                         }
472                                         mface++;
473                                         
474                 }/* while a */          
475                                 /* give it away */
476                                 if (disp_mesh) {
477                                         displistmesh_free(disp_mesh);
478                                 }
479                                 if (dm) {
480                                         if (dmNeedsFree) dm->release(dm);
481                                 } 
482                 } /* if(ob->pd && ob->pd->deflect) */
483        }/* if (base->object->type==OB_MESH && (base->lay & par_layer)) { */
484            base = base->next;
485         } /* while (base) */
486         
487         return deflected;
488         
489 }
490
491 /* aye this belongs to arith.c */
492 static void Vec3PlusStVec(float *v, float s, float *v1)
493 {
494         v[0] += s*v1[0];
495         v[1] += s*v1[1];
496         v[2] += s*v1[2];
497 }
498
499 static int sb_deflect_face(Object *ob,float *actpos, float *futurepos,float *collisionpos, float *facenormal,float *force,float *cf )
500 {
501         int deflected;
502         float s_actpos[3], s_futurepos[3];
503         
504         VECCOPY(s_actpos,actpos);
505         if(futurepos)
506                 VECCOPY(s_futurepos,futurepos);
507         
508         deflected= sb_detect_collision(s_actpos, facenormal, cf, force , ob->lay, ob);
509         return(deflected);
510 }
511
512
513 static int is_there_deflection(unsigned int layer)
514 {
515         Base *base;
516         
517         for(base = G.scene->base.first; base; base= base->next) {
518                 if( (base->lay & layer) && base->object->pd) {
519                         if(base->object->pd->deflect) 
520                                 return 1;
521                 }
522         }
523         return 0;
524 }
525
526 static void softbody_calc_forces(Object *ob, float forcetime)
527 {
528 /* rule we never alter free variables :bp->vec bp->pos in here ! 
529  * this will ruin adaptive stepsize AKA heun! (BM) 
530  */
531         SoftBody *sb= ob->soft; /* is supposed to be there */
532         BodyPoint  *bp;
533         BodyPoint *bproot;
534         BodySpring *bs; 
535         ListBase *do_effector;
536         float iks, ks, kd, gravity, actspringlen, forcefactor, sd[3];
537         float fieldfactor = 1000.0f, windfactor  = 250.0f;   
538         int a, b, do_deflector;
539         
540         /* clear forces */
541         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
542                 bp->force[0]= bp->force[1]= bp->force[2]= 0.0;
543         }
544         
545         gravity = sb->grav * sb_grav_force_scale(ob);   
546         
547         /* check! */
548         do_deflector= is_there_deflection(ob->lay);
549         do_effector= pdInitEffectors(ob, NULL);
550         
551         iks  = 1.0f/(1.0f-sb->inspring)-1.0f ;/* inner spring constants function */
552         bproot= sb->bpoint; /* need this for proper spring addressing */
553         
554         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
555                 if(bp->goal < SOFTGOALSNAP){ /* ommit this bp when i snaps */
556                         float auxvect[3];  
557                         float velgoal[3];
558                         float absvel =0, projvel= 0;
559                         
560                         /* do goal stuff */
561                         if(ob->softflag & OB_SB_GOAL) {
562                                 /* true elastic goal */
563                                 VecSubf(auxvect,bp->origT,bp->pos);
564                                 ks  = 1.0f/(1.0f- bp->goal*sb->goalspring)-1.0f ;
565                                 bp->force[0]= ks*(auxvect[0]);
566                                 bp->force[1]= ks*(auxvect[1]);
567                                 bp->force[2]= ks*(auxvect[2]);
568                                 /* calulate damping forces generated by goals*/
569                                 VecSubf(velgoal,bp->origS, bp->origE);
570                                 kd =  sb->goalfrict * sb_fric_force_scale(ob) ;
571                                 
572                                 if (forcetime > 0.0 ) { /* make sure friction does not become rocket motor on time reversal */
573                                         bp->force[0]-= kd * (velgoal[0] + bp->vec[0]);
574                                         bp->force[1]-= kd * (velgoal[1] + bp->vec[1]);
575                                         bp->force[2]-= kd * (velgoal[2] + bp->vec[2]);
576                                 }
577                                 else {
578                                         bp->force[0]-= kd * (velgoal[0] - bp->vec[0]);
579                                         bp->force[1]-= kd * (velgoal[1] - bp->vec[1]);
580                                         bp->force[2]-= kd * (velgoal[2] - bp->vec[2]);
581                                 }
582                         }
583                         /* done goal stuff */
584                         
585                         
586                         /* gravitation */
587                         bp->force[2]-= gravity*sb->nodemass; /* individual mass of node here */
588                         
589                         /* particle field & vortex */
590                         if(do_effector) {
591                                 float force[3]= {0.0f, 0.0f, 0.0f};
592                                 float speed[3]= {0.0f, 0.0f, 0.0f};
593                                 float eval_sb_fric_force_scale = sb_fric_force_scale(ob); /* just for calling function once */
594                                 
595                                 pdDoEffectors(do_effector, bp->pos, force, speed, (float)G.scene->r.cfra, 0.0f, PE_WIND_AS_SPEED);
596                                 
597                                 /* note: now we have wind as motion of media, so we can do anisotropic stuff here, */
598                                 /* if we had vertex normals here(BM) */
599                                 /* apply forcefield*/
600                                 VecMulf(force,fieldfactor* eval_sb_fric_force_scale); 
601                                 VECADD(bp->force, bp->force, force);
602                                 
603                                 /* friction in moving media */  
604                                 kd= sb->mediafrict* eval_sb_fric_force_scale;  
605                                 bp->force[0] -= kd * (bp->vec[0] + windfactor*speed[0]/eval_sb_fric_force_scale);
606                                 bp->force[1] -= kd * (bp->vec[1] + windfactor*speed[1]/eval_sb_fric_force_scale);
607                                 bp->force[2] -= kd * (bp->vec[2] + windfactor*speed[2]/eval_sb_fric_force_scale);
608                                 /* now we'll have nice centrifugal effect for vortex */
609                                 
610                         }
611                         else {
612                                 /* friction in media (not) moving*/
613                                 kd= sb->mediafrict* sb_fric_force_scale(ob);  
614                                 /* assume it to be proportional to actual velocity */
615                                 bp->force[0]-= bp->vec[0]*kd;
616                                 bp->force[1]-= bp->vec[1]*kd;
617                                 bp->force[2]-= bp->vec[2]*kd;
618                                 /* friction in media done */
619                         }
620                         
621                         /*other forces*/
622                         /* this is the place where other forces can be added
623                         yes, constraints and collision stuff should go here too (read baraff papers on that!)
624                         */
625                         /* moving collision targets */
626                         if(do_deflector) {
627                                 float defforce[3] = {0.0f,0.0f,0.0f}, collisionpos[3],facenormal[3], cf = 1.0f;
628                                 kd = 1.0f;
629                                 
630                                 if (sb_deflect_face(ob,bp->pos, bp->pos, collisionpos, facenormal,defforce,&cf)){
631                                         Vec3PlusStVec(bp->force,kd,defforce);
632                                         bp->contactfrict = cf;
633                                 }
634                                 else{ 
635                                         bp->contactfrict = 0.0f;
636                                 }
637                                 
638                         }
639                         
640                         /*other forces done*/
641                         /* nice things could be done with anisotropic friction
642                         like wind/air resistance in normal direction
643                         --> having a piece of cloth sailing down 
644                         but this needs to have a *valid* vertex normal
645                         *valid* means to be calulated on time axis
646                         hrms .. may be a rough one could be used as well .. let's see 
647                         */
648                         
649                         if(ob->softflag & OB_SB_EDGES) {
650                                 if (sb->bspring){ /* spring list exists at all ? */
651                                         for(b=bp->nofsprings;b>0;b--){
652                                                 bs = sb->bspring + bp->springs[b-1];
653                                                 if (( (sb->totpoint-a) == bs->v1) ){ 
654                                                         actspringlen= VecLenf( (bproot+bs->v2)->pos, bp->pos);
655                                                         VecSubf(sd,(bproot+bs->v2)->pos, bp->pos);
656                                                         Normalise(sd);
657                                                         
658                                                         /* friction stuff V1 */
659                                                         VecSubf(velgoal,bp->vec,(bproot+bs->v2)->vec);
660                                                         kd = sb->infrict * sb_fric_force_scale(ob);
661                                                         absvel  = Normalise(velgoal);
662                                                         projvel = ABS(Inpf(sd,velgoal));
663                                                         kd *= absvel * projvel;
664                                                         Vec3PlusStVec(bp->force,-kd,velgoal);
665                                                         
666                                                         if(bs->len > 0.0) /* check for degenerated springs */
667                                                                 forcefactor = (bs->len - actspringlen)/bs->len * iks;
668                                                         else
669                                                                 forcefactor = actspringlen * iks;
670                                                         
671                                                         Vec3PlusStVec(bp->force,-forcefactor,sd);
672                                                         
673                                                 }
674                                                 
675                                                 if (( (sb->totpoint-a) == bs->v2) ){ 
676                                                         actspringlen= VecLenf( (bproot+bs->v1)->pos, bp->pos);
677                                                         VecSubf(sd,bp->pos,(bproot+bs->v1)->pos);
678                                                         Normalise(sd);
679                                                         
680                                                         /* friction stuff V2 */
681                                                         VecSubf(velgoal,bp->vec,(bproot+bs->v1)->vec);
682                                                         kd = sb->infrict * sb_fric_force_scale(ob);
683                                                         absvel  = Normalise(velgoal);
684                                                         projvel = ABS(Inpf(sd,velgoal));
685                                                         kd *= absvel * projvel;
686                                                         Vec3PlusStVec(bp->force,-kd,velgoal);
687                                                         
688                                                         if(bs->len > 0.0)
689                                                                 forcefactor = (bs->len - actspringlen)/bs->len * iks;
690                                                         else
691                                                                 forcefactor = actspringlen * iks;
692                                                         Vec3PlusStVec(bp->force,+forcefactor,sd);                                                       
693                                                 }
694                                         }/* loop springs */
695                                 }/* existing spring list */ 
696                         }/*any edges*/
697                 }/*omit on snap */
698         }/*loop all bp's*/
699
700         /* cleanup */
701         if(do_effector)
702                 pdEndEffectors(do_effector);
703
704 }
705
706 static void softbody_apply_forces(Object *ob, float forcetime, int mode, float *err)
707 {
708         /* time evolution */
709         /* actually does an explicit euler step mode == 0 */
710         /* or heun ~ 2nd order runge-kutta steps, mode 1,2 */
711         SoftBody *sb= ob->soft; /* is supposed to be there */
712         BodyPoint *bp;
713         float dx[3],dv[3];
714         float timeovermass;
715         float maxerr = 0.0;
716         int a;
717
718     forcetime *= sb_time_scale(ob);
719     
720         /* claim a minimum mass for vertex */
721         if (sb->nodemass > 0.09999f) timeovermass = forcetime/sb->nodemass;
722         else timeovermass = forcetime/0.09999f;
723         
724         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
725                 if(bp->goal < SOFTGOALSNAP){
726                         
727                         /* so here is (v)' = a(cceleration) = sum(F_springs)/m + gravitation + some friction forces  + more forces*/
728                         /* the ( ... )' operator denotes derivate respective time */
729                         /* the euler step for velocity then becomes */
730                         /* v(t + dt) = v(t) + a(t) * dt */ 
731                         bp->force[0]*= timeovermass; /* individual mass of node here */ 
732                         bp->force[1]*= timeovermass;
733                         bp->force[2]*= timeovermass;
734                         /* some nasty if's to have heun in here too */
735                         VECCOPY(dv,bp->force); 
736                         if (mode == 1){
737                                 VECCOPY(bp->prevvec, bp->vec);
738                                 VECCOPY(bp->prevdv, dv);
739                         }
740                         if (mode ==2){
741                                 /* be optimistic and execute step */
742                                 bp->vec[0] = bp->prevvec[0] + 0.5f * (dv[0] + bp->prevdv[0]);
743                                 bp->vec[1] = bp->prevvec[1] + 0.5f * (dv[1] + bp->prevdv[1]);
744                                 bp->vec[2] = bp->prevvec[2] + 0.5f * (dv[2] + bp->prevdv[2]);
745                                 /* compare euler to heun to estimate error for step sizing */
746                                 maxerr = MAX2(maxerr,ABS(dv[0] - bp->prevdv[0]));
747                                 maxerr = MAX2(maxerr,ABS(dv[1] - bp->prevdv[1]));
748                                 maxerr = MAX2(maxerr,ABS(dv[2] - bp->prevdv[2]));
749                         }
750                         else {VECADD(bp->vec, bp->vec, bp->force);}
751
752                         /* so here is (x)'= v(elocity) */
753                         /* the euler step for location then becomes */
754                         /* x(t + dt) = x(t) + v(t) * dt */ 
755                         
756                         VECCOPY(dx,bp->vec);
757                         dx[0]*=forcetime ; 
758                         dx[1]*=forcetime ; 
759                         dx[2]*=forcetime ; 
760                         
761                         /* again some nasty if's to have heun in here too */
762                         if (mode ==1){
763                                 VECCOPY(bp->prevpos,bp->pos);
764                                 VECCOPY(bp->prevdx ,dx);
765                         }
766                         
767                         if (mode ==2){
768                                 bp->pos[0] = bp->prevpos[0] + 0.5f * ( dx[0] + bp->prevdx[0]);
769                                 bp->pos[1] = bp->prevpos[1] + 0.5f * ( dx[1] + bp->prevdx[1]);
770                                 bp->pos[2] = bp->prevpos[2] + 0.5f* ( dx[2] + bp->prevdx[2]);
771                                 maxerr = MAX2(maxerr,ABS(dx[0] - bp->prevdx[0]));
772                                 maxerr = MAX2(maxerr,ABS(dx[1] - bp->prevdx[1]));
773                                 maxerr = MAX2(maxerr,ABS(dx[2] - bp->prevdx[2]));
774 /* kind of hack .. while inside collision target .. make movement more *viscous* */
775                                 if (bp->contactfrict > 0.0f){
776                                         bp->vec[0] *= (1.0f - bp->contactfrict);
777                                         bp->vec[1] *= (1.0f - bp->contactfrict);
778                                         bp->vec[2] *= (1.0f - bp->contactfrict);
779                                 }
780                         }
781                         else { VECADD(bp->pos, bp->pos, dx);}
782                 }/*snap*/
783         } /*for*/
784         
785         if (err){ /* so step size will be controlled by biggest difference in slope */
786                 *err = maxerr;
787         }
788 }
789
790 /* used by heun when it overshoots */
791 static void softbody_restore_prev_step(Object *ob)
792 {
793         SoftBody *sb= ob->soft; /* is supposed to be there*/
794         BodyPoint *bp;
795         int a;
796         
797         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
798                 VECCOPY(bp->vec, bp->prevvec);
799                 VECCOPY(bp->pos, bp->prevpos);
800         }
801 }
802
803
804 static void softbody_apply_goalsnap(Object *ob)
805 {
806         SoftBody *sb= ob->soft; /* is supposed to be there */
807         BodyPoint *bp;
808         int a;
809         
810         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
811                 if (bp->goal >= SOFTGOALSNAP){
812                         VECCOPY(bp->prevpos,bp->pos);
813                         VECCOPY(bp->pos,bp->origT);
814                 }               
815         }
816 }
817
818 /* expects full initialized softbody */
819 static void interpolate_exciter(Object *ob, int timescale, int time)
820 {
821         SoftBody *sb= ob->soft;
822         BodyPoint *bp;
823         float f;
824         int a;
825         
826         f = (float)time/(float)timescale;
827         
828         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {   
829                 bp->origT[0] = bp->origS[0] + f*(bp->origE[0] - bp->origS[0]); 
830                 bp->origT[1] = bp->origS[1] + f*(bp->origE[1] - bp->origS[1]); 
831                 bp->origT[2] = bp->origS[2] + f*(bp->origE[2] - bp->origS[2]); 
832                 if (bp->goal >= SOFTGOALSNAP){
833                         bp->vec[0] = bp->origE[0] - bp->origS[0];
834                         bp->vec[1] = bp->origE[1] - bp->origS[1];
835                         bp->vec[2] = bp->origE[2] - bp->origS[2];
836                 }
837         }
838         
839 }
840
841
842 /* ************ convertors ********** */
843
844 /*  for each object type we need;
845     - xxxx_to_softbody(Object *ob)      : a full (new) copy, creates SB geometry
846 */
847
848 static void get_scalar_from_vertexgroup(Object *ob, int vertID, short groupindex, float *target)
849 /* result 0 on success, else indicates error number
850 -- kind of *inverse* result defintion,
851 -- but this way we can signal error condition to caller  
852 -- and yes this function must not be here but in a *vertex group module*
853 */
854 {
855         MDeformVert *dv;
856         int i;
857         
858         /* spot the vert in deform vert list at mesh */
859         if(ob->type==OB_MESH) {
860                 if (((Mesh *)ob->data)->dvert) {
861                         dv = ((Mesh*)ob->data)->dvert + vertID; 
862                         /* Lets see if this vert is in the weight group */
863                         for (i=0; i<dv->totweight; i++){
864                                 if (dv->dw[i].def_nr == groupindex){
865                                         *target= dv->dw[i].weight; /* got it ! */
866                                         break;
867                                 }
868                         }
869                 }
870         }
871
872
873 /* Resetting a Mesh SB object's springs */  
874 /* Spring lenght are caculted from'raw' mesh vertices that are NOT altered by modifier stack. */ 
875 static void springs_from_mesh(Object *ob)
876 {
877         SoftBody *sb;
878         Mesh *me= ob->data;
879         BodyPoint *bp;
880         int a;
881         
882         sb= ob->soft;   
883         if (me && sb)
884         { 
885         /* using bp->origS as a container for spring calcualtions here
886         ** will be overwritten sbObjectStep() to receive 
887         ** actual modifier stack positions
888         */
889                 if(me->totvert) {    
890                         bp= ob->soft->bpoint;
891                         for(a=0; a<me->totvert; a++, bp++) {
892                                 VECCOPY(bp->origS, me->mvert[a].co);                            
893                                 Mat4MulVecfl(ob->obmat, bp->origS);
894                         }
895                         
896                 }
897                 /* recalculate spring length for meshes here */
898                 for(a=0; a<sb->totspring; a++) {
899                         BodySpring *bs = &sb->bspring[a];
900                         bs->len= VecLenf(sb->bpoint[bs->v1].origS, sb->bpoint[bs->v2].origS);
901                 }
902         }
903 }
904
905
906 /* makes totally fresh start situation */
907 static void mesh_to_softbody(Object *ob)
908 {
909         SoftBody *sb;
910         Mesh *me= ob->data;
911         MEdge *medge= me->medge;
912         BodyPoint *bp;
913         BodySpring *bs;
914         float goalfac;
915         int a, totedge;
916         if (ob->softflag & OB_SB_EDGES) totedge= me->totedge;
917         else totedge= 0;
918         
919         /* renew ends with ob->soft with points and edges, also checks & makes ob->soft */
920         renew_softbody(ob, me->totvert, totedge);
921                 
922         /* we always make body points */
923         sb= ob->soft;   
924         bp= sb->bpoint;
925         goalfac= ABS(sb->maxgoal - sb->mingoal);
926         
927         for(a=0; a<me->totvert; a++, bp++) {
928                 /* get scalar values needed  *per vertex* from vertex group functions,
929                 so we can *paint* them nicly .. 
930                 they are normalized [0.0..1.0] so may be we need amplitude for scale
931                 which can be done by caller but still .. i'd like it to go this way 
932                 */ 
933                 
934                 if((ob->softflag & OB_SB_GOAL) && sb->vertgroup) {
935                         get_scalar_from_vertexgroup(ob, a,(short) (sb->vertgroup-1), &bp->goal);
936                         /* do this always, regardless successfull read from vertex group */
937                         bp->goal= sb->mingoal + bp->goal*goalfac;
938                 }
939                 /* a little ad hoc changing the goal control to be less *sharp* */
940                 bp->goal = (float)pow(bp->goal, 4.0f);
941                         
942                 /* to proove the concept
943                 this would enable per vertex *mass painting*
944                 strcpy(name,"SOFTMASS");
945                 error = get_scalar_from_named_vertexgroup(ob,name, a,&temp);
946                 if (!error) bp->mass = temp * ob->rangeofmass;
947                 */
948         }
949
950         /* but we only optionally add body edge springs */
951         if (ob->softflag & OB_SB_EDGES) {
952                 if(medge) {
953                         bs= sb->bspring;
954                         for(a=me->totedge; a>0; a--, medge++, bs++) {
955                                 bs->v1= medge->v1;
956                                 bs->v2= medge->v2;
957                                 bs->strength= 1.0;
958                         }
959
960                 
961                         /* insert *diagonal* springs in quads if desired */
962                         if (ob->softflag & OB_SB_QUADS) {
963                                 add_mesh_quad_diag_springs(ob);
964                         }
965
966                         build_bps_springlist(ob); /* scan for springs attached to bodypoints ONCE */
967                         springs_from_mesh(ob); /* write the 'rest'-lenght of the springs */
968                 }
969         }
970         
971 }
972
973
974
975 /*
976 helper function to get proper spring length 
977 when object is rescaled
978 */
979 float globallen(float *v1,float *v2,Object *ob)
980 {
981         float p1[3],p2[3];
982         VECCOPY(p1,v1);
983         Mat4MulVecfl(ob->obmat, p1);    
984         VECCOPY(p2,v2);
985         Mat4MulVecfl(ob->obmat, p2);
986         return VecLenf(p1,p2);
987 }
988
989 static void makelatticesprings(Lattice *lt,     BodySpring *bs, int dostiff,Object *ob)
990 {
991         BPoint *bp=lt->def, *bpu;
992         int u, v, w, dv, dw, bpc=0, bpuc;
993         
994         dv= lt->pntsu;
995         dw= dv*lt->pntsv;
996         
997         for(w=0; w<lt->pntsw; w++) {
998                 
999                 for(v=0; v<lt->pntsv; v++) {
1000                         
1001                         for(u=0, bpuc=0, bpu=NULL; u<lt->pntsu; u++, bp++, bpc++) {
1002                                 
1003                                 if(w) {
1004                                         bs->v1 = bpc;
1005                                         bs->v2 = bpc-dw;
1006                                     bs->strength= 1.0;
1007                                         bs->len= globallen((bp-dw)->vec, bp->vec,ob);
1008                                         bs++;
1009                                 }
1010                                 if(v) {
1011                                         bs->v1 = bpc;
1012                                         bs->v2 = bpc-dv;
1013                                     bs->strength= 1.0;
1014                                         bs->len= globallen((bp-dv)->vec, bp->vec,ob);
1015                                         bs++;
1016                                 }
1017                                 if(u) {
1018                                         bs->v1 = bpuc;
1019                                         bs->v2 = bpc;
1020                                     bs->strength= 1.0;
1021                                         bs->len= globallen((bpu)->vec, bp->vec,ob);
1022                                         bs++;
1023                                 }
1024                                 
1025                                 if (dostiff) {
1026
1027                                         if(w){
1028                                                 if( v && u ) {
1029                                                         bs->v1 = bpc;
1030                                                         bs->v2 = bpc-dw-dv-1;
1031                                                         bs->strength= 1.0;
1032                                                         bs->len= globallen((bp-dw-dv-1)->vec, bp->vec,ob);
1033                                                         bs++;
1034                                                 }                                               
1035                                                 if( (v < lt->pntsv-1) && (u) ) {
1036                                                         bs->v1 = bpc;
1037                                                         bs->v2 = bpc-dw+dv-1;
1038                                                         bs->strength= 1.0;
1039                                                         bs->len= globallen((bp-dw+dv-1)->vec, bp->vec,ob);
1040                                                         bs++;
1041                                                 }                                               
1042                                         }
1043
1044                                         if(w < lt->pntsw -1){
1045                                                 if( v && u ) {
1046                                                         bs->v1 = bpc;
1047                                                         bs->v2 = bpc+dw-dv-1;
1048                                                         bs->strength= 1.0;
1049                                                         bs->len= globallen((bp+dw-dv-1)->vec, bp->vec,ob);
1050                                                         bs++;
1051                                                 }                                               
1052                                                 if( (v < lt->pntsv-1) && (u) ) {
1053                                                         bs->v1 = bpc;
1054                                                         bs->v2 = bpc+dw+dv-1;
1055                                                         bs->strength= 1.0;
1056                                                          bs->len= globallen((bp+dw+dv-1)->vec, bp->vec,ob);
1057                                                         bs++;
1058                                                 }                                               
1059                                         }
1060                                 }
1061                                 bpu = bp;
1062                                 bpuc = bpc;
1063                         }
1064                 }
1065         }
1066 }
1067
1068
1069 /* makes totally fresh start situation */
1070 static void lattice_to_softbody(Object *ob)
1071 {
1072         Lattice *lt= ob->data;
1073         SoftBody *sb;
1074         int totvert, totspring = 0;
1075
1076         totvert= lt->pntsu*lt->pntsv*lt->pntsw;
1077
1078         if (ob->softflag & OB_SB_EDGES){
1079                 totspring = ((lt->pntsu -1) * lt->pntsv 
1080                           + (lt->pntsv -1) * lt->pntsu) * lt->pntsw
1081                                   +lt->pntsu*lt->pntsv*(lt->pntsw -1);
1082                 if (ob->softflag & OB_SB_QUADS){
1083                         totspring += 4*(lt->pntsu -1) *  (lt->pntsv -1)  * (lt->pntsw-1);
1084                 }
1085         }
1086         
1087
1088         /* renew ends with ob->soft with points and edges, also checks & makes ob->soft */
1089         renew_softbody(ob, totvert, totspring);
1090         sb= ob->soft;   /* can be created in renew_softbody() */
1091         
1092         /* weights from bpoints, same code used as for mesh vertices */
1093         if((ob->softflag & OB_SB_GOAL) && sb->vertgroup) {
1094                 BodyPoint *bp= sb->bpoint;
1095                 BPoint *bpnt= lt->def;
1096                 float goalfac= ABS(sb->maxgoal - sb->mingoal);
1097                 int a;
1098
1099                 for(a=0; a<totvert; a++, bp++, bpnt++) {
1100                         bp->goal= sb->mingoal + bpnt->weight*goalfac;
1101                         /* a little ad hoc changing the goal control to be less *sharp* */
1102                         bp->goal = (float)pow(bp->goal, 4.0f);
1103                 }
1104         }       
1105         
1106         /* create some helper edges to enable SB lattice to be usefull at all */
1107         if (ob->softflag & OB_SB_EDGES){
1108                 makelatticesprings(lt,ob->soft->bspring,ob->softflag & OB_SB_QUADS,ob);
1109                 build_bps_springlist(ob); /* link bps to springs */
1110         }
1111 }
1112
1113 /* makes totally fresh start situation */
1114 static void curve_surf_to_softbody(Object *ob)
1115 {
1116         Curve *cu= ob->data;
1117         SoftBody *sb;
1118         BodyPoint *bp;
1119         BodySpring *bs;
1120         Nurb *nu;
1121         BezTriple *bezt;
1122         BPoint *bpnt;
1123         float goalfac;
1124         int a, curindex=0;
1125         int totvert, totspring = 0, setgoal=0;
1126         
1127         totvert= count_curveverts(&cu->nurb);
1128         
1129         if (ob->softflag & OB_SB_EDGES){
1130                 if(ob->type==OB_CURVE) {
1131                         totspring= totvert - BLI_countlist(&cu->nurb);
1132                 }
1133         }
1134         
1135         /* renew ends with ob->soft with points and edges, also checks & makes ob->soft */
1136         renew_softbody(ob, totvert, totspring);
1137         sb= ob->soft;   /* can be created in renew_softbody() */
1138                 
1139         /* set vars now */
1140         goalfac= ABS(sb->maxgoal - sb->mingoal);
1141         bp= sb->bpoint;
1142         bs= sb->bspring;
1143         
1144         /* weights from bpoints, same code used as for mesh vertices */
1145         if((ob->softflag & OB_SB_GOAL) && sb->vertgroup)
1146                 setgoal= 1;
1147                 
1148         for(nu= cu->nurb.first; nu; nu= nu->next) {
1149                 if(nu->bezt) {
1150                         for(bezt=nu->bezt, a=0; a<nu->pntsu; a++, bezt++, bp+=3, curindex+=3) {
1151                                 if(setgoal) {
1152                                         bp->goal= sb->mingoal + bezt->weight*goalfac;
1153                                         /* a little ad hoc changing the goal control to be less *sharp* */
1154                                         bp->goal = (float)pow(bp->goal, 4.0f);
1155                                         
1156                                         /* all three triples */
1157                                         (bp+1)->goal= bp->goal;
1158                                         (bp+2)->goal= bp->goal;
1159                                 }
1160                                 
1161                                 if(totspring) {
1162                                         if(a>0) {
1163                                                 bs->v1= curindex-1;
1164                                                 bs->v2= curindex;
1165                                                 bs->strength= 1.0;
1166                                                 bs->len= globallen( (bezt-1)->vec[2], bezt->vec[0], ob );
1167                                                 bs++;
1168                                         }
1169                                         bs->v1= curindex;
1170                                         bs->v2= curindex+1;
1171                                         bs->strength= 1.0;
1172                                         bs->len= globallen( bezt->vec[0], bezt->vec[1], ob );
1173                                         bs++;
1174                                         
1175                                         bs->v1= curindex+1;
1176                                         bs->v2= curindex+2;
1177                                         bs->strength= 1.0;
1178                                         bs->len= globallen( bezt->vec[1], bezt->vec[2], ob );
1179                                         bs++;
1180                                 }
1181                         }
1182                 }
1183                 else {
1184                         for(bpnt=nu->bp, a=0; a<nu->pntsu*nu->pntsv; a++, bpnt++, bp++, curindex++) {
1185                                 if(setgoal) {
1186                                         bp->goal= sb->mingoal + bpnt->weight*goalfac;
1187                                         /* a little ad hoc changing the goal control to be less *sharp* */
1188                                         bp->goal = (float)pow(bp->goal, 4.0f);
1189                                 }
1190                                 if(totspring && a>0) {
1191                                         bs->v1= curindex-1;
1192                                         bs->v2= curindex;
1193                                         bs->strength= 1.0;
1194                                         bs->len= globallen( (bpnt-1)->vec, bpnt->vec , ob );
1195                                         bs++;
1196                                 }
1197                         }
1198                 }
1199         }
1200         
1201         if(totspring)
1202                 build_bps_springlist(ob); /* link bps to springs */
1203 }
1204
1205
1206 /* copies softbody result back in object */
1207 static void softbody_to_object(Object *ob, float (*vertexCos)[3], int numVerts)
1208 {
1209         BodyPoint *bp= ob->soft->bpoint;
1210         int a;
1211
1212         /* inverse matrix is not uptodate... */
1213         Mat4Invert(ob->imat, ob->obmat);
1214         
1215         for(a=0; a<numVerts; a++, bp++) {
1216                 VECCOPY(vertexCos[a], bp->pos);
1217                 Mat4MulVecfl(ob->imat, vertexCos[a]);   /* softbody is in global coords */
1218         }
1219 }
1220
1221 /* return 1 if succesfully baked and applied step */
1222 static int softbody_baked_step(Object *ob, float framenr, float (*vertexCos)[3], int numVerts)
1223 {
1224         SoftBody *sb= ob->soft;
1225         SBVertex *key0, *key1, *key2, *key3;
1226         BodyPoint *bp;
1227         float data[4], sfra, efra, cfra, dfra, fac;     /* start, end, current, delta */
1228         int ofs1, a;
1229
1230         /* precondition check */
1231         if(sb==NULL || sb->keys==NULL || sb->totkey==0) return 0;
1232         /* so we got keys, but no bodypoints... even without simul we need it for the bake */    
1233         if(sb->bpoint==NULL) sb->bpoint= MEM_callocN( sb->totpoint*sizeof(BodyPoint), "bodypoint");      
1234         
1235         /* convert cfra time to system time */
1236         sfra= (float)sb->sfra;
1237         cfra= bsystem_time(ob, NULL, framenr, 0.0);
1238         efra= (float)sb->efra;
1239         dfra= (float)sb->interval;
1240
1241         /* offset in keys array */
1242         ofs1= (int)floor( (cfra-sfra)/dfra );
1243
1244         if(ofs1 < 0) {
1245                 key0=key1=key2=key3= *sb->keys;
1246         }
1247         else if(ofs1 >= sb->totkey-1) {
1248                 key0=key1=key2=key3= *(sb->keys+sb->totkey-1);
1249         }
1250         else {
1251                 key1= *(sb->keys+ofs1);
1252                 key2= *(sb->keys+ofs1+1);
1253
1254                 if(ofs1>0) key0= *(sb->keys+ofs1-1);
1255                 else key0= key1;
1256                 
1257                 if(ofs1<sb->totkey-2) key3= *(sb->keys+ofs1+2);
1258                 else key3= key2;
1259         }
1260         
1261         sb->ctime= cfra;        /* needed? */
1262         
1263         /* timing */
1264         fac= ((cfra-sfra)/dfra) - (float)ofs1;
1265         CLAMP(fac, 0.0, 1.0);
1266         set_four_ipo(fac, data, KEY_BSPLINE);
1267         
1268         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++, key0++, key1++, key2++, key3++) {
1269                 bp->pos[0]= data[0]*key0->vec[0] +  data[1]*key1->vec[0] + data[2]*key2->vec[0] + data[3]*key3->vec[0];
1270                 bp->pos[1]= data[0]*key0->vec[1] +  data[1]*key1->vec[1] + data[2]*key2->vec[1] + data[3]*key3->vec[1];
1271                 bp->pos[2]= data[0]*key0->vec[2] +  data[1]*key1->vec[2] + data[2]*key2->vec[2] + data[3]*key3->vec[2];
1272         }
1273         
1274         softbody_to_object(ob, vertexCos, numVerts);
1275         
1276         return 1;
1277 }
1278
1279 /* only gets called after succesfully doing softbody_step */
1280 /* already checked for OB_SB_BAKE flag */
1281 static void softbody_baked_add(Object *ob, float framenr)
1282 {
1283         SoftBody *sb= ob->soft;
1284         SBVertex *key;
1285         BodyPoint *bp;
1286         float sfra, efra, cfra, dfra, fac1;     /* start, end, current, delta */
1287         int ofs1, a;
1288         
1289         /* convert cfra time to system time */
1290         sfra= (float)sb->sfra;
1291         cfra= bsystem_time(ob, NULL, framenr, 0.0);
1292         efra= (float)sb->efra;
1293         dfra= (float)sb->interval;
1294         
1295         if(sb->totkey==0) {
1296                 if(sb->sfra >= sb->efra) return;                /* safety, UI or py setting allows */
1297                 if(sb->interval<1) sb->interval= 1;             /* just be sure */
1298                 
1299                 sb->totkey= 1 + (int)(ceil( (efra-sfra)/dfra ) );
1300                 sb->keys= MEM_callocN( sizeof(void *)*sb->totkey, "sb keys");
1301         }
1302         
1303         /* now find out if we have to store a key */
1304         
1305         /* offset in keys array */
1306         if(cfra==efra) {
1307                 ofs1= sb->totkey-1;
1308                 fac1= 0.0;
1309         }
1310         else {
1311                 ofs1= (int)floor( (cfra-sfra)/dfra );
1312                 fac1= ((cfra-sfra)/dfra) - (float)ofs1;
1313         }       
1314         if( fac1 < 1.0/dfra ) {
1315                 
1316                 key= *(sb->keys+ofs1);
1317                 if(key == NULL) {
1318                         *(sb->keys+ofs1)= key= MEM_mallocN(sb->totpoint*sizeof(SBVertex), "softbody key");
1319                         
1320                         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++, key++) {
1321                                 VECCOPY(key->vec, bp->pos);
1322                         }
1323                 }
1324         }
1325 }
1326
1327 /* ************ Object level, exported functions *************** */
1328
1329 /* allocates and initializes general main data */
1330 SoftBody *sbNew(void)
1331 {
1332         SoftBody *sb;
1333         
1334         sb= MEM_callocN(sizeof(SoftBody), "softbody");
1335         
1336         sb->mediafrict= 0.5; 
1337         sb->nodemass= 1.0;
1338         sb->grav= 0.0; 
1339         sb->physics_speed= 1.0;
1340         sb->rklimit= 0.1f;
1341
1342         sb->goalspring= 0.5; 
1343         sb->goalfrict= 0.0; 
1344         sb->mingoal= 0.0;  
1345         sb->maxgoal= 1.0;
1346         sb->defgoal= 0.7f;
1347         
1348         sb->inspring= 0.5;
1349         sb->infrict= 0.5; 
1350         
1351         sb->interval= 10;
1352         sb->sfra= G.scene->r.sfra;
1353         sb->efra= G.scene->r.efra;
1354         
1355         return sb;
1356 }
1357
1358 /* frees all */
1359 void sbFree(SoftBody *sb)
1360 {
1361         free_softbody_intern(sb);
1362         MEM_freeN(sb);
1363 }
1364
1365
1366 /* makes totally fresh start situation */
1367 void sbObjectToSoftbody(Object *ob)
1368 {
1369         ob->softflag |= OB_SB_REDO;
1370
1371         free_softbody_intern(ob->soft);
1372 }
1373
1374 static int object_has_edges(Object *ob) 
1375 {
1376         if(ob->type==OB_MESH) {
1377                 return ((Mesh*) ob->data)->totedge;
1378         }
1379         else if(ob->type==OB_LATTICE) {
1380                 return 1;
1381         }
1382         else {
1383                 return 0;
1384         }
1385 }
1386
1387 /* simulates one step. framenr is in frames */
1388 void sbObjectStep(Object *ob, float framenr, float (*vertexCos)[3], int numVerts)
1389 {
1390         SoftBody *sb;
1391         BodyPoint *bp;
1392         int a;
1393         float dtime,ctime,forcetime,err;
1394         
1395         /* baking works with global time */
1396         if(!(ob->softflag & OB_SB_BAKEDO) )
1397                 if(softbody_baked_step(ob, framenr, vertexCos, numVerts) ) return;
1398
1399         
1400         /* This part only sets goals and springs, based on original mesh/curve/lattice data.
1401            Copying coordinates happens in next chunk by setting softbody flag OB_SB_RESET */
1402         /* remake softbody if: */
1403         if(             (ob->softflag & OB_SB_REDO) ||          /* signal after weightpainting */
1404                         (ob->soft==NULL) ||                                     /* just to be nice we allow full init */
1405                         (ob->soft->bpoint==NULL) ||             /* after reading new file, or acceptable as signal to refresh */
1406                         (numVerts!=ob->soft->totpoint) ||       /* should never happen, just to be safe */
1407                         ((ob->softflag & OB_SB_EDGES) && !ob->soft->bspring && object_has_edges(ob))) /* happens when in UI edges was set */
1408         {
1409                 switch(ob->type) {
1410                 case OB_MESH:
1411                         mesh_to_softbody(ob);
1412                         break;
1413                 case OB_LATTICE:
1414                         lattice_to_softbody(ob);
1415                         break;
1416                 case OB_CURVE:
1417                 case OB_SURF:
1418                         curve_surf_to_softbody(ob);
1419                         break;
1420                 default:
1421                         renew_softbody(ob, numVerts, 0);
1422                         break;
1423                 }
1424                 
1425                         /* still need to update to correct vertex locations, happens on next step */
1426                 ob->softflag |= OB_SB_RESET; 
1427                 ob->softflag &= ~OB_SB_REDO;
1428         }
1429
1430         sb= ob->soft;
1431
1432         /* still no points? go away */
1433         if(sb->totpoint==0) return;
1434         
1435         /* reset deflector cache, sumohandle is free, but its still sorta abuse... (ton) */
1436         /* we don't use that any more (BM) */
1437
1438         /* checking time: */
1439
1440         ctime= bsystem_time(ob, NULL, framenr, 0.0);
1441
1442         if (ob->softflag&OB_SB_RESET) {
1443                 dtime = 0.0;
1444         } else {
1445                 dtime= ctime - sb->ctime;
1446         }
1447
1448         /* the simulator */
1449
1450                 /* update the vertex locations */
1451         if (dtime!=0.0) {
1452                 for(a=0,bp=sb->bpoint; a<numVerts; a++, bp++) {
1453                         /* store where goals are now */ 
1454                         VECCOPY(bp->origS, bp->origE);
1455                         /* copy the position of the goals at desired end time */
1456                         VECCOPY(bp->origE, vertexCos[a]);
1457                         /* vertexCos came from local world, go global */
1458                         Mat4MulVecfl(ob->obmat, bp->origE); 
1459                          /* just to be save give bp->origT a defined value
1460                         will be calulated in interpolate_exciter()*/
1461                         VECCOPY(bp->origT, bp->origE); 
1462                 }
1463         }
1464     /* see if we need to interrupt integration stream */ 
1465         if((ob->softflag&OB_SB_RESET) ||        /* got a reset signal */
1466                 dtime<0.0 ||                                    /* back in time */
1467                 dtime>=9.9*G.scene->r.framelen) /* too far forward in time --> goals won't be accurate enough */
1468         {
1469                 for(a=0,bp=sb->bpoint; a<numVerts; a++, bp++) {
1470                         VECCOPY(bp->pos, vertexCos[a]);
1471                         Mat4MulVecfl(ob->obmat, bp->pos);  /* yep, sofbody is global coords*/
1472                         VECCOPY(bp->origS, bp->pos);
1473                         VECCOPY(bp->origE, bp->pos);
1474                         VECCOPY(bp->origT, bp->pos);
1475                         bp->vec[0]= bp->vec[1]= bp->vec[2]= 0.0f;
1476
1477                         /* the bp->prev*'s are for rolling back from a canceled try to propagate in time
1478                         adaptive step size algo in a nutshell:
1479             1.  set sheduled time step to new dtime
1480                         2.  try to advance the sheduled time step, beeing optimistic execute it
1481             3.  check for success
1482                         3.a we 're fine continue, may be we can increase sheduled time again ?? if so, do so! 
1483                         3.b we did exceed error limit --> roll back, shorten the sheduled time and try again at 2.
1484                         4.  check if we did reach dtime 
1485                         4.a nope we need to do some more at 2.
1486                         4.b yup we're done
1487                         */
1488
1489                         VECCOPY(bp->prevpos, bp->pos);
1490                         VECCOPY(bp->prevvec, bp->vec);
1491                         VECCOPY(bp->prevdx, bp->vec);
1492                         VECCOPY(bp->prevdv, bp->vec);
1493                 }
1494                 
1495                 ob->softflag &= ~OB_SB_RESET;
1496         }
1497         else if(dtime>0.0) {
1498                 if (TRUE) {     /*  */
1499                         /* special case of 2nd order Runge-Kutta type AKA Heun */
1500                         float timedone =0.0; /* how far did we get without violating error condition */
1501                         /* loops = counter for emergency brake
1502                          * we don't want to lock up the system if physics fail
1503                          */
1504                         int loops =0 ; 
1505                         SoftHeunTol = sb->rklimit; /* humm .. this should be calculated from sb parameters and sizes */
1506
1507                         forcetime = dtime; /* hope for integrating in one step */
1508                         while ( (ABS(timedone) < ABS(dtime)) && (loops < 2000) )
1509                         {
1510                                 /* set goals in time */ 
1511                                 interpolate_exciter(ob,200,(int)(200.0*(timedone/dtime)));
1512                                 /* do predictive euler step */
1513                                 softbody_calc_forces(ob, forcetime);
1514                                 softbody_apply_forces(ob, forcetime, 1, NULL);
1515                                 /* crop new slope values to do averaged slope step */
1516                                 softbody_calc_forces(ob, forcetime);
1517                                 softbody_apply_forces(ob, forcetime, 2, &err);
1518                                 softbody_apply_goalsnap(ob);
1519
1520                                 if (err > SoftHeunTol){ /* error needs to be scaled to some quantity */
1521                                         softbody_restore_prev_step(ob);
1522                                         forcetime /= 2.0;
1523                                 }
1524                                 else {
1525                                         float newtime = forcetime * 1.1f; /* hope for 1.1 times better conditions in next step */
1526                                         
1527                                         if (err > SoftHeunTol/2.0){ /* stay with this stepsize unless err really small */
1528                                                 newtime = forcetime;
1529                                         }
1530                                         timedone += forcetime;
1531                                         if (forcetime > 0.0)
1532                                                 forcetime = MIN2(dtime - timedone,newtime);
1533                                         else 
1534                                                 forcetime = MAX2(dtime - timedone,newtime);
1535                                 }
1536                                 loops++;
1537                         }
1538                         /* move snapped to final position */
1539                         interpolate_exciter(ob, 2, 2);
1540                         softbody_apply_goalsnap(ob);
1541                         
1542                         if(G.f & G_DEBUG) {
1543                                 if (loops > HEUNWARNLIMIT) /* monitor high loop counts say 1000 after testing */
1544                                         printf("%d heun integration loops/frame \n",loops);
1545                         }
1546                 }
1547                 else{
1548                         /* do brute force explicit euler */
1549                         /* removed but left this branch for better integrators / solvers (BM) */
1550                         /* yah! Nicholas Guttenberg (NichG) here is the place to plug in */
1551                 }
1552         }
1553
1554         softbody_to_object(ob, vertexCos, numVerts);
1555         sb->ctime= ctime;
1556
1557         /* reset deflector cache */
1558         /* we don't use that any more (BM) */
1559         
1560         if(ob->softflag & OB_SB_BAKEDO) softbody_baked_add(ob, framenr);
1561 }
1562