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