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