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