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