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