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