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