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