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