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