Fixed all gcc 4 warnings in blenkernel. Found 2 potentially harmful
[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 /* ********** soft body engine ******* */
86
87 typedef struct BodyPoint {
88         float origS[3], origE[3], origT[3], pos[3], vec[3], force[3];
89         float weight, goal;
90         float prevpos[3], prevvec[3], prevdx[3], prevdv[3]; /* used for Heun integration */
91     int nofsprings; int *springs;
92         float contactfrict;
93         float colball;
94 } BodyPoint;
95
96 typedef struct BodySpring {
97         int v1, v2;
98         float len, strength;
99         float ext_force[3]; /* edges colliding and sailing */
100         short order;
101         short flag;
102 } BodySpring;
103
104
105 #define SOFTGOALSNAP  0.999f 
106 /* if bp-> goal is above make it a *forced follow original* and skip all ODE stuff for this bp
107    removes *unnecessary* stiffnes from ODE system
108 */
109 #define HEUNWARNLIMIT 1 /* 50 would be fine i think for detecting severe *stiff* stuff */
110
111
112 #define BSF_INTERSECT   1 /* edge intersects collider face */
113
114
115 float SoftHeunTol = 1.0f; /* humm .. this should be calculated from sb parameters and sizes */
116
117 /* local prototypes */
118 static void free_softbody_intern(SoftBody *sb);
119 /* aye this belongs to arith.c */
120 static void Vec3PlusStVec(float *v, float s, float *v1);
121
122
123 /*+++ frame based timing +++*/
124
125 /*physical unit of force is [kg * m / sec^2]*/
126
127 static float sb_grav_force_scale(Object *ob)
128 /* since unit of g is [m/sec^2] and F = mass * g we rescale unit mass of node to 1 gramm
129   put it to a function here, so we can add user options later without touching simulation code
130 */
131 {
132         return (0.001f);
133 }
134
135 static float sb_fric_force_scale(Object *ob)
136 /* rescaling unit of drag [1 / sec] to somehow reasonable
137   put it to a function here, so we can add user options later without touching simulation code
138 */
139 {
140         return (0.01f);
141 }
142
143 static float sb_time_scale(Object *ob)
144 /* defining the frames to *real* time relation */
145 {
146         SoftBody *sb= ob->soft; /* is supposed to be there */
147         if (sb){
148                 return(sb->physics_speed); 
149                 /*hrms .. this could be IPO as well :) 
150                  estimated range [0.001 sluggish slug - 100.0 very fast (i hope ODE solver can handle that)]
151                  1 approx = a unit 1 pendulum at g = 9.8 [earth conditions]  has period 65 frames
152          theory would give a 50 frames period .. so there must be something inaccurate .. looking for that (BM) 
153                  */
154         }
155         return (1.0f);
156         /* 
157         this would be frames/sec independant timing assuming 25 fps is default
158         but does not work very well with NLA
159                 return (25.0f/G.scene->r.frs_sec)
160         */
161 }
162 /*--- frame based timing ---*/
163
164 /*+++ collider caching and dicing +++*/
165
166 /********************
167 for each target object/face the axis aligned bounding box (AABB) is stored
168 faces paralell to global axes 
169 so only simple "value" in [min,max] ckecks are used
170 float operations still
171 */
172
173 /* just an ID here to reduce the prob for killing objects
174 ** ob->sumohandle points to we should not kill :)
175 */ 
176 const int CCD_SAVETY = 190561; 
177
178 typedef struct ccdf_minmax{
179 float minx,miny,minz,maxx,maxy,maxz;
180 }ccdf_minmax;
181
182
183
184 typedef struct ccd_Mesh {
185         int totvert, totface;
186         MVert *mvert;
187         MFace *mface;
188         int savety;
189         ccdf_minmax *mima;
190         /* Axis Aligned Bounding Box AABB */
191         float bbmin[3];
192         float bbmax[3];
193 }ccd_Mesh;
194
195
196
197 #if (0)
198 /* build cache for self intersection */
199 /* waiting for being needed          */
200 ccd_Mesh *ccd_mesh_make_self(Object *ob)
201 {
202         SoftBody *sb;
203         BodyPoint *bp;
204
205     ccd_Mesh *pccd_M = NULL;
206         ccdf_minmax *mima =NULL;
207         MFace *mface=NULL;
208         float v[3],hull;
209         int i;
210
211         Mesh *me= ob->data;
212         sb=ob->soft;
213
214         
215         /* first some paranoia checks */
216         if (!me) return NULL;
217         if ((!me->totface) || (!me->totvert)) return NULL;
218         
219         pccd_M = MEM_mallocN(sizeof(ccd_Mesh),"ccd_Mesh");
220         pccd_M->totvert = me->totvert;
221         pccd_M->totface = me->totface;
222         pccd_M->savety  = CCD_SAVETY;
223         pccd_M->bbmin[0]=pccd_M->bbmin[1]=pccd_M->bbmin[2]=1e30f;
224         pccd_M->bbmax[0]=pccd_M->bbmax[1]=pccd_M->bbmax[2]=-1e30f;
225         
226         
227     /* blow it up with forcefield ranges */
228         hull = MAX2(ob->pd->pdef_sbift,ob->pd->pdef_sboft);
229         
230         /* alloc and copy verts*/
231     pccd_M->mvert = MEM_mallocN(sizeof(MVert)*pccd_M->totvert,"ccd_Mesh_Vert");
232     /* ah yeah, put the verices to global coords once */        
233         /* and determine the ortho BB on the fly */ 
234         bp=sb->bpoint;
235         for(i=0; i < pccd_M->totvert; i++,bp++){
236                 VECCOPY(pccd_M->mvert[i].co,bp->pos);
237                 //Mat4MulVecfl(ob->obmat, pccd_M->mvert[i].co);
238                 
239         /* evaluate limits */
240                 VECCOPY(v,pccd_M->mvert[i].co);
241                 pccd_M->bbmin[0] = MIN2(pccd_M->bbmin[0],v[0]-hull);
242                 pccd_M->bbmin[1] = MIN2(pccd_M->bbmin[1],v[1]-hull);
243                 pccd_M->bbmin[2] = MIN2(pccd_M->bbmin[2],v[2]-hull);
244                 
245                 pccd_M->bbmax[0] = MAX2(pccd_M->bbmax[0],v[0]+hull);
246                 pccd_M->bbmax[1] = MAX2(pccd_M->bbmax[1],v[1]+hull);
247                 pccd_M->bbmax[2] = MAX2(pccd_M->bbmax[2],v[2]+hull);
248                 
249         }
250         /* alloc and copy faces*/
251     pccd_M->mface = MEM_mallocN(sizeof(MFace)*pccd_M->totface,"ccd_Mesh_Faces");
252         memcpy(pccd_M->mface,me->mface,sizeof(MFace)*pccd_M->totface);
253         
254         /* OBBs for idea1 */
255     pccd_M->mima = MEM_mallocN(sizeof(ccdf_minmax)*pccd_M->totface,"ccd_Mesh_Faces_mima");
256         mima  = pccd_M->mima;
257         mface = pccd_M->mface;
258
259
260         /* anyhoo we need to walk the list of faces and find OBB they live in */
261         for(i=0; i < pccd_M->totface; i++){
262                 mima->minx=mima->miny=mima->minz=1e30f;
263                 mima->maxx=mima->maxy=mima->maxz=-1e30f;
264                 
265         VECCOPY(v,pccd_M->mvert[mface->v1].co);
266                 mima->minx = MIN2(mima->minx,v[0]-hull);
267                 mima->miny = MIN2(mima->miny,v[1]-hull);
268                 mima->minz = MIN2(mima->minz,v[2]-hull);
269                 mima->maxx = MAX2(mima->maxx,v[0]+hull);
270                 mima->maxy = MAX2(mima->maxy,v[1]+hull);
271                 mima->maxz = MAX2(mima->maxz,v[2]+hull);
272                 
273         VECCOPY(v,pccd_M->mvert[mface->v2].co);
274                 mima->minx = MIN2(mima->minx,v[0]-hull);
275                 mima->miny = MIN2(mima->miny,v[1]-hull);
276                 mima->minz = MIN2(mima->minz,v[2]-hull);
277                 mima->maxx = MAX2(mima->maxx,v[0]+hull);
278                 mima->maxy = MAX2(mima->maxy,v[1]+hull);
279                 mima->maxz = MAX2(mima->maxz,v[2]+hull);
280                 
281                 VECCOPY(v,pccd_M->mvert[mface->v3].co);
282                 mima->minx = MIN2(mima->minx,v[0]-hull);
283                 mima->miny = MIN2(mima->miny,v[1]-hull);
284                 mima->minz = MIN2(mima->minz,v[2]-hull);
285                 mima->maxx = MAX2(mima->maxx,v[0]+hull);
286                 mima->maxy = MAX2(mima->maxy,v[1]+hull);
287                 mima->maxz = MAX2(mima->maxz,v[2]+hull);
288         
289                 if(mface->v4){
290                         VECCOPY(v,pccd_M->mvert[mface->v4].co);
291                 mima->minx = MIN2(mima->minx,v[0]-hull);
292                 mima->miny = MIN2(mima->miny,v[1]-hull);
293                 mima->minz = MIN2(mima->minz,v[2]-hull);
294                 mima->maxx = MAX2(mima->maxx,v[0]+hull);
295                 mima->maxy = MAX2(mima->maxy,v[1]+hull);
296                 mima->maxz = MAX2(mima->maxz,v[2]+hull);
297                 }
298
299                 
300         mima++;
301         mface++;
302                 
303         }
304         return pccd_M;
305 }
306 #endif
307
308 ccd_Mesh *ccd_mesh_make(Object *ob, DispListMesh *dm)
309 {
310     ccd_Mesh *pccd_M = NULL;
311         ccdf_minmax *mima =NULL;
312         MFace *mface=NULL;
313         float v[3],hull;
314         int i;
315         
316         /* first some paranoia checks */
317         if (!dm) return NULL;
318         if ((!dm->totface) || (!dm->totvert)) return NULL;
319         
320         pccd_M = MEM_mallocN(sizeof(ccd_Mesh),"ccd_Mesh");
321         pccd_M->totvert = dm->totvert;
322         pccd_M->totface = dm->totface;
323         pccd_M->savety  = CCD_SAVETY;
324         pccd_M->bbmin[0]=pccd_M->bbmin[1]=pccd_M->bbmin[2]=1e30f;
325         pccd_M->bbmax[0]=pccd_M->bbmax[1]=pccd_M->bbmax[2]=-1e30f;
326         
327         
328     /* blow it up with forcefield ranges */
329         hull = MAX2(ob->pd->pdef_sbift,ob->pd->pdef_sboft);
330         
331         /* alloc and copy verts*/
332     pccd_M->mvert = MEM_mallocN(sizeof(MVert)*pccd_M->totvert,"ccd_Mesh_Vert");
333         memcpy(pccd_M->mvert,dm->mvert,sizeof(MVert)*pccd_M->totvert);
334     /* ah yeah, put the verices to global coords once */        
335         /* and determine the ortho BB on the fly */ 
336         for(i=0; i < pccd_M->totvert; i++){
337                 Mat4MulVecfl(ob->obmat, pccd_M->mvert[i].co);
338                 
339         /* evaluate limits */
340                 VECCOPY(v,pccd_M->mvert[i].co);
341                 pccd_M->bbmin[0] = MIN2(pccd_M->bbmin[0],v[0]-hull);
342                 pccd_M->bbmin[1] = MIN2(pccd_M->bbmin[1],v[1]-hull);
343                 pccd_M->bbmin[2] = MIN2(pccd_M->bbmin[2],v[2]-hull);
344                 
345                 pccd_M->bbmax[0] = MAX2(pccd_M->bbmax[0],v[0]+hull);
346                 pccd_M->bbmax[1] = MAX2(pccd_M->bbmax[1],v[1]+hull);
347                 pccd_M->bbmax[2] = MAX2(pccd_M->bbmax[2],v[2]+hull);
348                 
349         }
350         /* alloc and copy faces*/
351     pccd_M->mface = MEM_mallocN(sizeof(MFace)*pccd_M->totface,"ccd_Mesh_Faces");
352         memcpy(pccd_M->mface,dm->mface,sizeof(MFace)*pccd_M->totface);
353         
354         /* OBBs for idea1 */
355     pccd_M->mima = MEM_mallocN(sizeof(ccdf_minmax)*pccd_M->totface,"ccd_Mesh_Faces_mima");
356         mima  = pccd_M->mima;
357         mface = pccd_M->mface;
358
359
360         /* anyhoo we need to walk the list of faces and find OBB they live in */
361         for(i=0; i < pccd_M->totface; i++){
362                 mima->minx=mima->miny=mima->minz=1e30f;
363                 mima->maxx=mima->maxy=mima->maxz=-1e30f;
364                 
365         VECCOPY(v,pccd_M->mvert[mface->v1].co);
366                 mima->minx = MIN2(mima->minx,v[0]-hull);
367                 mima->miny = MIN2(mima->miny,v[1]-hull);
368                 mima->minz = MIN2(mima->minz,v[2]-hull);
369                 mima->maxx = MAX2(mima->maxx,v[0]+hull);
370                 mima->maxy = MAX2(mima->maxy,v[1]+hull);
371                 mima->maxz = MAX2(mima->maxz,v[2]+hull);
372                 
373         VECCOPY(v,pccd_M->mvert[mface->v2].co);
374                 mima->minx = MIN2(mima->minx,v[0]-hull);
375                 mima->miny = MIN2(mima->miny,v[1]-hull);
376                 mima->minz = MIN2(mima->minz,v[2]-hull);
377                 mima->maxx = MAX2(mima->maxx,v[0]+hull);
378                 mima->maxy = MAX2(mima->maxy,v[1]+hull);
379                 mima->maxz = MAX2(mima->maxz,v[2]+hull);
380                 
381                 VECCOPY(v,pccd_M->mvert[mface->v3].co);
382                 mima->minx = MIN2(mima->minx,v[0]-hull);
383                 mima->miny = MIN2(mima->miny,v[1]-hull);
384                 mima->minz = MIN2(mima->minz,v[2]-hull);
385                 mima->maxx = MAX2(mima->maxx,v[0]+hull);
386                 mima->maxy = MAX2(mima->maxy,v[1]+hull);
387                 mima->maxz = MAX2(mima->maxz,v[2]+hull);
388         
389                 if(mface->v4){
390                         VECCOPY(v,pccd_M->mvert[mface->v4].co);
391                 mima->minx = MIN2(mima->minx,v[0]-hull);
392                 mima->miny = MIN2(mima->miny,v[1]-hull);
393                 mima->minz = MIN2(mima->minz,v[2]-hull);
394                 mima->maxx = MAX2(mima->maxx,v[0]+hull);
395                 mima->maxy = MAX2(mima->maxy,v[1]+hull);
396                 mima->maxz = MAX2(mima->maxz,v[2]+hull);
397                 }
398
399                 
400         mima++;
401         mface++;
402                 
403         }
404         return pccd_M;
405 }
406
407 void ccd_mesh_free(ccd_Mesh *ccdm)
408 {
409         if(ccdm && (ccdm->savety == CCD_SAVETY )){ /*make sure we're not nuking objects we don't know*/
410                 MEM_freeN(ccdm->mface);
411                 MEM_freeN(ccdm->mvert);
412                 MEM_freeN(ccdm->mima);
413                 MEM_freeN(ccdm);
414                 ccdm = NULL;
415         }
416 }
417
418 void free_sumo_handles()
419 {
420         Base *base;
421         for(base= G.scene->base.first; base; base= base->next) {
422                 if(base->object->sumohandle) {
423                         ccd_mesh_free(base->object->sumohandle);
424                         base->object->sumohandle= NULL;
425                 }
426         }
427         
428 }
429
430 void ccd_build_deflector_cache(Object *vertexowner)
431 {
432         Base *base;
433         Object *ob;
434         base= G.scene->base.first;
435         base= G.scene->base.first;
436         while (base) {
437                 /*Only proceed for mesh object in same layer */
438                 if(base->object->type==OB_MESH && (base->lay & vertexowner->lay)) {
439                         ob= base->object;
440                         if((vertexowner) && (ob == vertexowner)){ 
441                                 /* if vertexowner is given  we don't want to check collision with owner object */ 
442                                 base = base->next;
443                                 continue;                               
444                         }
445
446                         /*+++ only with deflecting set */
447                         if(ob->pd && ob->pd->deflect) {
448                                 DerivedMesh *dm= NULL;
449                                 int dmNeedsFree;        
450                                 
451                                 if(ob->softflag & OB_SB_COLLFINAL) { /* so maybe someone wants overkill to collide with subsurfed */
452                                         dm = mesh_get_derived_final(ob, &dmNeedsFree);
453                                 } else {
454                                         dm = mesh_get_derived_deform(ob, &dmNeedsFree);
455                                 }
456                                 if(dm){
457                                         DispListMesh *disp_mesh= NULL;
458                                         disp_mesh = dm->convertToDispListMesh(dm, 0);
459                                         ob->sumohandle=ccd_mesh_make(ob,disp_mesh);
460                                         /* we did copy & modify all we need so give 'em away again */
461                                         if (disp_mesh) {
462                                                 displistmesh_free(disp_mesh);
463                                         }
464                                         if (dm) {
465                                                 if (dmNeedsFree) dm->release(dm);
466                                         } 
467                                         
468                                 }
469                         }/*--- only with deflecting set */
470
471                 }/* mesh && layer*/             
472            base = base->next;
473         } /* while (base) */
474 }
475
476 /*--- collider caching and dicing ---*/
477
478
479 static int count_mesh_quads(Mesh *me)
480 {
481         int a,result = 0;
482         MFace *mface= me->mface;
483         
484         if(mface) {
485                 for(a=me->totface; a>0; a--, mface++) {
486                         if(mface->v4) result++;
487                 }
488         }       
489         return result;
490 }
491
492 static void add_mesh_quad_diag_springs(Object *ob)
493 {
494         Mesh *me= ob->data;
495         MFace *mface= me->mface;
496         BodyPoint *bp;
497         BodySpring *bs, *bs_new;
498         int a ;
499         
500         if (ob->soft){
501                 int nofquads;
502                 
503                 nofquads = count_mesh_quads(me);
504                 if (nofquads) {
505                         /* resize spring-array to hold additional quad springs */
506                         bs_new= MEM_callocN( (ob->soft->totspring + nofquads *2 )*sizeof(BodySpring), "bodyspring");
507                         memcpy(bs_new,ob->soft->bspring,(ob->soft->totspring )*sizeof(BodySpring));
508                         
509                         if(ob->soft->bspring)
510                                 MEM_freeN(ob->soft->bspring); /* do this before reassigning the pointer  or have a 1st class memory leak */
511                         ob->soft->bspring = bs_new; 
512                         
513                         /* fill the tail */
514                         a = 0;
515                         bs = bs_new+ob->soft->totspring;
516                         bp= ob->soft->bpoint;
517                         if(mface ) {
518                                 for(a=me->totface; a>0; a--, mface++) {
519                                         if(mface->v4) {
520                                                 bs->v1= mface->v1;
521                                                 bs->v2= mface->v3;
522                                                 bs->strength= 1.0;
523                                                 bs->order   =2;
524                                                 bs++;
525                                                 bs->v1= mface->v2;
526                                                 bs->v2= mface->v4;
527                                                 bs->strength= 1.0;
528                                                 bs->order   =2;
529                                                 bs++;
530                                                 
531                                         }
532                                 }       
533                         }
534                         
535             /* now we can announce new springs */
536                         ob->soft->totspring += nofquads *2;
537                 }
538         }
539 }
540
541 static void add_2nd_order_roller(Object *ob,float stiffness,int *counter, int addsprings)
542 {
543         /*assume we have a softbody*/
544         SoftBody *sb= ob->soft; /* is supposed to be there */
545         BodyPoint *bp,*bpo;     
546         BodySpring *bs,*bs2,*bs3= NULL;
547         int a,b,c,notthis= 0,v0;
548         if (!sb->bspring){return;} /* we are 2nd order here so 1rst should have been build :) */
549         /* first run counting  second run adding */
550         *counter = 0;
551         if (addsprings) bs3 = ob->soft->bspring+ob->soft->totspring;
552         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
553                 /*scan for neighborhood*/
554                 bpo = NULL;
555                 v0  = (sb->totpoint-a);
556                 for(b=bp->nofsprings;b>0;b--){
557                         bs = sb->bspring + bp->springs[b-1];
558                         /*nasty thing here that springs have two ends
559                         so here we have to make sure we examine the other */
560                         if (( v0 == bs->v1) ){ 
561                                 bpo =sb->bpoint+bs->v2;
562                                 notthis = bs->v2;
563                         }
564                         else {
565                         if (( v0 == bs->v2) ){
566                                 bpo =sb->bpoint+bs->v1;
567                                 notthis = bs->v1;
568                         } 
569                         else {printf("oops we should not get here -  add_2nd_order_springs");}
570                         }
571             if (bpo){/* so now we have a 2nd order humpdidump */
572                                 for(c=bpo->nofsprings;c>0;c--){
573                                         bs2 = sb->bspring + bpo->springs[c-1];
574                                         if ((bs2->v1 != notthis)  && (bs2->v1 > v0)){
575                                                 (*counter)++;/*hit */
576                                                 if (addsprings){
577                                                         bs3->v1= v0;
578                                                         bs3->v2= bs2->v1;
579                                                         bs3->strength= stiffness;
580                                                         bs3->order=2;
581                                                         bs3++;
582                                                 }
583                                         }
584                                         if ((bs2->v2 !=notthis)&&(bs2->v2 > v0)){
585                                         (*counter)++;/*hit */
586                                                 if (addsprings){
587                                                         bs3->v1= v0;
588                                                         bs3->v2= bs2->v2;
589                                                         bs3->strength= stiffness;
590                                                         bs3->order=2;
591                                                         bs3++;
592                                                 }
593
594                                         }
595                                 }
596                                 
597                         }
598                         
599                 }
600                 /*scan for neighborhood done*/
601         }
602 }
603
604
605 static void add_2nd_order_springs(Object *ob,float stiffness)
606 {
607         int counter = 0;
608         BodySpring *bs_new;
609         
610         add_2nd_order_roller(ob,stiffness,&counter,0); /* counting */
611         if (counter) {
612                 /* resize spring-array to hold additional springs */
613                 bs_new= MEM_callocN( (ob->soft->totspring + counter )*sizeof(BodySpring), "bodyspring");
614                 memcpy(bs_new,ob->soft->bspring,(ob->soft->totspring )*sizeof(BodySpring));
615                 
616                 if(ob->soft->bspring)
617                         MEM_freeN(ob->soft->bspring); 
618                 ob->soft->bspring = bs_new; 
619                 
620                 add_2nd_order_roller(ob,stiffness,&counter,1); /* adding */
621                 ob->soft->totspring +=counter ;
622         }
623 }
624
625 static void add_bp_springlist(BodyPoint *bp,int springID)
626 {
627         int *newlist;
628         
629         if (bp->springs == NULL) {
630                 bp->springs = MEM_callocN( sizeof(int), "bpsprings");
631                 bp->springs[0] = springID;
632                 bp->nofsprings = 1;
633         }
634         else {
635                 bp->nofsprings++;
636                 newlist = MEM_callocN(bp->nofsprings * sizeof(int), "bpsprings");
637                 memcpy(newlist,bp->springs,(bp->nofsprings-1)* sizeof(int));
638                 MEM_freeN(bp->springs);
639                 bp->springs = newlist;
640                 bp->springs[bp->nofsprings-1] = springID;
641         }
642 }
643
644 /* do this once when sb is build
645 it is O(N^2) so scanning for springs every iteration is too expensive
646 */
647 static void build_bps_springlist(Object *ob)
648 {
649         SoftBody *sb= ob->soft; /* is supposed to be there */
650         BodyPoint *bp;  
651         BodySpring *bs; 
652         int a,b;
653         
654         if (sb==NULL) return; /* paranoya check */
655         
656         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
657                 /* throw away old list */
658                 if (bp->springs) {
659                         MEM_freeN(bp->springs);
660                         bp->springs=NULL;
661                 }
662                 /* scan for attached inner springs */   
663                 for(b=sb->totspring, bs= sb->bspring; b>0; b--, bs++) {
664                         if (( (sb->totpoint-a) == bs->v1) ){ 
665                                 add_bp_springlist(bp,sb->totspring -b);
666                         }
667                         if (( (sb->totpoint-a) == bs->v2) ){ 
668                                 add_bp_springlist(bp,sb->totspring -b);
669                         }
670                 }/*for springs*/
671         }/*for bp*/             
672 }
673
674 static void calculate_collision_balls(Object *ob)
675 {
676         SoftBody *sb= ob->soft; /* is supposed to be there */
677         BodyPoint *bp;  
678         BodySpring *bs; 
679         int a,b,akku_count;
680         float min,max,akku;
681
682         if (sb==NULL) return; /* paranoya check */
683
684         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
685                 bp->colball=0;
686                 akku =0.0f;
687                 akku_count=0;
688                 min = 1e22f;
689                 max = -1e22f;
690                 /* first estimation based on attached */
691                 for(b=bp->nofsprings;b>0;b--){
692                         bs = sb->bspring + bp->springs[b-1];
693                         if (bs->order == 1){
694                         akku += bs->len;
695                         akku_count++,
696                         min = MIN2(bs->len,min);
697                         max = MAX2(bs->len,max);
698                         }
699                 }
700
701                 if (akku_count > 0) {
702                         if (sb->sbc_mode == 0){
703                                 bp->colball=sb->colball;
704                         }               
705                         if (sb->sbc_mode == 1){
706                          bp->colball = akku/(float)akku_count*sb->colball;
707                         }
708                         if (sb->colball == 2){
709                                 bp->colball=min*sb->colball;
710                         }               
711                         if (sb->colball == 3){
712                                 bp->colball=max*sb->colball;
713                         }               
714                         if (sb->colball == 4){
715                         bp->colball = (min + max)/2.0f*sb->colball;
716                         }               
717                 }
718                 else bp->colball=0;
719         }/*for bp*/             
720 }
721
722
723 /* creates new softbody if didn't exist yet, makes new points and springs arrays */
724 static void renew_softbody(Object *ob, int totpoint, int totspring)  
725 {
726         SoftBody *sb;
727         int i;
728         
729         if(ob->soft==NULL) ob->soft= sbNew();
730         else free_softbody_intern(ob->soft);
731         sb= ob->soft;
732            
733         if(totpoint) {
734                 sb->totpoint= totpoint;
735                 sb->totspring= totspring;
736                 
737                 sb->bpoint= MEM_mallocN( totpoint*sizeof(BodyPoint), "bodypoint");
738                 if(totspring) 
739                         sb->bspring= MEM_mallocN( totspring*sizeof(BodySpring), "bodyspring");
740
741                         /* initialise BodyPoint array */
742                 for (i=0; i<totpoint; i++) {
743                         BodyPoint *bp = &sb->bpoint[i];
744
745                         bp->weight= 1.0;
746                         if(ob->softflag & OB_SB_GOAL) {
747                                 bp->goal= ob->soft->defgoal;
748                         }
749                         else { 
750                                 bp->goal= 0.0f; 
751                                 /* so this will definily be below SOFTGOALSNAP */
752                         }
753                         
754                         bp->nofsprings= 0;
755                         bp->springs= NULL;
756                         bp->contactfrict = 0.0f;
757                         bp->colball = 0.0f;
758
759                 }
760         }
761 }
762
763 static void free_softbody_baked(SoftBody *sb)
764 {
765         SBVertex *key;
766         int k;
767
768         for(k=0; k<sb->totkey; k++) {
769                 key= *(sb->keys + k);
770                 if(key) MEM_freeN(key);
771         }
772         if(sb->keys) MEM_freeN(sb->keys);
773         
774         sb->keys= NULL;
775         sb->totkey= 0;
776         
777 }
778
779 /* only frees internal data */
780 static void free_softbody_intern(SoftBody *sb)
781 {
782         if(sb) {
783                 int a;
784                 BodyPoint *bp;
785                 
786                 if(sb->bpoint){
787                         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
788                                 /* free spring list */ 
789                                 if (bp->springs != NULL) {
790                                         MEM_freeN(bp->springs);
791                                 }
792                         }
793                         MEM_freeN(sb->bpoint);
794                 }
795                 
796                 if(sb->bspring) MEM_freeN(sb->bspring);
797                 
798                 sb->totpoint= sb->totspring= 0;
799                 sb->bpoint= NULL;
800                 sb->bspring= NULL;
801                 
802                 free_softbody_baked(sb);
803         }
804 }
805
806
807 /* ************ dynamics ********** */
808
809 /* the most general (micro physics correct) way to do collision 
810 ** (only needs the current particle position)  
811 **
812 ** it actually checks if the particle intrudes a short range force field generated 
813 ** by the faces of the target object and returns a force to drive the particel out
814 ** the strenght of the field grows exponetially if the particle is on the 'wrong' side of the face
815 ** 'wrong' side : projection to the face normal is negative (all referred to a vertex in the face)
816 **
817 ** flaw of this: 'fast' particles as well as 'fast' colliding faces 
818 ** give a 'tunnel' effect such that the particle passes through the force field 
819 ** without ever 'seeing' it 
820 ** this is fully compliant to heisenberg: h >= fuzzy(location) * fuzzy(time)
821 ** besides our h is way larger than in QM because forces propagate way slower here
822 ** we have to deal with fuzzy(time) in the range of 1/25 seconds (typical frame rate)
823 ** yup collision targets are not known here any better 
824 ** and 1/25 second is looong compared to real collision events
825 ** Q: why not use 'simple' collision here like bouncing back a particle 
826 **   --> reverting is velocity on the face normal
827 ** A: because our particles are not alone here 
828 **    and need to tell their neighbours exactly what happens via spring forces 
829 ** unless sbObjectStep( .. ) is called on sub frame timing level
830 ** BTW that also questions the use of a 'implicit' solvers on softbodies  
831 ** since that would only valid for 'slow' moving collision targets and dito particles
832 */
833
834 /* aye this belongs to arith.c */
835 static void Vec3PlusStVec(float *v, float s, float *v1)
836 {
837         v[0] += s*v1[0];
838         v[1] += s*v1[1];
839         v[2] += s*v1[2];
840 }
841
842 /* +++ the spring external section*/
843
844 int sb_detect_edge_collisionCached(float edge_v1[3],float edge_v2[3],float *damp,                                               
845                                                                    float force[3], unsigned int par_layer,struct Object *vertexowner)
846 {
847         Base *base;
848         Object *ob;
849         float nv1[3], nv2[3], nv3[3], nv4[3], edge1[3], edge2[3], d_nvect[3], aabbmin[3],aabbmax[3];
850         float t,el;
851         int a, deflected=0;
852
853         aabbmin[0] = MIN2(edge_v1[0],edge_v2[0]);
854         aabbmin[1] = MIN2(edge_v1[1],edge_v2[1]);
855         aabbmin[2] = MIN2(edge_v1[2],edge_v2[2]);
856         aabbmax[0] = MAX2(edge_v1[0],edge_v2[0]);
857         aabbmax[1] = MAX2(edge_v1[1],edge_v2[1]);
858         aabbmax[2] = MAX2(edge_v1[2],edge_v2[2]);
859
860         el = VecLenf(edge_v1,edge_v2);
861
862         base= G.scene->base.first;
863     while (base) {
864                 /*Only proceed for mesh object in same layer */
865                 if(base->object->type==OB_MESH && (base->lay & par_layer)) {
866                         ob= base->object;
867                         if((vertexowner) && (ob == vertexowner)){ 
868                                 /* if vertexowner is given  we don't want to check collision with owner object */ 
869                                 base = base->next;
870                                 continue;                               
871                         }
872
873                         /* only with deflecting set */
874                         if(ob->pd && ob->pd->deflect) {
875                                 MFace *mface= NULL;
876                                 MVert *mvert= NULL;
877                                 ccdf_minmax *mima= NULL;
878
879                                 if(ob->sumohandle){
880                                         ccd_Mesh *ccdm=ob->sumohandle;
881                                         mface= ccdm->mface;
882                                         mvert= ccdm->mvert;
883                                         mima= ccdm->mima;
884                                         a = ccdm->totface;
885
886                                         if ((aabbmax[0] < ccdm->bbmin[0]) || 
887                                                 (aabbmax[1] < ccdm->bbmin[1]) ||
888                                                 (aabbmax[2] < ccdm->bbmin[2]) ||
889                                                 (aabbmin[0] > ccdm->bbmax[0]) || 
890                                                 (aabbmin[1] > ccdm->bbmax[1]) || 
891                                                 (aabbmin[2] > ccdm->bbmax[2]) ) {
892                                                 /* boxes dont intersect */ 
893                                                 base = base->next;
894                                                 continue;                               
895                                         }                                       
896
897                                 }
898                                 else{
899                                         /*aye that should be cached*/
900                                         printf("missing cache error \n");
901                                         base = base->next;
902                                         continue;                               
903                                 }
904
905
906                                 /* use mesh*/
907                                 while (a--) {
908                                         if (
909                                                 (aabbmax[0] < mima->minx) || 
910                                                 (aabbmin[0] > mima->maxx) || 
911                                                 (aabbmax[1] < mima->miny) ||
912                                                 (aabbmin[1] > mima->maxy) || 
913                                                 (aabbmax[2] < mima->minz) ||
914                                                 (aabbmin[2] > mima->maxz) 
915                                                 ) {
916                                                 mface++;
917                                                 mima++;
918                                                 continue;
919                                         }
920
921
922                                         if (mvert){
923
924                                                 VECCOPY(nv1,mvert[mface->v1].co);                                               
925                                                 VECCOPY(nv2,mvert[mface->v2].co);
926                                                 VECCOPY(nv3,mvert[mface->v3].co);
927                                                 if (mface->v4){
928                                                         VECCOPY(nv4,mvert[mface->v4].co);
929                                                 }
930                                         }
931
932                                         /* switch origin to be nv2*/
933                                         VECSUB(edge1, nv1, nv2);
934                                         VECSUB(edge2, nv3, nv2);
935
936                                         Crossf(d_nvect, edge2, edge1);
937                                         Normalise(d_nvect);
938                                         if ( LineIntersectsTriangle(edge_v1, edge_v2, nv1, nv2, nv3, &t)){
939                                                 float v1[3],v2[3];
940                                                 float intrusiondepth,i1,i2;
941                                                 VECSUB(v1, edge_v1, nv2);
942                                                 VECSUB(v2, edge_v2, nv2);
943                                                 i1 = Inpf(v1,d_nvect);
944                                                 i2 = Inpf(v2,d_nvect);
945                                                 intrusiondepth = -MIN2(i1,i2)/el;
946                                                 Vec3PlusStVec(force,intrusiondepth,d_nvect);
947                                                 *damp=ob->pd->pdef_sbdamp;
948                                                 deflected = 2;
949                                         }
950                                         if (mface->v4){ /* quad */
951                                                 /* switch origin to be nv4 */
952                                                 VECSUB(edge1, nv3, nv4);
953                                                 VECSUB(edge2, nv1, nv4);
954
955                                                 Crossf(d_nvect, edge2, edge1);
956                                                 Normalise(d_nvect);                                             
957                                                 if (LineIntersectsTriangle( edge_v1, edge_v2,nv1, nv3, nv4, &t)){
958                                                         float v1[3],v2[3];
959                                                         float intrusiondepth,i1,i2;
960                                                         VECSUB(v1, edge_v1, nv4);
961                                                         VECSUB(v2, edge_v2, nv4);
962                                                 i1 = Inpf(v1,d_nvect);
963                                                 i2 = Inpf(v2,d_nvect);
964                                                 intrusiondepth = -MIN2(i1,i2)/el;
965
966
967                                                         Vec3PlusStVec(force,intrusiondepth,d_nvect);
968                                                         *damp=ob->pd->pdef_sbdamp;
969                                                         deflected = 2;
970                                                 }
971                                         }
972                                         mface++;
973                                         mima++;                                 
974                                 }/* while a */          
975                         } /* if(ob->pd && ob->pd->deflect) */
976                 }/* if (base->object->type==OB_MESH && (base->lay & par_layer)) { */
977                 base = base->next;
978         } /* while (base) */
979         return deflected;       
980 }
981
982
983 void scan_for_ext_spring_forces(Object *ob)
984 {
985         SoftBody *sb = ob->soft;
986         ListBase *do_effector;
987         int a;
988         float damp; /* note, damp is mute here, but might be weight painted in future */
989         float feedback[3];
990         do_effector= pdInitEffectors(ob,NULL);
991
992         if (sb && sb->totspring){
993                 for(a=0; a<sb->totspring; a++) {
994                         BodySpring *bs = &sb->bspring[a];
995                         bs->ext_force[0]=bs->ext_force[1]=bs->ext_force[2]=0.0f; 
996                         feedback[0]=feedback[1]=feedback[2]=0.0f;
997                         bs->flag &= ~BSF_INTERSECT;
998
999                         if (bs->order ==1){
1000                                 /* +++ springs colliding */
1001                                 if (ob->softflag & OB_SB_EDGECOLL){
1002                                         if ( sb_detect_edge_collisionCached (sb->bpoint[bs->v1].pos , sb->bpoint[bs->v2].pos,
1003                                                 &damp,feedback,ob->lay,ob)){
1004                                                         VecAddf(bs->ext_force,bs->ext_force,feedback);
1005                                                         bs->flag |= BSF_INTERSECT;
1006
1007                                         }
1008                                 }
1009                                 /* ---- springs colliding */
1010
1011                                 /* +++ springs seeing wind ... n stuff depending on their orientation*/
1012                                 /* note we don't use sb->mediafrict but use sb->aeroedge for magnitude of effect*/ 
1013                                 if(sb->aeroedge){
1014                                         float vel[3],sp[3],pr[3],force[3];
1015                                         float f,windfactor  = 250.0f;   
1016                                         /*see if we have wind*/
1017                                         if(do_effector) {
1018                                                 float speed[3]={0.0f,0.0f,0.0f};
1019                                                 float pos[3];
1020                                                 VecMidf(pos, sb->bpoint[bs->v1].pos , sb->bpoint[bs->v2].pos);
1021                                                 VecMidf(vel, sb->bpoint[bs->v1].vec , sb->bpoint[bs->v2].vec);
1022                                                 pdDoEffectors(do_effector, pos, force, speed, (float)G.scene->r.cfra, 0.0f, PE_WIND_AS_SPEED);
1023                                                 VecMulf(speed,windfactor); 
1024                                                 VecAddf(vel,vel,speed);
1025                                         }
1026                                         /* media in rest */
1027                                         else{
1028                                                 VECADD(vel, sb->bpoint[bs->v1].vec , sb->bpoint[bs->v2].vec);
1029                                         }
1030                                         f = Normalise(vel);
1031                                         f = -0.0001f*f*f*sb->aeroedge;
1032                                         /* todo add a nice angle dependant function */
1033                                         /* look up one at bergman scheafer */
1034
1035                                         VECSUB(sp, sb->bpoint[bs->v1].pos , sb->bpoint[bs->v2].pos);
1036                                         Projf(pr,vel,sp);
1037                                         VECSUB(vel,vel,pr);
1038                                         Normalise(vel);
1039                                         Vec3PlusStVec(bs->ext_force,f,vel);
1040                                 }
1041                                 /* --- springs seeing wind */
1042                         }
1043                 }
1044         }
1045         if(do_effector)
1046                 pdEndEffectors(do_effector);
1047 }
1048 /* --- the spring external section*/
1049
1050 int sb_detect_vertex_collisionCached(float opco[3], float facenormal[3], float *damp,
1051                                                                          float force[3], unsigned int par_layer,struct Object *vertexowner)
1052 {
1053         Base *base;
1054         Object *ob;
1055         float nv1[3], nv2[3], nv3[3], nv4[3], edge1[3], edge2[3],d_nvect[3], dv1[3],
1056                 facedist,n_mag,force_mag_norm,minx,miny,minz,maxx,maxy,maxz,
1057                 innerfacethickness = -0.5f, outerfacethickness = 0.2f,
1058                 ee = 5.0f, ff = 0.1f, fa;
1059         int a, deflected=0;
1060
1061         base= G.scene->base.first;
1062         while (base) {
1063                 /*Only proceed for mesh object in same layer */
1064                 if(base->object->type==OB_MESH && (base->lay & par_layer)) {
1065                         ob= base->object;
1066                         if((vertexowner) && (ob == vertexowner)){ 
1067                                 /* if vertexowner is given  we don't want to check collision with owner object */ 
1068                                 base = base->next;
1069                                 continue;                               
1070                         }
1071
1072                         /* only with deflecting set */
1073                         if(ob->pd && ob->pd->deflect) {
1074                                 MFace *mface= NULL;
1075                                 MVert *mvert= NULL;
1076                                 ccdf_minmax *mima= NULL;
1077
1078                                 if(ob->sumohandle){
1079                                         ccd_Mesh *ccdm=ob->sumohandle;
1080                                         mface= ccdm->mface;
1081                                         mvert= ccdm->mvert;
1082                                         mima= ccdm->mima;
1083                                         a = ccdm->totface;
1084
1085                                         minx =ccdm->bbmin[0]; 
1086                                         miny =ccdm->bbmin[1]; 
1087                                         minz =ccdm->bbmin[2];
1088
1089                                         maxx =ccdm->bbmax[0]; 
1090                                         maxy =ccdm->bbmax[1]; 
1091                                         maxz =ccdm->bbmax[2]; 
1092
1093                                         if ((opco[0] < minx) || 
1094                                                 (opco[1] < miny) ||
1095                                                 (opco[2] < minz) ||
1096                                                 (opco[0] > maxx) || 
1097                                                 (opco[1] > maxy) || 
1098                                                 (opco[2] > maxz) ) {
1099                                                         /* outside the padded boundbox --> collision object is too far away */ 
1100                                                         base = base->next;
1101                                                         continue;                               
1102                                         }                                       
1103                                 }
1104                                 else{
1105                                         /*aye that should be cached*/
1106                                         printf("missing cache error \n");
1107                                         base = base->next;
1108                                         continue;                               
1109                                 }
1110
1111                                 /* do object level stuff */
1112                                 /* need to have user control for that since it depends on model scale */
1113                                 innerfacethickness =-ob->pd->pdef_sbift;
1114                                 outerfacethickness =ob->pd->pdef_sboft;
1115                                 fa = (ff*outerfacethickness-outerfacethickness);
1116                                 fa *= fa;
1117                                 fa = 1.0f/fa;
1118
1119                                 /* use mesh*/
1120                                 while (a--) {
1121                                         if (
1122                                                 (opco[0] < mima->minx) || 
1123                                                 (opco[0] > mima->maxx) || 
1124                                                 (opco[1] < mima->miny) ||
1125                                                 (opco[1] > mima->maxy) || 
1126                                                 (opco[2] < mima->minz) ||
1127                                                 (opco[2] > mima->maxz) 
1128                                                 ) {
1129                                                         mface++;
1130                                                         mima++;
1131                                                         continue;
1132                                         }
1133
1134                                         if (mvert){
1135
1136                                                 VECCOPY(nv1,mvert[mface->v1].co);                                               
1137                                                 VECCOPY(nv2,mvert[mface->v2].co);
1138                                                 VECCOPY(nv3,mvert[mface->v3].co);
1139                                                 if (mface->v4){
1140                                                         VECCOPY(nv4,mvert[mface->v4].co);
1141                                                 }
1142                                         }
1143
1144                                         /* switch origin to be nv2*/
1145                                         VECSUB(edge1, nv1, nv2);
1146                                         VECSUB(edge2, nv3, nv2);
1147                                         VECSUB(dv1,opco,nv2); /* abuse dv1 to have vertex in question at *origin* of triangle */
1148
1149                                         Crossf(d_nvect, edge2, edge1);
1150                                         n_mag = Normalise(d_nvect);
1151                                         facedist = Inpf(dv1,d_nvect);
1152
1153                                         if ((facedist > innerfacethickness) && (facedist < outerfacethickness)){                
1154                                                 if (point_in_tri_prism(opco, nv1, nv2, nv3) ){
1155                                                         force_mag_norm =(float)exp(-ee*facedist);
1156                                                         if (facedist > outerfacethickness*ff)
1157                                                                 force_mag_norm =(float)force_mag_norm*fa*(facedist - outerfacethickness)*(facedist - outerfacethickness);
1158                                                         Vec3PlusStVec(force,force_mag_norm,d_nvect);
1159                                                         *damp=ob->pd->pdef_sbdamp;
1160                                                         deflected = 2;
1161                                                 }
1162                                         }               
1163                                         if (mface->v4){ /* quad */
1164                                                 /* switch origin to be nv4 */
1165                                                 VECSUB(edge1, nv3, nv4);
1166                                                 VECSUB(edge2, nv1, nv4);
1167                                                 VECSUB(dv1,opco,nv4); /* abuse dv1 to have vertex in question at *origin* of triangle */
1168
1169                                                 Crossf(d_nvect, edge2, edge1);
1170                                                 n_mag = Normalise(d_nvect);
1171                                                 facedist = Inpf(dv1,d_nvect);
1172
1173                                                 if ((facedist > innerfacethickness) && (facedist < outerfacethickness)){
1174                                                         if (point_in_tri_prism(opco, nv1, nv3, nv4) ){
1175                                                                 force_mag_norm =(float)exp(-ee*facedist);
1176                                                                 if (facedist > outerfacethickness*ff)
1177                                                                         force_mag_norm =(float)force_mag_norm*fa*(facedist - outerfacethickness)*(facedist - outerfacethickness);
1178                                                                 Vec3PlusStVec(force,force_mag_norm,d_nvect);
1179                                                                 *damp=ob->pd->pdef_sbdamp;
1180                                                                 deflected = 2;
1181                                                         }
1182
1183                                                 }
1184                                         }
1185                                         mface++;
1186                                         mima++;                                 
1187                                 }/* while a */          
1188                         } /* if(ob->pd && ob->pd->deflect) */
1189                 }/* if (base->object->type==OB_MESH && (base->lay & par_layer)) { */
1190                 base = base->next;
1191         } /* while (base) */
1192         return deflected;       
1193 }
1194
1195
1196
1197 static int sb_deflect_face(Object *ob,float *actpos, float *futurepos,float *collisionpos, float *facenormal,float *force,float *cf)
1198 {
1199         float s_actpos[3];
1200         int deflected;
1201         
1202         VECCOPY(s_actpos,actpos);
1203         deflected= sb_detect_vertex_collisionCached(s_actpos, facenormal, cf, force , ob->lay, ob);
1204         return(deflected);
1205 }
1206
1207
1208 static int is_there_deflection(unsigned int layer)
1209 {
1210         Base *base;
1211         
1212         for(base = G.scene->base.first; base; base= base->next) {
1213                 if( (base->lay & layer) && base->object->pd) {
1214                         if(base->object->pd->deflect) 
1215                                 return 1;
1216                 }
1217         }
1218         return 0;
1219 }
1220
1221 static void softbody_calc_forces(Object *ob, float forcetime)
1222 {
1223 /* rule we never alter free variables :bp->vec bp->pos in here ! 
1224  * this will ruin adaptive stepsize AKA heun! (BM) 
1225  */
1226         SoftBody *sb= ob->soft; /* is supposed to be there */
1227         BodyPoint  *bp;
1228         BodyPoint *bproot;
1229         BodySpring *bs; 
1230         ListBase *do_effector;
1231         float iks, ks, kd, gravity, actspringlen, forcefactor, sd[3];
1232         float fieldfactor = 1000.0f, windfactor  = 250.0f;   
1233         int a, b,  do_deflector,do_selfcollision,do_springcollision,do_aero;
1234         
1235         /* clear forces */
1236         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
1237                 bp->force[0]= bp->force[1]= bp->force[2]= 0.0;
1238         }
1239         
1240         gravity = sb->grav * sb_grav_force_scale(ob);   
1241         
1242         /* check conditions for various options */
1243         do_deflector= is_there_deflection(ob->lay);
1244         do_effector= pdInitEffectors(ob,NULL);
1245         do_selfcollision=((ob->softflag & OB_SB_EDGES) && (sb->bspring)&& (ob->softflag & OB_SB_SELF));
1246         do_springcollision=do_deflector && (ob->softflag & OB_SB_EDGES) &&(ob->softflag & OB_SB_EDGECOLL);
1247         do_aero=((sb->aeroedge)&& (ob->softflag & OB_SB_EDGES));
1248         
1249         iks  = 1.0f/(1.0f-sb->inspring)-1.0f ;/* inner spring constants function */
1250         bproot= sb->bpoint; /* need this for proper spring addressing */
1251         
1252
1253         
1254         if (do_springcollision || do_aero)  scan_for_ext_spring_forces(ob);
1255         
1256         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
1257                 /* naive ball self collision */
1258                 /* needs to be done if goal snaps or not */
1259                 if(do_selfcollision){
1260                                 int attached;
1261                                 BodyPoint   *obp;
1262                                 int c,b;
1263                                 float velcenter[3],dvel[3],def[3];
1264                                 float tune = sb->ballstiff;
1265                                 float distance;
1266                                 float compare;
1267
1268                                 for(c=sb->totpoint, obp= sb->bpoint; c>0; c--, obp++) {
1269                                         if (c < a ) continue; /* exploit force(a,b) == -force(b,a) part1/2 */
1270                                         compare = (obp->colball + bp->colball);         
1271                                         VecSubf(def, bp->pos, obp->pos);
1272                                         distance = Normalise(def);
1273                                         if (distance < compare ){
1274                                                 /* exclude body points attached with a spring */
1275                                                 attached = 0;
1276                                                 for(b=obp->nofsprings;b>0;b--){
1277                                                         bs = sb->bspring + obp->springs[b-1];
1278                                                         if (( sb->totpoint-a == bs->v2)  || ( sb->totpoint-a == bs->v1)){
1279                                                                 attached=1;
1280                                                                 continue;}
1281                                                 }
1282                                                 if (!attached){
1283                                                         float f = tune/(distance) + tune/(compare*compare)*distance - 2.0f*tune/compare ;
1284
1285                                                         VecMidf(velcenter, bp->vec, obp->vec);
1286                                                         VecSubf(dvel,velcenter,bp->vec);
1287                                                         VecMulf(dvel,sb->nodemass);
1288
1289                                                         Vec3PlusStVec(bp->force,sb->balldamp,dvel);
1290                                                         Vec3PlusStVec(bp->force,f*(1.0f-sb->balldamp),def);
1291                                                         /* exploit force(a,b) == -force(b,a) part2/2 */
1292                                                         VecSubf(dvel,velcenter,obp->vec);
1293                                                         VecMulf(dvel,sb->nodemass);
1294
1295                                                         Vec3PlusStVec(obp->force,sb->balldamp,dvel);
1296                                                         Vec3PlusStVec(obp->force,-f*(1.0f-sb->balldamp),def);
1297
1298                                                 }
1299                                         }
1300                                 }
1301                 }
1302                 /* naive ball self collision done */
1303
1304                 if(bp->goal < SOFTGOALSNAP){ /* ommit this bp when it snaps */
1305                         float auxvect[3];  
1306                         float velgoal[3];
1307                         float absvel =0, projvel= 0;
1308                         
1309                         /* do goal stuff */
1310                         if(ob->softflag & OB_SB_GOAL) {
1311                                 /* true elastic goal */
1312                                 VecSubf(auxvect,bp->origT,bp->pos);
1313                                 ks  = 1.0f/(1.0f- bp->goal*sb->goalspring)-1.0f ;
1314                                 bp->force[0]+= ks*(auxvect[0]);
1315                                 bp->force[1]+= ks*(auxvect[1]);
1316                                 bp->force[2]+= ks*(auxvect[2]);
1317                                 /* calulate damping forces generated by goals*/
1318                                 VecSubf(velgoal,bp->origS, bp->origE);
1319                                 kd =  sb->goalfrict * sb_fric_force_scale(ob) ;
1320                                 
1321                                 if (forcetime > 0.0 ) { /* make sure friction does not become rocket motor on time reversal */
1322                                         bp->force[0]-= kd * (velgoal[0] + bp->vec[0]);
1323                                         bp->force[1]-= kd * (velgoal[1] + bp->vec[1]);
1324                                         bp->force[2]-= kd * (velgoal[2] + bp->vec[2]);
1325                                 }
1326                                 else {
1327                                         bp->force[0]-= kd * (velgoal[0] - bp->vec[0]);
1328                                         bp->force[1]-= kd * (velgoal[1] - bp->vec[1]);
1329                                         bp->force[2]-= kd * (velgoal[2] - bp->vec[2]);
1330                                 }
1331                         }
1332                         /* done goal stuff */
1333                         
1334                         
1335                         /* gravitation */
1336                         bp->force[2]-= gravity*sb->nodemass; /* individual mass of node here */
1337                         
1338                         /* particle field & vortex */
1339                         if(do_effector) {
1340                                 float force[3]= {0.0f, 0.0f, 0.0f};
1341                                 float speed[3]= {0.0f, 0.0f, 0.0f};
1342                                 float eval_sb_fric_force_scale = sb_fric_force_scale(ob); /* just for calling function once */
1343                                 
1344                                 pdDoEffectors(do_effector, bp->pos, force, speed, (float)G.scene->r.cfra, 0.0f, PE_WIND_AS_SPEED);
1345                                 
1346                                 /* apply forcefield*/
1347                                 VecMulf(force,fieldfactor* eval_sb_fric_force_scale); 
1348                                 VECADD(bp->force, bp->force, force);
1349                                 
1350                                 /* BP friction in moving media */       
1351                                 kd= sb->mediafrict* eval_sb_fric_force_scale;  
1352                                 bp->force[0] -= kd * (bp->vec[0] + windfactor*speed[0]/eval_sb_fric_force_scale);
1353                                 bp->force[1] -= kd * (bp->vec[1] + windfactor*speed[1]/eval_sb_fric_force_scale);
1354                                 bp->force[2] -= kd * (bp->vec[2] + windfactor*speed[2]/eval_sb_fric_force_scale);
1355                                 /* now we'll have nice centrifugal effect for vortex */
1356                                 
1357                         }
1358                         else {
1359                                 /* BP friction in media (not) moving*/
1360                                 kd= sb->mediafrict* sb_fric_force_scale(ob);  
1361                                 /* assume it to be proportional to actual velocity */
1362                                 bp->force[0]-= bp->vec[0]*kd;
1363                                 bp->force[1]-= bp->vec[1]*kd;
1364                                 bp->force[2]-= bp->vec[2]*kd;
1365                                 /* friction in media done */
1366                         }
1367                         
1368                         /* +++cached collision targets */
1369                         if(do_deflector) {
1370                                 float defforce[3] = {0.0f,0.0f,0.0f}, collisionpos[3],facenormal[3], cf = 1.0f;
1371                                 kd = 1.0f;
1372                                 
1373                                 if (sb_deflect_face(ob,bp->pos, bp->pos, collisionpos, facenormal,defforce,&cf)){
1374                                         Vec3PlusStVec(bp->force,kd,defforce);
1375                                         bp->contactfrict = cf;
1376                                 }
1377                                 else{ 
1378                                         bp->contactfrict = 0.0f;
1379                                 }
1380                         } 
1381                         else
1382                         {
1383                                         bp->contactfrict = 0.0f;
1384                         }
1385                         /* ---cached collision targets */
1386                         
1387                         /* +++springs */
1388                         if(ob->softflag & OB_SB_EDGES) {
1389                                 if (sb->bspring){ /* spring list exists at all ? */
1390                                         for(b=bp->nofsprings;b>0;b--){
1391                                                 bs = sb->bspring + bp->springs[b-1];
1392                                                 if (do_springcollision || do_aero){
1393                                                         VecAddf(bp->force,bp->force,bs->ext_force);
1394                                                         if (bs->flag & BSF_INTERSECT)
1395                                                                 bp->contactfrict = 0.9f; /* another ad hoc magic */
1396
1397                                                 }
1398
1399                                                 if (( (sb->totpoint-a) == bs->v1) ){ 
1400                                                         actspringlen= VecLenf( (bproot+bs->v2)->pos, bp->pos);
1401                                                         VecSubf(sd,(bproot+bs->v2)->pos, bp->pos);
1402                                                         Normalise(sd);
1403                                                         
1404                                                         /* friction stuff V1 */
1405                                                         VecSubf(velgoal,bp->vec,(bproot+bs->v2)->vec);
1406                                                         kd = sb->infrict * sb_fric_force_scale(ob);
1407                                                         absvel  = Normalise(velgoal);
1408                                                         projvel = ABS(Inpf(sd,velgoal));
1409                                                         kd *= absvel * projvel;
1410                                                         Vec3PlusStVec(bp->force,-kd,velgoal);
1411                                                         
1412                                                         if(bs->len > 0.0) /* check for degenerated springs */
1413                                                                 forcefactor = (bs->len - actspringlen)/bs->len * iks;
1414                                                         else
1415                                                                 forcefactor = actspringlen * iks;
1416                                                         forcefactor *= bs->strength; 
1417                                                         
1418                                                         Vec3PlusStVec(bp->force,-forcefactor,sd);
1419                                                         
1420                                                 }
1421                                                 
1422                                                 if (( (sb->totpoint-a) == bs->v2) ){ 
1423                                                         actspringlen= VecLenf( (bproot+bs->v1)->pos, bp->pos);
1424                                                         VecSubf(sd,bp->pos,(bproot+bs->v1)->pos);
1425                                                         Normalise(sd);
1426                                                         
1427                                                         /* friction stuff V2 */
1428                                                         VecSubf(velgoal,bp->vec,(bproot+bs->v1)->vec);
1429                                                         kd = sb->infrict * sb_fric_force_scale(ob);
1430                                                         absvel  = Normalise(velgoal);
1431                                                         projvel = ABS(Inpf(sd,velgoal));
1432                                                         kd *= absvel * projvel;
1433                                                         Vec3PlusStVec(bp->force,-kd,velgoal);
1434                                                         
1435                                                         if(bs->len > 0.0)
1436                                                                 forcefactor = (bs->len - actspringlen)/bs->len * iks;
1437                                                         else
1438                                                                 forcefactor = actspringlen * iks;
1439                                                         forcefactor *= bs->strength; 
1440                                                         Vec3PlusStVec(bp->force,+forcefactor,sd);                                                       
1441                                                 }
1442                                         }/* loop springs */
1443                                 }/* existing spring list */ 
1444                         }/*any edges*/
1445                         /* ---springs */
1446                 }/*omit on snap */
1447         }/*loop all bp's*/
1448
1449         /* cleanup */
1450         if(do_effector) pdEndEffectors(do_effector);
1451 }
1452
1453
1454
1455 static void softbody_apply_forces(Object *ob, float forcetime, int mode, float *err)
1456 {
1457         /* time evolution */
1458         /* actually does an explicit euler step mode == 0 */
1459         /* or heun ~ 2nd order runge-kutta steps, mode 1,2 */
1460         SoftBody *sb= ob->soft; /* is supposed to be there */
1461         BodyPoint *bp;
1462         float dx[3],dv[3];
1463         float timeovermass;
1464         float maxerr = 0.0;
1465         int a;
1466
1467     forcetime *= sb_time_scale(ob);
1468     
1469         /* claim a minimum mass for vertex */
1470         if (sb->nodemass > 0.09999f) timeovermass = forcetime/sb->nodemass;
1471         else timeovermass = forcetime/0.09999f;
1472         
1473         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
1474                 if(bp->goal < SOFTGOALSNAP){
1475                         
1476                         /* so here is (v)' = a(cceleration) = sum(F_springs)/m + gravitation + some friction forces  + more forces*/
1477                         /* the ( ... )' operator denotes derivate respective time */
1478                         /* the euler step for velocity then becomes */
1479                         /* v(t + dt) = v(t) + a(t) * dt */ 
1480                         bp->force[0]*= timeovermass; /* individual mass of node here */ 
1481                         bp->force[1]*= timeovermass;
1482                         bp->force[2]*= timeovermass;
1483                         /* some nasty if's to have heun in here too */
1484                         VECCOPY(dv,bp->force); 
1485                         if (mode == 1){
1486                                 VECCOPY(bp->prevvec, bp->vec);
1487                                 VECCOPY(bp->prevdv, dv);
1488                         }
1489                         if (mode ==2){
1490                                 /* be optimistic and execute step */
1491                                 bp->vec[0] = bp->prevvec[0] + 0.5f * (dv[0] + bp->prevdv[0]);
1492                                 bp->vec[1] = bp->prevvec[1] + 0.5f * (dv[1] + bp->prevdv[1]);
1493                                 bp->vec[2] = bp->prevvec[2] + 0.5f * (dv[2] + bp->prevdv[2]);
1494                                 /* compare euler to heun to estimate error for step sizing */
1495                                 maxerr = MAX2(maxerr,ABS(dv[0] - bp->prevdv[0]));
1496                                 maxerr = MAX2(maxerr,ABS(dv[1] - bp->prevdv[1]));
1497                                 maxerr = MAX2(maxerr,ABS(dv[2] - bp->prevdv[2]));
1498                         }
1499                         else {VECADD(bp->vec, bp->vec, bp->force);}
1500
1501                         /* so here is (x)'= v(elocity) */
1502                         /* the euler step for location then becomes */
1503                         /* x(t + dt) = x(t) + v(t) * dt */ 
1504                         
1505                         VECCOPY(dx,bp->vec);
1506                         dx[0]*=forcetime ; 
1507                         dx[1]*=forcetime ; 
1508                         dx[2]*=forcetime ; 
1509                         
1510                         /* again some nasty if's to have heun in here too */
1511                         if (mode ==1){
1512                                 VECCOPY(bp->prevpos,bp->pos);
1513                                 VECCOPY(bp->prevdx ,dx);
1514                         }
1515                         
1516                         if (mode ==2){
1517                                 bp->pos[0] = bp->prevpos[0] + 0.5f * ( dx[0] + bp->prevdx[0]);
1518                                 bp->pos[1] = bp->prevpos[1] + 0.5f * ( dx[1] + bp->prevdx[1]);
1519                                 bp->pos[2] = bp->prevpos[2] + 0.5f* ( dx[2] + bp->prevdx[2]);
1520                                 maxerr = MAX2(maxerr,ABS(dx[0] - bp->prevdx[0]));
1521                                 maxerr = MAX2(maxerr,ABS(dx[1] - bp->prevdx[1]));
1522                                 maxerr = MAX2(maxerr,ABS(dx[2] - bp->prevdx[2]));
1523 /* weak point:  not knowing anything about targets dynamics we assume it to be resting */
1524 /* while inside collision target .. make movement more *viscous* */
1525                                 if (bp->contactfrict > 0.0f){
1526                                         bp->vec[0] *= (1.0f - bp->contactfrict);
1527                                         bp->vec[1] *= (1.0f - bp->contactfrict);
1528                                         bp->vec[2] *= (1.0f - bp->contactfrict);
1529                                 }
1530                         }
1531                         else { VECADD(bp->pos, bp->pos, dx);}
1532                 }/*snap*/
1533         } /*for*/
1534         
1535         if (err){ /* so step size will be controlled by biggest difference in slope */
1536                 *err = maxerr;
1537         }
1538 }
1539
1540 /* used by heun when it overshoots */
1541 static void softbody_restore_prev_step(Object *ob)
1542 {
1543         SoftBody *sb= ob->soft; /* is supposed to be there*/
1544         BodyPoint *bp;
1545         int a;
1546         
1547         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
1548                 VECCOPY(bp->vec, bp->prevvec);
1549                 VECCOPY(bp->pos, bp->prevpos);
1550         }
1551 }
1552
1553 /* care for bodypoints taken out of the 'ordinary' solver step
1554 ** because they are screwed to goal by bolts
1555 ** they just need to move along with the goal in time 
1556 ** we need to adjust them on sub frame timing in solver 
1557 ** so now when frame is done .. put 'em to the position at the end of frame
1558 */
1559 static void softbody_apply_goalsnap(Object *ob)
1560 {
1561         SoftBody *sb= ob->soft; /* is supposed to be there */
1562         BodyPoint *bp;
1563         int a;
1564         
1565         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
1566                 if (bp->goal >= SOFTGOALSNAP){
1567                         VECCOPY(bp->prevpos,bp->pos);
1568                         VECCOPY(bp->pos,bp->origT);
1569                 }               
1570         }
1571 }
1572
1573 /* expects full initialized softbody */
1574 static void interpolate_exciter(Object *ob, int timescale, int time)
1575 {
1576         SoftBody *sb= ob->soft;
1577         BodyPoint *bp;
1578         float f;
1579         int a;
1580         
1581         f = (float)time/(float)timescale;
1582         
1583         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {   
1584                 bp->origT[0] = bp->origS[0] + f*(bp->origE[0] - bp->origS[0]); 
1585                 bp->origT[1] = bp->origS[1] + f*(bp->origE[1] - bp->origS[1]); 
1586                 bp->origT[2] = bp->origS[2] + f*(bp->origE[2] - bp->origS[2]); 
1587                 if (bp->goal >= SOFTGOALSNAP){
1588                         bp->vec[0] = bp->origE[0] - bp->origS[0];
1589                         bp->vec[1] = bp->origE[1] - bp->origS[1];
1590                         bp->vec[2] = bp->origE[2] - bp->origS[2];
1591                 }
1592         }
1593         
1594 }
1595
1596
1597 /* ************ convertors ********** */
1598
1599 /*  for each object type we need;
1600     - xxxx_to_softbody(Object *ob)      : a full (new) copy, creates SB geometry
1601 */
1602
1603 static void get_scalar_from_vertexgroup(Object *ob, int vertID, short groupindex, float *target)
1604 /* result 0 on success, else indicates error number
1605 -- kind of *inverse* result defintion,
1606 -- but this way we can signal error condition to caller  
1607 -- and yes this function must not be here but in a *vertex group module*
1608 */
1609 {
1610         MDeformVert *dv= NULL;
1611         int i;
1612         
1613         /* spot the vert in deform vert list at mesh */
1614         if(ob->type==OB_MESH) {
1615                 Mesh *me= ob->data;
1616                 if (me->dvert)
1617                         dv = me->dvert + vertID;
1618         }
1619         else if(ob->type==OB_LATTICE) { /* not yet supported in softbody btw */
1620                 Lattice *lt= ob->data;
1621                 if (lt->dvert)
1622                         dv = lt->dvert + vertID;
1623         }
1624         if(dv) {
1625                 /* Lets see if this vert is in the weight group */
1626                 for (i=0; i<dv->totweight; i++){
1627                         if (dv->dw[i].def_nr == groupindex){
1628                                 *target= dv->dw[i].weight; /* got it ! */
1629                                 break;
1630                         }
1631                 }
1632         }
1633
1634
1635 /* Resetting a Mesh SB object's springs */  
1636 /* Spring lenght are caculted from'raw' mesh vertices that are NOT altered by modifier stack. */ 
1637 static void springs_from_mesh(Object *ob)
1638 {
1639         SoftBody *sb;
1640         Mesh *me= ob->data;
1641         BodyPoint *bp;
1642         int a;
1643         
1644         sb= ob->soft;   
1645         if (me && sb)
1646         { 
1647         /* using bp->origS as a container for spring calcualtions here
1648         ** will be overwritten sbObjectStep() to receive 
1649         ** actual modifier stack positions
1650         */
1651                 if(me->totvert) {    
1652                         bp= ob->soft->bpoint;
1653                         for(a=0; a<me->totvert; a++, bp++) {
1654                                 VECCOPY(bp->origS, me->mvert[a].co);                            
1655                                 Mat4MulVecfl(ob->obmat, bp->origS);
1656                         }
1657                         
1658                 }
1659                 /* recalculate spring length for meshes here */
1660                 for(a=0; a<sb->totspring; a++) {
1661                         BodySpring *bs = &sb->bspring[a];
1662                         bs->len= VecLenf(sb->bpoint[bs->v1].origS, sb->bpoint[bs->v2].origS);
1663                 }
1664         }
1665 }
1666
1667
1668 /* makes totally fresh start situation */
1669 static void mesh_to_softbody(Object *ob)
1670 {
1671         SoftBody *sb;
1672         Mesh *me= ob->data;
1673         MEdge *medge= me->medge;
1674         BodyPoint *bp;
1675         BodySpring *bs;
1676         float goalfac;
1677         int a, totedge;
1678         if (ob->softflag & OB_SB_EDGES) totedge= me->totedge;
1679         else totedge= 0;
1680         
1681         /* renew ends with ob->soft with points and edges, also checks & makes ob->soft */
1682         renew_softbody(ob, me->totvert, totedge);
1683                 
1684         /* we always make body points */
1685         sb= ob->soft;   
1686         bp= sb->bpoint;
1687         goalfac= ABS(sb->maxgoal - sb->mingoal);
1688         
1689         for(a=0; a<me->totvert; a++, bp++) {
1690                 /* get scalar values needed  *per vertex* from vertex group functions,
1691                 so we can *paint* them nicly .. 
1692                 they are normalized [0.0..1.0] so may be we need amplitude for scale
1693                 which can be done by caller but still .. i'd like it to go this way 
1694                 */ 
1695                 
1696                 if((ob->softflag & OB_SB_GOAL) && sb->vertgroup) {
1697                         get_scalar_from_vertexgroup(ob, a,(short) (sb->vertgroup-1), &bp->goal);
1698                         /* do this always, regardless successfull read from vertex group */
1699                         bp->goal= sb->mingoal + bp->goal*goalfac;
1700                 }
1701                 /* a little ad hoc changing the goal control to be less *sharp* */
1702                 bp->goal = (float)pow(bp->goal, 4.0f);
1703                         
1704                 /* to proove the concept
1705                 this would enable per vertex *mass painting*
1706                 strcpy(name,"SOFTMASS");
1707                 error = get_scalar_from_named_vertexgroup(ob,name, a,&temp);
1708                 if (!error) bp->mass = temp * ob->rangeofmass;
1709                 */
1710         }
1711
1712         /* but we only optionally add body edge springs */
1713         if (ob->softflag & OB_SB_EDGES) {
1714                 if(medge) {
1715                         bs= sb->bspring;
1716                         for(a=me->totedge; a>0; a--, medge++, bs++) {
1717                                 bs->v1= medge->v1;
1718                                 bs->v2= medge->v2;
1719                                 bs->strength= 1.0;
1720                                 bs->order=1;
1721                         }
1722
1723                 
1724                         /* insert *diagonal* springs in quads if desired */
1725                         if (ob->softflag & OB_SB_QUADS) {
1726                                 add_mesh_quad_diag_springs(ob);
1727                         }
1728
1729                         build_bps_springlist(ob); /* scan for springs attached to bodypoints ONCE */
1730                         /* insert *other second order* springs if desired */
1731                         if (sb->secondspring > 0.0000001f) {
1732                                 add_2nd_order_springs(ob,sb->secondspring); /* exploits the the first run of build_bps_springlist(ob);*/
1733                                 build_bps_springlist(ob); /* yes we need to do it again*/
1734                         }
1735                         springs_from_mesh(ob); /* write the 'rest'-lenght of the springs */
1736                 if (ob->softflag & OB_SB_SELF) {calculate_collision_balls(ob);}
1737                 }
1738         }
1739         
1740 }
1741
1742
1743
1744 /*
1745 helper function to get proper spring length 
1746 when object is rescaled
1747 */
1748 float globallen(float *v1,float *v2,Object *ob)
1749 {
1750         float p1[3],p2[3];
1751         VECCOPY(p1,v1);
1752         Mat4MulVecfl(ob->obmat, p1);    
1753         VECCOPY(p2,v2);
1754         Mat4MulVecfl(ob->obmat, p2);
1755         return VecLenf(p1,p2);
1756 }
1757
1758 static void makelatticesprings(Lattice *lt,     BodySpring *bs, int dostiff,Object *ob)
1759 {
1760         BPoint *bp=lt->def, *bpu;
1761         int u, v, w, dv, dw, bpc=0, bpuc;
1762         
1763         dv= lt->pntsu;
1764         dw= dv*lt->pntsv;
1765         
1766         for(w=0; w<lt->pntsw; w++) {
1767                 
1768                 for(v=0; v<lt->pntsv; v++) {
1769                         
1770                         for(u=0, bpuc=0, bpu=NULL; u<lt->pntsu; u++, bp++, bpc++) {
1771                                 
1772                                 if(w) {
1773                                         bs->v1 = bpc;
1774                                         bs->v2 = bpc-dw;
1775                                     bs->strength= 1.0;
1776                                         bs->order=1;
1777                                         bs->len= globallen((bp-dw)->vec, bp->vec,ob);
1778                                         bs++;
1779                                 }
1780                                 if(v) {
1781                                         bs->v1 = bpc;
1782                                         bs->v2 = bpc-dv;
1783                                     bs->strength= 1.0;
1784                                         bs->order=1;
1785                                         bs->len= globallen((bp-dv)->vec, bp->vec,ob);
1786                                         bs++;
1787                                 }
1788                                 if(u) {
1789                                         bs->v1 = bpuc;
1790                                         bs->v2 = bpc;
1791                                     bs->strength= 1.0;
1792                                         bs->order=1;
1793                                         bs->len= globallen((bpu)->vec, bp->vec,ob);
1794                                         bs++;
1795                                 }
1796                                 
1797                                 if (dostiff) {
1798
1799                                         if(w){
1800                                                 if( v && u ) {
1801                                                         bs->v1 = bpc;
1802                                                         bs->v2 = bpc-dw-dv-1;
1803                                                         bs->strength= 1.0;
1804                                         bs->order=2;
1805                                                         bs->len= globallen((bp-dw-dv-1)->vec, bp->vec,ob);
1806                                                         bs++;
1807                                                 }                                               
1808                                                 if( (v < lt->pntsv-1) && (u) ) {
1809                                                         bs->v1 = bpc;
1810                                                         bs->v2 = bpc-dw+dv-1;
1811                                                         bs->strength= 1.0;
1812                                         bs->order=2;
1813                                                         bs->len= globallen((bp-dw+dv-1)->vec, bp->vec,ob);
1814                                                         bs++;
1815                                                 }                                               
1816                                         }
1817
1818                                         if(w < lt->pntsw -1){
1819                                                 if( v && u ) {
1820                                                         bs->v1 = bpc;
1821                                                         bs->v2 = bpc+dw-dv-1;
1822                                                         bs->strength= 1.0;
1823                                         bs->order=2;
1824                                                         bs->len= globallen((bp+dw-dv-1)->vec, bp->vec,ob);
1825                                                         bs++;
1826                                                 }                                               
1827                                                 if( (v < lt->pntsv-1) && (u) ) {
1828                                                         bs->v1 = bpc;
1829                                                         bs->v2 = bpc+dw+dv-1;
1830                                                         bs->strength= 1.0;
1831                                         bs->order=2;
1832                                                          bs->len= globallen((bp+dw+dv-1)->vec, bp->vec,ob);
1833                                                         bs++;
1834                                                 }                                               
1835                                         }
1836                                 }
1837                                 bpu = bp;
1838                                 bpuc = bpc;
1839                         }
1840                 }
1841         }
1842 }
1843
1844
1845 /* makes totally fresh start situation */
1846 static void lattice_to_softbody(Object *ob)
1847 {
1848         Lattice *lt= ob->data;
1849         SoftBody *sb;
1850         int totvert, totspring = 0;
1851
1852         totvert= lt->pntsu*lt->pntsv*lt->pntsw;
1853
1854         if (ob->softflag & OB_SB_EDGES){
1855                 totspring = ((lt->pntsu -1) * lt->pntsv 
1856                           + (lt->pntsv -1) * lt->pntsu) * lt->pntsw
1857                                   +lt->pntsu*lt->pntsv*(lt->pntsw -1);
1858                 if (ob->softflag & OB_SB_QUADS){
1859                         totspring += 4*(lt->pntsu -1) *  (lt->pntsv -1)  * (lt->pntsw-1);
1860                 }
1861         }
1862         
1863
1864         /* renew ends with ob->soft with points and edges, also checks & makes ob->soft */
1865         renew_softbody(ob, totvert, totspring);
1866         sb= ob->soft;   /* can be created in renew_softbody() */
1867         
1868         /* weights from bpoints, same code used as for mesh vertices */
1869         if((ob->softflag & OB_SB_GOAL) && sb->vertgroup) {
1870                 BodyPoint *bp= sb->bpoint;
1871                 BPoint *bpnt= lt->def;
1872                 float goalfac= ABS(sb->maxgoal - sb->mingoal);
1873                 int a;
1874
1875                 for(a=0; a<totvert; a++, bp++, bpnt++) {
1876                         bp->goal= sb->mingoal + bpnt->weight*goalfac;
1877                         /* a little ad hoc changing the goal control to be less *sharp* */
1878                         bp->goal = (float)pow(bp->goal, 4.0f);
1879                 }
1880         }       
1881         
1882         /* create some helper edges to enable SB lattice to be usefull at all */
1883         if (ob->softflag & OB_SB_EDGES){
1884                 makelatticesprings(lt,ob->soft->bspring,ob->softflag & OB_SB_QUADS,ob);
1885                 build_bps_springlist(ob); /* link bps to springs */
1886         }
1887 }
1888
1889 /* makes totally fresh start situation */
1890 static void curve_surf_to_softbody(Object *ob)
1891 {
1892         Curve *cu= ob->data;
1893         SoftBody *sb;
1894         BodyPoint *bp;
1895         BodySpring *bs;
1896         Nurb *nu;
1897         BezTriple *bezt;
1898         BPoint *bpnt;
1899         float goalfac;
1900         int a, curindex=0;
1901         int totvert, totspring = 0, setgoal=0;
1902         
1903         totvert= count_curveverts(&cu->nurb);
1904         
1905         if (ob->softflag & OB_SB_EDGES){
1906                 if(ob->type==OB_CURVE) {
1907                         totspring= totvert - BLI_countlist(&cu->nurb);
1908                 }
1909         }
1910         
1911         /* renew ends with ob->soft with points and edges, also checks & makes ob->soft */
1912         renew_softbody(ob, totvert, totspring);
1913         sb= ob->soft;   /* can be created in renew_softbody() */
1914                 
1915         /* set vars now */
1916         goalfac= ABS(sb->maxgoal - sb->mingoal);
1917         bp= sb->bpoint;
1918         bs= sb->bspring;
1919         
1920         /* weights from bpoints, same code used as for mesh vertices */
1921         if((ob->softflag & OB_SB_GOAL) && sb->vertgroup)
1922                 setgoal= 1;
1923                 
1924         for(nu= cu->nurb.first; nu; nu= nu->next) {
1925                 if(nu->bezt) {
1926                         for(bezt=nu->bezt, a=0; a<nu->pntsu; a++, bezt++, bp+=3, curindex+=3) {
1927                                 if(setgoal) {
1928                                         bp->goal= sb->mingoal + bezt->weight*goalfac;
1929                                         /* a little ad hoc changing the goal control to be less *sharp* */
1930                                         bp->goal = (float)pow(bp->goal, 4.0f);
1931                                         
1932                                         /* all three triples */
1933                                         (bp+1)->goal= bp->goal;
1934                                         (bp+2)->goal= bp->goal;
1935                                 }
1936                                 
1937                                 if(totspring) {
1938                                         if(a>0) {
1939                                                 bs->v1= curindex-1;
1940                                                 bs->v2= curindex;
1941                                                 bs->strength= 1.0;
1942                                                 bs->order=1;
1943                                                 bs->len= globallen( (bezt-1)->vec[2], bezt->vec[0], ob );
1944                                                 bs++;
1945                                         }
1946                                         bs->v1= curindex;
1947                                         bs->v2= curindex+1;
1948                                         bs->strength= 1.0;
1949                                         bs->order=1;
1950                                         bs->len= globallen( bezt->vec[0], bezt->vec[1], ob );
1951                                         bs++;
1952                                         
1953                                         bs->v1= curindex+1;
1954                                         bs->v2= curindex+2;
1955                                         bs->strength= 1.0;
1956                                         bs->order=1;
1957                                         bs->len= globallen( bezt->vec[1], bezt->vec[2], ob );
1958                                         bs++;
1959                                 }
1960                         }
1961                 }
1962                 else {
1963                         for(bpnt=nu->bp, a=0; a<nu->pntsu*nu->pntsv; a++, bpnt++, bp++, curindex++) {
1964                                 if(setgoal) {
1965                                         bp->goal= sb->mingoal + bpnt->weight*goalfac;
1966                                         /* a little ad hoc changing the goal control to be less *sharp* */
1967                                         bp->goal = (float)pow(bp->goal, 4.0f);
1968                                 }
1969                                 if(totspring && a>0) {
1970                                         bs->v1= curindex-1;
1971                                         bs->v2= curindex;
1972                                         bs->strength= 1.0;
1973                                         bs->order=1;
1974                                         bs->len= globallen( (bpnt-1)->vec, bpnt->vec , ob );
1975                                         bs++;
1976                                 }
1977                         }
1978                 }
1979         }
1980         
1981         if(totspring)
1982         {
1983                 build_bps_springlist(ob); /* link bps to springs */
1984                 if (ob->softflag & OB_SB_SELF) {calculate_collision_balls(ob);}
1985         }
1986 }
1987
1988
1989 /* copies softbody result back in object */
1990 static void softbody_to_object(Object *ob, float (*vertexCos)[3], int numVerts, int local)
1991 {
1992         BodyPoint *bp= ob->soft->bpoint;
1993         int a;
1994
1995         /* inverse matrix is not uptodate... */
1996         Mat4Invert(ob->imat, ob->obmat);
1997         
1998         for(a=0; a<numVerts; a++, bp++) {
1999                 VECCOPY(vertexCos[a], bp->pos);
2000                 if(local==0) 
2001                         Mat4MulVecfl(ob->imat, vertexCos[a]);   /* softbody is in global coords, baked optionally not */
2002         }
2003 }
2004
2005 /* return 1 if succesfully baked and applied step */
2006 static int softbody_baked_step(Object *ob, float framenr, float (*vertexCos)[3], int numVerts)
2007 {
2008         SoftBody *sb= ob->soft;
2009         SBVertex *key0, *key1, *key2, *key3;
2010         BodyPoint *bp;
2011         float data[4], sfra, efra, cfra, dfra, fac;     /* start, end, current, delta */
2012         int ofs1, a;
2013
2014         /* precondition check */
2015         if(sb==NULL || sb->keys==NULL || sb->totkey==0) return 0;
2016         /* so we got keys, but no bodypoints... even without simul we need it for the bake */    
2017         if(sb->bpoint==NULL) sb->bpoint= MEM_callocN( sb->totpoint*sizeof(BodyPoint), "bodypoint");      
2018         
2019         /* convert cfra time to system time */
2020         sfra= (float)sb->sfra;
2021         cfra= bsystem_time(ob, NULL, framenr, 0.0);
2022         efra= (float)sb->efra;
2023         dfra= (float)sb->interval;
2024
2025         /* offset in keys array */
2026         ofs1= (int)floor( (cfra-sfra)/dfra );
2027
2028         if(ofs1 < 0) {
2029                 key0=key1=key2=key3= *sb->keys;
2030         }
2031         else if(ofs1 >= sb->totkey-1) {
2032                 key0=key1=key2=key3= *(sb->keys+sb->totkey-1);
2033         }
2034         else {
2035                 key1= *(sb->keys+ofs1);
2036                 key2= *(sb->keys+ofs1+1);
2037
2038                 if(ofs1>0) key0= *(sb->keys+ofs1-1);
2039                 else key0= key1;
2040                 
2041                 if(ofs1<sb->totkey-2) key3= *(sb->keys+ofs1+2);
2042                 else key3= key2;
2043         }
2044         
2045         sb->ctime= cfra;        /* needed? */
2046         
2047         /* timing */
2048         fac= ((cfra-sfra)/dfra) - (float)ofs1;
2049         CLAMP(fac, 0.0, 1.0);
2050         set_four_ipo(fac, data, KEY_BSPLINE);
2051         
2052         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++, key0++, key1++, key2++, key3++) {
2053                 bp->pos[0]= data[0]*key0->vec[0] +  data[1]*key1->vec[0] + data[2]*key2->vec[0] + data[3]*key3->vec[0];
2054                 bp->pos[1]= data[0]*key0->vec[1] +  data[1]*key1->vec[1] + data[2]*key2->vec[1] + data[3]*key3->vec[1];
2055                 bp->pos[2]= data[0]*key0->vec[2] +  data[1]*key1->vec[2] + data[2]*key2->vec[2] + data[3]*key3->vec[2];
2056         }
2057         
2058         softbody_to_object(ob, vertexCos, numVerts, sb->local);
2059         
2060         return 1;
2061 }
2062
2063 /* only gets called after succesfully doing softbody_step */
2064 /* already checked for OB_SB_BAKE flag */
2065 static void softbody_baked_add(Object *ob, float framenr)
2066 {
2067         SoftBody *sb= ob->soft;
2068         SBVertex *key;
2069         BodyPoint *bp;
2070         float sfra, efra, cfra, dfra, fac1;     /* start, end, current, delta */
2071         int ofs1, a;
2072         
2073         /* convert cfra time to system time */
2074         sfra= (float)sb->sfra;
2075         fac1= ob->sf; ob->sf= 0.0f;     /* disable startframe */
2076         cfra= bsystem_time(ob, NULL, framenr, 0.0);
2077         ob->sf= fac1;
2078         efra= (float)sb->efra;
2079         dfra= (float)sb->interval;
2080         
2081         if(sb->totkey==0) {
2082                 if(sb->sfra >= sb->efra) return;                /* safety, UI or py setting allows */
2083                 if(sb->interval<1) sb->interval= 1;             /* just be sure */
2084                 
2085                 sb->totkey= 1 + (int)(ceil( (efra-sfra)/dfra ) );
2086                 sb->keys= MEM_callocN( sizeof(void *)*sb->totkey, "sb keys");
2087         }
2088         
2089         /* inverse matrix might not be uptodate... */
2090         Mat4Invert(ob->imat, ob->obmat);
2091         
2092         /* now find out if we have to store a key */
2093         
2094         /* offset in keys array */
2095         if(cfra>=(efra)) {
2096                 ofs1= sb->totkey-1;
2097                 fac1= 0.0;
2098         }
2099         else {
2100                 ofs1= (int)floor( (cfra-sfra)/dfra );
2101                 fac1= ((cfra-sfra)/dfra) - (float)ofs1;
2102         }       
2103         if( fac1 < 1.0/dfra ) {
2104                 
2105                 key= *(sb->keys+ofs1);
2106                 if(key == NULL) {
2107                         *(sb->keys+ofs1)= key= MEM_mallocN(sb->totpoint*sizeof(SBVertex), "softbody key");
2108                         
2109                         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++, key++) {
2110                                 VECCOPY(key->vec, bp->pos);
2111                                 if(sb->local)
2112                                         Mat4MulVecfl(ob->imat, key->vec);
2113                         }
2114                 }
2115         }
2116 }
2117
2118 /* ************ Object level, exported functions *************** */
2119
2120 /* allocates and initializes general main data */
2121 SoftBody *sbNew(void)
2122 {
2123         SoftBody *sb;
2124         
2125         sb= MEM_callocN(sizeof(SoftBody), "softbody");
2126         
2127         sb->mediafrict= 0.5f; 
2128         sb->nodemass= 1.0f;
2129         sb->grav= 0.0f; 
2130         sb->physics_speed= 1.0f;
2131         sb->rklimit= 0.5f;
2132
2133         sb->goalspring= 0.5f; 
2134         sb->goalfrict= 0.0f; 
2135         sb->mingoal= 0.0f;  
2136         sb->maxgoal= 1.0f;
2137         sb->defgoal= 0.7f;
2138         
2139         sb->inspring= 0.5f;
2140         sb->infrict= 0.5f; 
2141         
2142         sb->interval= 10;
2143         sb->sfra= G.scene->r.sfra;
2144         sb->efra= G.scene->r.efra;
2145
2146         sb->colball  = 0.5f;
2147         sb->balldamp = 0.05f;
2148         sb->ballstiff= 1.0f;
2149         sb->sbc_mode = 1;
2150         
2151         return sb;
2152 }
2153
2154 /* frees all */
2155 void sbFree(SoftBody *sb)
2156 {
2157         free_softbody_intern(sb);
2158         MEM_freeN(sb);
2159 }
2160
2161
2162 /* makes totally fresh start situation */
2163 void sbObjectToSoftbody(Object *ob)
2164 {
2165         ob->softflag |= OB_SB_REDO;
2166
2167         free_softbody_intern(ob->soft);
2168 }
2169
2170 static int object_has_edges(Object *ob) 
2171 {
2172         if(ob->type==OB_MESH) {
2173                 return ((Mesh*) ob->data)->totedge;
2174         }
2175         else if(ob->type==OB_LATTICE) {
2176                 return 1;
2177         }
2178         else {
2179                 return 0;
2180         }
2181 }
2182
2183 /* simulates one step. framenr is in frames */
2184 void sbObjectStep(Object *ob, float framenr, float (*vertexCos)[3], int numVerts)
2185 {
2186         SoftBody *sb;
2187         BodyPoint *bp;
2188         int a;
2189         float dtime,ctime,forcetime,err;
2190
2191         /* baking works with global time */
2192         if(!(ob->softflag & OB_SB_BAKEDO) )
2193                 if(softbody_baked_step(ob, framenr, vertexCos, numVerts) ) return;
2194
2195         
2196         /* This part only sets goals and springs, based on original mesh/curve/lattice data.
2197            Copying coordinates happens in next chunk by setting softbody flag OB_SB_RESET */
2198         /* remake softbody if: */
2199         if(             (ob->softflag & OB_SB_REDO) ||          /* signal after weightpainting */
2200                         (ob->soft==NULL) ||                                     /* just to be nice we allow full init */
2201                         (ob->soft->bpoint==NULL) ||             /* after reading new file, or acceptable as signal to refresh */
2202                         (numVerts!=ob->soft->totpoint) ||       /* should never happen, just to be safe */
2203                         ((ob->softflag & OB_SB_EDGES) && !ob->soft->bspring && object_has_edges(ob))) /* happens when in UI edges was set */
2204         {
2205                 switch(ob->type) {
2206                 case OB_MESH:
2207                         mesh_to_softbody(ob);
2208                         break;
2209                 case OB_LATTICE:
2210                         lattice_to_softbody(ob);
2211                         break;
2212                 case OB_CURVE:
2213                 case OB_SURF:
2214                         curve_surf_to_softbody(ob);
2215                         break;
2216                 default:
2217                         renew_softbody(ob, numVerts, 0);
2218                         break;
2219                 }
2220                 
2221                         /* still need to update to correct vertex locations, happens on next step */
2222                 ob->softflag |= OB_SB_RESET; 
2223                 ob->softflag &= ~OB_SB_REDO;
2224         }
2225
2226         sb= ob->soft;
2227
2228         /* still no points? go away */
2229         if(sb->totpoint==0) return;
2230         
2231
2232         /* checking time: */
2233
2234         ctime= bsystem_time(ob, NULL, framenr, 0.0);
2235
2236         if (ob->softflag&OB_SB_RESET) {
2237                 dtime = 0.0;
2238         } else {
2239                 dtime= ctime - sb->ctime;
2240         }
2241
2242         /* the simulator */
2243
2244                 /* update the vertex locations */
2245         if (dtime!=0.0) {
2246                 for(a=0,bp=sb->bpoint; a<numVerts; a++, bp++) {
2247                         /* store where goals are now */ 
2248                         VECCOPY(bp->origS, bp->origE);
2249                         /* copy the position of the goals at desired end time */
2250                         VECCOPY(bp->origE, vertexCos[a]);
2251                         /* vertexCos came from local world, go global */
2252                         Mat4MulVecfl(ob->obmat, bp->origE); 
2253                          /* just to be save give bp->origT a defined value
2254                         will be calulated in interpolate_exciter()*/
2255                         VECCOPY(bp->origT, bp->origE); 
2256                 }
2257         }
2258     /* see if we need to interrupt integration stream */ 
2259         if((ob->softflag&OB_SB_RESET) ||        /* got a reset signal */
2260                 dtime<0.0 ||                                    /* back in time */
2261                 dtime>=9.9*G.scene->r.framelen) /* too far forward in time --> goals won't be accurate enough */
2262         {
2263                 for(a=0,bp=sb->bpoint; a<numVerts; a++, bp++) {
2264                         VECCOPY(bp->pos, vertexCos[a]);
2265                         Mat4MulVecfl(ob->obmat, bp->pos);  /* yep, sofbody is global coords*/
2266                         VECCOPY(bp->origS, bp->pos);
2267                         VECCOPY(bp->origE, bp->pos);
2268                         VECCOPY(bp->origT, bp->pos);
2269                         bp->vec[0]= bp->vec[1]= bp->vec[2]= 0.0f;
2270
2271                         /* the bp->prev*'s are for rolling back from a canceled try to propagate in time
2272                         adaptive step size algo in a nutshell:
2273             1.  set sheduled time step to new dtime
2274                         2.  try to advance the sheduled time step, beeing optimistic execute it
2275             3.  check for success
2276                         3.a we 're fine continue, may be we can increase sheduled time again ?? if so, do so! 
2277                         3.b we did exceed error limit --> roll back, shorten the sheduled time and try again at 2.
2278                         4.  check if we did reach dtime 
2279                         4.a nope we need to do some more at 2.
2280                         4.b yup we're done
2281                         */
2282
2283                         VECCOPY(bp->prevpos, bp->pos);
2284                         VECCOPY(bp->prevvec, bp->vec);
2285                         VECCOPY(bp->prevdx, bp->vec);
2286                         VECCOPY(bp->prevdv, bp->vec);
2287                 }
2288                 
2289                 ob->softflag &= ~OB_SB_RESET;
2290         }
2291         else if(dtime>0.0) {
2292
2293
2294                 /* reset deflector cache, sumohandle is free, but its still sorta abuse... (ton) */
2295                 free_sumo_handles();
2296                 ccd_build_deflector_cache(ob);
2297                 
2298                 if (TRUE) {     /*  */
2299                         /* special case of 2nd order Runge-Kutta type AKA Heun */
2300                         float forcetimemax = 0.25f;
2301                         float forcetimemin = 0.001f;
2302                         float timedone =0.0; /* how far did we get without violating error condition */
2303                                                                  /* loops = counter for emergency brake
2304                                                                  * we don't want to lock up the system if physics fail
2305                         */
2306
2307                         int loops =0 ; 
2308                         SoftHeunTol = sb->rklimit; /* humm .. this should be calculated from sb parameters and sizes */
2309                         
2310                         //forcetime = dtime; /* hope for integrating in one step */
2311                         forcetime =forcetimemax; /* hope for integrating in one step */
2312                         while ( (ABS(timedone) < ABS(dtime)) && (loops < 2000) )
2313                         {
2314                                 /* set goals in time */ 
2315                                 interpolate_exciter(ob,200,(int)(200.0*(timedone/dtime)));
2316                                 /* do predictive euler step */
2317                                 softbody_calc_forces(ob, forcetime);
2318                                 softbody_apply_forces(ob, forcetime, 1, NULL);
2319                                 /* crop new slope values to do averaged slope step */
2320                                 softbody_calc_forces(ob, forcetime);
2321                                 softbody_apply_forces(ob, forcetime, 2, &err);
2322                                 softbody_apply_goalsnap(ob);
2323                                 
2324                                 if (err > SoftHeunTol){ /* error needs to be scaled to some quantity */
2325                                         softbody_restore_prev_step(ob);
2326                                         forcetime /= 2.0;
2327                                 }
2328                                 else {
2329                                         float newtime = forcetime * 1.1f; /* hope for 1.1 times better conditions in next step */
2330                                         
2331                                         if (err > SoftHeunTol/2.0){ /* stay with this stepsize unless err really small */
2332                                                 newtime = forcetime;
2333                                         }
2334                                         timedone += forcetime;
2335                                         newtime=MIN2(forcetimemax,MAX2(forcetime,forcetimemin));
2336                                         if (forcetime > 0.0)
2337                                                 forcetime = MIN2(dtime - timedone,newtime);
2338                                         else 
2339                                                 forcetime = MAX2(dtime - timedone,newtime);
2340                                 }
2341                                 loops++;
2342                         }
2343                         /* move snapped to final position */
2344                         interpolate_exciter(ob, 2, 2);
2345                         softbody_apply_goalsnap(ob);
2346
2347                         if(G.f & G_DEBUG){
2348                                 if (loops > HEUNWARNLIMIT) /* monitor high loop counts say 1000 after testing */
2349                                         printf("%d heun integration loops/frame \n",loops);
2350                         }
2351                         
2352                 }
2353                 else{
2354                         /* do brute force explicit euler */
2355                         /* removed but left this branch for better integrators / solvers (BM) */
2356                         /* yah! Nicholas Guttenberg (NichG) here is the place to plug in */
2357                 }
2358                         /* reset deflector cache */
2359                 free_sumo_handles();
2360         }
2361
2362         softbody_to_object(ob, vertexCos, numVerts, 0);
2363         sb->ctime= ctime;
2364
2365         
2366         if(ob->softflag & OB_SB_BAKEDO) softbody_baked_add(ob, framenr);
2367 }
2368