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