softbodies
[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         float min,max;
684
685         if (sb==NULL) return; /* paranoya check */
686
687         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
688                 bp->colball=0;
689                 min = 1e22f;
690                 max = -1e22f;
691                 /* first estimation based on attached */
692                 for(b=bp->nofsprings;b>0;b--){
693                         bs = sb->bspring + bp->springs[b-1];
694                         bp->colball += bs->len;
695                         min = MIN2(bs->len,min);
696                         max = MAX2(bs->len,max);
697                 }
698
699                 if (bp->nofsprings != 0) {
700                         /*bp->colball /= bp->nofsprings;*/
701                         bp->colball = (min + max)/2.0f;
702                         if (sb->colball > 0.0f){
703                                 bp->colball=sb->colball;
704                         }               
705                         if (sb->colball == -1.0f){
706                                 bp->colball=max;
707                         }               
708                 }
709                 else bp->colball=0;
710                 printf("collision ballsize %f \n",bp->colball); 
711         }/*for bp*/             
712 }
713
714
715 /* creates new softbody if didn't exist yet, makes new points and springs arrays */
716 static void renew_softbody(Object *ob, int totpoint, int totspring)  
717 {
718         SoftBody *sb;
719         int i;
720         
721         if(ob->soft==NULL) ob->soft= sbNew();
722         else free_softbody_intern(ob->soft);
723         sb= ob->soft;
724            
725         if(totpoint) {
726                 sb->totpoint= totpoint;
727                 sb->totspring= totspring;
728                 
729                 sb->bpoint= MEM_mallocN( totpoint*sizeof(BodyPoint), "bodypoint");
730                 if(totspring) 
731                         sb->bspring= MEM_mallocN( totspring*sizeof(BodySpring), "bodyspring");
732
733                         /* initialise BodyPoint array */
734                 for (i=0; i<totpoint; i++) {
735                         BodyPoint *bp = &sb->bpoint[i];
736
737                         bp->weight= 1.0;
738                         if(ob->softflag & OB_SB_GOAL) {
739                                 bp->goal= ob->soft->defgoal;
740                         }
741                         else { 
742                                 bp->goal= 0.0f; 
743                                 /* so this will definily be below SOFTGOALSNAP */
744                         }
745                         
746                         bp->nofsprings= 0;
747                         bp->springs= NULL;
748                         bp->contactfrict = 0.0f;
749                 }
750         }
751 }
752
753 static void free_softbody_baked(SoftBody *sb)
754 {
755         SBVertex *key;
756         int k;
757
758         for(k=0; k<sb->totkey; k++) {
759                 key= *(sb->keys + k);
760                 if(key) MEM_freeN(key);
761         }
762         if(sb->keys) MEM_freeN(sb->keys);
763         
764         sb->keys= NULL;
765         sb->totkey= 0;
766         
767 }
768
769 /* only frees internal data */
770 static void free_softbody_intern(SoftBody *sb)
771 {
772         if(sb) {
773                 int a;
774                 BodyPoint *bp;
775                 
776                 if(sb->bpoint){
777                         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
778                                 /* free spring list */ 
779                                 if (bp->springs != NULL) {
780                                         MEM_freeN(bp->springs);
781                                 }
782                         }
783                         MEM_freeN(sb->bpoint);
784                 }
785                 
786                 if(sb->bspring) MEM_freeN(sb->bspring);
787                 
788                 sb->totpoint= sb->totspring= 0;
789                 sb->bpoint= NULL;
790                 sb->bspring= NULL;
791                 
792                 free_softbody_baked(sb);
793         }
794 }
795
796
797 /* ************ dynamics ********** */
798
799 /* the most general (micro physics correct) way to do collision 
800 ** (only needs the current particle position)  
801 **
802 ** it actually checks if the particle intrudes a short range force field generated 
803 ** by the faces of the target object and returns a force to drive the particel out
804 ** the strenght of the field grows exponetially if the particle is on the 'wrong' side of the face
805 ** 'wrong' side : projection to the face normal is negative (all referred to a vertex in the face)
806 **
807 ** flaw of this: 'fast' particles as well as 'fast' colliding faces 
808 ** give a 'tunnel' effect such that the particle passes through the force field 
809 ** without ever 'seeing' it 
810 ** this is fully compliant to heisenberg: h >= fuzzy(location) * fuzzy(time)
811 ** besides our h is way larger than in QM because forces propagate way slower here
812 ** we have to deal with fuzzy(time) in the range of 1/25 seconds (typical frame rate)
813 ** yup collision targets are not known here any better 
814 ** and 1/25 second is looong compared to real collision events
815 ** Q: why not use 'simple' collision here like bouncing back a particle 
816 **   --> reverting is velocity on the face normal
817 ** A: because our particles are not alone here 
818 **    and need to tell their neighbours exactly what happens via spring forces 
819 ** unless sbObjectStep( .. ) is called on sub frame timing level
820 ** BTW that also questions the use of a 'implicit' solvers on softbodies  
821 ** since that would only valid for 'slow' moving collision targets and dito particles
822 */
823
824 int sb_detect_collisionCached(float opco[3], float facenormal[3], float *damp,
825                                                 float force[3], unsigned int par_layer,struct Object *vertexowner)
826 {
827         Base *base;
828         Object *ob;
829         float nv1[3], nv2[3], nv3[3], nv4[3], edge1[3], edge2[3],d_nvect[3], dv1[3], dv2[3],
830                 facedist,n_mag,t,force_mag_norm,minx,miny,minz,maxx,maxy,maxz,
831                 innerfacethickness = -0.5f, outerfacethickness = 0.2f,
832                 ee = 5.0f, ff = 0.1f, fa;
833         int a, deflected=0;
834                 
835         base= G.scene->base.first;
836         while (base) {
837                 /*Only proceed for mesh object in same layer */
838                 if(base->object->type==OB_MESH && (base->lay & par_layer)) {
839                         ob= base->object;
840                         if((vertexowner) && (ob == vertexowner)){ 
841                                 /* if vertexowner is given  we don't want to check collision with owner object */ 
842                                 base = base->next;
843                                 continue;                               
844                         }
845
846                         /* only with deflecting set */
847                         if(ob->pd && ob->pd->deflect) {
848                                 MFace *mface= NULL;
849                                 MVert *mvert= NULL;
850                                 ccdf_minmax *mima= NULL;
851                                 
852                                 if(ob->sumohandle){
853                                         ccd_Mesh *ccdm=ob->sumohandle;
854                                         mface= ccdm->mface;
855                                         mvert= ccdm->mvert;
856                                         mima= ccdm->mima;
857                                         a = ccdm->totface;
858
859                                         minx =ccdm->bbmin[0]; 
860                                         miny =ccdm->bbmin[1]; 
861                                         minz =ccdm->bbmin[2];
862                                         
863                                         maxx =ccdm->bbmax[0]; 
864                                         maxy =ccdm->bbmax[1]; 
865                                         maxz =ccdm->bbmax[2]; 
866                                         
867                                         if ((opco[0] < minx) || 
868                                                 (opco[1] < miny) ||
869                                                 (opco[2] < minz) ||
870                                                 (opco[0] > maxx) || 
871                                                 (opco[1] > maxy) || 
872                                                 (opco[2] > maxz) ) {
873                                                 /* outside the padded boundbox --> collision object is too far away */ 
874                                                 base = base->next;
875                                                 continue;                               
876                                         }                                       
877                                 }
878                                 else{
879                                         /*aye that should be cached*/
880                                         printf("missing cache error \n");
881                                         base = base->next;
882                                         continue;                               
883                                 }
884                                 
885                                 /* do object level stuff */
886                                 /* need to have user control for that since it depends on model scale */
887                                 innerfacethickness =-ob->pd->pdef_sbift;
888                                 outerfacethickness =ob->pd->pdef_sboft;
889                                 fa = (ff*outerfacethickness-outerfacethickness);
890                                 fa *= fa;
891                                 fa = 1.0f/fa;
892                                 
893                                 /* use mesh*/
894                                 while (a--) {
895
896                                         if (
897                                                 (opco[0] < mima->minx) || 
898                                                 (opco[0] > mima->maxx) || 
899                                                 (opco[1] < mima->miny) ||
900                                                 (opco[1] > mima->maxy) || 
901                                                 (opco[2] < mima->minz) ||
902                                                 (opco[2] > mima->maxz) 
903                                                 ) {
904                                                 mface++;
905                                                 mima++;
906                                                 continue;
907                                         }
908                                         
909                                         if (mvert){
910                                                 
911                                                 VECCOPY(nv1,mvert[mface->v1].co);                                               
912                                                 VECCOPY(nv2,mvert[mface->v2].co);
913                                                 VECCOPY(nv3,mvert[mface->v3].co);
914                                                 if (mface->v4){
915                                                         VECCOPY(nv4,mvert[mface->v4].co);
916                                                 }
917                                         }
918                                         
919
920                                         
921                                         /* switch origin to be nv2*/
922                                         VECSUB(edge1, nv1, nv2);
923                                         VECSUB(edge2, nv3, nv2);
924                                         VECSUB(dv1,opco,nv2); /* abuse dv1 to have vertex in question at *origin* of triangle */
925                                         
926                                         Crossf(d_nvect, edge2, edge1);
927                                         n_mag = Normalise(d_nvect);
928                                         facedist = Inpf(dv1,d_nvect);
929                                         
930                                         if ((facedist > innerfacethickness) && (facedist < outerfacethickness)){                
931                                                 dv2[0] = opco[0] - 2.0f*facedist*d_nvect[0];
932                                                 dv2[1] = opco[1] - 2.0f*facedist*d_nvect[1];
933                                                 dv2[2] = opco[2] - 2.0f*facedist*d_nvect[2];
934                                                 if ( LineIntersectsTriangle( opco, dv2, nv1, nv2, nv3, &t)){
935                                                         force_mag_norm =(float)exp(-ee*facedist);
936                                                         if (facedist > outerfacethickness*ff)
937                                                                 force_mag_norm =(float)force_mag_norm*fa*(facedist - outerfacethickness)*(facedist - outerfacethickness);
938                                                         Vec3PlusStVec(force,force_mag_norm,d_nvect);
939                                                         *damp=ob->pd->pdef_sbdamp;
940                                                         deflected = 2;
941                                                 }
942                                         }               
943                                         if (mface->v4){ /* quad */
944                                                 /* switch origin to be nv4 */
945                                                 VECSUB(edge1, nv3, nv4);
946                                                 VECSUB(edge2, nv1, nv4);
947                                                 VECSUB(dv1,opco,nv4); /* abuse dv1 to have vertex in question at *origin* of triangle */
948                                                 
949                                                 Crossf(d_nvect, edge2, edge1);
950                                                 n_mag = Normalise(d_nvect);
951                                                 facedist = Inpf(dv1,d_nvect);
952                                                 
953                                                 if ((facedist > innerfacethickness) && (facedist < outerfacethickness)){
954                                                         dv2[0] = opco[0] - 2.0f*facedist*d_nvect[0];
955                                                         dv2[1] = opco[1] - 2.0f*facedist*d_nvect[1];
956                                                         dv2[2] = opco[2] - 2.0f*facedist*d_nvect[2];
957                                                         if (LineIntersectsTriangle( opco, dv2, nv1, nv3, nv4, &t)){
958                                                                 force_mag_norm =(float)exp(-ee*facedist);
959                                                                 if (facedist > outerfacethickness*ff)
960                                                                         force_mag_norm =(float)force_mag_norm*fa*(facedist - outerfacethickness)*(facedist - outerfacethickness);
961                                                                 Vec3PlusStVec(force,force_mag_norm,d_nvect);
962                                                                 *damp=ob->pd->pdef_sbdamp;
963                                                                 deflected = 2;
964                                                         }
965                                                         
966                                                 }
967                                         }
968                                         mface++;
969                                         mima++;                                 
970                 }/* while a */          
971                                 /* give it away */
972                 } /* if(ob->pd && ob->pd->deflect) */
973        }/* if (base->object->type==OB_MESH && (base->lay & par_layer)) { */
974            base = base->next;
975         } /* while (base) */
976         return deflected;       
977 }
978
979
980 /* aye this belongs to arith.c */
981 static void Vec3PlusStVec(float *v, float s, float *v1)
982 {
983         v[0] += s*v1[0];
984         v[1] += s*v1[1];
985         v[2] += s*v1[2];
986 }
987
988 static int sb_deflect_face(Object *ob,float *actpos, float *futurepos,float *collisionpos, float *facenormal,float *force,float *cf)
989 {
990         float s_actpos[3], s_futurepos[3];
991         int deflected;
992         
993         VECCOPY(s_actpos,actpos);
994         if(futurepos) VECCOPY(s_futurepos,futurepos);
995         deflected= sb_detect_collisionCached(s_actpos, facenormal, cf, force , ob->lay, ob);
996         return(deflected);
997 }
998
999
1000 static int is_there_deflection(unsigned int layer)
1001 {
1002         Base *base;
1003         
1004         for(base = G.scene->base.first; base; base= base->next) {
1005                 if( (base->lay & layer) && base->object->pd) {
1006                         if(base->object->pd->deflect) 
1007                                 return 1;
1008                 }
1009         }
1010         return 0;
1011 }
1012
1013 static void softbody_calc_forces(Object *ob, float forcetime)
1014 {
1015 /* rule we never alter free variables :bp->vec bp->pos in here ! 
1016  * this will ruin adaptive stepsize AKA heun! (BM) 
1017  */
1018         SoftBody *sb= ob->soft; /* is supposed to be there */
1019         BodyPoint  *bp;
1020         BodyPoint *bproot;
1021         BodySpring *bs; 
1022         ListBase *do_effector;
1023         float iks, ks, kd, gravity, actspringlen, forcefactor, sd[3];
1024         float fieldfactor = 1000.0f, windfactor  = 250.0f;   
1025         int a, b,  do_deflector;
1026         
1027         /* clear forces */
1028         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
1029                 bp->force[0]= bp->force[1]= bp->force[2]= 0.0;
1030         }
1031         
1032         gravity = sb->grav * sb_grav_force_scale(ob);   
1033         
1034         /* check! */
1035         do_deflector= is_there_deflection(ob->lay);
1036         do_effector= pdInitEffectors(ob,NULL);
1037         
1038         iks  = 1.0f/(1.0f-sb->inspring)-1.0f ;/* inner spring constants function */
1039         bproot= sb->bpoint; /* need this for proper spring addressing */
1040         
1041         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
1042                 if(bp->goal < SOFTGOALSNAP){ /* ommit this bp when it snaps */
1043                         float auxvect[3];  
1044                         float velgoal[3];
1045                         float absvel =0, projvel= 0;
1046                         
1047                         /* do goal stuff */
1048                         if(ob->softflag & OB_SB_GOAL) {
1049                                 /* true elastic goal */
1050                                 VecSubf(auxvect,bp->origT,bp->pos);
1051                                 ks  = 1.0f/(1.0f- bp->goal*sb->goalspring)-1.0f ;
1052                                 bp->force[0]= ks*(auxvect[0]);
1053                                 bp->force[1]= ks*(auxvect[1]);
1054                                 bp->force[2]= ks*(auxvect[2]);
1055                                 /* calulate damping forces generated by goals*/
1056                                 VecSubf(velgoal,bp->origS, bp->origE);
1057                                 kd =  sb->goalfrict * sb_fric_force_scale(ob) ;
1058                                 
1059                                 if (forcetime > 0.0 ) { /* make sure friction does not become rocket motor on time reversal */
1060                                         bp->force[0]-= kd * (velgoal[0] + bp->vec[0]);
1061                                         bp->force[1]-= kd * (velgoal[1] + bp->vec[1]);
1062                                         bp->force[2]-= kd * (velgoal[2] + bp->vec[2]);
1063                                 }
1064                                 else {
1065                                         bp->force[0]-= kd * (velgoal[0] - bp->vec[0]);
1066                                         bp->force[1]-= kd * (velgoal[1] - bp->vec[1]);
1067                                         bp->force[2]-= kd * (velgoal[2] - bp->vec[2]);
1068                                 }
1069                         }
1070                         /* done goal stuff */
1071                         
1072                         
1073                         /* gravitation */
1074                         bp->force[2]-= gravity*sb->nodemass; /* individual mass of node here */
1075                         
1076                         /* particle field & vortex */
1077                         if(do_effector) {
1078                                 float force[3]= {0.0f, 0.0f, 0.0f};
1079                                 float speed[3]= {0.0f, 0.0f, 0.0f};
1080                                 float eval_sb_fric_force_scale = sb_fric_force_scale(ob); /* just for calling function once */
1081                                 
1082                                 pdDoEffectors(do_effector, bp->pos, force, speed, (float)G.scene->r.cfra, 0.0f, PE_WIND_AS_SPEED);
1083                                 
1084                                 /* note: now we have wind as motion of media, so we can do anisotropic stuff here, */
1085                                 /* if we had vertex normals here(BM) */
1086                                 /* apply forcefield*/
1087                                 VecMulf(force,fieldfactor* eval_sb_fric_force_scale); 
1088                                 VECADD(bp->force, bp->force, force);
1089                                 
1090                                 /* friction in moving media */  
1091                                 kd= sb->mediafrict* eval_sb_fric_force_scale;  
1092                                 bp->force[0] -= kd * (bp->vec[0] + windfactor*speed[0]/eval_sb_fric_force_scale);
1093                                 bp->force[1] -= kd * (bp->vec[1] + windfactor*speed[1]/eval_sb_fric_force_scale);
1094                                 bp->force[2] -= kd * (bp->vec[2] + windfactor*speed[2]/eval_sb_fric_force_scale);
1095                                 /* now we'll have nice centrifugal effect for vortex */
1096                                 
1097                         }
1098                         else {
1099                                 /* friction in media (not) moving*/
1100                                 kd= sb->mediafrict* sb_fric_force_scale(ob);  
1101                                 /* assume it to be proportional to actual velocity */
1102                                 bp->force[0]-= bp->vec[0]*kd;
1103                                 bp->force[1]-= bp->vec[1]*kd;
1104                                 bp->force[2]-= bp->vec[2]*kd;
1105                                 /* friction in media done */
1106                         }
1107                         
1108                         /*other forces*/
1109                         /* this is the place where other forces can be added
1110                         yes, constraints and collision stuff should go here too (read baraff papers on that!)
1111                         */
1112                         /* moving collision targets */
1113                         if(do_deflector) {
1114                                 float defforce[3] = {0.0f,0.0f,0.0f}, collisionpos[3],facenormal[3], cf = 1.0f;
1115                                 kd = 1.0f;
1116                                 
1117                                 if (sb_deflect_face(ob,bp->pos, bp->pos, collisionpos, facenormal,defforce,&cf)){
1118                                         Vec3PlusStVec(bp->force,kd,defforce);
1119                                         bp->contactfrict = cf;
1120                                 }
1121                                 else{ 
1122                                         bp->contactfrict = 0.0f;
1123                                 }
1124                                 
1125                         } 
1126                         else
1127                         {
1128                                         bp->contactfrict = 0.0f;
1129                         }
1130                         /* naive ball self collision */
1131                         if((ob->softflag & OB_SB_EDGES) && (sb->bspring)
1132                                 && (ob->softflag & OB_SB_SELF)){
1133                                 int attached;
1134                                 BodyPoint   *obp;
1135                                 int c,b;
1136                                 float def[3];
1137                                 float tune2 = 0.5f;
1138                                 float tune = 1.0f;
1139                                 float distance;
1140                                 float compare;
1141
1142                                 for(c=sb->totpoint, obp= sb->bpoint; c>0; c--, obp++) {
1143
1144                                         if (c < a ) continue; /* exploit force(a,b) == force(b,a) part1/2 */
1145
1146                                         compare = (obp->colball + bp->colball) * tune2;         
1147                                         VecSubf(def, bp->pos, obp->pos);
1148                                         distance = Normalise(def);
1149                                         
1150                                         
1151                                         if (distance < compare ){
1152                                     /* exclude body points attached with a spring */
1153                                                 attached = 0;
1154                                                 for(b=obp->nofsprings;b>0;b--){
1155                                                         bs = sb->bspring + obp->springs[b-1];
1156                                                         if (( sb->totpoint-a == bs->v2)  || ( sb->totpoint-a == bs->v1)){
1157                                                                 attached=1;
1158                                                                 continue;}
1159                                                         
1160                                                 }
1161                                                 if (!attached){
1162                                                         /* would need another UI parameter defining fricton on self contact */
1163                                                         float ccfriction = 0.05;
1164                                                         float f = tune/(distance) + tune/(compare*compare)*distance - 2.0f*tune/compare ;
1165                                                         Vec3PlusStVec(bp->force,f,def);
1166                                                         if (bp->contactfrict == 0.0f) bp->contactfrict = ccfriction*compare/distance; 
1167                                                         /* exploit force(a,b) == force(b,a) part2/2 */
1168                                                         Vec3PlusStVec(obp->force,-f,def);
1169                                                         if (obp->contactfrict == 0.0f) obp->contactfrict = ccfriction*compare/distance;
1170                                                 }
1171                                                 
1172                                         }
1173                                 }
1174                         }
1175                         /* naive ball self collision done */
1176                         
1177                         /*other forces done*/
1178                         /* nice things could be done with anisotropic friction
1179                         like wind/air resistance in normal direction
1180                         --> having a piece of cloth sailing down 
1181                         but this needs to have a *valid* vertex normal
1182                         *valid* means to be calulated on time axis
1183                         hrms .. may be a rough one could be used as well .. let's see 
1184                         */
1185                         
1186                         if(ob->softflag & OB_SB_EDGES) {
1187                                 if (sb->bspring){ /* spring list exists at all ? */
1188                                         for(b=bp->nofsprings;b>0;b--){
1189                                                 bs = sb->bspring + bp->springs[b-1];
1190                                                 if (( (sb->totpoint-a) == bs->v1) ){ 
1191                                                         actspringlen= VecLenf( (bproot+bs->v2)->pos, bp->pos);
1192                                                         VecSubf(sd,(bproot+bs->v2)->pos, bp->pos);
1193                                                         Normalise(sd);
1194                                                         
1195                                                         /* friction stuff V1 */
1196                                                         VecSubf(velgoal,bp->vec,(bproot+bs->v2)->vec);
1197                                                         kd = sb->infrict * sb_fric_force_scale(ob);
1198                                                         absvel  = Normalise(velgoal);
1199                                                         projvel = ABS(Inpf(sd,velgoal));
1200                                                         kd *= absvel * projvel;
1201                                                         Vec3PlusStVec(bp->force,-kd,velgoal);
1202                                                         
1203                                                         if(bs->len > 0.0) /* check for degenerated springs */
1204                                                                 forcefactor = (bs->len - actspringlen)/bs->len * iks;
1205                                                         else
1206                                                                 forcefactor = actspringlen * iks;
1207                                                         forcefactor *= bs->strength; 
1208                                                         
1209                                                         Vec3PlusStVec(bp->force,-forcefactor,sd);
1210                                                         
1211                                                 }
1212                                                 
1213                                                 if (( (sb->totpoint-a) == bs->v2) ){ 
1214                                                         actspringlen= VecLenf( (bproot+bs->v1)->pos, bp->pos);
1215                                                         VecSubf(sd,bp->pos,(bproot+bs->v1)->pos);
1216                                                         Normalise(sd);
1217                                                         
1218                                                         /* friction stuff V2 */
1219                                                         VecSubf(velgoal,bp->vec,(bproot+bs->v1)->vec);
1220                                                         kd = sb->infrict * sb_fric_force_scale(ob);
1221                                                         absvel  = Normalise(velgoal);
1222                                                         projvel = ABS(Inpf(sd,velgoal));
1223                                                         kd *= absvel * projvel;
1224                                                         Vec3PlusStVec(bp->force,-kd,velgoal);
1225                                                         
1226                                                         if(bs->len > 0.0)
1227                                                                 forcefactor = (bs->len - actspringlen)/bs->len * iks;
1228                                                         else
1229                                                                 forcefactor = actspringlen * iks;
1230                                                         forcefactor *= bs->strength; 
1231                                                         Vec3PlusStVec(bp->force,+forcefactor,sd);                                                       
1232                                                 }
1233                                         }/* loop springs */
1234                                 }/* existing spring list */ 
1235                         }/*any edges*/
1236                 }/*omit on snap */
1237         }/*loop all bp's*/
1238         /* cleanup */
1239         if(do_effector)
1240                 pdEndEffectors(do_effector);
1241
1242 }
1243
1244
1245
1246 static void softbody_apply_forces(Object *ob, float forcetime, int mode, float *err)
1247 {
1248         /* time evolution */
1249         /* actually does an explicit euler step mode == 0 */
1250         /* or heun ~ 2nd order runge-kutta steps, mode 1,2 */
1251         SoftBody *sb= ob->soft; /* is supposed to be there */
1252         BodyPoint *bp;
1253         float dx[3],dv[3];
1254         float timeovermass;
1255         float maxerr = 0.0;
1256         int a;
1257
1258     forcetime *= sb_time_scale(ob);
1259     
1260         /* claim a minimum mass for vertex */
1261         if (sb->nodemass > 0.09999f) timeovermass = forcetime/sb->nodemass;
1262         else timeovermass = forcetime/0.09999f;
1263         
1264         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
1265                 if(bp->goal < SOFTGOALSNAP){
1266                         
1267                         /* so here is (v)' = a(cceleration) = sum(F_springs)/m + gravitation + some friction forces  + more forces*/
1268                         /* the ( ... )' operator denotes derivate respective time */
1269                         /* the euler step for velocity then becomes */
1270                         /* v(t + dt) = v(t) + a(t) * dt */ 
1271                         bp->force[0]*= timeovermass; /* individual mass of node here */ 
1272                         bp->force[1]*= timeovermass;
1273                         bp->force[2]*= timeovermass;
1274                         /* some nasty if's to have heun in here too */
1275                         VECCOPY(dv,bp->force); 
1276                         if (mode == 1){
1277                                 VECCOPY(bp->prevvec, bp->vec);
1278                                 VECCOPY(bp->prevdv, dv);
1279                         }
1280                         if (mode ==2){
1281                                 /* be optimistic and execute step */
1282                                 bp->vec[0] = bp->prevvec[0] + 0.5f * (dv[0] + bp->prevdv[0]);
1283                                 bp->vec[1] = bp->prevvec[1] + 0.5f * (dv[1] + bp->prevdv[1]);
1284                                 bp->vec[2] = bp->prevvec[2] + 0.5f * (dv[2] + bp->prevdv[2]);
1285                                 /* compare euler to heun to estimate error for step sizing */
1286                                 maxerr = MAX2(maxerr,ABS(dv[0] - bp->prevdv[0]));
1287                                 maxerr = MAX2(maxerr,ABS(dv[1] - bp->prevdv[1]));
1288                                 maxerr = MAX2(maxerr,ABS(dv[2] - bp->prevdv[2]));
1289                         }
1290                         else {VECADD(bp->vec, bp->vec, bp->force);}
1291
1292                         /* so here is (x)'= v(elocity) */
1293                         /* the euler step for location then becomes */
1294                         /* x(t + dt) = x(t) + v(t) * dt */ 
1295                         
1296                         VECCOPY(dx,bp->vec);
1297                         dx[0]*=forcetime ; 
1298                         dx[1]*=forcetime ; 
1299                         dx[2]*=forcetime ; 
1300                         
1301                         /* again some nasty if's to have heun in here too */
1302                         if (mode ==1){
1303                                 VECCOPY(bp->prevpos,bp->pos);
1304                                 VECCOPY(bp->prevdx ,dx);
1305                         }
1306                         
1307                         if (mode ==2){
1308                                 bp->pos[0] = bp->prevpos[0] + 0.5f * ( dx[0] + bp->prevdx[0]);
1309                                 bp->pos[1] = bp->prevpos[1] + 0.5f * ( dx[1] + bp->prevdx[1]);
1310                                 bp->pos[2] = bp->prevpos[2] + 0.5f* ( dx[2] + bp->prevdx[2]);
1311                                 maxerr = MAX2(maxerr,ABS(dx[0] - bp->prevdx[0]));
1312                                 maxerr = MAX2(maxerr,ABS(dx[1] - bp->prevdx[1]));
1313                                 maxerr = MAX2(maxerr,ABS(dx[2] - bp->prevdx[2]));
1314 /* kind of hack .. while inside collision target .. make movement more *viscous* */
1315                                 if (bp->contactfrict > 0.0f){
1316                                         bp->vec[0] *= (1.0f - bp->contactfrict);
1317                                         bp->vec[1] *= (1.0f - bp->contactfrict);
1318                                         bp->vec[2] *= (1.0f - bp->contactfrict);
1319                                 }
1320                         }
1321                         else { VECADD(bp->pos, bp->pos, dx);}
1322                 }/*snap*/
1323         } /*for*/
1324         
1325         if (err){ /* so step size will be controlled by biggest difference in slope */
1326                 *err = maxerr;
1327         }
1328 }
1329
1330 /* used by heun when it overshoots */
1331 static void softbody_restore_prev_step(Object *ob)
1332 {
1333         SoftBody *sb= ob->soft; /* is supposed to be there*/
1334         BodyPoint *bp;
1335         int a;
1336         
1337         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
1338                 VECCOPY(bp->vec, bp->prevvec);
1339                 VECCOPY(bp->pos, bp->prevpos);
1340         }
1341 }
1342
1343 /* care for bodypoints taken out of the 'ordinary' solver step
1344 ** because they are screwed to goal by bolts
1345 ** they just need to move along with the goal in time 
1346 ** we need to adjust them on sub frame timing in solver 
1347 ** so now when frame is done .. put 'em to the position at the end of frame
1348 */
1349 static void softbody_apply_goalsnap(Object *ob)
1350 {
1351         SoftBody *sb= ob->soft; /* is supposed to be there */
1352         BodyPoint *bp;
1353         int a;
1354         
1355         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
1356                 if (bp->goal >= SOFTGOALSNAP){
1357                         VECCOPY(bp->prevpos,bp->pos);
1358                         VECCOPY(bp->pos,bp->origT);
1359                 }               
1360         }
1361 }
1362
1363 /* expects full initialized softbody */
1364 static void interpolate_exciter(Object *ob, int timescale, int time)
1365 {
1366         SoftBody *sb= ob->soft;
1367         BodyPoint *bp;
1368         float f;
1369         int a;
1370         
1371         f = (float)time/(float)timescale;
1372         
1373         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {   
1374                 bp->origT[0] = bp->origS[0] + f*(bp->origE[0] - bp->origS[0]); 
1375                 bp->origT[1] = bp->origS[1] + f*(bp->origE[1] - bp->origS[1]); 
1376                 bp->origT[2] = bp->origS[2] + f*(bp->origE[2] - bp->origS[2]); 
1377                 if (bp->goal >= SOFTGOALSNAP){
1378                         bp->vec[0] = bp->origE[0] - bp->origS[0];
1379                         bp->vec[1] = bp->origE[1] - bp->origS[1];
1380                         bp->vec[2] = bp->origE[2] - bp->origS[2];
1381                 }
1382         }
1383         
1384 }
1385
1386
1387 /* ************ convertors ********** */
1388
1389 /*  for each object type we need;
1390     - xxxx_to_softbody(Object *ob)      : a full (new) copy, creates SB geometry
1391 */
1392
1393 static void get_scalar_from_vertexgroup(Object *ob, int vertID, short groupindex, float *target)
1394 /* result 0 on success, else indicates error number
1395 -- kind of *inverse* result defintion,
1396 -- but this way we can signal error condition to caller  
1397 -- and yes this function must not be here but in a *vertex group module*
1398 */
1399 {
1400         MDeformVert *dv= NULL;
1401         int i;
1402         
1403         /* spot the vert in deform vert list at mesh */
1404         if(ob->type==OB_MESH) {
1405                 Mesh *me= ob->data;
1406                 if (me->dvert)
1407                         dv = me->dvert + vertID;
1408         }
1409         else if(ob->type==OB_LATTICE) { /* not yet supported in softbody btw */
1410                 Lattice *lt= ob->data;
1411                 if (lt->dvert)
1412                         dv = lt->dvert + vertID;
1413         }
1414         if(dv) {
1415                 /* Lets see if this vert is in the weight group */
1416                 for (i=0; i<dv->totweight; i++){
1417                         if (dv->dw[i].def_nr == groupindex){
1418                                 *target= dv->dw[i].weight; /* got it ! */
1419                                 break;
1420                         }
1421                 }
1422         }
1423
1424
1425 /* Resetting a Mesh SB object's springs */  
1426 /* Spring lenght are caculted from'raw' mesh vertices that are NOT altered by modifier stack. */ 
1427 static void springs_from_mesh(Object *ob)
1428 {
1429         SoftBody *sb;
1430         Mesh *me= ob->data;
1431         BodyPoint *bp;
1432         int a;
1433         
1434         sb= ob->soft;   
1435         if (me && sb)
1436         { 
1437         /* using bp->origS as a container for spring calcualtions here
1438         ** will be overwritten sbObjectStep() to receive 
1439         ** actual modifier stack positions
1440         */
1441                 if(me->totvert) {    
1442                         bp= ob->soft->bpoint;
1443                         for(a=0; a<me->totvert; a++, bp++) {
1444                                 VECCOPY(bp->origS, me->mvert[a].co);                            
1445                                 Mat4MulVecfl(ob->obmat, bp->origS);
1446                         }
1447                         
1448                 }
1449                 /* recalculate spring length for meshes here */
1450                 for(a=0; a<sb->totspring; a++) {
1451                         BodySpring *bs = &sb->bspring[a];
1452                         bs->len= VecLenf(sb->bpoint[bs->v1].origS, sb->bpoint[bs->v2].origS);
1453                 }
1454         }
1455 }
1456
1457
1458 /* makes totally fresh start situation */
1459 static void mesh_to_softbody(Object *ob)
1460 {
1461         SoftBody *sb;
1462         Mesh *me= ob->data;
1463         MEdge *medge= me->medge;
1464         BodyPoint *bp;
1465         BodySpring *bs;
1466         float goalfac;
1467         int a, totedge;
1468         if (ob->softflag & OB_SB_EDGES) totedge= me->totedge;
1469         else totedge= 0;
1470         
1471         /* renew ends with ob->soft with points and edges, also checks & makes ob->soft */
1472         renew_softbody(ob, me->totvert, totedge);
1473                 
1474         /* we always make body points */
1475         sb= ob->soft;   
1476         bp= sb->bpoint;
1477         goalfac= ABS(sb->maxgoal - sb->mingoal);
1478         
1479         for(a=0; a<me->totvert; a++, bp++) {
1480                 /* get scalar values needed  *per vertex* from vertex group functions,
1481                 so we can *paint* them nicly .. 
1482                 they are normalized [0.0..1.0] so may be we need amplitude for scale
1483                 which can be done by caller but still .. i'd like it to go this way 
1484                 */ 
1485                 
1486                 if((ob->softflag & OB_SB_GOAL) && sb->vertgroup) {
1487                         get_scalar_from_vertexgroup(ob, a,(short) (sb->vertgroup-1), &bp->goal);
1488                         /* do this always, regardless successfull read from vertex group */
1489                         bp->goal= sb->mingoal + bp->goal*goalfac;
1490                 }
1491                 /* a little ad hoc changing the goal control to be less *sharp* */
1492                 bp->goal = (float)pow(bp->goal, 4.0f);
1493                         
1494                 /* to proove the concept
1495                 this would enable per vertex *mass painting*
1496                 strcpy(name,"SOFTMASS");
1497                 error = get_scalar_from_named_vertexgroup(ob,name, a,&temp);
1498                 if (!error) bp->mass = temp * ob->rangeofmass;
1499                 */
1500         }
1501
1502         /* but we only optionally add body edge springs */
1503         if (ob->softflag & OB_SB_EDGES) {
1504                 if(medge) {
1505                         bs= sb->bspring;
1506                         for(a=me->totedge; a>0; a--, medge++, bs++) {
1507                                 bs->v1= medge->v1;
1508                                 bs->v2= medge->v2;
1509                                 bs->strength= 1.0;
1510                         }
1511
1512                 
1513                         /* insert *diagonal* springs in quads if desired */
1514                         if (ob->softflag & OB_SB_QUADS) {
1515                                 add_mesh_quad_diag_springs(ob);
1516                         }
1517
1518                         build_bps_springlist(ob); /* scan for springs attached to bodypoints ONCE */
1519                         /* insert *other second order* springs if desired */
1520                         if (sb->secondspring > 0.0000001f) {
1521                                 add_2nd_order_springs(ob,sb->secondspring); /* exploits the the first run of build_bps_springlist(ob);*/
1522                                 build_bps_springlist(ob); /* yes we need to do it again*/
1523                         }
1524                         springs_from_mesh(ob); /* write the 'rest'-lenght of the springs */
1525             calculate_collision_balls(ob);
1526                 }
1527         }
1528         
1529 }
1530
1531
1532
1533 /*
1534 helper function to get proper spring length 
1535 when object is rescaled
1536 */
1537 float globallen(float *v1,float *v2,Object *ob)
1538 {
1539         float p1[3],p2[3];
1540         VECCOPY(p1,v1);
1541         Mat4MulVecfl(ob->obmat, p1);    
1542         VECCOPY(p2,v2);
1543         Mat4MulVecfl(ob->obmat, p2);
1544         return VecLenf(p1,p2);
1545 }
1546
1547 static void makelatticesprings(Lattice *lt,     BodySpring *bs, int dostiff,Object *ob)
1548 {
1549         BPoint *bp=lt->def, *bpu;
1550         int u, v, w, dv, dw, bpc=0, bpuc;
1551         
1552         dv= lt->pntsu;
1553         dw= dv*lt->pntsv;
1554         
1555         for(w=0; w<lt->pntsw; w++) {
1556                 
1557                 for(v=0; v<lt->pntsv; v++) {
1558                         
1559                         for(u=0, bpuc=0, bpu=NULL; u<lt->pntsu; u++, bp++, bpc++) {
1560                                 
1561                                 if(w) {
1562                                         bs->v1 = bpc;
1563                                         bs->v2 = bpc-dw;
1564                                     bs->strength= 1.0;
1565                                         bs->len= globallen((bp-dw)->vec, bp->vec,ob);
1566                                         bs++;
1567                                 }
1568                                 if(v) {
1569                                         bs->v1 = bpc;
1570                                         bs->v2 = bpc-dv;
1571                                     bs->strength= 1.0;
1572                                         bs->len= globallen((bp-dv)->vec, bp->vec,ob);
1573                                         bs++;
1574                                 }
1575                                 if(u) {
1576                                         bs->v1 = bpuc;
1577                                         bs->v2 = bpc;
1578                                     bs->strength= 1.0;
1579                                         bs->len= globallen((bpu)->vec, bp->vec,ob);
1580                                         bs++;
1581                                 }
1582                                 
1583                                 if (dostiff) {
1584
1585                                         if(w){
1586                                                 if( v && u ) {
1587                                                         bs->v1 = bpc;
1588                                                         bs->v2 = bpc-dw-dv-1;
1589                                                         bs->strength= 1.0;
1590                                                         bs->len= globallen((bp-dw-dv-1)->vec, bp->vec,ob);
1591                                                         bs++;
1592                                                 }                                               
1593                                                 if( (v < lt->pntsv-1) && (u) ) {
1594                                                         bs->v1 = bpc;
1595                                                         bs->v2 = bpc-dw+dv-1;
1596                                                         bs->strength= 1.0;
1597                                                         bs->len= globallen((bp-dw+dv-1)->vec, bp->vec,ob);
1598                                                         bs++;
1599                                                 }                                               
1600                                         }
1601
1602                                         if(w < lt->pntsw -1){
1603                                                 if( v && u ) {
1604                                                         bs->v1 = bpc;
1605                                                         bs->v2 = bpc+dw-dv-1;
1606                                                         bs->strength= 1.0;
1607                                                         bs->len= globallen((bp+dw-dv-1)->vec, bp->vec,ob);
1608                                                         bs++;
1609                                                 }                                               
1610                                                 if( (v < lt->pntsv-1) && (u) ) {
1611                                                         bs->v1 = bpc;
1612                                                         bs->v2 = bpc+dw+dv-1;
1613                                                         bs->strength= 1.0;
1614                                                          bs->len= globallen((bp+dw+dv-1)->vec, bp->vec,ob);
1615                                                         bs++;
1616                                                 }                                               
1617                                         }
1618                                 }
1619                                 bpu = bp;
1620                                 bpuc = bpc;
1621                         }
1622                 }
1623         }
1624 }
1625
1626
1627 /* makes totally fresh start situation */
1628 static void lattice_to_softbody(Object *ob)
1629 {
1630         Lattice *lt= ob->data;
1631         SoftBody *sb;
1632         int totvert, totspring = 0;
1633
1634         totvert= lt->pntsu*lt->pntsv*lt->pntsw;
1635
1636         if (ob->softflag & OB_SB_EDGES){
1637                 totspring = ((lt->pntsu -1) * lt->pntsv 
1638                           + (lt->pntsv -1) * lt->pntsu) * lt->pntsw
1639                                   +lt->pntsu*lt->pntsv*(lt->pntsw -1);
1640                 if (ob->softflag & OB_SB_QUADS){
1641                         totspring += 4*(lt->pntsu -1) *  (lt->pntsv -1)  * (lt->pntsw-1);
1642                 }
1643         }
1644         
1645
1646         /* renew ends with ob->soft with points and edges, also checks & makes ob->soft */
1647         renew_softbody(ob, totvert, totspring);
1648         sb= ob->soft;   /* can be created in renew_softbody() */
1649         
1650         /* weights from bpoints, same code used as for mesh vertices */
1651         if((ob->softflag & OB_SB_GOAL) && sb->vertgroup) {
1652                 BodyPoint *bp= sb->bpoint;
1653                 BPoint *bpnt= lt->def;
1654                 float goalfac= ABS(sb->maxgoal - sb->mingoal);
1655                 int a;
1656
1657                 for(a=0; a<totvert; a++, bp++, bpnt++) {
1658                         bp->goal= sb->mingoal + bpnt->weight*goalfac;
1659                         /* a little ad hoc changing the goal control to be less *sharp* */
1660                         bp->goal = (float)pow(bp->goal, 4.0f);
1661                 }
1662         }       
1663         
1664         /* create some helper edges to enable SB lattice to be usefull at all */
1665         if (ob->softflag & OB_SB_EDGES){
1666                 makelatticesprings(lt,ob->soft->bspring,ob->softflag & OB_SB_QUADS,ob);
1667                 build_bps_springlist(ob); /* link bps to springs */
1668         }
1669 }
1670
1671 /* makes totally fresh start situation */
1672 static void curve_surf_to_softbody(Object *ob)
1673 {
1674         Curve *cu= ob->data;
1675         SoftBody *sb;
1676         BodyPoint *bp;
1677         BodySpring *bs;
1678         Nurb *nu;
1679         BezTriple *bezt;
1680         BPoint *bpnt;
1681         float goalfac;
1682         int a, curindex=0;
1683         int totvert, totspring = 0, setgoal=0;
1684         
1685         totvert= count_curveverts(&cu->nurb);
1686         
1687         if (ob->softflag & OB_SB_EDGES){
1688                 if(ob->type==OB_CURVE) {
1689                         totspring= totvert - BLI_countlist(&cu->nurb);
1690                 }
1691         }
1692         
1693         /* renew ends with ob->soft with points and edges, also checks & makes ob->soft */
1694         renew_softbody(ob, totvert, totspring);
1695         sb= ob->soft;   /* can be created in renew_softbody() */
1696                 
1697         /* set vars now */
1698         goalfac= ABS(sb->maxgoal - sb->mingoal);
1699         bp= sb->bpoint;
1700         bs= sb->bspring;
1701         
1702         /* weights from bpoints, same code used as for mesh vertices */
1703         if((ob->softflag & OB_SB_GOAL) && sb->vertgroup)
1704                 setgoal= 1;
1705                 
1706         for(nu= cu->nurb.first; nu; nu= nu->next) {
1707                 if(nu->bezt) {
1708                         for(bezt=nu->bezt, a=0; a<nu->pntsu; a++, bezt++, bp+=3, curindex+=3) {
1709                                 if(setgoal) {
1710                                         bp->goal= sb->mingoal + bezt->weight*goalfac;
1711                                         /* a little ad hoc changing the goal control to be less *sharp* */
1712                                         bp->goal = (float)pow(bp->goal, 4.0f);
1713                                         
1714                                         /* all three triples */
1715                                         (bp+1)->goal= bp->goal;
1716                                         (bp+2)->goal= bp->goal;
1717                                 }
1718                                 
1719                                 if(totspring) {
1720                                         if(a>0) {
1721                                                 bs->v1= curindex-1;
1722                                                 bs->v2= curindex;
1723                                                 bs->strength= 1.0;
1724                                                 bs->len= globallen( (bezt-1)->vec[2], bezt->vec[0], ob );
1725                                                 bs++;
1726                                         }
1727                                         bs->v1= curindex;
1728                                         bs->v2= curindex+1;
1729                                         bs->strength= 1.0;
1730                                         bs->len= globallen( bezt->vec[0], bezt->vec[1], ob );
1731                                         bs++;
1732                                         
1733                                         bs->v1= curindex+1;
1734                                         bs->v2= curindex+2;
1735                                         bs->strength= 1.0;
1736                                         bs->len= globallen( bezt->vec[1], bezt->vec[2], ob );
1737                                         bs++;
1738                                 }
1739                         }
1740                 }
1741                 else {
1742                         for(bpnt=nu->bp, a=0; a<nu->pntsu*nu->pntsv; a++, bpnt++, bp++, curindex++) {
1743                                 if(setgoal) {
1744                                         bp->goal= sb->mingoal + bpnt->weight*goalfac;
1745                                         /* a little ad hoc changing the goal control to be less *sharp* */
1746                                         bp->goal = (float)pow(bp->goal, 4.0f);
1747                                 }
1748                                 if(totspring && a>0) {
1749                                         bs->v1= curindex-1;
1750                                         bs->v2= curindex;
1751                                         bs->strength= 1.0;
1752                                         bs->len= globallen( (bpnt-1)->vec, bpnt->vec , ob );
1753                                         bs++;
1754                                 }
1755                         }
1756                 }
1757         }
1758         
1759         if(totspring)
1760                 build_bps_springlist(ob); /* link bps to springs */
1761 }
1762
1763
1764 /* copies softbody result back in object */
1765 static void softbody_to_object(Object *ob, float (*vertexCos)[3], int numVerts, int local)
1766 {
1767         BodyPoint *bp= ob->soft->bpoint;
1768         int a;
1769
1770         /* inverse matrix is not uptodate... */
1771         Mat4Invert(ob->imat, ob->obmat);
1772         
1773         for(a=0; a<numVerts; a++, bp++) {
1774                 VECCOPY(vertexCos[a], bp->pos);
1775                 if(local==0) 
1776                         Mat4MulVecfl(ob->imat, vertexCos[a]);   /* softbody is in global coords, baked optionally not */
1777         }
1778 }
1779
1780 /* return 1 if succesfully baked and applied step */
1781 static int softbody_baked_step(Object *ob, float framenr, float (*vertexCos)[3], int numVerts)
1782 {
1783         SoftBody *sb= ob->soft;
1784         SBVertex *key0, *key1, *key2, *key3;
1785         BodyPoint *bp;
1786         float data[4], sfra, efra, cfra, dfra, fac;     /* start, end, current, delta */
1787         int ofs1, a;
1788
1789         /* precondition check */
1790         if(sb==NULL || sb->keys==NULL || sb->totkey==0) return 0;
1791         /* so we got keys, but no bodypoints... even without simul we need it for the bake */    
1792         if(sb->bpoint==NULL) sb->bpoint= MEM_callocN( sb->totpoint*sizeof(BodyPoint), "bodypoint");      
1793         
1794         /* convert cfra time to system time */
1795         sfra= (float)sb->sfra;
1796         cfra= bsystem_time(ob, NULL, framenr, 0.0);
1797         efra= (float)sb->efra;
1798         dfra= (float)sb->interval;
1799
1800         /* offset in keys array */
1801         ofs1= (int)floor( (cfra-sfra)/dfra );
1802
1803         if(ofs1 < 0) {
1804                 key0=key1=key2=key3= *sb->keys;
1805         }
1806         else if(ofs1 >= sb->totkey-1) {
1807                 key0=key1=key2=key3= *(sb->keys+sb->totkey-1);
1808         }
1809         else {
1810                 key1= *(sb->keys+ofs1);
1811                 key2= *(sb->keys+ofs1+1);
1812
1813                 if(ofs1>0) key0= *(sb->keys+ofs1-1);
1814                 else key0= key1;
1815                 
1816                 if(ofs1<sb->totkey-2) key3= *(sb->keys+ofs1+2);
1817                 else key3= key2;
1818         }
1819         
1820         sb->ctime= cfra;        /* needed? */
1821         
1822         /* timing */
1823         fac= ((cfra-sfra)/dfra) - (float)ofs1;
1824         CLAMP(fac, 0.0, 1.0);
1825         set_four_ipo(fac, data, KEY_BSPLINE);
1826         
1827         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++, key0++, key1++, key2++, key3++) {
1828                 bp->pos[0]= data[0]*key0->vec[0] +  data[1]*key1->vec[0] + data[2]*key2->vec[0] + data[3]*key3->vec[0];
1829                 bp->pos[1]= data[0]*key0->vec[1] +  data[1]*key1->vec[1] + data[2]*key2->vec[1] + data[3]*key3->vec[1];
1830                 bp->pos[2]= data[0]*key0->vec[2] +  data[1]*key1->vec[2] + data[2]*key2->vec[2] + data[3]*key3->vec[2];
1831         }
1832         
1833         softbody_to_object(ob, vertexCos, numVerts, sb->local);
1834         
1835         return 1;
1836 }
1837
1838 /* only gets called after succesfully doing softbody_step */
1839 /* already checked for OB_SB_BAKE flag */
1840 static void softbody_baked_add(Object *ob, float framenr)
1841 {
1842         SoftBody *sb= ob->soft;
1843         SBVertex *key;
1844         BodyPoint *bp;
1845         float sfra, efra, cfra, dfra, fac1;     /* start, end, current, delta */
1846         int ofs1, a;
1847         
1848         /* convert cfra time to system time */
1849         sfra= (float)sb->sfra;
1850         fac1= ob->sf; ob->sf= 0.0f;     /* disable startframe */
1851         cfra= bsystem_time(ob, NULL, framenr, 0.0);
1852         ob->sf= fac1;
1853         efra= (float)sb->efra;
1854         dfra= (float)sb->interval;
1855         
1856         if(sb->totkey==0) {
1857                 if(sb->sfra >= sb->efra) return;                /* safety, UI or py setting allows */
1858                 if(sb->interval<1) sb->interval= 1;             /* just be sure */
1859                 
1860                 sb->totkey= 1 + (int)(ceil( (efra-sfra)/dfra ) );
1861                 sb->keys= MEM_callocN( sizeof(void *)*sb->totkey, "sb keys");
1862         }
1863         
1864         /* inverse matrix might not be uptodate... */
1865         Mat4Invert(ob->imat, ob->obmat);
1866         
1867         /* now find out if we have to store a key */
1868         
1869         /* offset in keys array */
1870         if(cfra>=(efra)) {
1871                 ofs1= sb->totkey-1;
1872                 fac1= 0.0;
1873         }
1874         else {
1875                 ofs1= (int)floor( (cfra-sfra)/dfra );
1876                 fac1= ((cfra-sfra)/dfra) - (float)ofs1;
1877         }       
1878         if( fac1 < 1.0/dfra ) {
1879                 
1880                 key= *(sb->keys+ofs1);
1881                 if(key == NULL) {
1882                         *(sb->keys+ofs1)= key= MEM_mallocN(sb->totpoint*sizeof(SBVertex), "softbody key");
1883                         
1884                         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++, key++) {
1885                                 VECCOPY(key->vec, bp->pos);
1886                                 if(sb->local)
1887                                         Mat4MulVecfl(ob->imat, key->vec);
1888                         }
1889                 }
1890         }
1891 }
1892
1893 /* ************ Object level, exported functions *************** */
1894
1895 /* allocates and initializes general main data */
1896 SoftBody *sbNew(void)
1897 {
1898         SoftBody *sb;
1899         
1900         sb= MEM_callocN(sizeof(SoftBody), "softbody");
1901         
1902         sb->mediafrict= 0.5; 
1903         sb->nodemass= 1.0;
1904         sb->grav= 0.0; 
1905         sb->physics_speed= 1.0;
1906         sb->rklimit= 0.1f;
1907
1908         sb->goalspring= 0.5; 
1909         sb->goalfrict= 0.0; 
1910         sb->mingoal= 0.0;  
1911         sb->maxgoal= 1.0;
1912         sb->defgoal= 0.7f;
1913         
1914         sb->inspring= 0.5;
1915         sb->infrict= 0.5; 
1916         
1917         sb->interval= 10;
1918         sb->sfra= G.scene->r.sfra;
1919         sb->efra= G.scene->r.efra;
1920         
1921         return sb;
1922 }
1923
1924 /* frees all */
1925 void sbFree(SoftBody *sb)
1926 {
1927         free_softbody_intern(sb);
1928         MEM_freeN(sb);
1929 }
1930
1931
1932 /* makes totally fresh start situation */
1933 void sbObjectToSoftbody(Object *ob)
1934 {
1935         ob->softflag |= OB_SB_REDO;
1936
1937         free_softbody_intern(ob->soft);
1938 }
1939
1940 static int object_has_edges(Object *ob) 
1941 {
1942         if(ob->type==OB_MESH) {
1943                 return ((Mesh*) ob->data)->totedge;
1944         }
1945         else if(ob->type==OB_LATTICE) {
1946                 return 1;
1947         }
1948         else {
1949                 return 0;
1950         }
1951 }
1952
1953 /* simulates one step. framenr is in frames */
1954 void sbObjectStep(Object *ob, float framenr, float (*vertexCos)[3], int numVerts)
1955 {
1956         SoftBody *sb;
1957         BodyPoint *bp;
1958         int a;
1959         float dtime,ctime,forcetime,err;
1960
1961         /* baking works with global time */
1962         if(!(ob->softflag & OB_SB_BAKEDO) )
1963                 if(softbody_baked_step(ob, framenr, vertexCos, numVerts) ) return;
1964
1965         
1966         /* This part only sets goals and springs, based on original mesh/curve/lattice data.
1967            Copying coordinates happens in next chunk by setting softbody flag OB_SB_RESET */
1968         /* remake softbody if: */
1969         if(             (ob->softflag & OB_SB_REDO) ||          /* signal after weightpainting */
1970                         (ob->soft==NULL) ||                                     /* just to be nice we allow full init */
1971                         (ob->soft->bpoint==NULL) ||             /* after reading new file, or acceptable as signal to refresh */
1972                         (numVerts!=ob->soft->totpoint) ||       /* should never happen, just to be safe */
1973                         ((ob->softflag & OB_SB_EDGES) && !ob->soft->bspring && object_has_edges(ob))) /* happens when in UI edges was set */
1974         {
1975                 switch(ob->type) {
1976                 case OB_MESH:
1977                         mesh_to_softbody(ob);
1978                         break;
1979                 case OB_LATTICE:
1980                         lattice_to_softbody(ob);
1981                         break;
1982                 case OB_CURVE:
1983                 case OB_SURF:
1984                         curve_surf_to_softbody(ob);
1985                         break;
1986                 default:
1987                         renew_softbody(ob, numVerts, 0);
1988                         break;
1989                 }
1990                 
1991                         /* still need to update to correct vertex locations, happens on next step */
1992                 ob->softflag |= OB_SB_RESET; 
1993                 ob->softflag &= ~OB_SB_REDO;
1994         }
1995
1996         sb= ob->soft;
1997
1998         /* still no points? go away */
1999         if(sb->totpoint==0) return;
2000         
2001
2002         /* checking time: */
2003
2004         ctime= bsystem_time(ob, NULL, framenr, 0.0);
2005
2006         if (ob->softflag&OB_SB_RESET) {
2007                 dtime = 0.0;
2008         } else {
2009                 dtime= ctime - sb->ctime;
2010         }
2011
2012         /* the simulator */
2013
2014                 /* update the vertex locations */
2015         if (dtime!=0.0) {
2016                 for(a=0,bp=sb->bpoint; a<numVerts; a++, bp++) {
2017                         /* store where goals are now */ 
2018                         VECCOPY(bp->origS, bp->origE);
2019                         /* copy the position of the goals at desired end time */
2020                         VECCOPY(bp->origE, vertexCos[a]);
2021                         /* vertexCos came from local world, go global */
2022                         Mat4MulVecfl(ob->obmat, bp->origE); 
2023                          /* just to be save give bp->origT a defined value
2024                         will be calulated in interpolate_exciter()*/
2025                         VECCOPY(bp->origT, bp->origE); 
2026                 }
2027         }
2028     /* see if we need to interrupt integration stream */ 
2029         if((ob->softflag&OB_SB_RESET) ||        /* got a reset signal */
2030                 dtime<0.0 ||                                    /* back in time */
2031                 dtime>=9.9*G.scene->r.framelen) /* too far forward in time --> goals won't be accurate enough */
2032         {
2033                 for(a=0,bp=sb->bpoint; a<numVerts; a++, bp++) {
2034                         VECCOPY(bp->pos, vertexCos[a]);
2035                         Mat4MulVecfl(ob->obmat, bp->pos);  /* yep, sofbody is global coords*/
2036                         VECCOPY(bp->origS, bp->pos);
2037                         VECCOPY(bp->origE, bp->pos);
2038                         VECCOPY(bp->origT, bp->pos);
2039                         bp->vec[0]= bp->vec[1]= bp->vec[2]= 0.0f;
2040
2041                         /* the bp->prev*'s are for rolling back from a canceled try to propagate in time
2042                         adaptive step size algo in a nutshell:
2043             1.  set sheduled time step to new dtime
2044                         2.  try to advance the sheduled time step, beeing optimistic execute it
2045             3.  check for success
2046                         3.a we 're fine continue, may be we can increase sheduled time again ?? if so, do so! 
2047                         3.b we did exceed error limit --> roll back, shorten the sheduled time and try again at 2.
2048                         4.  check if we did reach dtime 
2049                         4.a nope we need to do some more at 2.
2050                         4.b yup we're done
2051                         */
2052
2053                         VECCOPY(bp->prevpos, bp->pos);
2054                         VECCOPY(bp->prevvec, bp->vec);
2055                         VECCOPY(bp->prevdx, bp->vec);
2056                         VECCOPY(bp->prevdv, bp->vec);
2057                 }
2058                 
2059                 ob->softflag &= ~OB_SB_RESET;
2060         }
2061         else if(dtime>0.0) {
2062
2063
2064                 /* reset deflector cache, sumohandle is free, but its still sorta abuse... (ton) */
2065                 free_sumo_handles();
2066                 ccd_build_deflector_cache(ob);
2067                 
2068                 if (TRUE) {     /*  */
2069                         /* special case of 2nd order Runge-Kutta type AKA Heun */
2070                         float timedone =0.0; /* how far did we get without violating error condition */
2071                                                                  /* loops = counter for emergency brake
2072                                                                  * we don't want to lock up the system if physics fail
2073                         */
2074                         int loops =0 ; 
2075                         SoftHeunTol = sb->rklimit; /* humm .. this should be calculated from sb parameters and sizes */
2076                         
2077                         forcetime = dtime; /* hope for integrating in one step */
2078                         while ( (ABS(timedone) < ABS(dtime)) && (loops < 2000) )
2079                         {
2080                                 /* set goals in time */ 
2081                                 interpolate_exciter(ob,200,(int)(200.0*(timedone/dtime)));
2082                                 /* do predictive euler step */
2083                                 softbody_calc_forces(ob, forcetime);
2084                                 softbody_apply_forces(ob, forcetime, 1, NULL);
2085                                 /* crop new slope values to do averaged slope step */
2086                                 softbody_calc_forces(ob, forcetime);
2087                                 softbody_apply_forces(ob, forcetime, 2, &err);
2088                                 softbody_apply_goalsnap(ob);
2089                                 
2090                                 if (err > SoftHeunTol){ /* error needs to be scaled to some quantity */
2091                                         softbody_restore_prev_step(ob);
2092                                         forcetime /= 2.0;
2093                                 }
2094                                 else {
2095                                         float newtime = forcetime * 1.1f; /* hope for 1.1 times better conditions in next step */
2096                                         
2097                                         if (err > SoftHeunTol/2.0){ /* stay with this stepsize unless err really small */
2098                                                 newtime = forcetime;
2099                                         }
2100                                         timedone += forcetime;
2101                                         if (forcetime > 0.0)
2102                                                 forcetime = MIN2(dtime - timedone,newtime);
2103                                         else 
2104                                                 forcetime = MAX2(dtime - timedone,newtime);
2105                                 }
2106                                 loops++;
2107                         }
2108                         /* move snapped to final position */
2109                         interpolate_exciter(ob, 2, 2);
2110                         softbody_apply_goalsnap(ob);
2111
2112                         if(G.f & G_DEBUG) {
2113                                 if (loops > HEUNWARNLIMIT) /* monitor high loop counts say 1000 after testing */
2114                                         printf("%d heun integration loops/frame \n",loops);
2115                         }
2116                         
2117                 }
2118                 else{
2119                         /* do brute force explicit euler */
2120                         /* removed but left this branch for better integrators / solvers (BM) */
2121                         /* yah! Nicholas Guttenberg (NichG) here is the place to plug in */
2122                 }
2123                         /* reset deflector cache */
2124                 free_sumo_handles();
2125         }
2126
2127         softbody_to_object(ob, vertexCos, numVerts, 0);
2128         sb->ctime= ctime;
2129
2130         
2131         if(ob->softflag & OB_SB_BAKEDO) softbody_baked_add(ob, framenr);
2132 }
2133