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