- Made SoftBody work with Particle Force Fields.
[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_mesh_types.h"
64 #include "DNA_meshdata_types.h"
65 #include "DNA_lattice_types.h"
66 #include "DNA_scene_types.h"
67
68 #include "BLI_blenlib.h"
69 #include "BLI_arithb.h"
70
71 #include "BKE_displist.h"
72 #include "BKE_effect.h"
73 #include "BKE_global.h"
74 #include "BKE_object.h"
75 #include "BKE_softbody.h"
76 #include "BKE_utildefines.h"
77
78 #include  "BIF_editdeform.h"
79
80 extern bDeformGroup *get_named_vertexgroup(Object *ob, char *name);
81 extern int  get_defgroup_num (Object *ob, bDeformGroup        *dg);
82
83
84 /* ********** soft body engine ******* */
85 #define SOFTGOALSNAP  0.999f 
86 // if bp-> goal is above make it a *forced follow original* and skip all ODE stuff for this bp
87 // removes *unnecessary* stiffnes from ODE system
88 #define HEUNWARNLIMIT 1 // 50 would be fine i think for detecting severe *stiff* stuff
89
90 float SoftHeunTol = 1.0f; // humm .. this should be calculated from sb parameters and sizes
91 float steptime =  1.0f/25.0f; // translate framerate to *real* time
92 float rescale_grav_to_framerate = 1.0f; // since unit of g is [m/sec^2] we need translation from frames to physics time
93 float rescale_friction_to_framerate = 1.0f; // since unit of drag is [kg/sec] we need translation from frames to physics time
94
95 /* local prototypes */
96 static void softbody_scale_time(float steptime);
97 static int get_scalar_from_named_vertexgroup(Object *ob, char *name, int vertID, float *target);
98 static void free_softbody_intern(SoftBody *sb);
99
100 static void softbody_scale_time(float steptime)
101 {
102   rescale_grav_to_framerate = steptime*steptime; 
103   rescale_friction_to_framerate = steptime;
104 }
105
106
107 static int count_mesh_quads(Mesh *me)
108 {
109         int a,result = 0;
110         MFace *mface= me->mface;
111         
112         if(mface) {
113                 for(a=me->totface; a>0; a--, mface++) {
114                         if(mface->v4) result++;
115                 }
116         }       
117         return result;
118 }
119
120 static void add_mesh_quad_diag_springs(Object *ob)
121 {
122         Mesh *me= ob->data;
123         MFace *mface= me->mface;
124         BodyPoint *bp;
125         BodySpring *bs, *bs_new;
126         int a ;
127         
128         if (ob->soft){
129                 int nofquads;
130                 
131                 nofquads = count_mesh_quads(me);
132                 if (nofquads) {
133                         /* resize spring-array to hold additional quad springs */
134                         bs_new= MEM_callocN( (ob->soft->totspring + nofquads *2 )*sizeof(BodySpring), "bodyspring");
135                         memcpy(bs_new,ob->soft->bspring,(ob->soft->totspring )*sizeof(BodySpring));
136                         
137                         if(ob->soft->bspring)
138                                 MEM_freeN(ob->soft->bspring); /* do this before reassigning the pointer  or have a 1st class memory leak */
139                         ob->soft->bspring = bs_new; 
140                         
141                         /* fill the tail */
142                         a = 0;
143                         bs = bs_new+ob->soft->totspring;
144                         bp= ob->soft->bpoint;
145                         if(mface ) {
146                                 for(a=me->totface; a>0; a--, mface++) {
147                                         if(mface->v4) {
148                                                 bs->v1= mface->v1;
149                                                 bs->v2= mface->v3;
150                                                 bs->strength= 1.0;
151                                                 bs->len= VecLenf( (bp+bs->v1)->origS, (bp+bs->v2)->origS);
152                                                 bs++;
153                                                 bs->v1= mface->v2;
154                                                 bs->v2= mface->v4;
155                                                 bs->strength= 1.0;
156                                                 bs->len= VecLenf( (bp+bs->v1)->origS, (bp+bs->v2)->origS);
157                                                 bs++;
158                                                 
159                                         }
160                                 }       
161                         }
162                         
163             /* now we can announce new springs */
164                         ob->soft->totspring += nofquads *2;
165                 }
166         }
167 }
168
169
170 static void add_bp_springlist(BodyPoint *bp,int springID)
171 {
172         int *newlist;
173         
174         if (bp->springs == NULL) {
175                 bp->springs = MEM_callocN( sizeof(int), "bpsprings");
176                 bp->springs[0] = springID;
177                 bp->nofsprings = 1;
178         }
179         else {
180                 bp->nofsprings++;
181                 newlist = MEM_callocN(bp->nofsprings * sizeof(int), "bpsprings");
182                 memcpy(newlist,bp->springs,(bp->nofsprings-1)* sizeof(int));
183                 MEM_freeN(bp->springs);
184                 bp->springs = newlist;
185                 bp->springs[bp->nofsprings-1] = springID;
186         }
187 }
188
189 /* do this once when sb is build
190 it is O(N^2) so scanning for springs every iteration is too expensive
191 */
192 static void build_bps_springlist(Object *ob)
193 {
194         SoftBody *sb= ob->soft; // is supposed to be there
195         BodyPoint *bp;  
196         BodySpring *bs; 
197         int a,b;
198         
199         if (sb==NULL) return; // paranoya check
200         
201         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
202                 /* scan for attached inner springs */   
203                 for(b=sb->totspring, bs= sb->bspring; b>0; b--, bs++) {
204                         if (( (sb->totpoint-a) == bs->v1) ){ 
205                                 add_bp_springlist(bp,sb->totspring -b);
206                         }
207                         if (( (sb->totpoint-a) == bs->v2) ){ 
208                                 add_bp_springlist(bp,sb->totspring -b);
209                         }
210                 }//for springs
211                 // if (bp->nofsprings) printf(" node %d has %d spring links\n",a,bp->nofsprings);
212         }//for bp               
213 }
214
215
216 /* creates new softbody if didn't exist yet, makes new points and springs arrays */
217 /* called in mesh_to_softbody */
218 static void renew_softbody(Object *ob, int totpoint, int totspring)  
219 {
220         SoftBody *sb;
221         
222         if(ob->soft==NULL) ob->soft= sbNew();
223         else free_softbody_intern(ob->soft);
224         sb= ob->soft;
225            
226         if(totpoint) {
227                 sb->totpoint= totpoint;
228                 sb->totspring= totspring;
229                 
230                 sb->bpoint= MEM_mallocN( totpoint*sizeof(BodyPoint), "bodypoint");
231                 if(totspring) 
232                         sb->bspring= MEM_mallocN( totspring*sizeof(BodySpring), "bodyspring");
233         }
234 }
235
236 /* only frees internal data */
237 static void free_softbody_intern(SoftBody *sb)
238 {
239         if(sb) {
240                 int a;
241                 BodyPoint *bp;
242                 
243                 if(sb->bpoint){
244                         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
245                                 /* free spring list */ 
246                                 if (bp->springs != NULL) {
247                                         MEM_freeN(bp->springs);
248                                 }
249                         }
250                         MEM_freeN(sb->bpoint);
251                 }
252                 
253                 if(sb->bspring) MEM_freeN(sb->bspring);
254                 
255                 sb->totpoint= sb->totspring= 0;
256                 sb->bpoint= NULL;
257                 sb->bspring= NULL;
258         }
259 }
260
261
262 /* ************ dynamics ********** */
263
264 /* aye this belongs to arith.c */
265 static void Vec3PlusStVec(float *v, float s, float *v1)
266 {
267         v[0] += s*v1[0];
268         v[1] += s*v1[1];
269         v[2] += s*v1[2];
270 }
271
272 #define USES_FIELD              1
273 #define USES_DEFLECT    2
274 static int is_there_deflection(unsigned int layer)
275 {
276         Base *base;
277         int retval= 0;
278         
279         for(base = G.scene->base.first; base; base= base->next) {
280                 if( (base->lay & layer) && base->object->pd) {
281                         if(base->object->pd->forcefield) retval |= USES_FIELD;
282                         if(base->object->pd->deflect) retval |= USES_DEFLECT;
283                 }
284         }
285         return retval;
286 }
287
288
289 static void softbody_calc_forces(Object *ob, float dtime)
290 {
291         SoftBody *sb= ob->soft; // is supposed to be there
292         BodyPoint  *bp;
293         BodyPoint *bproot;
294         BodySpring *bs; 
295         float iks, ks, kd, gravity, actspringlen, forcefactor, sd[3];
296         int a, b, do_effector;
297         
298         /* clear forces */
299         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
300                 bp->force[0]= bp->force[1]= bp->force[2]= 0.0;
301         }
302         
303         /* check! */
304         do_effector= is_there_deflection(ob->lay);
305
306         gravity = sb->nodemass * sb->grav * rescale_grav_to_framerate;  
307         iks  = 1.0f/(1.0f-sb->inspring)-1.0f ;/* inner spring constants function */
308         bproot= sb->bpoint; /* need this for proper spring addressing */
309         
310         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
311                 if(bp->goal < SOFTGOALSNAP){ // ommit this bp when i snaps
312                         float auxvect[3]; // aux unit vector  
313                         float velgoal[3];
314                         float absvel =0, projvel= 0;
315                         
316                         /* do goal stuff */
317                         if(ob->softflag & OB_SB_GOAL) {
318                                 /* true elastic goal */
319                                 VecSubf(auxvect,bp->origT,bp->pos);
320                                 ks  = 1.0f/(1.0f- bp->goal*sb->goalspring)-1.0f ;
321                                 bp->force[0]= ks*(auxvect[0]);
322                                 bp->force[1]= ks*(auxvect[1]);
323                                 bp->force[2]= ks*(auxvect[2]);
324                                 /* calulate damping forces generated by goals*/
325                                 VecSubf(velgoal,bp->origS, bp->origE);
326                                 kd =  sb->goalfrict * rescale_friction_to_framerate ;
327
328                                 if (dtime > 0.0 ) { // make sure friction does not become rocket motor on time reversal
329                                         bp->force[0]-= kd * (velgoal[0] + bp->vec[0]);
330                                         bp->force[1]-= kd * (velgoal[1] + bp->vec[1]);
331                                         bp->force[2]-= kd * (velgoal[2] + bp->vec[2]);
332                                 }
333                                 else {
334                                         bp->force[0]-= kd * (velgoal[0] - bp->vec[0]);
335                                         bp->force[1]-= kd * (velgoal[1] - bp->vec[1]);
336                                         bp->force[2]-= kd * (velgoal[2] - bp->vec[2]);
337                                 }
338                         }
339                         /* done goal stuff */
340                         
341                         
342                         /* gravitation */
343                         bp->force[2]-= gravity*sb->nodemass; /* individual mass of node here */
344                         
345                         /* friction in media */
346                         kd= sb->mediafrict* rescale_friction_to_framerate;  
347                         /* assume it to be proportional to actual velocity */
348                         bp->force[0]-= bp->vec[0]*kd;
349                         bp->force[1]-= bp->vec[1]*kd;
350                         bp->force[2]-= bp->vec[2]*kd;
351                         /* friction in media done */
352
353                         /*other forces*/
354                         /* this is the place where other forces can be added
355                         yes, constraints and collision stuff should go here too (read baraff papers on that!)
356                         */
357                         
358                         /* particle field & deflectors */
359                         if(do_effector & USES_FIELD) {
360                                 float force[3]= {0.0f, 0.0f, 0.0f};
361                                 float speed[3]= {0.0f, 0.0f, 0.0f};
362                                 
363                                 pdDoEffector(bp->pos, force, speed, (float)G.scene->r.cfra, ob->lay);
364                                 /* apply force */
365                                 //  VecMulf(force, rescale_grav_to_framerate);  <- didn't work, made value far too low!
366                                 VECADD(bp->force, bp->force, force);
367                                 /* apply speed. note; deflector can give 'speed' only.... */
368                                 VecMulf(speed, rescale_grav_to_framerate);      
369                                 VECADD(bp->vec, bp->vec, speed);
370                         }
371 #if 0                   
372                         /* copied from particles... doesn't work really! */
373                         if(do_effector & USES_DEFLECT) {
374                                 int deflected;
375                                 int last_ob = -1;
376                                 int last_fc = -1;
377                                 int same_fc = 0;
378                                 float last[3], lastv[3];
379                                 int finish_defs = 1;
380                                 int def_count = 0;
381                                 
382                                 VecSubf(last, bp->pos, bp->vec);
383                                 VECCOPY(lastv, bp->vec);
384
385                                 while (finish_defs) {
386                                         deflected= pdDoDeflection(last, bp->pos, lastv,
387                                                                                           bp->vec, dtime, bp->force, 0,
388                                                                                           G.scene->r.cfra, ob->lay, &last_ob, &last_fc, &same_fc);
389                                         if (deflected) {
390                                                 def_count = def_count + 1;
391                                                 //deflection = 1;
392                                                 if (def_count==10) finish_defs = 0;
393                                         }
394                                         else {
395                                                 finish_defs = 0;
396                                         }
397                                 }
398                         }
399 #endif                  
400                         /*other forces done*/
401
402                         /* nice things could be done with anisotropic friction
403                         like wind/air resistance in normal direction
404                         --> having a piece of cloth sailing down 
405                         but this needs to have a *valid* vertex normal
406                         *valid* means to be calulated on time axis
407                         hrms .. may be a rough one could be used as well .. let's see 
408                         */
409
410                         if(ob->softflag & OB_SB_EDGES) {
411                                 if (1){ /* big mesh optimization */
412                                         /* run over attached inner spring list */       
413                                         if (sb->bspring){ // spring list exists at all ? 
414                                                 for(b=bp->nofsprings;b>0;b--){
415                                                         bs = sb->bspring + bp->springs[b-1];
416                                                         if (( (sb->totpoint-a) == bs->v1) ){ 
417                                                                 actspringlen= VecLenf( (bproot+bs->v2)->pos, bp->pos);
418                                                                 VecSubf(sd,(bproot+bs->v2)->pos, bp->pos);
419                                                                 Normalise(sd);
420
421                                                                 // friction stuff V1
422                                                                 VecSubf(velgoal,bp->vec,(bproot+bs->v2)->vec);
423                                                                 kd = sb->infrict * rescale_friction_to_framerate ;
424                                                                 absvel  = Normalise(velgoal);
425                                                                 projvel = ABS(Inpf(sd,velgoal));
426                                                                 kd *= absvel * projvel;
427                                                                 Vec3PlusStVec(bp->force,-kd,velgoal);
428                                                                 
429                                                                 if(bs->len > 0.0) /* check for degenerated springs */
430                                                                         forcefactor = (bs->len - actspringlen)/bs->len * iks;
431                                                                 else
432                                                                         forcefactor = actspringlen * iks;
433                                                                 
434                                                                 Vec3PlusStVec(bp->force,-forcefactor,sd);
435
436                                                         }
437                                                         
438                                                         if (( (sb->totpoint-a) == bs->v2) ){ 
439                                                                 actspringlen= VecLenf( (bproot+bs->v1)->pos, bp->pos);
440                                                                 VecSubf(sd,bp->pos,(bproot+bs->v1)->pos);
441                                                                 Normalise(sd);
442
443                                                                 // friction stuff V2
444                                                                 VecSubf(velgoal,bp->vec,(bproot+bs->v1)->vec);
445                                                                 kd = sb->infrict * rescale_friction_to_framerate ;
446                                                                 absvel  = Normalise(velgoal);
447                                                                 projvel = ABS(Inpf(sd,velgoal));
448                                                                 kd *= absvel * projvel;
449                                                                 Vec3PlusStVec(bp->force,-kd,velgoal);
450                                                                 
451                                                                 if(bs->len > 0.0)
452                                                                         forcefactor = (bs->len - actspringlen)/bs->len * iks;
453                                                                 else
454                                                                         forcefactor = actspringlen * iks;
455                                                                 Vec3PlusStVec(bp->force,+forcefactor,sd);                                                       
456                                                         }
457                                                 }
458                                         } //if spring list exists at all ?
459                                 }
460                                 else{ // this branch is not completly uptaded for friction stuff 
461                                         /* scan for attached inner springs makes it a O(N^2) thing = bad !*/    
462                                         /* obsolete .. but if someone wants to try the effect :) */
463                                         for(b=sb->totspring, bs= sb->bspring; b>0; b--, bs++) {
464                                                 if (( (sb->totpoint-a) == bs->v1) ){ 
465                                                         actspringlen= VecLenf( (bproot+bs->v2)->pos, bp->pos);
466                                                         VecSubf(sd,(bproot+bs->v2)->pos, bp->pos);
467                                                         Normalise(sd);
468
469
470                                                         if(bs->len > 0.0) /* check for degenerated springs */
471                                                                 forcefactor = (bs->len - actspringlen)/bs->len * iks;
472                                                         else
473                                                                 forcefactor = actspringlen * iks;
474                                                         Vec3PlusStVec(bp->force,-forcefactor,sd);
475                                                 }
476                                                 
477                                                 if (( (sb->totpoint-a) == bs->v2) ){ 
478                                                         actspringlen= VecLenf( (bproot+bs->v1)->pos, bp->pos);
479                                                         VecSubf(sd,bp->pos,(bproot+bs->v1)->pos);
480                                                         Normalise(sd);
481                                                         
482                                                         if(bs->len > 0.0)
483                                                                 forcefactor = (bs->len - actspringlen)/bs->len * iks;
484                                                         else
485                                                                 forcefactor = actspringlen * iks;
486                                                         Vec3PlusStVec(bp->force,+forcefactor,sd);                                               
487                                                 }
488                                         }// no snap
489                                 }//for
490                         }// if use edges
491                 }       
492         }
493 }
494
495 static void softbody_apply_forces(Object *ob, float dtime, int mode, float *err)
496 {
497         /* time evolution */
498         /* actually does an explicit euler step mode == 0 */
499         /* or heun ~ 2nd order runge-kutta steps, mode 1,2 */
500         SoftBody *sb= ob->soft; // is supposed to be there
501         BodyPoint *bp;
502         float dx[3],dv[3];
503         float timeovermass;
504         float maxerr = 0.0;
505         int a;
506         
507         if (sb->nodemass > 0.09999f) timeovermass = dtime/sb->nodemass;
508         else timeovermass = dtime/0.09999f;
509         
510         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
511                 if(bp->goal < SOFTGOALSNAP){
512                         
513                         /* so here is dv/dt = a = sum(F_springs)/m + gravitation + some friction forces */
514                         /* the euler step for velocity then becomes */
515                         /* v(t + dt) = v(t) + a(t) * dt */ 
516                         bp->force[0]*= timeovermass; /* individual mass of node here */ 
517                         bp->force[1]*= timeovermass;
518                         bp->force[2]*= timeovermass;
519                         /* some nasty if's to have heun in here too */
520                         VECCOPY(dv,bp->force); 
521                         if (mode == 1){
522                                 VECCOPY(bp->prevvec, bp->vec);
523                                 VECCOPY(bp->prevdv, dv);
524                         }
525                         if (mode ==2){
526                                 /* be optimistic and execute step */
527                                 bp->vec[0] = bp->prevvec[0] + 0.5f * (dv[0] + bp->prevdv[0]);
528                                 bp->vec[1] = bp->prevvec[1] + 0.5f * (dv[1] + bp->prevdv[1]);
529                                 bp->vec[2] = bp->prevvec[2] + 0.5f * (dv[2] + bp->prevdv[2]);
530                                 /* compare euler to heun to estimate error for step sizing */
531                                 maxerr = MAX2(maxerr,ABS(dv[0] - bp->prevdv[0]));
532                                 maxerr = MAX2(maxerr,ABS(dv[1] - bp->prevdv[1]));
533                                 maxerr = MAX2(maxerr,ABS(dv[2] - bp->prevdv[2]));
534                         }
535                         else {VECADD(bp->vec, bp->vec, bp->force);}
536
537                         /* so here is dx/dt = v */
538                         /* the euler step for location then becomes */
539                         /* x(t + dt) = x(t) + v(t) * dt */ 
540                         
541                         VECCOPY(dx,bp->vec);
542                         dx[0]*=dtime ; 
543                         dx[1]*=dtime ; 
544                         dx[2]*=dtime ; 
545                         
546                         /* again some nasty if's to have heun in here too */
547                         if (mode ==1){
548                                 VECCOPY(bp->prevpos,bp->pos);
549                                 VECCOPY(bp->prevdx ,dx);
550                         }
551                         
552                         if (mode ==2){
553                                 bp->pos[0] = bp->prevpos[0] + 0.5f * ( dx[0] + bp->prevdx[0]);
554                                 bp->pos[1] = bp->prevpos[1] + 0.5f * ( dx[1] + bp->prevdx[1]);
555                                 bp->pos[2] = bp->prevpos[2] + 0.5f* ( dx[2] + bp->prevdx[2]);
556                                 maxerr = MAX2(maxerr,ABS(dx[0] - bp->prevdx[0]));
557                                 maxerr = MAX2(maxerr,ABS(dx[1] - bp->prevdx[1]));
558                                 maxerr = MAX2(maxerr,ABS(dx[2] - bp->prevdx[2]));
559                         }
560                         else { VECADD(bp->pos, bp->pos, dx);}
561                         
562                 }//snap
563         } //for
564         if (err){ /* so step size will be controlled by biggest difference in slope */
565                 *err = maxerr;
566         }
567 }
568
569 /* used by heun when it overshoots */
570 static void softbody_restore_prev_step(Object *ob)
571 {
572         SoftBody *sb= ob->soft; // is supposed to be there
573         BodyPoint *bp;
574         int a;
575         
576         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
577                 VECCOPY(bp->vec, bp->prevvec);
578                 VECCOPY(bp->pos, bp->prevpos);
579         }
580 }
581
582
583 static void softbody_apply_goalsnap(Object *ob)
584 {
585         SoftBody *sb= ob->soft; // is supposed to be there
586         BodyPoint *bp;
587         int a;
588         
589         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
590                 if (bp->goal >= SOFTGOALSNAP){
591                         VECCOPY(bp->prevpos,bp->pos);
592                         VECCOPY(bp->pos,bp->origT);
593                 }               
594         }
595 }
596
597 /* unused */
598 #if 0
599 static void softbody_force_goal(Object *ob)
600 {
601         SoftBody *sb= ob->soft; // is supposed to be there
602         BodyPoint *bp;
603         int a;
604         
605         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {           
606                 VECCOPY(bp->pos,bp->origT);
607                 bp->vec[0] = bp->origE[0] - bp->origS[0];
608                 bp->vec[1] = bp->origE[1] - bp->origS[1];
609                 bp->vec[2] = bp->origE[2] - bp->origS[2];               
610         }
611 }
612 #endif
613
614 /* expects full initialized softbody */
615 static void interpolate_exciter(Object *ob, int timescale, int time)
616 {
617         SoftBody *sb= ob->soft;
618         BodyPoint *bp;
619         float f;
620         int a;
621
622         // note: i removed Mesh usage here, softbody should remain generic! (ton)
623         
624         f = (float)time/(float)timescale;
625         
626         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {   
627                 bp->origT[0] = bp->origS[0] + f*(bp->origE[0] - bp->origS[0]); 
628                 bp->origT[1] = bp->origS[1] + f*(bp->origE[1] - bp->origS[1]); 
629                 bp->origT[2] = bp->origS[2] + f*(bp->origE[2] - bp->origS[2]); 
630                 if (bp->goal >= SOFTGOALSNAP){
631                         bp->vec[0] = bp->origE[0] - bp->origS[0];
632                         bp->vec[1] = bp->origE[1] - bp->origS[1];
633                         bp->vec[2] = bp->origE[2] - bp->origS[2];
634                 }
635         }
636         
637         if(ob->softflag & OB_SB_EDGES) {
638                 /* hrms .. do springs alter their lenght ?
639                 bs= ob->soft->bspring;
640                 bp= ob->soft->bpoint;
641                 for(a=0; (a<me->totedge && a < ob->soft->totspring ); a++, bs++) {
642                         bs->len= VecLenf( (bp+bs->v1)->origT, (bp+bs->v2)->origT);
643                 }
644                 */
645         }
646 }
647
648
649 /* ************ convertors ********** */
650
651 /*  for each object type we need;
652     - xxxx_to_softbody(Object *ob)      : a full (new) copy
653         - xxxx_update_softbody(Object *ob)  : update refreshes current positions
654     - softbody_to_xxxx(Object *ob)      : after simulation, copy vertex locations back
655 */
656
657 /* helper  call */
658 static int object_has_edges(Object *ob) 
659 {
660         if(ob->type==OB_MESH) {
661                 Mesh *me= ob->data;
662                 if(me->medge) return 1;
663         }
664         else if(ob->type==OB_LATTICE) {
665                 ;
666         }
667         
668         return 0;
669 }
670
671 /* helper  call */
672 static void set_body_point(Object *ob, BodyPoint *bp, float *vec)
673 {
674         
675         VECCOPY(bp->pos, vec);
676         Mat4MulVecfl(ob->obmat, bp->pos);  // yep, sofbody is global coords
677         VECCOPY(bp->origS, bp->pos);
678         VECCOPY(bp->origE, bp->pos);
679         VECCOPY(bp->origT, bp->pos);
680         
681         bp->vec[0]= bp->vec[1]= bp->vec[2]= 0.0;
682         bp->weight= 1.0;
683         bp->goal= ob->soft->defgoal;
684         
685         bp->nofsprings= 0;
686         bp->springs= NULL;
687 }
688
689
690 /* copy original (new) situation in softbody, as result of matrices or deform */
691 /* is assumed to enter function with ob->soft, but can be without points */
692 static void mesh_update_softbody(Object *ob)
693 {
694         Mesh *me= ob->data;
695         MVert *mvert= me->mvert;
696 /*      MEdge *medge= me->medge;  */ /*unused*/
697         BodyPoint *bp;
698         int a;
699         
700         /* possible after a file read... */
701         if(ob->soft->totpoint!=me->totvert) sbObjectToSoftbody(ob);
702         
703         if(me->totvert) {
704         
705                 bp= ob->soft->bpoint;
706                 for(a=0; a<me->totvert; a++, mvert++, bp++) {
707                         VECCOPY(bp->origS, bp->origE);
708                         VECCOPY(bp->origE, mvert->co);
709                         Mat4MulVecfl(ob->obmat, bp->origE);
710                         VECCOPY(bp->origT, bp->origE);
711                 }
712                 
713                 if(ob->softflag & OB_SB_EDGES) {
714                         
715                         /* happens when in UI edges was set */
716                         if(ob->soft->bspring==NULL) 
717                                 if(object_has_edges(ob)) sbObjectToSoftbody(ob);
718                 
719                         /* hrms .. do springs alter their lenght ? (yes, mesh keys would (ton))
720                         if(medge) {
721                                 bs= ob->soft->bspring;
722                                 bp= ob->soft->bpoint;
723                                 for(a=0; (a<me->totedge && a < ob->soft->totspring ); a++, medge++, bs++) { 
724                                         bs->len= VecLenf( (bp+bs->v1)->origE, (bp+bs->v2)->origE);
725                                 }
726                         }
727                         */
728                 }
729         }
730 }
731
732
733 static int get_scalar_from_named_vertexgroup(Object *ob, char *name, int vertID, float *target)
734 /* result 0 on success, else indicates error number
735 -- kind of *inverse* result defintion,
736 -- but this way we can signal error condition to caller  
737 -- and yes this function must not be here but in a *vertex group module*
738 */
739 {
740         bDeformGroup *locGroup = NULL;
741         MDeformVert *dv;
742         int i, groupindex;
743         
744         locGroup = get_named_vertexgroup(ob,name);
745         if(locGroup){
746                 /* retrieve index for that group */
747                 groupindex =  get_defgroup_num(ob,locGroup); 
748                 /* spot the vert in deform vert list at mesh */
749                 /* todo (coder paranoya) what if ob->data is not a mesh .. */ 
750                 /* hrms.. would like to have the same for lattices anyhoo */
751                 if (((Mesh *)ob->data)->dvert) {
752                         dv = ((Mesh*)ob->data)->dvert + vertID; 
753                         /* Lets see if this vert is in the weight group */
754                         for (i=0; i<dv->totweight; i++){
755                                 if (dv->dw[i].def_nr == groupindex){
756                                         *target= dv->dw[i].weight; /* got it ! */
757                                         return 0;
758                                 }
759                         }
760                 }
761                 return 2;
762         }/*if(locGroup)*/
763         return 1;
764
765
766 /* makes totally fresh start situation */
767 static void mesh_to_softbody(Object *ob)
768 {
769         SoftBody *sb;
770         Mesh *me= ob->data;
771         MVert *mvert= me->mvert;
772         MEdge *medge= me->medge;
773         BodyPoint *bp;
774         BodySpring *bs;
775         float goalfac;
776         int a, totedge;
777         
778         if (ob->softflag & OB_SB_EDGES) totedge= me->totedge;
779         else totedge= 0;
780         
781         /* renew ends with ob->soft with points and edges, also checks & makes ob->soft */
782         renew_softbody(ob, me->totvert, totedge);
783                 
784         /* we always make body points */
785         sb= ob->soft;   
786         bp= sb->bpoint;
787         goalfac= ABS(sb->maxgoal - sb->mingoal);
788         
789         for(a=me->totvert; a>0; a--, mvert++, bp++) {
790                 
791                 set_body_point(ob, bp, mvert->co);
792                 
793                 if (1) { /* switch to vg scalars*/
794                         /* get scalar values needed  *per vertex* from vertex group functions,
795                            so we can *paint* them nicly .. 
796                            they are normalized [0.0..1.0] so may be we need amplitude for scale
797                            which can be done by caller
798                            but still .. i'd like it to go this way 
799                         */ 
800                         int error;
801                         char name[32] = "SOFTGOAL";
802                         float temp;
803                         
804                         error = get_scalar_from_named_vertexgroup(ob, name, me->totvert - a, &temp);
805                         if (!error) {
806                                 bp->goal = temp;
807                                 
808                                 /* works assuming goal is <0.0, 1.0> */
809                                 bp->goal= sb->mingoal + bp->goal*goalfac;
810                                 
811                         }
812                         /* a little ad hoc changing the goal control to be less *sharp* */
813                         bp->goal = (float)pow(bp->goal, 4.0f);
814                         
815                         /* to proove the concept
816                         this would enable per vertex *mass painting*
817                         strcpy(name,"SOFTMASS");
818                         error = get_scalar_from_named_vertexgroup(ob,name,me->totvert - a,&temp);
819                         if (!error) bp->mass = temp * ob->rangeofmass;
820                         */
821
822
823
824                 } /* switch to vg scalars */
825         }
826
827         /* but we only optionally add body edge springs */
828         if (ob->softflag & OB_SB_EDGES) {
829                 if(medge) {
830                         bs= sb->bspring;
831                         bp= sb->bpoint;
832                         for(a=me->totedge; a>0; a--, medge++, bs++) {
833                                 bs->v1= medge->v1;
834                                 bs->v2= medge->v2;
835                                 bs->strength= 1.0;
836                                 bs->len= VecLenf( (bp+bs->v1)->origS, (bp+bs->v2)->origS);
837                         }
838
839                 
840                         /* insert *diagonal* springs in quads if desired */
841                         if (ob->softflag & OB_SB_QUADS) {
842                                 add_mesh_quad_diag_springs(ob);
843                         }
844
845                         build_bps_springlist(ob); /* big mesh optimization */
846                 }
847         }
848         
849 }
850
851 /* copies current sofbody position in mesh, so do this within modifier stacks! */
852 static void softbody_to_mesh(Object *ob)
853 {
854         Mesh *me= ob->data;
855         MVert *mvert;
856         BodyPoint *bp;
857         int a;
858
859         bp= ob->soft->bpoint;
860         mvert= me->mvert;
861         for(a=me->totvert; a>0; a--, mvert++, bp++) {
862                 VECCOPY(mvert->co, bp->pos);
863                 Mat4MulVecfl(ob->imat, mvert->co);      // softbody is in global coords
864         }
865
866 }
867
868 /* makes totally fresh start situation */
869 static void lattice_to_softbody(Object *ob)
870 {
871         SoftBody *sb;
872         Lattice *lt= ob->data;
873         BodyPoint *bop;
874         BPoint *bp;
875         int a, totvert;
876         
877         totvert= lt->pntsu*lt->pntsv*lt->pntsw;
878         
879         /* renew ends with ob->soft with points and edges, also checks & makes ob->soft */
880         renew_softbody(ob, totvert, 0);
881         
882         /* we always make body points */
883         sb= ob->soft;   
884         
885         for(a= totvert, bp= lt->def, bop= sb->bpoint; a>0; a--, bp++, bop++) {
886                 set_body_point(ob, bop, bp->vec);
887         }
888 }
889
890 /* copies current sofbody position */
891 static void softbody_to_lattice(Object *ob)
892 {
893         SoftBody *sb;
894         Lattice *lt= ob->data;
895         BodyPoint *bop;
896         BPoint *bp;
897         int a, totvert;
898         
899         totvert= lt->pntsu*lt->pntsv*lt->pntsw;
900         sb= ob->soft;   
901         
902         for(a= totvert, bp= lt->def, bop= sb->bpoint; a>0; a--, bp++, bop++) {
903                 VECCOPY(bp->vec, bop->pos);
904                 Mat4MulVecfl(ob->imat, bp->vec);        // softbody is in global coords
905         }
906 }
907
908 /* copy original (new) situation in softbody, as result of matrices or deform */
909 /* is assumed to enter function with ob->soft, but can be without points */
910 static void lattice_update_softbody(Object *ob)
911 {
912         Lattice *lt= ob->data;
913         BodyPoint *bop;
914         BPoint *bp;
915         int a, totvert;
916         
917         totvert= lt->pntsu*lt->pntsv*lt->pntsw;
918         
919         /* possible after a file read... */
920         if(ob->soft->totpoint!=totvert) sbObjectToSoftbody(ob);
921         
922         for(a= totvert, bp= lt->def, bop= ob->soft->bpoint; a>0; a--, bp++, bop++) {
923                 VECCOPY(bop->origS, bop->origE);
924                 VECCOPY(bop->origE, bp->vec);
925                 Mat4MulVecfl(ob->obmat, bop->origE);
926                 VECCOPY(bop->origT, bop->origE);
927         }       
928 }
929
930
931 /* copies softbody result back in object */
932 /* only used in sbObjectStep() */
933 static void softbody_to_object(Object *ob)
934 {
935         
936         if(ob->soft==NULL) return;
937         
938         /* inverse matrix is not uptodate... */
939         Mat4Invert(ob->imat, ob->obmat);
940         
941         switch(ob->type) {
942         case OB_MESH:
943                 softbody_to_mesh(ob);
944                 break;
945         case OB_LATTICE:
946                 softbody_to_lattice(ob);
947                 break;
948         }
949 }
950
951 /* copy original (new) situation in softbody, as result of matrices or deform */
952 /* used in sbObjectStep() and sbObjectReset() */
953 /* assumes to have ob->soft, but can be entered without points */
954 static void object_update_softbody(Object *ob)
955 {
956         
957         switch(ob->type) {
958         case OB_MESH:
959                 mesh_update_softbody(ob);
960                 break;
961         case OB_LATTICE:
962                 lattice_update_softbody(ob);
963                 break;
964         }
965         
966 }
967
968
969
970 /* ************ Object level, exported functions *************** */
971
972 /* allocates and initializes general main data */
973 SoftBody *sbNew(void)
974 {
975         SoftBody *sb;
976         
977         sb= MEM_callocN(sizeof(SoftBody), "softbody");
978         
979         sb->mediafrict= 0.5; 
980         sb->nodemass= 1.0;
981         sb->grav= 0.0; 
982         
983         sb->goalspring= 0.5; 
984         sb->goalfrict= 0.0; 
985         sb->mingoal= 0.0;  
986         sb->maxgoal= 1.0;
987         sb->defgoal= 0.7;
988         
989         sb->inspring= 0.5;
990         sb->infrict= 0.5; 
991         
992         return sb;
993 }
994
995 /* frees all */
996 void sbFree(SoftBody *sb)
997 {
998         free_softbody_intern(sb);
999         MEM_freeN(sb);
1000 }
1001
1002
1003 /* makes totally fresh start situation */
1004 void sbObjectToSoftbody(Object *ob)
1005 {
1006         
1007         switch(ob->type) {
1008         case OB_MESH:
1009                 mesh_to_softbody(ob);
1010                 break;
1011         case OB_LATTICE:
1012                 lattice_to_softbody(ob);
1013                 break;
1014         }
1015         
1016         if(ob->soft) ob->soft->ctime= bsystem_time(ob, NULL, (float)G.scene->r.cfra, 0.0);
1017         ob->softflag &= ~OB_SB_REDO;
1018 }
1019
1020 /* reset all motion */
1021 void sbObjectReset(Object *ob)
1022 {
1023         SoftBody *sb= ob->soft;
1024         BodyPoint *bp;
1025         int a;
1026         
1027         if(sb==NULL) return;
1028         sb->ctime= bsystem_time(ob, NULL, (float)G.scene->r.cfra, 0.0);
1029         
1030         object_update_softbody(ob);
1031         
1032         for(a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
1033                 // origS is previous timestep
1034                 VECCOPY(bp->origS, bp->origE);
1035                 VECCOPY(bp->pos, bp->origE);
1036                 VECCOPY(bp->origT, bp->origE);
1037                 bp->vec[0]= bp->vec[1]= bp->vec[2]= 0.0f;
1038
1039                 // no idea about the Heun stuff! (ton)
1040                 VECCOPY(bp->prevpos, bp->pos);
1041                 VECCOPY(bp->prevvec, bp->vec);
1042                 VECCOPY(bp->prevdx, bp->vec);
1043                 VECCOPY(bp->prevdv, bp->vec);
1044         }
1045 }
1046
1047
1048 /* simulates one step. framenr is in frames */
1049 /* copies result back to object, displist */
1050 void sbObjectStep(Object *ob, float framenr)
1051 {
1052         SoftBody *sb;
1053         Base *base;
1054         float dtime;
1055         int timescale,t;
1056         float ctime, forcetime;
1057         float err;
1058
1059         /* remake softbody if: */
1060         if( (ob->softflag & OB_SB_REDO) ||              // signal after weightpainting
1061                 (ob->soft==NULL) ||                                     // just to be nice we allow full init
1062                 (ob->soft->totpoint==0) )                       // after reading new file, or acceptable as signal to refresh
1063                         sbObjectToSoftbody(ob);
1064         
1065         sb= ob->soft;
1066
1067         /* still no points? go away */
1068         if(sb->totpoint==0) return;
1069         
1070         /* reset deflector cache, sumohandle is free, but its still sorta abuse... (ton) */
1071         for(base= G.scene->base.first; base; base= base->next) {
1072                 base->object->sumohandle= NULL;
1073         }
1074         
1075         /* checking time: */
1076         ctime= bsystem_time(ob, NULL, framenr, 0.0);
1077     softbody_scale_time(steptime); // translate frames/sec and lenghts unit to SI system
1078         dtime= ctime - sb->ctime;
1079                 // dtime= ABS(dtime); no no we want to go back in time with IPOs
1080         timescale = (int)(sb->rklimit * ABS(dtime)); 
1081                 // bail out for negative or for large steps
1082         if(dtime<0.0 || dtime >= 9.9*G.scene->r.framelen) {
1083                 sbObjectReset(ob);
1084                 return;
1085         }
1086         
1087         /* the simulator */
1088         
1089         if(ABS(dtime) > 0.0) {  // note: what does this mean now? (ton)
1090                 
1091                 object_update_softbody(ob);
1092                 
1093                 if (TRUE) {     // RSOL1 always true now (ton)
1094                         /* special case of 2nd order Runge-Kutta type AKA Heun */
1095                         float timedone =0.0;
1096                         /* counter for emergency brake
1097                          * we don't want to lock up the system if physics fail
1098                          */
1099                         int loops =0 ; 
1100                         SoftHeunTol = sb->rklimit; // humm .. this should be calculated from sb parameters and sizes
1101
1102                         forcetime = dtime; /* hope for integrating in one step */
1103                         while ( (ABS(timedone) < ABS(dtime)) && (loops < 2000) )
1104                         {
1105                                 if (ABS(dtime) > 3.0 ){
1106                                         if(G.f & G_DEBUG) printf("SB_STEPSIZE \n");
1107                                         break; // sorry but i must assume goal movement can't be interpolated any more
1108                                 }
1109                                 //set goals in time
1110                                 interpolate_exciter(ob,200,(int)(200.0*(timedone/dtime)));
1111                                 // do predictive euler step
1112                                 softbody_calc_forces(ob, forcetime);
1113                                 softbody_apply_forces(ob, forcetime, 1, NULL);
1114                                 // crop new slope values to do averaged slope step
1115                                 softbody_calc_forces(ob, forcetime);
1116                                 softbody_apply_forces(ob, forcetime, 2, &err);
1117                                 softbody_apply_goalsnap(ob);
1118
1119                                 if (err > SoftHeunTol){ // error needs to be scaled to some quantity
1120                                         softbody_restore_prev_step(ob);
1121                                         forcetime /= 2.0;
1122                                 }
1123                                 else {
1124                                         float newtime = forcetime * 1.1f; // hope for 1.1 times better conditions in next step
1125                                         
1126                                         if (err > SoftHeunTol/2.0){ // stay with this stepsize unless err really small
1127                                                 newtime = forcetime;
1128                                         }
1129                                         timedone += forcetime;
1130                                         if (forcetime > 0.0)
1131                                                 forcetime = MIN2(dtime - timedone,newtime);
1132                                         else 
1133                                                 forcetime = MAX2(dtime - timedone,newtime);
1134                                 }
1135                                 loops++;
1136                         }
1137                         // move snapped to final position
1138                         interpolate_exciter(ob, 2, 2);
1139                         softbody_apply_goalsnap(ob);
1140                         
1141                         if(G.f & G_DEBUG) {
1142                                 if (loops > HEUNWARNLIMIT) /* monitor high loop counts say 1000 after testing */
1143                                         printf("%d heun integration loops/frame \n",loops);
1144                         }
1145                 }
1146                 else
1147         /* do brute force explicit euler */
1148                 /* inner intagration loop */
1149                 /* */
1150                 // loop n times so that n*h = duration of one frame := 1
1151                 // x(t+h) = x(t) + h*v(t);
1152                 // v(t+h) = v(t) + h*f(x(t),t);
1153                 for(t=1 ; t <= timescale; t++) {
1154                         if (ABS(dtime) > 15 ) break;
1155                         
1156                         /* the *goal* mesh must use the n*h timing too !
1157                         use *cheap* linear intepolation for that  */
1158                         interpolate_exciter(ob,timescale,t);                    
1159                         if (timescale > 0 ) {
1160                                 forcetime = dtime/timescale;
1161
1162                                 /* does not fit the concept sloving ODEs :) */
1163                                 /*                      softbody_apply_goal(ob,forcetime );  */
1164                                 
1165                                 /* explicit Euler integration */
1166                                 /* we are not controling a nuclear power plant! 
1167                                 so rought *almost* physical behaviour is acceptable.
1168                                 in cases of *mild* stiffnes cranking up timscale -> decreasing stepsize *h*
1169                                 avoids instability      */
1170                                 softbody_calc_forces(ob,forcetime);
1171                                 softbody_apply_forces(ob,forcetime,0, NULL);
1172                                 softbody_apply_goalsnap(ob);
1173
1174                                 //      if (0){
1175                                         /* ok here comes the ├╝berhammer
1176                                         use a semi implicit euler integration to tackle *all* stiff conditions 
1177                                         but i doubt the cost/benifit holds for most of the cases
1178                                         -- to be coded*/
1179                                 //      }
1180                                 
1181                         }
1182                 }
1183                 
1184                 /* and apply to vertices */
1185                  softbody_to_object(ob);
1186                 
1187                 sb->ctime= ctime;
1188         } // if(ABS(dtime) > 0.0) 
1189         else {
1190                 // rule : you have asked for the current state of the softobject 
1191                 // since dtime= ctime - ob->soft->ctime;
1192                 // and we were not notifified about any other time changes 
1193                 // so here it is !
1194                 softbody_to_object(ob);
1195         }
1196
1197         /* reset deflector cache */
1198         for(base= G.scene->base.first; base; base= base->next) {
1199                 if(base->object->sumohandle) {
1200                         MEM_freeN(base->object->sumohandle);
1201                         base->object->sumohandle= NULL;
1202                 }
1203         }
1204         
1205 }
1206