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