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