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