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