Cleanup: style, use braces for blenkernel
[blender.git] / source / blender / blenkernel / intern / softbody.c
1 /*
2  * This program is free software; you can redistribute it and/or
3  * modify it under the terms of the GNU General Public License
4  * as published by the Free Software Foundation; either version 2
5  * of the License, or (at your option) any later version.
6  *
7  * This program is distributed in the hope that it will be useful,
8  * but WITHOUT ANY WARRANTY; without even the implied warranty of
9  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
10  * GNU General Public License for more details.
11  *
12  * You should have received a copy of the GNU General Public License
13  * along with this program; if not, write to the Free Software Foundation,
14  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
15  *
16  * The Original Code is Copyright (C) Blender Foundation
17  * All rights reserved.
18  */
19
20 /** \file
21  * \ingroup bke
22  */
23
24 /**
25  * variables on the UI for now
26  * <pre>
27  * float mediafrict;  friction to env
28  * float nodemass;    softbody mass of *vertex*
29  * float grav;        softbody amount of gravitation to apply
30  *
31  * float goalspring;  softbody goal springs
32  * float goalfrict;   softbody goal springs friction
33  * float mingoal;     quick limits for goal
34  * float maxgoal;
35  *
36  * float inspring;    softbody inner springs
37  * float infrict;     softbody inner springs friction
38  * </pre>
39  */
40
41 #include <math.h>
42 #include <stdlib.h>
43 #include <string.h>
44
45 #include "CLG_log.h"
46
47 #include "MEM_guardedalloc.h"
48
49 /* types */
50 #include "DNA_collection_types.h"
51 #include "DNA_curve_types.h"
52 #include "DNA_lattice_types.h"
53 #include "DNA_meshdata_types.h"
54 #include "DNA_mesh_types.h"
55 #include "DNA_object_types.h"
56 #include "DNA_scene_types.h"
57
58 #include "BLI_math.h"
59 #include "BLI_utildefines.h"
60 #include "BLI_listbase.h"
61 #include "BLI_ghash.h"
62 #include "BLI_threads.h"
63
64 #include "BKE_collection.h"
65 #include "BKE_collision.h"
66 #include "BKE_curve.h"
67 #include "BKE_effect.h"
68 #include "BKE_global.h"
69 #include "BKE_layer.h"
70 #include "BKE_modifier.h"
71 #include "BKE_softbody.h"
72 #include "BKE_pointcache.h"
73 #include "BKE_deform.h"
74 #include "BKE_mesh.h"
75 #include "BKE_scene.h"
76
77 #include "DEG_depsgraph.h"
78 #include "DEG_depsgraph_query.h"
79
80 #include "PIL_time.h"
81
82 static CLG_LogRef LOG = {"bke.softbody"};
83
84 /* callbacks for errors and interrupts and some goo */
85 static int (*SB_localInterruptCallBack)(void) = NULL;
86
87 /* ********** soft body engine ******* */
88
89 typedef enum { SB_EDGE = 1, SB_BEND = 2, SB_STIFFQUAD = 3, SB_HANDLE = 4 } type_spring;
90
91 typedef struct BodySpring {
92   int v1, v2;
93   float len, cf, load;
94   float ext_force[3]; /* edges colliding and sailing */
95   type_spring springtype;
96   short flag;
97 } BodySpring;
98
99 typedef struct BodyFace {
100   int v1, v2, v3;
101   float ext_force[3]; /* faces colliding */
102   short flag;
103 } BodyFace;
104
105 typedef struct ReferenceVert {
106   float pos[3]; /* position relative to com */
107   float mass;   /* node mass */
108 } ReferenceVert;
109
110 typedef struct ReferenceState {
111   float com[3];         /* center of mass*/
112   ReferenceVert *ivert; /* list of initial values */
113 } ReferenceState;
114
115 /*private scratch pad for caching and other data only needed when alive*/
116 typedef struct SBScratch {
117   GHash *colliderhash;
118   short needstobuildcollider;
119   short flag;
120   BodyFace *bodyface;
121   int totface;
122   float aabbmin[3], aabbmax[3];
123   ReferenceState Ref;
124 } SBScratch;
125
126 typedef struct SB_thread_context {
127   Scene *scene;
128   Object *ob;
129   float forcetime;
130   float timenow;
131   int ifirst;
132   int ilast;
133   ListBase *effectors;
134   int do_deflector;
135   float fieldfactor;
136   float windfactor;
137   int nr;
138   int tot;
139 } SB_thread_context;
140
141 #define MID_PRESERVE 1
142
143 #define SOFTGOALSNAP 0.999f
144 /* if bp-> goal is above make it a *forced follow original* and skip all ODE stuff for this bp
145  * removes *unnecessary* stiffness from ODE system
146  */
147 #define HEUNWARNLIMIT 1 /* 500 would be fine i think for detecting severe *stiff* stuff */
148
149 #define BSF_INTERSECT 1 /* edge intersects collider face */
150
151 /* private definitions for bodypoint states */
152 #define SBF_DOFUZZY 1        /* Bodypoint do fuzzy */
153 #define SBF_OUTOFCOLLISION 2 /* Bodypoint does not collide  */
154
155 #define BFF_INTERSECT 1 /* collider edge   intrudes face */
156 #define BFF_CLOSEVERT 2 /* collider vertex repulses face */
157
158 static float SoftHeunTol =
159     1.0f; /* humm .. this should be calculated from sb parameters and sizes */
160
161 /* local prototypes */
162 static void free_softbody_intern(SoftBody *sb);
163
164 /*+++ frame based timing +++*/
165
166 /*physical unit of force is [kg * m / sec^2]*/
167
168 static float sb_grav_force_scale(Object *UNUSED(ob))
169 /* since unit of g is [m/sec^2] and F = mass * g we rescale unit mass of node to 1 gramm
170  * put it to a function here, so we can add user options later without touching simulation code
171  */
172 {
173   return (0.001f);
174 }
175
176 static float sb_fric_force_scale(Object *UNUSED(ob))
177 /* rescaling unit of drag [1 / sec] to somehow reasonable
178  * put it to a function here, so we can add user options later without touching simulation code
179  */
180 {
181   return (0.01f);
182 }
183
184 static float sb_time_scale(Object *ob)
185 /* defining the frames to *real* time relation */
186 {
187   SoftBody *sb = ob->soft; /* is supposed to be there */
188   if (sb) {
189     return (sb->physics_speed);
190     /* hrms .. this could be IPO as well :)
191      * estimated range [0.001 sluggish slug - 100.0 very fast (i hope ODE solver can handle that)]
192      * 1 approx = a unit 1 pendulum at g = 9.8 [earth conditions]  has period 65 frames
193      * theory would give a 50 frames period .. so there must be something inaccurate .. looking for that (BM)
194      */
195   }
196   return (1.0f);
197   /*
198    * this would be frames/sec independent timing assuming 25 fps is default
199    * but does not work very well with NLA
200    * return (25.0f/scene->r.frs_sec)
201    */
202 }
203 /*--- frame based timing ---*/
204
205 /* helper functions for everything is animatable jow_go_for2_5 +++++++*/
206 /* introducing them here, because i know: steps in properties  ( at frame timing )
207  * will cause unwanted responses of the softbody system (which does inter frame calculations )
208  * so first 'cure' would be: interpolate linear in time ..
209  * Q: why do i write this?
210  * A: because it happened once, that some eger coder 'streamlined' code to fail.
211  * We DO linear interpolation for goals .. and i think we should do on animated properties as well
212  */
213
214 /* animate sb->maxgoal, sb->mingoal */
215 static float _final_goal(Object *ob, BodyPoint *bp) /*jow_go_for2_5 */
216 {
217   float f = -1999.99f;
218   if (ob) {
219     SoftBody *sb = ob->soft; /* is supposed to be there */
220     if (!(ob->softflag & OB_SB_GOAL)) {
221       return (0.0f);
222     }
223     if (sb && bp) {
224       if (bp->goal < 0.0f) {
225         return (0.0f);
226       }
227       f = sb->mingoal + bp->goal * fabsf(sb->maxgoal - sb->mingoal);
228       f = pow(f, 4.0f);
229       return (f);
230     }
231   }
232   CLOG_ERROR(&LOG, "sb or bp == NULL");
233   return f; /*using crude but spot able values some times helps debuggin */
234 }
235
236 static float _final_mass(Object *ob, BodyPoint *bp)
237 {
238   if (ob) {
239     SoftBody *sb = ob->soft; /* is supposed to be there */
240     if (sb && bp) {
241       return (bp->mass * sb->nodemass);
242     }
243   }
244   CLOG_ERROR(&LOG, "sb or bp == NULL");
245   return 1.0f;
246 }
247 /* helper functions for everything is animateble jow_go_for2_5 ------*/
248
249 /*+++ collider caching and dicing +++*/
250
251 /*
252  * for each target object/face the axis aligned bounding box (AABB) is stored
253  * faces parallel to global axes
254  * so only simple "value" in [min, max] ckecks are used
255  * float operations still
256  */
257
258 /* just an ID here to reduce the prob for killing objects
259  * ob->sumohandle points to we should not kill :)
260  */
261 static const int CCD_SAVETY = 190561;
262
263 typedef struct ccdf_minmax {
264   float minx, miny, minz, maxx, maxy, maxz;
265 } ccdf_minmax;
266
267 typedef struct ccd_Mesh {
268   int mvert_num, tri_num;
269   const MVert *mvert;
270   const MVert *mprevvert;
271   const MVertTri *tri;
272   int savety;
273   ccdf_minmax *mima;
274   /* Axis Aligned Bounding Box AABB */
275   float bbmin[3];
276   float bbmax[3];
277 } ccd_Mesh;
278
279 static ccd_Mesh *ccd_mesh_make(Object *ob)
280 {
281   CollisionModifierData *cmd;
282   ccd_Mesh *pccd_M = NULL;
283   ccdf_minmax *mima;
284   const MVertTri *vt;
285   float hull;
286   int i;
287
288   cmd = (CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
289
290   /* first some paranoia checks */
291   if (!cmd) {
292     return NULL;
293   }
294   if (!cmd->mvert_num || !cmd->tri_num) {
295     return NULL;
296   }
297
298   pccd_M = MEM_mallocN(sizeof(ccd_Mesh), "ccd_Mesh");
299   pccd_M->mvert_num = cmd->mvert_num;
300   pccd_M->tri_num = cmd->tri_num;
301   pccd_M->savety = CCD_SAVETY;
302   pccd_M->bbmin[0] = pccd_M->bbmin[1] = pccd_M->bbmin[2] = 1e30f;
303   pccd_M->bbmax[0] = pccd_M->bbmax[1] = pccd_M->bbmax[2] = -1e30f;
304   pccd_M->mprevvert = NULL;
305
306   /* blow it up with forcefield ranges */
307   hull = max_ff(ob->pd->pdef_sbift, ob->pd->pdef_sboft);
308
309   /* alloc and copy verts*/
310   pccd_M->mvert = MEM_dupallocN(cmd->xnew);
311   /* note that xnew coords are already in global space, */
312   /* determine the ortho BB */
313   for (i = 0; i < pccd_M->mvert_num; i++) {
314     const float *v;
315
316     /* evaluate limits */
317     v = pccd_M->mvert[i].co;
318     pccd_M->bbmin[0] = min_ff(pccd_M->bbmin[0], v[0] - hull);
319     pccd_M->bbmin[1] = min_ff(pccd_M->bbmin[1], v[1] - hull);
320     pccd_M->bbmin[2] = min_ff(pccd_M->bbmin[2], v[2] - hull);
321
322     pccd_M->bbmax[0] = max_ff(pccd_M->bbmax[0], v[0] + hull);
323     pccd_M->bbmax[1] = max_ff(pccd_M->bbmax[1], v[1] + hull);
324     pccd_M->bbmax[2] = max_ff(pccd_M->bbmax[2], v[2] + hull);
325   }
326   /* alloc and copy faces*/
327   pccd_M->tri = MEM_dupallocN(cmd->tri);
328
329   /* OBBs for idea1 */
330   pccd_M->mima = MEM_mallocN(sizeof(ccdf_minmax) * pccd_M->tri_num, "ccd_Mesh_Faces_mima");
331
332   /* anyhoo we need to walk the list of faces and find OBB they live in */
333   for (i = 0, mima = pccd_M->mima, vt = pccd_M->tri; i < pccd_M->tri_num; i++, mima++, vt++) {
334     const float *v;
335
336     mima->minx = mima->miny = mima->minz = 1e30f;
337     mima->maxx = mima->maxy = mima->maxz = -1e30f;
338
339     v = pccd_M->mvert[vt->tri[0]].co;
340     mima->minx = min_ff(mima->minx, v[0] - hull);
341     mima->miny = min_ff(mima->miny, v[1] - hull);
342     mima->minz = min_ff(mima->minz, v[2] - hull);
343     mima->maxx = max_ff(mima->maxx, v[0] + hull);
344     mima->maxy = max_ff(mima->maxy, v[1] + hull);
345     mima->maxz = max_ff(mima->maxz, v[2] + hull);
346
347     v = pccd_M->mvert[vt->tri[1]].co;
348     mima->minx = min_ff(mima->minx, v[0] - hull);
349     mima->miny = min_ff(mima->miny, v[1] - hull);
350     mima->minz = min_ff(mima->minz, v[2] - hull);
351     mima->maxx = max_ff(mima->maxx, v[0] + hull);
352     mima->maxy = max_ff(mima->maxy, v[1] + hull);
353     mima->maxz = max_ff(mima->maxz, v[2] + hull);
354
355     v = pccd_M->mvert[vt->tri[2]].co;
356     mima->minx = min_ff(mima->minx, v[0] - hull);
357     mima->miny = min_ff(mima->miny, v[1] - hull);
358     mima->minz = min_ff(mima->minz, v[2] - hull);
359     mima->maxx = max_ff(mima->maxx, v[0] + hull);
360     mima->maxy = max_ff(mima->maxy, v[1] + hull);
361     mima->maxz = max_ff(mima->maxz, v[2] + hull);
362   }
363
364   return pccd_M;
365 }
366 static void ccd_mesh_update(Object *ob, ccd_Mesh *pccd_M)
367 {
368   CollisionModifierData *cmd;
369   ccdf_minmax *mima;
370   const MVertTri *vt;
371   float hull;
372   int i;
373
374   cmd = (CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
375
376   /* first some paranoia checks */
377   if (!cmd) {
378     return;
379   }
380   if (!cmd->mvert_num || !cmd->tri_num) {
381     return;
382   }
383
384   if ((pccd_M->mvert_num != cmd->mvert_num) || (pccd_M->tri_num != cmd->tri_num)) {
385     return;
386   }
387
388   pccd_M->bbmin[0] = pccd_M->bbmin[1] = pccd_M->bbmin[2] = 1e30f;
389   pccd_M->bbmax[0] = pccd_M->bbmax[1] = pccd_M->bbmax[2] = -1e30f;
390
391   /* blow it up with forcefield ranges */
392   hull = max_ff(ob->pd->pdef_sbift, ob->pd->pdef_sboft);
393
394   /* rotate current to previous */
395   if (pccd_M->mprevvert) {
396     MEM_freeN((void *)pccd_M->mprevvert);
397   }
398   pccd_M->mprevvert = pccd_M->mvert;
399   /* alloc and copy verts*/
400   pccd_M->mvert = MEM_dupallocN(cmd->xnew);
401   /* note that xnew coords are already in global space, */
402   /* determine the ortho BB */
403   for (i = 0; i < pccd_M->mvert_num; i++) {
404     const float *v;
405
406     /* evaluate limits */
407     v = pccd_M->mvert[i].co;
408     pccd_M->bbmin[0] = min_ff(pccd_M->bbmin[0], v[0] - hull);
409     pccd_M->bbmin[1] = min_ff(pccd_M->bbmin[1], v[1] - hull);
410     pccd_M->bbmin[2] = min_ff(pccd_M->bbmin[2], v[2] - hull);
411
412     pccd_M->bbmax[0] = max_ff(pccd_M->bbmax[0], v[0] + hull);
413     pccd_M->bbmax[1] = max_ff(pccd_M->bbmax[1], v[1] + hull);
414     pccd_M->bbmax[2] = max_ff(pccd_M->bbmax[2], v[2] + hull);
415
416     /* evaluate limits */
417     v = pccd_M->mprevvert[i].co;
418     pccd_M->bbmin[0] = min_ff(pccd_M->bbmin[0], v[0] - hull);
419     pccd_M->bbmin[1] = min_ff(pccd_M->bbmin[1], v[1] - hull);
420     pccd_M->bbmin[2] = min_ff(pccd_M->bbmin[2], v[2] - hull);
421
422     pccd_M->bbmax[0] = max_ff(pccd_M->bbmax[0], v[0] + hull);
423     pccd_M->bbmax[1] = max_ff(pccd_M->bbmax[1], v[1] + hull);
424     pccd_M->bbmax[2] = max_ff(pccd_M->bbmax[2], v[2] + hull);
425   }
426
427   /* anyhoo we need to walk the list of faces and find OBB they live in */
428   for (i = 0, mima = pccd_M->mima, vt = pccd_M->tri; i < pccd_M->tri_num; i++, mima++, vt++) {
429     const float *v;
430
431     mima->minx = mima->miny = mima->minz = 1e30f;
432     mima->maxx = mima->maxy = mima->maxz = -1e30f;
433
434     /* mvert */
435     v = pccd_M->mvert[vt->tri[0]].co;
436     mima->minx = min_ff(mima->minx, v[0] - hull);
437     mima->miny = min_ff(mima->miny, v[1] - hull);
438     mima->minz = min_ff(mima->minz, v[2] - hull);
439     mima->maxx = max_ff(mima->maxx, v[0] + hull);
440     mima->maxy = max_ff(mima->maxy, v[1] + hull);
441     mima->maxz = max_ff(mima->maxz, v[2] + hull);
442
443     v = pccd_M->mvert[vt->tri[1]].co;
444     mima->minx = min_ff(mima->minx, v[0] - hull);
445     mima->miny = min_ff(mima->miny, v[1] - hull);
446     mima->minz = min_ff(mima->minz, v[2] - hull);
447     mima->maxx = max_ff(mima->maxx, v[0] + hull);
448     mima->maxy = max_ff(mima->maxy, v[1] + hull);
449     mima->maxz = max_ff(mima->maxz, v[2] + hull);
450
451     v = pccd_M->mvert[vt->tri[2]].co;
452     mima->minx = min_ff(mima->minx, v[0] - hull);
453     mima->miny = min_ff(mima->miny, v[1] - hull);
454     mima->minz = min_ff(mima->minz, v[2] - hull);
455     mima->maxx = max_ff(mima->maxx, v[0] + hull);
456     mima->maxy = max_ff(mima->maxy, v[1] + hull);
457     mima->maxz = max_ff(mima->maxz, v[2] + hull);
458
459     /* mprevvert */
460     v = pccd_M->mprevvert[vt->tri[0]].co;
461     mima->minx = min_ff(mima->minx, v[0] - hull);
462     mima->miny = min_ff(mima->miny, v[1] - hull);
463     mima->minz = min_ff(mima->minz, v[2] - hull);
464     mima->maxx = max_ff(mima->maxx, v[0] + hull);
465     mima->maxy = max_ff(mima->maxy, v[1] + hull);
466     mima->maxz = max_ff(mima->maxz, v[2] + hull);
467
468     v = pccd_M->mprevvert[vt->tri[1]].co;
469     mima->minx = min_ff(mima->minx, v[0] - hull);
470     mima->miny = min_ff(mima->miny, v[1] - hull);
471     mima->minz = min_ff(mima->minz, v[2] - hull);
472     mima->maxx = max_ff(mima->maxx, v[0] + hull);
473     mima->maxy = max_ff(mima->maxy, v[1] + hull);
474     mima->maxz = max_ff(mima->maxz, v[2] + hull);
475
476     v = pccd_M->mprevvert[vt->tri[2]].co;
477     mima->minx = min_ff(mima->minx, v[0] - hull);
478     mima->miny = min_ff(mima->miny, v[1] - hull);
479     mima->minz = min_ff(mima->minz, v[2] - hull);
480     mima->maxx = max_ff(mima->maxx, v[0] + hull);
481     mima->maxy = max_ff(mima->maxy, v[1] + hull);
482     mima->maxz = max_ff(mima->maxz, v[2] + hull);
483   }
484   return;
485 }
486
487 static void ccd_mesh_free(ccd_Mesh *ccdm)
488 {
489   if (ccdm && (ccdm->savety == CCD_SAVETY)) { /*make sure we're not nuking objects we don't know*/
490     MEM_freeN((void *)ccdm->mvert);
491     MEM_freeN((void *)ccdm->tri);
492     if (ccdm->mprevvert) {
493       MEM_freeN((void *)ccdm->mprevvert);
494     }
495     MEM_freeN(ccdm->mima);
496     MEM_freeN(ccdm);
497     ccdm = NULL;
498   }
499 }
500
501 static void ccd_build_deflector_hash_single(GHash *hash, Object *ob)
502 {
503   /* only with deflecting set */
504   if (ob->pd && ob->pd->deflect) {
505     void **val_p;
506     if (!BLI_ghash_ensure_p(hash, ob, &val_p)) {
507       ccd_Mesh *ccdmesh = ccd_mesh_make(ob);
508       *val_p = ccdmesh;
509     }
510   }
511 }
512
513 /**
514  * \note collection overrides scene when not NULL.
515  */
516 static void ccd_build_deflector_hash(Depsgraph *depsgraph,
517                                      Collection *collection,
518                                      Object *vertexowner,
519                                      GHash *hash)
520 {
521   if (!hash) {
522     return;
523   }
524
525   unsigned int numobjects;
526   Object **objects = BKE_collision_objects_create(
527       depsgraph, vertexowner, collection, &numobjects, eModifierType_Collision);
528
529   for (int i = 0; i < numobjects; i++) {
530     Object *ob = objects[i];
531
532     if (ob->type == OB_MESH) {
533       ccd_build_deflector_hash_single(hash, ob);
534     }
535   }
536
537   BKE_collision_objects_free(objects);
538 }
539
540 static void ccd_update_deflector_hash_single(GHash *hash, Object *ob)
541 {
542   if (ob->pd && ob->pd->deflect) {
543     ccd_Mesh *ccdmesh = BLI_ghash_lookup(hash, ob);
544     if (ccdmesh) {
545       ccd_mesh_update(ob, ccdmesh);
546     }
547   }
548 }
549
550 /**
551  * \note collection overrides scene when not NULL.
552  */
553 static void ccd_update_deflector_hash(Depsgraph *depsgraph,
554                                       Collection *collection,
555                                       Object *vertexowner,
556                                       GHash *hash)
557 {
558   if ((!hash) || (!vertexowner)) {
559     return;
560   }
561
562   unsigned int numobjects;
563   Object **objects = BKE_collision_objects_create(
564       depsgraph, vertexowner, collection, &numobjects, eModifierType_Collision);
565
566   for (int i = 0; i < numobjects; i++) {
567     Object *ob = objects[i];
568
569     if (ob->type == OB_MESH) {
570       ccd_update_deflector_hash_single(hash, ob);
571     }
572   }
573
574   BKE_collision_objects_free(objects);
575 }
576
577 /*--- collider caching and dicing ---*/
578
579 static int count_mesh_quads(Mesh *me)
580 {
581   int a, result = 0;
582   const MPoly *mp = me->mpoly;
583
584   if (mp) {
585     for (a = me->totpoly; a > 0; a--, mp++) {
586       if (mp->totloop == 4) {
587         result++;
588       }
589     }
590   }
591   return result;
592 }
593
594 static void add_mesh_quad_diag_springs(Object *ob)
595 {
596   Mesh *me = ob->data;
597   /*BodyPoint *bp;*/ /*UNUSED*/
598   int a;
599
600   if (ob->soft) {
601     int nofquads;
602     //float s_shear = ob->soft->shearstiff*ob->soft->shearstiff;
603
604     nofquads = count_mesh_quads(me);
605     if (nofquads) {
606       const MLoop *mloop = me->mloop;
607       const MPoly *mp = me->mpoly;
608       BodySpring *bs;
609
610       /* resize spring-array to hold additional quad springs */
611       ob->soft->bspring = MEM_recallocN(ob->soft->bspring,
612                                         sizeof(BodySpring) * (ob->soft->totspring + nofquads * 2));
613
614       /* fill the tail */
615       a = 0;
616       bs = &ob->soft->bspring[ob->soft->totspring];
617       /*bp= ob->soft->bpoint; */ /*UNUSED*/
618       for (a = me->totpoly; a > 0; a--, mp++) {
619         if (mp->totloop == 4) {
620           bs->v1 = mloop[mp->loopstart + 0].v;
621           bs->v2 = mloop[mp->loopstart + 2].v;
622           bs->springtype = SB_STIFFQUAD;
623           bs++;
624           bs->v1 = mloop[mp->loopstart + 1].v;
625           bs->v2 = mloop[mp->loopstart + 3].v;
626           bs->springtype = SB_STIFFQUAD;
627           bs++;
628         }
629       }
630
631       /* now we can announce new springs */
632       ob->soft->totspring += nofquads * 2;
633     }
634   }
635 }
636
637 static void add_2nd_order_roller(Object *ob, float UNUSED(stiffness), int *counter, int addsprings)
638 {
639   /*assume we have a softbody*/
640   SoftBody *sb = ob->soft; /* is supposed to be there */
641   BodyPoint *bp, *bpo;
642   BodySpring *bs, *bs2, *bs3 = NULL;
643   int a, b, c, notthis = 0, v0;
644   if (!sb->bspring) {
645     return;
646   } /* we are 2nd order here so 1rst should have been build :) */
647   /* first run counting  second run adding */
648   *counter = 0;
649   if (addsprings) {
650     bs3 = ob->soft->bspring + ob->soft->totspring;
651   }
652   for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
653     /*scan for neighborhood*/
654     bpo = NULL;
655     v0 = (sb->totpoint - a);
656     for (b = bp->nofsprings; b > 0; b--) {
657       bs = sb->bspring + bp->springs[b - 1];
658       /* Nasty thing here that springs have two ends
659        * so here we have to make sure we examine the other */
660       if (v0 == bs->v1) {
661         bpo = sb->bpoint + bs->v2;
662         notthis = bs->v2;
663       }
664       else {
665         if (v0 == bs->v2) {
666           bpo = sb->bpoint + bs->v1;
667           notthis = bs->v1;
668         }
669         else {
670           CLOG_ERROR(&LOG, "oops we should not get here");
671         }
672       }
673       if (bpo) { /* so now we have a 2nd order humpdidump */
674         for (c = bpo->nofsprings; c > 0; c--) {
675           bs2 = sb->bspring + bpo->springs[c - 1];
676           if ((bs2->v1 != notthis) && (bs2->v1 > v0)) {
677             (*counter)++; /*hit */
678             if (addsprings) {
679               bs3->v1 = v0;
680               bs3->v2 = bs2->v1;
681               bs3->springtype = SB_BEND;
682               bs3++;
683             }
684           }
685           if ((bs2->v2 != notthis) && (bs2->v2 > v0)) {
686             (*counter)++; /* hit */
687             if (addsprings) {
688               bs3->v1 = v0;
689               bs3->v2 = bs2->v2;
690               bs3->springtype = SB_BEND;
691               bs3++;
692             }
693           }
694         }
695       }
696     }
697     /*scan for neighborhood done*/
698   }
699 }
700
701 static void add_2nd_order_springs(Object *ob, float stiffness)
702 {
703   int counter = 0;
704   BodySpring *bs_new;
705   stiffness *= stiffness;
706
707   add_2nd_order_roller(ob, stiffness, &counter, 0); /* counting */
708   if (counter) {
709     /* resize spring-array to hold additional springs */
710     bs_new = MEM_callocN((ob->soft->totspring + counter) * sizeof(BodySpring), "bodyspring");
711     memcpy(bs_new, ob->soft->bspring, (ob->soft->totspring) * sizeof(BodySpring));
712
713     if (ob->soft->bspring) {
714       MEM_freeN(ob->soft->bspring);
715     }
716     ob->soft->bspring = bs_new;
717
718     add_2nd_order_roller(ob, stiffness, &counter, 1); /* adding */
719     ob->soft->totspring += counter;
720   }
721 }
722
723 static void add_bp_springlist(BodyPoint *bp, int springID)
724 {
725   int *newlist;
726
727   if (bp->springs == NULL) {
728     bp->springs = MEM_callocN(sizeof(int), "bpsprings");
729     bp->springs[0] = springID;
730     bp->nofsprings = 1;
731   }
732   else {
733     bp->nofsprings++;
734     newlist = MEM_callocN(bp->nofsprings * sizeof(int), "bpsprings");
735     memcpy(newlist, bp->springs, (bp->nofsprings - 1) * sizeof(int));
736     MEM_freeN(bp->springs);
737     bp->springs = newlist;
738     bp->springs[bp->nofsprings - 1] = springID;
739   }
740 }
741
742 /**
743  * Do this once when sb is build it is `O(N^2)`
744  * so scanning for springs every iteration is too expensive.
745  */
746 static void build_bps_springlist(Object *ob)
747 {
748   SoftBody *sb = ob->soft; /* is supposed to be there */
749   BodyPoint *bp;
750   BodySpring *bs;
751   int a, b;
752
753   if (sb == NULL) {
754     return; /* paranoya check */
755   }
756
757   for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
758     /* throw away old list */
759     if (bp->springs) {
760       MEM_freeN(bp->springs);
761       bp->springs = NULL;
762     }
763     /* scan for attached inner springs */
764     for (b = sb->totspring, bs = sb->bspring; b > 0; b--, bs++) {
765       if (((sb->totpoint - a) == bs->v1)) {
766         add_bp_springlist(bp, sb->totspring - b);
767       }
768       if (((sb->totpoint - a) == bs->v2)) {
769         add_bp_springlist(bp, sb->totspring - b);
770       }
771     } /*for springs*/
772   }   /*for bp*/
773 }
774
775 static void calculate_collision_balls(Object *ob)
776 {
777   SoftBody *sb = ob->soft; /* is supposed to be there */
778   BodyPoint *bp;
779   BodySpring *bs;
780   int a, b, akku_count;
781   float min, max, akku;
782
783   if (sb == NULL) {
784     return; /* paranoya check */
785   }
786
787   for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
788     bp->colball = 0;
789     akku = 0.0f;
790     akku_count = 0;
791     min = 1e22f;
792     max = -1e22f;
793     /* first estimation based on attached */
794     for (b = bp->nofsprings; b > 0; b--) {
795       bs = sb->bspring + bp->springs[b - 1];
796       if (bs->springtype == SB_EDGE) {
797         akku += bs->len;
798         akku_count++;
799         min = min_ff(bs->len, min);
800         max = max_ff(bs->len, max);
801       }
802     }
803
804     if (akku_count > 0) {
805       if (sb->sbc_mode == SBC_MODE_MANUAL) {
806         bp->colball = sb->colball;
807       }
808       if (sb->sbc_mode == SBC_MODE_AVG) {
809         bp->colball = akku / (float)akku_count * sb->colball;
810       }
811       if (sb->sbc_mode == SBC_MODE_MIN) {
812         bp->colball = min * sb->colball;
813       }
814       if (sb->sbc_mode == SBC_MODE_MAX) {
815         bp->colball = max * sb->colball;
816       }
817       if (sb->sbc_mode == SBC_MODE_AVGMINMAX) {
818         bp->colball = (min + max) / 2.0f * sb->colball;
819       }
820     }
821     else {
822       bp->colball = 0;
823     }
824   } /*for bp*/
825 }
826
827 /* creates new softbody if didn't exist yet, makes new points and springs arrays */
828 static void renew_softbody(Scene *scene, Object *ob, int totpoint, int totspring)
829 {
830   SoftBody *sb;
831   int i;
832   short softflag;
833   if (ob->soft == NULL) {
834     ob->soft = sbNew(scene);
835   }
836   else {
837     free_softbody_intern(ob->soft);
838   }
839   sb = ob->soft;
840   softflag = ob->softflag;
841
842   if (totpoint) {
843     sb->totpoint = totpoint;
844     sb->totspring = totspring;
845
846     sb->bpoint = MEM_mallocN(totpoint * sizeof(BodyPoint), "bodypoint");
847     if (totspring) {
848       sb->bspring = MEM_mallocN(totspring * sizeof(BodySpring), "bodyspring");
849     }
850
851     /* initialize BodyPoint array */
852     for (i = 0; i < totpoint; i++) {
853       BodyPoint *bp = &sb->bpoint[i];
854
855       /* hum as far as i see this is overridden by _final_goal() now jow_go_for2_5 */
856       /* sadly breaks compatibility with older versions */
857       /* but makes goals behave the same for meshes, lattices and curves */
858       if (softflag & OB_SB_GOAL) {
859         bp->goal = sb->defgoal;
860       }
861       else {
862         bp->goal = 0.0f;
863         /* so this will definily be below SOFTGOALSNAP */
864       }
865
866       bp->nofsprings = 0;
867       bp->springs = NULL;
868       bp->choke = 0.0f;
869       bp->choke2 = 0.0f;
870       bp->frozen = 1.0f;
871       bp->colball = 0.0f;
872       bp->loc_flag = 0;
873       bp->springweight = 1.0f;
874       bp->mass = 1.0f;
875     }
876   }
877 }
878
879 static void free_softbody_baked(SoftBody *sb)
880 {
881   SBVertex *key;
882   int k;
883
884   for (k = 0; k < sb->totkey; k++) {
885     key = *(sb->keys + k);
886     if (key) {
887       MEM_freeN(key);
888     }
889   }
890   if (sb->keys) {
891     MEM_freeN(sb->keys);
892   }
893
894   sb->keys = NULL;
895   sb->totkey = 0;
896 }
897 static void free_scratch(SoftBody *sb)
898 {
899   if (sb->scratch) {
900     /* todo make sure everything is cleaned up nicly */
901     if (sb->scratch->colliderhash) {
902       BLI_ghash_free(sb->scratch->colliderhash,
903                      NULL,
904                      (GHashValFreeFP)ccd_mesh_free); /*this hoepfully will free all caches*/
905       sb->scratch->colliderhash = NULL;
906     }
907     if (sb->scratch->bodyface) {
908       MEM_freeN(sb->scratch->bodyface);
909     }
910     if (sb->scratch->Ref.ivert) {
911       MEM_freeN(sb->scratch->Ref.ivert);
912     }
913     MEM_freeN(sb->scratch);
914     sb->scratch = NULL;
915   }
916 }
917
918 /* only frees internal data */
919 static void free_softbody_intern(SoftBody *sb)
920 {
921   if (sb) {
922     int a;
923     BodyPoint *bp;
924
925     if (sb->bpoint) {
926       for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
927         /* free spring list */
928         if (bp->springs != NULL) {
929           MEM_freeN(bp->springs);
930         }
931       }
932       MEM_freeN(sb->bpoint);
933     }
934
935     if (sb->bspring) {
936       MEM_freeN(sb->bspring);
937     }
938
939     sb->totpoint = sb->totspring = 0;
940     sb->bpoint = NULL;
941     sb->bspring = NULL;
942
943     free_scratch(sb);
944     free_softbody_baked(sb);
945   }
946 }
947
948 /* ************ dynamics ********** */
949
950 /* the most general (micro physics correct) way to do collision
951  * (only needs the current particle position)
952  *
953  * it actually checks if the particle intrudes a short range force field generated
954  * by the faces of the target object and returns a force to drive the particel out
955  * the strength of the field grows exponetially if the particle is on the 'wrong' side of the face
956  * 'wrong' side : projection to the face normal is negative (all referred to a vertex in the face)
957  *
958  * flaw of this: 'fast' particles as well as 'fast' colliding faces
959  * give a 'tunnel' effect such that the particle passes through the force field
960  * without ever 'seeing' it
961  * this is fully compliant to heisenberg: h >= fuzzy(location) * fuzzy(time)
962  * besides our h is way larger than in QM because forces propagate way slower here
963  * we have to deal with fuzzy(time) in the range of 1/25 seconds (typical frame rate)
964  * yup collision targets are not known here any better
965  * and 1/25 second is looong compared to real collision events
966  * Q: why not use 'simple' collision here like bouncing back a particle
967  *   --> reverting is velocity on the face normal
968  * A: because our particles are not alone here
969  *    and need to tell their neighbors exactly what happens via spring forces
970  * unless sbObjectStep( .. ) is called on sub frame timing level
971  * BTW that also questions the use of a 'implicit' solvers on softbodies
972  * since that would only valid for 'slow' moving collision targets and dito particles
973  */
974
975 /* +++ dependency information functions*/
976
977 /**
978  * \note collection overrides scene when not NULL.
979  */
980 static int query_external_colliders(Depsgraph *depsgraph, Collection *collection)
981 {
982   unsigned int numobjects;
983   Object **objects = BKE_collision_objects_create(
984       depsgraph, NULL, collection, &numobjects, eModifierType_Collision);
985   BKE_collision_objects_free(objects);
986
987   return (numobjects != 0);
988 }
989 /* --- dependency information functions*/
990
991 /* +++ the aabb "force" section*/
992 static int sb_detect_aabb_collisionCached(float UNUSED(force[3]),
993                                           struct Object *vertexowner,
994                                           float UNUSED(time))
995 {
996   Object *ob;
997   SoftBody *sb = vertexowner->soft;
998   GHash *hash;
999   GHashIterator *ihash;
1000   float aabbmin[3], aabbmax[3];
1001   int deflected = 0;
1002 #if 0
1003   int a;
1004 #endif
1005
1006   if ((sb == NULL) || (sb->scratch == NULL)) {
1007     return 0;
1008   }
1009   copy_v3_v3(aabbmin, sb->scratch->aabbmin);
1010   copy_v3_v3(aabbmax, sb->scratch->aabbmax);
1011
1012   hash = vertexowner->soft->scratch->colliderhash;
1013   ihash = BLI_ghashIterator_new(hash);
1014   while (!BLI_ghashIterator_done(ihash)) {
1015
1016     ccd_Mesh *ccdm = BLI_ghashIterator_getValue(ihash);
1017     ob = BLI_ghashIterator_getKey(ihash);
1018     {
1019       /* only with deflecting set */
1020       if (ob->pd && ob->pd->deflect) {
1021         if (ccdm) {
1022           if ((aabbmax[0] < ccdm->bbmin[0]) || (aabbmax[1] < ccdm->bbmin[1]) ||
1023               (aabbmax[2] < ccdm->bbmin[2]) || (aabbmin[0] > ccdm->bbmax[0]) ||
1024               (aabbmin[1] > ccdm->bbmax[1]) || (aabbmin[2] > ccdm->bbmax[2])) {
1025             /* boxes don't intersect */
1026             BLI_ghashIterator_step(ihash);
1027             continue;
1028           }
1029
1030           /* so now we have the 2 boxes overlapping */
1031           /* forces actually not used */
1032           deflected = 2;
1033         }
1034         else {
1035           /*aye that should be cached*/
1036           CLOG_ERROR(&LOG, "missing cache error");
1037           BLI_ghashIterator_step(ihash);
1038           continue;
1039         }
1040       } /* if (ob->pd && ob->pd->deflect) */
1041       BLI_ghashIterator_step(ihash);
1042     }
1043   } /* while () */
1044   BLI_ghashIterator_free(ihash);
1045   return deflected;
1046 }
1047 /* --- the aabb section*/
1048
1049 /* +++ the face external section*/
1050 static int sb_detect_face_pointCached(float face_v1[3],
1051                                       float face_v2[3],
1052                                       float face_v3[3],
1053                                       float *damp,
1054                                       float force[3],
1055                                       struct Object *vertexowner,
1056                                       float time)
1057 {
1058   Object *ob;
1059   GHash *hash;
1060   GHashIterator *ihash;
1061   float nv1[3], edge1[3], edge2[3], d_nvect[3], aabbmin[3], aabbmax[3];
1062   float facedist, outerfacethickness, tune = 10.f;
1063   int a, deflected = 0;
1064
1065   aabbmin[0] = min_fff(face_v1[0], face_v2[0], face_v3[0]);
1066   aabbmin[1] = min_fff(face_v1[1], face_v2[1], face_v3[1]);
1067   aabbmin[2] = min_fff(face_v1[2], face_v2[2], face_v3[2]);
1068   aabbmax[0] = max_fff(face_v1[0], face_v2[0], face_v3[0]);
1069   aabbmax[1] = max_fff(face_v1[1], face_v2[1], face_v3[1]);
1070   aabbmax[2] = max_fff(face_v1[2], face_v2[2], face_v3[2]);
1071
1072   /* calculate face normal once again SIGH */
1073   sub_v3_v3v3(edge1, face_v1, face_v2);
1074   sub_v3_v3v3(edge2, face_v3, face_v2);
1075   cross_v3_v3v3(d_nvect, edge2, edge1);
1076   normalize_v3(d_nvect);
1077
1078   hash = vertexowner->soft->scratch->colliderhash;
1079   ihash = BLI_ghashIterator_new(hash);
1080   while (!BLI_ghashIterator_done(ihash)) {
1081
1082     ccd_Mesh *ccdm = BLI_ghashIterator_getValue(ihash);
1083     ob = BLI_ghashIterator_getKey(ihash);
1084     {
1085       /* only with deflecting set */
1086       if (ob->pd && ob->pd->deflect) {
1087         const MVert *mvert = NULL;
1088         const MVert *mprevvert = NULL;
1089         if (ccdm) {
1090           mvert = ccdm->mvert;
1091           a = ccdm->mvert_num;
1092           mprevvert = ccdm->mprevvert;
1093           outerfacethickness = ob->pd->pdef_sboft;
1094           if ((aabbmax[0] < ccdm->bbmin[0]) || (aabbmax[1] < ccdm->bbmin[1]) ||
1095               (aabbmax[2] < ccdm->bbmin[2]) || (aabbmin[0] > ccdm->bbmax[0]) ||
1096               (aabbmin[1] > ccdm->bbmax[1]) || (aabbmin[2] > ccdm->bbmax[2])) {
1097             /* boxes don't intersect */
1098             BLI_ghashIterator_step(ihash);
1099             continue;
1100           }
1101         }
1102         else {
1103           /*aye that should be cached*/
1104           CLOG_ERROR(&LOG, "missing cache error");
1105           BLI_ghashIterator_step(ihash);
1106           continue;
1107         }
1108
1109         /* use mesh*/
1110         if (mvert) {
1111           while (a) {
1112             copy_v3_v3(nv1, mvert[a - 1].co);
1113             if (mprevvert) {
1114               mul_v3_fl(nv1, time);
1115               madd_v3_v3fl(nv1, mprevvert[a - 1].co, 1.0f - time);
1116             }
1117             /* origin to face_v2*/
1118             sub_v3_v3(nv1, face_v2);
1119             facedist = dot_v3v3(nv1, d_nvect);
1120             if (ABS(facedist) < outerfacethickness) {
1121               if (isect_point_tri_prism_v3(nv1, face_v1, face_v2, face_v3)) {
1122                 float df;
1123                 if (facedist > 0) {
1124                   df = (outerfacethickness - facedist) / outerfacethickness;
1125                 }
1126                 else {
1127                   df = (outerfacethickness + facedist) / outerfacethickness;
1128                 }
1129
1130                 *damp = df * tune * ob->pd->pdef_sbdamp;
1131
1132                 df = 0.01f * expf(-100.0f * df);
1133                 madd_v3_v3fl(force, d_nvect, -df);
1134                 deflected = 3;
1135               }
1136             }
1137             a--;
1138           } /* while (a)*/
1139         }   /* if (mvert) */
1140       }     /* if (ob->pd && ob->pd->deflect) */
1141       BLI_ghashIterator_step(ihash);
1142     }
1143   } /* while () */
1144   BLI_ghashIterator_free(ihash);
1145   return deflected;
1146 }
1147
1148 static int sb_detect_face_collisionCached(float face_v1[3],
1149                                           float face_v2[3],
1150                                           float face_v3[3],
1151                                           float *damp,
1152                                           float force[3],
1153                                           struct Object *vertexowner,
1154                                           float time)
1155 {
1156   Object *ob;
1157   GHash *hash;
1158   GHashIterator *ihash;
1159   float nv1[3], nv2[3], nv3[3], edge1[3], edge2[3], d_nvect[3], aabbmin[3], aabbmax[3];
1160   float t, tune = 10.0f;
1161   int a, deflected = 0;
1162
1163   aabbmin[0] = min_fff(face_v1[0], face_v2[0], face_v3[0]);
1164   aabbmin[1] = min_fff(face_v1[1], face_v2[1], face_v3[1]);
1165   aabbmin[2] = min_fff(face_v1[2], face_v2[2], face_v3[2]);
1166   aabbmax[0] = max_fff(face_v1[0], face_v2[0], face_v3[0]);
1167   aabbmax[1] = max_fff(face_v1[1], face_v2[1], face_v3[1]);
1168   aabbmax[2] = max_fff(face_v1[2], face_v2[2], face_v3[2]);
1169
1170   hash = vertexowner->soft->scratch->colliderhash;
1171   ihash = BLI_ghashIterator_new(hash);
1172   while (!BLI_ghashIterator_done(ihash)) {
1173
1174     ccd_Mesh *ccdm = BLI_ghashIterator_getValue(ihash);
1175     ob = BLI_ghashIterator_getKey(ihash);
1176     {
1177       /* only with deflecting set */
1178       if (ob->pd && ob->pd->deflect) {
1179         const MVert *mvert = NULL;
1180         const MVert *mprevvert = NULL;
1181         const MVertTri *vt = NULL;
1182         const ccdf_minmax *mima = NULL;
1183
1184         if (ccdm) {
1185           mvert = ccdm->mvert;
1186           vt = ccdm->tri;
1187           mprevvert = ccdm->mprevvert;
1188           mima = ccdm->mima;
1189           a = ccdm->tri_num;
1190
1191           if ((aabbmax[0] < ccdm->bbmin[0]) || (aabbmax[1] < ccdm->bbmin[1]) ||
1192               (aabbmax[2] < ccdm->bbmin[2]) || (aabbmin[0] > ccdm->bbmax[0]) ||
1193               (aabbmin[1] > ccdm->bbmax[1]) || (aabbmin[2] > ccdm->bbmax[2])) {
1194             /* boxes don't intersect */
1195             BLI_ghashIterator_step(ihash);
1196             continue;
1197           }
1198         }
1199         else {
1200           /*aye that should be cached*/
1201           CLOG_ERROR(&LOG, "missing cache error");
1202           BLI_ghashIterator_step(ihash);
1203           continue;
1204         }
1205
1206         /* use mesh*/
1207         while (a--) {
1208           if ((aabbmax[0] < mima->minx) || (aabbmin[0] > mima->maxx) ||
1209               (aabbmax[1] < mima->miny) || (aabbmin[1] > mima->maxy) ||
1210               (aabbmax[2] < mima->minz) || (aabbmin[2] > mima->maxz)) {
1211             mima++;
1212             vt++;
1213             continue;
1214           }
1215
1216           if (mvert) {
1217
1218             copy_v3_v3(nv1, mvert[vt->tri[0]].co);
1219             copy_v3_v3(nv2, mvert[vt->tri[1]].co);
1220             copy_v3_v3(nv3, mvert[vt->tri[2]].co);
1221
1222             if (mprevvert) {
1223               mul_v3_fl(nv1, time);
1224               madd_v3_v3fl(nv1, mprevvert[vt->tri[0]].co, 1.0f - time);
1225
1226               mul_v3_fl(nv2, time);
1227               madd_v3_v3fl(nv2, mprevvert[vt->tri[1]].co, 1.0f - time);
1228
1229               mul_v3_fl(nv3, time);
1230               madd_v3_v3fl(nv3, mprevvert[vt->tri[2]].co, 1.0f - time);
1231             }
1232           }
1233
1234           /* switch origin to be nv2*/
1235           sub_v3_v3v3(edge1, nv1, nv2);
1236           sub_v3_v3v3(edge2, nv3, nv2);
1237           cross_v3_v3v3(d_nvect, edge2, edge1);
1238           normalize_v3(d_nvect);
1239           if (isect_line_segment_tri_v3(nv1, nv2, face_v1, face_v2, face_v3, &t, NULL) ||
1240               isect_line_segment_tri_v3(nv2, nv3, face_v1, face_v2, face_v3, &t, NULL) ||
1241               isect_line_segment_tri_v3(nv3, nv1, face_v1, face_v2, face_v3, &t, NULL)) {
1242             madd_v3_v3fl(force, d_nvect, -0.5f);
1243             *damp = tune * ob->pd->pdef_sbdamp;
1244             deflected = 2;
1245           }
1246           mima++;
1247           vt++;
1248         } /* while a */
1249       }   /* if (ob->pd && ob->pd->deflect) */
1250       BLI_ghashIterator_step(ihash);
1251     }
1252   } /* while () */
1253   BLI_ghashIterator_free(ihash);
1254   return deflected;
1255 }
1256
1257 static void scan_for_ext_face_forces(Object *ob, float timenow)
1258 {
1259   SoftBody *sb = ob->soft;
1260   BodyFace *bf;
1261   int a;
1262   float damp = 0.0f, choke = 1.0f;
1263   float tune = -10.0f;
1264   float feedback[3];
1265
1266   if (sb && sb->scratch->totface) {
1267
1268     bf = sb->scratch->bodyface;
1269     for (a = 0; a < sb->scratch->totface; a++, bf++) {
1270       bf->ext_force[0] = bf->ext_force[1] = bf->ext_force[2] = 0.0f;
1271       /*+++edges intruding*/
1272       bf->flag &= ~BFF_INTERSECT;
1273       zero_v3(feedback);
1274       if (sb_detect_face_collisionCached(sb->bpoint[bf->v1].pos,
1275                                          sb->bpoint[bf->v2].pos,
1276                                          sb->bpoint[bf->v3].pos,
1277                                          &damp,
1278                                          feedback,
1279                                          ob,
1280                                          timenow)) {
1281         madd_v3_v3fl(sb->bpoint[bf->v1].force, feedback, tune);
1282         madd_v3_v3fl(sb->bpoint[bf->v2].force, feedback, tune);
1283         madd_v3_v3fl(sb->bpoint[bf->v3].force, feedback, tune);
1284         //              madd_v3_v3fl(bf->ext_force, feedback, tune);
1285         bf->flag |= BFF_INTERSECT;
1286         choke = min_ff(max_ff(damp, choke), 1.0f);
1287       }
1288       /*---edges intruding*/
1289
1290       /*+++ close vertices*/
1291       if ((bf->flag & BFF_INTERSECT) == 0) {
1292         bf->flag &= ~BFF_CLOSEVERT;
1293         tune = -1.0f;
1294         zero_v3(feedback);
1295         if (sb_detect_face_pointCached(sb->bpoint[bf->v1].pos,
1296                                        sb->bpoint[bf->v2].pos,
1297                                        sb->bpoint[bf->v3].pos,
1298                                        &damp,
1299                                        feedback,
1300                                        ob,
1301                                        timenow)) {
1302           madd_v3_v3fl(sb->bpoint[bf->v1].force, feedback, tune);
1303           madd_v3_v3fl(sb->bpoint[bf->v2].force, feedback, tune);
1304           madd_v3_v3fl(sb->bpoint[bf->v3].force, feedback, tune);
1305           //                  madd_v3_v3fl(bf->ext_force, feedback, tune);
1306           bf->flag |= BFF_CLOSEVERT;
1307           choke = min_ff(max_ff(damp, choke), 1.0f);
1308         }
1309       }
1310       /*--- close vertices*/
1311     }
1312     bf = sb->scratch->bodyface;
1313     for (a = 0; a < sb->scratch->totface; a++, bf++) {
1314       if ((bf->flag & BFF_INTERSECT) || (bf->flag & BFF_CLOSEVERT)) {
1315         sb->bpoint[bf->v1].choke2 = max_ff(sb->bpoint[bf->v1].choke2, choke);
1316         sb->bpoint[bf->v2].choke2 = max_ff(sb->bpoint[bf->v2].choke2, choke);
1317         sb->bpoint[bf->v3].choke2 = max_ff(sb->bpoint[bf->v3].choke2, choke);
1318       }
1319     }
1320   }
1321 }
1322
1323 /*  --- the face external section*/
1324
1325 /* +++ the spring external section*/
1326
1327 static int sb_detect_edge_collisionCached(float edge_v1[3],
1328                                           float edge_v2[3],
1329                                           float *damp,
1330                                           float force[3],
1331                                           struct Object *vertexowner,
1332                                           float time)
1333 {
1334   Object *ob;
1335   GHash *hash;
1336   GHashIterator *ihash;
1337   float nv1[3], nv2[3], nv3[3], edge1[3], edge2[3], d_nvect[3], aabbmin[3], aabbmax[3];
1338   float t, el;
1339   int a, deflected = 0;
1340
1341   minmax_v3v3_v3(aabbmin, aabbmax, edge_v1);
1342   minmax_v3v3_v3(aabbmin, aabbmax, edge_v2);
1343
1344   el = len_v3v3(edge_v1, edge_v2);
1345
1346   hash = vertexowner->soft->scratch->colliderhash;
1347   ihash = BLI_ghashIterator_new(hash);
1348   while (!BLI_ghashIterator_done(ihash)) {
1349
1350     ccd_Mesh *ccdm = BLI_ghashIterator_getValue(ihash);
1351     ob = BLI_ghashIterator_getKey(ihash);
1352     {
1353       /* only with deflecting set */
1354       if (ob->pd && ob->pd->deflect) {
1355         const MVert *mvert = NULL;
1356         const MVert *mprevvert = NULL;
1357         const MVertTri *vt = NULL;
1358         const ccdf_minmax *mima = NULL;
1359
1360         if (ccdm) {
1361           mvert = ccdm->mvert;
1362           mprevvert = ccdm->mprevvert;
1363           vt = ccdm->tri;
1364           mima = ccdm->mima;
1365           a = ccdm->tri_num;
1366
1367           if ((aabbmax[0] < ccdm->bbmin[0]) || (aabbmax[1] < ccdm->bbmin[1]) ||
1368               (aabbmax[2] < ccdm->bbmin[2]) || (aabbmin[0] > ccdm->bbmax[0]) ||
1369               (aabbmin[1] > ccdm->bbmax[1]) || (aabbmin[2] > ccdm->bbmax[2])) {
1370             /* boxes don't intersect */
1371             BLI_ghashIterator_step(ihash);
1372             continue;
1373           }
1374         }
1375         else {
1376           /*aye that should be cached*/
1377           CLOG_ERROR(&LOG, "missing cache error");
1378           BLI_ghashIterator_step(ihash);
1379           continue;
1380         }
1381
1382         /* use mesh*/
1383         while (a--) {
1384           if ((aabbmax[0] < mima->minx) || (aabbmin[0] > mima->maxx) ||
1385               (aabbmax[1] < mima->miny) || (aabbmin[1] > mima->maxy) ||
1386               (aabbmax[2] < mima->minz) || (aabbmin[2] > mima->maxz)) {
1387             mima++;
1388             vt++;
1389             continue;
1390           }
1391
1392           if (mvert) {
1393
1394             copy_v3_v3(nv1, mvert[vt->tri[0]].co);
1395             copy_v3_v3(nv2, mvert[vt->tri[1]].co);
1396             copy_v3_v3(nv3, mvert[vt->tri[2]].co);
1397
1398             if (mprevvert) {
1399               mul_v3_fl(nv1, time);
1400               madd_v3_v3fl(nv1, mprevvert[vt->tri[0]].co, 1.0f - time);
1401
1402               mul_v3_fl(nv2, time);
1403               madd_v3_v3fl(nv2, mprevvert[vt->tri[1]].co, 1.0f - time);
1404
1405               mul_v3_fl(nv3, time);
1406               madd_v3_v3fl(nv3, mprevvert[vt->tri[2]].co, 1.0f - time);
1407             }
1408           }
1409
1410           /* switch origin to be nv2*/
1411           sub_v3_v3v3(edge1, nv1, nv2);
1412           sub_v3_v3v3(edge2, nv3, nv2);
1413
1414           cross_v3_v3v3(d_nvect, edge2, edge1);
1415           normalize_v3(d_nvect);
1416           if (isect_line_segment_tri_v3(edge_v1, edge_v2, nv1, nv2, nv3, &t, NULL)) {
1417             float v1[3], v2[3];
1418             float intrusiondepth, i1, i2;
1419             sub_v3_v3v3(v1, edge_v1, nv2);
1420             sub_v3_v3v3(v2, edge_v2, nv2);
1421             i1 = dot_v3v3(v1, d_nvect);
1422             i2 = dot_v3v3(v2, d_nvect);
1423             intrusiondepth = -min_ff(i1, i2) / el;
1424             madd_v3_v3fl(force, d_nvect, intrusiondepth);
1425             *damp = ob->pd->pdef_sbdamp;
1426             deflected = 2;
1427           }
1428
1429           mima++;
1430           vt++;
1431         } /* while a */
1432       }   /* if (ob->pd && ob->pd->deflect) */
1433       BLI_ghashIterator_step(ihash);
1434     }
1435   } /* while () */
1436   BLI_ghashIterator_free(ihash);
1437   return deflected;
1438 }
1439
1440 static void _scan_for_ext_spring_forces(
1441     Scene *scene, Object *ob, float timenow, int ifirst, int ilast, struct ListBase *effectors)
1442 {
1443   SoftBody *sb = ob->soft;
1444   int a;
1445   float damp;
1446   float feedback[3];
1447
1448   if (sb && sb->totspring) {
1449     for (a = ifirst; a < ilast; a++) {
1450       BodySpring *bs = &sb->bspring[a];
1451       bs->ext_force[0] = bs->ext_force[1] = bs->ext_force[2] = 0.0f;
1452       feedback[0] = feedback[1] = feedback[2] = 0.0f;
1453       bs->flag &= ~BSF_INTERSECT;
1454
1455       if (bs->springtype == SB_EDGE) {
1456         /* +++ springs colliding */
1457         if (ob->softflag & OB_SB_EDGECOLL) {
1458           if (sb_detect_edge_collisionCached(
1459                   sb->bpoint[bs->v1].pos, sb->bpoint[bs->v2].pos, &damp, feedback, ob, timenow)) {
1460             add_v3_v3(bs->ext_force, feedback);
1461             bs->flag |= BSF_INTERSECT;
1462             //bs->cf=damp;
1463             bs->cf = sb->choke * 0.01f;
1464           }
1465         }
1466         /* ---- springs colliding */
1467
1468         /* +++ springs seeing wind ... n stuff depending on their orientation*/
1469         /* note we don't use sb->mediafrict but use sb->aeroedge for magnitude of effect*/
1470         if (sb->aeroedge) {
1471           float vel[3], sp[3], pr[3], force[3];
1472           float f, windfactor = 0.25f;
1473           /*see if we have wind*/
1474           if (effectors) {
1475             EffectedPoint epoint;
1476             float speed[3] = {0.0f, 0.0f, 0.0f};
1477             float pos[3];
1478             mid_v3_v3v3(pos, sb->bpoint[bs->v1].pos, sb->bpoint[bs->v2].pos);
1479             mid_v3_v3v3(vel, sb->bpoint[bs->v1].vec, sb->bpoint[bs->v2].vec);
1480             pd_point_from_soft(scene, pos, vel, -1, &epoint);
1481             BKE_effectors_apply(effectors, NULL, sb->effector_weights, &epoint, force, speed);
1482
1483             mul_v3_fl(speed, windfactor);
1484             add_v3_v3(vel, speed);
1485           }
1486           /* media in rest */
1487           else {
1488             add_v3_v3v3(vel, sb->bpoint[bs->v1].vec, sb->bpoint[bs->v2].vec);
1489           }
1490           f = normalize_v3(vel);
1491           f = -0.0001f * f * f * sb->aeroedge;
1492           /* (todo) add a nice angle dependent function done for now BUT */
1493           /* still there could be some nice drag/lift function, but who needs it */
1494
1495           sub_v3_v3v3(sp, sb->bpoint[bs->v1].pos, sb->bpoint[bs->v2].pos);
1496           project_v3_v3v3(pr, vel, sp);
1497           sub_v3_v3(vel, pr);
1498           normalize_v3(vel);
1499           if (ob->softflag & OB_SB_AERO_ANGLE) {
1500             normalize_v3(sp);
1501             madd_v3_v3fl(bs->ext_force, vel, f * (1.0f - fabsf(dot_v3v3(vel, sp))));
1502           }
1503           else {
1504             madd_v3_v3fl(bs->ext_force, vel, f);  // to keep compatible with 2.45 release files
1505           }
1506         }
1507         /* --- springs seeing wind */
1508       }
1509     }
1510   }
1511 }
1512
1513 static void *exec_scan_for_ext_spring_forces(void *data)
1514 {
1515   SB_thread_context *pctx = (SB_thread_context *)data;
1516   _scan_for_ext_spring_forces(
1517       pctx->scene, pctx->ob, pctx->timenow, pctx->ifirst, pctx->ilast, pctx->effectors);
1518   return NULL;
1519 }
1520
1521 static void sb_sfesf_threads_run(struct Depsgraph *depsgraph,
1522                                  Scene *scene,
1523                                  struct Object *ob,
1524                                  float timenow,
1525                                  int totsprings,
1526                                  int *UNUSED(ptr_to_break_func(void)))
1527 {
1528   ListBase threads;
1529   SB_thread_context *sb_threads;
1530   int i, totthread, left, dec;
1531   int lowsprings =
1532       100; /* wild guess .. may increase with better thread management 'above' or even be UI option sb->spawn_cf_threads_nopts */
1533
1534   ListBase *effectors = BKE_effectors_create(depsgraph, ob, NULL, ob->soft->effector_weights);
1535
1536   /* figure the number of threads while preventing pretty pointless threading overhead */
1537   totthread = BKE_scene_num_threads(scene);
1538   /* what if we got zillions of CPUs running but less to spread*/
1539   while ((totsprings / totthread < lowsprings) && (totthread > 1)) {
1540     totthread--;
1541   }
1542
1543   sb_threads = MEM_callocN(sizeof(SB_thread_context) * totthread, "SBSpringsThread");
1544   memset(sb_threads, 0, sizeof(SB_thread_context) * totthread);
1545   left = totsprings;
1546   dec = totsprings / totthread + 1;
1547   for (i = 0; i < totthread; i++) {
1548     sb_threads[i].scene = scene;
1549     sb_threads[i].ob = ob;
1550     sb_threads[i].forcetime = 0.0;  // not used here
1551     sb_threads[i].timenow = timenow;
1552     sb_threads[i].ilast = left;
1553     left = left - dec;
1554     if (left > 0) {
1555       sb_threads[i].ifirst = left;
1556     }
1557     else {
1558       sb_threads[i].ifirst = 0;
1559     }
1560     sb_threads[i].effectors = effectors;
1561     sb_threads[i].do_deflector = false;  // not used here
1562     sb_threads[i].fieldfactor = 0.0f;    // not used here
1563     sb_threads[i].windfactor = 0.0f;     // not used here
1564     sb_threads[i].nr = i;
1565     sb_threads[i].tot = totthread;
1566   }
1567   if (totthread > 1) {
1568     BLI_threadpool_init(&threads, exec_scan_for_ext_spring_forces, totthread);
1569
1570     for (i = 0; i < totthread; i++) {
1571       BLI_threadpool_insert(&threads, &sb_threads[i]);
1572     }
1573
1574     BLI_threadpool_end(&threads);
1575   }
1576   else {
1577     exec_scan_for_ext_spring_forces(&sb_threads[0]);
1578   }
1579   /* clean up */
1580   MEM_freeN(sb_threads);
1581
1582   BKE_effectors_free(effectors);
1583 }
1584
1585 /* --- the spring external section*/
1586
1587 static int choose_winner(
1588     float *w, float *pos, float *a, float *b, float *c, float *ca, float *cb, float *cc)
1589 {
1590   float mindist, cp;
1591   int winner = 1;
1592   mindist = fabsf(dot_v3v3(pos, a));
1593
1594   cp = fabsf(dot_v3v3(pos, b));
1595   if (mindist < cp) {
1596     mindist = cp;
1597     winner = 2;
1598   }
1599
1600   cp = fabsf(dot_v3v3(pos, c));
1601   if (mindist < cp) {
1602     mindist = cp;
1603     winner = 3;
1604   }
1605   switch (winner) {
1606     case 1:
1607       copy_v3_v3(w, ca);
1608       break;
1609     case 2:
1610       copy_v3_v3(w, cb);
1611       break;
1612     case 3:
1613       copy_v3_v3(w, cc);
1614   }
1615   return (winner);
1616 }
1617
1618 static int sb_detect_vertex_collisionCached(float opco[3],
1619                                             float facenormal[3],
1620                                             float *damp,
1621                                             float force[3],
1622                                             struct Object *vertexowner,
1623                                             float time,
1624                                             float vel[3],
1625                                             float *intrusion)
1626 {
1627   Object *ob = NULL;
1628   GHash *hash;
1629   GHashIterator *ihash;
1630   float nv1[3], nv2[3], nv3[3], edge1[3], edge2[3], d_nvect[3], dv1[3], ve[3],
1631       avel[3] = {0.0, 0.0, 0.0}, vv1[3], vv2[3], vv3[3], coledge[3] = {0.0f, 0.0f, 0.0f},
1632       mindistedge = 1000.0f, outerforceaccu[3], innerforceaccu[3], facedist,
1633       /* n_mag, */ /* UNUSED */ force_mag_norm, minx, miny, minz, maxx, maxy, maxz,
1634       innerfacethickness = -0.5f, outerfacethickness = 0.2f, ee = 5.0f, ff = 0.1f, fa = 1;
1635   int a, deflected = 0, cavel = 0, ci = 0;
1636   /* init */
1637   *intrusion = 0.0f;
1638   hash = vertexowner->soft->scratch->colliderhash;
1639   ihash = BLI_ghashIterator_new(hash);
1640   outerforceaccu[0] = outerforceaccu[1] = outerforceaccu[2] = 0.0f;
1641   innerforceaccu[0] = innerforceaccu[1] = innerforceaccu[2] = 0.0f;
1642   /* go */
1643   while (!BLI_ghashIterator_done(ihash)) {
1644
1645     ccd_Mesh *ccdm = BLI_ghashIterator_getValue(ihash);
1646     ob = BLI_ghashIterator_getKey(ihash);
1647     {
1648       /* only with deflecting set */
1649       if (ob->pd && ob->pd->deflect) {
1650         const MVert *mvert = NULL;
1651         const MVert *mprevvert = NULL;
1652         const MVertTri *vt = NULL;
1653         const ccdf_minmax *mima = NULL;
1654
1655         if (ccdm) {
1656           mvert = ccdm->mvert;
1657           mprevvert = ccdm->mprevvert;
1658           vt = ccdm->tri;
1659           mima = ccdm->mima;
1660           a = ccdm->tri_num;
1661
1662           minx = ccdm->bbmin[0];
1663           miny = ccdm->bbmin[1];
1664           minz = ccdm->bbmin[2];
1665
1666           maxx = ccdm->bbmax[0];
1667           maxy = ccdm->bbmax[1];
1668           maxz = ccdm->bbmax[2];
1669
1670           if ((opco[0] < minx) || (opco[1] < miny) || (opco[2] < minz) || (opco[0] > maxx) ||
1671               (opco[1] > maxy) || (opco[2] > maxz)) {
1672             /* outside the padded boundbox --> collision object is too far away */
1673             BLI_ghashIterator_step(ihash);
1674             continue;
1675           }
1676         }
1677         else {
1678           /*aye that should be cached*/
1679           CLOG_ERROR(&LOG, "missing cache error");
1680           BLI_ghashIterator_step(ihash);
1681           continue;
1682         }
1683
1684         /* do object level stuff */
1685         /* need to have user control for that since it depends on model scale */
1686         innerfacethickness = -ob->pd->pdef_sbift;
1687         outerfacethickness = ob->pd->pdef_sboft;
1688         fa = (ff * outerfacethickness - outerfacethickness);
1689         fa *= fa;
1690         fa = 1.0f / fa;
1691         avel[0] = avel[1] = avel[2] = 0.0f;
1692         /* use mesh*/
1693         while (a--) {
1694           if ((opco[0] < mima->minx) || (opco[0] > mima->maxx) || (opco[1] < mima->miny) ||
1695               (opco[1] > mima->maxy) || (opco[2] < mima->minz) || (opco[2] > mima->maxz)) {
1696             mima++;
1697             vt++;
1698             continue;
1699           }
1700
1701           if (mvert) {
1702
1703             copy_v3_v3(nv1, mvert[vt->tri[0]].co);
1704             copy_v3_v3(nv2, mvert[vt->tri[1]].co);
1705             copy_v3_v3(nv3, mvert[vt->tri[2]].co);
1706
1707             if (mprevvert) {
1708               /* grab the average speed of the collider vertices
1709                * before we spoil nvX
1710                * humm could be done once a SB steps but then we' need to store that too
1711                * since the AABB reduced propabitlty to get here drasticallly
1712                * it might be a nice tradeof CPU <--> memory
1713                */
1714               sub_v3_v3v3(vv1, nv1, mprevvert[vt->tri[0]].co);
1715               sub_v3_v3v3(vv2, nv2, mprevvert[vt->tri[1]].co);
1716               sub_v3_v3v3(vv3, nv3, mprevvert[vt->tri[2]].co);
1717
1718               mul_v3_fl(nv1, time);
1719               madd_v3_v3fl(nv1, mprevvert[vt->tri[0]].co, 1.0f - time);
1720
1721               mul_v3_fl(nv2, time);
1722               madd_v3_v3fl(nv2, mprevvert[vt->tri[1]].co, 1.0f - time);
1723
1724               mul_v3_fl(nv3, time);
1725               madd_v3_v3fl(nv3, mprevvert[vt->tri[2]].co, 1.0f - time);
1726             }
1727           }
1728
1729           /* switch origin to be nv2*/
1730           sub_v3_v3v3(edge1, nv1, nv2);
1731           sub_v3_v3v3(edge2, nv3, nv2);
1732           sub_v3_v3v3(
1733               dv1, opco, nv2); /* abuse dv1 to have vertex in question at *origin* of triangle */
1734
1735           cross_v3_v3v3(d_nvect, edge2, edge1);
1736           /* n_mag = */ /* UNUSED */ normalize_v3(d_nvect);
1737           facedist = dot_v3v3(dv1, d_nvect);
1738           // so rules are
1739           //
1740
1741           if ((facedist > innerfacethickness) && (facedist < outerfacethickness)) {
1742             if (isect_point_tri_prism_v3(opco, nv1, nv2, nv3)) {
1743               force_mag_norm = (float)exp(-ee * facedist);
1744               if (facedist > outerfacethickness * ff) {
1745                 force_mag_norm = (float)force_mag_norm * fa * (facedist - outerfacethickness) *
1746                                  (facedist - outerfacethickness);
1747               }
1748               *damp = ob->pd->pdef_sbdamp;
1749               if (facedist > 0.0f) {
1750                 *damp *= (1.0f - facedist / outerfacethickness);
1751                 madd_v3_v3fl(outerforceaccu, d_nvect, force_mag_norm);
1752                 deflected = 3;
1753               }
1754               else {
1755                 madd_v3_v3fl(innerforceaccu, d_nvect, force_mag_norm);
1756                 if (deflected < 2) {
1757                   deflected = 2;
1758                 }
1759               }
1760               if ((mprevvert) && (*damp > 0.0f)) {
1761                 choose_winner(ve, opco, nv1, nv2, nv3, vv1, vv2, vv3);
1762                 add_v3_v3(avel, ve);
1763                 cavel++;
1764               }
1765               *intrusion += facedist;
1766               ci++;
1767             }
1768           }
1769
1770           mima++;
1771           vt++;
1772         } /* while a */
1773       }   /* if (ob->pd && ob->pd->deflect) */
1774       BLI_ghashIterator_step(ihash);
1775     }
1776   } /* while () */
1777
1778   if (deflected == 1) {  // no face but 'outer' edge cylinder sees vert
1779     force_mag_norm = (float)exp(-ee * mindistedge);
1780     if (mindistedge > outerfacethickness * ff) {
1781       force_mag_norm = (float)force_mag_norm * fa * (mindistedge - outerfacethickness) *
1782                        (mindistedge - outerfacethickness);
1783     }
1784     madd_v3_v3fl(force, coledge, force_mag_norm);
1785     *damp = ob->pd->pdef_sbdamp;
1786     if (mindistedge > 0.0f) {
1787       *damp *= (1.0f - mindistedge / outerfacethickness);
1788     }
1789   }
1790   if (deflected == 2) {  //  face inner detected
1791     add_v3_v3(force, innerforceaccu);
1792   }
1793   if (deflected == 3) {  //  face outer detected
1794     add_v3_v3(force, outerforceaccu);
1795   }
1796
1797   BLI_ghashIterator_free(ihash);
1798   if (cavel) {
1799     mul_v3_fl(avel, 1.0f / (float)cavel);
1800   }
1801   copy_v3_v3(vel, avel);
1802   if (ci) {
1803     *intrusion /= ci;
1804   }
1805   if (deflected) {
1806     normalize_v3_v3(facenormal, force);
1807   }
1808   return deflected;
1809 }
1810
1811 /* sandbox to plug in various deflection algos */
1812 static int sb_deflect_face(Object *ob,
1813                            float *actpos,
1814                            float *facenormal,
1815                            float *force,
1816                            float *cf,
1817                            float time,
1818                            float *vel,
1819                            float *intrusion)
1820 {
1821   float s_actpos[3];
1822   int deflected;
1823   copy_v3_v3(s_actpos, actpos);
1824   deflected = sb_detect_vertex_collisionCached(
1825       s_actpos, facenormal, cf, force, ob, time, vel, intrusion);
1826   //deflected= sb_detect_vertex_collisionCachedEx(s_actpos, facenormal, cf, force, ob, time, vel, intrusion);
1827   return (deflected);
1828 }
1829
1830 /* hiding this for now .. but the jacobian may pop up on other tasks .. so i'd like to keep it */
1831 #if 0
1832 static void dfdx_spring(int ia, int ic, int op, float dir[3], float L, float len, float factor)
1833 {
1834   float m, delta_ij;
1835   int i, j;
1836   if (L < len) {
1837     for (i = 0; i < 3; i++) {
1838       for (j = 0; j < 3; j++) {
1839         delta_ij = (i == j ? (1.0f) : (0.0f));
1840         m = factor * (dir[i] * dir[j] + (1 - L / len) * (delta_ij - dir[i] * dir[j]));
1841         EIG_linear_solver_matrix_add(ia + i, op + ic + j, m);
1842       }
1843     }
1844   }
1845   else {
1846     for (i = 0; i < 3; i++) {
1847       for (j = 0; j < 3; j++) {
1848         m = factor * dir[i] * dir[j];
1849         EIG_linear_solver_matrix_add(ia + i, op + ic + j, m);
1850       }
1851     }
1852   }
1853 }
1854
1855 static void dfdx_goal(int ia, int ic, int op, float factor)
1856 {
1857   int i;
1858   for (i = 0; i < 3; i++)
1859     EIG_linear_solver_matrix_add(ia + i, op + ic + i, factor);
1860 }
1861
1862 static void dfdv_goal(int ia, int ic, float factor)
1863 {
1864   int i;
1865   for (i = 0; i < 3; i++)
1866     EIG_linear_solver_matrix_add(ia + i, ic + i, factor);
1867 }
1868 #endif /* if 0 */
1869
1870 static void sb_spring_force(
1871     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     CLOG_WARN(&LOG, "bodypoint <bpi> is not attached to spring  <*bs>");
1901     return;
1902   }
1903
1904   /* do bp1 <--> bp2 elastic */
1905   sub_v3_v3v3(dir, bp1->pos, bp2->pos);
1906   distance = normalize_v3(dir);
1907   if (bs->len < distance) {
1908     iks = 1.0f / (1.0f - sb->inspring) - 1.0f; /* inner spring constants function */
1909   }
1910   else {
1911     iks = 1.0f / (1.0f - sb->inpush) - 1.0f; /* inner spring constants function */
1912   }
1913
1914   if (bs->len > 0.0f) { /* check for degenerated springs */
1915     forcefactor = iks / bs->len;
1916   }
1917   else {
1918     forcefactor = iks;
1919   }
1920   kw = (bp1->springweight + bp2->springweight) / 2.0f;
1921   kw = kw * kw;
1922   kw = kw * kw;
1923   switch (bs->springtype) {
1924     case SB_EDGE:
1925     case SB_HANDLE:
1926       forcefactor *= kw;
1927       break;
1928     case SB_BEND:
1929       forcefactor *= sb->secondspring * kw;
1930       break;
1931     case SB_STIFFQUAD:
1932       forcefactor *= sb->shearstiff * sb->shearstiff * kw;
1933       break;
1934     default:
1935       break;
1936   }
1937
1938   madd_v3_v3fl(bp1->force, dir, (bs->len - distance) * forcefactor);
1939
1940   /* do bp1 <--> bp2 viscous */
1941   sub_v3_v3v3(dvel, bp1->vec, bp2->vec);
1942   kd = sb->infrict * sb_fric_force_scale(ob);
1943   absvel = normalize_v3(dvel);
1944   projvel = dot_v3v3(dir, dvel);
1945   kd *= absvel * projvel;
1946   madd_v3_v3fl(bp1->force, dir, -kd);
1947 }
1948
1949 /* since this is definitely the most CPU consuming task here .. try to spread it */
1950 /* core function _softbody_calc_forces_slice_in_a_thread */
1951 /* result is int to be able to flag user break */
1952 static int _softbody_calc_forces_slice_in_a_thread(Scene *scene,
1953                                                    Object *ob,
1954                                                    float forcetime,
1955                                                    float timenow,
1956                                                    int ifirst,
1957                                                    int ilast,
1958                                                    int *UNUSED(ptr_to_break_func(void)),
1959                                                    ListBase *effectors,
1960                                                    int do_deflector,
1961                                                    float fieldfactor,
1962                                                    float windfactor)
1963 {
1964   float iks;
1965   int bb, do_selfcollision, do_springcollision, do_aero;
1966   int number_of_points_here = ilast - ifirst;
1967   SoftBody *sb = ob->soft; /* is supposed to be there */
1968   BodyPoint *bp;
1969
1970   /* initialize */
1971   if (sb) {
1972     /* check conditions for various options */
1973     /* +++ could be done on object level to squeeze out the last bits of it */
1974     do_selfcollision = ((ob->softflag & OB_SB_EDGES) && (sb->bspring) &&
1975                         (ob->softflag & OB_SB_SELF));
1976     do_springcollision = do_deflector && (ob->softflag & OB_SB_EDGES) &&
1977                          (ob->softflag & OB_SB_EDGECOLL);
1978     do_aero = ((sb->aeroedge) && (ob->softflag & OB_SB_EDGES));
1979     /* --- could be done on object level to squeeze out the last bits of it */
1980   }
1981   else {
1982     CLOG_ERROR(&LOG, "expected a SB here");
1983     return (999);
1984   }
1985
1986   /* debugerin */
1987   if (sb->totpoint < ifirst) {
1988     printf("Aye 998");
1989     return (998);
1990   }
1991   /* debugerin */
1992
1993   bp = &sb->bpoint[ifirst];
1994   for (bb = number_of_points_here; bb > 0; bb--, bp++) {
1995     /* clear forces  accumulator */
1996     bp->force[0] = bp->force[1] = bp->force[2] = 0.0;
1997     /* naive ball self collision */
1998     /* needs to be done if goal snaps or not */
1999     if (do_selfcollision) {
2000       int attached;
2001       BodyPoint *obp;
2002       BodySpring *bs;
2003       int c, b;
2004       float velcenter[3], dvel[3], def[3];
2005       float distance;
2006       float compare;
2007       float bstune = sb->ballstiff;
2008
2009       /* running in a slice we must not assume anything done with obp  neither alter the data of obp */
2010       for (c = sb->totpoint, obp = sb->bpoint; c > 0; c--, obp++) {
2011         compare = (obp->colball + bp->colball);
2012         sub_v3_v3v3(def, bp->pos, obp->pos);
2013         /* rather check the AABBoxes before ever calculating the real distance */
2014         /* mathematically it is completely nuts, but performance is pretty much (3) times faster */
2015         if ((ABS(def[0]) > compare) || (ABS(def[1]) > compare) || (ABS(def[2]) > compare)) {
2016           continue;
2017         }
2018         distance = normalize_v3(def);
2019         if (distance < compare) {
2020           /* exclude body points attached with a spring */
2021           attached = 0;
2022           for (b = obp->nofsprings; b > 0; b--) {
2023             bs = sb->bspring + obp->springs[b - 1];
2024             if ((ilast - bb == bs->v2) || (ilast - bb == bs->v1)) {
2025               attached = 1;
2026               continue;
2027             }
2028           }
2029           if (!attached) {
2030             float f = bstune / (distance) + bstune / (compare * compare) * distance -
2031                       2.0f * bstune / compare;
2032
2033             mid_v3_v3v3(velcenter, bp->vec, obp->vec);
2034             sub_v3_v3v3(dvel, velcenter, bp->vec);
2035             mul_v3_fl(dvel, _final_mass(ob, bp));
2036
2037             madd_v3_v3fl(bp->force, def, f * (1.0f - sb->balldamp));
2038             madd_v3_v3fl(bp->force, dvel, sb->balldamp);
2039           }
2040         }
2041       }
2042     }
2043     /* naive ball self collision done */
2044
2045     if (_final_goal(ob, bp) < SOFTGOALSNAP) { /* omit this bp when it snaps */
2046       float auxvect[3];
2047       float velgoal[3];
2048
2049       /* do goal stuff */
2050       if (ob->softflag & OB_SB_GOAL) {
2051         /* true elastic goal */
2052         float ks, kd;
2053         sub_v3_v3v3(auxvect, bp->pos, bp->origT);
2054         ks = 1.0f / (1.0f - _final_goal(ob, bp) * sb->goalspring) - 1.0f;
2055         bp->force[0] += -ks * (auxvect[0]);
2056         bp->force[1] += -ks * (auxvect[1]);
2057         bp->force[2] += -ks * (auxvect[2]);
2058
2059         /* calculate damping forces generated by goals*/
2060         sub_v3_v3v3(velgoal, bp->origS, bp->origE);
2061         kd = sb->goalfrict * sb_fric_force_scale(ob);
2062         add_v3_v3v3(auxvect, velgoal, bp->vec);
2063
2064         if (forcetime >
2065             0.0f) { /* make sure friction does not become rocket motor on time reversal */
2066           bp->force[0] -= kd * (auxvect[0]);
2067           bp->force[1] -= kd * (auxvect[1]);
2068           bp->force[2] -= kd * (auxvect[2]);
2069         }
2070         else {
2071           bp->force[0] -= kd * (velgoal[0] - bp->vec[0]);
2072           bp->force[1] -= kd * (velgoal[1] - bp->vec[1]);
2073           bp->force[2] -= kd * (velgoal[2] - bp->vec[2]);
2074         }
2075       }
2076       /* done goal stuff */
2077
2078       /* gravitation */
2079       if (scene->physics_settings.flag & PHYS_GLOBAL_GRAVITY) {
2080         float gravity[3];
2081         copy_v3_v3(gravity, scene->physics_settings.gravity);
2082         mul_v3_fl(gravity,
2083                   sb_grav_force_scale(ob) * _final_mass(ob, bp) *
2084                       sb->effector_weights->global_gravity); /* individual mass of node here */
2085         add_v3_v3(bp->force, gravity);
2086       }
2087
2088       /* particle field & vortex */
2089       if (effectors) {
2090         EffectedPoint epoint;
2091         float kd;
2092         float force[3] = {0.0f, 0.0f, 0.0f};
2093         float speed[3] = {0.0f, 0.0f, 0.0f};
2094         float eval_sb_fric_force_scale = sb_fric_force_scale(
2095             ob); /* just for calling function once */
2096         pd_point_from_soft(scene, bp->pos, bp->vec, sb->bpoint - bp, &epoint);
2097         BKE_effectors_apply(effectors, NULL, sb->effector_weights, &epoint, force, speed);
2098
2099         /* apply forcefield*/
2100         mul_v3_fl(force, fieldfactor * eval_sb_fric_force_scale);
2101         add_v3_v3(bp->force, force);
2102
2103         /* BP friction in moving media */
2104         kd = sb->mediafrict * eval_sb_fric_force_scale;
2105         bp->force[0] -= kd * (bp->vec[0] + windfactor * speed[0] / eval_sb_fric_force_scale);
2106         bp->force[1] -= kd * (bp->vec[1] + windfactor * speed[1] / eval_sb_fric_force_scale);
2107         bp->force[2] -= kd * (bp->vec[2] + windfactor * speed[2] / eval_sb_fric_force_scale);
2108         /* now we'll have nice centrifugal effect for vortex */
2109       }
2110       else {
2111         /* BP friction in media (not) moving*/
2112         float kd = sb->mediafrict * sb_fric_force_scale(ob);
2113         /* assume it to be proportional to actual velocity */
2114         bp->force[0] -= bp->vec[0] * kd;
2115         bp->force[1] -= bp->vec[1] * kd;
2116         bp->force[2] -= bp->vec[2] * kd;
2117         /* friction in media done */
2118       }
2119       /* +++cached collision targets */
2120       bp->choke = 0.0f;
2121       bp->choke2 = 0.0f;
2122       bp->loc_flag &= ~SBF_DOFUZZY;
2123       if (do_deflector && !(bp->loc_flag & SBF_OUTOFCOLLISION)) {
2124         float cfforce[3], defforce[3] = {0.0f, 0.0f, 0.0f}, vel[3] = {0.0f, 0.0f, 0.0f},
2125                           facenormal[3], cf = 1.0f, intrusion;
2126         float kd = 1.0f;
2127
2128         if (sb_deflect_face(ob, bp->pos, facenormal, defforce, &cf, timenow, vel, &intrusion)) {
2129           if (intrusion < 0.0f) {
2130             sb->scratch->flag |= SBF_DOFUZZY;
2131             bp->loc_flag |= SBF_DOFUZZY;
2132             bp->choke = sb->choke * 0.01f;
2133           }
2134
2135           sub_v3_v3v3(cfforce, bp->vec, vel);
2136           madd_v3_v3fl(bp->force, cfforce, -cf * 50.0f);
2137
2138           madd_v3_v3fl(bp->force, defforce, kd);
2139         }
2140       }
2141       /* ---cached collision targets */
2142
2143       /* +++springs */
2144       iks = 1.0f / (1.0f - sb->inspring) - 1.0f; /* inner spring constants function */
2145       if (ob->softflag & OB_SB_EDGES) {
2146         if (sb->bspring) { /* spring list exists at all ? */
2147           int b;
2148           BodySpring *bs;
2149           for (b = bp->nofsprings; b > 0; b--) {
2150             bs = sb->bspring + bp->springs[b - 1];
2151             if (do_springcollision || do_aero) {
2152               add_v3_v3(bp->force, bs->ext_force);
2153               if (bs->flag & BSF_INTERSECT) {
2154                 bp->choke = bs->cf;
2155               }
2156             }
2157             // sb_spring_force(Object *ob, int bpi, BodySpring *bs, float iks, float forcetime)
2158             sb_spring_force(ob, ilast - bb, bs, iks, forcetime);
2159           } /* loop springs */
2160         }   /* existing spring list */
2161       }     /*any edges*/
2162       /* ---springs */
2163     }       /*omit on snap */
2164   }         /*loop all bp's*/
2165   return 0; /*done fine*/
2166 }
2167
2168 static void *exec_softbody_calc_forces(void *data)
2169 {
2170   SB_thread_context *pctx = (SB_thread_context *)data;
2171   _softbody_calc_forces_slice_in_a_thread(pctx->scene,
2172                                           pctx->ob,
2173                                           pctx->forcetime,
2174                                           pctx->timenow,
2175                                           pctx->ifirst,
2176                                           pctx->ilast,
2177                                           NULL,
2178                                           pctx->effectors,
2179                                           pctx->do_deflector,
2180                                           pctx->fieldfactor,
2181                                           pctx->windfactor);
2182   return NULL;
2183 }
2184
2185 static void sb_cf_threads_run(Scene *scene,
2186                               Object *ob,
2187                               float forcetime,
2188                               float timenow,
2189                               int totpoint,
2190                               int *UNUSED(ptr_to_break_func(void)),
2191                               struct ListBase *effectors,
2192                               int do_deflector,
2193                               float fieldfactor,
2194                               float windfactor)
2195 {
2196   ListBase threads;
2197   SB_thread_context *sb_threads;
2198   int i, totthread, left, dec;
2199   int lowpoints =
2200       100; /* wild guess .. may increase with better thread management 'above' or even be UI option sb->spawn_cf_threads_nopts */
2201
2202   /* figure the number of threads while preventing pretty pointless threading overhead */
2203   totthread = BKE_scene_num_threads(scene);
2204   /* what if we got zillions of CPUs running but less to spread*/
2205   while ((totpoint / totthread < lowpoints) && (totthread > 1)) {
2206     totthread--;
2207   }
2208
2209   /* printf("sb_cf_threads_run spawning %d threads\n", totthread); */
2210
2211   sb_threads = MEM_callocN(sizeof(SB_thread_context) * totthread, "SBThread");
2212   memset(sb_threads, 0, sizeof(SB_thread_context) * totthread);
2213   left = totpoint;
2214   dec = totpoint / totthread + 1;
2215   for (i = 0; i < totthread; i++) {
2216     sb_threads[i].scene = scene;
2217     sb_threads[i].ob = ob;
2218     sb_threads[i].forcetime = forcetime;
2219     sb_threads[i].timenow = timenow;
2220     sb_threads[i].ilast = left;
2221     left = left - dec;
2222     if (left > 0) {
2223       sb_threads[i].ifirst = left;
2224     }
2225     else {
2226       sb_threads[i].ifirst = 0;
2227     }
2228     sb_threads[i].effectors = effectors;
2229     sb_threads[i].do_deflector = do_deflector;
2230     sb_threads[i].fieldfactor = fieldfactor;
2231     sb_threads[i].windfactor = windfactor;
2232     sb_threads[i].nr = i;
2233     sb_threads[i].tot = totthread;
2234   }
2235
2236   if (totthread > 1) {
2237     BLI_threadpool_init(&threads, exec_softbody_calc_forces, totthread);
2238
2239     for (i = 0; i < totthread; i++) {
2240       BLI_threadpool_insert(&threads, &sb_threads[i]);
2241     }
2242
2243     BLI_threadpool_end(&threads);
2244   }
2245   else {
2246     exec_softbody_calc_forces(&sb_threads[0]);
2247   }
2248   /* clean up */
2249   MEM_freeN(sb_threads);
2250 }
2251
2252 static void softbody_calc_forces(
2253     struct Depsgraph *depsgraph, Scene *scene, Object *ob, float forcetime, float timenow)
2254 {
2255   /* rule we never alter free variables :bp->vec bp->pos in here !
2256    * this will ruin adaptive stepsize AKA heun! (BM)
2257    */
2258   SoftBody *sb = ob->soft; /* is supposed to be there */
2259   /*BodyPoint *bproot;*/   /* UNUSED */
2260   /* float gravity; */     /* UNUSED */
2261   /* float iks; */
2262   float fieldfactor = -1.0f, windfactor = 0.25;
2263   int do_deflector /*, do_selfcollision*/, do_springcollision, do_aero;
2264
2265   /* gravity = sb->grav * sb_grav_force_scale(ob); */ /* UNUSED */
2266
2267   /* check conditions for various options */
2268   do_deflector = query_external_colliders(depsgraph, sb->collision_group);
2269   /* do_selfcollision=((ob->softflag & OB_SB_EDGES) && (sb->bspring)&& (ob->softflag & OB_SB_SELF)); */ /* UNUSED */
2270   do_springcollision = do_deflector && (ob->softflag & OB_SB_EDGES) &&
2271                        (ob->softflag & OB_SB_EDGECOLL);
2272   do_aero = ((sb->aeroedge) && (ob->softflag & OB_SB_EDGES));
2273
2274   /* iks  = 1.0f/(1.0f-sb->inspring)-1.0f; */ /* inner spring constants function */ /* UNUSED */
2275   /* bproot= sb->bpoint; */ /* need this for proper spring addressing */            /* UNUSED */
2276
2277   if (do_springcollision || do_aero) {
2278     sb_sfesf_threads_run(depsgraph, scene, ob, timenow, sb->totspring, NULL);
2279   }
2280
2281   /* after spring scan because it uses Effoctors too */
2282   ListBase *effectors = BKE_effectors_create(depsgraph, ob, NULL, sb->effector_weights);
2283
2284   if (do_deflector) {
2285     float defforce[3];
2286     do_deflector = sb_detect_aabb_collisionCached(defforce, ob, timenow);
2287   }
2288
2289   sb_cf_threads_run(scene,
2290                     ob,
2291                     forcetime,
2292                     timenow,
2293                     sb->totpoint,
2294                     NULL,
2295                     effectors,
2296                     do_deflector,
2297                     fieldfactor,
2298                     windfactor);
2299
2300   /* finally add forces caused by face collision */
2301   if (ob->softflag & OB_SB_FACECOLL) {
2302     scan_for_ext_face_forces(ob, timenow);
2303   }
2304
2305   /* finish matrix and solve */
2306   BKE_effectors_free(effectors);
2307 }
2308
2309 static void softbody_apply_forces(Object *ob, float forcetime, int mode, float *err, int mid_flags)
2310 {
2311   /* time evolution */
2312   /* actually does an explicit euler step mode == 0 */
2313   /* or heun ~ 2nd order runge-kutta steps, mode 1, 2 */
2314   SoftBody *sb = ob->soft; /* is supposed to be there */
2315   BodyPoint *bp;
2316   float dx[3] = {0}, dv[3], aabbmin[3], aabbmax[3], cm[3] = {0.0f, 0.0f, 0.0f};
2317   float timeovermass /*, freezeloc=0.00001f, freezeforce=0.00000000001f*/;
2318   float maxerrpos = 0.0f, maxerrvel = 0.0f;
2319   int a, fuzzy = 0;
2320
2321   forcetime *= sb_time_scale(ob);
2322
2323   aabbmin[0] = aabbmin[1] = aabbmin[2] = 1e20f;
2324   aabbmax[0] = aabbmax[1] = aabbmax[2] = -1e20f;
2325
2326   /* old one with homogeneous masses  */
2327   /* claim a minimum mass for vertex */
2328 #if 0
2329   if (sb->nodemass > 0.009999f)
2330     timeovermass = forcetime / sb->nodemass;
2331   else
2332     timeovermass = forcetime / 0.009999f;
2333 #endif
2334
2335   for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
2336     /* now we have individual masses   */
2337     /* claim a minimum mass for vertex */
2338     if (_final_mass(ob, bp) > 0.009999f) {
2339       timeovermass = forcetime / _final_mass(ob, bp);
2340     }
2341     else {
2342       timeovermass = forcetime / 0.009999f;
2343     }
2344
2345     if (_final_goal(ob, bp) < SOFTGOALSNAP) {
2346       /* this makes t~ = t */
2347       if (mid_flags & MID_PRESERVE) {
2348         copy_v3_v3(dx, bp->vec);
2349       }
2350
2351       /* so here is (v)' = a(cceleration) = sum(F_springs)/m + gravitation + some friction forces  + more forces*/
2352       /* the ( ... )' operator denotes derivate respective time */
2353       /* the euler step for velocity then becomes */
2354       /* v(t + dt) = v(t) + a(t) * dt */
2355       mul_v3_fl(bp->force, timeovermass); /* individual mass of node here */
2356       /* some nasty if's to have heun in here too */
2357       copy_v3_v3(dv, bp->force);
2358
2359       if (mode == 1) {
2360         copy_v3_v3(bp->prevvec, bp->vec);
2361         copy_v3_v3(bp->prevdv, dv);
2362       }
2363
2364       if (mode == 2) {
2365         /* be optimistic and execute step */
2366         bp->vec[0] = bp->prevvec[0] + 0.5f * (dv[0] + bp->prevdv[0]);
2367         bp->vec[1] = bp->prevvec[1] + 0.5f * (dv[1] + bp->prevdv[1]);
2368         bp->vec[2] = bp->prevvec[2] + 0.5f * (dv[2] + bp->prevdv[2]);
2369         /* compare euler to heun to estimate error for step sizing */
2370         maxerrvel = max_ff(maxerrvel, fabsf(dv[0] - bp->prevdv[0]));
2371         maxerrvel = max_ff(maxerrvel, fabsf(dv[1] - bp->prevdv[1]));
2372         maxerrvel = max_ff(maxerrvel, fabsf(dv[2] - bp->prevdv[2]));
2373       }
2374       else {
2375         add_v3_v3(bp->vec, bp->force);
2376       }
2377
2378       /* this makes t~ = t+dt */
2379       if (!(mid_flags & MID_PRESERVE)) {
2380         copy_v3_v3(dx, bp->vec);
2381       }
2382
2383       /* so here is (x)'= v(elocity) */
2384       /* the euler step for location then becomes */
2385       /* x(t + dt) = x(t) + v(t~) * dt */
2386       mul_v3_fl(dx, forcetime);
2387
2388       /* the freezer coming sooner or later */
2389 #if 0
2390       if ((dot_v3v3(dx, dx) < freezeloc) && (dot_v3v3(bp->force, bp->force) < freezeforce)) {
2391         bp->frozen /= 2;
2392       }
2393       else {
2394         bp->frozen = min_ff(bp->frozen * 1.05f, 1.0f);
2395       }
2396       mul_v3_fl(dx, bp->frozen);
2397 #endif
2398       /* again some nasty if's to have heun in here too */
2399       if (mode == 1) {
2400         copy_v3_v3(bp->prevpos, bp->pos);
2401         copy_v3_v3(bp->prevdx, dx);
2402       }
2403
2404       if (mode == 2) {
2405         bp->pos[0] = bp->prevpos[0] + 0.5f * (dx[0] + bp->prevdx[0]);
2406         bp->pos[1] = bp->prevpos[1] + 0.5f * (dx[1] + bp->prevdx[1]);
2407         bp->pos[2] = bp->prevpos[2] + 0.5f * (dx[2] + bp->prevdx[2]);
2408         maxerrpos = max_ff(maxerrpos, fabsf(dx[0] - bp->prevdx[0]));
2409         maxerrpos = max_ff(maxerrpos, fabsf(dx[1] - bp->prevdx[1]));
2410         maxerrpos = max_ff(maxerrpos, fabsf(dx[2] - bp->prevdx[2]));
2411
2412         /* bp->choke is set when we need to pull a vertex or edge out of the collider.
2413          * the collider object signals to get out by pushing hard. on the other hand
2414          * we don't want to end up in deep space so we add some <viscosity>
2415          * to balance that out */
2416         if (bp->choke2 > 0.0f) {
2417           mul_v3_fl(bp->vec, (1.0f - bp->choke2));
2418         }
2419         if (bp->choke > 0.0f) {
2420           mul_v3_fl(bp->vec, (1.0f - bp->choke));
2421         }
2422       }
2423       else {
2424         add_v3_v3(bp->pos, dx);
2425       }
2426     } /*snap*/
2427     /* so while we are looping BPs anyway do statistics on the fly */
2428     minmax_v3v3_v3(aabbmin, aabbmax, bp->pos);
2429     if (bp->loc_flag & SBF_DOFUZZY) {
2430       fuzzy = 1;
2431     }
2432   } /*for*/
2433
2434   if (sb->totpoint) {
2435     mul_v3_fl(cm, 1.0f / sb->totpoint);
2436   }
2437   if (sb->scratch) {
2438     copy_v3_v3(sb->scratch->aabbmin, aabbmin);
2439     copy_v3_v3(sb->scratch->aabbmax, aabbmax);
2440   }
2441
2442   if (err) { /* so step size will be controlled by biggest difference in slope */
2443     if (sb->solverflags & SBSO_OLDERR) {
2444       *err = max_ff(maxerrpos, maxerrvel);
2445     }
2446     else {
2447       *err = maxerrpos;
2448     }
2449     //printf("EP %f EV %f\n", maxerrpos, maxerrvel);
2450     if (fuzzy) {
2451       *err /= sb->fuzzyness;
2452     }
2453   }
2454 }
2455
2456 /* used by heun when it overshoots */
2457 static void softbody_restore_prev_step(Object *ob)
2458 {
2459   SoftBody *sb = ob->soft; /* is supposed to be there*/
2460   BodyPoint *bp;
2461   int a;
2462
2463   for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
2464     copy_v3_v3(bp->vec, bp->prevvec);
2465     copy_v3_v3(bp->pos, bp->prevpos);
2466   }
2467 }
2468
2469 #if 0
2470 static void softbody_store_step(Object *ob)
2471 {
2472   SoftBody *sb = ob->soft; /* is supposed to be there*/
2473   BodyPoint *bp;
2474   int a;
2475
2476   for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
2477     copy_v3_v3(bp->prevvec, bp->vec);
2478     copy_v3_v3(bp->prevpos, bp->pos);
2479   }
2480 }
2481
2482 /* used by predictors and correctors */
2483 static void softbody_store_state(Object *ob, float *ppos, float *pvel)
2484 {
2485   SoftBody *sb = ob->soft; /* is supposed to be there*/
2486   BodyPoint *bp;
2487   int a;
2488   float *pp = ppos, *pv = pvel;
2489
2490   for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
2491
2492     copy_v3_v3(pv, bp->vec);
2493     pv += 3;
2494
2495     copy_v3_v3(pp, bp->pos);
2496     pp += 3;
2497   }
2498 }
2499
2500 /* used by predictors and correctors */
2501 static void softbody_retrieve_state(Object *ob, float *ppos, float *pvel)
2502 {
2503   SoftBody *sb = ob->soft; /* is supposed to be there*/
2504   BodyPoint *bp;
2505   int a;
2506   float *pp = ppos, *pv = pvel;
2507
2508   for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
2509
2510     copy_v3_v3(bp->vec, pv);
2511     pv += 3;
2512
2513     copy_v3_v3(bp->pos, pp);
2514     pp += 3;
2515   }
2516 }
2517
2518 /* used by predictors and correctors */
2519 static void softbody_swap_state(Object *ob, float *ppos, float *pvel)
2520 {
2521   SoftBody *sb = ob->soft; /* is supposed to be there*/
2522   BodyPoint *bp;
2523   int a;
2524   float *pp = ppos, *pv = pvel;
2525   float temp[3];
2526
2527   for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
2528
2529     copy_v3_v3(temp, bp->vec);
2530     copy_v3_v3(bp->vec, pv);
2531     copy_v3_v3(pv, temp);
2532     pv += 3;
2533
2534     copy_v3_v3(temp, bp->pos);
2535     copy_v3_v3(bp->pos, pp);
2536     copy_v3_v3(pp, temp);
2537     pp += 3;
2538   }
2539 }
2540 #endif
2541
2542 /* care for bodypoints taken out of the 'ordinary' solver step
2543  * because they are screwed to goal by bolts
2544  * they just need to move along with the goal in time
2545  * we need to adjust them on sub frame timing in solver
2546  * so now when frame is done .. put 'em to the position at the end of frame
2547  */
2548 static void softbody_apply_goalsnap(Object *ob)
2549 {
2550   SoftBody *sb = ob->soft; /* is supposed to be there */
2551   BodyPoint *bp;
2552   int a;
2553
2554   for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
2555     if (_final_goal(ob, bp) >= SOFTGOALSNAP) {
2556       copy_v3_v3(bp->prevpos, bp->pos);
2557       copy_v3_v3(bp->pos, bp->origT);
2558     }
2559   }
2560 }
2561
2562 static void apply_spring_memory(Object *ob)
2563 {
2564   SoftBody *sb = ob->soft;
2565   BodySpring *bs;
2566   BodyPoint *bp1, *bp2;
2567   int a;
2568   float b, l, r;
2569
2570   if (sb && sb->totspring) {
2571     b = sb->plastic;
2572     for (a = 0; a < sb->totspring; a++) {
2573       bs = &sb->bspring[a];
2574       bp1 = &sb->bpoint[bs->v1];
2575       bp2 = &sb->bpoint[bs->v2];
2576       l = len_v3v3(bp1->pos, bp2->pos);
2577       r = bs->len / l;
2578       if ((r > 1.05f) || (r < 0.95f)) {
2579         bs->len = ((100.0f - b) * bs->len + b * l) / 100.0f;
2580       }
2581     }
2582   }
2583 }
2584
2585 /* expects full initialized softbody */
2586 static void interpolate_exciter(Object *ob, int timescale, int time)
2587 {
2588   SoftBody *sb = ob->soft;
2589   BodyPoint *bp;
2590   float f;
2591   int a;
2592
2593   f = (float)time / (float)timescale;
2594
2595   for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
2596     bp->origT[0] = bp->origS[0] + f * (bp->origE[0] - bp->origS[0]);
2597     bp->origT[1] = bp->origS[1] + f * (bp->origE[1] - bp->origS[1]);
2598     bp->origT[2] = bp->origS[2] + f * (bp->origE[2] - bp->origS[2]);
2599     if (_final_goal(ob, bp) >= SOFTGOALSNAP) {
2600       bp->vec[0] = bp->origE[0] - bp->origS[0];
2601       bp->vec[1] = bp->origE[1] - bp->origS[1];
2602       bp->vec[2] = bp->origE[2] - bp->origS[2];
2603     }
2604   }
2605 }
2606
2607 /* ************ convertors ********** */
2608
2609 /* for each object type we need;
2610  * - xxxx_to_softbody(Object *ob)      : a full (new) copy, creates SB geometry
2611  */
2612
2613 /* Resetting a Mesh SB object's springs */
2614 /* Spring length are caculted from'raw' mesh vertices that are NOT altered by modifier stack. */
2615 static void springs_from_mesh(Object *ob)
2616 {
2617   SoftBody *sb;
2618   Mesh *me = ob->data;
2619   BodyPoint *bp;
2620   int a;
2621   float scale = 1.0f;
2622
2623   sb = ob->soft;
2624   if (me && sb) {
2625     /* using bp->origS as a container for spring calculations here
2626      * will be overwritten sbObjectStep() to receive
2627      * actual modifier stack positions
2628      */
2629     if (me->totvert) {
2630       bp = ob->soft->bpoint;
2631       for (a = 0; a < me->totvert; a++, bp++) {
2632         copy_v3_v3(bp->origS, me->mvert[a].co);
2633         mul_m4_v3(ob->obmat, bp->origS);
2634       }
2635     }
2636     /* recalculate spring length for meshes here */
2637     /* public version shrink to fit */
2638     if (sb->springpreload != 0) {
2639       scale = sb->springpreload / 100.0f;
2640     }
2641     for (a = 0; a < sb->totspring; a++) {
2642       BodySpring *bs = &sb->bspring[a];
2643       bs->len = scale * len_v3v3(sb->bpoint[bs->v1].origS, sb->bpoint[bs->v2].origS);
2644     }
2645   }
2646 }
2647
2648 /* makes totally fresh start situation */
2649 static void mesh_to_softbody(Scene *scene, Object *ob)
2650 {
2651   SoftBody *sb;
2652   Mesh *me = ob->data;
2653   MEdge *medge = me->medge;
2654   BodyPoint *bp;
2655   BodySpring *bs;
2656   int a, totedge;
2657   int defgroup_index, defgroup_index_mass, defgroup_index_spring;
2658
2659   if (ob->softflag & OB_SB_EDGES) {
2660     totedge = me->totedge;
2661   }
2662   else {
2663     totedge = 0;
2664   }
2665
2666   /* renew ends with ob->soft with points and edges, also checks & makes ob->soft */
2667   renew_softbody(scene, ob, me->totvert, totedge);
2668
2669   /* we always make body points */
2670   sb = ob->soft;
2671   bp = sb->bpoint;
2672
2673   defgroup_index = me->dvert ? (sb->vertgroup - 1) : -1;
2674   defgroup_index_mass = me->dvert ? defgroup_name_index(ob, sb->namedVG_Mass) : -1;
2675   defgroup_index_spring = me->dvert ? defgroup_name_index(ob, sb->namedVG_Spring_K) : -1;
2676
2677   for (a = 0; a < me->totvert; a++, bp++) {
2678     /* get scalar values needed  *per vertex* from vertex group functions,
2679      * so we can *paint* them nicly ..
2680      * they are normalized [0.0..1.0] so may be we need amplitude for scale
2681      * which can be done by caller but still .. i'd like it to go this way
2682      */
2683
2684     if (ob->softflag & OB_SB_GOAL) {
2685       BLI_assert(bp->goal == sb->defgoal);
2686     }
2687     if ((ob->softflag & OB_SB_GOAL) && (defgroup_index != -1)) {
2688       bp->goal *= defvert_find_weight(&me->dvert[a], defgroup_index);
2689     }
2690
2691     /* to proof the concept
2692      * this enables per vertex *mass painting*
2693      */
2694
2695     if (defgroup_index_mass != -1) {
2696       bp->mass *= defvert_find_weight(&me->dvert[a], defgroup_index_mass);
2697     }
2698
2699     if (defgroup_index_spring != -1) {
2700       bp->springweight *= defvert_find_weight(&me->dvert[a], defgroup_index_spring);
2701     }
2702   }
2703
2704   /* but we only optionally add body edge springs */
2705   if (ob->softflag & OB_SB_EDGES) {
2706     if (medge) {
2707       bs = sb->bspring;
2708       for (a = me->totedge; a > 0; a--, medge++, bs++) {
2709         bs->v1 = medge->v1;
2710         bs->v2 = medge->v2;
2711         bs->springtype = SB_EDGE;
2712       }
2713
2714       /* insert *diagonal* springs in quads if desired */
2715       if (ob->softflag & OB_SB_QUADS) {
2716         add_mesh_quad_diag_springs(ob);
2717       }
2718
2719       build_bps_springlist(ob); /* scan for springs attached to bodypoints ONCE */
2720       /* insert *other second order* springs if desired */
2721       if (sb->secondspring > 0.0000001f) {
2722         add_2nd_order_springs(
2723             ob, sb->secondspring); /* exploits the first run of build_bps_springlist(ob);*/
2724         build_bps_springlist(ob);  /* yes we need to do it again*/
2725       }
2726       springs_from_mesh(ob); /* write the 'rest'-length of the springs */
2727       if (ob->softflag & OB_SB_SELF) {
2728         calculate_collision_balls(ob);
2729       }
2730     }
2731   }
2732 }
2733
2734 static void mesh_faces_to_scratch(Object *ob)
2735 {
2736   SoftBody *sb = ob->soft;
2737   const Mesh *me = ob->data;
2738   MLoopTri *looptri, *lt;
2739   BodyFace *bodyface;
2740   int a;
2741   /* alloc and copy faces*/
2742
2743   sb->scratch->totface = poly_to_tri_count(me->totpoly, me->totloop);
2744   looptri = lt = MEM_mallocN(sizeof(*looptri) * sb->scratch->totface, __func__);
2745   BKE_mesh_recalc_looptri(me->mloop, me->mpoly, me->mvert, me->totloop, me->totpoly, looptri);
2746
2747   bodyface = sb->scratch->bodyface = MEM_mallocN(sizeof(BodyFace) * sb->scratch->totface,
2748                                                  "SB_body_Faces");
2749
2750   for (a = 0; a < sb->scratch->totface; a++, lt++, bodyface++) {
2751     bodyface->v1 = me->mloop[lt->tri[0]].v;
2752     bodyface->v2 = me->mloop[lt->tri[1]].v;
2753     bodyface->v3 = me->mloop[lt->tri[2]].v;
2754     zero_v3(bodyface->ext_force);
2755     bodyface->ext_force[0] = bodyface->ext_force[1] = bodyface->ext_force[2] = 0.0f;
2756     bodyface->flag = 0;
2757   }
2758
2759   MEM_freeN(looptri);
2760 }
2761 static void reference_to_scratch(Object *ob)
2762 {
2763   SoftBody *sb = ob->soft;
2764   ReferenceVert *rp;
2765   BodyPoint *bp;
2766   float accu_pos[3] = {0.f, 0.f, 0.f};
2767   float accu_mass = 0.f;
2768   int a;
2769
2770   sb->scratch->Ref.ivert = MEM_mallocN(sizeof(ReferenceVert) * sb->totpoint, "SB_Reference");
2771   bp = ob->soft->bpoint;
2772   rp = sb->scratch->Ref.ivert;
2773   for (a = 0; a < sb->totpoint; a++, rp++, bp++) {
2774     copy_v3_v3(rp->pos, bp->pos);
2775     add_v3_v3(accu_pos, bp->pos);
2776     accu_mass += _final_mass(ob, bp);
2777   }
2778   mul_v3_fl(accu_pos, 1.0f / accu_mass);
2779   copy_v3_v3(sb->scratch->Ref.com, accu_pos);
2780   /* printf("reference_to_scratch\n"); */
2781 }
2782
2783 /*
2784  * helper function to get proper spring length
2785  * when object is rescaled
2786  */
2787 static float globallen(float *v1, float *v2, Object *ob)
2788 {
2789   float p1[3], p2[3];
2790   copy_v3_v3(p1, v1);
2791   mul_m4_v3(ob->obmat, p1);
2792   copy_v3_v3(p2, v2);
2793   mul_m4_v3(ob->obmat, p2);
2794   return len_v3v3(p1, p2);
2795 }
2796
2797 static void makelatticesprings(Lattice *lt, BodySpring *bs, int dostiff, Object *ob)
2798 {
2799   BPoint *bp = lt->def, *bpu;
2800   int u, v, w, dv, dw, bpc = 0, bpuc;
2801
2802   dv = lt->pntsu;
2803   dw = dv * lt->pntsv;
2804
2805   for (w = 0; w < lt->pntsw; w++) {
2806
2807     for (v = 0; v < lt->pntsv; v++) {
2808
2809       for (u = 0, bpuc = 0, bpu = NULL; u < lt->pntsu; u++, bp++, bpc++) {
2810
2811         if (w) {
2812           bs->v1 = bpc;
2813           bs->v2 = bpc - dw;
2814           bs->springtype = SB_EDGE;
2815           bs->len = globallen((bp - dw)->vec, bp->vec, ob);
2816           bs++;
2817         }
2818         if (v) {
2819           bs->v1 = bpc;
2820           bs->v2 = bpc - dv;
2821           bs->springtype = SB_EDGE;
2822           bs->len = globallen((bp - dv)->vec, bp->vec, ob);
2823           bs++;
2824         }
2825         if (u) {
2826           bs->v1 = bpuc;
2827           bs->v2 = bpc;
2828           bs->springtype = SB_EDGE;
2829           bs->len = globallen((bpu)->vec, bp->vec, ob);
2830           bs++;
2831         }
2832
2833         if (dostiff) {
2834
2835           if (w) {
2836             if (v && u) {
2837               bs->v1 = bpc;
2838               bs->v2 = bpc - dw - dv - 1;
2839               bs->springtype = SB_BEND;
2840               bs->len = globallen((bp - dw - dv - 1)->vec, bp->vec, ob);
2841               bs++;
2842             }
2843             if ((v < lt->pntsv - 1) && (u != 0)) {
2844               bs->v1 = bpc;
2845               bs->v2 = bpc - dw + dv - 1;
2846               bs->springtype = SB_BEND;
2847               bs->len = globallen((bp - dw + dv - 1)->vec, bp->vec, ob);
2848               bs++;
2849             }
2850           }
2851
2852           if (w < lt->pntsw - 1) {
2853             if (v && u) {
2854               bs->v1 = bpc;
2855               bs->v2 = bpc + dw - dv - 1;
2856               bs->springtype = SB_BEND;
2857               bs->len = globallen((bp + dw - dv - 1)->vec, bp->vec, ob);
2858               bs++;
2859             }
2860             if ((v < lt->pntsv - 1) && (u != 0)) {
2861               bs->v1 = bpc;
2862               bs->v2 = bpc + dw + dv - 1;
2863               bs->springtype = SB_BEND;
2864               bs->len = globallen((bp + dw + dv - 1)->vec, bp->vec, ob);
2865               bs++;
2866             }
2867           }
2868         }
2869         bpu = bp;
2870         bpuc = bpc;
2871       }
2872     }
2873   }
2874 }
2875
2876 /* makes totally fresh start situation */
2877 static void lattice_to_softbody(Scene *scene, Object *ob)
2878 {
2879   Lattice *lt = ob->data;
2880   SoftBody *sb;
2881   int totvert, totspring = 0, a;
2882   BodyPoint *bp;
2883   BPoint *bpnt = lt->def;
2884   int defgroup_index, defgroup_index_mass, defgroup_index_spring;
2885
2886   totvert = lt->pntsu * lt->pntsv * lt->pntsw;
2887
2888   if (ob->softflag & OB_SB_EDGES) {
2889     totspring = ((lt->pntsu - 1) * lt->pntsv + (lt->pntsv - 1) * lt->pntsu) * lt->pntsw +
2890                 lt->pntsu * lt->pntsv * (lt->pntsw - 1);
2891     if (ob->softflag & OB_SB_QUADS) {
2892       totspring += 4 * (lt->pntsu - 1) * (lt->pntsv - 1) * (lt->pntsw - 1);
2893     }
2894   }
2895
2896   /* renew ends with ob->soft with points and edges, also checks & makes ob->soft */
2897   renew_softbody(scene, ob, totvert, totspring);
2898   sb = ob->soft; /* can be created in renew_softbody() */
2899   bp = sb->bpoint;
2900
2901   defgroup_index = lt->dvert ? (sb->vertgroup - 1) : -1;
2902   defgroup_index_mass = lt->dvert ? defgroup_name_index(ob, sb->namedVG_Mass) : -1;
2903   defgroup_index_spring = lt->dvert ? defgroup_name_index(ob, sb->namedVG_Spring_K) : -1;
2904
2905   /* same code used as for mesh vertices */
2906   for (a = 0; a < totvert; a++, bp++, bpnt++) {
2907
2908     if (ob->softflag & OB_SB_GOAL) {
2909       BLI_assert(bp->goal == sb->defgoal);
2910     }
2911
2912     if ((ob->softflag & OB_SB_GOAL) && (defgroup_index != -1)) {
2913       bp->goal *= defvert_find_weight(&lt->dvert[a], defgroup_index);
2914     }
2915     else {
2916       bp->goal *= bpnt->weight;
2917     }
2918
2919     if (defgroup_index_mass != -1) {
2920       bp->mass *= defvert_find_weight(&lt->dvert[a], defgroup_index_mass);
2921     }
2922
2923     if (defgroup_index_spring != -1) {
2924       bp->springweight *= defvert_find_weight(&lt->dvert[a], defgroup_index_spring);
2925     }
2926   }
2927
2928   /* create some helper edges to enable SB lattice to be useful at all */
2929   if (ob->softflag & OB_SB_EDGES) {
2930     makelatticesprings(lt, ob->soft->bspring, ob->softflag & OB_SB_QUADS, ob);
2931     build_bps_springlist(ob); /* link bps to springs */
2932   }
2933 }
2934
2935 /* makes totally fresh start situation */
2936 static void curve_surf_to_softbody(Scene *scene, Object *ob)
2937 {
2938   Curve *cu = ob->data;
2939   SoftBody *sb;
2940   BodyPoint *bp;
2941   BodySpring *bs;
2942   Nurb *nu;
2943   BezTriple *bezt;
2944   BPoint *bpnt;
2945   int a, curindex = 0;
2946   int totvert, totspring = 0, setgoal = 0;
2947
2948   totvert = BKE_nurbList_verts_count(&cu->nurb);
2949
2950   if (ob->softflag & OB_SB_EDGES) {
2951     if (ob->type == OB_CURVE) {
2952       totspring = totvert - BLI_listbase_count(&cu->nurb);
2953     }
2954   }
2955
2956   /* renew ends with ob->soft with points and edges, also checks & makes ob->soft */
2957   renew_softbody(scene, ob, totvert, totspring);
2958   sb = ob->soft; /* can be created in renew_softbody() */
2959
2960   /* set vars now */
2961   bp = sb->bpoint;
2962   bs = sb->bspring;
2963
2964   /* weights from bpoints, same code used as for mesh vertices */
2965   /* if ((ob->softflag & OB_SB_GOAL) && sb->vertgroup) 2.4x hack*/
2966   /* new! take the weights from curve vertex anyhow */
2967   if (ob->softflag & OB_SB_GOAL) {
2968     setgoal = 1;
2969   }
2970
2971   for (nu = cu->nurb.first; nu; nu = nu->next) {
2972     if (nu->bezt) {
2973       /* bezier case ; this is nicly said naive; who ever wrote this part, it was not me (JOW) :) */
2974       /* a: never ever make tangent handles (sub) and or (ob)ject to collision */
2975       /* b: rather calculate them using some C2 (C2= continuous in second derivate -> no jump in bending ) condition */
2976       /* not too hard to do, but needs some more code to care for;  some one may want look at it  JOW 2010/06/12*/
2977       for (bezt = nu->bezt, a = 0; a < nu->pntsu; a++, bezt++, bp += 3, curindex += 3) {
2978         if (setgoal) {
2979           bp->goal *= bezt->weight;
2980
2981           /* all three triples */
2982           (bp + 1)->goal = bp->goal;
2983           (bp + 2)->goal = bp->goal;
2984           /*do not collide handles */
2985           (bp + 1)->loc_flag |= SBF_OUTOFCOLLISION;
2986           (bp + 2)->loc_flag |= SBF_OUTOFCOLLISION;
2987         }
2988
2989         if (totspring) {
2990           if (a > 0) {
2991             bs->v1 = curindex - 3;
2992             bs->v2 = curindex;
2993             bs->springtype = SB_HANDLE;
2994             bs->len = globallen((bezt - 1)->vec[0], bezt->vec[0], ob);
2995             bs++;
2996           }
2997           bs->v1 = curindex;
2998           bs->v2 = curindex + 1;
2999           bs->springtype = SB_HANDLE;
3000           bs->len = globallen(bezt->vec[0], bezt->vec[1], ob);
3001           bs++;
3002
3003           bs->v1 = curindex + 1;
3004           bs->v2 = curindex + 2;
3005           bs->springtype = SB_HANDLE;
3006           bs->len = globallen(bezt->vec[1], bezt->vec[2], ob);
3007           bs++;
3008         }
3009       }
3010     }
3011     else {
3012       for (bpnt = nu->bp, a = 0; a < nu->pntsu * nu->pntsv; a++, bpnt++, bp++, curindex++) {
3013         if (setgoal) {
3014           bp->goal *= bpnt->weight;
3015         }
3016         if (totspring && a > 0) {
3017           bs->v1 = curindex - 1;
3018           bs->v2 = curindex;
3019           bs->springtype = SB_EDGE;
3020           bs->len = globallen((bpnt - 1)->vec, bpnt->vec, ob);
3021           bs++;
3022         }
3023       }
3024     }
3025   }
3026
3027   if (totspring) {
3028     build_bps_springlist(ob); /* link bps to springs */
3029     if (ob->softflag & OB_SB_SELF) {
3030       calculate_collision_balls(ob);
3031     }
3032   }
3033 }
3034
3035 /* copies softbody result back in object */
3036 static void softbody_to_object(Object *ob, float (*vertexCos)[3], int numVerts, int local)
3037 {
3038   SoftBody *sb = ob->soft;
3039   if (sb) {
3040     BodyPoint *bp = sb->bpoint;
3041     int a;
3042     if (sb->solverflags & SBSO_ESTIMATEIPO) {
3043       SB_estimate_transform(ob, sb->lcom, sb->lrot, sb->lscale);
3044     }
3045     /* inverse matrix is not uptodate... */
3046     invert_m4_m4(ob->imat, ob->obmat);
3047
3048     for (a = 0; a < numVerts; a++, bp++) {
3049       copy_v3_v3(vertexCos[a], bp->pos);
3050       if (local == 0) {
3051         mul_m4_v3(ob->imat, vertexCos[a]); /* softbody is in global coords, baked optionally not */
3052       }
3053     }
3054   }
3055 }
3056
3057 /* +++ ************ maintaining scratch *************** */
3058 static void sb_new_scratch(SoftBody *sb)
3059 {
3060   if (!sb) {
3061     return;
3062   }
3063   sb->scratch = MEM_callocN(sizeof(SBScratch), "SBScratch");
3064   sb->scratch->colliderhash = BLI_ghash_ptr_new("sb_new_scratch gh");
3065   sb->scratch->bodyface = NULL;
3066   sb->scratch->totface = 0;
3067   sb->scratch->aabbmax[0] = sb->scratch->aabbmax[1] = sb->scratch->aabbmax[2] = 1.0e30f;
3068   sb->scratch->aabbmin[0] = sb->scratch->aabbmin[1] = sb->scratch->aabbmin[2] = -1.0e30f;
3069   sb->scratch->Ref.ivert = NULL;
3070 }
3071 /* --- ************ maintaining scratch *************** */
3072
3073 /* ************ Object level, exported functions *************** */
3074
3075 /* allocates and initializes general main data */
3076 SoftBody *sbNew(Scene *scene)
3077 {
3078   SoftBody *sb;
3079
3080   sb = MEM_callocN(sizeof(SoftBody), "softbody");
3081
3082   sb->mediafrict = 0.5f;
3083   sb->nodemass = 1.0f;
3084   sb->grav = 9.8f;
3085   sb->physics_speed = 1.0f;
3086   sb->rklimit = 0.1f;
3087
3088   sb->goalspring = 0.5f;
3089   sb->goalfrict = 0.0f;
3090   sb->mingoal = 0.0f;
3091   sb->maxgoal = 1.0f;
3092   sb->defgoal = 0.7f;
3093
3094   sb->inspring = 0.5f;
3095   sb->infrict = 0.5f;
3096   /*todo backward file compat should copy inspring to inpush while reading old files*/
3097   sb->inpush = 0.5f;
3098
3099   sb->interval = 10;
3100   sb->sfra = scene->r.sfra;
3101   sb->efra = scene->r.efra;
3102
3103   sb->colball = 0.49f;
3104   sb->balldamp = 0.50f;
3105   sb->ballstiff = 1.0f;
3106   sb->sbc_mode = 1;
3107
3108   sb->minloops = 10;
3109   sb->maxloops = 300;
3110
3111   sb->choke = 3;
3112   sb_new_scratch(sb);
3113   /*todo backward file compat should set sb->shearstiff = 1.0f while reading old files*/
3114   sb->shearstiff = 1.0f;
3115   sb->solverflags |= SBSO_OLDERR;
3116
3117   sb->shared = MEM_callocN(sizeof(*sb->shared), "SoftBody_Shared");
3118   sb->shared->pointcache = BKE_ptcache_add(&sb->shared->ptcaches);
3119
3120   if (!sb->effector_weights) {
3121     sb->effector_weights = BKE_effector_add_weights(NULL);
3122   }
3123
3124   sb->last_frame = MINFRAME - 1;
3125
3126   return sb;
3127 }
3128
3129 /* frees all */
3130 void sbFree(Object *ob)
3131 {
3132   SoftBody *sb = ob->soft;
3133   if (sb == NULL) {
3134     return;
3135   }
3136
3137   free_softbody_intern(sb);
3138
3139   if ((ob->id.tag & LIB_TAG_NO_MAIN) == 0) {
3140     /* Only free shared data on non-CoW copies */
3141     BKE_ptcache_free_list(&sb->shared->ptcaches);
3142     sb->shared->pointcache = NULL;
3143     MEM_freeN(sb->shared);
3144   }
3145   if (sb->effector_weights) {
3146     MEM_freeN(sb->effector_weights);
3147   }
3148   MEM_freeN(sb);
3149
3150   ob->soft = NULL;
3151 }
3152
3153 void sbFreeSimulation(SoftBody *sb)
3154 {
3155   free_softbody_intern(sb);
3156 }
3157
3158 /* makes totally fresh start situation */
3159 void sbObjectToSoftbody(Object *ob)
3160 {
3161   //ob->softflag |= OB_SB_REDO;
3162
3163   free_softbody_intern(ob->soft);
3164 }
3165
3166 static int object_has_edges(Object *ob)
3167 {
3168   if (ob->type == OB_MESH) {
3169     return ((Mesh *)ob->data)->totedge;
3170   }
3171   else if (ob->type == OB_LATTICE) {
3172     return 1;
3173   }
3174   else {
3175     return 0;
3176   }
3177 }
3178
3179 /* SB global visible functions */
3180 void sbSetInterruptCallBack(int (*f)(void))
3181 {
3182   SB_localInterruptCallBack = f;
3183 }
3184
3185 static void softbody_update_positions(Object *ob,
3186                                       SoftBody *sb,
3187                                       float (*vertexCos)[3],
3188                                       int numVerts)
3189 {
3190   BodyPoint *bp;
3191   int a;
3192
3193   if (!sb || !sb->bpoint) {
3194     return;
3195   }
3196
3197   for (a = 0, bp = sb->bpoint; a < numVerts; a++, bp++) {
3198     /* store where goals are now */
3199     copy_v3_v3(bp->origS, bp->origE);
3200     /* copy the position of the goals at desired end time */
3201     copy_v3_v3(bp->origE, vertexCos[a]);
3202     /* vertexCos came from local world, go global */
3203     mul_m4_v3(ob->obmat, bp->origE);
3204     /* just to be save give bp->origT a defined value
3205      * will be calculated in interpolate_exciter() */
3206     copy_v3_v3(bp->origT, bp->origE);
3207   }
3208 }
3209
3210 /* void SB_estimate_transform */
3211 /* input   Object *ob out (says any object that can do SB like mesh, lattice, curve )
3212  * output  float lloc[3], float lrot[3][3], float lscale[3][3]
3213  * that is:
3214  * a precise position vector denoting the motion of the center of mass
3215  * give a rotation/scale matrix using averaging method, that's why estimate and not calculate
3216  * see: this is kind of reverse engineering: having to states of a point cloud and recover what happened
3217  * our advantage here we know the identity of the vertex
3218  * there are others methods giving other results.
3219  * lloc, lrot, lscale are allowed to be NULL, just in case you don't need it.
3220  * should be pretty useful for pythoneers :)
3221  * not! velocity .. 2nd order stuff
3222  * vcloud_estimate_transform_v3 see
3223  */
3224
3225 void SB_estimate_transform(Object *ob, float lloc[3], float lrot[3][3], float lscale[3][3])
3226 {
3227   BodyPoint *bp;
3228   ReferenceVert *rp;
3229   SoftBody *sb = NULL;
3230   float(*opos)[3];
3231   float(*rpos)[3];
3232   float com[3], rcom[3];
3233   int a;
3234
3235   if (!ob || !ob->soft) {
3236     return; /* why did we get here ? */
3237   }
3238   sb = ob->soft;
3239   if (!sb || !sb->bpoint) {
3240     return;
3241   }
3242   opos = MEM_callocN((sb->totpoint) * 3 * sizeof(float), "SB_OPOS");
3243   rpos = MEM_callocN((sb->totpoint) * 3 * sizeof(float), "SB_RPOS");
3244   /* might filter vertex selection with a vertex group */
3245   for (a = 0, bp = sb->bpoint, rp = sb->scratch->Ref.ivert; a < sb->totpoint; a++, bp++, rp++) {
3246     copy_v3_v3(rpos[a], rp->pos);
3247     copy_v3_v3(opos[a], bp->pos);
3248   }
3249
3250   vcloud_estimate_transform_v3(sb->totpoint, opos, NULL, rpos, NULL, com, rcom, lrot, lscale);
3251   //sub_v3_v3(com, rcom);
3252   if (lloc) {
3253     copy_v3_v3(lloc, com);
3254   }
3255   copy_v3_v3(sb->lcom, com);
3256   if (lscale) {
3257     copy_m3_m3(sb->lscale, lscale);
3258   }
3259   if (lrot) {
3260     copy_m3_m3(sb->lrot, lrot);
3261   }
3262
3263   MEM_freeN(opos);
3264   MEM_freeN(rpos);
3265 }
3266
3267 static void softbody_reset(Object *ob, SoftBody *sb, float (*vertexCos)[3], int numVerts)
3268 {
3269   BodyPoint *bp;
3270   int a;
3271
3272   for (a = 0, bp = sb->bpoint; a < numVerts; a++, bp++) {
3273     copy_v3_v3(bp->pos, vertexCos[a]);
3274     mul_m4_v3(ob->obmat, bp->pos); /* yep, sofbody is global coords*/
3275     copy_v3_v3(bp->origS, bp->pos);
3276     copy_v3_v3(bp->origE, bp->pos);
3277     copy_v3_v3(bp->origT, bp->pos);
3278     bp->vec[0] = bp->vec[1] = bp->vec[2] = 0.0f;
3279
3280     /* the bp->prev*'s are for rolling back from a canceled try to propagate in time
3281      * adaptive step size algo in a nutshell:
3282      * 1.  set scheduled time step to new dtime
3283      * 2.  try to advance the scheduled time step, being optimistic execute it
3284      * 3.  check for success
3285      * 3.a we 're fine continue, may be we can increase scheduled time again ?? if so, do so!
3286      * 3.b we did exceed error limit --> roll back, shorten the scheduled time and try again at 2.
3287      * 4.  check if we did reach dtime
3288      * 4.a nope we need to do some more at 2.
3289      * 4.b yup we're done
3290      */
3291
3292     copy_v3_v3(bp->prevpos, bp->pos);
3293     copy_v3_v3(bp->prevvec, bp->vec);
3294     copy_v3_v3(bp->prevdx, bp->vec);
3295     copy_v3_v3(bp->prevdv, bp->vec);
3296   }
3297
3298   /* make a nice clean scratch struc */
3299   free_scratch(sb);   /* clear if any */
3300   sb_new_scratch(sb); /* make a new */
3301   sb->scratch->needstobuildcollider = 1;
3302   zero_v3(sb->lcom);
3303   unit_m3(sb->lrot);
3304   unit_m3(sb->lscale);
3305
3306   /* copy some info to scratch */
3307   /* we only need that if we want to reconstruct IPO */
3308   if (1) {
3309     reference_to_scratch(ob);
3310     SB_estimate_transform(ob, NULL, NULL, NULL);
3311     SB_estimate_transform(ob, NULL, NULL, NULL);
3312   }
3313   switch (ob->type) {
3314     case OB_MESH:
3315       if (ob->softflag & OB_SB_FACECOLL) {
3316         mesh_faces_to_scratch(ob);
3317       }
3318       break;
3319     case OB_LATTICE:
3320       break;
3321     case OB_CURVE:
3322     case OB_SURF:
3323       break;
3324     default:
3325       break;
3326   }
3327 }
3328
3329 static void softbody_step(
3330     struct Depsgraph *depsgraph, Scene *scene, Object *ob, SoftBody *sb, float dtime)
3331 {
3332   /* the simulator */
3333   float forcetime;
3334   double sct, sst;
3335
3336   sst = PIL_check_seconds_timer();
3337   /* Integration back in time is possible in theory, but pretty useless here.
3338    * So we refuse to do so. Since we do not know anything about 'outside' changes
3339    * especially colliders we refuse to go more than 10 frames.
3340    */
3341   if (dtime < 0 || dtime > 10.5f) {
3342     return;
3343   }
3344
3345   ccd_update_deflector_hash(depsgraph, sb->collision_group, ob, sb->scratch->colliderhash);
3346
3347   if (sb->scratch->needstobuildcollider) {
3348     ccd_build_deflector_hash(depsgraph, sb->collision_group, ob, sb->scratch->colliderhash);
3349     sb->scratch->needstobuildcollider = 0;
3350   }
3351
3352   if (sb->solver_ID < 2) {
3353     /* special case of 2nd order Runge-Kutta type AKA Heun */
3354     int mid_flags = 0;
3355     float err = 0;
3356     /* Set defaults guess we shall do one frame */
3357     float forcetimemax = 1.0f;
3358     /* Set defaults guess 1/100 is tight enough */
3359     float forcetimemin = 0.01f;
3360     /* How far did we get without violating error condition. */
3361     float timedone = 0.0;
3362     /* Loops = counter for emergency brake we don't want to lock up the system if physics fail. */
3363     int loops = 0;
3364
3365     SoftHeunTol = sb->rklimit; /* humm .. this should be calculated from sb parameters and sizes */
3366     /* adjust loop limits */
3367     if (sb->minloops > 0) {
3368       forcetimemax = dtime / sb->minloops;
3369     }
3370     if (sb->maxloops > 0) {
3371       forcetimemin = dtime / sb->maxloops;
3372     }
3373
3374     if (sb->solver_ID > 0) {
3375       mid_flags |= MID_PRESERVE;
3376     }
3377
3378     forcetime = forcetimemax; /* hope for integrating in one step */
3379     while ((ABS(timedone) < ABS(dtime)) && (loops < 2000)) {
3380       /* set goals in time */
3381       interpolate_exciter(ob, 200, (int)(200.0f * (timedone / dtime)));
3382
3383       sb->scratch->flag &= ~SBF_DOFUZZY;
3384       /* do predictive euler step */
3385       softbody_calc_forces(depsgraph, scene, ob, forcetime, timedone / dtime);
3386
3387       softbody_apply_forces(ob, forcetime, 1, NULL, mid_flags);
3388
3389       /* crop new slope values to do averaged slope step */
3390       softbody_calc_forces(depsgraph, scene, ob, forcetime, timedone / dtime);
3391
3392       softbody_apply_forces(ob, forcetime, 2, &err, mid_flags);
3393       softbody_apply_goalsnap(ob);
3394
3395       if (err > SoftHeunTol) { /* error needs to be scaled to some quantity */
3396
3397         if (forcetime > forcetimemin) {
3398           forcetime = max_ff(forcetime / 2.0f, forcetimemin);
3399           softbody_restore_prev_step(ob);
3400           //printf("down, ");
3401         }
3402         else {
3403           timedone += forcetime;
3404         }
3405       }
3406       else {
3407         float newtime = forcetime * 1.1f; /* hope for 1.1 times better conditions in next step */
3408
3409         if (sb->scratch->flag & SBF_DOFUZZY) {
3410           //if (err > SoftHeunTol/(2.0f*sb->fuzzyness)) { /* stay with this stepsize unless err really small */
3411           newtime = forcetime;
3412           //}
3413         }
3414         else {
3415           if (err > SoftHeunTol / 2.0f) { /* stay with this stepsize unless err really small */
3416             newtime = forcetime;
3417           }
3418         }
3419         timedone += forcetime;
3420         newtime = min_ff(forcetimemax, max_ff(newtime, forcetimemin));
3421         //if (newtime > forcetime) printf("up, ");
3422         if (forcetime > 0.0f) {
3423           forcetime = min_ff(dtime - timedone, newtime);
3424         }
3425         else {
3426           forcetime = max_ff(dtime - timedone, newtime);
3427         }
3428       }
3429       loops++;
3430       if (sb->solverflags & SBSO_MONITOR) {
3431         sct = PIL_check_seconds_timer();
3432         if (sct - sst > 0.5) {
3433           printf("%3.0f%% \r", 100.0f * timedone / dtime);
3434         }
3435       }
3436       /* ask for user break */
3437       if (SB_localInterruptCallBack && SB_localInterruptCallBack()) {
3438         break;
3439       }
3440     }
3441     /* move snapped to final position */
3442     interpolate_exciter(ob, 2, 2);
3443     softbody_apply_goalsnap(ob);
3444
3445     //              if (G.debug & G_DEBUG) {
3446     if (sb->solverflags & SBSO_MONITOR) {
3447       if (loops > HEUNWARNLIMIT) { /* monitor high loop counts */
3448         printf("\r needed %d steps/frame", loops);
3449       }
3450     }
3451   }
3452   else if (sb->solver_ID == 2) {
3453     /* do semi "fake" implicit euler */
3454     //removed
3455   } /*SOLVER SELECT*/
3456   else if (sb->solver_ID == 4) {
3457     /* do semi "fake" implicit euler */
3458   } /*SOLVER SELECT*/
3459   else if (sb->solver_ID == 3) {
3460     /* do "stupid" semi "fake" implicit euler */
3461     //removed
3462
3463   } /*SOLVER SELECT*/
3464   else {
3465     CLOG_ERROR(&LOG, "softbody no valid solver ID!");
3466   } /*SOLVER SELECT*/
3467   if (sb->plastic) {
3468     apply_spring_memory(ob);
3469   }
3470
3471   if (sb->solverflags & SBSO_MONITOR) {
3472     sct = PIL_check_seconds_timer();
3473     if ((sct - sst > 0.5) || (G.debug & G_DEBUG)) {
3474       printf(" solver time %f sec %s\n", sct - sst, ob->id.name);
3475     }
3476   }
3477 }
3478
3479 static void sbStoreLastFrame(struct Depsgraph *depsgraph, Object *object, float framenr)
3480 {
3481   if (!DEG_is_active(depsgraph)) {
3482     return;
3483   }
3484   Object *object_orig = DEG_get_original_object(object);
3485   object->soft->last_frame = framenr;
3486   object_orig->soft->last_frame = framenr;
3487 }
3488
3489 /* simulates one step. framenr is in frames */
3490 void sbObjectStep(struct Depsgraph *depsgraph,
3491                   Scene *scene,
3492                   Object *ob,
3493                   float cfra,
3494                   float (*vertexCos)[3],
3495                   int numVerts)
3496 {
3497   SoftBody *sb = ob->soft;
3498   PointCache *cache;
3499   PTCacheID pid;
3500   float dtime, timescale;
3501   int framedelta, framenr, startframe, endframe;
3502   int cache_result;
3503   cache = sb->shared->pointcache;
3504
3505   framenr = (int)cfra;
3506   framedelta = framenr - cache->simframe;
3507
3508   BKE_ptcache_id_from_softbody(&pid, ob, sb);
3509   BKE_ptcache_id_time(&pid, scene, framenr, &startframe, &endframe, &timescale);
3510
3511   /* check for changes in mesh, should only happen in case the mesh
3512    * structure changes during an animation */
3513   if (sb->bpoint && numVerts != sb->totpoint) {
3514     BKE_ptcache_invalidate(cache);
3515     return;
3516   }
3517
3518   /* clamp frame ranges */
3519   if (framenr < startframe) {
3520     BKE_ptcache_invalidate(cache);
3521     return;
3522   }
3523   else if (framenr > endframe) {
3524     framenr = endframe;
3525   }
3526
3527   /* verify if we need to create the softbody data */
3528   if (sb->bpoint == NULL ||
3529       ((ob->softflag & OB_SB_EDGES) && !ob->soft->bspring && object_has_edges(ob))) {
3530
3531     switch (ob->type) {
3532       case OB_MESH:
3533         mesh_to_softbody(scene, ob);
3534         break;
3535       case OB_LATTICE:
3536         lattice_to_softbody(scene, ob);
3537         break;
3538       case OB_CURVE:
3539       case OB_SURF:
3540         curve_surf_to_softbody(scene, ob);
3541         break;
3542       default:
3543         renew_softbody(scene, ob, numVerts, 0);
3544         break;
3545     }
3546
3547     softbody_update_positions(ob, sb, vertexCos, numVerts);
3548     softbody_reset(ob, sb, vertexCos, numVerts);
3549   }
3550
3551   /* still no points? go away */
3552   if (sb->totpoint == 0) {
3553     return;
3554   }
3555   if (framenr == startframe) {
3556     BKE_ptcache_id_reset(scene, &pid, PTCACHE_RESET_OUTDATED);
3557
3558     /* first frame, no simulation to do, just set the positions */
3559     softbody_update_positions(ob, sb, vertexCos, numVerts);
3560
3561     BKE_ptcache_validate(cache, framenr);
3562     cache->flag &= ~PTCACHE_REDO_NEEDED;
3563
3564     sbStoreLastFrame(depsgraph, ob, framenr);
3565
3566     return;
3567   }
3568
3569   /* try to read from cache */
3570   bool can_write_cache = DEG_is_active(depsgraph);
3571   bool can_simulate = (framenr == sb->last_frame + 1) && !(cache->flag & PTCACHE_BAKED) &&
3572                       can_write_cache;
3573
3574   cache_result = BKE_ptcache_read(&pid, (float)framenr + scene->r.subframe, can_simulate);
3575
3576   if (cache_result == PTCACHE_READ_EXACT || cache_result == PTCACHE_READ_INTERPOLATED ||
3577       (!can_simulate && cache_result == PTCACHE_READ_OLD)) {
3578     softbody_to_object(ob, vertexCos, numVerts, sb->local);
3579
3580     BKE_ptcache_validate(cache, framenr);
3581