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