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