Merge branch 'master' into blender2.8
[blender.git] / source / blender / blenkernel / intern / softbody.c
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) Blender Foundation
19  * All rights reserved.
20  *
21  * The Original Code is: all of this file.
22  *
23  * Contributor(s): none yet.
24  *
25  * ***** END GPL LICENSE BLOCK *****
26  */
27
28 /** \file blender/blenkernel/intern/softbody.c
29  *  \ingroup bke
30  */
31
32
33 /*
34 ******
35 variables on the UI for now
36
37         float mediafrict;  friction to env
38         float nodemass;   softbody mass of *vertex*
39         float grav;        softbody amount of gravitaion to apply
40
41         float goalspring;  softbody goal springs
42         float goalfrict;   softbody goal springs friction
43         float mingoal;     quick limits for goal
44         float maxgoal;
45
46         float inspring;   softbody inner springs
47         float infrict;     softbody inner springs friction
48
49 *****
50 */
51
52
53 #include <math.h>
54 #include <stdlib.h>
55 #include <string.h>
56
57 #include "MEM_guardedalloc.h"
58
59 /* types */
60 #include "DNA_object_types.h"
61 #include "DNA_scene_types.h"
62 #include "DNA_lattice_types.h"
63 #include "DNA_curve_types.h"
64 #include "DNA_mesh_types.h"
65 #include "DNA_meshdata_types.h"
66 #include "DNA_object_force.h"
67
68 #include "BLI_math.h"
69 #include "BLI_utildefines.h"
70 #include "BLI_listbase.h"
71 #include "BLI_ghash.h"
72 #include "BLI_threads.h"
73
74 #include "BKE_curve.h"
75 #include "BKE_effect.h"
76 #include "BKE_global.h"
77 #include "BKE_modifier.h"
78 #include "BKE_softbody.h"
79 #include "BKE_deform.h"
80 #include "BKE_mesh.h"
81 #include "BKE_scene.h"
82
83 #include  "PIL_time.h"
84
85 /* callbacks for errors and interrupts and some goo */
86 static int (*SB_localInterruptCallBack)(void) = NULL;
87
88
89 /* ********** soft body engine ******* */
90
91 typedef enum {SB_EDGE=1, SB_BEND=2, SB_STIFFQUAD=3, SB_HANDLE=4} type_spring;
92
93 typedef struct BodySpring {
94         int v1, v2;
95         float len, cf, load;
96         float ext_force[3]; /* edges colliding and sailing */
97         type_spring springtype;
98         short flag;
99 } BodySpring;
100
101 typedef struct BodyFace {
102         int v1, v2, v3;
103         float ext_force[3]; /* faces colliding */
104         short flag;
105 } BodyFace;
106
107 typedef struct ReferenceVert {
108         float pos[3]; /* position relative to com */
109         float mass;   /* node mass */
110 } ReferenceVert;
111
112 typedef struct ReferenceState {
113         float com[3]; /* center of mass*/
114         ReferenceVert *ivert; /* list of intial values */
115 } ReferenceState;
116
117
118 /*private scratch pad for caching and other data only needed when alive*/
119 typedef struct SBScratch {
120         GHash *colliderhash;
121         short needstobuildcollider;
122         short flag;
123         BodyFace *bodyface;
124         int totface;
125         float aabbmin[3], aabbmax[3];
126         ReferenceState Ref;
127 } SBScratch;
128
129 typedef struct  SB_thread_context {
130                 Scene *scene;
131                 Object *ob;
132                 float forcetime;
133                 float timenow;
134                 int ifirst;
135                 int ilast;
136                 ListBase *do_effector;
137                 int do_deflector;
138                 float fieldfactor;
139                 float windfactor;
140                 int nr;
141                 int tot;
142 } SB_thread_context;
143
144 #define MID_PRESERVE 1
145
146 #define SOFTGOALSNAP  0.999f
147 /* if bp-> goal is above make it a *forced follow original* and skip all ODE stuff for this bp
148  * removes *unnecessary* stiffness from ODE system
149  */
150 #define HEUNWARNLIMIT 1 /* 500 would be fine i think for detecting severe *stiff* stuff */
151
152
153 #define BSF_INTERSECT   1 /* edge intersects collider face */
154
155 /* private definitions for bodypoint states */
156 #define SBF_DOFUZZY          1 /* Bodypoint do fuzzy */
157 #define SBF_OUTOFCOLLISION   2 /* Bodypoint does not collide  */
158
159
160 #define BFF_INTERSECT   1 /* collider edge   intrudes face */
161 #define BFF_CLOSEVERT   2 /* collider vertex repulses face */
162
163
164 static float SoftHeunTol = 1.0f; /* humm .. this should be calculated from sb parameters and sizes */
165
166 /* local prototypes */
167 static void free_softbody_intern(SoftBody *sb);
168
169 /*+++ frame based timing +++*/
170
171 /*physical unit of force is [kg * m / sec^2]*/
172
173 static float sb_grav_force_scale(Object *UNUSED(ob))
174 /* since unit of g is [m/sec^2] and F = mass * g we rescale unit mass of node to 1 gramm
175  * put it to a function here, so we can add user options later without touching simulation code
176  */
177 {
178         return (0.001f);
179 }
180
181 static float sb_fric_force_scale(Object *UNUSED(ob))
182 /* rescaling unit of drag [1 / sec] to somehow reasonable
183  * put it to a function here, so we can add user options later without touching simulation code
184  */
185 {
186         return (0.01f);
187 }
188
189 static float sb_time_scale(Object *ob)
190 /* defining the frames to *real* time relation */
191 {
192         SoftBody *sb= ob->soft; /* is supposed to be there */
193         if (sb) {
194                 return(sb->physics_speed);
195                 /* hrms .. this could be IPO as well :)
196                  * estimated range [0.001 sluggish slug - 100.0 very fast (i hope ODE solver can handle that)]
197                  * 1 approx = a unit 1 pendulum at g = 9.8 [earth conditions]  has period 65 frames
198                  * theory would give a 50 frames period .. so there must be something inaccurate .. looking for that (BM)
199                  */
200         }
201         return (1.0f);
202         /*
203          * this would be frames/sec independent timing assuming 25 fps is default
204          * but does not work very well with NLA
205          * return (25.0f/scene->r.frs_sec)
206          */
207 }
208 /*--- frame based timing ---*/
209
210 /* helper functions for everything is animatable jow_go_for2_5 +++++++*/
211 /* introducing them here, because i know: steps in properties  ( at frame timing )
212  * will cause unwanted responses of the softbody system (which does inter frame calculations )
213  * so first 'cure' would be: interpolate linear in time ..
214  * Q: why do i write this?
215  * A: because it happend once, that some eger coder 'streamlined' code to fail.
216  * We DO linear interpolation for goals .. and i think we should do on animated properties as well
217  */
218
219 /* animate sb->maxgoal, sb->mingoal */
220 static float _final_goal(Object *ob, BodyPoint *bp)/*jow_go_for2_5 */
221 {
222         float f = -1999.99f;
223         if (ob) {
224                 SoftBody *sb= ob->soft; /* is supposed to be there */
225                 if (!(ob->softflag & OB_SB_GOAL)) return (0.0f);
226                 if (sb&&bp) {
227                         if (bp->goal < 0.0f) return (0.0f);
228                         f = sb->mingoal + bp->goal * fabsf(sb->maxgoal - sb->mingoal);
229                         f = pow(f, 4.0f);
230                         return (f);
231                 }
232         }
233         printf("_final_goal failed! sb or bp ==NULL\n");
234         return f; /*using crude but spot able values some times helps debuggin */
235 }
236
237 static float _final_mass(Object *ob, BodyPoint *bp)
238 {
239         if (ob) {
240                 SoftBody *sb= ob->soft; /* is supposed to be there */
241                 if (sb&&bp) {
242                         return(bp->mass*sb->nodemass);
243                 }
244         }
245         printf("_final_mass failed! sb or bp ==NULL\n");
246         return 1.0f;
247 }
248 /* helper functions for everything is animateble jow_go_for2_5 ------*/
249
250 /*+++ collider caching and dicing +++*/
251
252 /********************
253 for each target object/face the axis aligned bounding box (AABB) is stored
254 faces parallel to global axes
255 so only simple "value" in [min, max] ckecks are used
256 float operations still
257 */
258
259 /* just an ID here to reduce the prob for killing objects
260 ** ob->sumohandle points to we should not kill :)
261 */
262 static const int CCD_SAVETY = 190561;
263
264 typedef struct ccdf_minmax {
265         float minx, miny, minz, maxx, maxy, maxz;
266 } ccdf_minmax;
267
268
269 typedef struct ccd_Mesh {
270         int mvert_num, tri_num;
271         const MVert *mvert;
272         const MVert *mprevvert;
273         const MVertTri *tri;
274         int savety;
275         ccdf_minmax *mima;
276         /* Axis Aligned Bounding Box AABB */
277         float bbmin[3];
278         float bbmax[3];
279 } ccd_Mesh;
280
281
282 static ccd_Mesh *ccd_mesh_make(Object *ob)
283 {
284         CollisionModifierData *cmd;
285         ccd_Mesh *pccd_M = NULL;
286         ccdf_minmax *mima;
287         const MVertTri *vt;
288         float hull;
289         int i;
290
291         cmd =(CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
292
293         /* first some paranoia checks */
294         if (!cmd) return NULL;
295         if (!cmd->mvert_num || !cmd->tri_num) return NULL;
296
297         pccd_M = MEM_mallocN(sizeof(ccd_Mesh), "ccd_Mesh");
298         pccd_M->mvert_num = cmd->mvert_num;
299         pccd_M->tri_num = cmd->tri_num;
300         pccd_M->savety  = CCD_SAVETY;
301         pccd_M->bbmin[0]=pccd_M->bbmin[1]=pccd_M->bbmin[2]=1e30f;
302         pccd_M->bbmax[0]=pccd_M->bbmax[1]=pccd_M->bbmax[2]=-1e30f;
303         pccd_M->mprevvert=NULL;
304
305         /* blow it up with forcefield ranges */
306         hull = max_ff(ob->pd->pdef_sbift, ob->pd->pdef_sboft);
307
308         /* alloc and copy verts*/
309         pccd_M->mvert = MEM_dupallocN(cmd->xnew);
310         /* note that xnew coords are already in global space, */
311         /* determine the ortho BB */
312         for (i = 0; i < pccd_M->mvert_num; i++) {
313                 const float *v;
314
315                 /* evaluate limits */
316                 v = pccd_M->mvert[i].co;
317                 pccd_M->bbmin[0] = min_ff(pccd_M->bbmin[0], v[0] - hull);
318                 pccd_M->bbmin[1] = min_ff(pccd_M->bbmin[1], v[1] - hull);
319                 pccd_M->bbmin[2] = min_ff(pccd_M->bbmin[2], v[2] - hull);
320
321                 pccd_M->bbmax[0] = max_ff(pccd_M->bbmax[0], v[0] + hull);
322                 pccd_M->bbmax[1] = max_ff(pccd_M->bbmax[1], v[1] + hull);
323                 pccd_M->bbmax[2] = max_ff(pccd_M->bbmax[2], v[2] + hull);
324
325         }
326         /* alloc and copy faces*/
327         pccd_M->tri = MEM_dupallocN(cmd->tri);
328
329         /* OBBs for idea1 */
330         pccd_M->mima = MEM_mallocN(sizeof(ccdf_minmax) * pccd_M->tri_num, "ccd_Mesh_Faces_mima");
331
332
333         /* anyhoo we need to walk the list of faces and find OBB they live in */
334         for (i = 0, mima  = pccd_M->mima, vt = pccd_M->tri; i < pccd_M->tri_num; i++, mima++, vt++) {
335                 const float *v;
336
337                 mima->minx=mima->miny=mima->minz=1e30f;
338                 mima->maxx=mima->maxy=mima->maxz=-1e30f;
339
340                 v = pccd_M->mvert[vt->tri[0]].co;
341                 mima->minx = min_ff(mima->minx, v[0] - hull);
342                 mima->miny = min_ff(mima->miny, v[1] - hull);
343                 mima->minz = min_ff(mima->minz, v[2] - hull);
344                 mima->maxx = max_ff(mima->maxx, v[0] + hull);
345                 mima->maxy = max_ff(mima->maxy, v[1] + hull);
346                 mima->maxz = max_ff(mima->maxz, v[2] + hull);
347
348                 v = pccd_M->mvert[vt->tri[1]].co;
349                 mima->minx = min_ff(mima->minx, v[0] - hull);
350                 mima->miny = min_ff(mima->miny, v[1] - hull);
351                 mima->minz = min_ff(mima->minz, v[2] - hull);
352                 mima->maxx = max_ff(mima->maxx, v[0] + hull);
353                 mima->maxy = max_ff(mima->maxy, v[1] + hull);
354                 mima->maxz = max_ff(mima->maxz, v[2] + hull);
355
356                 v = pccd_M->mvert[vt->tri[2]].co;
357                 mima->minx = min_ff(mima->minx, v[0] - hull);
358                 mima->miny = min_ff(mima->miny, v[1] - hull);
359                 mima->minz = min_ff(mima->minz, v[2] - hull);
360                 mima->maxx = max_ff(mima->maxx, v[0] + hull);
361                 mima->maxy = max_ff(mima->maxy, v[1] + hull);
362                 mima->maxz = max_ff(mima->maxz, v[2] + hull);
363         }
364
365         return pccd_M;
366 }
367 static void ccd_mesh_update(Object *ob, ccd_Mesh *pccd_M)
368 {
369         CollisionModifierData *cmd;
370         ccdf_minmax *mima;
371         const MVertTri *vt;
372         float hull;
373         int i;
374
375         cmd =(CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
376
377         /* first some paranoia checks */
378         if (!cmd) return;
379         if (!cmd->mvert_num || !cmd->tri_num) return;
380
381         if ((pccd_M->mvert_num != cmd->mvert_num) ||
382             (pccd_M->tri_num != cmd->tri_num))
383         {
384                 return;
385         }
386
387         pccd_M->bbmin[0]=pccd_M->bbmin[1]=pccd_M->bbmin[2]=1e30f;
388         pccd_M->bbmax[0]=pccd_M->bbmax[1]=pccd_M->bbmax[2]=-1e30f;
389
390
391         /* blow it up with forcefield ranges */
392         hull = max_ff(ob->pd->pdef_sbift, ob->pd->pdef_sboft);
393
394         /* rotate current to previous */
395         if (pccd_M->mprevvert) MEM_freeN((void *)pccd_M->mprevvert);
396         pccd_M->mprevvert = pccd_M->mvert;
397         /* alloc and copy verts*/
398         pccd_M->mvert = MEM_dupallocN(cmd->xnew);
399         /* note that xnew coords are already in global space, */
400         /* determine the ortho BB */
401         for (i=0; i < pccd_M->mvert_num; i++) {
402                 const float *v;
403
404                 /* evaluate limits */
405                 v = pccd_M->mvert[i].co;
406                 pccd_M->bbmin[0] = min_ff(pccd_M->bbmin[0], v[0] - hull);
407                 pccd_M->bbmin[1] = min_ff(pccd_M->bbmin[1], v[1] - hull);
408                 pccd_M->bbmin[2] = min_ff(pccd_M->bbmin[2], v[2] - hull);
409
410                 pccd_M->bbmax[0] = max_ff(pccd_M->bbmax[0], v[0] + hull);
411                 pccd_M->bbmax[1] = max_ff(pccd_M->bbmax[1], v[1] + hull);
412                 pccd_M->bbmax[2] = max_ff(pccd_M->bbmax[2], v[2] + hull);
413
414                 /* evaluate limits */
415                 v = pccd_M->mprevvert[i].co;
416                 pccd_M->bbmin[0] = min_ff(pccd_M->bbmin[0], v[0] - hull);
417                 pccd_M->bbmin[1] = min_ff(pccd_M->bbmin[1], v[1] - hull);
418                 pccd_M->bbmin[2] = min_ff(pccd_M->bbmin[2], v[2] - hull);
419
420                 pccd_M->bbmax[0] = max_ff(pccd_M->bbmax[0], v[0] + hull);
421                 pccd_M->bbmax[1] = max_ff(pccd_M->bbmax[1], v[1] + hull);
422                 pccd_M->bbmax[2] = max_ff(pccd_M->bbmax[2], v[2] + hull);
423
424         }
425
426         /* anyhoo we need to walk the list of faces and find OBB they live in */
427         for (i = 0, mima  = pccd_M->mima, vt = pccd_M->tri; i < pccd_M->tri_num; i++, mima++, vt++) {
428                 const float *v;
429
430                 mima->minx=mima->miny=mima->minz=1e30f;
431                 mima->maxx=mima->maxy=mima->maxz=-1e30f;
432
433                 /* mvert */
434                 v = pccd_M->mvert[vt->tri[0]].co;
435                 mima->minx = min_ff(mima->minx, v[0] - hull);
436                 mima->miny = min_ff(mima->miny, v[1] - hull);
437                 mima->minz = min_ff(mima->minz, v[2] - hull);
438                 mima->maxx = max_ff(mima->maxx, v[0] + hull);
439                 mima->maxy = max_ff(mima->maxy, v[1] + hull);
440                 mima->maxz = max_ff(mima->maxz, v[2] + hull);
441
442                 v = pccd_M->mvert[vt->tri[1]].co;
443                 mima->minx = min_ff(mima->minx, v[0] - hull);
444                 mima->miny = min_ff(mima->miny, v[1] - hull);
445                 mima->minz = min_ff(mima->minz, v[2] - hull);
446                 mima->maxx = max_ff(mima->maxx, v[0] + hull);
447                 mima->maxy = max_ff(mima->maxy, v[1] + hull);
448                 mima->maxz = max_ff(mima->maxz, v[2] + hull);
449
450                 v = pccd_M->mvert[vt->tri[2]].co;
451                 mima->minx = min_ff(mima->minx, v[0] - hull);
452                 mima->miny = min_ff(mima->miny, v[1] - hull);
453                 mima->minz = min_ff(mima->minz, v[2] - hull);
454                 mima->maxx = max_ff(mima->maxx, v[0] + hull);
455                 mima->maxy = max_ff(mima->maxy, v[1] + hull);
456                 mima->maxz = max_ff(mima->maxz, v[2] + hull);
457
458
459                 /* mprevvert */
460                 v = pccd_M->mprevvert[vt->tri[0]].co;
461                 mima->minx = min_ff(mima->minx, v[0] - hull);
462                 mima->miny = min_ff(mima->miny, v[1] - hull);
463                 mima->minz = min_ff(mima->minz, v[2] - hull);
464                 mima->maxx = max_ff(mima->maxx, v[0] + hull);
465                 mima->maxy = max_ff(mima->maxy, v[1] + hull);
466                 mima->maxz = max_ff(mima->maxz, v[2] + hull);
467
468                 v = pccd_M->mprevvert[vt->tri[1]].co;
469                 mima->minx = min_ff(mima->minx, v[0] - hull);
470                 mima->miny = min_ff(mima->miny, v[1] - hull);
471                 mima->minz = min_ff(mima->minz, v[2] - hull);
472                 mima->maxx = max_ff(mima->maxx, v[0] + hull);
473                 mima->maxy = max_ff(mima->maxy, v[1] + hull);
474                 mima->maxz = max_ff(mima->maxz, v[2] + hull);
475
476                 v = pccd_M->mprevvert[vt->tri[2]].co;
477                 mima->minx = min_ff(mima->minx, v[0] - hull);
478                 mima->miny = min_ff(mima->miny, v[1] - hull);
479                 mima->minz = min_ff(mima->minz, v[2] - hull);
480                 mima->maxx = max_ff(mima->maxx, v[0] + hull);
481                 mima->maxy = max_ff(mima->maxy, v[1] + hull);
482                 mima->maxz = max_ff(mima->maxz, v[2] + hull);
483         }
484         return;
485 }
486
487 static void ccd_mesh_free(ccd_Mesh *ccdm)
488 {
489         if (ccdm && (ccdm->savety == CCD_SAVETY )) { /*make sure we're not nuking objects we don't know*/
490                 MEM_freeN((void *)ccdm->mvert);
491                 MEM_freeN((void *)ccdm->tri);
492                 if (ccdm->mprevvert) MEM_freeN((void *)ccdm->mprevvert);
493                 MEM_freeN(ccdm->mima);
494                 MEM_freeN(ccdm);
495                 ccdm = NULL;
496         }
497 }
498
499 static void ccd_build_deflector_hash(Scene *scene, Object *vertexowner, GHash *hash)
500 {
501         Base *base= scene->base.first;
502         Object *ob;
503
504         if (!hash) return;
505         while (base) {
506                 /*Only proceed for mesh object in same layer */
507                 if (base->object->type==OB_MESH && (base->lay & vertexowner->lay)) {
508                         ob= base->object;
509                         if ((vertexowner) && (ob == vertexowner)) {
510                                 /* if vertexowner is given  we don't want to check collision with owner object */
511                                 base = base->next;
512                                 continue;
513                         }
514
515                         /*+++ only with deflecting set */
516                         if (ob->pd && ob->pd->deflect && BLI_ghash_lookup(hash, ob) == NULL) {
517                                 ccd_Mesh *ccdmesh = ccd_mesh_make(ob);
518                                 BLI_ghash_insert(hash, ob, ccdmesh);
519                         }/*--- only with deflecting set */
520
521                 }/* mesh && layer*/
522                 base = base->next;
523         } /* while (base) */
524 }
525
526 static void ccd_update_deflector_hash(Scene *scene, Object *vertexowner, GHash *hash)
527 {
528         Base *base= scene->base.first;
529         Object *ob;
530
531         if ((!hash) || (!vertexowner)) return;
532         while (base) {
533                 /*Only proceed for mesh object in same layer */
534                 if (base->object->type==OB_MESH && (base->lay & vertexowner->lay)) {
535                         ob= base->object;
536                         if (ob == vertexowner) {
537                                 /* if vertexowner is given  we don't want to check collision with owner object */
538                                 base = base->next;
539                                 continue;
540                         }
541
542                         /*+++ only with deflecting set */
543                         if (ob->pd && ob->pd->deflect) {
544                                 ccd_Mesh *ccdmesh = BLI_ghash_lookup(hash, ob);
545                                 if (ccdmesh)
546                                         ccd_mesh_update(ob, ccdmesh);
547                         }/*--- only with deflecting set */
548
549                 }/* mesh && layer*/
550                 base = base->next;
551         } /* while (base) */
552 }
553
554
555 /*--- collider caching and dicing ---*/
556
557
558 static int count_mesh_quads(Mesh *me)
559 {
560         int a, result = 0;
561         const MPoly *mp = me->mpoly;
562
563         if (mp) {
564                 for (a = me->totpoly; a > 0; a--, mp++) {
565                         if (mp->totloop == 4) {
566                                 result++;
567                         }
568                 }
569         }
570         return result;
571 }
572
573 static void add_mesh_quad_diag_springs(Object *ob)
574 {
575         Mesh *me= ob->data;
576         /*BodyPoint *bp;*/ /*UNUSED*/
577         int a;
578
579         if (ob->soft) {
580                 int nofquads;
581                 //float s_shear = ob->soft->shearstiff*ob->soft->shearstiff;
582
583                 nofquads = count_mesh_quads(me);
584                 if (nofquads) {
585                         const MLoop *mloop = me->mloop;
586                         const MPoly *mp = me->mpoly;
587                         BodySpring *bs;
588
589                         /* resize spring-array to hold additional quad springs */
590                         ob->soft->bspring = MEM_recallocN(ob->soft->bspring, sizeof(BodySpring) * (ob->soft->totspring + nofquads * 2));
591
592                         /* fill the tail */
593                         a = 0;
594                         bs = &ob->soft->bspring[ob->soft->totspring];
595                         /*bp= ob->soft->bpoint; */ /*UNUSED*/
596                         for (a = me->totpoly; a > 0; a--, mp++) {
597                                 if (mp->totloop == 4) {
598                                         bs->v1 = mloop[mp->loopstart + 0].v;
599                                         bs->v2 = mloop[mp->loopstart + 2].v;
600                                         bs->springtype   = SB_STIFFQUAD;
601                                         bs++;
602                                         bs->v1 = mloop[mp->loopstart + 1].v;
603                                         bs->v2 = mloop[mp->loopstart + 3].v;
604                                         bs->springtype   = SB_STIFFQUAD;
605                                         bs++;
606                                 }
607                         }
608
609                         /* now we can announce new springs */
610                         ob->soft->totspring += nofquads * 2;
611                 }
612         }
613 }
614
615 static void add_2nd_order_roller(Object *ob, float UNUSED(stiffness), int *counter, int addsprings)
616 {
617         /*assume we have a softbody*/
618         SoftBody *sb= ob->soft; /* is supposed to be there */
619         BodyPoint *bp, *bpo;
620         BodySpring *bs, *bs2, *bs3= NULL;
621         int a, b, c, notthis= 0, v0;
622         if (!sb->bspring) {return;} /* we are 2nd order here so 1rst should have been build :) */
623         /* first run counting  second run adding */
624         *counter = 0;
625         if (addsprings) bs3 = ob->soft->bspring+ob->soft->totspring;
626         for (a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
627                 /*scan for neighborhood*/
628                 bpo = NULL;
629                 v0  = (sb->totpoint-a);
630                 for (b=bp->nofsprings;b>0;b--) {
631                         bs = sb->bspring + bp->springs[b-1];
632                         /*nasty thing here that springs have two ends
633                         so here we have to make sure we examine the other */
634                         if (v0 == bs->v1) {
635                                 bpo = sb->bpoint+bs->v2;
636                                 notthis = bs->v2;
637                         }
638                         else {
639                         if (v0 == bs->v2) {
640                                 bpo = sb->bpoint+bs->v1;
641                                 notthis = bs->v1;
642                         }
643                         else {printf("oops we should not get here -  add_2nd_order_springs");}
644                         }
645                         if (bpo) {/* so now we have a 2nd order humpdidump */
646                                 for (c=bpo->nofsprings;c>0;c--) {
647                                         bs2 = sb->bspring + bpo->springs[c-1];
648                                         if ((bs2->v1 != notthis) && (bs2->v1 > v0)) {
649                                                 (*counter)++;/*hit */
650                                                 if (addsprings) {
651                                                         bs3->v1= v0;
652                                                         bs3->v2= bs2->v1;
653                                                         bs3->springtype   = SB_BEND;
654                                                         bs3++;
655                                                 }
656                                         }
657                                         if ((bs2->v2 !=notthis)&&(bs2->v2 > v0)) {
658                                                 (*counter)++;  /* hit */
659                                                 if (addsprings) {
660                                                         bs3->v1= v0;
661                                                         bs3->v2= bs2->v2;
662                                                         bs3->springtype   = SB_BEND;
663                                                         bs3++;
664                                                 }
665
666                                         }
667                                 }
668
669                         }
670
671                 }
672                 /*scan for neighborhood done*/
673         }
674 }
675
676
677 static void add_2nd_order_springs(Object *ob, float stiffness)
678 {
679         int counter = 0;
680         BodySpring *bs_new;
681         stiffness *=stiffness;
682
683         add_2nd_order_roller(ob, stiffness, &counter, 0); /* counting */
684         if (counter) {
685                 /* resize spring-array to hold additional springs */
686                 bs_new= MEM_callocN((ob->soft->totspring + counter )*sizeof(BodySpring), "bodyspring");
687                 memcpy(bs_new, ob->soft->bspring, (ob->soft->totspring )*sizeof(BodySpring));
688
689                 if (ob->soft->bspring)
690                         MEM_freeN(ob->soft->bspring);
691                 ob->soft->bspring = bs_new;
692
693                 add_2nd_order_roller(ob, stiffness, &counter, 1); /* adding */
694                 ob->soft->totspring += counter;
695         }
696 }
697
698 static void add_bp_springlist(BodyPoint *bp, int springID)
699 {
700         int *newlist;
701
702         if (bp->springs == NULL) {
703                 bp->springs = MEM_callocN(sizeof(int), "bpsprings");
704                 bp->springs[0] = springID;
705                 bp->nofsprings = 1;
706         }
707         else {
708                 bp->nofsprings++;
709                 newlist = MEM_callocN(bp->nofsprings * sizeof(int), "bpsprings");
710                 memcpy(newlist, bp->springs, (bp->nofsprings-1)* sizeof(int));
711                 MEM_freeN(bp->springs);
712                 bp->springs = newlist;
713                 bp->springs[bp->nofsprings-1] = springID;
714         }
715 }
716
717 /* do this once when sb is build
718 it is O(N^2) so scanning for springs every iteration is too expensive
719 */
720 static void build_bps_springlist(Object *ob)
721 {
722         SoftBody *sb= ob->soft; /* is supposed to be there */
723         BodyPoint *bp;
724         BodySpring *bs;
725         int a, b;
726
727         if (sb==NULL) return; /* paranoya check */
728
729         for (a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
730                 /* throw away old list */
731                 if (bp->springs) {
732                         MEM_freeN(bp->springs);
733                         bp->springs=NULL;
734                 }
735                 /* scan for attached inner springs */
736                 for (b=sb->totspring, bs= sb->bspring; b>0; b--, bs++) {
737                         if (( (sb->totpoint-a) == bs->v1) ) {
738                                 add_bp_springlist(bp, sb->totspring -b);
739                         }
740                         if (( (sb->totpoint-a) == bs->v2) ) {
741                                 add_bp_springlist(bp, sb->totspring -b);
742                         }
743                 }/*for springs*/
744         }/*for bp*/
745 }
746
747 static void calculate_collision_balls(Object *ob)
748 {
749         SoftBody *sb= ob->soft; /* is supposed to be there */
750         BodyPoint *bp;
751         BodySpring *bs;
752         int a, b, akku_count;
753         float min, max, akku;
754
755         if (sb==NULL) return; /* paranoya check */
756
757         for (a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
758                 bp->colball=0;
759                 akku =0.0f;
760                 akku_count=0;
761                 min = 1e22f;
762                 max = -1e22f;
763                 /* first estimation based on attached */
764                 for (b=bp->nofsprings;b>0;b--) {
765                         bs = sb->bspring + bp->springs[b-1];
766                         if (bs->springtype == SB_EDGE) {
767                                 akku += bs->len;
768                                 akku_count++;
769                                 min = min_ff(bs->len, min);
770                                 max = max_ff(bs->len, max);
771                         }
772                 }
773
774                 if (akku_count > 0) {
775                         if (sb->sbc_mode == SBC_MODE_MANUAL) {
776                                 bp->colball=sb->colball;
777                         }
778                         if (sb->sbc_mode == SBC_MODE_AVG) {
779                                 bp->colball = akku/(float)akku_count*sb->colball;
780                         }
781                         if (sb->sbc_mode == SBC_MODE_MIN) {
782                                 bp->colball=min*sb->colball;
783                         }
784                         if (sb->sbc_mode == SBC_MODE_MAX) {
785                                 bp->colball=max*sb->colball;
786                         }
787                         if (sb->sbc_mode == SBC_MODE_AVGMINMAX) {
788                                 bp->colball = (min + max)/2.0f*sb->colball;
789                         }
790                 }
791                 else bp->colball=0;
792         }/*for bp*/
793 }
794
795
796 /* creates new softbody if didn't exist yet, makes new points and springs arrays */
797 static void renew_softbody(Scene *scene, Object *ob, int totpoint, int totspring)
798 {
799         SoftBody *sb;
800         int i;
801         short softflag;
802         if (ob->soft==NULL) ob->soft= sbNew(scene);
803         else free_softbody_intern(ob->soft);
804         sb= ob->soft;
805         softflag=ob->softflag;
806
807         if (totpoint) {
808                 sb->totpoint= totpoint;
809                 sb->totspring= totspring;
810
811                 sb->bpoint= MEM_mallocN(totpoint*sizeof(BodyPoint), "bodypoint");
812                 if (totspring)
813                         sb->bspring= MEM_mallocN(totspring*sizeof(BodySpring), "bodyspring");
814
815                         /* initialize BodyPoint array */
816                 for (i=0; i<totpoint; i++) {
817                         BodyPoint *bp = &sb->bpoint[i];
818
819
820                         /* hum as far as i see this is overridden by _final_goal() now jow_go_for2_5 */
821                         /* sadly breaks compatibility with older versions */
822                         /* but makes goals behave the same for meshes, lattices and curves */
823                         if (softflag & OB_SB_GOAL) {
824                                 bp->goal= sb->defgoal;
825                         }
826                         else {
827                                 bp->goal= 0.0f;
828                                 /* so this will definily be below SOFTGOALSNAP */
829                         }
830
831                         bp->nofsprings= 0;
832                         bp->springs= NULL;
833                         bp->choke = 0.0f;
834                         bp->choke2 = 0.0f;
835                         bp->frozen = 1.0f;
836                         bp->colball = 0.0f;
837                         bp->loc_flag = 0;
838                         bp->springweight = 1.0f;
839                         bp->mass = 1.0f;
840                 }
841         }
842 }
843
844 static void free_softbody_baked(SoftBody *sb)
845 {
846         SBVertex *key;
847         int k;
848
849         for (k=0; k<sb->totkey; k++) {
850                 key= *(sb->keys + k);
851                 if (key) MEM_freeN(key);
852         }
853         if (sb->keys) MEM_freeN(sb->keys);
854
855         sb->keys= NULL;
856         sb->totkey= 0;
857 }
858 static void free_scratch(SoftBody *sb)
859 {
860         if (sb->scratch) {
861                 /* todo make sure everything is cleaned up nicly */
862                 if (sb->scratch->colliderhash) {
863                         BLI_ghash_free(sb->scratch->colliderhash, NULL,
864                                         (GHashValFreeFP) ccd_mesh_free); /*this hoepfully will free all caches*/
865                         sb->scratch->colliderhash = NULL;
866                 }
867                 if (sb->scratch->bodyface) {
868                         MEM_freeN(sb->scratch->bodyface);
869                 }
870                 if (sb->scratch->Ref.ivert) {
871                         MEM_freeN(sb->scratch->Ref.ivert);
872                 }
873                 MEM_freeN(sb->scratch);
874                 sb->scratch = NULL;
875         }
876
877 }
878
879 /* only frees internal data */
880 static void free_softbody_intern(SoftBody *sb)
881 {
882         if (sb) {
883                 int a;
884                 BodyPoint *bp;
885
886                 if (sb->bpoint) {
887                         for (a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
888                                 /* free spring list */
889                                 if (bp->springs != NULL) {
890                                         MEM_freeN(bp->springs);
891                                 }
892                         }
893                         MEM_freeN(sb->bpoint);
894                 }
895
896                 if (sb->bspring) MEM_freeN(sb->bspring);
897
898                 sb->totpoint= sb->totspring= 0;
899                 sb->bpoint= NULL;
900                 sb->bspring= NULL;
901
902                 free_scratch(sb);
903                 free_softbody_baked(sb);
904         }
905 }
906
907
908 /* ************ dynamics ********** */
909
910 /* the most general (micro physics correct) way to do collision
911 ** (only needs the current particle position)
912 **
913 ** it actually checks if the particle intrudes a short range force field generated
914 ** by the faces of the target object and returns a force to drive the particel out
915 ** the strenght of the field grows exponetially if the particle is on the 'wrong' side of the face
916 ** 'wrong' side : projection to the face normal is negative (all referred to a vertex in the face)
917 **
918 ** flaw of this: 'fast' particles as well as 'fast' colliding faces
919 ** give a 'tunnel' effect such that the particle passes through the force field
920 ** without ever 'seeing' it
921 ** this is fully compliant to heisenberg: h >= fuzzy(location) * fuzzy(time)
922 ** besides our h is way larger than in QM because forces propagate way slower here
923 ** we have to deal with fuzzy(time) in the range of 1/25 seconds (typical frame rate)
924 ** yup collision targets are not known here any better
925 ** and 1/25 second is looong compared to real collision events
926 ** Q: why not use 'simple' collision here like bouncing back a particle
927 **   --> reverting is velocity on the face normal
928 ** A: because our particles are not alone here
929 **    and need to tell their neighbors exactly what happens via spring forces
930 ** unless sbObjectStep( .. ) is called on sub frame timing level
931 ** BTW that also questions the use of a 'implicit' solvers on softbodies
932 ** since that would only valid for 'slow' moving collision targets and dito particles
933 */
934
935 /* +++ dependency information functions*/
936
937 static int are_there_deflectors(Scene *scene, unsigned int layer)
938 {
939         Base *base;
940
941         for (base = scene->base.first; base; base= base->next) {
942                 if ( (base->lay & layer) && base->object->pd) {
943                         if (base->object->pd->deflect)
944                                 return 1;
945                 }
946         }
947         return 0;
948 }
949
950 static int query_external_colliders(Scene *scene, Object *me)
951 {
952         return(are_there_deflectors(scene, me->lay));
953 }
954 /* --- dependency information functions*/
955
956
957 /* +++ the aabb "force" section*/
958 static int sb_detect_aabb_collisionCached(float UNUSED(force[3]), unsigned int UNUSED(par_layer), struct Object *vertexowner, float UNUSED(time))
959 {
960         Object *ob;
961         SoftBody *sb=vertexowner->soft;
962         GHash *hash;
963         GHashIterator *ihash;
964         float  aabbmin[3], aabbmax[3];
965         int deflected=0;
966 #if 0
967         int a;
968 #endif
969
970         if ((sb == NULL) || (sb->scratch ==NULL)) return 0;
971         copy_v3_v3(aabbmin, sb->scratch->aabbmin);
972         copy_v3_v3(aabbmax, sb->scratch->aabbmax);
973
974         hash  = vertexowner->soft->scratch->colliderhash;
975         ihash = BLI_ghashIterator_new(hash);
976         while (!BLI_ghashIterator_done(ihash)) {
977
978                 ccd_Mesh *ccdm = BLI_ghashIterator_getValue     (ihash);
979                 ob             = BLI_ghashIterator_getKey       (ihash);
980                 {
981                         /* only with deflecting set */
982                         if (ob->pd && ob->pd->deflect) {
983                                 if (ccdm) {
984                                         if ((aabbmax[0] < ccdm->bbmin[0]) ||
985                                             (aabbmax[1] < ccdm->bbmin[1]) ||
986                                             (aabbmax[2] < ccdm->bbmin[2]) ||
987                                             (aabbmin[0] > ccdm->bbmax[0]) ||
988                                             (aabbmin[1] > ccdm->bbmax[1]) ||
989                                             (aabbmin[2] > ccdm->bbmax[2]) )
990                                         {
991                                                 /* boxes don't intersect */
992                                                 BLI_ghashIterator_step(ihash);
993                                                 continue;
994                                         }
995
996                                         /* so now we have the 2 boxes overlapping */
997                                         /* forces actually not used */
998                                         deflected = 2;
999
1000                                 }
1001                                 else {
1002                                         /*aye that should be cached*/
1003                                         printf("missing cache error\n");
1004                                         BLI_ghashIterator_step(ihash);
1005                                         continue;
1006                                 }
1007                         } /* if (ob->pd && ob->pd->deflect) */
1008                         BLI_ghashIterator_step(ihash);
1009                 }
1010         } /* while () */
1011         BLI_ghashIterator_free(ihash);
1012         return deflected;
1013 }
1014 /* --- the aabb section*/
1015
1016
1017 /* +++ the face external section*/
1018 static int sb_detect_face_pointCached(float face_v1[3], float face_v2[3], float face_v3[3], float *damp,
1019                                       float force[3], unsigned int UNUSED(par_layer), struct Object *vertexowner, float time)
1020 {
1021         Object *ob;
1022         GHash *hash;
1023         GHashIterator *ihash;
1024         float nv1[3], edge1[3], edge2[3], d_nvect[3], aabbmin[3], aabbmax[3];
1025         float facedist, outerfacethickness, tune = 10.f;
1026         int a, deflected=0;
1027
1028         aabbmin[0] = min_fff(face_v1[0], face_v2[0], face_v3[0]);
1029         aabbmin[1] = min_fff(face_v1[1], face_v2[1], face_v3[1]);
1030         aabbmin[2] = min_fff(face_v1[2], face_v2[2], face_v3[2]);
1031         aabbmax[0] = max_fff(face_v1[0], face_v2[0], face_v3[0]);
1032         aabbmax[1] = max_fff(face_v1[1], face_v2[1], face_v3[1]);
1033         aabbmax[2] = max_fff(face_v1[2], face_v2[2], face_v3[2]);
1034
1035         /* calculate face normal once again SIGH */
1036         sub_v3_v3v3(edge1, face_v1, face_v2);
1037         sub_v3_v3v3(edge2, face_v3, face_v2);
1038         cross_v3_v3v3(d_nvect, edge2, edge1);
1039         normalize_v3(d_nvect);
1040
1041
1042         hash  = vertexowner->soft->scratch->colliderhash;
1043         ihash = BLI_ghashIterator_new(hash);
1044         while (!BLI_ghashIterator_done(ihash)) {
1045
1046                 ccd_Mesh *ccdm = BLI_ghashIterator_getValue     (ihash);
1047                 ob             = BLI_ghashIterator_getKey       (ihash);
1048                 {
1049                         /* only with deflecting set */
1050                         if (ob->pd && ob->pd->deflect) {
1051                                 const MVert *mvert= NULL;
1052                                 const MVert *mprevvert= NULL;
1053                                 if (ccdm) {
1054                                         mvert = ccdm->mvert;
1055                                         a     = ccdm->mvert_num;
1056                                         mprevvert= ccdm->mprevvert;
1057                                         outerfacethickness = ob->pd->pdef_sboft;
1058                                         if ((aabbmax[0] < ccdm->bbmin[0]) ||
1059                                             (aabbmax[1] < ccdm->bbmin[1]) ||
1060                                             (aabbmax[2] < ccdm->bbmin[2]) ||
1061                                             (aabbmin[0] > ccdm->bbmax[0]) ||
1062                                             (aabbmin[1] > ccdm->bbmax[1]) ||
1063                                             (aabbmin[2] > ccdm->bbmax[2]) )
1064                                         {
1065                                                 /* boxes don't intersect */
1066                                                 BLI_ghashIterator_step(ihash);
1067                                                 continue;
1068                                         }
1069
1070                                 }
1071                                 else {
1072                                         /*aye that should be cached*/
1073                                         printf("missing cache error\n");
1074                                         BLI_ghashIterator_step(ihash);
1075                                         continue;
1076                                 }
1077
1078
1079                                 /* use mesh*/
1080                                 if (mvert) {
1081                                         while (a) {
1082                                                 copy_v3_v3(nv1, mvert[a-1].co);
1083                                                 if (mprevvert) {
1084                                                         mul_v3_fl(nv1, time);
1085                                                         madd_v3_v3fl(nv1, mprevvert[a - 1].co, 1.0f - time);
1086                                                 }
1087                                                 /* origin to face_v2*/
1088                                                 sub_v3_v3(nv1, face_v2);
1089                                                 facedist = dot_v3v3(nv1, d_nvect);
1090                                                 if (ABS(facedist)<outerfacethickness) {
1091                                                         if (isect_point_tri_prism_v3(nv1, face_v1, face_v2, face_v3) ) {
1092                                                                 float df;
1093                                                                 if (facedist > 0) {
1094                                                                         df = (outerfacethickness-facedist)/outerfacethickness;
1095                                                                 }
1096                                                                 else {
1097                                                                         df = (outerfacethickness+facedist)/outerfacethickness;
1098                                                                 }
1099
1100                                                                 *damp=df*tune*ob->pd->pdef_sbdamp;
1101
1102                                                                 df = 0.01f * expf(-100.0f * df);
1103                                                                 madd_v3_v3fl(force, d_nvect, -df);
1104                                                                 deflected = 3;
1105                                                         }
1106                                                 }
1107                                                 a--;
1108                                         }/* while (a)*/
1109                                 } /* if (mvert) */
1110                         } /* if (ob->pd && ob->pd->deflect) */
1111                         BLI_ghashIterator_step(ihash);
1112                 }
1113         } /* while () */
1114         BLI_ghashIterator_free(ihash);
1115         return deflected;
1116 }
1117
1118
1119 static int sb_detect_face_collisionCached(float face_v1[3], float face_v2[3], float face_v3[3], float *damp,
1120                                           float force[3], unsigned int UNUSED(par_layer), struct Object *vertexowner, float time)
1121 {
1122         Object *ob;
1123         GHash *hash;
1124         GHashIterator *ihash;
1125         float nv1[3], nv2[3], nv3[3], edge1[3], edge2[3], d_nvect[3], aabbmin[3], aabbmax[3];
1126         float t, tune = 10.0f;
1127         int a, deflected=0;
1128
1129         aabbmin[0] = min_fff(face_v1[0], face_v2[0], face_v3[0]);
1130         aabbmin[1] = min_fff(face_v1[1], face_v2[1], face_v3[1]);
1131         aabbmin[2] = min_fff(face_v1[2], face_v2[2], face_v3[2]);
1132         aabbmax[0] = max_fff(face_v1[0], face_v2[0], face_v3[0]);
1133         aabbmax[1] = max_fff(face_v1[1], face_v2[1], face_v3[1]);
1134         aabbmax[2] = max_fff(face_v1[2], face_v2[2], face_v3[2]);
1135
1136         hash  = vertexowner->soft->scratch->colliderhash;
1137         ihash = BLI_ghashIterator_new(hash);
1138         while (!BLI_ghashIterator_done(ihash)) {
1139
1140                 ccd_Mesh *ccdm = BLI_ghashIterator_getValue     (ihash);
1141                 ob             = BLI_ghashIterator_getKey       (ihash);
1142                 {
1143                         /* only with deflecting set */
1144                         if (ob->pd && ob->pd->deflect) {
1145                                 const MVert *mvert = NULL;
1146                                 const MVert *mprevvert = NULL;
1147                                 const MVertTri *vt = NULL;
1148                                 const ccdf_minmax *mima = NULL;
1149
1150                                 if (ccdm) {
1151                                         mvert = ccdm->mvert;
1152                                         vt = ccdm->tri;
1153                                         mprevvert = ccdm->mprevvert;
1154                                         mima = ccdm->mima;
1155                                         a = ccdm->tri_num;
1156
1157                                         if ((aabbmax[0] < ccdm->bbmin[0]) ||
1158                                             (aabbmax[1] < ccdm->bbmin[1]) ||
1159                                             (aabbmax[2] < ccdm->bbmin[2]) ||
1160                                             (aabbmin[0] > ccdm->bbmax[0]) ||
1161                                             (aabbmin[1] > ccdm->bbmax[1]) ||
1162                                             (aabbmin[2] > ccdm->bbmax[2]) )
1163                                         {
1164                                                 /* boxes don't intersect */
1165                                                 BLI_ghashIterator_step(ihash);
1166                                                 continue;
1167                                         }
1168
1169                                 }
1170                                 else {
1171                                         /*aye that should be cached*/
1172                                         printf("missing cache error\n");
1173                                         BLI_ghashIterator_step(ihash);
1174                                         continue;
1175                                 }
1176
1177
1178                                 /* use mesh*/
1179                                 while (a--) {
1180                                         if ((aabbmax[0] < mima->minx) ||
1181                                             (aabbmin[0] > mima->maxx) ||
1182                                             (aabbmax[1] < mima->miny) ||
1183                                             (aabbmin[1] > mima->maxy) ||
1184                                             (aabbmax[2] < mima->minz) ||
1185                                             (aabbmin[2] > mima->maxz))
1186                                         {
1187                                                 mima++;
1188                                                 vt++;
1189                                                 continue;
1190                                         }
1191
1192
1193                                         if (mvert) {
1194
1195                                                 copy_v3_v3(nv1, mvert[vt->tri[0]].co);
1196                                                 copy_v3_v3(nv2, mvert[vt->tri[1]].co);
1197                                                 copy_v3_v3(nv3, mvert[vt->tri[2]].co);
1198
1199                                                 if (mprevvert) {
1200                                                         mul_v3_fl(nv1, time);
1201                                                         madd_v3_v3fl(nv1, mprevvert[vt->tri[0]].co, 1.0f - time);
1202
1203                                                         mul_v3_fl(nv2, time);
1204                                                         madd_v3_v3fl(nv2, mprevvert[vt->tri[1]].co, 1.0f - time);
1205
1206                                                         mul_v3_fl(nv3, time);
1207                                                         madd_v3_v3fl(nv3, mprevvert[vt->tri[2]].co, 1.0f - time);
1208                                                 }
1209                                         }
1210
1211                                         /* switch origin to be nv2*/
1212                                         sub_v3_v3v3(edge1, nv1, nv2);
1213                                         sub_v3_v3v3(edge2, nv3, nv2);
1214                                         cross_v3_v3v3(d_nvect, edge2, edge1);
1215                                         normalize_v3(d_nvect);
1216                                         if (isect_line_segment_tri_v3(nv1, nv2, face_v1, face_v2, face_v3, &t, NULL) ||
1217                                             isect_line_segment_tri_v3(nv2, nv3, face_v1, face_v2, face_v3, &t, NULL) ||
1218                                             isect_line_segment_tri_v3(nv3, nv1, face_v1, face_v2, face_v3, &t, NULL) )
1219                                         {
1220                                                 madd_v3_v3fl(force, d_nvect, -0.5f);
1221                                                 *damp=tune*ob->pd->pdef_sbdamp;
1222                                                 deflected = 2;
1223                                         }
1224                                         mima++;
1225                                         vt++;
1226                                 }/* while a */
1227                         } /* if (ob->pd && ob->pd->deflect) */
1228                         BLI_ghashIterator_step(ihash);
1229                 }
1230         } /* while () */
1231         BLI_ghashIterator_free(ihash);
1232         return deflected;
1233 }
1234
1235
1236
1237 static void scan_for_ext_face_forces(Object *ob, float timenow)
1238 {
1239         SoftBody *sb = ob->soft;
1240         BodyFace *bf;
1241         int a;
1242         float damp=0.0f, choke=1.0f;
1243         float tune = -10.0f;
1244         float feedback[3];
1245
1246         if (sb && sb->scratch->totface) {
1247
1248
1249                 bf = sb->scratch->bodyface;
1250                 for (a=0; a<sb->scratch->totface; a++, bf++) {
1251                         bf->ext_force[0]=bf->ext_force[1]=bf->ext_force[2]=0.0f;
1252 /*+++edges intruding*/
1253                         bf->flag &= ~BFF_INTERSECT;
1254                         zero_v3(feedback);
1255                         if (sb_detect_face_collisionCached(
1256                                 sb->bpoint[bf->v1].pos, sb->bpoint[bf->v2].pos, sb->bpoint[bf->v3].pos,
1257                                 &damp, feedback, ob->lay, ob, timenow))
1258                         {
1259                                 madd_v3_v3fl(sb->bpoint[bf->v1].force, feedback, tune);
1260                                 madd_v3_v3fl(sb->bpoint[bf->v2].force, feedback, tune);
1261                                 madd_v3_v3fl(sb->bpoint[bf->v3].force, feedback, tune);
1262 //                              madd_v3_v3fl(bf->ext_force, feedback, tune);
1263                                 bf->flag |= BFF_INTERSECT;
1264                                 choke = min_ff(max_ff(damp, choke), 1.0f);
1265                         }
1266 /*---edges intruding*/
1267
1268 /*+++ close vertices*/
1269                         if (( bf->flag & BFF_INTERSECT)==0) {
1270                                 bf->flag &= ~BFF_CLOSEVERT;
1271                                 tune = -1.0f;
1272                                 zero_v3(feedback);
1273                                 if (sb_detect_face_pointCached(
1274                                         sb->bpoint[bf->v1].pos, sb->bpoint[bf->v2].pos, sb->bpoint[bf->v3].pos,
1275                                         &damp,  feedback, ob->lay, ob, timenow))
1276                                 {
1277                                         madd_v3_v3fl(sb->bpoint[bf->v1].force, feedback, tune);
1278                                         madd_v3_v3fl(sb->bpoint[bf->v2].force, feedback, tune);
1279                                         madd_v3_v3fl(sb->bpoint[bf->v3].force, feedback, tune);
1280 //                                      madd_v3_v3fl(bf->ext_force, feedback, tune);
1281                                         bf->flag |= BFF_CLOSEVERT;
1282                                         choke = min_ff(max_ff(damp, choke), 1.0f);
1283                                 }
1284                         }
1285 /*--- close vertices*/
1286                 }
1287                 bf = sb->scratch->bodyface;
1288                 for (a=0; a<sb->scratch->totface; a++, bf++) {
1289                         if (( bf->flag & BFF_INTERSECT) || ( bf->flag & BFF_CLOSEVERT)) {
1290                                 sb->bpoint[bf->v1].choke2 = max_ff(sb->bpoint[bf->v1].choke2, choke);
1291                                 sb->bpoint[bf->v2].choke2 = max_ff(sb->bpoint[bf->v2].choke2, choke);
1292                                 sb->bpoint[bf->v3].choke2 = max_ff(sb->bpoint[bf->v3].choke2, choke);
1293                         }
1294                 }
1295         }
1296 }
1297
1298 /*  --- the face external section*/
1299
1300
1301 /* +++ the spring external section*/
1302
1303 static int sb_detect_edge_collisionCached(float edge_v1[3], float edge_v2[3], float *damp,
1304                                                                    float force[3], unsigned int UNUSED(par_layer), struct Object *vertexowner, float time)
1305 {
1306         Object *ob;
1307         GHash *hash;
1308         GHashIterator *ihash;
1309         float nv1[3], nv2[3], nv3[3], edge1[3], edge2[3], d_nvect[3], aabbmin[3], aabbmax[3];
1310         float t, el;
1311         int a, deflected=0;
1312
1313         minmax_v3v3_v3(aabbmin, aabbmax, edge_v1);
1314         minmax_v3v3_v3(aabbmin, aabbmax, edge_v2);
1315
1316         el = len_v3v3(edge_v1, edge_v2);
1317
1318         hash  = vertexowner->soft->scratch->colliderhash;
1319         ihash = BLI_ghashIterator_new(hash);
1320         while (!BLI_ghashIterator_done(ihash)) {
1321
1322                 ccd_Mesh *ccdm = BLI_ghashIterator_getValue     (ihash);
1323                 ob             = BLI_ghashIterator_getKey       (ihash);
1324                 {
1325                         /* only with deflecting set */
1326                         if (ob->pd && ob->pd->deflect) {
1327                                 const MVert *mvert = NULL;
1328                                 const MVert *mprevvert = NULL;
1329                                 const MVertTri *vt = NULL;
1330                                 const ccdf_minmax *mima = NULL;
1331
1332                                 if (ccdm) {
1333                                         mvert = ccdm->mvert;
1334                                         mprevvert = ccdm->mprevvert;
1335                                         vt = ccdm->tri;
1336                                         mima = ccdm->mima;
1337                                         a = ccdm->tri_num;
1338
1339                                         if ((aabbmax[0] < ccdm->bbmin[0]) ||
1340                                             (aabbmax[1] < ccdm->bbmin[1]) ||
1341                                             (aabbmax[2] < ccdm->bbmin[2]) ||
1342                                             (aabbmin[0] > ccdm->bbmax[0]) ||
1343                                             (aabbmin[1] > ccdm->bbmax[1]) ||
1344                                             (aabbmin[2] > ccdm->bbmax[2]) )
1345                                         {
1346                                                 /* boxes don't intersect */
1347                                                 BLI_ghashIterator_step(ihash);
1348                                                 continue;
1349                                         }
1350
1351                                 }
1352                                 else {
1353                                         /*aye that should be cached*/
1354                                         printf("missing cache error\n");
1355                                         BLI_ghashIterator_step(ihash);
1356                                         continue;
1357                                 }
1358
1359
1360                                 /* use mesh*/
1361                                 while (a--) {
1362                                         if ((aabbmax[0] < mima->minx) ||
1363                                             (aabbmin[0] > mima->maxx) ||
1364                                             (aabbmax[1] < mima->miny) ||
1365                                             (aabbmin[1] > mima->maxy) ||
1366                                             (aabbmax[2] < mima->minz) ||
1367                                             (aabbmin[2] > mima->maxz))
1368                                         {
1369                                                 mima++;
1370                                                 vt++;
1371                                                 continue;
1372                                         }
1373
1374
1375                                         if (mvert) {
1376
1377                                                 copy_v3_v3(nv1, mvert[vt->tri[0]].co);
1378                                                 copy_v3_v3(nv2, mvert[vt->tri[1]].co);
1379                                                 copy_v3_v3(nv3, mvert[vt->tri[2]].co);
1380
1381                                                 if (mprevvert) {
1382                                                         mul_v3_fl(nv1, time);
1383                                                         madd_v3_v3fl(nv1, mprevvert[vt->tri[0]].co, 1.0f - time);
1384
1385                                                         mul_v3_fl(nv2, time);
1386                                                         madd_v3_v3fl(nv2, mprevvert[vt->tri[1]].co, 1.0f - time);
1387
1388                                                         mul_v3_fl(nv3, time);
1389                                                         madd_v3_v3fl(nv3, mprevvert[vt->tri[2]].co, 1.0f - time);
1390                                                 }
1391                                         }
1392
1393                                         /* switch origin to be nv2*/
1394                                         sub_v3_v3v3(edge1, nv1, nv2);
1395                                         sub_v3_v3v3(edge2, nv3, nv2);
1396
1397                                         cross_v3_v3v3(d_nvect, edge2, edge1);
1398                                         normalize_v3(d_nvect);
1399                                         if (isect_line_segment_tri_v3(edge_v1, edge_v2, nv1, nv2, nv3, &t, NULL)) {
1400                                                 float v1[3], v2[3];
1401                                                 float intrusiondepth, i1, i2;
1402                                                 sub_v3_v3v3(v1, edge_v1, nv2);
1403                                                 sub_v3_v3v3(v2, edge_v2, nv2);
1404                                                 i1 = dot_v3v3(v1, d_nvect);
1405                                                 i2 = dot_v3v3(v2, d_nvect);
1406                                                 intrusiondepth = -min_ff(i1, i2) / el;
1407                                                 madd_v3_v3fl(force, d_nvect, intrusiondepth);
1408                                                 *damp=ob->pd->pdef_sbdamp;
1409                                                 deflected = 2;
1410                                         }
1411
1412                                         mima++;
1413                                         vt++;
1414                                 }/* while a */
1415                         } /* if (ob->pd && ob->pd->deflect) */
1416                         BLI_ghashIterator_step(ihash);
1417                 }
1418         } /* while () */
1419         BLI_ghashIterator_free(ihash);
1420         return deflected;
1421 }
1422
1423 static void _scan_for_ext_spring_forces(Scene *scene, Object *ob, float timenow, int ifirst, int ilast, struct ListBase *do_effector)
1424 {
1425         SoftBody *sb = ob->soft;
1426         int a;
1427         float damp;
1428         float feedback[3];
1429
1430         if (sb && sb->totspring) {
1431                 for (a=ifirst; a<ilast; a++) {
1432                         BodySpring *bs = &sb->bspring[a];
1433                         bs->ext_force[0]=bs->ext_force[1]=bs->ext_force[2]=0.0f;
1434                         feedback[0]=feedback[1]=feedback[2]=0.0f;
1435                         bs->flag &= ~BSF_INTERSECT;
1436
1437                         if (bs->springtype == SB_EDGE) {
1438                                 /* +++ springs colliding */
1439                                 if (ob->softflag & OB_SB_EDGECOLL) {
1440                                         if ( sb_detect_edge_collisionCached (sb->bpoint[bs->v1].pos, sb->bpoint[bs->v2].pos,
1441                                                 &damp, feedback, ob->lay, ob, timenow)) {
1442                                                         add_v3_v3(bs->ext_force, feedback);
1443                                                         bs->flag |= BSF_INTERSECT;
1444                                                         //bs->cf=damp;
1445                                                         bs->cf=sb->choke*0.01f;
1446
1447                                         }
1448                                 }
1449                                 /* ---- springs colliding */
1450
1451                                 /* +++ springs seeing wind ... n stuff depending on their orientation*/
1452                                 /* note we don't use sb->mediafrict but use sb->aeroedge for magnitude of effect*/
1453                                 if (sb->aeroedge) {
1454                                         float vel[3], sp[3], pr[3], force[3];
1455                                         float f, windfactor  = 0.25f;
1456                                         /*see if we have wind*/
1457                                         if (do_effector) {
1458                                                 EffectedPoint epoint;
1459                                                 float speed[3] = {0.0f, 0.0f, 0.0f};
1460                                                 float pos[3];
1461                                                 mid_v3_v3v3(pos, sb->bpoint[bs->v1].pos, sb->bpoint[bs->v2].pos);
1462                                                 mid_v3_v3v3(vel, sb->bpoint[bs->v1].vec, sb->bpoint[bs->v2].vec);
1463                                                 pd_point_from_soft(scene, pos, vel, -1, &epoint);
1464                                                 pdDoEffectors(do_effector, NULL, sb->effector_weights, &epoint, force, speed);
1465
1466                                                 mul_v3_fl(speed, windfactor);
1467                                                 add_v3_v3(vel, speed);
1468                                         }
1469                                         /* media in rest */
1470                                         else {
1471                                                 add_v3_v3v3(vel, sb->bpoint[bs->v1].vec, sb->bpoint[bs->v2].vec);
1472                                         }
1473                                         f = normalize_v3(vel);
1474                                         f = -0.0001f*f*f*sb->aeroedge;
1475                                         /* (todo) add a nice angle dependent function done for now BUT */
1476                                         /* still there could be some nice drag/lift function, but who needs it */
1477
1478                                         sub_v3_v3v3(sp, sb->bpoint[bs->v1].pos, sb->bpoint[bs->v2].pos);
1479                                         project_v3_v3v3(pr, vel, sp);
1480                                         sub_v3_v3(vel, pr);
1481                                         normalize_v3(vel);
1482                                         if (ob->softflag & OB_SB_AERO_ANGLE) {
1483                                                 normalize_v3(sp);
1484                                                 madd_v3_v3fl(bs->ext_force, vel, f * (1.0f - fabsf(dot_v3v3(vel, sp))));
1485                                         }
1486                                         else {
1487                                                 madd_v3_v3fl(bs->ext_force, vel, f); // to keep compatible with 2.45 release files
1488                                         }
1489                                 }
1490                                 /* --- springs seeing wind */
1491                         }
1492                 }
1493         }
1494 }
1495
1496
1497 static void scan_for_ext_spring_forces(Scene *scene, Object *ob, float timenow)
1498 {
1499         SoftBody *sb = ob->soft;
1500         ListBase *do_effector = NULL;
1501
1502         do_effector = pdInitEffectors(scene, ob, sb->effector_weights, true);
1503         _scan_for_ext_spring_forces(scene, ob, timenow, 0, sb->totspring, do_effector);
1504         pdEndEffectors(&do_effector);
1505 }
1506
1507 static void *exec_scan_for_ext_spring_forces(void *data)
1508 {
1509         SB_thread_context *pctx = (SB_thread_context*)data;
1510         _scan_for_ext_spring_forces(pctx->scene, pctx->ob, pctx->timenow, pctx->ifirst, pctx->ilast, pctx->do_effector);
1511         return NULL;
1512 }
1513
1514 static void sb_sfesf_threads_run(Scene *scene, struct Object *ob, float timenow, int totsprings, int *UNUSED(ptr_to_break_func(void)))
1515 {
1516         ListBase *do_effector = NULL;
1517         ListBase threads;
1518         SB_thread_context *sb_threads;
1519         int i, totthread, left, dec;
1520         int lowsprings =100; /* wild guess .. may increase with better thread management 'above' or even be UI option sb->spawn_cf_threads_nopts */
1521
1522         do_effector= pdInitEffectors(scene, ob, ob->soft->effector_weights, true);
1523
1524         /* figure the number of threads while preventing pretty pointless threading overhead */
1525         totthread= BKE_scene_num_threads(scene);
1526         /* what if we got zillions of CPUs running but less to spread*/
1527         while ((totsprings/totthread < lowsprings) && (totthread > 1)) {
1528                 totthread--;
1529         }
1530
1531         sb_threads= MEM_callocN(sizeof(SB_thread_context)*totthread, "SBSpringsThread");
1532         memset(sb_threads, 0, sizeof(SB_thread_context)*totthread);
1533         left = totsprings;
1534         dec = totsprings/totthread +1;
1535         for (i=0; i<totthread; i++) {
1536                 sb_threads[i].scene = scene;
1537                 sb_threads[i].ob = ob;
1538                 sb_threads[i].forcetime = 0.0; // not used here
1539                 sb_threads[i].timenow = timenow;
1540                 sb_threads[i].ilast   = left;
1541                 left = left - dec;
1542                 if (left >0) {
1543                         sb_threads[i].ifirst  = left;
1544                 }
1545                 else
1546                         sb_threads[i].ifirst  = 0;
1547                 sb_threads[i].do_effector = do_effector;
1548                 sb_threads[i].do_deflector = false;// not used here
1549                 sb_threads[i].fieldfactor = 0.0f;// not used here
1550                 sb_threads[i].windfactor  = 0.0f;// not used here
1551                 sb_threads[i].nr= i;
1552                 sb_threads[i].tot= totthread;
1553         }
1554         if (totthread > 1) {
1555                 BLI_init_threads(&threads, exec_scan_for_ext_spring_forces, totthread);
1556
1557                 for (i=0; i<totthread; i++)
1558                         BLI_insert_thread(&threads, &sb_threads[i]);
1559
1560                 BLI_end_threads(&threads);
1561         }
1562         else
1563                 exec_scan_for_ext_spring_forces(&sb_threads[0]);
1564         /* clean up */
1565         MEM_freeN(sb_threads);
1566
1567         pdEndEffectors(&do_effector);
1568 }
1569
1570
1571 /* --- the spring external section*/
1572
1573 static int choose_winner(float*w, float* pos, float*a, float*b, float*c, float*ca, float*cb, float*cc)
1574 {
1575         float mindist, cp;
1576         int winner =1;
1577         mindist = fabsf(dot_v3v3(pos, a));
1578
1579         cp = fabsf(dot_v3v3(pos, b));
1580         if ( mindist < cp ) {
1581                 mindist = cp;
1582                 winner =2;
1583         }
1584
1585         cp = fabsf(dot_v3v3(pos, c));
1586         if (mindist < cp ) {
1587                 mindist = cp;
1588                 winner =3;
1589         }
1590         switch (winner) {
1591                 case 1: copy_v3_v3(w, ca); break;
1592                 case 2: copy_v3_v3(w, cb); break;
1593                 case 3: copy_v3_v3(w, cc);
1594         }
1595         return(winner);
1596 }
1597
1598
1599
1600 static int sb_detect_vertex_collisionCached(
1601         float opco[3], float facenormal[3], float *damp,
1602         float force[3], unsigned int UNUSED(par_layer), struct Object *vertexowner,
1603         float time, float vel[3], float *intrusion)
1604 {
1605         Object *ob= NULL;
1606         GHash *hash;
1607         GHashIterator *ihash;
1608         float nv1[3], nv2[3], nv3[3], edge1[3], edge2[3], d_nvect[3], dv1[3], ve[3], avel[3] = {0.0, 0.0, 0.0},
1609               vv1[3], vv2[3], vv3[3], coledge[3] = {0.0f, 0.0f, 0.0f}, mindistedge = 1000.0f,
1610               outerforceaccu[3], innerforceaccu[3],
1611               facedist, /* n_mag, */ /* UNUSED */ force_mag_norm, minx, miny, minz, maxx, maxy, maxz,
1612               innerfacethickness = -0.5f, outerfacethickness = 0.2f,
1613               ee = 5.0f, ff = 0.1f, fa=1;
1614         int a, deflected=0, cavel=0, ci=0;
1615 /* init */
1616         *intrusion = 0.0f;
1617         hash  = vertexowner->soft->scratch->colliderhash;
1618         ihash = BLI_ghashIterator_new(hash);
1619         outerforceaccu[0]=outerforceaccu[1]=outerforceaccu[2]=0.0f;
1620         innerforceaccu[0]=innerforceaccu[1]=innerforceaccu[2]=0.0f;
1621 /* go */
1622         while (!BLI_ghashIterator_done(ihash)) {
1623
1624                 ccd_Mesh *ccdm = BLI_ghashIterator_getValue     (ihash);
1625                 ob             = BLI_ghashIterator_getKey       (ihash);
1626                 {
1627                         /* only with deflecting set */
1628                         if (ob->pd && ob->pd->deflect) {
1629                                 const MVert *mvert = NULL;
1630                                 const MVert *mprevvert = NULL;
1631                                 const MVertTri *vt = NULL;
1632                                 const ccdf_minmax *mima = NULL;
1633
1634                                 if (ccdm) {
1635                                         mvert = ccdm->mvert;
1636                                         mprevvert = ccdm->mprevvert;
1637                                         vt = ccdm->tri;
1638                                         mima = ccdm->mima;
1639                                         a = ccdm->tri_num;
1640
1641                                         minx = ccdm->bbmin[0];
1642                                         miny = ccdm->bbmin[1];
1643                                         minz = ccdm->bbmin[2];
1644
1645                                         maxx = ccdm->bbmax[0];
1646                                         maxy = ccdm->bbmax[1];
1647                                         maxz = ccdm->bbmax[2];
1648
1649                                         if ((opco[0] < minx) ||
1650                                             (opco[1] < miny) ||
1651                                             (opco[2] < minz) ||
1652                                             (opco[0] > maxx) ||
1653                                             (opco[1] > maxy) ||
1654                                             (opco[2] > maxz) )
1655                                         {
1656                                                 /* outside the padded boundbox --> collision object is too far away */
1657                                                 BLI_ghashIterator_step(ihash);
1658                                                 continue;
1659                                         }
1660                                 }
1661                                 else {
1662                                         /*aye that should be cached*/
1663                                         printf("missing cache error\n");
1664                                         BLI_ghashIterator_step(ihash);
1665                                         continue;
1666                                 }
1667
1668                                 /* do object level stuff */
1669                                 /* need to have user control for that since it depends on model scale */
1670                                 innerfacethickness = -ob->pd->pdef_sbift;
1671                                 outerfacethickness =  ob->pd->pdef_sboft;
1672                                 fa = (ff*outerfacethickness-outerfacethickness);
1673                                 fa *= fa;
1674                                 fa = 1.0f/fa;
1675                                 avel[0]=avel[1]=avel[2]=0.0f;
1676                                 /* use mesh*/
1677                                 while (a--) {
1678                                         if ((opco[0] < mima->minx) ||
1679                                             (opco[0] > mima->maxx) ||
1680                                             (opco[1] < mima->miny) ||
1681                                             (opco[1] > mima->maxy) ||
1682                                             (opco[2] < mima->minz) ||
1683                                             (opco[2] > mima->maxz))
1684                                         {
1685                                                 mima++;
1686                                                 vt++;
1687                                                 continue;
1688                                         }
1689
1690                                         if (mvert) {
1691
1692                                                 copy_v3_v3(nv1, mvert[vt->tri[0]].co);
1693                                                 copy_v3_v3(nv2, mvert[vt->tri[1]].co);
1694                                                 copy_v3_v3(nv3, mvert[vt->tri[2]].co);
1695
1696                                                 if (mprevvert) {
1697                                                         /* grab the average speed of the collider vertices
1698                                                         before we spoil nvX
1699                                                         humm could be done once a SB steps but then we' need to store that too
1700                                                         since the AABB reduced propabitlty to get here drasticallly
1701                                                         it might be a nice tradeof CPU <--> memory
1702                                                         */
1703                                                         sub_v3_v3v3(vv1, nv1, mprevvert[vt->tri[0]].co);
1704                                                         sub_v3_v3v3(vv2, nv2, mprevvert[vt->tri[1]].co);
1705                                                         sub_v3_v3v3(vv3, nv3, mprevvert[vt->tri[2]].co);
1706
1707                                                         mul_v3_fl(nv1, time);
1708                                                         madd_v3_v3fl(nv1, mprevvert[vt->tri[0]].co, 1.0f - time);
1709
1710                                                         mul_v3_fl(nv2, time);
1711                                                         madd_v3_v3fl(nv2, mprevvert[vt->tri[1]].co, 1.0f - time);
1712
1713                                                         mul_v3_fl(nv3, time);
1714                                                         madd_v3_v3fl(nv3, mprevvert[vt->tri[2]].co, 1.0f - time);
1715                                                 }
1716                                         }
1717
1718                                         /* switch origin to be nv2*/
1719                                         sub_v3_v3v3(edge1, nv1, nv2);
1720                                         sub_v3_v3v3(edge2, nv3, nv2);
1721                                         sub_v3_v3v3(dv1, opco, nv2); /* abuse dv1 to have vertex in question at *origin* of triangle */
1722
1723                                         cross_v3_v3v3(d_nvect, edge2, edge1);
1724                                         /* n_mag = */ /* UNUSED */ normalize_v3(d_nvect);
1725                                         facedist = dot_v3v3(dv1, d_nvect);
1726                                         // so rules are
1727                                         //
1728
1729                                         if ((facedist > innerfacethickness) && (facedist < outerfacethickness)) {
1730                                                 if (isect_point_tri_prism_v3(opco, nv1, nv2, nv3) ) {
1731                                                         force_mag_norm =(float)exp(-ee*facedist);
1732                                                         if (facedist > outerfacethickness*ff)
1733                                                                 force_mag_norm =(float)force_mag_norm*fa*(facedist - outerfacethickness)*(facedist - outerfacethickness);
1734                                                         *damp=ob->pd->pdef_sbdamp;
1735                                                         if (facedist > 0.0f) {
1736                                                                 *damp *= (1.0f - facedist/outerfacethickness);
1737                                                                 madd_v3_v3fl(outerforceaccu, d_nvect, force_mag_norm);
1738                                                                 deflected = 3;
1739
1740                                                         }
1741                                                         else {
1742                                                                 madd_v3_v3fl(innerforceaccu, d_nvect, force_mag_norm);
1743                                                                 if (deflected < 2) deflected = 2;
1744                                                         }
1745                                                         if ((mprevvert) && (*damp > 0.0f)) {
1746                                                                 choose_winner(ve, opco, nv1, nv2, nv3, vv1, vv2, vv3);
1747                                                                 add_v3_v3(avel, ve);
1748                                                                 cavel ++;
1749                                                         }
1750                                                         *intrusion += facedist;
1751                                                         ci++;
1752                                                 }
1753                                         }
1754
1755                                         mima++;
1756                                         vt++;
1757                                 }/* while a */
1758                         } /* if (ob->pd && ob->pd->deflect) */
1759                         BLI_ghashIterator_step(ihash);
1760                 }
1761         } /* while () */
1762
1763         if (deflected == 1) { // no face but 'outer' edge cylinder sees vert
1764                 force_mag_norm =(float)exp(-ee*mindistedge);
1765                 if (mindistedge > outerfacethickness*ff)
1766                         force_mag_norm =(float)force_mag_norm*fa*(mindistedge - outerfacethickness)*(mindistedge - outerfacethickness);
1767                 madd_v3_v3fl(force, coledge, force_mag_norm);
1768                 *damp=ob->pd->pdef_sbdamp;
1769                 if (mindistedge > 0.0f) {
1770                         *damp *= (1.0f - mindistedge/outerfacethickness);
1771                 }
1772
1773         }
1774         if (deflected == 2) { //  face inner detected
1775                 add_v3_v3(force, innerforceaccu);
1776         }
1777         if (deflected == 3) { //  face outer detected
1778                 add_v3_v3(force, outerforceaccu);
1779         }
1780
1781         BLI_ghashIterator_free(ihash);
1782         if (cavel) mul_v3_fl(avel, 1.0f/(float)cavel);
1783         copy_v3_v3(vel, avel);
1784         if (ci) *intrusion /= ci;
1785         if (deflected) {
1786                 normalize_v3_v3(facenormal, force);
1787         }
1788         return deflected;
1789 }
1790
1791
1792 /* sandbox to plug in various deflection algos */
1793 static int sb_deflect_face(Object *ob, float *actpos, float *facenormal, float *force, float *cf, float time, float *vel, float *intrusion)
1794 {
1795         float s_actpos[3];
1796         int deflected;
1797         copy_v3_v3(s_actpos, actpos);
1798         deflected= sb_detect_vertex_collisionCached(s_actpos, facenormal, cf, force, ob->lay, ob, time, vel, intrusion);
1799         //deflected= sb_detect_vertex_collisionCachedEx(s_actpos, facenormal, cf, force, ob->lay, ob, time, vel, intrusion);
1800         return(deflected);
1801 }
1802
1803 /* hiding this for now .. but the jacobian may pop up on other tasks .. so i'd like to keep it
1804 static void dfdx_spring(int ia, int ic, int op, float dir[3], float L, float len, float factor)
1805 {
1806         float m, delta_ij;
1807         int i, j;
1808         if (L < len) {
1809                 for (i=0;i<3;i++)
1810                         for (j=0;j<3;j++) {
1811                                 delta_ij = (i==j ? (1.0f): (0.0f));
1812                                 m=factor*(dir[i]*dir[j] + (1-L/len)*(delta_ij - dir[i]*dir[j]));
1813                                 EIG_linear_solver_matrix_add(ia+i, op+ic+j, m);
1814                         }
1815         }
1816         else {
1817                 for (i=0;i<3;i++)
1818                         for (j=0;j<3;j++) {
1819                                 m=factor*dir[i]*dir[j];
1820                                 EIG_linear_solver_matrix_add(ia+i, op+ic+j, m);
1821                         }
1822         }
1823 }
1824
1825
1826 static void dfdx_goal(int ia, int ic, int op, float factor)
1827 {
1828         int i;
1829         for (i=0;i<3;i++) EIG_linear_solver_matrix_add(ia+i, op+ic+i, factor);
1830 }
1831
1832 static void dfdv_goal(int ia, int ic, float factor)
1833 {
1834         int i;
1835         for (i=0;i<3;i++) EIG_linear_solver_matrix_add(ia+i, ic+i, factor);
1836 }
1837 */
1838 static void sb_spring_force(Object *ob, int bpi, BodySpring *bs, float iks, float UNUSED(forcetime))
1839 {
1840         SoftBody *sb= ob->soft; /* is supposed to be there */
1841         BodyPoint  *bp1, *bp2;
1842
1843         float dir[3], dvel[3];
1844         float distance, forcefactor, kd, absvel, projvel, kw;
1845 #if 0   /* UNUSED */
1846         int ia, ic;
1847 #endif
1848         /* prepare depending on which side of the spring we are on */
1849         if (bpi == bs->v1) {
1850                 bp1 = &sb->bpoint[bs->v1];
1851                 bp2 = &sb->bpoint[bs->v2];
1852 #if 0   /* UNUSED */
1853                 ia =3*bs->v1;
1854                 ic =3*bs->v2;
1855 #endif
1856         }
1857         else if (bpi == bs->v2) {
1858                 bp1 = &sb->bpoint[bs->v2];
1859                 bp2 = &sb->bpoint[bs->v1];
1860 #if 0   /* UNUSED */
1861                 ia =3*bs->v2;
1862                 ic =3*bs->v1;
1863 #endif
1864         }
1865         else {
1866                 /* TODO make this debug option */
1867                 /**/
1868                 printf("bodypoint <bpi> is not attached to spring  <*bs> --> sb_spring_force()\n");
1869                 return;
1870         }
1871
1872         /* do bp1 <--> bp2 elastic */
1873         sub_v3_v3v3(dir, bp1->pos, bp2->pos);
1874         distance = normalize_v3(dir);
1875         if (bs->len < distance)
1876                 iks  = 1.0f/(1.0f-sb->inspring)-1.0f ;/* inner spring constants function */
1877         else
1878                 iks  = 1.0f/(1.0f-sb->inpush)-1.0f ;/* inner spring constants function */
1879
1880         if (bs->len > 0.0f) /* check for degenerated springs */
1881                 forcefactor = iks/bs->len;
1882         else
1883                 forcefactor = iks;
1884         kw = (bp1->springweight+bp2->springweight)/2.0f;
1885         kw = kw * kw;
1886         kw = kw * kw;
1887         switch (bs->springtype) {
1888                 case SB_EDGE:
1889                 case SB_HANDLE:
1890                         forcefactor *=  kw;
1891                         break;
1892                 case SB_BEND:
1893                         forcefactor *=sb->secondspring*kw;
1894                         break;
1895                 case SB_STIFFQUAD:
1896                         forcefactor *=sb->shearstiff*sb->shearstiff* kw;
1897                         break;
1898                 default:
1899                         break;
1900         }
1901
1902
1903         madd_v3_v3fl(bp1->force, dir, (bs->len - distance) * forcefactor);
1904
1905         /* do bp1 <--> bp2 viscous */
1906         sub_v3_v3v3(dvel, bp1->vec, bp2->vec);
1907         kd = sb->infrict * sb_fric_force_scale(ob);
1908         absvel  = normalize_v3(dvel);
1909         projvel = dot_v3v3(dir, dvel);
1910         kd     *= absvel * projvel;
1911         madd_v3_v3fl(bp1->force, dir, -kd);
1912 }
1913
1914
1915 /* since this is definitely the most CPU consuming task here .. try to spread it */
1916 /* core function _softbody_calc_forces_slice_in_a_thread */
1917 /* result is int to be able to flag user break */
1918 static int _softbody_calc_forces_slice_in_a_thread(Scene *scene, Object *ob, float forcetime, float timenow, int ifirst, int ilast, int *UNUSED(ptr_to_break_func(void)), ListBase *do_effector, int do_deflector, float fieldfactor, float windfactor)
1919 {
1920         float iks;
1921         int bb, do_selfcollision, do_springcollision, do_aero;
1922         int number_of_points_here = ilast - ifirst;
1923         SoftBody *sb= ob->soft; /* is supposed to be there */
1924         BodyPoint  *bp;
1925
1926         /* intitialize */
1927         if (sb) {
1928         /* check conditions for various options */
1929         /* +++ could be done on object level to squeeze out the last bits of it */
1930         do_selfcollision=((ob->softflag & OB_SB_EDGES) && (sb->bspring)&& (ob->softflag & OB_SB_SELF));
1931         do_springcollision=do_deflector && (ob->softflag & OB_SB_EDGES) &&(ob->softflag & OB_SB_EDGECOLL);
1932         do_aero=((sb->aeroedge)&& (ob->softflag & OB_SB_EDGES));
1933         /* --- could be done on object level to squeeze out the last bits of it */
1934         }
1935         else {
1936                 printf("Error expected a SB here\n");
1937                 return (999);
1938         }
1939
1940 /* debugerin */
1941         if (sb->totpoint < ifirst) {
1942                 printf("Aye 998");
1943                 return (998);
1944         }
1945 /* debugerin */
1946
1947
1948         bp = &sb->bpoint[ifirst];
1949         for (bb=number_of_points_here; bb>0; bb--, bp++) {
1950                 /* clear forces  accumulator */
1951                 bp->force[0] = bp->force[1] = bp->force[2] = 0.0;
1952                 /* naive ball self collision */
1953                 /* needs to be done if goal snaps or not */
1954                 if (do_selfcollision) {
1955                         int attached;
1956                         BodyPoint   *obp;
1957                         BodySpring *bs;
1958                         int c, b;
1959                         float velcenter[3], dvel[3], def[3];
1960                         float distance;
1961                         float compare;
1962                         float bstune = sb->ballstiff;
1963
1964                         for (c=sb->totpoint, obp= sb->bpoint; c>=ifirst+bb; c--, obp++) {
1965                                 compare = (obp->colball + bp->colball);
1966                                 sub_v3_v3v3(def, bp->pos, obp->pos);
1967                                 /* rather check the AABBoxes before ever calulating the real distance */
1968                                 /* mathematically it is completely nuts, but performance is pretty much (3) times faster */
1969                                 if ((ABS(def[0]) > compare) || (ABS(def[1]) > compare) || (ABS(def[2]) > compare)) continue;
1970                                 distance = normalize_v3(def);
1971                                 if (distance < compare ) {
1972                                         /* exclude body points attached with a spring */
1973                                         attached = 0;
1974                                         for (b=obp->nofsprings;b>0;b--) {
1975                                                 bs = sb->bspring + obp->springs[b-1];
1976                                                 if (( ilast-bb == bs->v2) || ( ilast-bb == bs->v1)) {
1977                                                         attached=1;
1978                                                         continue;}
1979                                         }
1980                                         if (!attached) {
1981                                                 float f = bstune / (distance) + bstune / (compare * compare) * distance - 2.0f * bstune / compare;
1982
1983                                                 mid_v3_v3v3(velcenter, bp->vec, obp->vec);
1984                                                 sub_v3_v3v3(dvel, velcenter, bp->vec);
1985                                                 mul_v3_fl(dvel, _final_mass(ob, bp));
1986
1987                                                 madd_v3_v3fl(bp->force, def, f * (1.0f - sb->balldamp));
1988                                                 madd_v3_v3fl(bp->force, dvel, sb->balldamp);
1989
1990                                                 /* exploit force(a, b) == -force(b, a) part2/2 */
1991                                                 sub_v3_v3v3(dvel, velcenter, obp->vec);
1992                                                 mul_v3_fl(dvel, _final_mass(ob, bp));
1993
1994                                                 madd_v3_v3fl(obp->force, dvel, sb->balldamp);
1995                                                 madd_v3_v3fl(obp->force, def, -f * (1.0f - sb->balldamp));
1996                                         }
1997                                 }
1998                         }
1999                 }
2000                 /* naive ball self collision done */
2001
2002                 if (_final_goal(ob, bp) < SOFTGOALSNAP) {  /* omit this bp when it snaps */
2003                         float auxvect[3];
2004                         float velgoal[3];
2005
2006                         /* do goal stuff */
2007                         if (ob->softflag & OB_SB_GOAL) {
2008                                 /* true elastic goal */
2009                                 float ks, kd;
2010                                 sub_v3_v3v3(auxvect, bp->pos, bp->origT);
2011                                 ks  = 1.0f / (1.0f - _final_goal(ob, bp) * sb->goalspring) - 1.0f;
2012                                 bp->force[0]+= -ks*(auxvect[0]);
2013                                 bp->force[1]+= -ks*(auxvect[1]);
2014                                 bp->force[2]+= -ks*(auxvect[2]);
2015
2016                                 /* calulate damping forces generated by goals*/
2017                                 sub_v3_v3v3(velgoal, bp->origS, bp->origE);
2018                                 kd =  sb->goalfrict * sb_fric_force_scale(ob);
2019                                 add_v3_v3v3(auxvect, velgoal, bp->vec);
2020
2021                                 if (forcetime > 0.0f) { /* make sure friction does not become rocket motor on time reversal */
2022                                         bp->force[0]-= kd * (auxvect[0]);
2023                                         bp->force[1]-= kd * (auxvect[1]);
2024                                         bp->force[2]-= kd * (auxvect[2]);
2025                                 }
2026                                 else {
2027                                         bp->force[0]-= kd * (velgoal[0] - bp->vec[0]);
2028                                         bp->force[1]-= kd * (velgoal[1] - bp->vec[1]);
2029                                         bp->force[2]-= kd * (velgoal[2] - bp->vec[2]);
2030                                 }
2031                         }
2032                         /* done goal stuff */
2033
2034                         /* gravitation */
2035                         if (scene->physics_settings.flag & PHYS_GLOBAL_GRAVITY) {
2036                                 float gravity[3];
2037                                 copy_v3_v3(gravity, scene->physics_settings.gravity);
2038                                 mul_v3_fl(gravity, sb_grav_force_scale(ob)*_final_mass(ob, bp)*sb->effector_weights->global_gravity); /* individual mass of node here */
2039                                 add_v3_v3(bp->force, gravity);
2040                         }
2041
2042                         /* particle field & vortex */
2043                         if (do_effector) {
2044                                 EffectedPoint epoint;
2045                                 float kd;
2046                                 float force[3] = {0.0f, 0.0f, 0.0f};
2047                                 float speed[3] = {0.0f, 0.0f, 0.0f};
2048                                 float eval_sb_fric_force_scale = sb_fric_force_scale(ob); /* just for calling function once */
2049                                 pd_point_from_soft(scene, bp->pos, bp->vec, sb->bpoint-bp, &epoint);
2050                                 pdDoEffectors(do_effector, NULL, sb->effector_weights, &epoint, force, speed);
2051
2052                                 /* apply forcefield*/
2053                                 mul_v3_fl(force, fieldfactor* eval_sb_fric_force_scale);
2054                                 add_v3_v3(bp->force, force);
2055
2056                                 /* BP friction in moving media */
2057                                 kd= sb->mediafrict* eval_sb_fric_force_scale;
2058                                 bp->force[0] -= kd * (bp->vec[0] + windfactor*speed[0]/eval_sb_fric_force_scale);
2059                                 bp->force[1] -= kd * (bp->vec[1] + windfactor*speed[1]/eval_sb_fric_force_scale);
2060                                 bp->force[2] -= kd * (bp->vec[2] + windfactor*speed[2]/eval_sb_fric_force_scale);
2061                                 /* now we'll have nice centrifugal effect for vortex */
2062
2063                         }
2064                         else {
2065                                 /* BP friction in media (not) moving*/
2066                                 float kd = sb->mediafrict* sb_fric_force_scale(ob);
2067                                 /* assume it to be proportional to actual velocity */
2068                                 bp->force[0]-= bp->vec[0]*kd;
2069                                 bp->force[1]-= bp->vec[1]*kd;
2070                                 bp->force[2]-= bp->vec[2]*kd;
2071                                 /* friction in media done */
2072                         }
2073                         /* +++cached collision targets */
2074                         bp->choke = 0.0f;
2075                         bp->choke2 = 0.0f;
2076                         bp->loc_flag &= ~SBF_DOFUZZY;
2077                         if (do_deflector && !(bp->loc_flag & SBF_OUTOFCOLLISION) ) {
2078                                 float cfforce[3], defforce[3] ={0.0f, 0.0f, 0.0f}, vel[3] = {0.0f, 0.0f, 0.0f}, facenormal[3], cf = 1.0f, intrusion;
2079                                 float kd = 1.0f;
2080
2081                                 if (sb_deflect_face(ob, bp->pos, facenormal, defforce, &cf, timenow, vel, &intrusion)) {
2082                                         if (intrusion < 0.0f) {
2083                                                 sb->scratch->flag |= SBF_DOFUZZY;
2084                                                 bp->loc_flag |= SBF_DOFUZZY;
2085                                                 bp->choke = sb->choke*0.01f;
2086                                         }
2087
2088                                         sub_v3_v3v3(cfforce, bp->vec, vel);
2089                                         madd_v3_v3fl(bp->force, cfforce, -cf * 50.0f);
2090
2091                                         madd_v3_v3fl(bp->force, defforce, kd);
2092                                 }
2093
2094                         }
2095                         /* ---cached collision targets */
2096
2097                         /* +++springs */
2098                         iks  = 1.0f/(1.0f-sb->inspring)-1.0f ;/* inner spring constants function */
2099                         if (ob->softflag & OB_SB_EDGES) {
2100                                 if (sb->bspring) { /* spring list exists at all ? */
2101                                         int b;
2102                                         BodySpring *bs;
2103                                         for (b=bp->nofsprings;b>0;b--) {
2104                                                 bs = sb->bspring + bp->springs[b-1];
2105                                                 if (do_springcollision || do_aero) {
2106                                                         add_v3_v3(bp->force, bs->ext_force);
2107                                                         if (bs->flag & BSF_INTERSECT)
2108                                                                 bp->choke = bs->cf;
2109
2110                                                 }
2111                                                 // sb_spring_force(Object *ob, int bpi, BodySpring *bs, float iks, float forcetime)
2112                                                 sb_spring_force(ob, ilast-bb, bs, iks, forcetime);
2113                                         }/* loop springs */
2114                                 }/* existing spring list */
2115                         }/*any edges*/
2116                         /* ---springs */
2117                 }/*omit on snap */
2118         }/*loop all bp's*/
2119         return 0; /*done fine*/
2120 }
2121
2122 static void *exec_softbody_calc_forces(void *data)
2123 {
2124         SB_thread_context *pctx = (SB_thread_context*)data;
2125         _softbody_calc_forces_slice_in_a_thread(pctx->scene, pctx->ob, pctx->forcetime, pctx->timenow, pctx->ifirst, pctx->ilast, NULL, pctx->do_effector, pctx->do_deflector, pctx->fieldfactor, pctx->windfactor);
2126         return NULL;
2127 }
2128
2129 static void sb_cf_threads_run(Scene *scene, Object *ob, float forcetime, float timenow, int totpoint, int *UNUSED(ptr_to_break_func(void)), struct ListBase *do_effector, int do_deflector, float fieldfactor, float windfactor)
2130 {
2131         ListBase threads;
2132         SB_thread_context *sb_threads;
2133         int i, totthread, left, dec;
2134         int lowpoints =100; /* wild guess .. may increase with better thread management 'above' or even be UI option sb->spawn_cf_threads_nopts */
2135
2136         /* figure the number of threads while preventing pretty pointless threading overhead */
2137         totthread= BKE_scene_num_threads(scene);
2138         /* what if we got zillions of CPUs running but less to spread*/
2139         while ((totpoint/totthread < lowpoints) && (totthread > 1)) {
2140                 totthread--;
2141         }
2142
2143         /* printf("sb_cf_threads_run spawning %d threads\n", totthread); */
2144
2145         sb_threads= MEM_callocN(sizeof(SB_thread_context)*totthread, "SBThread");
2146         memset(sb_threads, 0, sizeof(SB_thread_context)*totthread);
2147         left = totpoint;
2148         dec = totpoint/totthread +1;
2149         for (i=0; i<totthread; i++) {
2150                 sb_threads[i].scene = scene;
2151                 sb_threads[i].ob = ob;
2152                 sb_threads[i].forcetime = forcetime;
2153                 sb_threads[i].timenow = timenow;
2154                 sb_threads[i].ilast   = left;
2155                 left = left - dec;
2156                 if (left >0) {
2157                         sb_threads[i].ifirst  = left;
2158                 }
2159                 else
2160                         sb_threads[i].ifirst  = 0;
2161                 sb_threads[i].do_effector = do_effector;
2162                 sb_threads[i].do_deflector = do_deflector;
2163                 sb_threads[i].fieldfactor = fieldfactor;
2164                 sb_threads[i].windfactor  = windfactor;
2165                 sb_threads[i].nr= i;
2166                 sb_threads[i].tot= totthread;
2167         }
2168
2169
2170         if (totthread > 1) {
2171                 BLI_init_threads(&threads, exec_softbody_calc_forces, totthread);
2172
2173                 for (i=0; i<totthread; i++)
2174                         BLI_insert_thread(&threads, &sb_threads[i]);
2175
2176                 BLI_end_threads(&threads);
2177         }
2178         else
2179                 exec_softbody_calc_forces(&sb_threads[0]);
2180         /* clean up */
2181         MEM_freeN(sb_threads);
2182 }
2183
2184 static void softbody_calc_forcesEx(Scene *scene, Object *ob, float forcetime, float timenow)
2185 {
2186 /* rule we never alter free variables :bp->vec bp->pos in here !
2187  * this will ruin adaptive stepsize AKA heun! (BM)
2188  */
2189         SoftBody *sb= ob->soft; /* is supposed to be there */
2190         /*BodyPoint *bproot;*/ /* UNUSED */
2191         ListBase *do_effector = NULL;
2192         /* float gravity; */ /* UNUSED */
2193         /* float iks; */
2194         float fieldfactor = -1.0f, windfactor  = 0.25;
2195         int   do_deflector /*, do_selfcollision*/, do_springcollision, do_aero;
2196
2197         /* gravity = sb->grav * sb_grav_force_scale(ob); */ /* UNUSED */
2198
2199         /* check conditions for various options */
2200         do_deflector= query_external_colliders(scene, ob);
2201         /* do_selfcollision=((ob->softflag & OB_SB_EDGES) && (sb->bspring)&& (ob->softflag & OB_SB_SELF)); */ /* UNUSED */
2202         do_springcollision=do_deflector && (ob->softflag & OB_SB_EDGES) &&(ob->softflag & OB_SB_EDGECOLL);
2203         do_aero=((sb->aeroedge)&& (ob->softflag & OB_SB_EDGES));
2204
2205         /* iks  = 1.0f/(1.0f-sb->inspring)-1.0f; */ /* inner spring constants function */ /* UNUSED */
2206         /* bproot= sb->bpoint; */ /* need this for proper spring addressing */ /* UNUSED */
2207
2208         if (do_springcollision || do_aero)
2209                 sb_sfesf_threads_run(scene, ob, timenow, sb->totspring, NULL);
2210
2211         /* after spring scan because it uses Effoctors too */
2212         do_effector= pdInitEffectors(scene, ob, sb->effector_weights, true);
2213
2214         if (do_deflector) {
2215                 float defforce[3];
2216                 do_deflector = sb_detect_aabb_collisionCached(defforce, ob->lay, ob, timenow);
2217         }
2218
2219         sb_cf_threads_run(scene, ob, forcetime, timenow, sb->totpoint, NULL, do_effector, do_deflector, fieldfactor, windfactor);
2220
2221         /* finally add forces caused by face collision */
2222         if (ob->softflag & OB_SB_FACECOLL) scan_for_ext_face_forces(ob, timenow);
2223
2224         /* finish matrix and solve */
2225         pdEndEffectors(&do_effector);
2226 }
2227
2228
2229 static void softbody_calc_forces(Scene *scene, Object *ob, float forcetime, float timenow)
2230 {
2231         /* redirection to the new threaded Version */
2232         if (!(G.debug_value & 0x10)) { // 16
2233                 softbody_calc_forcesEx(scene, ob, forcetime, timenow);
2234                 return;
2235         }
2236         else {
2237                 /* so the following will die  */
2238                 /* |||||||||||||||||||||||||| */
2239                 /* VVVVVVVVVVVVVVVVVVVVVVVVVV */
2240                 /*backward compatibility note:
2241                 fixing bug [17428] which forces adaptive step size to tiny steps
2242                 in some situations
2243                 .. keeping G.debug_value==17 0x11 option for old files 'needing' the bug*/
2244
2245                 /* rule we never alter free variables :bp->vec bp->pos in here !
2246                  * this will ruin adaptive stepsize AKA heun! (BM)
2247                 */
2248                 SoftBody *sb= ob->soft; /* is supposed to be there */
2249                 BodyPoint  *bp;
2250                 /* BodyPoint *bproot; */ /* UNUSED */
2251                 BodySpring *bs;
2252                 ListBase *do_effector = NULL;
2253                 float iks, ks, kd, gravity[3] = {0.0f, 0.0f, 0.0f};
2254                 float fieldfactor = -1.0f, windfactor  = 0.25f;
2255                 float tune = sb->ballstiff;
2256                 int do_deflector, do_selfcollision, do_springcollision, do_aero;
2257
2258                 if (scene->physics_settings.flag & PHYS_GLOBAL_GRAVITY) {
2259                         copy_v3_v3(gravity, scene->physics_settings.gravity);
2260                         mul_v3_fl(gravity, sb_grav_force_scale(ob)*sb->effector_weights->global_gravity);
2261                 }
2262
2263                 /* check conditions for various options */
2264                 do_deflector= query_external_colliders(scene, ob);
2265                 do_selfcollision=((ob->softflag & OB_SB_EDGES) && (sb->bspring)&& (ob->softflag & OB_SB_SELF));
2266                 do_springcollision=do_deflector && (ob->softflag & OB_SB_EDGES) &&(ob->softflag & OB_SB_EDGECOLL);
2267                 do_aero=((sb->aeroedge)&& (ob->softflag & OB_SB_EDGES));
2268
2269                 iks  = 1.0f/(1.0f-sb->inspring)-1.0f ;/* inner spring constants function */
2270                 /* bproot= sb->bpoint; */ /* need this for proper spring addressing */ /* UNUSED */
2271
2272                 if (do_springcollision || do_aero)  scan_for_ext_spring_forces(scene, ob, timenow);
2273                 /* after spring scan because it uses Effoctors too */
2274                 do_effector= pdInitEffectors(scene, ob, ob->soft->effector_weights, true);
2275
2276                 if (do_deflector) {
2277                         float defforce[3];
2278                         do_deflector = sb_detect_aabb_collisionCached(defforce, ob->lay, ob, timenow);
2279                 }
2280
2281                 bp = sb->bpoint;
2282                 for (int a = sb->totpoint; a > 0; a--, bp++) {
2283                         /* clear forces  accumulator */
2284                         bp->force[0] = bp->force[1] = bp->force[2] = 0.0;
2285
2286                         /* naive ball self collision */
2287                         /* needs to be done if goal snaps or not */
2288                         if (do_selfcollision) {
2289                                 int attached;
2290                                 BodyPoint   *obp;
2291                                 int c, b;
2292                                 float velcenter[3], dvel[3], def[3];
2293                                 float distance;
2294                                 float compare;
2295
2296                                 for (c=sb->totpoint, obp= sb->bpoint; c>=a; c--, obp++) {
2297
2298                                         //if ((bp->octantflag & obp->octantflag) == 0) continue;
2299
2300                                         compare = (obp->colball + bp->colball);
2301                                         sub_v3_v3v3(def, bp->pos, obp->pos);
2302
2303                                         /* rather check the AABBoxes before ever calulating the real distance */
2304                                         /* mathematically it is completely nuts, but performance is pretty much (3) times faster */
2305                                         if ((ABS(def[0]) > compare) || (ABS(def[1]) > compare) || (ABS(def[2]) > compare)) continue;
2306
2307                                         distance = normalize_v3(def);
2308                                         if (distance < compare ) {
2309                                                 /* exclude body points attached with a spring */
2310                                                 attached = 0;
2311                                                 for (b=obp->nofsprings;b>0;b--) {
2312                                                         bs = sb->bspring + obp->springs[b-1];
2313                                                         if (( sb->totpoint-a == bs->v2) || ( sb->totpoint-a == bs->v1)) {
2314                                                                 attached=1;
2315                                                                 continue;}
2316                                                 }
2317                                                 if (!attached) {
2318                                                         float f = tune / (distance) + tune / (compare * compare) * distance - 2.0f * tune/compare;
2319
2320                                                         mid_v3_v3v3(velcenter, bp->vec, obp->vec);
2321                                                         sub_v3_v3v3(dvel, velcenter, bp->vec);
2322                                                         mul_v3_fl(dvel, _final_mass(ob, bp));
2323
2324                                                         madd_v3_v3fl(bp->force, def, f * (1.0f - sb->balldamp));
2325                                                         madd_v3_v3fl(bp->force, dvel, sb->balldamp);
2326
2327                                                         /* exploit force(a, b) == -force(b, a) part2/2 */
2328                                                         sub_v3_v3v3(dvel, velcenter, obp->vec);
2329                                                         mul_v3_fl(dvel, (_final_mass(ob, bp)+_final_mass(ob, obp))/2.0f);
2330
2331                                                         madd_v3_v3fl(obp->force, dvel, sb->balldamp);
2332                                                         madd_v3_v3fl(obp->force, def, -f * (1.0f - sb->balldamp));
2333                                                 }
2334                                         }
2335                                 }
2336                         }
2337                         /* naive ball self collision done */
2338
2339                         if (_final_goal(ob, bp) < SOFTGOALSNAP) {  /* omit this bp when it snaps */
2340                                 float auxvect[3];
2341                                 float velgoal[3];
2342
2343                                 /* do goal stuff */
2344                                 if (ob->softflag & OB_SB_GOAL) {
2345                                         /* true elastic goal */
2346                                         sub_v3_v3v3(auxvect, bp->pos, bp->origT);
2347                                         ks  = 1.0f / (1.0f- _final_goal(ob, bp) * sb->goalspring) - 1.0f;
2348                                         bp->force[0]+= -ks*(auxvect[0]);
2349                                         bp->force[1]+= -ks*(auxvect[1]);
2350                                         bp->force[2]+= -ks*(auxvect[2]);
2351
2352                                         /* calulate damping forces generated by goals*/
2353                                         sub_v3_v3v3(velgoal, bp->origS, bp->origE);
2354                                         kd = sb->goalfrict * sb_fric_force_scale(ob);
2355                                         add_v3_v3v3(auxvect, velgoal, bp->vec);
2356
2357                                         if (forcetime > 0.0f) { /* make sure friction does not become rocket motor on time reversal */
2358                                                 bp->force[0]-= kd * (auxvect[0]);
2359                                                 bp->force[1]-= kd * (auxvect[1]);
2360                                                 bp->force[2]-= kd * (auxvect[2]);
2361
2362                                         }
2363                                         else {
2364                                                 bp->force[0]-= kd * (velgoal[0] - bp->vec[0]);
2365                                                 bp->force[1]-= kd * (velgoal[1] - bp->vec[1]);
2366                                                 bp->force[2]-= kd * (velgoal[2] - bp->vec[2]);
2367                                         }
2368                                 }
2369                                 /* done goal stuff */
2370
2371
2372                                 /* gravitation */
2373                                 madd_v3_v3fl(bp->force, gravity, _final_mass(ob, bp)); /* individual mass of node here */
2374
2375
2376                                 /* particle field & vortex */
2377                                 if (do_effector) {
2378                                         EffectedPoint epoint;
2379                                         float force[3] = {0.0f, 0.0f, 0.0f};
2380                                         float speed[3] = {0.0f, 0.0f, 0.0f};
2381                                         float eval_sb_fric_force_scale = sb_fric_force_scale(ob); /* just for calling function once */
2382                                         pd_point_from_soft(scene, bp->pos, bp->vec, sb->bpoint-bp, &epoint);
2383                                         pdDoEffectors(do_effector, NULL, sb->effector_weights, &epoint, force, speed);
2384
2385                                         /* apply forcefield*/
2386                                         mul_v3_fl(force, fieldfactor* eval_sb_fric_force_scale);
2387                                         add_v3_v3(bp->force, force);
2388
2389                                         /* BP friction in moving media */
2390                                         kd= sb->mediafrict* eval_sb_fric_force_scale;
2391                                         bp->force[0] -= kd * (bp->vec[0] + windfactor*speed[0]/eval_sb_fric_force_scale);
2392                                         bp->force[1] -= kd * (bp->vec[1] + windfactor*speed[1]/eval_sb_fric_force_scale);
2393                                         bp->force[2] -= kd * (bp->vec[2] + windfactor*speed[2]/eval_sb_fric_force_scale);
2394                                         /* now we'll have nice centrifugal effect for vortex */
2395
2396                                 }
2397                                 else {
2398                                         /* BP friction in media (not) moving*/
2399                                         kd= sb->mediafrict* sb_fric_force_scale(ob);
2400                                         /* assume it to be proportional to actual velocity */
2401                                         bp->force[0]-= bp->vec[0]*kd;
2402                                         bp->force[1]-= bp->vec[1]*kd;
2403                                         bp->force[2]-= bp->vec[2]*kd;
2404                                         /* friction in media done */
2405                                 }
2406                                 /* +++cached collision targets */
2407                                 bp->choke = 0.0f;
2408                                 bp->choke2 = 0.0f;
2409                                 bp->loc_flag &= ~SBF_DOFUZZY;
2410                                 if (do_deflector) {
2411                                         float cfforce[3], defforce[3] ={0.0f, 0.0f, 0.0f}, vel[3] = {0.0f, 0.0f, 0.0f}, facenormal[3], cf = 1.0f, intrusion;
2412                                         kd = 1.0f;
2413
2414                                         if (sb_deflect_face(ob, bp->pos, facenormal, defforce, &cf, timenow, vel, &intrusion)) {
2415                                                 if (intrusion < 0.0f) {
2416                                                         if (G.debug_value & 0x01) { // 17 we did check for bit 0x10 before
2417                                                                 /*fixing bug [17428] this forces adaptive step size to tiny steps
2418                                                                 in some situations .. keeping G.debug_value==17 option for old files 'needing' the bug
2419                                                                 */
2420                                                                 /*bjornmose:  uugh.. what an evil hack
2421                                                                 violation of the 'don't touch bp->pos in here' rule
2422                                                                 but works nice, like this-->
2423                                                                 we predict the solution being out of the collider
2424                                                                 in heun step No1 and leave the heun step No2 adapt to it
2425                                                                 so we kind of introduced a implicit solver for this case
2426                                                                 */
2427                                                                 madd_v3_v3fl(bp->pos, facenormal, -intrusion);
2428                                                         }
2429                                                         else {
2430
2431                                                                 sub_v3_v3v3(cfforce, bp->vec, vel);
2432                                                                 madd_v3_v3fl(bp->force, cfforce, -cf * 50.0f);
2433                                                         }
2434
2435
2436                                                         sb->scratch->flag |= SBF_DOFUZZY;
2437                                                         bp->loc_flag |= SBF_DOFUZZY;
2438                                                         bp->choke = sb->choke*0.01f;
2439                                                 }
2440                                                 else {
2441                                                         sub_v3_v3v3(cfforce, bp->vec, vel);
2442                                                         madd_v3_v3fl(bp->force, cfforce, -cf * 50.0f);
2443                                                 }
2444                                                 madd_v3_v3fl(bp->force, defforce, kd);
2445
2446                                         }
2447
2448                                 }
2449                                 /* ---cached collision targets */
2450
2451                                 /* +++springs */
2452                                 if (ob->softflag & OB_SB_EDGES) {
2453                                         if (sb->bspring) { /* spring list exists at all ? */
2454                                                 for (int b = bp->nofsprings; b > 0; b--) {
2455                                                         bs = sb->bspring + bp->springs[b-1];
2456                                                         if (do_springcollision || do_aero) {
2457                                                                 add_v3_v3(bp->force, bs->ext_force);
2458                                                                 if (bs->flag & BSF_INTERSECT)
2459                                                                         bp->choke = bs->cf;
2460
2461                                                         }
2462                                                         // sb_spring_force(Object *ob, int bpi, BodySpring *bs, float iks, float forcetime)
2463                                                         // rather remove nl_falgs from code .. will make things a lot cleaner
2464                                                         sb_spring_force(ob, sb->totpoint-a, bs, iks, forcetime);
2465                                                 }/* loop springs */
2466                                         }/* existing spring list */
2467                                 }/*any edges*/
2468                                 /* ---springs */
2469                         }/*omit on snap */
2470                 }/*loop all bp's*/
2471
2472
2473                 /* finally add forces caused by face collision */
2474                 if (ob->softflag & OB_SB_FACECOLL) scan_for_ext_face_forces(ob, timenow);
2475                 pdEndEffectors(&do_effector);
2476         }
2477 }
2478
2479 static void softbody_apply_forces(Object *ob, float forcetime, int mode, float *err, int mid_flags)
2480 {
2481         /* time evolution */
2482         /* actually does an explicit euler step mode == 0 */
2483         /* or heun ~ 2nd order runge-kutta steps, mode 1, 2 */
2484         SoftBody *sb= ob->soft; /* is supposed to be there */
2485         BodyPoint *bp;
2486         float dx[3] = {0}, dv[3], aabbmin[3], aabbmax[3], cm[3] = {0.0f, 0.0f, 0.0f};
2487         float timeovermass/*, freezeloc=0.00001f, freezeforce=0.00000000001f*/;
2488         float maxerrpos= 0.0f, maxerrvel = 0.0f;
2489         int a, fuzzy=0;
2490
2491         forcetime *= sb_time_scale(ob);
2492
2493         aabbmin[0]=aabbmin[1]=aabbmin[2] = 1e20f;
2494         aabbmax[0]=aabbmax[1]=aabbmax[2] = -1e20f;
2495
2496         /* old one with homogeneous masses  */
2497         /* claim a minimum mass for vertex */
2498         /*
2499         if (sb->nodemass > 0.009999f) timeovermass = forcetime/sb->nodemass;
2500         else timeovermass = forcetime/0.009999f;
2501         */
2502
2503         for (a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
2504 /* now we have individual masses   */
2505 /* claim a minimum mass for vertex */
2506                 if (_final_mass(ob, bp) > 0.009999f) timeovermass = forcetime/_final_mass(ob, bp);
2507                 else timeovermass = forcetime/0.009999f;
2508
2509
2510                 if (_final_goal(ob, bp) < SOFTGOALSNAP) {
2511                         /* this makes t~ = t */
2512                         if (mid_flags & MID_PRESERVE) copy_v3_v3(dx, bp->vec);
2513
2514                         /* so here is (v)' = a(cceleration) = sum(F_springs)/m + gravitation + some friction forces  + more forces*/
2515                         /* the ( ... )' operator denotes derivate respective time */
2516                         /* the euler step for velocity then becomes */
2517                         /* v(t + dt) = v(t) + a(t) * dt */
2518                         mul_v3_fl(bp->force, timeovermass);/* individual mass of node here */
2519                         /* some nasty if's to have heun in here too */
2520                         copy_v3_v3(dv, bp->force);
2521
2522                         if (mode == 1) {
2523                                 copy_v3_v3(bp->prevvec, bp->vec);
2524                                 copy_v3_v3(bp->prevdv, dv);
2525                         }
2526
2527                         if (mode ==2) {
2528                                 /* be optimistic and execute step */
2529                                 bp->vec[0] = bp->prevvec[0] + 0.5f * (dv[0] + bp->prevdv[0]);
2530                                 bp->vec[1] = bp->prevvec[1] + 0.5f * (dv[1] + bp->prevdv[1]);
2531                                 bp->vec[2] = bp->prevvec[2] + 0.5f * (dv[2] + bp->prevdv[2]);
2532                                 /* compare euler to heun to estimate error for step sizing */
2533                                 maxerrvel = max_ff(maxerrvel, fabsf(dv[0] - bp->prevdv[0]));
2534                                 maxerrvel = max_ff(maxerrvel, fabsf(dv[1] - bp->prevdv[1]));
2535                                 maxerrvel = max_ff(maxerrvel, fabsf(dv[2] - bp->prevdv[2]));
2536                         }
2537                         else { add_v3_v3(bp->vec, bp->force); }
2538
2539                         /* this makes t~ = t+dt */
2540                         if (!(mid_flags & MID_PRESERVE)) copy_v3_v3(dx, bp->vec);
2541
2542                         /* so here is (x)'= v(elocity) */
2543                         /* the euler step for location then becomes */
2544                         /* x(t + dt) = x(t) + v(t~) * dt */
2545                         mul_v3_fl(dx, forcetime);
2546
2547                         /* the freezer coming sooner or later */
2548 #if 0
2549                         if ((dot_v3v3(dx, dx)<freezeloc )&&(dot_v3v3(bp->force, bp->force)<freezeforce )) {
2550                                 bp->frozen /=2;
2551                         }
2552                         else {
2553                                 bp->frozen = min_ff(bp->frozen*1.05f, 1.0f);
2554                         }
2555                         mul_v3_fl(dx, bp->frozen);
2556 #endif
2557                         /* again some nasty if's to have heun in here too */
2558                         if (mode ==1) {
2559                                 copy_v3_v3(bp->prevpos, bp->pos);
2560                                 copy_v3_v3(bp->prevdx, dx);
2561                         }
2562
2563                         if (mode ==2) {
2564                                 bp->pos[0] = bp->prevpos[0] + 0.5f * ( dx[0] + bp->prevdx[0]);
2565                                 bp->pos[1] = bp->prevpos[1] + 0.5f * ( dx[1] + bp->prevdx[1]);
2566                                 bp->pos[2] = bp->prevpos[2] + 0.5f * ( dx[2] + bp->prevdx[2]);
2567                                 maxerrpos = max_ff(maxerrpos, fabsf(dx[0] - bp->prevdx[0]));
2568                                 maxerrpos = max_ff(maxerrpos, fabsf(dx[1] - bp->prevdx[1]));
2569                                 maxerrpos = max_ff(maxerrpos, fabsf(dx[2] - bp->prevdx[2]));
2570
2571                                 /* bp->choke is set when we need to pull a vertex or edge out of the collider.
2572                                  * the collider object signals to get out by pushing hard. on the other hand
2573                                  * we don't want to end up in deep space so we add some <viscosity>
2574                                  * to balance that out */
2575                                 if (bp->choke2 > 0.0f) {
2576                                         mul_v3_fl(bp->vec, (1.0f - bp->choke2));
2577                                 }
2578                                 if (bp->choke > 0.0f) {
2579                                         mul_v3_fl(bp->vec, (1.0f - bp->choke));
2580                                 }
2581
2582                         }
2583                         else { add_v3_v3(bp->pos, dx);}
2584                 }/*snap*/
2585                 /* so while we are looping BPs anyway do statistics on the fly */
2586                 minmax_v3v3_v3(aabbmin, aabbmax, bp->pos);
2587                 if (bp->loc_flag & SBF_DOFUZZY) fuzzy =1;
2588         } /*for*/
2589
2590         if (sb->totpoint) mul_v3_fl(cm, 1.0f/sb->totpoint);
2591         if (sb->scratch) {
2592                 copy_v3_v3(sb->scratch->aabbmin, aabbmin);
2593                 copy_v3_v3(sb->scratch->aabbmax, aabbmax);
2594         }
2595
2596         if (err) { /* so step size will be controlled by biggest difference in slope */
2597                 if (sb->solverflags & SBSO_OLDERR)
2598                         *err = max_ff(maxerrpos, maxerrvel);
2599                 else
2600                         *err = maxerrpos;
2601                 //printf("EP %f EV %f\n", maxerrpos, maxerrvel);
2602                 if (fuzzy) {
2603                         *err /= sb->fuzzyness;
2604                 }
2605         }
2606 }
2607
2608 /* used by heun when it overshoots */
2609 static void softbody_restore_prev_step(Object *ob)
2610 {
2611         SoftBody *sb= ob->soft; /* is supposed to be there*/
2612         BodyPoint *bp;
2613         int a;
2614
2615         for (a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
2616                 copy_v3_v3(bp->vec, bp->prevvec);
2617                 copy_v3_v3(bp->pos, bp->prevpos);
2618         }
2619 }
2620
2621 #if 0
2622 static void softbody_store_step(Object *ob)
2623 {
2624         SoftBody *sb= ob->soft; /* is supposed to be there*/
2625         BodyPoint *bp;
2626         int a;
2627
2628         for (a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
2629                 copy_v3_v3(bp->prevvec, bp->vec);
2630                 copy_v3_v3(bp->prevpos, bp->pos);
2631         }
2632 }
2633
2634
2635 /* used by predictors and correctors */
2636 static void softbody_store_state(Object *ob, float *ppos, float *pvel)
2637 {
2638         SoftBody *sb= ob->soft; /* is supposed to be there*/
2639         BodyPoint *bp;
2640         int a;
2641         float *pp=ppos, *pv=pvel;
2642
2643         for (a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
2644
2645                 copy_v3_v3(pv, bp->vec);
2646                 pv+=3;
2647
2648                 copy_v3_v3(pp, bp->pos);
2649                 pp+=3;
2650         }
2651 }
2652
2653 /* used by predictors and correctors */
2654 static void softbody_retrieve_state(Object *ob, float *ppos, float *pvel)
2655 {
2656         SoftBody *sb= ob->soft; /* is supposed to be there*/
2657         BodyPoint *bp;
2658         int a;
2659         float *pp=ppos, *pv=pvel;
2660
2661         for (a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
2662
2663                 copy_v3_v3(bp->vec, pv);
2664                 pv+=3;
2665
2666                 copy_v3_v3(bp->pos, pp);
2667                 pp+=3;
2668         }
2669 }
2670
2671 /* used by predictors and correctors */
2672 static void softbody_swap_state(Object *ob, float *ppos, float *pvel)
2673 {
2674         SoftBody *sb= ob->soft; /* is supposed to be there*/
2675         BodyPoint *bp;
2676         int a;
2677         float *pp=ppos, *pv=pvel;
2678         float temp[3];
2679
2680         for (a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
2681
2682                 copy_v3_v3(temp, bp->vec);
2683                 copy_v3_v3(bp->vec, pv);
2684                 copy_v3_v3(pv, temp);
2685                 pv+=3;
2686
2687                 copy_v3_v3(temp, bp->pos);
2688                 copy_v3_v3(bp->pos, pp);
2689                 copy_v3_v3(pp, temp);
2690                 pp+=3;
2691         }
2692 }
2693 #endif
2694
2695
2696 /* care for bodypoints taken out of the 'ordinary' solver step
2697 ** because they are screwed to goal by bolts
2698 ** they just need to move along with the goal in time
2699 ** we need to adjust them on sub frame timing in solver
2700 ** so now when frame is done .. put 'em to the position at the end of frame
2701 */
2702 static void softbody_apply_goalsnap(Object *ob)
2703 {
2704         SoftBody *sb= ob->soft; /* is supposed to be there */
2705         BodyPoint *bp;
2706         int a;
2707
2708         for (a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
2709                 if (_final_goal(ob, bp) >= SOFTGOALSNAP) {
2710                         copy_v3_v3(bp->prevpos, bp->pos);
2711                         copy_v3_v3(bp->pos, bp->origT);
2712                 }
2713         }
2714 }
2715
2716
2717 static void apply_spring_memory(Object *ob)
2718 {
2719         SoftBody *sb = ob->soft;
2720         BodySpring *bs;
2721         BodyPoint *bp1, *bp2;
2722         int a;
2723         float b, l, r;
2724
2725         if (sb && sb->totspring) {
2726                 b = sb->plastic;
2727                 for (a=0; a<sb->totspring; a++) {
2728                         bs  = &sb->bspring[a];
2729                         bp1 =&sb->bpoint[bs->v1];
2730                         bp2 =&sb->bpoint[bs->v2];
2731                         l = len_v3v3(bp1->pos, bp2->pos);
2732                         r = bs->len/l;
2733                         if (( r > 1.05f) || (r < 0.95f)) {
2734                         bs->len = ((100.0f - b) * bs->len  + b*l)/100.0f;
2735                         }
2736                 }
2737         }
2738 }
2739
2740 /* expects full initialized softbody */
2741 static void interpolate_exciter(Object *ob, int timescale, int time)
2742 {
2743         SoftBody *sb= ob->soft;
2744         BodyPoint *bp;
2745         float f;
2746         int a;
2747
2748         f = (float)time/(float)timescale;
2749
2750         for (a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
2751                 bp->origT[0] = bp->origS[0] + f*(bp->origE[0] - bp->origS[0]);
2752                 bp->origT[1] = bp->origS[1] + f*(bp->origE[1] - bp->origS[1]);
2753                 bp->origT[2] = bp->origS[2] + f*(bp->origE[2] - bp->origS[2]);
2754                 if (_final_goal(ob, bp) >= SOFTGOALSNAP) {
2755                         bp->vec[0] = bp->origE[0] - bp->origS[0];
2756                         bp->vec[1] = bp->origE[1] - bp->origS[1];
2757                         bp->vec[2] = bp->origE[2] - bp->origS[2];
2758                 }
2759         }
2760
2761 }
2762
2763
2764 /* ************ convertors ********** */
2765
2766 /*  for each object type we need;
2767         - xxxx_to_softbody(Object *ob)      : a full (new) copy, creates SB geometry
2768 */
2769
2770 /* Resetting a Mesh SB object's springs */
2771 /* Spring length are caculted from'raw' mesh vertices that are NOT altered by modifier stack. */
2772 static void springs_from_mesh(Object *ob)
2773 {
2774         SoftBody *sb;
2775         Mesh *me= ob->data;
2776         BodyPoint *bp;
2777         int a;
2778         float scale =1.0f;
2779
2780         sb= ob->soft;
2781         if (me && sb) {
2782         /* using bp->origS as a container for spring calcualtions here
2783          * will be overwritten sbObjectStep() to receive
2784          * actual modifier stack positions
2785          */
2786                 if (me->totvert) {
2787                         bp= ob->soft->bpoint;
2788                         for (a=0; a<me->totvert; a++, bp++) {
2789                                 copy_v3_v3(bp->origS, me->mvert[a].co);
2790                                 mul_m4_v3(ob->obmat, bp->origS);
2791                         }
2792
2793                 }
2794                 /* recalculate spring length for meshes here */
2795                 /* public version shrink to fit */
2796                 if (sb->springpreload != 0 ) {
2797                         scale = sb->springpreload / 100.0f;
2798                 }
2799                 for (a=0; a<sb->totspring; a++) {
2800                         BodySpring *bs = &sb->bspring[a];
2801                         bs->len= scale*len_v3v3(sb->bpoint[bs->v1].origS, sb->bpoint[bs->v2].origS);
2802                 }
2803         }
2804 }
2805
2806
2807
2808
2809 /* makes totally fresh start situation */
2810 static void mesh_to_softbody(Scene *scene, Object *ob)
2811 {
2812         SoftBody *sb;
2813         Mesh *me= ob->data;
2814         MEdge *medge= me->medge;
2815         BodyPoint *bp;
2816         BodySpring *bs;
2817         int a, totedge;
2818         int defgroup_index, defgroup_index_mass, defgroup_index_spring;
2819         
2820         if (ob->softflag & OB_SB_EDGES) totedge= me->totedge;
2821         else totedge= 0;
2822
2823         /* renew ends with ob->soft with points and edges, also checks & makes ob->soft */
2824         renew_softbody(scene, ob, me->totvert, totedge);
2825
2826         /* we always make body points */
2827         sb = ob->soft;
2828         bp= sb->bpoint;
2829
2830         defgroup_index        = me->dvert ? (sb->vertgroup - 1) : -1;
2831         defgroup_index_mass   = me->dvert ? defgroup_name_index(ob, sb->namedVG_Mass) : -1;
2832         defgroup_index_spring = me->dvert ? defgroup_name_index(ob, sb->namedVG_Spring_K) : -1;
2833
2834         for (a=0; a<me->totvert; a++, bp++) {
2835                 /* get scalar values needed  *per vertex* from vertex group functions,
2836