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