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