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