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