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