03fbdc24ce41f8648e26f9c5746e4b50d6afa1b2
[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 /* humm .. this should be calculated from sb parameters and sizes. */
159 static float SoftHeunTol = 1.0f;
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 ..
194      * looking for that (BM). */
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] checks 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 exponentially 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 very long 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
1532   /* wild guess .. may increase with better thread management 'above'
1533    * or even be UI option sb->spawn_cf_threads_nopts */
1534   int lowsprings = 100;
1535
1536   ListBase *effectors = BKE_effectors_create(depsgraph, ob, NULL, ob->soft->effector_weights);
1537
1538   /* figure the number of threads while preventing pretty pointless threading overhead */
1539   totthread = BKE_scene_num_threads(scene);
1540   /* what if we got zillions of CPUs running but less to spread*/
1541   while ((totsprings / totthread < lowsprings) && (totthread > 1)) {
1542     totthread--;
1543   }
1544
1545   sb_threads = MEM_callocN(sizeof(SB_thread_context) * totthread, "SBSpringsThread");
1546   memset(sb_threads, 0, sizeof(SB_thread_context) * totthread);
1547   left = totsprings;
1548   dec = totsprings / totthread + 1;
1549   for (i = 0; i < totthread; i++) {
1550     sb_threads[i].scene = scene;
1551     sb_threads[i].ob = ob;
1552     sb_threads[i].forcetime = 0.0;  // not used here
1553     sb_threads[i].timenow = timenow;
1554     sb_threads[i].ilast = left;
1555     left = left - dec;
1556     if (left > 0) {
1557       sb_threads[i].ifirst = left;
1558     }
1559     else {
1560       sb_threads[i].ifirst = 0;
1561     }
1562     sb_threads[i].effectors = effectors;
1563     sb_threads[i].do_deflector = false;  // not used here
1564     sb_threads[i].fieldfactor = 0.0f;    // not used here
1565     sb_threads[i].windfactor = 0.0f;     // not used here
1566     sb_threads[i].nr = i;
1567     sb_threads[i].tot = totthread;
1568   }
1569   if (totthread > 1) {
1570     BLI_threadpool_init(&threads, exec_scan_for_ext_spring_forces, totthread);
1571
1572     for (i = 0; i < totthread; i++) {
1573       BLI_threadpool_insert(&threads, &sb_threads[i]);
1574     }
1575
1576     BLI_threadpool_end(&threads);
1577   }
1578   else {
1579     exec_scan_for_ext_spring_forces(&sb_threads[0]);
1580   }
1581   /* clean up */
1582   MEM_freeN(sb_threads);
1583
1584   BKE_effectors_free(effectors);
1585 }
1586
1587 /* --- the spring external section*/
1588
1589 static int choose_winner(
1590     float *w, float *pos, float *a, float *b, float *c, float *ca, float *cb, float *cc)
1591 {
1592   float mindist, cp;
1593   int winner = 1;
1594   mindist = fabsf(dot_v3v3(pos, a));
1595
1596   cp = fabsf(dot_v3v3(pos, b));
1597   if (mindist < cp) {
1598     mindist = cp;
1599     winner = 2;
1600   }
1601
1602   cp = fabsf(dot_v3v3(pos, c));
1603   if (mindist < cp) {
1604     mindist = cp;
1605     winner = 3;
1606   }
1607   switch (winner) {
1608     case 1:
1609       copy_v3_v3(w, ca);
1610       break;
1611     case 2:
1612       copy_v3_v3(w, cb);
1613       break;
1614     case 3:
1615       copy_v3_v3(w, cc);
1616   }
1617   return (winner);
1618 }
1619
1620 static int sb_detect_vertex_collisionCached(float opco[3],
1621                                             float facenormal[3],
1622                                             float *damp,
1623                                             float force[3],
1624                                             struct Object *vertexowner,
1625                                             float time,
1626                                             float vel[3],
1627                                             float *intrusion)
1628 {
1629   Object *ob = NULL;
1630   GHash *hash;
1631   GHashIterator *ihash;
1632   float nv1[3], nv2[3], nv3[3], edge1[3], edge2[3], d_nvect[3], dv1[3], ve[3],
1633       avel[3] = {0.0, 0.0, 0.0}, vv1[3], vv2[3], vv3[3], coledge[3] = {0.0f, 0.0f, 0.0f},
1634       mindistedge = 1000.0f, outerforceaccu[3], innerforceaccu[3], facedist,
1635       /* n_mag, */ /* UNUSED */ force_mag_norm, minx, miny, minz, maxx, maxy, maxz,
1636       innerfacethickness = -0.5f, outerfacethickness = 0.2f, ee = 5.0f, ff = 0.1f, fa = 1;
1637   int a, deflected = 0, cavel = 0, ci = 0;
1638   /* init */
1639   *intrusion = 0.0f;
1640   hash = vertexowner->soft->scratch->colliderhash;
1641   ihash = BLI_ghashIterator_new(hash);
1642   outerforceaccu[0] = outerforceaccu[1] = outerforceaccu[2] = 0.0f;
1643   innerforceaccu[0] = innerforceaccu[1] = innerforceaccu[2] = 0.0f;
1644   /* go */
1645   while (!BLI_ghashIterator_done(ihash)) {
1646
1647     ccd_Mesh *ccdm = BLI_ghashIterator_getValue(ihash);
1648     ob = BLI_ghashIterator_getKey(ihash);
1649     {
1650       /* only with deflecting set */
1651       if (ob->pd && ob->pd->deflect) {
1652         const MVert *mvert = NULL;
1653         const MVert *mprevvert = NULL;
1654         const MVertTri *vt = NULL;
1655         const ccdf_minmax *mima = NULL;
1656
1657         if (ccdm) {
1658           mvert = ccdm->mvert;
1659           mprevvert = ccdm->mprevvert;
1660           vt = ccdm->tri;
1661           mima = ccdm->mima;
1662           a = ccdm->tri_num;
1663
1664           minx = ccdm->bbmin[0];
1665           miny = ccdm->bbmin[1];
1666           minz = ccdm->bbmin[2];
1667
1668           maxx = ccdm->bbmax[0];
1669           maxy = ccdm->bbmax[1];
1670           maxz = ccdm->bbmax[2];
1671
1672           if ((opco[0] < minx) || (opco[1] < miny) || (opco[2] < minz) || (opco[0] > maxx) ||
1673               (opco[1] > maxy) || (opco[2] > maxz)) {
1674             /* outside the padded boundbox --> collision object is too far away */
1675             BLI_ghashIterator_step(ihash);
1676             continue;
1677           }
1678         }
1679         else {
1680           /*aye that should be cached*/
1681           CLOG_ERROR(&LOG, "missing cache error");
1682           BLI_ghashIterator_step(ihash);
1683           continue;
1684         }
1685
1686         /* do object level stuff */
1687         /* need to have user control for that since it depends on model scale */
1688         innerfacethickness = -ob->pd->pdef_sbift;
1689         outerfacethickness = ob->pd->pdef_sboft;
1690         fa = (ff * outerfacethickness - outerfacethickness);
1691         fa *= fa;
1692         fa = 1.0f / fa;
1693         avel[0] = avel[1] = avel[2] = 0.0f;
1694         /* use mesh*/
1695         while (a--) {
1696           if ((opco[0] < mima->minx) || (opco[0] > mima->maxx) || (opco[1] < mima->miny) ||
1697               (opco[1] > mima->maxy) || (opco[2] < mima->minz) || (opco[2] > mima->maxz)) {
1698             mima++;
1699             vt++;
1700             continue;
1701           }
1702
1703           if (mvert) {
1704
1705             copy_v3_v3(nv1, mvert[vt->tri[0]].co);
1706             copy_v3_v3(nv2, mvert[vt->tri[1]].co);
1707             copy_v3_v3(nv3, mvert[vt->tri[2]].co);
1708
1709             if (mprevvert) {
1710               /* Grab the average speed of the collider vertices before we spoil nvX
1711                * humm could be done once a SB steps but then we' need to store that too
1712                * since the AABB reduced probability to get here drastically
1713                * it might be a nice tradeoff CPU <--> memory.
1714                */
1715               sub_v3_v3v3(vv1, nv1, mprevvert[vt->tri[0]].co);
1716               sub_v3_v3v3(vv2, nv2, mprevvert[vt->tri[1]].co);
1717               sub_v3_v3v3(vv3, nv3, mprevvert[vt->tri[2]].co);
1718
1719               mul_v3_fl(nv1, time);
1720               madd_v3_v3fl(nv1, mprevvert[vt->tri[0]].co, 1.0f - time);
1721
1722               mul_v3_fl(nv2, time);
1723               madd_v3_v3fl(nv2, mprevvert[vt->tri[1]].co, 1.0f - time);
1724
1725               mul_v3_fl(nv3, time);
1726               madd_v3_v3fl(nv3, mprevvert[vt->tri[2]].co, 1.0f - time);
1727             }
1728           }
1729
1730           /* switch origin to be nv2*/
1731           sub_v3_v3v3(edge1, nv1, nv2);
1732           sub_v3_v3v3(edge2, nv3, nv2);
1733           sub_v3_v3v3(
1734               dv1, opco, nv2); /* abuse dv1 to have vertex in question at *origin* of triangle */
1735
1736           cross_v3_v3v3(d_nvect, edge2, edge1);
1737           /* n_mag = */ /* UNUSED */ normalize_v3(d_nvect);
1738           facedist = dot_v3v3(dv1, d_nvect);
1739           // so rules are
1740           //
1741
1742           if ((facedist > innerfacethickness) && (facedist < outerfacethickness)) {
1743             if (isect_point_tri_prism_v3(opco, nv1, nv2, nv3)) {
1744               force_mag_norm = (float)exp(-ee * facedist);
1745               if (facedist > outerfacethickness * ff) {
1746                 force_mag_norm = (float)force_mag_norm * fa * (facedist - outerfacethickness) *
1747                                  (facedist - outerfacethickness);
1748               }
1749               *damp = ob->pd->pdef_sbdamp;
1750               if (facedist > 0.0f) {
1751                 *damp *= (1.0f - facedist / outerfacethickness);
1752                 madd_v3_v3fl(outerforceaccu, d_nvect, force_mag_norm);
1753                 deflected = 3;
1754               }
1755               else {
1756                 madd_v3_v3fl(innerforceaccu, d_nvect, force_mag_norm);
1757                 if (deflected < 2) {
1758                   deflected = 2;
1759                 }
1760               }
1761               if ((mprevvert) && (*damp > 0.0f)) {
1762                 choose_winner(ve, opco, nv1, nv2, nv3, vv1, vv2, vv3);
1763                 add_v3_v3(avel, ve);
1764                 cavel++;
1765               }
1766               *intrusion += facedist;
1767               ci++;
1768             }
1769           }
1770
1771           mima++;
1772           vt++;
1773         } /* while a */
1774       }   /* if (ob->pd && ob->pd->deflect) */
1775       BLI_ghashIterator_step(ihash);
1776     }
1777   } /* while () */
1778
1779   if (deflected == 1) {  // no face but 'outer' edge cylinder sees vert
1780     force_mag_norm = (float)exp(-ee * mindistedge);
1781     if (mindistedge > outerfacethickness * ff) {
1782       force_mag_norm = (float)force_mag_norm * fa * (mindistedge - outerfacethickness) *
1783                        (mindistedge - outerfacethickness);
1784     }
1785     madd_v3_v3fl(force, coledge, force_mag_norm);
1786     *damp = ob->pd->pdef_sbdamp;
1787     if (mindistedge > 0.0f) {
1788       *damp *= (1.0f - mindistedge / outerfacethickness);
1789     }
1790   }
1791   if (deflected == 2) {  //  face inner detected
1792     add_v3_v3(force, innerforceaccu);
1793   }
1794   if (deflected == 3) {  //  face outer detected
1795     add_v3_v3(force, outerforceaccu);
1796   }
1797
1798   BLI_ghashIterator_free(ihash);
1799   if (cavel) {
1800     mul_v3_fl(avel, 1.0f / (float)cavel);
1801   }
1802   copy_v3_v3(vel, avel);
1803   if (ci) {
1804     *intrusion /= ci;
1805   }
1806   if (deflected) {
1807     normalize_v3_v3(facenormal, force);
1808   }
1809   return deflected;
1810 }
1811
1812 /* sandbox to plug in various deflection algos */
1813 static int sb_deflect_face(Object *ob,
1814                            float *actpos,
1815                            float *facenormal,
1816                            float *force,
1817                            float *cf,
1818                            float time,
1819                            float *vel,
1820                            float *intrusion)
1821 {
1822   float s_actpos[3];
1823   int deflected;
1824   copy_v3_v3(s_actpos, actpos);
1825   deflected = sb_detect_vertex_collisionCached(
1826       s_actpos, facenormal, cf, force, ob, time, vel, intrusion);
1827 #if 0
1828   deflected = sb_detect_vertex_collisionCachedEx(
1829       s_actpos, facenormal, cf, force, ob, time, vel, intrusion);
1830 #endif
1831   return (deflected);
1832 }
1833
1834 /* hiding this for now .. but the jacobian may pop up on other tasks .. so i'd like to keep it */
1835 #if 0
1836 static void dfdx_spring(int ia, int ic, int op, float dir[3], float L, float len, float factor)
1837 {
1838   float m, delta_ij;
1839   int i, j;
1840   if (L < len) {
1841     for (i = 0; i < 3; i++) {
1842       for (j = 0; j < 3; j++) {
1843         delta_ij = (i == j ? (1.0f) : (0.0f));
1844         m = factor * (dir[i] * dir[j] + (1 - L / len) * (delta_ij - dir[i] * dir[j]));
1845         EIG_linear_solver_matrix_add(ia + i, op + ic + j, m);
1846       }
1847     }
1848   }
1849   else {
1850     for (i = 0; i < 3; i++) {
1851       for (j = 0; j < 3; j++) {
1852         m = factor * dir[i] * dir[j];
1853         EIG_linear_solver_matrix_add(ia + i, op + ic + j, m);
1854       }
1855     }
1856   }
1857 }
1858
1859 static void dfdx_goal(int ia, int ic, int op, float factor)
1860 {
1861   int i;
1862   for (i = 0; i < 3; i++) {
1863     EIG_linear_solver_matrix_add(ia + i, op + ic + i, factor);
1864   }
1865 }
1866
1867 static void dfdv_goal(int ia, int ic, float factor)
1868 {
1869   int i;
1870   for (i = 0; i < 3; i++) {
1871     EIG_linear_solver_matrix_add(ia + i, ic + i, factor);
1872   }
1873 }
1874 #endif /* if 0 */
1875
1876 static void sb_spring_force(
1877     Object *ob, int bpi, BodySpring *bs, float iks, float UNUSED(forcetime))
1878 {
1879   SoftBody *sb = ob->soft; /* is supposed to be there */
1880   BodyPoint *bp1, *bp2;
1881
1882   float dir[3], dvel[3];
1883   float distance, forcefactor, kd, absvel, projvel, kw;
1884 #if 0 /* UNUSED */
1885   int ia, ic;
1886 #endif
1887   /* prepare depending on which side of the spring we are on */
1888   if (bpi == bs->v1) {
1889     bp1 = &sb->bpoint[bs->v1];
1890     bp2 = &sb->bpoint[bs->v2];
1891 #if 0 /* UNUSED */
1892     ia = 3 * bs->v1;
1893     ic = 3 * bs->v2;
1894 #endif
1895   }
1896   else if (bpi == bs->v2) {
1897     bp1 = &sb->bpoint[bs->v2];
1898     bp2 = &sb->bpoint[bs->v1];
1899 #if 0 /* UNUSED */
1900     ia = 3 * bs->v2;
1901     ic = 3 * bs->v1;
1902 #endif
1903   }
1904   else {
1905     /* TODO make this debug option */
1906     CLOG_WARN(&LOG, "bodypoint <bpi> is not attached to spring  <*bs>");
1907     return;
1908   }
1909
1910   /* do bp1 <--> bp2 elastic */
1911   sub_v3_v3v3(dir, bp1->pos, bp2->pos);
1912   distance = normalize_v3(dir);
1913   if (bs->len < distance) {
1914     iks = 1.0f / (1.0f - sb->inspring) - 1.0f; /* inner spring constants function */
1915   }
1916   else {
1917     iks = 1.0f / (1.0f - sb->inpush) - 1.0f; /* inner spring constants function */
1918   }
1919
1920   if (bs->len > 0.0f) { /* check for degenerated springs */
1921     forcefactor = iks / bs->len;
1922   }
1923   else {
1924     forcefactor = iks;
1925   }
1926   kw = (bp1->springweight + bp2->springweight) / 2.0f;
1927   kw = kw * kw;
1928   kw = kw * kw;
1929   switch (bs->springtype) {
1930     case SB_EDGE:
1931     case SB_HANDLE:
1932       forcefactor *= kw;
1933       break;
1934     case SB_BEND:
1935       forcefactor *= sb->secondspring * kw;
1936       break;
1937     case SB_STIFFQUAD:
1938       forcefactor *= sb->shearstiff * sb->shearstiff * kw;
1939       break;
1940     default:
1941       break;
1942   }
1943
1944   madd_v3_v3fl(bp1->force, dir, (bs->len - distance) * forcefactor);
1945
1946   /* do bp1 <--> bp2 viscous */
1947   sub_v3_v3v3(dvel, bp1->vec, bp2->vec);
1948   kd = sb->infrict * sb_fric_force_scale(ob);
1949   absvel = normalize_v3(dvel);
1950   projvel = dot_v3v3(dir, dvel);
1951   kd *= absvel * projvel;
1952   madd_v3_v3fl(bp1->force, dir, -kd);
1953 }
1954
1955 /* since this is definitely the most CPU consuming task here .. try to spread it */
1956 /* core function _softbody_calc_forces_slice_in_a_thread */
1957 /* result is int to be able to flag user break */
1958 static int _softbody_calc_forces_slice_in_a_thread(Scene *scene,
1959                                                    Object *ob,
1960                                                    float forcetime,
1961                                                    float timenow,
1962                                                    int ifirst,
1963                                                    int ilast,
1964                                                    int *UNUSED(ptr_to_break_func(void)),
1965                                                    ListBase *effectors,
1966                                                    int do_deflector,
1967                                                    float fieldfactor,
1968                                                    float windfactor)
1969 {
1970   float iks;
1971   int bb, do_selfcollision, do_springcollision, do_aero;
1972   int number_of_points_here = ilast - ifirst;
1973   SoftBody *sb = ob->soft; /* is supposed to be there */
1974   BodyPoint *bp;
1975
1976   /* initialize */
1977   if (sb) {
1978     /* check conditions for various options */
1979     /* +++ could be done on object level to squeeze out the last bits of it */
1980     do_selfcollision = ((ob->softflag & OB_SB_EDGES) && (sb->bspring) &&
1981                         (ob->softflag & OB_SB_SELF));
1982     do_springcollision = do_deflector && (ob->softflag & OB_SB_EDGES) &&
1983                          (ob->softflag & OB_SB_EDGECOLL);
1984     do_aero = ((sb->aeroedge) && (ob->softflag & OB_SB_EDGES));
1985     /* --- could be done on object level to squeeze out the last bits of it */
1986   }
1987   else {
1988     CLOG_ERROR(&LOG, "expected a SB here");
1989     return (999);
1990   }
1991
1992   /* debugerin */
1993   if (sb->totpoint < ifirst) {
1994     printf("Aye 998");
1995     return (998);
1996   }
1997   /* debugerin */
1998
1999   bp = &sb->bpoint[ifirst];
2000   for (bb = number_of_points_here; bb > 0; bb--, bp++) {
2001     /* clear forces  accumulator */
2002     bp->force[0] = bp->force[1] = bp->force[2] = 0.0;
2003     /* naive ball self collision */
2004     /* needs to be done if goal snaps or not */
2005     if (do_selfcollision) {
2006       int attached;
2007       BodyPoint *obp;
2008       BodySpring *bs;
2009       int c, b;
2010       float velcenter[3], dvel[3], def[3];
2011       float distance;
2012       float compare;
2013       float bstune = sb->ballstiff;
2014
2015       /* Running in a slice we must not assume anything done with obp
2016        * neither alter the data of obp. */
2017       for (c = sb->totpoint, obp = sb->bpoint; c > 0; c--, obp++) {
2018         compare = (obp->colball + bp->colball);
2019         sub_v3_v3v3(def, bp->pos, obp->pos);
2020         /* rather check the AABBoxes before ever calculating the real distance */
2021         /* mathematically it is completely nuts, but performance is pretty much (3) times faster */
2022         if ((ABS(def[0]) > compare) || (ABS(def[1]) > compare) || (ABS(def[2]) > compare)) {
2023           continue;
2024         }
2025         distance = normalize_v3(def);
2026         if (distance < compare) {
2027           /* exclude body points attached with a spring */
2028           attached = 0;
2029           for (b = obp->nofsprings; b > 0; b--) {
2030             bs = sb->bspring + obp->springs[b - 1];
2031             if ((ilast - bb == bs->v2) || (ilast - bb == bs->v1)) {
2032               attached = 1;
2033               continue;
2034             }
2035           }
2036           if (!attached) {
2037             float f = bstune / (distance) + bstune / (compare * compare) * distance -
2038                       2.0f * bstune / compare;
2039
2040             mid_v3_v3v3(velcenter, bp->vec, obp->vec);
2041             sub_v3_v3v3(dvel, velcenter, bp->vec);
2042             mul_v3_fl(dvel, _final_mass(ob, bp));
2043
2044             madd_v3_v3fl(bp->force, def, f * (1.0f - sb->balldamp));
2045             madd_v3_v3fl(bp->force, dvel, sb->balldamp);
2046           }
2047         }
2048       }
2049     }
2050     /* naive ball self collision done */
2051
2052     if (_final_goal(ob, bp) < SOFTGOALSNAP) { /* omit this bp when it snaps */
2053       float auxvect[3];
2054       float velgoal[3];
2055
2056       /* do goal stuff */
2057       if (ob->softflag & OB_SB_GOAL) {
2058         /* true elastic goal */
2059         float ks, kd;
2060         sub_v3_v3v3(auxvect, bp->pos, bp->origT);
2061         ks = 1.0f / (1.0f - _final_goal(ob, bp) * sb->goalspring) - 1.0f;
2062         bp->force[0] += -ks * (auxvect[0]);
2063         bp->force[1] += -ks * (auxvect[1]);
2064         bp->force[2] += -ks * (auxvect[2]);
2065
2066         /* calculate damping forces generated by goals*/
2067         sub_v3_v3v3(velgoal, bp->origS, bp->origE);
2068         kd = sb->goalfrict * sb_fric_force_scale(ob);
2069         add_v3_v3v3(auxvect, velgoal, bp->vec);
2070
2071         if (forcetime >
2072             0.0f) { /* make sure friction does not become rocket motor on time reversal */
2073           bp->force[0] -= kd * (auxvect[0]);
2074           bp->force[1] -= kd * (auxvect[1]);
2075           bp->force[2] -= kd * (auxvect[2]);
2076         }
2077         else {
2078           bp->force[0] -= kd * (velgoal[0] - bp->vec[0]);
2079           bp->force[1] -= kd * (velgoal[1] - bp->vec[1]);
2080           bp->force[2] -= kd * (velgoal[2] - bp->vec[2]);
2081         }
2082       }
2083       /* done goal stuff */
2084
2085       /* gravitation */
2086       if (scene->physics_settings.flag & PHYS_GLOBAL_GRAVITY) {
2087         float gravity[3];
2088         copy_v3_v3(gravity, scene->physics_settings.gravity);
2089         mul_v3_fl(gravity,
2090                   sb_grav_force_scale(ob) * _final_mass(ob, bp) *
2091                       sb->effector_weights->global_gravity); /* individual mass of node here */
2092         add_v3_v3(bp->force, gravity);
2093       }
2094
2095       /* particle field & vortex */
2096       if (effectors) {
2097         EffectedPoint epoint;
2098         float kd;
2099         float force[3] = {0.0f, 0.0f, 0.0f};
2100         float speed[3] = {0.0f, 0.0f, 0.0f};
2101         float eval_sb_fric_force_scale = sb_fric_force_scale(
2102             ob); /* just for calling function once */
2103         pd_point_from_soft(scene, bp->pos, bp->vec, sb->bpoint - bp, &epoint);
2104         BKE_effectors_apply(effectors, NULL, sb->effector_weights, &epoint, force, speed);
2105
2106         /* apply forcefield*/
2107         mul_v3_fl(force, fieldfactor * eval_sb_fric_force_scale);
2108         add_v3_v3(bp->force, force);
2109
2110         /* BP friction in moving media */
2111         kd = sb->mediafrict * eval_sb_fric_force_scale;
2112         bp->force[0] -= kd * (bp->vec[0] + windfactor * speed[0] / eval_sb_fric_force_scale);
2113         bp->force[1] -= kd * (bp->vec[1] + windfactor * speed[1] / eval_sb_fric_force_scale);
2114         bp->force[2] -= kd * (bp->vec[2] + windfactor * speed[2] / eval_sb_fric_force_scale);
2115         /* now we'll have nice centrifugal effect for vortex */
2116       }
2117       else {
2118         /* BP friction in media (not) moving*/
2119         float kd = sb->mediafrict * sb_fric_force_scale(ob);
2120         /* assume it to be proportional to actual velocity */
2121         bp->force[0] -= bp->vec[0] * kd;
2122         bp->force[1] -= bp->vec[1] * kd;
2123         bp->force[2] -= bp->vec[2] * kd;
2124         /* friction in media done */
2125       }
2126       /* +++cached collision targets */
2127       bp->choke = 0.0f;
2128       bp->choke2 = 0.0f;
2129       bp->loc_flag &= ~SBF_DOFUZZY;
2130       if (do_deflector && !(bp->loc_flag & SBF_OUTOFCOLLISION)) {
2131         float cfforce[3], defforce[3] = {0.0f, 0.0f, 0.0f}, vel[3] = {0.0f, 0.0f, 0.0f},
2132                           facenormal[3], cf = 1.0f, intrusion;
2133         float kd = 1.0f;
2134
2135         if (sb_deflect_face(ob, bp->pos, facenormal, defforce, &cf, timenow, vel, &intrusion)) {
2136           if (intrusion < 0.0f) {
2137             sb->scratch->flag |= SBF_DOFUZZY;
2138             bp->loc_flag |= SBF_DOFUZZY;
2139             bp->choke = sb->choke * 0.01f;
2140           }
2141
2142           sub_v3_v3v3(cfforce, bp->vec, vel);
2143           madd_v3_v3fl(bp->force, cfforce, -cf * 50.0f);
2144
2145           madd_v3_v3fl(bp->force, defforce, kd);
2146         }
2147       }
2148       /* ---cached collision targets */
2149
2150       /* +++springs */
2151       iks = 1.0f / (1.0f - sb->inspring) - 1.0f; /* inner spring constants function */
2152       if (ob->softflag & OB_SB_EDGES) {
2153         if (sb->bspring) { /* spring list exists at all ? */
2154           int b;
2155           BodySpring *bs;
2156           for (b = bp->nofsprings; b > 0; b--) {
2157             bs = sb->bspring + bp->springs[b - 1];
2158             if (do_springcollision || do_aero) {
2159               add_v3_v3(bp->force, bs->ext_force);
2160               if (bs->flag & BSF_INTERSECT) {
2161                 bp->choke = bs->cf;
2162               }
2163             }
2164             // sb_spring_force(Object *ob, int bpi, BodySpring *bs, float iks, float forcetime)
2165             sb_spring_force(ob, ilast - bb, bs, iks, forcetime);
2166           } /* loop springs */
2167         }   /* existing spring list */
2168       }     /*any edges*/
2169       /* ---springs */
2170     }       /*omit on snap */
2171   }         /*loop all bp's*/
2172   return 0; /*done fine*/
2173 }
2174
2175 static void *exec_softbody_calc_forces(void *data)
2176 {
2177   SB_thread_context *pctx = (SB_thread_context *)data;
2178   _softbody_calc_forces_slice_in_a_thread(pctx->scene,
2179                                           pctx->ob,
2180                                           pctx->forcetime,
2181                                           pctx->timenow,
2182                                           pctx->ifirst,
2183                                           pctx->ilast,
2184                                           NULL,
2185                                           pctx->effectors,
2186                                           pctx->do_deflector,
2187                                           pctx->fieldfactor,
2188                                           pctx->windfactor);
2189   return NULL;
2190 }
2191
2192 static void sb_cf_threads_run(Scene *scene,
2193                               Object *ob,
2194                               float forcetime,
2195                               float timenow,
2196                               int totpoint,
2197                               int *UNUSED(ptr_to_break_func(void)),
2198                               struct ListBase *effectors,
2199                               int do_deflector,
2200                               float fieldfactor,
2201                               float windfactor)
2202 {
2203   ListBase threads;
2204   SB_thread_context *sb_threads;
2205   int i, totthread, left, dec;
2206
2207   /* wild guess .. may increase with better thread management 'above'
2208    * or even be UI option sb->spawn_cf_threads_nopts. */
2209   int lowpoints = 100;
2210
2211   /* figure the number of threads while preventing pretty pointless threading overhead */
2212   totthread = BKE_scene_num_threads(scene);
2213   /* what if we got zillions of CPUs running but less to spread*/
2214   while ((totpoint / totthread < lowpoints) && (totthread > 1)) {
2215     totthread--;
2216   }
2217
2218   /* printf("sb_cf_threads_run spawning %d threads\n", totthread); */
2219
2220   sb_threads = MEM_callocN(sizeof(SB_thread_context) * totthread, "SBThread");
2221   memset(sb_threads, 0, sizeof(SB_thread_context) * totthread);
2222   left = totpoint;
2223   dec = totpoint / totthread + 1;
2224   for (i = 0; i < totthread; i++) {
2225     sb_threads[i].scene = scene;
2226     sb_threads[i].ob = ob;
2227     sb_threads[i].forcetime = forcetime;
2228     sb_threads[i].timenow = timenow;
2229     sb_threads[i].ilast = left;
2230     left = left - dec;
2231     if (left > 0) {
2232       sb_threads[i].ifirst = left;
2233     }
2234     else {
2235       sb_threads[i].ifirst = 0;
2236     }
2237     sb_threads[i].effectors = effectors;
2238     sb_threads[i].do_deflector = do_deflector;
2239     sb_threads[i].fieldfactor = fieldfactor;
2240     sb_threads[i].windfactor = windfactor;
2241     sb_threads[i].nr = i;
2242     sb_threads[i].tot = totthread;
2243   }
2244
2245   if (totthread > 1) {
2246     BLI_threadpool_init(&threads, exec_softbody_calc_forces, totthread);
2247
2248     for (i = 0; i < totthread; i++) {
2249       BLI_threadpool_insert(&threads, &sb_threads[i]);
2250     }
2251
2252     BLI_threadpool_end(&threads);
2253   }
2254   else {
2255     exec_softbody_calc_forces(&sb_threads[0]);
2256   }
2257   /* clean up */
2258   MEM_freeN(sb_threads);
2259 }
2260
2261 static void softbody_calc_forces(
2262     struct Depsgraph *depsgraph, Scene *scene, Object *ob, float forcetime, float timenow)
2263 {
2264   /* rule we never alter free variables :bp->vec bp->pos in here !
2265    * this will ruin adaptive stepsize AKA heun! (BM)
2266    */
2267   SoftBody *sb = ob->soft; /* is supposed to be there */
2268   /*BodyPoint *bproot;*/   /* UNUSED */
2269   /* float gravity; */     /* UNUSED */
2270   /* float iks; */
2271   float fieldfactor = -1.0f, windfactor = 0.25;
2272   int do_deflector /*, do_selfcollision*/, do_springcollision, do_aero;
2273
2274   /* gravity = sb->grav * sb_grav_force_scale(ob); */ /* UNUSED */
2275
2276   /* check conditions for various options */
2277   do_deflector = query_external_colliders(depsgraph, sb->collision_group);
2278 #if 0
2279   do_selfcollision=((ob->softflag & OB_SB_EDGES) && (sb->bspring)&& (ob->softflag & OB_SB_SELF));
2280 #endif
2281   do_springcollision = do_deflector && (ob->softflag & OB_SB_EDGES) &&
2282                        (ob->softflag & OB_SB_EDGECOLL);
2283   do_aero = ((sb->aeroedge) && (ob->softflag & OB_SB_EDGES));
2284
2285   /* iks  = 1.0f/(1.0f-sb->inspring)-1.0f; */ /* inner spring constants function */ /* UNUSED */
2286   /* bproot= sb->bpoint; */ /* need this for proper spring addressing */            /* UNUSED */
2287
2288   if (do_springcollision || do_aero) {
2289     sb_sfesf_threads_run(depsgraph, scene, ob, timenow, sb->totspring, NULL);
2290   }
2291
2292   /* after spring scan because it uses Effoctors too */
2293   ListBase *effectors = BKE_effectors_create(depsgraph, ob, NULL, sb->effector_weights);
2294
2295   if (do_deflector) {
2296     float defforce[3];
2297     do_deflector = sb_detect_aabb_collisionCached(defforce, ob, timenow);
2298   }
2299
2300   sb_cf_threads_run(scene,
2301                     ob,
2302                     forcetime,
2303                     timenow,
2304                     sb->totpoint,
2305                     NULL,
2306                     effectors,
2307                     do_deflector,
2308                     fieldfactor,
2309                     windfactor);
2310
2311   /* finally add forces caused by face collision */
2312   if (ob->softflag & OB_SB_FACECOLL) {
2313     scan_for_ext_face_forces(ob, timenow);
2314   }
2315
2316   /* finish matrix and solve */
2317   BKE_effectors_free(effectors);
2318 }
2319
2320 static void softbody_apply_forces(Object *ob, float forcetime, int mode, float *err, int mid_flags)
2321 {
2322   /* time evolution */
2323   /* actually does an explicit euler step mode == 0 */
2324   /* or heun ~ 2nd order runge-kutta steps, mode 1, 2 */
2325   SoftBody *sb = ob->soft; /* is supposed to be there */
2326   BodyPoint *bp;
2327   float dx[3] = {0}, dv[3], aabbmin[3], aabbmax[3], cm[3] = {0.0f, 0.0f, 0.0f};
2328   float timeovermass /*, freezeloc=0.00001f, freezeforce=0.00000000001f*/;
2329   float maxerrpos = 0.0f, maxerrvel = 0.0f;
2330   int a, fuzzy = 0;
2331
2332   forcetime *= sb_time_scale(ob);
2333
2334   aabbmin[0] = aabbmin[1] = aabbmin[2] = 1e20f;
2335   aabbmax[0] = aabbmax[1] = aabbmax[2] = -1e20f;
2336
2337   /* old one with homogeneous masses  */
2338   /* claim a minimum mass for vertex */
2339 #if 0
2340   if (sb->nodemass > 0.009999f) {
2341     timeovermass = forcetime / sb->nodemass;
2342   }
2343   else {
2344     timeovermass = forcetime / 0.009999f;
2345   }
2346 #endif
2347
2348   for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
2349     /* Now we have individual masses. */
2350     /* claim a minimum mass for vertex */
2351     if (_final_mass(ob, bp) > 0.009999f) {
2352       timeovermass = forcetime / _final_mass(ob, bp);
2353     }
2354     else {
2355       timeovermass = forcetime / 0.009999f;
2356     }
2357
2358     if (_final_goal(ob, bp) < SOFTGOALSNAP) {
2359       /* this makes t~ = t */
2360       if (mid_flags & MID_PRESERVE) {
2361         copy_v3_v3(dx, bp->vec);
2362       }
2363
2364       /**
2365        * So here is:
2366        * <pre>
2367        * (v)' = a(cceleration) =
2368        *     sum(F_springs)/m + gravitation + some friction forces + more forces.
2369        * </pre>
2370        *
2371        * The ( ... )' operator denotes derivate respective time.
2372        *
2373        * The euler step for velocity then becomes:
2374        * <pre>
2375        * v(t + dt) = v(t) + a(t) * dt
2376        * </pre>
2377        */
2378       mul_v3_fl(bp->force, timeovermass); /* individual mass of node here */
2379       /* some nasty if's to have heun in here too */
2380       copy_v3_v3(dv, bp->force);
2381
2382       if (mode == 1) {
2383         copy_v3_v3(bp->prevvec, bp->vec);
2384         copy_v3_v3(bp->prevdv, dv);
2385       }
2386
2387       if (mode == 2) {
2388         /* be optimistic and execute step */
2389         bp->vec[0] = bp->prevvec[0] + 0.5f * (dv[0] + bp->prevdv[0]);
2390         bp->vec[1] = bp->prevvec[1] + 0.5f * (dv[1] + bp->prevdv[1]);
2391         bp->vec[2] = bp->prevvec[2] + 0.5f * (dv[2] + bp->prevdv[2]);
2392         /* compare euler to heun to estimate error for step sizing */
2393         maxerrvel = max_ff(maxerrvel, fabsf(dv[0] - bp->prevdv[0]));
2394         maxerrvel = max_ff(maxerrvel, fabsf(dv[1] - bp->prevdv[1]));
2395         maxerrvel = max_ff(maxerrvel, fabsf(dv[2] - bp->prevdv[2]));
2396       }
2397       else {
2398         add_v3_v3(bp->vec, bp->force);
2399       }
2400
2401       /* this makes t~ = t+dt */
2402       if (!(mid_flags & MID_PRESERVE)) {
2403         copy_v3_v3(dx, bp->vec);
2404       }
2405
2406       /* so here is (x)'= v(elocity) */
2407       /* the euler step for location then becomes */
2408       /* x(t + dt) = x(t) + v(t~) * dt */
2409       mul_v3_fl(dx, forcetime);
2410
2411       /* the freezer coming sooner or later */
2412 #if 0
2413       if ((dot_v3v3(dx, dx) < freezeloc) && (dot_v3v3(bp->force, bp->force) < freezeforce)) {
2414         bp->frozen /= 2;
2415       }
2416       else {
2417         bp->frozen = min_ff(bp->frozen * 1.05f, 1.0f);
2418       }
2419       mul_v3_fl(dx, bp->frozen);
2420 #endif
2421       /* again some nasty if's to have heun in here too */
2422       if (mode == 1) {
2423         copy_v3_v3(bp->prevpos, bp->pos);
2424         copy_v3_v3(bp->prevdx, dx);
2425       }
2426
2427       if (mode == 2) {
2428         bp->pos[0] = bp->prevpos[0] + 0.5f * (dx[0] + bp->prevdx[0]);
2429         bp->pos[1] = bp->prevpos[1] + 0.5f * (dx[1] + bp->prevdx[1]);
2430         bp->pos[2] = bp->prevpos[2] + 0.5f * (dx[2] + bp->prevdx[2]);
2431         maxerrpos = max_ff(maxerrpos, fabsf(dx[0] - bp->prevdx[0]));
2432         maxerrpos = max_ff(maxerrpos, fabsf(dx[1] - bp->prevdx[1]));
2433         maxerrpos = max_ff(maxerrpos, fabsf(dx[2] - bp->prevdx[2]));
2434
2435         /* bp->choke is set when we need to pull a vertex or edge out of the collider.
2436          * the collider object signals to get out by pushing hard. on the other hand
2437          * we don't want to end up in deep space so we add some <viscosity>
2438          * to balance that out */
2439         if (bp->choke2 > 0.0f) {
2440           mul_v3_fl(bp->vec, (1.0f - bp->choke2));
2441         }
2442         if (bp->choke > 0.0f) {
2443           mul_v3_fl(bp->vec, (1.0f - bp->choke));
2444         }
2445       }
2446       else {
2447         add_v3_v3(bp->pos, dx);
2448       }
2449     } /*snap*/
2450     /* so while we are looping BPs anyway do statistics on the fly */
2451     minmax_v3v3_v3(aabbmin, aabbmax, bp->pos);
2452     if (bp->loc_flag & SBF_DOFUZZY) {
2453       fuzzy = 1;
2454     }
2455   } /*for*/
2456
2457   if (sb->totpoint) {
2458     mul_v3_fl(cm, 1.0f / sb->totpoint);
2459   }
2460   if (sb->scratch) {
2461     copy_v3_v3(sb->scratch->aabbmin, aabbmin);
2462     copy_v3_v3(sb->scratch->aabbmax, aabbmax);
2463   }
2464
2465   if (err) { /* so step size will be controlled by biggest difference in slope */
2466     if (sb->solverflags & SBSO_OLDERR) {
2467       *err = max_ff(maxerrpos, maxerrvel);
2468     }
2469     else {
2470       *err = maxerrpos;
2471     }
2472     // printf("EP %f EV %f\n", maxerrpos, maxerrvel);
2473     if (fuzzy) {
2474       *err /= sb->fuzzyness;
2475     }
2476   }
2477 }
2478
2479 /* used by heun when it overshoots */
2480 static void softbody_restore_prev_step(Object *ob)
2481 {
2482   SoftBody *sb = ob->soft; /* is supposed to be there*/
2483   BodyPoint *bp;
2484   int a;
2485
2486   for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
2487     copy_v3_v3(bp->vec, bp->prevvec);
2488     copy_v3_v3(bp->pos, bp->prevpos);
2489   }
2490 }
2491
2492 #if 0
2493 static void softbody_store_step(Object *ob)
2494 {
2495   SoftBody *sb = ob->soft; /* is supposed to be there*/
2496   BodyPoint *bp;
2497   int a;
2498
2499   for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
2500     copy_v3_v3(bp->prevvec, bp->vec);
2501     copy_v3_v3(bp->prevpos, bp->pos);
2502   }
2503 }
2504
2505 /* used by predictors and correctors */
2506 static void softbody_store_state(Object *ob, float *ppos, float *pvel)
2507 {
2508   SoftBody *sb = ob->soft; /* is supposed to be there*/
2509   BodyPoint *bp;
2510   int a;
2511   float *pp = ppos, *pv = pvel;
2512
2513   for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
2514
2515     copy_v3_v3(pv, bp->vec);
2516     pv += 3;
2517
2518     copy_v3_v3(pp, bp->pos);
2519     pp += 3;
2520   }
2521 }
2522
2523 /* used by predictors and correctors */
2524 static void softbody_retrieve_state(Object *ob, float *ppos, float *pvel)
2525 {
2526   SoftBody *sb = ob->soft; /* is supposed to be there*/
2527   BodyPoint *bp;
2528   int a;
2529   float *pp = ppos, *pv = pvel;
2530
2531   for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
2532
2533     copy_v3_v3(bp->vec, pv);
2534     pv += 3;
2535
2536     copy_v3_v3(bp->pos, pp);
2537     pp += 3;
2538   }
2539 }
2540
2541 /* used by predictors and correctors */
2542 static void softbody_swap_state(Object *ob, float *ppos, float *pvel)
2543 {
2544   SoftBody *sb = ob->soft; /* is supposed to be there*/
2545   BodyPoint *bp;
2546   int a;
2547   float *pp = ppos, *pv = pvel;
2548   float temp[3];
2549
2550   for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
2551
2552     copy_v3_v3(temp, bp->vec);
2553     copy_v3_v3(bp->vec, pv);
2554     copy_v3_v3(pv, temp);
2555     pv += 3;
2556
2557     copy_v3_v3(temp, bp->pos);
2558     copy_v3_v3(bp->pos, pp);
2559     copy_v3_v3(pp, temp);
2560     pp += 3;
2561   }
2562 }
2563 #endif
2564
2565 /* care for bodypoints taken out of the 'ordinary' solver step
2566  * because they are screwed to goal by bolts
2567  * they just need to move along with the goal in time
2568  * we need to adjust them on sub frame timing in solver
2569  * so now when frame is done .. put 'em to the position at the end of frame
2570  */
2571 static void softbody_apply_goalsnap(Object *ob)
2572 {
2573   SoftBody *sb = ob->soft; /* is supposed to be there */
2574   BodyPoint *bp;
2575   int a;
2576
2577   for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
2578     if (_final_goal(ob, bp) >= SOFTGOALSNAP) {
2579       copy_v3_v3(bp->prevpos, bp->pos);
2580       copy_v3_v3(bp->pos, bp->origT);
2581     }
2582   }
2583 }
2584
2585 static void apply_spring_memory(Object *ob)
2586 {
2587   SoftBody *sb = ob->soft;
2588   BodySpring *bs;
2589   BodyPoint *bp1, *bp2;
2590   int a;
2591   float b, l, r;
2592
2593   if (sb && sb->totspring) {
2594     b = sb->plastic;
2595     for (a = 0; a < sb->totspring; a++) {
2596       bs = &sb->bspring[a];
2597       bp1 = &sb->bpoint[bs->v1];
2598       bp2 = &sb->bpoint[bs->v2];
2599       l = len_v3v3(bp1->pos, bp2->pos);
2600       r = bs->len / l;
2601       if ((r > 1.05f) || (r < 0.95f)) {
2602         bs->len = ((100.0f - b) * bs->len + b * l) / 100.0f;
2603       }
2604     }
2605   }
2606 }
2607
2608 /* expects full initialized softbody */
2609 static void interpolate_exciter(Object *ob, int timescale, int time)
2610 {
2611   SoftBody *sb = ob->soft;
2612   BodyPoint *bp;
2613   float f;
2614   int a;
2615
2616   f = (float)time / (float)timescale;
2617
2618   for (a = sb->totpoint, bp = sb->bpoint; a > 0; a--, bp++) {
2619     bp->origT[0] = bp->origS[0] + f * (bp->origE[0] - bp->origS[0]);
2620     bp->origT[1] = bp->origS[1] + f * (bp->origE[1] - bp->origS[1]);
2621     bp->origT[2] = bp->origS[2] + f * (bp->origE[2] - bp->origS[2]);
2622     if (_final_goal(ob, bp) >= SOFTGOALSNAP) {
2623       bp->vec[0] = bp->origE[0] - bp->origS[0];
2624       bp->vec[1] = bp->origE[1] - bp->origS[1];
2625       bp->vec[2] = bp->origE[2] - bp->origS[2];
2626     }
2627   }
2628 }
2629
2630 /* ************ convertors ********** */
2631
2632 /* for each object type we need;
2633  * - xxxx_to_softbody(Object *ob)      : a full (new) copy, creates SB geometry
2634  */
2635
2636 /* Resetting a Mesh SB object's springs */
2637 /* Spring length are caculted from'raw' mesh vertices that are NOT altered by modifier stack. */
2638 static void springs_from_mesh(Object *ob)
2639 {
2640   SoftBody *sb;
2641   Mesh *me = ob->data;
2642   BodyPoint *bp;
2643   int a;
2644   float scale = 1.0f;
2645
2646   sb = ob->soft;
2647   if (me && sb) {
2648     /* using bp->origS as a container for spring calculations here
2649      * will be overwritten sbObjectStep() to receive
2650      * actual modifier stack positions
2651      */
2652     if (me->totvert) {
2653       bp = ob->soft->bpoint;
2654       for (a = 0; a < me->totvert; a++, bp++) {
2655         copy_v3_v3(bp->origS, me->mvert[a].co);
2656         mul_m4_v3(ob->obmat, bp->origS);
2657       }
2658     }
2659     /* recalculate spring length for meshes here */
2660     /* public version shrink to fit */
2661     if (sb->springpreload != 0) {
2662       scale = sb->springpreload / 100.0f;
2663     }
2664     for (a = 0; a < sb->totspring; a++) {
2665       BodySpring *bs = &sb->bspring[a];
2666       bs->len = scale * len_v3v3(sb->bpoint[bs->v1].origS, sb->bpoint[bs->v2].origS);
2667     }
2668   }
2669 }
2670
2671 /* makes totally fresh start situation */
2672 static void mesh_to_softbody(Scene *scene, Object *ob)
2673 {
2674   SoftBody *sb;
2675   Mesh *me = ob->data;
2676   MEdge *medge = me->medge;
2677   BodyPoint *bp;
2678   BodySpring *bs;
2679   int a, totedge;
2680   int defgroup_index, defgroup_index_mass, defgroup_index_spring;
2681
2682   if (ob->softflag & OB_SB_EDGES) {
2683     totedge = me->totedge;
2684   }
2685   else {
2686     totedge = 0;
2687   }
2688
2689   /* renew ends with ob->soft with points and edges, also checks & makes ob->soft */
2690   renew_softbody(scene, ob, me->totvert, totedge);
2691
2692   /* we always make body points */
2693   sb = ob->soft;
2694   bp = sb->bpoint;
2695
2696   defgroup_index = me->dvert ? (sb->vertgroup - 1) : -1;
2697   defgroup_index_mass = me->dvert ? defgroup_name_index(ob, sb->namedVG_Mass) : -1;
2698   defgroup_index_spring = me->dvert ? defgroup_name_index(ob, sb->namedVG_Spring_K) : -1;
2699
2700   for (a = 0; a < me->totvert; a++, bp++) {
2701     /* get scalar values needed  *per vertex* from vertex group functions,
2702      * so we can *paint* them nicly ..
2703      * they are normalized [0.0..1.0] so may be we need amplitude for scale
2704      * which can be done by caller but still .. i'd like it to go this way
2705      */
2706
2707     if (ob->softflag & OB_SB_GOAL) {
2708       BLI_assert(bp->goal == sb->defgoal);
2709     }
2710     if ((ob->softflag & OB_SB_GOAL) && (defgroup_index != -1)) {
2711       bp->goal *= defvert_find_weight(&me->dvert[a], defgroup_index);
2712     }
2713
2714     /* to proof the concept
2715      * this enables per vertex *mass painting*
2716      */
2717
2718     if (defgroup_index_mass != -1) {
2719       bp->mass *= defvert_find_weight(&me->dvert[a], defgroup_index_mass);
2720     }
2721
2722     if (defgroup_index_spring != -1) {
2723       bp->springweight *= defvert_find_weight(&me->dvert[a], defgroup_index_spring);
2724     }
2725   }
2726
2727   /* but we only optionally add body edge springs */
2728   if (ob->softflag & OB_SB_EDGES) {
2729     if (medge) {
2730       bs = sb->bspring;
2731       for (a = me->totedge; a > 0; a--, medge++, bs++) {
2732         bs->v1 = medge->v1;
2733         bs->v2 = medge->v2;
2734         bs->springtype = SB_EDGE;
2735       }
2736
2737       /* insert *diagonal* springs in quads if desired */
2738       if (ob->softflag & OB_SB_QUADS) {
2739         add_mesh_quad_diag_springs(ob);
2740       }
2741
2742       build_bps_springlist(ob); /* scan for springs attached to bodypoints ONCE */
2743       /* insert *other second order* springs if desired */
2744       if (sb->secondspring > 0.0000001f) {
2745         add_2nd_order_springs(
2746             ob, sb->secondspring); /* exploits the first run of build_bps_springlist(ob);*/
2747         build_bps_springlist(ob);  /* yes we need to do it again*/
2748       }
2749       springs_from_mesh(ob); /* write the 'rest'-length of the springs */
2750       if (ob->softflag & OB_SB_SELF) {
2751         calculate_collision_balls(ob);
2752       }
2753     }
2754   }
2755 }
2756
2757 static void mesh_faces_to_scratch(Object *ob)
2758 {
2759   SoftBody *sb = ob->soft;
2760   const Mesh *me = ob->data;
2761   MLoopTri *looptri, *lt;
2762   BodyFace *bodyface;
2763   int a;
2764   /* alloc and copy faces*/
2765
2766   sb->scratch->totface = poly_to_tri_count(me->totpoly, me->totloop);
2767   looptri = lt = MEM_mallocN(sizeof(*looptri) * sb->scratch->totface, __func__);
2768   BKE_mesh_recalc_looptri(me->mloop, me->mpoly, me->mvert, me->totloop, me->totpoly, looptri);
2769
2770   bodyface = sb->scratch->bodyface = MEM_mallocN(sizeof(BodyFace) * sb->scratch->totface,
2771                                                  "SB_body_Faces");
2772
2773   for (a = 0; a < sb->scratch->totface; a++, lt++, bodyface++) {
2774     bodyface->v1 = me->mloop[lt->tri[0]].v;
2775     bodyface->v2 = me->mloop[lt->tri[1]].v;
2776     bodyface->v3 = me->mloop[lt->tri[2]].v;
2777     zero_v3(bodyface->ext_force);
2778     bodyface->ext_force[0] = bodyface->ext_force[1] = bodyface->ext_force[2] = 0.0f;
2779     bodyface->flag = 0;
2780   }
2781
2782   MEM_freeN(looptri);
2783 }
2784 static void reference_to_scratch(Object *ob)
2785 {
2786   SoftBody *sb = ob->soft;
2787   ReferenceVert *rp;
2788   BodyPoint *bp;
2789   float accu_pos[3] = {0.f, 0.f, 0.f};
2790   float accu_mass = 0.f;
2791   int a;
2792
2793   sb->scratch->Ref.ivert = MEM_mallocN(sizeof(ReferenceVert) * sb->totpoint, "SB_Reference");
2794   bp = ob->soft->bpoint;
2795   rp = sb->scratch->Ref.ivert;
2796   for (a = 0; a < sb->totpoint; a++, rp++, bp++) {
2797     copy_v3_v3(rp->pos, bp->pos);
2798     add_v3_v3(accu_pos, bp->pos);
2799     accu_mass += _final_mass(ob, bp);
2800   }
2801   mul_v3_fl(accu_pos, 1.0f / accu_mass);
2802   copy_v3_v3(sb->scratch->Ref.com, accu_pos);
2803   /* printf("reference_to_scratch\n"); */
2804 }
2805
2806 /*
2807  * helper function to get proper spring length
2808  * when object is rescaled
2809  */
2810 static float globallen(float *v1, float *v2, Object *ob)
2811 {
2812   float p1[3], p2[3];
2813   copy_v3_v3(p1, v1);
2814   mul_m4_v3(ob->obmat, p1);
2815   copy_v3_v3(p2, v2);
2816   mul_m4_v3(ob->obmat, p2);
2817   return len_v3v3(p1, p2);
2818 }
2819
2820 static void makelatticesprings(Lattice *lt, BodySpring *bs, int dostiff, Object *ob)
2821 {
2822   BPoint *bp = lt->def, *bpu;
2823   int u, v, w, dv, dw, bpc = 0, bpuc;
2824
2825   dv = lt->pntsu;
2826   dw = dv * lt->pntsv;
2827
2828   for (w = 0; w < lt->pntsw; w++) {
2829
2830     for (v = 0; v < lt->pntsv; v++) {
2831
2832       for (u = 0, bpuc = 0, bpu = NULL; u < lt->pntsu; u++, bp++, bpc++) {
2833
2834         if (w) {
2835           bs->v1 = bpc;
2836           bs->v2 = bpc - dw;
2837           bs->springtype = SB_EDGE;
2838           bs->len = globallen((bp - dw)->vec, bp->vec, ob);
2839           bs++;
2840         }
2841         if (v) {
2842           bs->v1 = bpc;
2843           bs->v2 = bpc - dv;
2844           bs->springtype = SB_EDGE;
2845           bs->len = globallen((bp - dv)->vec, bp->vec, ob);
2846           bs++;
2847         }
2848         if (u) {
2849           bs->v1 = bpuc;
2850           bs->v2 = bpc;
2851           bs->springtype = SB_EDGE;
2852           bs->len = globallen((bpu)->vec, bp->vec, ob);
2853           bs++;
2854         }
2855
2856         if (dostiff) {
2857
2858           if (w) {
2859             if (v && u) {
2860               bs->v1 = bpc;
2861               bs->v2 = bpc - dw - dv - 1;
2862               bs->springtype = SB_BEND;
2863               bs->len = globallen((bp - dw - dv - 1)->vec, bp->vec, ob);
2864               bs++;
2865             }
2866             if ((v < lt->pntsv - 1) && (u != 0)) {
2867               bs->v1 = bpc;
2868               bs->v2 = bpc - dw + dv - 1;
2869               bs->springtype = SB_BEND;
2870               bs->len = globallen((bp - dw + dv - 1)->vec, bp->vec, ob);
2871               bs++;
2872             }
2873           }
2874
2875           if (w < lt->pntsw - 1) {
2876             if (v && u) {
2877               bs->v1 = bpc;
2878               bs->v2 = bpc + dw - dv - 1;
2879               bs->springtype = SB_BEND;
2880               bs->len = globallen((bp + dw - dv - 1)->vec, bp->vec, ob);
2881               bs++;
2882             }
2883             if ((v < lt->pntsv - 1) && (u != 0)) {
2884               bs->v1 = bpc;
2885               bs->v2 = bpc + dw + dv - 1;
2886               bs->springtype = SB_BEND;
2887               bs->len = globallen((bp + dw + dv - 1)->vec, bp->vec, ob);
2888               bs++;
2889             }
2890           }
2891         }
2892         bpu = bp;
2893         bpuc = bpc;
2894       }
2895     }
2896   }
2897 }
2898
2899 /* makes totally fresh start situation */
2900 static void lattice_to_softbody(Scene *scene, Object *ob)
2901 {
2902   Lattice *lt = ob->data;
2903   SoftBody *sb;
2904   int totvert, totspring = 0, a;
2905   BodyPoint *bp;
2906   BPoint *bpnt = lt->def;
2907   int defgroup_index, defgroup_index_mass, defgroup_index_spring;
2908
2909   totvert = lt->pntsu * lt->pntsv * lt->pntsw;
2910
2911   if (ob->softflag & OB_SB_EDGES) {
2912     totspring = ((lt->pntsu - 1) * lt->pntsv + (lt->pntsv - 1) * lt->pntsu) * lt->pntsw +
2913                 lt->pntsu * lt->pntsv * (lt->pntsw - 1);
2914     if (ob->softflag & OB_SB_QUADS) {
2915       totspring += 4 * (lt->pntsu - 1) * (lt->pntsv - 1) * (lt->pntsw - 1);
2916     }
2917   }
2918
2919   /* renew ends with ob->soft with points and edges, also checks & makes ob->soft */
2920   renew_softbody(scene, ob, totvert, totspring);
2921   sb = ob->soft; /* can be created in renew_softbody() */
2922   bp = sb->bpoint;
2923
2924   defgroup_index = lt->dvert ? (sb->vertgroup - 1) : -1;
2925   defgroup_index_mass = lt->dvert ? defgroup_name_index(ob, sb->namedVG_Mass) : -1;
2926   defgroup_index_spring = lt->dvert ? defgroup_name_index(ob, sb->namedVG_Spring_K) : -1;
2927
2928   /* same code used as for mesh vertices */
2929   for (a = 0; a < totvert; a++, bp++, bpnt++) {
2930
2931     if (ob->softflag & OB_SB_GOAL) {
2932       BLI_assert(bp->goal == sb->defgoal);
2933     }
2934
2935     if ((ob->softflag & OB_SB_GOAL) && (defgroup_index != -1)) {
2936       bp->goal *= defvert_find_weight(&lt->dvert[a], defgroup_index);
2937     }
2938     else {
2939       bp->goal *= bpnt->weight;
2940     }
2941
2942     if (defgroup_index_mass != -1) {
2943       bp->mass *= defvert_find_weight(&lt->dvert[a], defgroup_index_mass);
2944     }
2945
2946     if (defgroup_index_spring != -1) {
2947       bp->springweight *= defvert_find_weight(&lt->dvert[a], defgroup_index_spring);
2948     }
2949   }
2950
2951   /* create some helper edges to enable SB lattice to be useful at all */
2952   if (ob->softflag & OB_SB_EDGES) {
2953     makelatticesprings(lt, ob->soft->bspring, ob->softflag & OB_SB_QUADS, ob);
2954     build_bps_springlist(ob); /* link bps to springs */
2955   }
2956 }
2957
2958 /* makes totally fresh start situation */
2959 static void curve_surf_to_softbody(Scene *scene, Object *ob)
2960 {
2961   Curve *cu = ob->data;
2962   SoftBody *sb;
2963   BodyPoint *bp;
2964   BodySpring *bs;
2965   Nurb *nu;
2966   BezTriple *bezt;
2967   BPoint *bpnt;
2968   int a, curindex = 0;
2969   int totvert, totspring = 0, setgoal = 0;
2970
2971   totvert = BKE_nurbList_verts_count(&cu->nurb);
2972
2973   if (ob->softflag & OB_SB_EDGES) {
2974     if (ob->type == OB_CURVE) {
2975       totspring = totvert - BLI_listbase_count(&cu->nurb);
2976     }
2977   }
2978
2979   /* renew ends with ob->soft with points and edges, also checks & makes ob->soft */
2980   renew_softbody(scene, ob, totvert, totspring);
2981   sb = ob->soft; /* can be created in renew_softbody() */
2982
2983   /* set vars now */
2984   bp = sb->bpoint;
2985   bs = sb->bspring;
2986
2987   /* weights from bpoints, same code used as for mesh vertices */
2988   /* if ((ob->softflag & OB_SB_GOAL) && sb->vertgroup) 2.4x hack*/
2989   /* new! take the weights from curve vertex anyhow */
2990   if (ob->softflag & OB_SB_GOAL) {
2991     setgoal = 1;
2992   }
2993
2994   for (nu = cu->nurb.first; nu; nu = nu->next) {
2995     if (nu->bezt) {
2996       /* Bezier case; this is nicly said naive; who ever wrote this part,
2997        * it was not me (JOW) :).
2998        *
2999        * a: never ever make tangent handles (sub) and or (ob)ject to collision.
3000        * b: rather calculate them using some C2
3001        *    (C2= continuous in second derivate -> no jump in bending ) condition.
3002        *
3003        * Not too hard to do, but needs some more code to care for;
3004        * some one may want look at it  JOW 2010/06/12. */
3005       for (bezt = nu->bezt, a = 0; a < nu->pntsu; a++, bezt++, bp += 3, curindex += 3) {
3006         if (setgoal) {
3007           bp->goal *= bezt->weight;
3008
3009           /* all three triples */
3010           (bp + 1)->goal = bp->goal;
3011           (bp + 2)->goal = bp->goal;
3012           /*do not collide handles */
3013           (bp + 1)->loc_flag |= SBF_OUTOFCOLLISION;
3014           (bp + 2)->loc_flag |= SBF_OUTOFCOLLISION;
3015         }
3016
3017         if (totspring) {
3018           if (a > 0) {
3019             bs->v1 = curindex - 3;
3020             bs->v2 = curindex;
3021             bs->springtype = SB_HANDLE;
3022             bs->len = globallen((bezt - 1)->vec[0], bezt->vec[0], ob);
3023             bs++;
3024           }
3025           bs->v1 = curindex;
3026           bs->v2 = curindex + 1;
3027           bs->springtype = SB_HANDLE;
3028           bs->len = globallen(bezt->vec[0], bezt->vec[1], ob);
3029           bs++;
3030
3031           bs->v1 = curindex + 1;
3032           bs->v2 = curindex + 2;
3033           bs->springtype = SB_HANDLE;
3034           bs->len = globallen(bezt->vec[1], bezt->vec[2], ob);
3035           bs++;
3036         }
3037       }
3038     }
3039     else {
3040       for (bpnt = nu->bp, a = 0; a < nu->pntsu * nu->pntsv; a++, bpnt++, bp++, curindex++) {
3041         if (setgoal) {
3042           bp->goal *= bpnt->weight;
3043         }
3044         if (totspring && a > 0) {
3045           bs->v1 = curindex - 1;
3046           bs->v2 = curindex;
3047           bs->springtype = SB_EDGE;
3048           bs->len = globallen((bpnt - 1)->vec, bpnt->vec, ob);
3049           bs++;
3050         }
3051       }
3052     }
3053   }
3054
3055   if (totspring) {
3056     build_bps_springlist(ob); /* link bps to springs */
3057     if (ob->softflag & OB_SB_SELF) {
3058       calculate_collision_balls(ob);
3059     }
3060   }
3061 }
3062
3063 /* copies softbody result back in object */
3064 static void softbody_to_object(Object *ob, float (*vertexCos)[3], int numVerts, int local)
3065 {
3066   SoftBody *sb = ob->soft;
3067   if (sb) {
3068     BodyPoint *bp = sb->bpoint;
3069     int a;
3070     if (sb->solverflags & SBSO_ESTIMATEIPO) {
3071       SB_estimate_transform(ob, sb->lcom, sb->lrot, sb->lscale);
3072     }
3073     /* inverse matrix is not uptodate... */
3074     invert_m4_m4(ob->imat, ob->obmat);
3075
3076     for (a = 0; a < numVerts; a++, bp++) {
3077       copy_v3_v3(vertexCos[a], bp->pos);
3078       if (local == 0) {
3079         mul_m4_v3(ob->imat, vertexCos[a]); /* softbody is in global coords, baked optionally not */
3080       }
3081     }
3082   }
3083 }
3084
3085 /* +++ ************ maintaining scratch *************** */
3086 static void sb_new_scratch(SoftBody *sb)
3087 {
3088   if (!sb) {
3089     return;
3090   }
3091   sb->scratch = MEM_callocN(sizeof(SBScratch), "SBScratch");
3092   sb->scratch->colliderhash = BLI_ghash_ptr_new("sb_new_scratch gh");
3093   sb->scratch->bodyface = NULL;
3094   sb->scratch->totface = 0;
3095   sb->scratch->aabbmax[0] = sb->scratch->aabbmax[1] = sb->scratch->aabbmax[2] = 1.0e30f;
3096   sb->scratch->aabbmin[0] = sb->scratch->aabbmin[1] = sb->scratch->aabbmin[2] = -1.0e30f;
3097   sb->scratch->Ref.ivert = NULL;
3098 }
3099 /* --- ************ maintaining scratch *************** */
3100
3101 /* ************ Object level, exported functions *************** */
3102
3103 /* allocates and initializes general main data */
3104 SoftBody *sbNew(Scene *scene)
3105 {
3106   SoftBody *sb;
3107
3108   sb = MEM_callocN(sizeof(SoftBody), "softbody");
3109
3110   sb->mediafrict = 0.5f;
3111   sb->nodemass = 1.0f;
3112   sb->grav = 9.8f;
3113   sb->physics_speed = 1.0f;
3114   sb->rklimit = 0.1f;
3115
3116   sb->goalspring = 0.5f;
3117   sb->goalfrict = 0.0f;
3118   sb->mingoal = 0.0f;
3119   sb->maxgoal = 1.0f;
3120   sb->defgoal = 0.7f;
3121
3122   sb->inspring = 0.5f;
3123   sb->infrict = 0.5f;
3124   /*todo backward file compat should copy inspring to inpush while reading old files*/
3125   sb->inpush = 0.5f;
3126
3127   sb->interval = 10;
3128   sb->sfra = scene->r.sfra;
3129   sb->efra = scene->r.efra;
3130
3131   sb->colball = 0.49f;
3132   sb->balldamp = 0.50f;
3133   sb->ballstiff = 1.0f;
3134   sb->sbc_mode = 1;
3135
3136   sb->minloops = 10;
3137   sb->maxloops = 300;
3138
3139   sb->choke = 3;
3140   sb_new_scratch(sb);
3141   /*todo backward file compat should set sb->shearstiff = 1.0f while reading old files*/
3142   sb->shearstiff = 1.0f;
3143   sb->solverflags |= SBSO_OLDERR;
3144
3145   sb->shared = MEM_callocN(sizeof(*sb->shared), "SoftBody_Shared");
3146   sb->shared->pointcache = BKE_ptcache_add(&sb->shared->ptcaches);
3147
3148   if (!sb->effector_weights) {
3149     sb->effector_weights = BKE_effector_add_weights(NULL);
3150   }
3151
3152   sb->last_frame = MINFRAME - 1;
3153
3154   return sb;
3155 }
3156
3157 /* frees all */
3158 void sbFree(Object *ob)
3159 {
3160   SoftBody *sb = ob->soft;
3161   if (sb == NULL) {
3162     return;
3163   }
3164
3165   free_softbody_intern(sb);
3166
3167   if ((ob->id.tag & LIB_TAG_NO_MAIN) == 0) {
3168     /* Only free shared data on non-CoW copies */
3169     BKE_ptcache_free_list(&sb->shared->ptcaches);
3170     sb->shared->pointcache = NULL;
3171     MEM_freeN(sb->shared);
3172   }
3173   if (sb->effector_weights) {
3174     MEM_freeN(sb->effector_weights);
3175   }
3176   MEM_freeN(sb);
3177
3178   ob->soft = NULL;
3179 }
3180
3181 void sbFreeSimulation(SoftBody *sb)
3182 {
3183   free_softbody_intern(sb);
3184 }
3185
3186 /* makes totally fresh start situation */
3187 void sbObjectToSoftbody(Object *ob)
3188 {
3189   // ob->softflag |= OB_SB_REDO;
3190
3191   free_softbody_intern(ob->soft);
3192 }
3193
3194 static int object_has_edges(Object *ob)
3195 {
3196   if (ob->type == OB_MESH) {
3197     return ((Mesh *)ob->data)->totedge;
3198   }
3199   else if (ob->type == OB_LATTICE) {
3200     return 1;
3201   }
3202   else {
3203     return 0;
3204   }
3205 }
3206
3207 /* SB global visible functions */
3208 void sbSetInterruptCallBack(int (*f)(void))
3209 {
3210   SB_localInterruptCallBack = f;
3211 }
3212
3213 static void softbody_update_positions(Object *ob,
3214                                       SoftBody *sb,
3215                                       float (*vertexCos)[3],
3216                                       int numVerts)
3217 {
3218   BodyPoint *bp;
3219   int a;
3220
3221   if (!sb || !sb->bpoint) {
3222     return;
3223   }
3224
3225   for (a = 0, bp = sb->bpoint; a < numVerts; a++, bp++) {
3226     /* store where goals are now */
3227     copy_v3_v3(bp->origS, bp->origE);
3228     /* copy the position of the goals at desired end time */
3229     copy_v3_v3(bp->origE, vertexCos[a]);
3230     /* vertexCos came from local world, go global */
3231     mul_m4_v3(ob->obmat, bp->origE);
3232     /* just to be save give bp->origT a defined value
3233      * will be calculated in interpolate_exciter() */
3234     copy_v3_v3(bp->origT, bp->origE);
3235   }
3236 }
3237
3238 /* void SB_estimate_transform */
3239 /* input   Object *ob out (says any object that can do SB like mesh, lattice, curve )
3240  * output  float lloc[3], float lrot[3][3], float lscale[3][3]
3241  * that is:
3242  * a precise position vector denoting the motion of the center of mass
3243  * give a rotation/scale matrix using averaging method, that's why estimate and not calculate
3244  * see: this is kind of reverse engineering: having to states of a point cloud and recover what
3245  * happened our advantage here we know the identity of the vertex there are others methods giving
3246  * other results. lloc, lrot, lscale are allowed to be NULL, just in case you don't need it.
3247  * should be pretty useful for pythoneers :)
3248  * not! velocity .. 2nd order stuff
3249  * vcloud_estimate_transform_v3 see
3250  */
3251
3252 void SB_estimate_transform(Object *ob, float lloc[3], float lrot[3][3], float lscale[3][3])
3253 {
3254   BodyPoint *bp;
3255   ReferenceVert *rp;
3256   SoftBody *sb = NULL;
3257   float(*opos)[3];
3258   float(*rpos)[3];
3259   float com[3], rcom[3];
3260   int a;
3261
3262   if (!ob || !ob->soft) {
3263     return; /* why did we get here ? */
3264   }
3265   sb = ob->soft;
3266   if (!sb || !sb->bpoint) {
3267     return;
3268   }
3269   opos = MEM_callocN((sb->totpoint) * 3 * sizeof(float), "SB_OPOS");
3270   rpos = MEM_callocN((sb->totpoint) * 3 * sizeof(float), "SB_RPOS");
3271   /* might filter vertex selection with a vertex group */
3272   for (a = 0, bp = sb->bpoint, rp = sb->scratch->Ref.ivert; a < sb->totpoint; a++, bp++, rp++) {
3273     copy_v3_v3(rpos[a], rp->pos);
3274     copy_v3_v3(opos[a], bp->pos);
3275   }
3276
3277   vcloud_estimate_transform_v3(sb->totpoint, opos, NULL, rpos, NULL, com, rcom, lrot, lscale);
3278   // sub_v3_v3(com, rcom);
3279   if (lloc) {
3280     copy_v3_v3(lloc, com);
3281   }
3282   copy_v3_v3(sb->lcom, com);
3283   if (lscale) {
3284     copy_m3_m3(sb->lscale, lscale);
3285   }
3286   if (lrot) {
3287     copy_m3_m3(sb->lrot, lrot);
3288   }
3289
3290   MEM_freeN(opos);
3291   MEM_freeN(rpos);
3292 }
3293
3294 static void softbody_reset(Object *ob, SoftBody *sb, float (*vertexCos)[3], int numVerts)
3295 {
3296   BodyPoint *bp;
3297   int a;
3298
3299   for (a = 0, bp = sb->bpoint; a < numVerts; a++, bp++) {
3300     copy_v3_v3(bp->pos, vertexCos[a]);
3301     mul_m4_v3(ob->obmat, bp->pos); /* yep, sofbody is global coords*/
3302     copy_v3_v3(bp->origS, bp->pos);
3303     copy_v3_v3(bp->origE, bp->pos);
3304     copy_v3_v3(bp->origT, bp->pos);
3305     bp->vec[0] = bp->vec[1] = bp->vec[2] = 0.0f;
3306
3307     /* the bp->prev*'s are for rolling back from a canceled try to propagate in time
3308      * adaptive step size algo in a nutshell:
3309      * 1.  set scheduled time step to new dtime
3310      * 2.  try to advance the scheduled time step, being optimistic execute it
3311      * 3.  check for success
3312      * 3.a we 're fine continue, may be we can increase scheduled time again ?? if so, do so!
3313      * 3.b we did exceed error limit --> roll back, shorten the scheduled time and try again at 2.
3314      * 4.  check if we did reach dtime
3315      * 4.a nope we need to do some more at 2.
3316      * 4.b yup we're done
3317      */
3318
3319     copy_v3_v3(bp->prevpos, bp->pos);
3320     copy_v3_v3(bp->prevvec, bp->vec);
3321     copy_v3_v3(bp->prevdx, bp->vec);
3322     copy_v3_v3(bp->prevdv, bp->vec);
3323   }
3324
3325   /* make a nice clean scratch struc */
3326   free_scratch(sb);   /* clear if any */
3327   sb_new_scratch(sb); /* make a new */
3328   sb->scratch->needstobuildcollider = 1;
3329   zero_v3(sb->lcom);
3330   unit_m3(sb->lrot);
3331   unit_m3(sb->lscale);
3332
3333   /* copy some info to scratch */
3334   /* we only need that if we want to reconstruct IPO */
3335   if (1) {
3336     reference_to_scratch(ob);
3337     SB_estimate_transform(ob, NULL, NULL, NULL);
3338     SB_estimate_transform(ob, NULL, NULL, NULL);
3339   }
3340   switch (ob->type) {
3341     case OB_MESH:
3342       if (ob->softflag & OB_SB_FACECOLL) {
3343         mesh_faces_to_scratch(ob);
3344       }
3345       break;
3346     case OB_LATTICE:
3347       break;
3348     case OB_CURVE:
3349     case OB_SURF:
3350       break;
3351     default:
3352       break;
3353   }
3354 }
3355
3356 static void softbody_step(
3357     struct Depsgraph *depsgraph, Scene *scene, Object *ob, SoftBody *sb, float dtime)
3358 {
3359   /* the simulator */
3360   float forcetime;
3361   double sct, sst;
3362
3363   sst = PIL_check_seconds_timer();
3364   /* Integration back in time is possible in theory, but pretty useless here.
3365    * So we refuse to do so. Since we do not know anything about 'outside' changes
3366    * especially colliders we refuse to go more than 10 frames.
3367    */
3368   if (dtime < 0 || dtime > 10.5f) {
3369     return;
3370   }
3371
3372   ccd_update_deflector_hash(depsgraph, sb->collision_group, ob, sb->scratch->colliderhash);
3373
3374   if (sb->scratch->needstobuildcollider) {
3375     ccd_build_deflector_hash(depsgraph, sb->collision_group, ob, sb->scratch->colliderhash);
3376     sb->scratch->needstobuildcollider = 0;
3377   }
3378
3379   if (sb->solver_ID < 2) {
3380     /* special case of 2nd order Runge-Kutta type AKA Heun */
3381     int mid_flags = 0;
3382     float err = 0;
3383     /* Set defaults guess we shall do one frame */
3384     float forcetimemax = 1.0f;
3385     /* Set defaults guess 1/100 is tight enough */
3386     float forcetimemin = 0.01f;
3387     /* How far did we get without violating error condition. */
3388     float timedone = 0.0;
3389     /* Loops = counter for emergency brake we don't want to lock up the system if physics fail. */
3390     int loops = 0;
3391
3392     SoftHeunTol = sb->rklimit; /* humm .. this should be calculated from sb parameters and sizes */
3393     /* adjust loop limits */
3394     if (sb->minloops > 0) {
3395       forcetimemax = dtime / sb->minloops;
3396     }
3397     if (sb->maxloops > 0) {
3398       forcetimemin = dtime / sb->maxloops;
3399     }
3400
3401     if (sb->solver_ID > 0) {
3402       mid_flags |= MID_PRESERVE;
3403     }
3404
3405     forcetime = forcetimemax; /* hope for integrating in one step */
3406     while ((ABS(timedone) < ABS(dtime)) && (loops < 2000)) {
3407       /* set goals in time */
3408       interpolate_exciter(ob, 200, (int)(200.0f * (timedone / dtime)));
3409
3410       sb->scratch->flag &= ~SBF_DOFUZZY;
3411       /* do predictive euler step */
3412       softbody_calc_forces(depsgraph, scene, ob, forcetime, timedone / dtime);
3413
3414       softbody_apply_forces(ob, forcetime, 1, NULL, mid_flags);
3415
3416       /* crop new slope values to do averaged slope step */
3417       softbody_calc_forces(depsgraph, scene, ob, forcetime, timedone / dtime);
3418
3419       softbody_apply_forces(ob, forcetime, 2, &err, mid_flags);
3420       softbody_apply_goalsnap(ob);
3421
3422       if (err > SoftHeunTol) { /* error needs to be scaled to some quantity */
3423
3424         if (forcetime > forcetimemin) {
3425           forcetime = max_ff(forcetime / 2.0f, forcetimemin);
3426           softbody_restore_prev_step(ob);
3427           // printf("down, ");
3428         }
3429         else {
3430           timedone += forcetime;
3431         }
3432       }
3433       else {
3434         float newtime = forcetime * 1.1f; /* hope for 1.1 times better conditions in next step */
3435
3436         if (sb->scratch->flag & SBF_DOFUZZY) {
3437           ///* stay with this stepsize unless err really small */
3438           // if (err > SoftHeunTol/(2.0f*sb->fuzzyness)) {
3439           newtime = forcetime;
3440           //}
3441         }
3442         else {
3443           if (err > SoftHeunTol / 2.0f) { /* stay with this stepsize unless err really small */
3444             newtime = forcetime;
3445           }
3446         }
3447         timedone += forcetime;
3448         newtime = min_ff(forcetimemax, max_ff(newtime, forcetimemin));
3449         // if (newtime > forcetime) printf("up, ");
3450         if (forcetime > 0.0f) {
3451           forcetime = min_ff(dtime - timedone, newtime);
3452         }
3453         else {
3454           forcetime = max_ff(dtime - timedone, newtime);
3455         }
3456       }
3457       loops++;
3458       if (sb->solverflags & SBSO_MONITOR) {
3459         sct = PIL_check_seconds_timer();
3460         if (sct - sst > 0.5) {
3461           printf("%3.0f%% \r", 100.0f * timedone / dtime);
3462         }
3463       }
3464       /* ask for user break */
3465       if (SB_localInterruptCallBack && SB_localInterruptCallBack()) {
3466         break;
3467       }
3468     }
3469     /* move snapped to final position */
3470     interpolate_exciter(ob, 2, 2);
3471     softbody_apply_goalsnap(ob);
3472
3473     //              if (G.debug & G_DEBUG) {
3474     if (sb->solverflags & SBSO_MONITOR) {
3475       if (loops > HEUNWARNLIMIT) { /* monitor high loop counts */
3476         printf("\r needed %d steps/frame", loops);
3477       }
3478     }
3479   }
3480   else if (sb->solver_ID == 2) {
3481     /* do semi "fake" implicit euler */
3482     // removed
3483   } /*SOLVER SELECT*/
3484   else if (sb->solver_ID == 4) {
3485     /* do semi "fake" implicit euler */
3486   } /*SOLVER SELECT*/
3487   else if (sb->solver_ID == 3) {
3488     /* do "stupid" semi "fake" implicit euler */
3489     // removed
3490
3491   } /*SOLVER SELECT*/
3492   else {
3493     CLOG_ERROR(&LOG, "softbody no valid solver ID!");
3494   } /*SOLVER SELECT*/
3495   if (sb->plastic) {
3496     apply_spring_memory(ob);
3497   }
3498
3499   if (sb->solverflags & SBSO_MONITOR) {
3500     sct = PIL_check_seconds_timer();
3501     if ((sct - sst > 0.5) || (G.debug & G_DEBUG)) {
3502       printf(" solver time %f sec %s\n", sct - sst, ob->id.name);
3503     }
3504   }
3505 }
3506
3507 static void sbStoreLastFrame(struct Depsgraph *depsgraph, Object *object, float framenr)
3508 {
3509   if (!DEG_is_active(depsgraph)) {
3510     return;
3511   }
3512   Object *object_orig = DEG_get_original_object(object);
3513   object->soft->last_frame = framenr;
3514   object_orig->soft->last_frame = framenr;
3515 }
3516
3517 /* simulates one step. framenr is in frames */
3518 void sbObjectStep(struct Depsgraph *depsgraph,
3519                   Scene *scene,
3520                   Object *ob,
3521                   float cfra,
3522                   float (*vertexCos)[3],
3523                   int numVerts)
3524 {
3525   SoftBody *sb = ob->soft;
3526   PointCache *cache;
3527   PTCacheID pid;
3528   float dtime, timescale;
3529   int framedelta, framenr, startframe, endframe;
3530   int cache_result;
3531   cache = sb->shared->pointcache;
3532
3533   framenr = (int)cfra;
3534   framedelta = framenr - cache->simframe;
3535
3536   BKE_ptcache_id_from_softbody(&pid, ob, sb);
3537   BKE_ptcache_id_time(&pid, scene, framenr, &startframe, &endframe, &timescale);
3538
3539   /* check for changes in mesh, should only happen in case the mesh
3540    * structure changes during an animation */
3541   if (sb->bpoint && numVerts != sb->totpoint) {
3542     BKE_ptcache_invalidate(cache);
3543     return;
3544   }
3545
3546   /* clamp frame ranges */
3547   if (framenr < startframe) {
3548     BKE_ptcache_invalidate(cache);
3549     return;
3550   }
3551   else if (framenr > endframe) {
3552     framenr = endframe;
3553   }
3554
3555   /* verify if we need to create the softbody data */
3556   if (sb->bpoint == NULL ||
3557       ((ob->softflag & OB_SB_EDGES) && !ob->soft->bspring && object_has_edges(ob))) {
3558
3559     switch (ob->type) {
3560       case OB_MESH:
3561         mesh_to_softbody(scene, ob);
3562         break;
3563       case OB_LATTICE:
3564         lattice_to_softbody(scene, ob);
3565         break;
3566       case OB_CURVE:
3567       case OB_SURF:
3568         curve_surf_to_softbody(scene, ob);
3569         break;
3570       default:
3571         renew_softbody(scene, ob, numVerts, 0);
3572         break;
3573     }
3574
3575     softbody_update_positions(ob, sb, vertexCos, numVerts);
3576     softbody_reset(ob, sb, vertexCos, numVerts);