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