source/blender/blenkernel/intern/softbody.c

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