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