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