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