OpenNL: removed unused softbody code.
[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
67 #include "BLI_math.h"
68 #include "BLI_utildefines.h"
69 #include "BLI_listbase.h"
70 #include "BLI_ghash.h"
71 #include "BLI_threads.h"
72
73 #include "BKE_curve.h"
74 #include "BKE_effect.h"
75 #include "BKE_global.h"
76 #include "BKE_modifier.h"
77 #include "BKE_softbody.h"
78 #include "BKE_pointcache.h"
79 #include "BKE_deform.h"
80 #include "BKE_mesh.h"
81 #include "BKE_scene.h"
82
83 #include  "PIL_time.h"
84 // #include  "ONL_opennl.h" remove linking to ONL for now
85
86 /* callbacks for errors and interrupts and some goo */
87 static int (*SB_localInterruptCallBack)(void) = NULL;
88
89
90 /* ********** soft body engine ******* */
91
92 typedef enum {SB_EDGE=1, SB_BEND=2, SB_STIFFQUAD=3, SB_HANDLE=4} type_spring;
93
94 typedef struct BodySpring {
95         int v1, v2;
96         float len, cf, load;
97         float ext_force[3]; /* edges colliding and sailing */
98         type_spring springtype;
99         short flag;
100 } BodySpring;
101
102 typedef struct BodyFace {
103         int v1, v2, v3;
104         float ext_force[3]; /* faces colliding */
105         short flag;
106 } BodyFace;
107
108 typedef struct ReferenceVert {
109         float pos[3]; /* position relative to com */
110         float mass;   /* node mass */
111 } ReferenceVert;
112
113 typedef struct ReferenceState {
114         float com[3]; /* center of mass*/
115         ReferenceVert *ivert; /* list of intial values */
116 } ReferenceState;
117
118
119 /*private scratch pad for caching and other data only needed when alive*/
120 typedef struct SBScratch {
121         GHash *colliderhash;
122         short needstobuildcollider;
123         short flag;
124         BodyFace *bodyface;
125         int totface;
126         float aabbmin[3], aabbmax[3];
127         ReferenceState Ref;
128 } SBScratch;
129
130 typedef struct  SB_thread_context {
131                 Scene *scene;
132                 Object *ob;
133                 float forcetime;
134                 float timenow;
135                 int ifirst;
136                 int ilast;
137                 ListBase *do_effector;
138                 int do_deflector;
139                 float fieldfactor;
140                 float windfactor;
141                 int nr;
142                 int tot;
143 } SB_thread_context;
144
145 #define MID_PRESERVE 1
146
147 #define SOFTGOALSNAP  0.999f
148 /* if bp-> goal is above make it a *forced follow original* and skip all ODE stuff for this bp
149  * removes *unnecessary* stiffness from ODE system
150  */
151 #define HEUNWARNLIMIT 1 /* 500 would be fine i think for detecting severe *stiff* stuff */
152
153
154 #define BSF_INTERSECT   1 /* edge intersects collider face */
155
156 /* private definitions for bodypoint states */
157 #define SBF_DOFUZZY          1 /* Bodypoint do fuzzy */
158 #define SBF_OUTOFCOLLISION   2 /* Bodypoint does not collide  */
159
160
161 #define BFF_INTERSECT   1 /* collider edge   intrudes face */
162 #define BFF_CLOSEVERT   2 /* collider vertex repulses face */
163
164
165 static float SoftHeunTol = 1.0f; /* humm .. this should be calculated from sb parameters and sizes */
166
167 /* local prototypes */
168 static void free_softbody_intern(SoftBody *sb);
169
170 /*+++ frame based timing +++*/
171
172 /*physical unit of force is [kg * m / sec^2]*/
173
174 static float sb_grav_force_scale(Object *UNUSED(ob))
175 /* since unit of g is [m/sec^2] and F = mass * g we rescale unit mass of node to 1 gramm
176  * put it to a function here, so we can add user options later without touching simulation code
177  */
178 {
179         return (0.001f);
180 }
181
182 static float sb_fric_force_scale(Object *UNUSED(ob))
183 /* rescaling unit of drag [1 / sec] to somehow reasonable
184  * put it to a function here, so we can add user options later without touching simulation code
185  */
186 {
187         return (0.01f);
188 }
189
190 static float sb_time_scale(Object *ob)
191 /* defining the frames to *real* time relation */
192 {
193         SoftBody *sb= ob->soft; /* is supposed to be there */
194         if (sb) {
195                 return(sb->physics_speed);
196                 /* hrms .. this could be IPO as well :)
197                  * estimated range [0.001 sluggish slug - 100.0 very fast (i hope ODE solver can handle that)]
198                  * 1 approx = a unit 1 pendulum at g = 9.8 [earth conditions]  has period 65 frames
199                  * theory would give a 50 frames period .. so there must be something inaccurate .. looking for that (BM)
200                  */
201         }
202         return (1.0f);
203         /*
204          * this would be frames/sec independent timing assuming 25 fps is default
205          * but does not work very well with NLA
206          * return (25.0f/scene->r.frs_sec)
207          */
208 }
209 /*--- frame based timing ---*/
210
211 /* helper functions for everything is animatable jow_go_for2_5 +++++++*/
212 /* introducing them here, because i know: steps in properties  ( at frame timing )
213  * will cause unwanted responses of the softbody system (which does inter frame calculations )
214  * so first 'cure' would be: interpolate linear in time ..
215  * Q: why do i write this?
216  * A: because it happend once, that some eger coder 'streamlined' code to fail.
217  * We DO linear interpolation for goals .. and i think we should do on animated properties as well
218  */
219
220 /* animate sb->maxgoal, sb->mingoal */
221 static float _final_goal(Object *ob, BodyPoint *bp)/*jow_go_for2_5 */
222 {
223         float f = -1999.99f;
224         if (ob) {
225                 SoftBody *sb= ob->soft; /* is supposed to be there */
226                 if (!(ob->softflag & OB_SB_GOAL)) return (0.0f);
227                 if (sb&&bp) {
228                         if (bp->goal < 0.0f) return (0.0f);
229                         f = sb->mingoal + bp->goal * fabsf(sb->maxgoal - sb->mingoal);
230                         f = pow(f, 4.0f);
231                         return (f);
232                 }
233         }
234         printf("_final_goal failed! sb or bp ==NULL\n");
235         return f; /*using crude but spot able values some times helps debuggin */
236 }
237
238 static float _final_mass(Object *ob, BodyPoint *bp)
239 {
240         if (ob) {
241                 SoftBody *sb= ob->soft; /* is supposed to be there */
242                 if (sb&&bp) {
243                         return(bp->mass*sb->nodemass);
244                 }
245         }
246         printf("_final_mass failed! sb or bp ==NULL\n");
247         return 1.0f;
248 }
249 /* helper functions for everything is animateble jow_go_for2_5 ------*/
250
251 /*+++ collider caching and dicing +++*/
252
253 /********************
254 for each target object/face the axis aligned bounding box (AABB) is stored
255 faces parallel to global axes
256 so only simple "value" in [min, max] ckecks are used
257 float operations still
258 */
259
260 /* just an ID here to reduce the prob for killing objects
261 ** ob->sumohandle points to we should not kill :)
262 */
263 static const int CCD_SAVETY = 190561;
264
265 typedef struct ccdf_minmax {
266         float minx, miny, minz, maxx, maxy, maxz;
267 } ccdf_minmax;
268
269
270 typedef struct ccd_Mesh {
271         int mvert_num, tri_num;
272         const MVert *mvert;
273         const MVert *mprevvert;
274         const MVertTri *tri;
275         int savety;
276         ccdf_minmax *mima;
277         /* Axis Aligned Bounding Box AABB */
278         float bbmin[3];
279         float bbmax[3];
280 } ccd_Mesh;
281
282
283 static ccd_Mesh *ccd_mesh_make(Object *ob)
284 {
285         CollisionModifierData *cmd;
286         ccd_Mesh *pccd_M = NULL;
287         ccdf_minmax *mima;
288         const MVertTri *vt;
289         float hull;
290         int i;
291
292         cmd =(CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
293
294         /* first some paranoia checks */
295         if (!cmd) return NULL;
296         if (!cmd->mvert_num || !cmd->tri_num) return NULL;
297
298         pccd_M = MEM_mallocN(sizeof(ccd_Mesh), "ccd_Mesh");
299         pccd_M->mvert_num = cmd->mvert_num;
300         pccd_M->tri_num = cmd->tri_num;
301         pccd_M->savety  = CCD_SAVETY;
302         pccd_M->bbmin[0]=pccd_M->bbmin[1]=pccd_M->bbmin[2]=1e30f;
303         pccd_M->bbmax[0]=pccd_M->bbmax[1]=pccd_M->bbmax[2]=-1e30f;
304         pccd_M->mprevvert=NULL;
305
306         /* blow it up with forcefield ranges */
307         hull = max_ff(ob->pd->pdef_sbift, ob->pd->pdef_sboft);
308
309         /* alloc and copy verts*/
310         pccd_M->mvert = MEM_dupallocN(cmd->xnew);
311         /* note that xnew coords are already in global space, */
312         /* determine the ortho BB */
313         for (i = 0; i < pccd_M->mvert_num; i++) {
314                 const float *v;
315
316                 /* evaluate limits */
317                 v = pccd_M->mvert[i].co;
318                 pccd_M->bbmin[0] = min_ff(pccd_M->bbmin[0], v[0] - hull);
319                 pccd_M->bbmin[1] = min_ff(pccd_M->bbmin[1], v[1] - hull);
320                 pccd_M->bbmin[2] = min_ff(pccd_M->bbmin[2], v[2] - hull);
321
322                 pccd_M->bbmax[0] = max_ff(pccd_M->bbmax[0], v[0] + hull);
323                 pccd_M->bbmax[1] = max_ff(pccd_M->bbmax[1], v[1] + hull);
324                 pccd_M->bbmax[2] = max_ff(pccd_M->bbmax[2], v[2] + hull);
325
326         }
327         /* alloc and copy faces*/
328         pccd_M->tri = MEM_dupallocN(cmd->tri);
329
330         /* OBBs for idea1 */
331         pccd_M->mima = MEM_mallocN(sizeof(ccdf_minmax) * pccd_M->tri_num, "ccd_Mesh_Faces_mima");
332
333
334         /* anyhoo we need to walk the list of faces and find OBB they live in */
335         for (i = 0, mima  = pccd_M->mima, vt = pccd_M->tri; i < pccd_M->tri_num; i++, mima++, vt++) {
336                 const float *v;
337
338                 mima->minx=mima->miny=mima->minz=1e30f;
339                 mima->maxx=mima->maxy=mima->maxz=-1e30f;
340
341                 v = pccd_M->mvert[vt->tri[0]].co;
342                 mima->minx = min_ff(mima->minx, v[0] - hull);
343                 mima->miny = min_ff(mima->miny, v[1] - hull);
344                 mima->minz = min_ff(mima->minz, v[2] - hull);
345                 mima->maxx = max_ff(mima->maxx, v[0] + hull);
346                 mima->maxy = max_ff(mima->maxy, v[1] + hull);
347                 mima->maxz = max_ff(mima->maxz, v[2] + hull);
348
349                 v = pccd_M->mvert[vt->tri[1]].co;
350                 mima->minx = min_ff(mima->minx, v[0] - hull);
351                 mima->miny = min_ff(mima->miny, v[1] - hull);
352                 mima->minz = min_ff(mima->minz, v[2] - hull);
353                 mima->maxx = max_ff(mima->maxx, v[0] + hull);
354                 mima->maxy = max_ff(mima->maxy, v[1] + hull);
355                 mima->maxz = max_ff(mima->maxz, v[2] + hull);
356
357                 v = pccd_M->mvert[vt->tri[2]].co;
358                 mima->minx = min_ff(mima->minx, v[0] - hull);
359                 mima->miny = min_ff(mima->miny, v[1] - hull);
360                 mima->minz = min_ff(mima->minz, v[2] - hull);
361                 mima->maxx = max_ff(mima->maxx, v[0] + hull);
362                 mima->maxy = max_ff(mima->maxy, v[1] + hull);
363                 mima->maxz = max_ff(mima->maxz, v[2] + hull);
364         }
365
366         return pccd_M;
367 }
368 static void ccd_mesh_update(Object *ob, ccd_Mesh *pccd_M)
369 {
370         CollisionModifierData *cmd;
371         ccdf_minmax *mima;
372         const MVertTri *vt;
373         float hull;
374         int i;
375
376         cmd =(CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
377
378         /* first some paranoia checks */
379         if (!cmd) return;
380         if (!cmd->mvert_num || !cmd->tri_num) return;
381
382         if ((pccd_M->mvert_num != cmd->mvert_num) ||
383             (pccd_M->tri_num != cmd->tri_num))
384         {
385                 return;
386         }
387
388         pccd_M->bbmin[0]=pccd_M->bbmin[1]=pccd_M->bbmin[2]=1e30f;
389         pccd_M->bbmax[0]=pccd_M->bbmax[1]=pccd_M->bbmax[2]=-1e30f;
390
391
392         /* blow it up with forcefield ranges */
393         hull = max_ff(ob->pd->pdef_sbift, ob->pd->pdef_sboft);
394
395         /* rotate current to previous */
396         if (pccd_M->mprevvert) MEM_freeN((void *)pccd_M->mprevvert);
397         pccd_M->mprevvert = pccd_M->mvert;
398         /* alloc and copy verts*/
399         pccd_M->mvert = MEM_dupallocN(cmd->xnew);
400         /* note that xnew coords are already in global space, */
401         /* determine the ortho BB */
402         for (i=0; i < pccd_M->mvert_num; i++) {
403                 const float *v;
404
405                 /* evaluate limits */
406                 v = pccd_M->mvert[i].co;
407                 pccd_M->bbmin[0] = min_ff(pccd_M->bbmin[0], v[0] - hull);
408                 pccd_M->bbmin[1] = min_ff(pccd_M->bbmin[1], v[1] - hull);
409                 pccd_M->bbmin[2] = min_ff(pccd_M->bbmin[2], v[2] - hull);
410
411                 pccd_M->bbmax[0] = max_ff(pccd_M->bbmax[0], v[0] + hull);
412                 pccd_M->bbmax[1] = max_ff(pccd_M->bbmax[1], v[1] + hull);
413                 pccd_M->bbmax[2] = max_ff(pccd_M->bbmax[2], v[2] + hull);
414
415                 /* evaluate limits */
416                 v = pccd_M->mprevvert[i].co;
417                 pccd_M->bbmin[0] = min_ff(pccd_M->bbmin[0], v[0] - hull);
418                 pccd_M->bbmin[1] = min_ff(pccd_M->bbmin[1], v[1] - hull);
419                 pccd_M->bbmin[2] = min_ff(pccd_M->bbmin[2], v[2] - hull);
420
421                 pccd_M->bbmax[0] = max_ff(pccd_M->bbmax[0], v[0] + hull);
422                 pccd_M->bbmax[1] = max_ff(pccd_M->bbmax[1], v[1] + hull);
423                 pccd_M->bbmax[2] = max_ff(pccd_M->bbmax[2], v[2] + hull);
424
425         }
426
427         /* anyhoo we need to walk the list of faces and find OBB they live in */
428         for (i = 0, mima  = pccd_M->mima, vt = pccd_M->tri; i < pccd_M->tri_num; i++, mima++, vt++) {
429                 const float *v;
430
431                 mima->minx=mima->miny=mima->minz=1e30f;
432                 mima->maxx=mima->maxy=mima->maxz=-1e30f;
433
434                 /* mvert */
435                 v = pccd_M->mvert[vt->tri[0]].co;
436                 mima->minx = min_ff(mima->minx, v[0] - hull);
437                 mima->miny = min_ff(mima->miny, v[1] - hull);
438                 mima->minz = min_ff(mima->minz, v[2] - hull);
439                 mima->maxx = max_ff(mima->maxx, v[0] + hull);
440                 mima->maxy = max_ff(mima->maxy, v[1] + hull);
441                 mima->maxz = max_ff(mima->maxz, v[2] + hull);
442
443                 v = pccd_M->mvert[vt->tri[1]].co;
444                 mima->minx = min_ff(mima->minx, v[0] - hull);
445                 mima->miny = min_ff(mima->miny, v[1] - hull);
446                 mima->minz = min_ff(mima->minz, v[2] - hull);
447                 mima->maxx = max_ff(mima->maxx, v[0] + hull);
448                 mima->maxy = max_ff(mima->maxy, v[1] + hull);
449                 mima->maxz = max_ff(mima->maxz, v[2] + hull);
450
451                 v = pccd_M->mvert[vt->tri[2]].co;
452                 mima->minx = min_ff(mima->minx, v[0] - hull);
453                 mima->miny = min_ff(mima->miny, v[1] - hull);
454                 mima->minz = min_ff(mima->minz, v[2] - hull);
455                 mima->maxx = max_ff(mima->maxx, v[0] + hull);
456                 mima->maxy = max_ff(mima->maxy, v[1] + hull);
457                 mima->maxz = max_ff(mima->maxz, v[2] + hull);
458
459
460                 /* mprevvert */
461                 v = pccd_M->mprevvert[vt->tri[0]].co;
462                 mima->minx = min_ff(mima->minx, v[0] - hull);
463                 mima->miny = min_ff(mima->miny, v[1] - hull);
464                 mima->minz = min_ff(mima->minz, v[2] - hull);
465                 mima->maxx = max_ff(mima->maxx, v[0] + hull);
466                 mima->maxy = max_ff(mima->maxy, v[1] + hull);
467                 mima->maxz = max_ff(mima->maxz, v[2] + hull);
468
469                 v = pccd_M->mprevvert[vt->tri[1]].co;
470                 mima->minx = min_ff(mima->minx, v[0] - hull);
471                 mima->miny = min_ff(mima->miny, v[1] - hull);
472                 mima->minz = min_ff(mima->minz, v[2] - hull);
473                 mima->maxx = max_ff(mima->maxx, v[0] + hull);
474                 mima->maxy = max_ff(mima->maxy, v[1] + hull);
475                 mima->maxz = max_ff(mima->maxz, v[2] + hull);
476
477                 v = pccd_M->mprevvert[vt->tri[2]].co;
478                 mima->minx = min_ff(mima->minx, v[0] - hull);
479                 mima->miny = min_ff(mima->miny, v[1] - hull);
480                 mima->minz = min_ff(mima->minz, v[2] - hull);
481                 mima->maxx = max_ff(mima->maxx, v[0] + hull);
482                 mima->maxy = max_ff(mima->maxy, v[1] + hull);
483                 mima->maxz = max_ff(mima->maxz, v[2] + hull);
484         }
485         return;
486 }
487
488 static void ccd_mesh_free(ccd_Mesh *ccdm)
489 {
490         if (ccdm && (ccdm->savety == CCD_SAVETY )) { /*make sure we're not nuking objects we don't know*/
491                 MEM_freeN((void *)ccdm->mvert);
492                 MEM_freeN((void *)ccdm->tri);
493                 if (ccdm->mprevvert) MEM_freeN((void *)ccdm->mprevvert);
494                 MEM_freeN(ccdm->mima);
495                 MEM_freeN(ccdm);
496                 ccdm = NULL;
497         }
498 }
499
500 static void ccd_build_deflector_hash(Scene *scene, Object *vertexowner, GHash *hash)
501 {
502         Base *base= scene->base.first;
503         Object *ob;
504
505         if (!hash) return;
506         while (base) {
507                 /*Only proceed for mesh object in same layer */
508                 if (base->object->type==OB_MESH && (base->lay & vertexowner->lay)) {
509                         ob= base->object;
510                         if ((vertexowner) && (ob == vertexowner)) {
511                                 /* if vertexowner is given  we don't want to check collision with owner object */
512                                 base = base->next;
513                                 continue;
514                         }
515
516                         /*+++ only with deflecting set */
517                         if (ob->pd && ob->pd->deflect && BLI_ghash_lookup(hash, ob) == NULL) {
518                                 ccd_Mesh *ccdmesh = ccd_mesh_make(ob);
519                                 BLI_ghash_insert(hash, ob, ccdmesh);
520                         }/*--- only with deflecting set */
521
522                 }/* mesh && layer*/
523                 base = base->next;
524         } /* while (base) */
525 }
526
527 static void ccd_update_deflector_hash(Scene *scene, Object *vertexowner, GHash *hash)
528 {
529         Base *base= scene->base.first;
530         Object *ob;
531
532         if ((!hash) || (!vertexowner)) return;
533         while (base) {
534                 /*Only proceed for mesh object in same layer */
535                 if (base->object->type==OB_MESH && (base->lay & vertexowner->lay)) {
536                         ob= base->object;
537                         if (ob == vertexowner) {
538                                 /* if vertexowner is given  we don't want to check collision with owner object */
539                                 base = base->next;
540                                 continue;
541                         }
542
543                         /*+++ only with deflecting set */
544                         if (ob->pd && ob->pd->deflect) {
545                                 ccd_Mesh *ccdmesh = BLI_ghash_lookup(hash, ob);
546                                 if (ccdmesh)
547                                         ccd_mesh_update(ob, ccdmesh);
548                         }/*--- only with deflecting set */
549
550                 }/* mesh && layer*/
551                 base = base->next;
552         } /* while (base) */
553 }
554
555
556 /*--- collider caching and dicing ---*/
557
558
559 static int count_mesh_quads(Mesh *me)
560 {
561         int a, result = 0;
562         const MPoly *mp = me->mpoly;
563
564         if (mp) {
565                 for (a = me->totpoly; a > 0; a--, mp++) {
566                         if (mp->totloop == 4) {
567                                 result++;
568                         }
569                 }
570         }
571         return result;
572 }
573
574 static void add_mesh_quad_diag_springs(Object *ob)
575 {
576         Mesh *me= ob->data;
577         /*BodyPoint *bp;*/ /*UNUSED*/
578         int a;
579
580         if (ob->soft) {
581                 int nofquads;
582                 //float s_shear = ob->soft->shearstiff*ob->soft->shearstiff;
583
584                 nofquads = count_mesh_quads(me);
585                 if (nofquads) {
586                         const MLoop *mloop = me->mloop;
587                         const MPoly *mp = me->mpoly;
588                         BodySpring *bs;
589
590                         /* resize spring-array to hold additional quad springs */
591                         ob->soft->bspring = MEM_recallocN(ob->soft->bspring, sizeof(BodySpring) * (ob->soft->totspring + nofquads * 2));
592
593                         /* fill the tail */
594                         a = 0;
595                         bs = &ob->soft->bspring[ob->soft->totspring];
596                         /*bp= ob->soft->bpoint; */ /*UNUSED*/
597                         for (a = me->totpoly; a > 0; a--, mp++) {
598                                 if (mp->totloop == 4) {
599                                         bs->v1 = mloop[mp->loopstart + 0].v;
600                                         bs->v2 = mloop[mp->loopstart + 2].v;
601                                         bs->springtype   = SB_STIFFQUAD;
602                                         bs++;
603                                         bs->v1 = mloop[mp->loopstart + 1].v;
604                                         bs->v2 = mloop[mp->loopstart + 3].v;
605                                         bs->springtype   = SB_STIFFQUAD;
606                                         bs++;
607                                 }
608                         }
609
610                         /* now we can announce new springs */
611                         ob->soft->totspring += nofquads * 2;
612                 }
613         }
614 }
615
616 static void add_2nd_order_roller(Object *ob, float UNUSED(stiffness), int *counter, int addsprings)
617 {
618         /*assume we have a softbody*/
619         SoftBody *sb= ob->soft; /* is supposed to be there */
620         BodyPoint *bp, *bpo;
621         BodySpring *bs, *bs2, *bs3= NULL;
622         int a, b, c, notthis= 0, v0;
623         if (!sb->bspring) {return;} /* we are 2nd order here so 1rst should have been build :) */
624         /* first run counting  second run adding */
625         *counter = 0;
626         if (addsprings) bs3 = ob->soft->bspring+ob->soft->totspring;
627         for (a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
628                 /*scan for neighborhood*/
629                 bpo = NULL;
630                 v0  = (sb->totpoint-a);
631                 for (b=bp->nofsprings;b>0;b--) {
632                         bs = sb->bspring + bp->springs[b-1];
633                         /*nasty thing here that springs have two ends
634                         so here we have to make sure we examine the other */
635                         if (v0 == bs->v1) {
636                                 bpo = sb->bpoint+bs->v2;
637                                 notthis = bs->v2;
638                         }
639                         else {
640                         if (v0 == bs->v2) {
641                                 bpo = sb->bpoint+bs->v1;
642                                 notthis = bs->v1;
643                         }
644                         else {printf("oops we should not get here -  add_2nd_order_springs");}
645                         }
646                         if (bpo) {/* so now we have a 2nd order humpdidump */
647                                 for (c=bpo->nofsprings;c>0;c--) {
648                                         bs2 = sb->bspring + bpo->springs[c-1];
649                                         if ((bs2->v1 != notthis) && (bs2->v1 > v0)) {
650                                                 (*counter)++;/*hit */
651                                                 if (addsprings) {
652                                                         bs3->v1= v0;
653                                                         bs3->v2= bs2->v1;
654                                                         bs3->springtype   = SB_BEND;
655                                                         bs3++;
656                                                 }
657                                         }
658                                         if ((bs2->v2 !=notthis)&&(bs2->v2 > v0)) {
659                                                 (*counter)++;  /* hit */
660                                                 if (addsprings) {
661                                                         bs3->v1= v0;
662                                                         bs3->v2= bs2->v2;
663                                                         bs3->springtype   = SB_BEND;
664                                                         bs3++;
665                                                 }
666
667                                         }
668                                 }
669
670                         }
671
672                 }
673                 /*scan for neighborhood done*/
674         }
675 }
676
677
678 static void add_2nd_order_springs(Object *ob, float stiffness)
679 {
680         int counter = 0;
681         BodySpring *bs_new;
682         stiffness *=stiffness;
683
684         add_2nd_order_roller(ob, stiffness, &counter, 0); /* counting */
685         if (counter) {
686                 /* resize spring-array to hold additional springs */
687                 bs_new= MEM_callocN((ob->soft->totspring + counter )*sizeof(BodySpring), "bodyspring");
688                 memcpy(bs_new, ob->soft->bspring, (ob->soft->totspring )*sizeof(BodySpring));
689
690                 if (ob->soft->bspring)
691                         MEM_freeN(ob->soft->bspring);
692                 ob->soft->bspring = bs_new;
693
694                 add_2nd_order_roller(ob, stiffness, &counter, 1); /* adding */
695                 ob->soft->totspring += counter;
696         }
697 }
698
699 static void add_bp_springlist(BodyPoint *bp, int springID)
700 {
701         int *newlist;
702
703         if (bp->springs == NULL) {
704                 bp->springs = MEM_callocN(sizeof(int), "bpsprings");
705                 bp->springs[0] = springID;
706                 bp->nofsprings = 1;
707         }
708         else {
709                 bp->nofsprings++;
710                 newlist = MEM_callocN(bp->nofsprings * sizeof(int), "bpsprings");
711                 memcpy(newlist, bp->springs, (bp->nofsprings-1)* sizeof(int));
712                 MEM_freeN(bp->springs);
713                 bp->springs = newlist;
714                 bp->springs[bp->nofsprings-1] = springID;
715         }
716 }
717
718 /* do this once when sb is build
719 it is O(N^2) so scanning for springs every iteration is too expensive
720 */
721 static void build_bps_springlist(Object *ob)
722 {
723         SoftBody *sb= ob->soft; /* is supposed to be there */
724         BodyPoint *bp;
725         BodySpring *bs;
726         int a, b;
727
728         if (sb==NULL) return; /* paranoya check */
729
730         for (a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
731                 /* throw away old list */
732                 if (bp->springs) {
733                         MEM_freeN(bp->springs);
734                         bp->springs=NULL;
735                 }
736                 /* scan for attached inner springs */
737                 for (b=sb->totspring, bs= sb->bspring; b>0; b--, bs++) {
738                         if (( (sb->totpoint-a) == bs->v1) ) {
739                                 add_bp_springlist(bp, sb->totspring -b);
740                         }
741                         if (( (sb->totpoint-a) == bs->v2) ) {
742                                 add_bp_springlist(bp, sb->totspring -b);
743                         }
744                 }/*for springs*/
745         }/*for bp*/
746 }
747
748 static void calculate_collision_balls(Object *ob)
749 {
750         SoftBody *sb= ob->soft; /* is supposed to be there */
751         BodyPoint *bp;
752         BodySpring *bs;
753         int a, b, akku_count;
754         float min, max, akku;
755
756         if (sb==NULL) return; /* paranoya check */
757
758         for (a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
759                 bp->colball=0;
760                 akku =0.0f;
761                 akku_count=0;
762                 min = 1e22f;
763                 max = -1e22f;
764                 /* first estimation based on attached */
765                 for (b=bp->nofsprings;b>0;b--) {
766                         bs = sb->bspring + bp->springs[b-1];
767                         if (bs->springtype == SB_EDGE) {
768                                 akku += bs->len;
769                                 akku_count++;
770                                 min = min_ff(bs->len, min);
771                                 max = max_ff(bs->len, max);
772                         }
773                 }
774
775                 if (akku_count > 0) {
776                         if (sb->sbc_mode == SBC_MODE_MANUAL) {
777                                 bp->colball=sb->colball;
778                         }
779                         if (sb->sbc_mode == SBC_MODE_AVG) {
780                                 bp->colball = akku/(float)akku_count*sb->colball;
781                         }
782                         if (sb->sbc_mode == SBC_MODE_MIN) {
783                                 bp->colball=min*sb->colball;
784                         }
785                         if (sb->sbc_mode == SBC_MODE_MAX) {
786                                 bp->colball=max*sb->colball;
787                         }
788                         if (sb->sbc_mode == SBC_MODE_AVGMINMAX) {
789                                 bp->colball = (min + max)/2.0f*sb->colball;
790                         }
791                 }
792                 else bp->colball=0;
793         }/*for bp*/
794 }
795
796
797 /* creates new softbody if didn't exist yet, makes new points and springs arrays */
798 static void renew_softbody(Scene *scene, Object *ob, int totpoint, int totspring)
799 {
800         SoftBody *sb;
801         int i;
802         short softflag;
803         if (ob->soft==NULL) ob->soft= sbNew(scene);
804         else free_softbody_intern(ob->soft);
805         sb= ob->soft;
806         softflag=ob->softflag;
807
808         if (totpoint) {
809                 sb->totpoint= totpoint;
810                 sb->totspring= totspring;
811
812                 sb->bpoint= MEM_mallocN(totpoint*sizeof(BodyPoint), "bodypoint");
813                 if (totspring)
814                         sb->bspring= MEM_mallocN(totspring*sizeof(BodySpring), "bodyspring");
815
816                         /* initialize BodyPoint array */
817                 for (i=0; i<totpoint; i++) {
818                         BodyPoint *bp = &sb->bpoint[i];
819
820
821                         /* hum as far as i see this is overridden by _final_goal() now jow_go_for2_5 */
822                         /* sadly breaks compatibility with older versions */
823                         /* but makes goals behave the same for meshes, lattices and curves */
824                         if (softflag & OB_SB_GOAL) {
825                                 bp->goal= sb->defgoal;
826                         }
827                         else {
828                                 bp->goal= 0.0f;
829                                 /* so this will definily be below SOFTGOALSNAP */
830                         }
831
832                         bp->nofsprings= 0;
833                         bp->springs= NULL;
834                         bp->choke = 0.0f;
835                         bp->choke2 = 0.0f;
836                         bp->frozen = 1.0f;
837                         bp->colball = 0.0f;
838                         bp->loc_flag = 0;
839                         bp->springweight = 1.0f;
840                         bp->mass = 1.0f;
841                 }
842         }
843 }
844
845 static void free_softbody_baked(SoftBody *sb)
846 {
847         SBVertex *key;
848         int k;
849
850         for (k=0; k<sb->totkey; k++) {
851                 key= *(sb->keys + k);
852                 if (key) MEM_freeN(key);
853         }
854         if (sb->keys) MEM_freeN(sb->keys);
855
856         sb->keys= NULL;
857         sb->totkey= 0;
858 }
859 static void free_scratch(SoftBody *sb)
860 {
861         if (sb->scratch) {
862                 /* todo make sure everything is cleaned up nicly */
863                 if (sb->scratch->colliderhash) {
864                         BLI_ghash_free(sb->scratch->colliderhash, NULL,
865                                         (GHashValFreeFP) ccd_mesh_free); /*this hoepfully will free all caches*/
866                         sb->scratch->colliderhash = NULL;
867                 }
868                 if (sb->scratch->bodyface) {
869                         MEM_freeN(sb->scratch->bodyface);
870                 }
871                 if (sb->scratch->Ref.ivert) {
872                         MEM_freeN(sb->scratch->Ref.ivert);
873                 }
874                 MEM_freeN(sb->scratch);
875                 sb->scratch = NULL;
876         }
877
878 }
879
880 /* only frees internal data */
881 static void free_softbody_intern(SoftBody *sb)
882 {
883         if (sb) {
884                 int a;
885                 BodyPoint *bp;
886
887                 if (sb->bpoint) {
888                         for (a=sb->totpoint, bp= sb->bpoint; a>0; a--, bp++) {
889                                 /* free spring list */
890                                 if (bp->springs != NULL) {
891                                         MEM_freeN(bp->springs);
892                                 }
893                         }
894                         MEM_freeN(sb->bpoint);
895                 }
896
897                 if (sb->bspring) MEM_freeN(sb->bspring);
898
899                 sb->totpoint= sb->totspring= 0;
900                 sb->bpoint= NULL;
901                 sb->bspring= NULL;
902
903                 free_scratch(sb);
904                 free_softbody_baked(sb);
905         }
906 }
907
908
909 /* ************ dynamics ********** */
910
911 /* the most general (micro physics correct) way to do collision
912 ** (only needs the current particle position)
913 **
914 ** it actually checks if the particle intrudes a short range force field generated
915 ** by the faces of the target object and returns a force to drive the particel out
916 ** the strenght of the field grows exponetially if the particle is on the 'wrong' side of the face
917 ** 'wrong' side : projection to the face normal is negative (all referred to a vertex in the face)
918 **
919 ** flaw of this: 'fast' particles as well as 'fast' colliding faces
920 ** give a 'tunnel' effect such that the particle passes through the force field
921 ** without ever 'seeing' it
922 ** this is fully compliant to heisenberg: h >= fuzzy(location) * fuzzy(time)
923 ** besides our h is way larger than in QM because forces propagate way slower here
924 ** we have to deal with fuzzy(time) in the range of 1/25 seconds (typical frame rate)
925 ** yup collision targets are not known here any better
926 ** and 1/25 second is looong compared to real collision events
927 ** Q: why not use 'simple' collision here like bouncing back a particle
928 **   --> reverting is velocity on the face normal
929 ** A: because our particles are not alone here
930 **    and need to tell their neighbors exactly what happens via spring forces
931 ** unless sbObjectStep( .. ) is called on sub frame timing level
932 ** BTW that also questions the use of a 'implicit' solvers on softbodies
933 ** since that would only valid for 'slow' moving collision targets and dito particles
934 */
935
936 /* +++ dependency information functions*/
937
938 static int are_there_deflectors(Scene *scene, unsigned int layer)
939 {
940         Base *base;
941
942         for (base = scene->base.first; base; base= base->next) {
943                 if ( (base->lay & layer) && base->object->pd) {
944                         if (base->object->pd->deflect)
945                                 return 1;
946                 }
947         }
948         return 0;
949 }
950
951 static int query_external_colliders(Scene *scene, Object *me)
952 {
953         return(are_there_deflectors(scene, me->lay));
954 }
955 /* --- dependency information functions*/
956
957
958 /* +++ the aabb "force" section*/
959 static int sb_detect_aabb_collisionCached(float UNUSED(force[3]), unsigned int UNUSED(par_layer), struct Object *vertexowner, float UNUSED(time))
960 {
961         Object *ob;
962         SoftBody *sb=vertexowner->soft;
963         GHash *hash;
964         GHashIterator *ihash;
965         float  aabbmin[3], aabbmax[3];
966         int deflected=0;
967 #if 0
968         int a;
969 #endif
970
971         if ((sb == NULL) || (sb->scratch ==NULL)) return 0;
972         copy_v3_v3(aabbmin, sb->scratch->aabbmin);
973         copy_v3_v3(aabbmax, sb->scratch->aabbmax);
974
975         hash  = vertexowner->soft->scratch->colliderhash;
976         ihash = BLI_ghashIterator_new(hash);
977         while (!BLI_ghashIterator_done(ihash)) {
978
979                 ccd_Mesh *ccdm = BLI_ghashIterator_getValue     (ihash);
980                 ob             = BLI_ghashIterator_getKey       (ihash);
981                 {
982                         /* only with deflecting set */
983                         if (ob->pd && ob->pd->deflect) {
984                                 if (ccdm) {
985                                         if ((aabbmax[0] < ccdm->bbmin[0]) ||
986                                             (aabbmax[1] < ccdm->bbmin[1]) ||
987                                             (aabbmax[2] < ccdm->bbmin[2]) ||
988                                             (aabbmin[0] > ccdm->bbmax[0]) ||
989                                             (aabbmin[1] > ccdm->bbmax[1]) ||
990                                             (aabbmin[2] > ccdm->bbmax[2]) )
991                                         {
992                                                 /* boxes don't intersect */
993                                                 BLI_ghashIterator_step(ihash);
994                                                 continue;
995                                         }
996
997                                         /* so now we have the 2 boxes overlapping */
998                                         /* forces actually not used */
999                                         deflected = 2;
1000
1001                                 }
1002                                 else {
1003                                         /*aye that should be cached*/
1004                                         printf("missing cache error\n");
1005                                         BLI_ghashIterator_step(ihash);
1006                                         continue;
1007                                 }
1008                         } /* if (ob->pd && ob->pd->deflect) */
1009                         BLI_ghashIterator_step(ihash);
1010                 }
1011         } /* while () */
1012         BLI_ghashIterator_free(ihash);
1013         return deflected;
1014 }
1015 /* --- the aabb section*/
1016
1017
1018 /* +++ the face external section*/
1019 static int sb_detect_face_pointCached(float face_v1[3], float face_v2[3], float face_v3[3], float *damp,
1020                                       float force[3], unsigned int UNUSED(par_layer), struct Object *vertexowner, float time)
1021 {
1022         Object *ob;
1023         GHash *hash;
1024         GHashIterator *ihash;
1025         float nv1[3], edge1[3], edge2[3], d_nvect[3], aabbmin[3], aabbmax[3];
1026         float facedist, outerfacethickness, tune = 10.f;
1027         int a, deflected=0;
1028
1029         aabbmin[0] = min_fff(face_v1[0], face_v2[0], face_v3[0]);
1030         aabbmin[1] = min_fff(face_v1[1], face_v2[1], face_v3[1]);
1031         aabbmin[2] = min_fff(face_v1[2], face_v2[2], face_v3[2]);
1032         aabbmax[0] = max_fff(face_v1[0], face_v2[0], face_v3[0]);
1033         aabbmax[1] = max_fff(face_v1[1], face_v2[1], face_v3[1]);
1034         aabbmax[2] = max_fff(face_v1[2], face_v2[2], face_v3[2]);
1035
1036         /* calculate face normal once again SIGH */
1037         sub_v3_v3v3(edge1, face_v1, face_v2);
1038         sub_v3_v3v3(edge2, face_v3, face_v2);
1039         cross_v3_v3v3(d_nvect, edge2, edge1);
1040         normalize_v3(d_nvect);
1041
1042
1043         hash  = vertexowner->soft->scratch->colliderhash;
1044         ihash = BLI_ghashIterator_new(hash);
1045         while (!BLI_ghashIterator_done(ihash)) {
1046
1047                 ccd_Mesh *ccdm = BLI_ghashIterator_getValue     (ihash);
1048                 ob             = BLI_ghashIterator_getKey       (ihash);
1049                 {
1050                         /* only with deflecting set */
1051                         if (ob->pd && ob->pd->deflect) {
1052                                 const MVert *mvert= NULL;
1053                                 const MVert *mprevvert= NULL;
1054                                 if (ccdm) {
1055                                         mvert = ccdm->mvert;
1056                                         a     = ccdm->mvert_num;
1057                                         mprevvert= ccdm->mprevvert;
1058                                         outerfacethickness = ob->pd->pdef_sboft;
1059                                         if ((aabbmax[0] < ccdm->bbmin[0]) ||
1060                                             (aabbmax[1] < ccdm->bbmin[1]) ||
1061                                             (aabbmax[2] < ccdm->bbmin[2]) ||
1062                                             (aabbmin[0] > ccdm->bbmax[0]) ||
1063                                             (aabbmin[1] > ccdm->bbmax[1]) ||
1064                                             (aabbmin[2] > ccdm->bbmax[2]) )
1065                                         {
1066                                                 /* boxes don't intersect */
1067                                                 BLI_ghashIterator_step(ihash);
1068                                                 continue;
1069                                         }
1070
1071                                 }
1072                                 else {
1073                                         /*aye that should be cached*/
1074                                         printf("missing cache error\n");
1075                                         BLI_ghashIterator_step(ihash);
1076                                         continue;
1077                                 }
1078
1079
1080                                 /* use mesh*/
1081                                 if (mvert) {
1082                                         while (a) {
1083                                                 copy_v3_v3(nv1, mvert[a-1].co);
1084                                                 if (mprevvert) {
1085                                                         mul_v3_fl(nv1, time);
1086                                                         madd_v3_v3fl(nv1, mprevvert[a - 1].co, 1.0f - time);
1087                                                 }
1088                                                 /* origin to face_v2*/
1089                                                 sub_v3_v3(nv1, face_v2);
1090                                                 facedist = dot_v3v3(nv1, d_nvect);
1091                                                 if (ABS(facedist)<outerfacethickness) {
1092                                                         if (isect_point_tri_prism_v3(nv1, face_v1, face_v2, face_v3) ) {
1093                                                                 float df;
1094                                                                 if (facedist > 0) {
1095                                                                         df = (outerfacethickness-facedist)/outerfacethickness;
1096                                                                 }
1097                                                                 else {
1098                                                                         df = (outerfacethickness+facedist)/outerfacethickness;
1099                                                                 }
1100
1101                                                                 *damp=df*tune*ob->pd->pdef_sbdamp;
1102
1103                                                                 df = 0.01f * expf(-100.0f * df);
1104                                                                 madd_v3_v3fl(force, d_nvect, -df);
1105                                                                 deflected = 3;
1106                                                         }
1107                                                 }
1108                                                 a--;
1109                                         }/* while (a)*/
1110                                 } /* if (mvert) */
1111                         } /* if (ob->pd && ob->pd->deflect) */
1112                         BLI_ghashIterator_step(ihash);
1113                 }
1114         } /* while () */
1115         BLI_ghashIterator_free(ihash);
1116         return deflected;
1117 }
1118
1119
1120 static int sb_detect_face_collisionCached(float face_v1[3], float face_v2[3], float face_v3[3], float *damp,
1121                                           float force[3], unsigned int UNUSED(par_layer), struct Object *vertexowner, float time)
1122 {
1123         Object *ob;
1124         GHash *hash;
1125         GHashIterator *ihash;
1126         float nv1[3], nv2[3], nv3[3], edge1[3], edge2[3], d_nvect[3], aabbmin[3], aabbmax[3];
1127         float t, tune = 10.0f;
1128         int a, deflected=0;
1129
1130         aabbmin[0] = min_fff(face_v1[0], face_v2[0], face_v3[0]);
1131         aabbmin[1] = min_fff(face_v1[1], face_v2[1], face_v3[1]);
1132         aabbmin[2] = min_fff(face_v1[2], face_v2[2], face_v3[2]);
1133         aabbmax[0] = max_fff(face_v1[0], face_v2[0], face_v3[0]);
1134         aabbmax[1] = max_fff(face_v1[1], face_v2[1], face_v3[1]);
1135         aabbmax[2] = max_fff(face_v1[2], face_v2[2], face_v3[2]);
1136
1137         hash  = vertexowner->soft->scratch->colliderhash;
1138         ihash = BLI_ghashIterator_new(hash);
1139         while (!BLI_ghashIterator_done(ihash)) {
1140
1141                 ccd_Mesh *ccdm = BLI_ghashIterator_getValue     (ihash);
1142                 ob             = BLI_ghashIterator_getKey       (ihash);
1143                 {
1144                         /* only with deflecting set */
1145                         if (ob->pd && ob->pd->deflect) {
1146                                 const MVert *mvert = NULL;
1147                                 const MVert *mprevvert = NULL;
1148                                 const MVertTri *vt = NULL;
1149                                 const ccdf_minmax *mima = NULL;
1150
1151                                 if (ccdm) {
1152                                         mvert = ccdm->mvert;
1153                                         vt = ccdm->tri;
1154                                         mprevvert = ccdm->mprevvert;
1155                                         mima = ccdm->mima;
1156                                         a = ccdm->tri_num;
1157
1158                                         if ((aabbmax[0] < ccdm->bbmin[0]) ||
1159                                             (aabbmax[1] < ccdm->bbmin[1]) ||
1160                                             (aabbmax[2] < ccdm->bbmin[2]) ||
1161                                             (aabbmin[0] > ccdm->bbmax[0]) ||
1162                                             (aabbmin[1] > ccdm->bbmax[1]) ||
1163                                             (aabbmin[2] > ccdm->bbmax[2]) )
1164                                         {
1165                                                 /* boxes don't intersect */
1166                                                 BLI_ghashIterator_step(ihash);
1167                                                 continue;
1168                                         }
1169
1170                                 }
1171                                 else {
1172                                         /*aye that should be cached*/
1173                                         printf("missing cache error\n");
1174                                         BLI_ghashIterator_step(ihash);
1175                                         continue;
1176                                 }
1177
1178
1179                                 /* use mesh*/
1180                                 while (a--) {
1181                                         if ((aabbmax[0] < mima->minx) ||
1182                                             (aabbmin[0] > mima->maxx) ||
1183                                             (aabbmax[1] < mima->miny) ||
1184                                             (aabbmin[1] > mima->maxy) ||
1185                                             (aabbmax[2] < mima->minz) ||
1186                                             (aabbmin[2] > mima->maxz))
1187                                         {
1188                                                 mima++;
1189                                                 vt++;
1190                                                 continue;
1191                                         }
1192
1193
1194                                         if (mvert) {
1195
1196                                                 copy_v3_v3(nv1, mvert[vt->tri[0]].co);
1197                                                 copy_v3_v3(nv2, mvert[vt->tri[1]].co);
1198                                                 copy_v3_v3(nv3, mvert[vt->tri[2]].co);
1199
1200                                                 if (mprevvert) {
1201                                                         mul_v3_fl(nv1, time);
1202                                                         madd_v3_v3fl(nv1, mprevvert[vt->tri[0]].co, 1.0f - time);
1203
1204                                                         mul_v3_fl(nv2, time);
1205                                                         madd_v3_v3fl(nv2, mprevvert[vt->tri[1]].co, 1.0f - time);
1206
1207                                                         mul_v3_fl(nv3, time);
1208                                                         madd_v3_v3fl(nv3, mprevvert[vt->tri[2]].co, 1.0f - time);
1209                                                 }
1210                                         }
1211
1212                                         /* switch origin to be nv2*/
1213                                         sub_v3_v3v3(edge1, nv1, nv2);
1214                                         sub_v3_v3v3(edge2, nv3, nv2);
1215                                         cross_v3_v3v3(d_nvect, edge2, edge1);
1216                                         normalize_v3(d_nvect);
1217                                         if (isect_line_tri_v3(nv1, nv2, face_v1, face_v2, face_v3, &t, NULL) ||
1218                                             isect_line_tri_v3(nv2, nv3, face_v1, face_v2, face_v3, &t, NULL) ||
1219                                             isect_line_tri_v3(nv3, nv1, face_v1, face_v2, face_v3, &t, NULL) )
1220                                         {
1221                                                 madd_v3_v3fl(force, d_nvect, -0.5f);
1222                                                 *damp=tune*ob->pd->pdef_sbdamp;
1223                                                 deflected = 2;
1224                                         }
1225                                         mima++;
1226                                         vt++;
1227                                 }/* while a */
1228                         } /* if (ob->pd && ob->pd->deflect) */
1229                         BLI_ghashIterator_step(ihash);
1230                 }
1231         } /* while () */
1232         BLI_ghashIterator_free(ihash);
1233         return deflected;
1234 }
1235
1236
1237
1238 static void scan_for_ext_face_forces(Object *ob, float timenow)
1239 {
1240         SoftBody *sb = ob->soft;
1241         BodyFace *bf;
1242         int a;
1243         float damp=0.0f, choke=1.0f;
1244         float tune = -10.0f;
1245         float feedback[3];
1246
1247         if (sb && sb->scratch->totface) {
1248
1249
1250                 bf = sb->scratch->bodyface;
1251                 for (a=0; a<sb->scratch->totface; a++, bf++) {
1252                         bf->ext_force[0]=bf->ext_force[1]=bf->ext_force[2]=0.0f;
1253 /*+++edges intruding*/
1254                         bf->flag &= ~BFF_INTERSECT;
1255                         zero_v3(feedback);
1256                         if (sb_detect_face_collisionCached(
1257                                 sb->bpoint[bf->v1].pos, sb->bpoint[bf->v2].pos, sb->bpoint[bf->v3].pos,
1258                                 &damp, feedback, ob->lay, ob, timenow))
1259                         {
1260                                 madd_v3_v3fl(sb->bpoint[bf->v1].force, feedback, tune);
1261                                 madd_v3_v3fl(sb->bpoint[bf->v2].force, feedback, tune);
1262                                 madd_v3_v3fl(sb->bpoint[bf->v3].force, feedback, tune);
1263 //                              madd_v3_v3fl(bf->ext_force, feedback, tune);
1264                                 bf->flag |= BFF_INTERSECT;
1265                                 choke = min_ff(max_ff(damp, choke), 1.0f);
1266                         }
1267 /*---edges intruding*/
1268
1269 /*+++ close vertices*/
1270                         if (( bf->flag & BFF_INTERSECT)==0) {
1271                                 bf->flag &= ~BFF_CLOSEVERT;
1272                                 tune = -1.0f;
1273                                 zero_v3(feedback);
1274                                 if (sb_detect_face_pointCached(
1275                                         sb->bpoint[bf->v1].pos, sb->bpoint[bf->v2].pos, sb->bpoint[bf->v3].pos,
1276                                         &damp,  feedback, ob->lay, ob, timenow))
1277                                 {
1278                                         madd_v3_v3fl(sb->bpoint[bf->v1].force, feedback, tune);
1279                                         madd_v3_v3fl(sb->bpoint[bf->v2].force, feedback, tune);
1280                                         madd_v3_v3fl(sb->bpoint[bf->v3].force, feedback, tune);
1281 //                                      madd_v3_v3fl(bf->ext_force, feedback, tune);
1282                                         bf->flag |= BFF_CLOSEVERT;
1283                                         choke = min_ff(max_ff(damp, choke), 1.0f);
1284                                 }
1285                         }
1286 /*--- close vertices*/
1287                 }
1288                 bf = sb->scratch->bodyface;
1289                 for (a=0; a<sb->scratch->totface; a++, bf++) {
1290                         if (( bf->flag & BFF_INTERSECT) || ( bf->flag & BFF_CLOSEVERT)) {
1291                                 sb->bpoint[bf->v1].choke2 = max_ff(sb->bpoint[bf->v1].choke2, choke);
1292                                 sb->bpoint[bf->v2].choke2 = max_ff(sb->bpoint[bf->v2].choke2, choke);
1293                                 sb->bpoint[bf->v3].choke2 = max_ff(sb->bpoint[bf->v3].choke2, choke);
1294                         }
1295                 }
1296         }
1297 }
1298
1299 /*  --- the face external section*/
1300
1301
1302 /* +++ the spring external section*/
1303
1304 static int sb_detect_edge_collisionCached(float edge_v1[3], float edge_v2[3], float *damp,
1305                                                                    float force[3], unsigned int UNUSED(par_layer), struct Object *vertexowner, float time)
1306 {
1307         Object *ob;
1308         GHash *hash;
1309         GHashIterator *ihash;
1310         float nv1[3], nv2[3], nv3[3], edge1[3], edge2[3], d_nvect[3], aabbmin[3], aabbmax[3];
1311         float t, el;
1312         int a, deflected=0;
1313
1314         minmax_v3v3_v3(aabbmin, aabbmax, edge_v1);
1315         minmax_v3v3_v3(aabbmin, aabbmax, edge_v2);
1316
1317         el = len_v3v3(edge_v1, edge_v2);
1318
1319         hash  = vertexowner->soft->scratch->colliderhash;
1320         ihash = BLI_ghashIterator_new(hash);
1321         while (!BLI_ghashIterator_done(ihash)) {
1322
1323                 ccd_Mesh *ccdm = BLI_ghashIterator_getValue     (ihash);
1324                 ob             = BLI_ghashIterator_getKey       (ihash);
1325                 {
1326                         /* only with deflecting set */
1327                         if (ob->pd && ob->pd->deflect) {
1328                                 const MVert *mvert = NULL;
1329                                 const MVert *mprevvert = NULL;
1330                                 const MVertTri *vt = NULL;
1331                                 const ccdf_minmax *mima = NULL;
1332
1333                                 if (ccdm) {
1334                                         mvert = ccdm->mvert;
1335                                         mprevvert = ccdm->mprevvert;
1336                                         vt = ccdm->tri;
1337                                         mima = ccdm->mima;
1338                                         a = ccdm->tri_num;
1339
1340                                         if ((aabbmax[0] < ccdm->bbmin[0]) ||
1341                                             (aabbmax[1] < ccdm->bbmin[1]) ||
1342                                             (aabbmax[2] < ccdm->bbmin[2]) ||
1343                                             (aabbmin[0] > ccdm->bbmax[0]) ||
1344                                             (aabbmin[1] > ccdm->bbmax[1]) ||
1345                                             (aabbmin[2] > ccdm->bbmax[2]) )
1346                                         {
1347                                                 /* boxes don't intersect */
1348                                                 BLI_ghashIterator_step(ihash);
1349                                                 continue;
1350                                         }
1351
1352                                 }
1353                                 else {
1354                                         /*aye that should be cached*/
1355                                         printf("missing cache error\n");
1356                                         BLI_ghashIterator_step(ihash);
1357                                         continue;
1358                                 }
1359
1360
1361                                 /* use mesh*/
1362                                 while (a--) {
1363                                         if ((aabbmax[0] < mima->minx) ||
1364                                             (aabbmin[0] > mima->maxx) ||
1365                                             (aabbmax[1] < mima->miny) ||
1366                                             (aabbmin[1] > mima->maxy) ||
1367                                             (aabbmax[2] < mima->minz) ||
1368                                             (aabbmin[2] > mima->maxz))
1369                                         {
1370                                                 mima++;
1371                                                 vt++;
1372                                                 continue;
1373                                         }
1374
1375
1376                                         if (mvert) {
1377
1378                                                 copy_v3_v3(nv1, mvert[vt->tri[0]].co);
1379                                                 copy_v3_v3(nv2, mvert[vt->tri[1]].co);
1380                                                 copy_v3_v3(nv3, mvert[vt->tri[2]].co);
1381
1382                                                 if (mprevvert) {
1383                                                         mul_v3_fl(nv1, time);
1384                                                         madd_v3_v3fl(nv1, mprevvert[vt->tri[0]].co, 1.0f - time);
1385
1386                                                         mul_v3_fl(nv2, time);
1387                                                         madd_v3_v3fl(nv2, mprevvert[vt->tri[1]].co, 1.0f - time);
1388
1389                                                         mul_v3_fl(nv3, time);
1390                                                         madd_v3_v3fl(nv3, mprevvert[vt->tri[2]].co, 1.0f - time);
1391                                                 }
1392                                         }
1393
1394                                         /* switch origin to be nv2*/
1395                                         sub_v3_v3v3(edge1, nv1, nv2);
1396                                         sub_v3_v3v3(edge2, nv3, nv2);
1397
1398                                         cross_v3_v3v3(d_nvect, edge2, edge1);
1399                                         normalize_v3(d_nvect);
1400                                         if ( isect_line_tri_v3(edge_v1, edge_v2, nv1, nv2, nv3, &t, NULL)) {
1401                                                 float v1[3], v2[3];
1402                                                 float intrusiondepth, i1, i2;
1403                                                 sub_v3_v3v3(v1, edge_v1, nv2);
1404                                                 sub_v3_v3v3(v2, edge_v2, nv2);
1405                                                 i1 = dot_v3v3(v1, d_nvect);
1406                                                 i2 = dot_v3v3(v2, d_nvect);
1407                                                 intrusiondepth = -min_ff(i1, i2) / el;
1408                                                 madd_v3_v3fl(force, d_nvect, intrusiondepth);
1409                                                 *damp=ob->pd->pdef_sbdamp;
1410                                                 deflected = 2;
1411                                         }
1412
1413                                         mima++;
1414                                         vt++;
1415                                 }/* while a */
1416                         } /* if (ob->pd && ob->pd->deflect) */
1417                         BLI_ghashIterator_step(ihash);
1418                 }
1419         } /* while () */
1420         BLI_ghashIterator_free(ihash);
1421         return deflected;
1422 }
1423
1424 static void _scan_for_ext_spring_forces(Scene *scene, Object *ob, float timenow, int ifirst, int ilast, struct ListBase *do_effector)
1425 {
1426         SoftBody *sb = ob->soft;
1427         int a;
1428         float damp;
1429         float feedback[3];
1430
1431         if (sb && sb->totspring) {
1432                 for (a=ifirst; a<ilast; a++) {
1433                         BodySpring *bs = &sb->bspring[a];
1434                         bs->ext_force[0]=bs->ext_force[1]=bs->ext_force[2]=0.0f;
1435                         feedback[0]=feedback[1]=feedback[2]=0.0f;
1436                         bs->flag &= ~BSF_INTERSECT;
1437
1438                         if (bs->springtype == SB_EDGE) {
1439                                 /* +++ springs colliding */
1440                                 if (ob->softflag & OB_SB_EDGECOLL) {
1441                                         if ( sb_detect_edge_collisionCached (sb->bpoint[bs->v1].pos, sb->bpoint[bs->v2].pos,
1442                                                 &damp, feedback, ob->lay, ob, timenow)) {
1443                                                         add_v3_v3(bs->ext_force, feedback);
1444                                                         bs->flag |= BSF_INTERSECT;
1445                                                         //bs->cf=damp;
1446                                                         bs->cf=sb->choke*0.01f;
1447
1448                                         }
1449                                 }
1450                                 /* ---- springs colliding */
1451
1452                                 /* +++ springs seeing wind ... n stuff depending on their orientation*/
1453                                 /* note we don't use sb->mediafrict but use sb->aeroedge for magnitude of effect*/
1454                                 if (sb->aeroedge) {
1455                                         float vel[3], sp[3], pr[3], force[3];
1456                                         float f, windfactor  = 0.25f;
1457                                         /*see if we have wind*/
1458                                         if (do_effector) {
1459                                                 EffectedPoint epoint;
1460                                                 float speed[3] = {0.0f, 0.0f, 0.0f};
1461                                                 float pos[3];
1462                                                 mid_v3_v3v3(pos, sb->bpoint[bs->v1].pos, sb->bpoint[bs->v2].pos);
1463                                                 mid_v3_v3v3(vel, sb->bpoint[bs->v1].vec, sb->bpoint[bs->v2].vec);
1464                                                 pd_point_from_soft(scene, pos, vel, -1, &epoint);
1465                                                 pdDoEffectors(do_effector, NULL, sb->effector_weights, &epoint, force, speed);
1466
1467                                                 mul_v3_fl(speed, windfactor);
1468                                                 add_v3_v3(vel, speed);
1469                                         }
1470                                         /* media in rest */
1471                                         else {
1472                                                 add_v3_v3v3(vel, sb->bpoint[bs->v1].vec, sb->bpoint[bs->v2].vec);
1473                                         }
1474                                         f = normalize_v3(vel);
1475                                         f = -0.0001f*f*f*sb->aeroedge;
1476                                         /* (todo) add a nice angle dependent function done for now BUT */
1477                                         /* still there could be some nice drag/lift function, but who needs it */
1478
1479                                         sub_v3_v3v3(sp, sb->bpoint[bs->v1].pos, sb->bpoint[bs->v2].pos);
1480                                         project_v3_v3v3(pr, vel, sp);
1481                                         sub_v3_v3(vel, pr);
1482                                         normalize_v3(vel);
1483                                         if (ob->softflag & OB_SB_AERO_ANGLE) {
1484                                                 normalize_v3(sp);
1485                                                 madd_v3_v3fl(bs->ext_force, vel, f * (1.0f - fabsf(dot_v3v3(vel, sp))));
1486                                         }
1487                                         else {
1488                                                 madd_v3_v3fl(bs->ext_force, vel, f); // to keep compatible with 2.45 release files
1489                                         }
1490                                 }
1491                                 /* --- springs seeing wind */
1492                         }
1493                 }
1494         }
1495 }
1496
1497
1498 static void scan_for_ext_spring_forces(Scene *scene, Object *ob, float timenow)
1499 {
1500         SoftBody *sb = ob->soft;
1501         ListBase *do_effector = NULL;
1502
1503         do_effector = pdInitEffectors(scene, ob, NULL, sb->effector_weights, true);
1504         _scan_for_ext_spring_forces(scene, ob, timenow, 0, sb->totspring, do_effector);
1505         pdEndEffectors(&do_effector);
1506 }
1507
1508 static void *exec_scan_for_ext_spring_forces(void *data)
1509 {
1510         SB_thread_context *pctx = (SB_thread_context*)data;
1511         _scan_for_ext_spring_forces(pctx->scene, pctx->ob, pctx->timenow, pctx->ifirst, pctx->ilast, pctx->do_effector);
1512         return NULL;
1513 }
1514
1515 static void sb_sfesf_threads_run(Scene *scene, struct Object *ob, float timenow, int totsprings, int *UNUSED(ptr_to_break_func(void)))
1516 {
1517         ListBase *do_effector = NULL;
1518         ListBase threads;
1519         SB_thread_context *sb_threads;
1520         int i, totthread, left, dec;
1521         int lowsprings =100; /* wild guess .. may increase with better thread management 'above' or even be UI option sb->spawn_cf_threads_nopts */
1522
1523         do_effector= pdInitEffectors(scene, ob, NULL, ob->soft->effector_weights, true);
1524
1525         /* figure the number of threads while preventing pretty pointless threading overhead */
1526         totthread= BKE_scene_num_threads(scene);
1527         /* what if we got zillions of CPUs running but less to spread*/
1528         while ((totsprings/totthread < lowsprings) && (totthread > 1)) {
1529                 totthread--;
1530         }
1531
1532         sb_threads= MEM_callocN(sizeof(SB_thread_context)*totthread, "SBSpringsThread");
1533         memset(sb_threads, 0, sizeof(SB_thread_context)*totthread);
1534         left = totsprings;
1535         dec = totsprings/totthread +1;
1536         for (i=0; i<totthread; i++) {
1537                 sb_threads[i].scene = scene;
1538                 sb_threads[i].ob = ob;
1539                 sb_threads[i].forcetime = 0.0; // not used here
1540                 sb_threads[i].timenow = timenow;
1541                 sb_threads[i].ilast   = left;
1542                 left = left - dec;
1543                 if (left >0) {
1544                         sb_threads[i].ifirst  = left;
1545                 }
1546                 else
1547                         sb_threads[i].ifirst  = 0;
1548                 sb_threads[i].do_effector = do_effector;
1549                 sb_threads[i].do_deflector = false;// not used here
1550                 sb_threads[i].fieldfactor = 0.0f;// not used here
1551                 sb_threads[i].windfactor  = 0.0f;// not used here
1552                 sb_threads[i].nr= i;
1553                 sb_threads[i].tot= totthread;
1554         }
1555         if (totthread > 1) {
1556                 BLI_init_threads(&threads, exec_scan_for_ext_spring_forces, totthread);
1557
1558                 for (i=0; i<totthread; i++)
1559                         BLI_insert_thread(&threads, &sb_threads[i]);
1560
1561                 BLI_end_threads(&threads);
1562         }
1563         else
1564                 exec_scan_for_ext_spring_forces(&sb_threads[0]);
1565         /* clean up */
1566         MEM_freeN(sb_threads);
1567
1568         pdEndEffectors(&do_effector);
1569 }
1570
1571
1572 /* --- the spring external section*/
1573
1574 static int choose_winner(float*w, float* pos, float*a, float*b, float*c, float*ca, float*cb, float*cc)
1575 {
1576         float mindist, cp;
1577         int winner =1;
1578         mindist = fabsf(dot_v3v3(pos, a));
1579
1580         cp = fabsf(dot_v3v3(pos, b));
1581         if ( mindist < cp ) {
1582                 mindist = cp;
1583                 winner =2;
1584         }
1585
1586         cp = fabsf(dot_v3v3(pos, c));
1587         if (mindist < cp ) {
1588                 mindist = cp;
1589                 winner =3;
1590         }
1591         switch (winner) {
1592                 case 1: copy_v3_v3(w, ca); break;
1593                 case 2: copy_v3_v3(w, cb); break;
1594                 case 3: copy_v3_v3(w, cc);
1595         }
1596         return(winner);
1597 }
1598
1599
1600
1601 static int sb_detect_vertex_collisionCached(
1602         float opco[3], float facenormal[3], float *damp,
1603         float force[3], unsigned int UNUSED(par_layer), struct Object *vertexowner,
1604         float time, float vel[3], float *intrusion)
1605 {
1606         Object *ob= NULL;
1607         GHash *hash;
1608         GHashIterator *ihash;
1609         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},
1610               vv1[3], vv2[3], vv3[3], coledge[3] = {0.0f, 0.0f, 0.0f}, mindistedge = 1000.0f,
1611               outerforceaccu[3], innerforceaccu[3],
1612               facedist, /* n_mag, */ /* UNUSED */ force_mag_norm, minx, miny, minz, maxx, maxy, maxz,
1613               innerfacethickness = -0.5f, outerfacethickness = 0.2f,
1614               ee = 5.0f, ff = 0.1f, fa=1;
1615         int a, deflected=0, cavel=0, ci=0;
1616 /* init */
1617         *intrusion = 0.0f;
1618         hash  = vertexowner->soft->scratch->colliderhash;
1619         ihash = BLI_ghashIterator_new(hash);
1620         outerforceaccu[0]=outerforceaccu[1]=outerforceaccu[2]=0.0f;
1621         innerforceaccu[0]=innerforceaccu[1]=innerforceaccu[2]=0.0f;
1622 /* go */
1623         while (!BLI_ghashIterator_done(ihash)) {
1624
1625                 ccd_Mesh *ccdm = BLI_ghashIterator_getValue     (ihash);
1626                 ob             = BLI_ghashIterator_getKey       (ihash);
1627                 {
1628                         /* only with deflecting set */
1629                         if (ob->pd && ob->pd->deflect) {
1630                                 const MVert *mvert = NULL;
1631                                 const MVert *mprevvert = NULL;
1632                                 const MVertTri *vt = NULL;
1633                                 const ccdf_minmax *mima = NULL;
1634
1635                                 if (ccdm) {
1636                                         mvert = ccdm->mvert;
1637                                         mprevvert = ccdm->mprevvert;
1638                                         vt = ccdm->tri;
1639                                         mima = ccdm->mima;
1640                                         a = ccdm->tri_num;
1641
1642                                         minx = ccdm->bbmin[0];
1643                                         miny = ccdm->bbmin[1];
1644                                         minz = ccdm->bbmin[2];
1645
1646                                         maxx = ccdm->bbmax[0];
1647                                         maxy = ccdm->bbmax[1];
1648                                         maxz = ccdm->bbmax[2];
1649
1650                                         if ((opco[0] < minx) ||
1651                                             (opco[1] < miny) ||
1652                                             (opco[2] < minz) ||
1653                                             (opco[0] > maxx) ||
1654                                             (opco[1] > maxy) ||
1655                                             (opco[2] > maxz) )
1656                                         {
1657                                                 /* outside the padded boundbox --> collision object is too far away */
1658                                                 BLI_ghashIterator_step(ihash);
1659                                                 continue;
1660                                         }
1661                                 }
1662                                 else {
1663                                         /*aye that should be cached*/
1664                                         printf("missing cache error\n");
1665                                         BLI_ghashIterator_step(ihash);
1666                                         continue;
1667                                 }
1668
1669                                 /* do object level stuff */
1670                                 /* need to have user control for that since it depends on model scale */
1671                                 innerfacethickness = -ob->pd->pdef_sbift;
1672                                 outerfacethickness =  ob->pd->pdef_sboft;
1673                                 fa = (ff*outerfacethickness-outerfacethickness);
1674                                 fa *= fa;
1675                                 fa = 1.0f/fa;
1676                                 avel[0]=avel[1]=avel[2]=0.0f;
1677                                 /* use mesh*/
1678                                 while (a--) {
1679                                         if ((opco[0] < mima->minx) ||
1680                                             (opco[0] > mima->maxx) ||
1681                                             (opco[1] < mima->miny) ||
1682                                             (opco[1] > mima->maxy) ||
1683                                             (opco[2] < mima->minz) ||
1684                                             (opco[2] > mima->maxz))
1685                                         {
1686                                                 mima++;
1687                                                 vt++;
1688                                                 continue;
1689                                         }
1690
1691                                         if (mvert) {
1692
1693                                                 copy_v3_v3(nv1, mvert[vt->tri[0]].co);
1694                                                 copy_v3_v3(nv2, mvert[vt->tri[1]].co);
1695                                                 copy_v3_v3(nv3, mvert[vt->tri[2]].co);
1696
1697                                                 if (mprevvert) {
1698                                                         /* grab the average speed of the collider vertices
1699                                                         before we spoil nvX
1700                                                         humm could be done once a SB steps but then we' need to store that too
1701                                                         since the AABB reduced propabitlty to get here drasticallly
1702                                                         it might be a nice tradeof CPU <--> memory
1703                                                         */
1704                                                         sub_v3_v3v3(vv1, nv1, mprevvert[vt->tri[0]].co);
1705                                                         sub_v3_v3v3(vv2, nv2, mprevvert[vt->tri[1]].co);
1706                                                         sub_v3_v3v3(vv3, nv3, mprevvert[vt->tri[2]].co);
1707
1708                                                         mul_v3_fl(nv1, time);
1709                                                         madd_v3_v3fl(nv1, mprevvert[vt->tri[0]].co, 1.0f - time);
1710
1711                                                         mul_v3_fl(nv2, time);
1712                                                         madd_v3_v3fl(nv2, mprevvert[vt->tri[1]].co, 1.0f - time);
1713
1714                                                         mul_v3_fl(nv3, time);
1715                                                         madd_v3_v3fl(nv3, mprevvert[vt->tri[2]].co, 1.0f - time);
1716                                                 }
1717                                         }
1718
1719                                         /* switch origin to be nv2*/
1720                                         sub_v3_v3v3(edge1, nv1, nv2);
1721                                         sub_v3_v3v3(edge2, nv3, nv2);
1722                                         sub_v3_v3v3(dv1, opco, nv2); /* abuse dv1 to have vertex in question at *origin* of triangle */
1723
1724                                         cross_v3_v3v3(d_nvect, edge2, edge1);
1725                                         /* n_mag = */ /* UNUSED */ normalize_v3(d_nvect);
1726                                         facedist = dot_v3v3(dv1, d_nvect);
1727                                         // so rules are
1728                                         //
1729
1730                                         if ((facedist > innerfacethickness) && (facedist < outerfacethickness)) {
1731                                                 if (isect_point_tri_prism_v3(opco, nv1, nv2, nv3) ) {
1732                                                         force_mag_norm =(float)exp(-ee*facedist);
1733                                                         if (facedist > outerfacethickness*ff)
1734                                                                 force_mag_norm =(float)force_mag_norm*fa*(facedist - outerfacethickness)*(facedist - outerfacethickness);
1735                                                         *damp=ob->pd->pdef_sbdamp;
1736                                                         if (facedist > 0.0f) {
1737                                                                 *damp *= (1.0f - facedist/outerfacethickness);
1738                                                                 madd_v3_v3fl(outerforceaccu, d_nvect, force_mag_norm);
1739                                                                 deflected = 3;
1740
1741                                                         }
1742                                                         else {
1743                                                                 madd_v3_v3fl(innerforceaccu, d_nvect, force_mag_norm);
1744                                                                 if (deflected < 2) deflected = 2;
1745                                                         }
1746                                                         if ((mprevvert) && (*damp > 0.0f)) {
1747                                                                 choose_winner(ve, opco, nv1, nv2, nv3, vv1, vv2, vv3);
1748                                                                 add_v3_v3(avel, ve);
1749                                                                 cavel ++;
1750                                                         }
1751                                                         *intrusion += facedist;
1752                                                         ci++;
1753                                                 }
1754                                         }
1755
1756                                         mima++;
1757                                         vt++;
1758                                 }/* while a */
1759                         } /* if (ob->pd && ob->pd->deflect) */
1760                         BLI_ghashIterator_step(ihash);
1761                 }
1762         } /* while () */
1763
1764         if (deflected == 1) { // no face but 'outer' edge cylinder sees vert
1765                 force_mag_norm =(float)exp(-ee*mindistedge);
1766                 if (mindistedge > outerfacethickness*ff)
1767                         force_mag_norm =(float)force_mag_norm*fa*(mindistedge - outerfacethickness)*(mindistedge - outerfacethickness);
1768                 madd_v3_v3fl(force, coledge, force_mag_norm);
1769                 *damp=ob->pd->pdef_sbdamp;
1770                 if (mindistedge > 0.0f) {
1771                         *damp *= (1.0f - mindistedge/outerfacethickness);
1772                 }
1773
1774         }
1775         if (deflected == 2) { //  face inner detected
1776                 add_v3_v3(force, innerforceaccu);
1777         }
1778         if (deflected == 3) { //  face outer detected
1779                 add_v3_v3(force, outerforceaccu);
1780         }
1781
1782         BLI_ghashIterator_free(ihash);
1783         if (cavel) mul_v3_fl(avel, 1.0f/(float)cavel);
1784         copy_v3_v3(vel, avel);
1785         if (ci) *intrusion /= ci;
1786         if (deflected) {
1787                 normalize_v3_v3(facenormal, force);
1788         }
1789         return deflected;
1790 }
1791
1792
1793 /* sandbox to plug in various deflection algos */
1794 static int sb_deflect_face(Object *ob, float *actpos, float *facenormal, float *force, float *cf, float time, float *vel, float *intrusion)
1795 {
1796         float s_actpos[3];
1797         int deflected;
1798         copy_v3_v3(s_actpos, actpos);
1799         deflected= sb_detect_vertex_collisionCached(s_actpos, facenormal, cf, force, ob->lay, ob, time, vel, intrusion);
1800         //deflected= sb_detect_vertex_collisionCachedEx(s_actpos, facenormal, cf, force, ob->lay, ob, time, vel, intrusion);
1801         return(deflected);
1802 }
1803
1804 /* hiding this for now .. but the jacobian may pop up on other tasks .. so i'd like to keep it
1805 static void dfdx_spring(int ia, int ic, int op, float dir[3], float L, float len, float factor)
1806 {
1807         float m, delta_ij;
1808         int i, j;
1809         if (L < len) {
1810                 for (i=0;i<3;i++)
1811                         for (j=0;j<3;j++) {
1812                                 delta_ij = (i==j ? (1.0f): (0.0f));
1813                                 m=factor*(dir[i]*dir[j] + (1-L/len)*(delta_ij - dir[i]*dir[j]));
1814                                 nlMatrixAdd(ia+i, op+ic+j, m);
1815                         }
1816         }
1817         else {
1818                 for (i=0;i<3;i++)
1819                         for (j=0;j<3;j++) {
1820                                 m=factor*dir[i]*dir[j];
1821                                 nlMatrixAdd(ia+i, op+ic+j, m);
1822                         }
1823         }
1824 }
1825
1826
1827 static void dfdx_goal(int ia, int ic, int op, float factor)
1828 {
1829         int i;
1830         for (i=0;i<3;i++) nlMatrixAdd(ia+i, op+ic+i, factor);
1831 }
1832
1833 static void dfdv_goal(int ia, int ic, float factor)
1834 {
1835         int i;
1836         for (i=0;i<3;i++) nlMatrixAdd(ia+i, ic+i, factor);
1837 }
1838 */
1839 static void sb_spring_force(Object *ob, int bpi, BodySpring *bs, float iks, float UNUSED(forcetime))
1840 {
1841         SoftBody *sb= ob->soft; /* is supposed to be there */
1842         BodyPoint  *bp1, *bp2;
1843
1844         float dir[3], dvel[3];
1845         float distance, forcefactor, kd, absvel, projvel, kw;
1846 #if 0   /* UNUSED */
1847         int ia, ic;
1848 #endif
1849         /* prepare depending on which side of the spring we are on */
1850         if (bpi == bs->v1) {
1851                 bp1 = &sb->bpoint[bs->v1];
1852                 bp2 = &sb->bpoint[bs->v2];
1853 #if 0   /* UNUSED */
1854                 ia =3*bs->v1;
1855                 ic =3*bs->v2;
1856 #endif
1857         }
1858         else if (bpi == bs->v2) {
1859                 bp1 = &sb->bpoint[bs->v2];
1860                 bp2 = &sb->bpoint[bs->v1];
1861 #if 0   /* UNUSED */
1862                 ia =3*bs->v2;
1863                 ic =3*bs->v1;
1864 #endif
1865         }
1866         else {
1867                 /* TODO make this debug option */
1868                 /**/
1869                 printf("bodypoint <bpi> is not attached to spring  <*bs> --> sb_spring_force()\n");
1870                 return;
1871         }
1872
1873         /* do bp1 <--> bp2 elastic */
1874         sub_v3_v3v3(dir, bp1->pos, bp2->pos);
1875         distance = normalize_v3(dir);
1876         if (bs->len < distance)
1877                 iks  = 1.0f/(1.0f-sb->inspring)-1.0f ;/* inner spring constants function */
1878         else
1879                 iks  = 1.0f/(1.0f-sb->inpush)-1.0f ;/* inner spring constants function */
1880
1881         if (bs->len > 0.0f) /* check for degenerated springs */
1882                 forcefactor = iks/bs->len;
1883         else
1884                 forcefactor = iks;
1885         kw = (bp1->springweight+bp2->springweight)/2.0f;
1886         kw = kw * kw;
1887         kw = kw * kw;
1888         switch (bs->springtype) {
1889                 case SB_EDGE:
1890                 case SB_HANDLE:
1891                         forcefactor *=  kw;
1892                         break;
1893                 case SB_BEND:
1894                         forcefactor *=sb->secondspring*kw;
1895                         break;
1896                 case SB_STIFFQUAD:
1897                         forcefactor *=sb->shearstiff*sb->shearstiff* kw;
1898                         break;
1899                 default:
1900                         break;
1901         }
1902
1903
1904         madd_v3_v3fl(bp1->force, dir, (bs->len - distance) * forcefactor);
1905
1906         /* do bp1 <--> bp2 viscous */
1907         sub_v3_v3v3(dvel, bp1->vec, bp2->vec);
1908         kd = sb->infrict * sb_fric_force_scale(ob);
1909         absvel  = normalize_v3(dvel);
1910         projvel = dot_v3v3(dir, dvel);
1911         kd     *= absvel * projvel;
1912         madd_v3_v3fl(bp1->force, dir, -kd);
1913 }
1914
1915
1916 /* since this is definitely the most CPU consuming task here .. try to spread it */
1917 /* core function _softbody_calc_forces_slice_in_a_thread */
1918 /* result is int to be able to flag user break */
1919 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)
1920 {
1921         float iks;
1922         int bb, do_selfcollision, do_springcollision, do_aero;
1923         int number_of_points_here = ilast - ifirst;
1924         SoftBody *sb= ob->soft; /* is supposed to be there */
1925         BodyPoint  *bp;
1926
1927         /* intitialize */
1928         if (sb) {
1929         /* check conditions for various options */
1930         /* +++ could be done on object level to squeeze out the last bits of it */
1931         do_selfcollision=((ob->softflag & OB_SB_EDGES) && (sb->bspring)&& (ob->softflag & OB_SB_SELF));
1932         do_springcollision=do_deflector && (ob->softflag & OB_SB_EDGES) &&(ob->softflag & OB_SB_EDGECOLL);
1933         do_aero=((sb->aeroedge)&& (ob->softflag & OB_SB_EDGES));
1934         /* --- could be done on object level to squeeze out the last bits of it */
1935         }
1936         else {
1937                 printf("Error expected a SB here\n");
1938                 return (999);
1939         }
1940
1941 /* debugerin */
1942         if (sb->totpoint < ifirst) {
1943                 printf("Aye 998");
1944                 return (998);
1945         }
1946 /* debugerin */
1947
1948
1949         bp = &sb->bpoint[ifirst];
1950         for (bb=number_of_points_here; bb>0; bb--, bp++) {
1951                 /* clear forces  accumulator */
1952                 bp->force[0] = bp->force[1] = bp->force[2] = 0.0;
1953                 /* naive ball self collision */
1954                 /* needs to be done if goal snaps or not */
1955                 if (do_selfcollision) {
1956                         int attached;
1957                         BodyPoint   *obp;
1958                         BodySpring *bs;
1959                         int c, b;
1960                         float velcenter[3], dvel[3], def[3];
1961                         float distance;
1962                         float compare;
1963                         float bstune = sb->ballstiff;
1964
1965                         for (c=sb->totpoint, obp= sb->bpoint; c>=ifirst+bb; c--, obp++) {
1966                                 compare = (obp->colball + bp->colball);
1967                                 sub_v3_v3v3(def, bp->pos, obp->pos);
1968                                 /* rather check the AABBoxes before ever calulating the real distance */
1969                                 /* mathematically it is completely nuts, but performance is pretty much (3) times faster */
1970                                 if ((ABS(def[0]) > compare) || (ABS(def[1]) > compare) || (ABS(def[2]) > compare)) continue;
1971                                 distance = normalize_v3(def);
1972                                 if (distance < compare ) {
1973                                         /* exclude body points attached with a spring */
1974                                         attached = 0;
1975                                         for (b=obp->nofsprings;b>0;b--) {
1976                                                 bs = sb->bspring + obp->springs[b-1];
1977                                                 if (( ilast-bb == bs->v2) || ( ilast-bb == bs->v1)) {
1978                                                         attached=1;
1979                                                         continue;}
1980                                         }
1981                                         if (!attached) {
1982                                                 float f = bstune / (distance) + bstune / (compare * compare) * distance - 2.0f * bstune / compare;
1983
1984                                                 mid_v3_v3v3(velcenter, bp->vec, obp->vec);
1985                                                 sub_v3_v3v3(dvel, velcenter, bp->vec);
1986                                                 mul_v3_fl(dvel, _final_mass(ob, bp));
1987
1988                                                 madd_v3_v3fl(bp->force, def, f * (1.0f - sb->balldamp));
1989                                                 madd_v3_v3fl(bp->force, dvel, sb->balldamp);
1990
1991                                                 /* exploit force(a, b) == -force(b, a) part2/2 */
1992                                                 sub_v3_v3v3(dvel, velcenter, obp->vec);
1993                                                 mul_v3_fl(dvel, _final_mass(ob, bp));
1994
1995                                                 madd_v3_v3fl(obp->force, dvel, sb->balldamp);
1996                                                 madd_v3_v3fl(obp->force, def, -f * (1.0f - sb->balldamp));
1997                                         }
1998                                 }
1999                         }
2000                 }
2001                 /* naive ball self collision done */
2002
2003                 if (_final_goal(ob, bp) < SOFTGOALSNAP) {  /* omit this bp when it snaps */
2004                         float auxvect[3];
2005                         float velgoal[3];
2006
2007                         /* do goal stuff */
2008                         if (ob->softflag & OB_SB_GOAL) {
2009                                 /* true elastic goal */
2010                                 float ks, kd;
2011                                 sub_v3_v3v3(auxvect, bp->pos, bp->origT);
2012                                 ks  = 1.0f / (1.0f - _final_goal(ob, bp) * sb->goalspring) - 1.0f;
2013                                 bp->force[0]+= -ks*(auxvect[0]);
2014                                 bp->force[1]+= -ks*(auxvect[1]);
2015                                 bp->force[2]+= -ks*(auxvect[2]);
2016
2017                                 /* calulate damping forces generated by goals*/
2018                                 sub_v3_v3v3(velgoal, bp->origS, bp->origE);
2019                                 kd =  sb->goalfrict * sb_fric_force_scale(ob);
2020                                 add_v3_v3v3(auxvect, velgoal, bp->vec);
2021
2022                                 if (forcetime > 0.0f) { /* make sure friction does not become rocket motor on time reversal */
2023                                         bp->force[0]-= kd * (auxvect[0]);
2024                                         bp->force[1]-= kd * (auxvect[1]);
2025                                         bp->force[2]-= kd * (auxvect[2]);
2026                                 }
2027                                 else {
2028                                         bp->force[0]-= kd * (velgoal[0] - bp->vec[0]);
2029                                         bp->force[1]-= kd * (velgoal[1] - bp->vec[1]);
2030                                         bp->force[2]-= kd * (velgoal[2] - bp->vec[2]);
2031                                 }
2032                         }
2033                         /* done goal stuff */
2034
2035                         /* gravitation */
2036                         if (scene->physics_settings.flag & PHYS_GLOBAL_GRAVITY) {
2037                                 float gravity[3];
2038                                 copy_v3_v3(gravity, scene->physics_settings.gravity);
2039                                 mul_v3_fl(gravity, sb_grav_force_scale(ob)*_final_mass(ob, bp)*sb->effector_weights->global_gravity); /* individual mass of node here */
2040                                 add_v3_v3(bp->force, gravity);
2041                         }
2042
2043                         /* particle field & vortex */
2044                         if (do_effector) {
2045                                 EffectedPoint epoint;
2046                                 float kd;
2047                                 float force[3] = {0.0f, 0.0f, 0.0f};
2048                                 float speed[3] = {0.0f, 0.0f, 0.0f};
2049                                 float eval_sb_fric_force_scale = sb_fric_force_scale(ob); /* just for calling function once */
2050                                 pd_point_from_soft(scene, bp->pos, bp->vec, sb->bpoint-bp, &epoint);
2051                                 pdDoEffectors(do_effector, NULL, sb->effector_weights, &epoint, force, speed);
2052
2053                                 /* apply forcefield*/
2054                                 mul_v3_fl(force, fieldfactor* eval_sb_fric_force_scale);
2055                                 add_v3_v3(bp->force, force);
2056
2057                                 /* BP friction in moving media */
2058                                 kd= sb->mediafrict* eval_sb_fric_force_scale;
2059                                 bp->force[0] -= kd * (bp->vec[0] + windfactor*speed[0]/eval_sb_fric_force_scale);
2060                                 bp->force[1] -= kd * (bp->vec[1] + windfactor*speed[1]/eval_sb_fric_force_scale);
2061                                 bp->force[2] -= kd * (bp->vec[2] + windfactor*speed[2]/eval_sb_fric_force_scale);
2062                                 /* now we'll have nice centrifugal effect for vortex */
2063
2064                         }
2065                         else {
2066                                 /* BP friction in media (not) moving*/
2067                                 float kd = sb->mediafrict* sb_fric_force_scale(ob);
2068                                 /* assume it to be proportional to actual velocity */
2069                                 bp->force[0]-= bp->vec[0]*kd;
2070                                 bp->force[1]-= bp->vec[1]*kd;
2071                                 bp->force[2]-= bp->vec[2]*kd;
2072                                 /* friction in media done */
2073                         }
2074                         /* +++cached collision targets */
2075                         bp->choke = 0.0f;
2076                         bp->choke2 = 0.0f;
2077                         bp->loc_flag &= ~SBF_DOFUZZY;
2078                         if (do_deflector && !(bp->loc_flag & SBF_OUTOFCOLLISION) ) {
2079                                 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;
2080                                 float kd = 1.0f;
2081
2082                                 if (sb_deflect_face(ob, bp->pos, facenormal, defforce, &cf, timenow, vel, &intrusion)) {
2083                                         if (intrusion < 0.0f) {
2084                                                 sb->scratch->flag |= SBF_DOFUZZY;
2085                                                 bp->loc_flag |= SBF_DOFUZZY;
2086                                                 bp->choke = sb->choke*0.01f;
2087                                         }
2088
2089                                         sub_v3_v3v3(cfforce, bp->vec, vel);
2090                                         madd_v3_v3fl(bp->force, cfforce, -cf * 50.0f);
2091
2092                                         madd_v3_v3fl(bp->force, defforce, kd);
2093                                 }
2094
2095                         }
2096                         /* ---cached collision targets */
2097
2098                         /* +++springs */
2099                         iks  = 1.0f/(1.0f-sb->inspring)-1.0f ;/* inner spring constants function */
2100                         if (ob->softflag & OB_SB_EDGES) {
2101                                 if (sb->bspring) { /* spring list exists at all ? */
2102                                         int b;
2103                                         BodySpring *bs;
2104                                         for (b=bp->nofsprings;b>0;b--) {
2105                                                 bs = sb->bspring + bp->springs[b-1];
2106                                                 if (do_springcollision || do_aero) {
2107                                                         add_v3_v3(bp->force, bs->ext_force);
2108                                                         if (bs->flag & BSF_INTERSECT)
2109                                                                 bp->choke = bs->cf;
2110
2111                                                 }
2112                                                 // sb_spring_force(Object *ob, int bpi, BodySpring *bs, float iks, float forcetime)
2113                                                 sb_spring_force(ob, ilast-bb, bs, iks, forcetime);
2114                                         }/* loop springs */
2115                                 }/* existing spring list */
2116                         }/*any edges*/
2117                         /* ---springs */
2118                 }/*omit on snap */
2119         }/*loop all bp's*/
2120         return 0; /*done fine*/
2121 }
2122
2123 static void *exec_softbody_calc_forces(void *data)
2124 {
2125         SB_thread_context *pctx = (SB_thread_context*)data;
2126         _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);
2127         return NULL;
2128 }
2129
2130 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)
2131 {
2132         ListBase threads;
2133         SB_thread_context *sb_threads;
2134         int i, totthread, left, dec;
2135         int lowpoints =100; /* wild guess .. may increase with better thread management 'above' or even be UI option sb->spawn_cf_threads_nopts */
2136
2137         /* figure the number of threads while preventing pretty pointless threading overhead */
2138         totthread= BKE_scene_num_threads(scene);
2139         /* what if we got zillions of CPUs running but less to spread*/
2140         while ((totpoint/totthread < lowpoints) && (totthread > 1)) {
2141                 totthread--;
2142         }
2143
2144         /* printf("sb_cf_threads_run spawning %d threads\n", totthread); */
2145
2146         sb_threads= MEM_callocN(sizeof(SB_thread_context)*totthread, "SBThread");
2147         memset(sb_threads, 0, sizeof(SB_thread_context)*totthread);
2148         left = totpoint;
2149         dec = totpoint/totthread +1;
2150         for (i=0; i<totthread; i++) {
2151                 sb_threads[i].scene = scene;
2152                 sb_threads[i].ob = ob;
2153                 sb_threads[i].forcetime = forcetime;
2154                 sb_threads[i].timenow = timenow;
2155                 sb_threads[i].ilast   = left;
2156                 left = left - dec;
2157                 if (left >0) {
2158                         sb_threads[i].ifirst  = left;
2159                 }
2160                 else
2161                         sb_threads[i].ifirst  = 0;
2162                 sb_threads[i].do_effector = do_effector;
2163                 sb_threads[i].do_deflector = do_deflector;
2164                 sb_threads[i].fieldfactor = fieldfactor;
2165                 sb_threads[i].windfactor  = windfactor;
2166                 sb_threads[i].nr= i;
2167                 sb_threads[i].tot= totthread;
2168         }
2169
2170
2171         if (totthread > 1) {
2172                 BLI_init_threads(&threads, exec_softbody_calc_forces, totthread);
2173
2174                 for (i=0; i<totthread; i++)
2175                         BLI_insert_thread(&threads, &sb_threads[i]);
2176
2177                 BLI_end_threads(&threads);
2178         }
2179         else
2180                 exec_softbody_calc_forces(&sb_threads[0]);
2181         /* clean up */
2182         MEM_freeN(sb_threads);
2183 }
2184
2185 static void softbody_calc_forcesEx(Scene *scene, Object *ob, float forcetime, float timenow)
2186 {
2187 /* rule we never alter free variables :bp->vec bp->pos in here !
2188  * this will ruin adaptive stepsize AKA heun! (BM)
2189  */
2190         SoftBody *sb= ob->soft; /* is supposed to be there */
2191         /*BodyPoint *bproot;*/ /* UNUSED */
2192         ListBase *do_effector = NULL;
2193         /* float gravity; */ /* UNUSED */
2194         /* float iks; */
2195         float fieldfactor = -1.0f, windfactor  = 0.25;
2196         int   do_deflector /*, do_selfcollision*/, do_springcollision, do_aero;
2197
2198         /* gravity = sb->grav * sb_grav_force_scale(ob); */ /* UNUSED */
2199
2200         /* check conditions for various options */
2201         do_deflector= query_external_colliders(scene, ob);
2202         /* do_selfcollision=((ob->softflag & OB_SB_EDGES) && (sb->bspring)&& (ob->softflag & OB_SB_SELF)); */ /* UNUSED */
2203         do_springcollision=do_deflector && (ob->softflag & OB_SB_EDGES) &&(ob->softflag & OB_SB_EDGECOLL);
2204         do_aero=((sb->aeroedge)&& (ob->softflag & OB_SB_EDGES));
2205
2206         /* iks  = 1.0f/(1.0f-sb->inspring)-1.0f; */ /* inner spring constants function */ /* UNUSED */
2207         /* bproot= sb->bpoint; */ /* need this for proper spring addressing */ /* UNUSED */
2208
2209         if (do_springcollision || do_aero)
2210                 sb_sfesf_threads_run(scene, ob, timenow, sb->totspring, NULL);
2211
2212         /* after spring scan because it uses Effoctors too */
2213         do_effector= pdInitEffectors(scene, ob, NULL, sb->effector_weights, true);
2214
2215         if (do_deflector) {
2216                 float defforce[3];
2217                 do_deflector = sb_detect_aabb_collisionCached(defforce, ob->lay, ob, timenow);
2218         }
2219
2220         sb_cf_threads_run(scene, ob, forcetime, timenow, sb->totpoint, NULL, do_effector, do_deflector, fieldfactor, windfactor);
2221
2222         /* finally add forces caused by face collision */
2223         if (ob->softflag & OB_SB_FACECOLL) scan_for_ext_face_forces(ob, timenow);
2224
2225         /* finish matrix and solve */
2226         pdEndEffectors(&do_effector);
2227 }
2228
2229
2230 static void softbody_calc_forces(Scene *scene, Object *ob, float forcetime, float timenow)
2231 {
2232         /* redirection to the new threaded Version */
2233         if (!(G.debug_value & 0x10)) { // 16
2234                 softbody_calc_forcesEx(scene, ob, forcetime, timenow);
2235                 return;
2236         }
2237         else {
2238                 /* so the following will die  */
2239                 /* |||||||||||||||||||||||||| */
2240                 /* VVVVVVVVVVVVVVVVVVVVVVVVVV */
2241                 /*backward compatibility note:
2242                 fixing bug [17428] which forces adaptive step size to tiny steps
2243                 in some situations
2244                 .. keeping G.debug_value==17 0x11 option for old files 'needing' the bug*/
2245
2246                 /* rule we never alter free variables :bp->vec bp->pos in here !
2247                 * this will ruin adaptive stepsize AKA heun! (BM)
2248                 */
2249                 SoftBody *sb= ob->soft; /* is supposed to be there */
2250                 BodyPoint  *bp;
2251                 /* BodyPoint *bproot; */ /* UNUSED */
2252                 BodySpring *bs;
2253                 ListBase *do_effector = NULL;
2254                 float iks, ks, kd, gravity[3] = {0.0f, 0.0f, 0.0f};
2255                 float fieldfactor = -1.0f, windfactor  = 0.25f;
2256                 float tune = sb->ballstiff;
2257                 int a, b,  do_deflector, do_selfcollision, do_springcollision, do_aero;
2258
2259                 if (scene->physics_settings.flag & PHYS_GLOBAL_GRAVITY) {
2260                         copy_v3_v3(gravity, scene->physics_settings.gravity);
2261                         mul_v3_fl(gravity, sb_grav_force_scale(ob)*sb->effector_weights->global_gravity);
2262                 }
2263
2264                 /* check conditions for various options */
2265                 do_deflector= query_external_colliders(scene, ob);
2266                 do_selfcollision=((ob->softflag & OB_SB_EDGES) && (sb->bspring)&& (ob->softflag & OB_SB_SELF));
2267                 do_springcollision=do_deflector && (ob->softflag & OB_SB_EDGES) &&(ob->softflag & OB_SB_EDGECOLL);
2268                 do_aero=((sb->aeroedge)&& (ob->softflag & OB_SB_EDGES));
2269
2270                 iks  = 1.0f/(1.0f-sb->inspring)-1.0f ;/* inner spring constants function */
2271                 /* bproot= sb->bpoint; */ /* need this for proper spring addressing */ /* UNUSED */
2272
2273                 if (do_springcollision || do_aero)  scan_for_ext_spring_forces(scene, ob, timenow);
2274                 /* after spring scan because it uses Effoctors too */
2275                 do_effector= pdInitEffectors(scene, ob, NULL, ob->soft->effector_weights, true);
2276
2277                 if (do_deflector) {
2278                         float defforce[3];
2279                         do_deflector = sb_detect_aabb_collisionCached(defforce, ob->lay, ob, timenow);
2280                 }
2281
2282                 for (a=sb->totpoint, bp= sb->bpoint; 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 (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                  * so we can *paint* them nicly ..