extern/bullet2/src/BulletSoftBody/btSoftBody.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15 ///btSoftBody implementation by Nathanael Presson
  16
  17 #include "btSoftBodyInternals.h"
  18
  19 //
  20 btSoftBody::btSoftBody(btSoftBodyWorldInfo*     worldInfo,int node_count,  const btVector3* x,  const btScalar* m)
  21 :m_worldInfo(worldInfo)
  22 {
  23         /* Init         */
  24         m_internalType          =       CO_SOFT_BODY;
  25         m_cfg.aeromodel         =       eAeroModel::V_Point;
  26         m_cfg.kVCF                      =       1;
  27         m_cfg.kDG                       =       0;
  28         m_cfg.kLF                       =       0;
  29         m_cfg.kDP                       =       0;
  30         m_cfg.kPR                       =       0;
  31         m_cfg.kVC                       =       0;
  32         m_cfg.kDF                       =       (btScalar)0.2;
  33         m_cfg.kMT                       =       0;
  34         m_cfg.kCHR                      =       (btScalar)1.0;
  35         m_cfg.kKHR                      =       (btScalar)0.1;
  36         m_cfg.kSHR                      =       (btScalar)1.0;
  37         m_cfg.kAHR                      =       (btScalar)0.7;
  38         m_cfg.kSRHR_CL          =       (btScalar)0.1;
  39         m_cfg.kSKHR_CL          =       (btScalar)1;
  40         m_cfg.kSSHR_CL          =       (btScalar)0.5;
  41         m_cfg.kSR_SPLT_CL       =       (btScalar)0.5;
  42         m_cfg.kSK_SPLT_CL       =       (btScalar)0.5;
  43         m_cfg.kSS_SPLT_CL       =       (btScalar)0.5;
  44         m_cfg.maxvolume         =       (btScalar)1;
  45         m_cfg.timescale         =       1;
  46         m_cfg.viterations       =       0;
  47         m_cfg.piterations       =       1;
  48         m_cfg.diterations       =       0;
  49         m_cfg.citerations       =       4;
  50         m_cfg.collisions        =       fCollision::Default;
  51         m_pose.m_bvolume        =       false;
  52         m_pose.m_bframe         =       false;
  53         m_pose.m_volume         =       0;
  54         m_pose.m_com            =       btVector3(0,0,0);
  55         m_pose.m_rot.setIdentity();
  56         m_pose.m_scl.setIdentity();
  57         m_tag                           =       0;
  58         m_timeacc                       =       0;
  59         m_bUpdateRtCst          =       true;
  60         m_bounds[0]                     =       btVector3(0,0,0);
  61         m_bounds[1]                     =       btVector3(0,0,0);
  62         m_worldTransform.setIdentity();
  63         setSolver(eSolverPresets::Positions);
  64         /* Default material     */
  65         Material*       pm=appendMaterial();
  66         pm->m_kLST      =       1;
  67         pm->m_kAST      =       1;
  68         pm->m_kVST      =       1;
  69         pm->m_flags     =       fMaterial::Default;
  70         /* Collision shape      */
  71         ///for now, create a collision shape internally
  72         setCollisionShape(new btSoftBodyCollisionShape(this));
  73         m_collisionShape->setMargin(0.25);
  74         /* Nodes                        */
  75         const btScalar          margin=getCollisionShape()->getMargin();
  76         m_nodes.resize(node_count);
  77         for(int i=0,ni=node_count;i<ni;++i)
  78         {
  79                 Node&   n=m_nodes[i];
  80                 ZeroInitialize(n);
  81                 n.m_x           =       x?*x++:btVector3(0,0,0);
  82                 n.m_q           =       n.m_x;
  83                 n.m_im          =       m?*m++:1;
  84                 n.m_im          =       n.m_im>0?1/n.m_im:0;
  85                 n.m_leaf        =       m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
  86                 n.m_material=   pm;
  87         }
  88         updateBounds();
  89
  90
  91 }
  92
  93 //
  94 btSoftBody::~btSoftBody()
  95 {
  96         //for now, delete the internal shape
  97         delete m_collisionShape;
  98         int i;
  99
 100         for(i=0;i<m_clusters.size();++i)
 101                 btAlignedFree(m_clusters[i]);
 102         for(i=0;i<m_materials.size();++i)
 103                 btAlignedFree(m_materials[i]);
 104         for(i=0;i<m_joints.size();++i)
 105                 btAlignedFree(m_joints[i]);
 106 }
 107
 108 //
 109 bool                    btSoftBody::checkLink(int node0,int node1) const
 110 {
 111         return(checkLink(&m_nodes[node0],&m_nodes[node1]));
 112 }
 113
 114 //
 115 bool                    btSoftBody::checkLink(const Node* node0,const Node* node1) const
 116 {
 117         const Node*     n[]={node0,node1};
 118         for(int i=0,ni=m_links.size();i<ni;++i)
 119         {
 120                 const Link&     l=m_links[i];
 121                 if(     (l.m_n[0]==n[0]&&l.m_n[1]==n[1])||
 122                         (l.m_n[0]==n[1]&&l.m_n[1]==n[0]))
 123                 {
 124                         return(true);
 125                 }
 126         }
 127         return(false);
 128 }
 129
 130 //
 131 bool                    btSoftBody::checkFace(int node0,int node1,int node2) const
 132 {
 133         const Node*     n[]={   &m_nodes[node0],
 134                 &m_nodes[node1],
 135                 &m_nodes[node2]};
 136         for(int i=0,ni=m_faces.size();i<ni;++i)
 137         {
 138                 const Face&     f=m_faces[i];
 139                 int                     c=0;
 140                 for(int j=0;j<3;++j)
 141                 {
 142                         if(     (f.m_n[j]==n[0])||
 143                                 (f.m_n[j]==n[1])||
 144                                 (f.m_n[j]==n[2])) c|=1<<j; else break;
 145                 }
 146                 if(c==7) return(true);
 147         }
 148         return(false);
 149 }
 150
 151 //
 152 btSoftBody::Material*           btSoftBody::appendMaterial()
 153 {
 154 Material*       pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
 155 if(m_materials.size()>0)
 156         *pm=*m_materials[0];
 157         else
 158         ZeroInitialize(*pm);
 159 m_materials.push_back(pm);
 160 return(pm);
 161 }
 162
 163 //
 164 void                    btSoftBody::appendNote( const char* text,
 165                                                                                 const btVector3& o,
 166                                                                                 const btVector4& c,
 167                                                                                 Node* n0,
 168                                                                                 Node* n1,
 169                                                                                 Node* n2,
 170                                                                                 Node* n3)
 171 {
 172 Note    n;
 173 ZeroInitialize(n);
 174 n.m_rank                =       0;
 175 n.m_text                =       text;
 176 n.m_offset              =       o;
 177 n.m_coords[0]   =       c.x();
 178 n.m_coords[1]   =       c.y();
 179 n.m_coords[2]   =       c.z();
 180 n.m_coords[3]   =       c.w();
 181 n.m_nodes[0]    =       n0;n.m_rank+=n0?1:0;
 182 n.m_nodes[1]    =       n1;n.m_rank+=n1?1:0;
 183 n.m_nodes[2]    =       n2;n.m_rank+=n2?1:0;
 184 n.m_nodes[3]    =       n3;n.m_rank+=n3?1:0;
 185 m_notes.push_back(n);
 186 }
 187
 188 //
 189 void                    btSoftBody::appendNote( const char* text,
 190                                                                                 const btVector3& o,
 191                                                                                 Node* feature)
 192 {
 193 appendNote(text,o,btVector4(1,0,0,0),feature);
 194 }
 195
 196 //
 197 void                    btSoftBody::appendNote( const char* text,
 198                                                                                 const btVector3& o,
 199                                                                                 Link* feature)
 200 {
 201 static const btScalar   w=1/(btScalar)2;
 202 appendNote(text,o,btVector4(w,w,0,0),   feature->m_n[0],
 203                                                                                 feature->m_n[1]);
 204 }
 205
 206 //
 207 void                    btSoftBody::appendNote( const char* text,
 208                                                                                 const btVector3& o,
 209                                                                                 Face* feature)
 210 {
 211 static const btScalar   w=1/(btScalar)3;
 212 appendNote(text,o,btVector4(w,w,w,0),   feature->m_n[0],
 213                                                                                 feature->m_n[1],
 214                                                                                 feature->m_n[2]);
 215 }
 216
 217 //
 218 void                    btSoftBody::appendNode( const btVector3& x,btScalar m)
 219 {
 220 if(m_nodes.capacity()==m_nodes.size())
 221         {
 222         pointersToIndices();
 223         m_nodes.reserve(m_nodes.size()*2+1);
 224         indicesToPointers();
 225         }
 226 const btScalar  margin=getCollisionShape()->getMargin();
 227 m_nodes.push_back(Node());
 228 Node&                   n=m_nodes[m_nodes.size()-1];
 229 ZeroInitialize(n);
 230 n.m_x                   =       x;
 231 n.m_q                   =       n.m_x;
 232 n.m_im                  =       m>0?1/m:0;
 233 n.m_material    =       m_materials[0];
 234 n.m_leaf                =       m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
 235 }
 236
 237 //
 238 void                    btSoftBody::appendLink(int model,Material* mat)
 239 {
 240 Link    l;
 241 if(model>=0)
 242         l=m_links[model];
 243         else
 244         { ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; }
 245 m_links.push_back(l);
 246 }
 247
 248 //
 249 void                    btSoftBody::appendLink( int node0,
 250                                                                            int node1,
 251                                                                            Material* mat,
 252                                                                            bool bcheckexist)
 253 {
 254         appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist);
 255 }
 256
 257 //
 258 void                    btSoftBody::appendLink( Node* node0,
 259                                                                            Node* node1,
 260                                                                            Material* mat,
 261                                                                            bool bcheckexist)
 262 {
 263         if((!bcheckexist)||(!checkLink(node0,node1)))
 264         {
 265                 appendLink(-1,mat);
 266                 Link&   l=m_links[m_links.size()-1];
 267                 l.m_n[0]                =       node0;
 268                 l.m_n[1]                =       node1;
 269                 l.m_rl                  =       (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
 270                 m_bUpdateRtCst=true;
 271         }
 272 }
 273
 274 //
 275 void                    btSoftBody::appendFace(int model,Material* mat)
 276 {
 277 Face    f;
 278 if(model>=0)
 279         { f=m_faces[model]; }
 280         else
 281         { ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; }
 282 m_faces.push_back(f);
 283 }
 284
 285 //
 286 void                    btSoftBody::appendFace(int node0,int node1,int node2,Material* mat)
 287 {
 288         appendFace(-1,mat);
 289         Face&   f=m_faces[m_faces.size()-1];
 290         btAssert(node0!=node1);
 291         btAssert(node1!=node2);
 292         btAssert(node2!=node0);
 293         f.m_n[0]        =       &m_nodes[node0];
 294         f.m_n[1]        =       &m_nodes[node1];
 295         f.m_n[2]        =       &m_nodes[node2];
 296         f.m_ra          =       AreaOf( f.m_n[0]->m_x,
 297                 f.m_n[1]->m_x,
 298                 f.m_n[2]->m_x);
 299         m_bUpdateRtCst=true;
 300 }
 301
 302 //
 303 void                    btSoftBody::appendAnchor(int node,btRigidBody* body)
 304 {
 305         Anchor  a;
 306         a.m_node                        =       &m_nodes[node];
 307         a.m_body                        =       body;
 308         a.m_local                       =       body->getInterpolationWorldTransform().inverse()*a.m_node->m_x;
 309         a.m_node->m_battach     =       1;
 310         m_anchors.push_back(a);
 311 }
 312
 313 //
 314 void                    btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1)
 315 {
 316 LJoint*         pj      =       new(btAlignedAlloc(sizeof(LJoint),16)) LJoint();
 317 pj->m_bodies[0] =       body0;
 318 pj->m_bodies[1] =       body1;
 319 pj->m_refs[0]   =       pj->m_bodies[0].xform().inverse()*specs.position;
 320 pj->m_refs[1]   =       pj->m_bodies[1].xform().inverse()*specs.position;
 321 pj->m_cfm               =       specs.cfm;
 322 pj->m_erp               =       specs.erp;
 323 pj->m_split             =       specs.split;
 324 m_joints.push_back(pj);
 325 }
 326
 327 //
 328 void                    btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body)
 329 {
 330 appendLinearJoint(specs,m_clusters[0],body);
 331 }
 332
 333 //
 334 void                    btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body)
 335 {
 336 appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]);
 337 }
 338
 339 //
 340 void                    btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1)
 341 {
 342 AJoint*         pj      =       new(btAlignedAlloc(sizeof(AJoint),16)) AJoint();
 343 pj->m_bodies[0] =       body0;
 344 pj->m_bodies[1] =       body1;
 345 pj->m_refs[0]   =       pj->m_bodies[0].xform().inverse().getBasis()*specs.axis;
 346 pj->m_refs[1]   =       pj->m_bodies[1].xform().inverse().getBasis()*specs.axis;
 347 pj->m_cfm               =       specs.cfm;
 348 pj->m_erp               =       specs.erp;
 349 pj->m_split             =       specs.split;
 350 pj->m_icontrol  =       specs.icontrol;
 351 m_joints.push_back(pj);
 352 }
 353
 354 //
 355 void                    btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body)
 356 {
 357 appendAngularJoint(specs,m_clusters[0],body);
 358 }
 359
 360 //
 361 void                    btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body)
 362 {
 363 appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]);
 364 }
 365
 366 //
 367 void                    btSoftBody::addForce(const btVector3& force)
 368 {
 369         for(int i=0,ni=m_nodes.size();i<ni;++i) addForce(force,i);
 370 }
 371
 372 //
 373 void                    btSoftBody::addForce(const btVector3& force,int node)
 374 {
 375         Node&   n=m_nodes[node];
 376         if(n.m_im>0)
 377         {
 378                 n.m_f   +=      force;
 379         }
 380 }
 381
 382 //
 383 void                    btSoftBody::addVelocity(const btVector3& velocity)
 384 {
 385         for(int i=0,ni=m_nodes.size();i<ni;++i) addVelocity(velocity,i);
 386 }
 387
 388 //
 389 void                    btSoftBody::addVelocity(const btVector3& velocity,int node)
 390 {
 391         Node&   n=m_nodes[node];
 392         if(n.m_im>0)
 393         {
 394                 n.m_v   +=      velocity;
 395         }
 396 }
 397
 398 //
 399 void                    btSoftBody::setMass(int node,btScalar mass)
 400 {
 401         m_nodes[node].m_im=mass>0?1/mass:0;
 402         m_bUpdateRtCst=true;
 403 }
 404
 405 //
 406 btScalar                btSoftBody::getMass(int node) const
 407 {
 408         return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0);
 409 }
 410
 411 //
 412 btScalar                btSoftBody::getTotalMass() const
 413 {
 414         btScalar        mass=0;
 415         for(int i=0;i<m_nodes.size();++i)
 416         {
 417                 mass+=getMass(i);
 418         }
 419         return(mass);
 420 }
 421
 422 //
 423 void                    btSoftBody::setTotalMass(btScalar mass,bool fromfaces)
 424 {
 425         int i;
 426
 427         if(fromfaces)
 428         {
 429
 430                 for(i=0;i<m_nodes.size();++i)
 431                 {
 432                         m_nodes[i].m_im=0;
 433                 }
 434                 for(i=0;i<m_faces.size();++i)
 435                 {
 436                         const Face&             f=m_faces[i];
 437                         const btScalar  twicearea=AreaOf(       f.m_n[0]->m_x,
 438                                                                                                 f.m_n[1]->m_x,
 439                                                                                                 f.m_n[2]->m_x);
 440                         for(int j=0;j<3;++j)
 441                         {
 442                                 f.m_n[j]->m_im+=twicearea;
 443                         }
 444                 }
 445                 for( i=0;i<m_nodes.size();++i)
 446                 {
 447                         m_nodes[i].m_im=1/m_nodes[i].m_im;
 448                 }
 449         }
 450         const btScalar  tm=getTotalMass();
 451         const btScalar  itm=1/tm;
 452         for( i=0;i<m_nodes.size();++i)
 453         {
 454                 m_nodes[i].m_im/=itm*mass;
 455         }
 456         m_bUpdateRtCst=true;
 457 }
 458
 459 //
 460 void                    btSoftBody::setTotalDensity(btScalar density)
 461 {
 462         setTotalMass(getVolume()*density,true);
 463 }
 464
 465 //
 466 void                    btSoftBody::transform(const btTransform& trs)
 467 {
 468         const btScalar  margin=getCollisionShape()->getMargin();
 469         for(int i=0,ni=m_nodes.size();i<ni;++i)
 470         {
 471                 Node&   n=m_nodes[i];
 472                 n.m_x=trs*n.m_x;
 473                 n.m_q=trs*n.m_q;
 474                 n.m_n=trs.getBasis()*n.m_n;
 475                 m_ndbvt.update(n.m_leaf,btDbvtVolume::FromCR(n.m_x,margin));
 476         }
 477         updateNormals();
 478         updateBounds();
 479         updateConstants();
 480 }
 481
 482 //
 483 void                    btSoftBody::translate(const btVector3& trs)
 484 {
 485 btTransform     t;
 486 t.setIdentity();
 487 t.setOrigin(trs);
 488 transform(t);
 489 }
 490
 491 //
 492 void                    btSoftBody::rotate(     const btQuaternion& rot)
 493 {
 494 btTransform     t;
 495 t.setIdentity();
 496 t.setRotation(rot);
 497 transform(t);
 498 }
 499
 500 //
 501 void                    btSoftBody::scale(const btVector3& scl)
 502 {
 503         const btScalar  margin=getCollisionShape()->getMargin();
 504         for(int i=0,ni=m_nodes.size();i<ni;++i)
 505         {
 506                 Node&   n=m_nodes[i];
 507                 n.m_x*=scl;
 508                 n.m_q*=scl;
 509                 m_ndbvt.update(n.m_leaf,btDbvtVolume::FromCR(n.m_x,margin));
 510         }
 511         updateNormals();
 512         updateBounds();
 513         updateConstants();
 514 }
 515
 516 //
 517 void                    btSoftBody::setPose(bool bvolume,bool bframe)
 518 {
 519         m_pose.m_bvolume        =       bvolume;
 520         m_pose.m_bframe         =       bframe;
 521         int i,ni;
 522
 523         /* Weights              */
 524         const btScalar  omass=getTotalMass();
 525         const btScalar  kmass=omass*m_nodes.size()*1000;
 526         btScalar                tmass=omass;
 527         m_pose.m_wgh.resize(m_nodes.size());
 528         for(i=0,ni=m_nodes.size();i<ni;++i)
 529         {
 530                 if(m_nodes[i].m_im<=0) tmass+=kmass;
 531         }
 532         for( i=0,ni=m_nodes.size();i<ni;++i)
 533         {
 534                 Node&   n=m_nodes[i];
 535                 m_pose.m_wgh[i]=        n.m_im>0                                        ?
 536                                                         1/(m_nodes[i].m_im*tmass)       :
 537                                                         kmass/tmass;
 538         }
 539         /* Pos          */
 540         const btVector3 com=evaluateCom();
 541         m_pose.m_pos.resize(m_nodes.size());
 542         for( i=0,ni=m_nodes.size();i<ni;++i)
 543         {
 544                 m_pose.m_pos[i]=m_nodes[i].m_x-com;
 545         }
 546         m_pose.m_volume =       bvolume?getVolume():0;
 547         m_pose.m_com    =       com;
 548         m_pose.m_rot.setIdentity();
 549         m_pose.m_scl.setIdentity();
 550         /* Aqq          */
 551         m_pose.m_aqq[0] =
 552         m_pose.m_aqq[1] =
 553         m_pose.m_aqq[2] =       btVector3(0,0,0);
 554         for( i=0,ni=m_nodes.size();i<ni;++i)
 555                 {
 556                 const btVector3&        q=m_pose.m_pos[i];
 557                 const btVector3         mq=m_pose.m_wgh[i]*q;
 558                 m_pose.m_aqq[0]+=mq.x()*q;
 559                 m_pose.m_aqq[1]+=mq.y()*q;
 560                 m_pose.m_aqq[2]+=mq.z()*q;
 561                 }
 562         m_pose.m_aqq=m_pose.m_aqq.inverse();
 563         updateConstants();
 564 }
 565
 566 //
 567 btScalar                btSoftBody::getVolume() const
 568 {
 569         btScalar        vol=0;
 570         if(m_nodes.size()>0)
 571         {
 572                 int i,ni;
 573
 574                 const btVector3 org=m_nodes[0].m_x;
 575                 for(i=0,ni=m_faces.size();i<ni;++i)
 576                 {
 577                         const Face&     f=m_faces[i];
 578                         vol+=dot(f.m_n[0]->m_x-org,cross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org));
 579                 }
 580                 vol/=(btScalar)6;
 581         }
 582         return(vol);
 583 }
 584
 585 //
 586 int                             btSoftBody::clusterCount() const
 587 {
 588 return(m_clusters.size());
 589 }
 590
 591 //
 592 btVector3               btSoftBody::clusterCom(const Cluster* cluster)
 593 {
 594 btVector3               com(0,0,0);
 595 for(int i=0,ni=cluster->m_nodes.size();i<ni;++i)
 596         {
 597         com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i];
 598         }
 599 return(com*cluster->m_imass);
 600 }
 601
 602 //
 603 btVector3               btSoftBody::clusterCom(int cluster) const
 604 {
 605 return(clusterCom(m_clusters[cluster]));
 606 }
 607
 608 //
 609 btVector3               btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos)
 610 {
 611 return(cluster->m_lv+cross(cluster->m_av,rpos));
 612 }
 613
 614 //
 615 void                    btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
 616 {
 617 const btVector3 li=cluster->m_imass*impulse;
 618 const btVector3 ai=cluster->m_invwi*cross(rpos,impulse);
 619 cluster->m_vimpulses[0]+=li;cluster->m_lv+=li;
 620 cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
 621 cluster->m_nvimpulses++;
 622 }
 623
 624 //
 625 void                    btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
 626 {
 627 const btVector3 li=cluster->m_imass*impulse;
 628 const btVector3 ai=cluster->m_invwi*cross(rpos,impulse);
 629 cluster->m_dimpulses[0]+=li;
 630 cluster->m_dimpulses[1]+=ai;
 631 cluster->m_ndimpulses++;
 632 }
 633
 634 //
 635 void                    btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse)
 636 {
 637 if(impulse.m_asVelocity)        clusterVImpulse(cluster,rpos,impulse.m_velocity);
 638 if(impulse.m_asDrift)           clusterDImpulse(cluster,rpos,impulse.m_drift);
 639 }
 640
 641 //
 642 void                    btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse)
 643 {
 644 const btVector3 ai=cluster->m_invwi*impulse;
 645 cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
 646 cluster->m_nvimpulses++;
 647 }
 648
 649 //
 650 void                    btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse)
 651 {
 652 const btVector3 ai=cluster->m_invwi*impulse;
 653 cluster->m_dimpulses[1]+=ai;
 654 cluster->m_ndimpulses++;
 655 }
 656
 657 //
 658 void                    btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse)
 659 {
 660 if(impulse.m_asVelocity)        clusterVAImpulse(cluster,impulse.m_velocity);
 661 if(impulse.m_asDrift)           clusterDAImpulse(cluster,impulse.m_drift);
 662 }
 663
 664 //
 665 void                    btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse)
 666 {
 667 cluster->m_dimpulses[0]+=impulse*cluster->m_imass;
 668 cluster->m_ndimpulses++;
 669 }
 670
 671 //
 672 int                             btSoftBody::generateBendingConstraints(int distance,Material* mat)
 673 {
 674         int i,j;
 675
 676         if(distance>1)
 677         {
 678                 /* Build graph  */
 679                 const int               n=m_nodes.size();
 680                 const unsigned  inf=(~(unsigned)0)>>1;
 681                 unsigned*               adj=new unsigned[n*n];
 682 #define IDX(_x_,_y_)    ((_y_)*n+(_x_))
 683                 for(j=0;j<n;++j)
 684                 {
 685                         for(i=0;i<n;++i)
 686                         {
 687                                 if(i!=j)        adj[IDX(i,j)]=adj[IDX(j,i)]=inf;
 688                                 else
 689                                         adj[IDX(i,j)]=adj[IDX(j,i)]=0;
 690                         }
 691                 }
 692                 for( i=0;i<m_links.size();++i)
 693                 {
 694                         const int       ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
 695                         const int       ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
 696                         adj[IDX(ia,ib)]=1;
 697                         adj[IDX(ib,ia)]=1;
 698                 }
 699                 for(int k=0;k<n;++k)
 700                 {
 701                         for(j=0;j<n;++j)
 702                         {
 703                                 for(i=j+1;i<n;++i)
 704                                 {
 705                                         const unsigned  sum=adj[IDX(i,k)]+adj[IDX(k,j)];
 706                                         if(adj[IDX(i,j)]>sum)
 707                                         {
 708                                                 adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
 709                                         }
 710                                 }
 711                         }
 712                 }
 713                 /* Build links  */
 714                 int     nlinks=0;
 715                 for(j=0;j<n;++j)
 716                 {
 717                         for(i=j+1;i<n;++i)
 718                         {
 719                                 if(adj[IDX(i,j)]==(unsigned)distance)
 720                                 {
 721                                         appendLink(i,j,mat);
 722                                         m_links[m_links.size()-1].m_bbending=1;
 723                                         ++nlinks;
 724                                 }
 725                         }
 726                 }
 727                 delete[] adj;
 728                 return(nlinks);
 729         }
 730         return(0);
 731 }
 732
 733 //
 734 void                    btSoftBody::randomizeConstraints()
 735 {
 736 unsigned long   seed=243703;
 737 #define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff)
 738 int i,ni;
 739
 740         for(i=0,ni=m_links.size();i<ni;++i)
 741         {
 742                 btSwap(m_links[i],m_links[NEXTRAND%ni]);
 743         }
 744         for(i=0,ni=m_faces.size();i<ni;++i)
 745         {
 746                 btSwap(m_faces[i],m_faces[NEXTRAND%ni]);
 747         }
 748 #undef NEXTRAND
 749 }
 750
 751 //
 752 int                             btSoftBody::generateClusters(int k,int maxiterations)
 753 {
 754         int i;
 755
 756 for(i=0;i<m_clusters.size();++i)
 757         {
 758         if(m_clusters[i]->m_leaf) m_cdbvt.remove(m_clusters[i]->m_leaf);
 759         btAlignedFree(m_clusters[i]);
 760         }
 761 m_clusters.resize(btMin(k,m_nodes.size()));
 762
 763
 764
 765 for(i=0;i<m_clusters.size();++i)
 766         {
 767         m_clusters[i]                   =       new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
 768         m_clusters[i]->m_collide=       true;
 769         }
 770 k=m_clusters.size();
 771 if(k>0)
 772         {
 773         /* Initialize           */
 774         btAlignedObjectArray<btVector3> centers;
 775         btVector3                                               cog(0,0,0);
 776         int                                                             i;
 777         for(i=0;i<m_nodes.size();++i)
 778                 {
 779                 cog+=m_nodes[i].m_x;
 780                 m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
 781                 }
 782         cog/=(btScalar)m_nodes.size();
 783         centers.resize(k,cog);
 784         /* Iterate                      */
 785         const btScalar  slope=16;
 786         bool                    changed;
 787         int                             iterations=0;
 788         do      {
 789                 const btScalar  w=2-btMin<btScalar>(1,iterations/slope);
 790                 changed=false;
 791                 iterations++;
 792                 int i;
 793
 794                 for(i=0;i<k;++i)
 795                         {
 796                         btVector3       c(0,0,0);
 797                         for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
 798                                 {
 799                                 c+=m_clusters[i]->m_nodes[j]->m_x;
 800                                 }
 801                         if(m_clusters[i]->m_nodes.size())
 802                                 {
 803                                 c                       /=      (btScalar)m_clusters[i]->m_nodes.size();
 804                                 c                       =       centers[i]+(c-centers[i])*w;
 805                                 changed         |=      ((c-centers[i]).length2()>SIMD_EPSILON);
 806                                 centers[i]      =       c;
 807                                 m_clusters[i]->m_nodes.resize(0);
 808                                 }
 809                         }
 810                 for(i=0;i<m_nodes.size();++i)
 811                         {
 812                         const btVector3 nx=m_nodes[i].m_x;
 813                         int                             kbest=0;
 814                         btScalar                kdist=ClusterMetric(centers[0],nx);
 815                         for(int j=1;j<k;++j)
 816                                 {
 817                                 const btScalar  d=ClusterMetric(centers[j],nx);
 818                                 if(d<kdist)
 819                                         {
 820                                         kbest=j;
 821                                         kdist=d;
 822                                         }
 823                                 }
 824                         m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
 825                         }
 826                 } while(changed&&(iterations<maxiterations));
 827         /* Merge                */
 828         btAlignedObjectArray<int>       cids;
 829         cids.resize(m_nodes.size(),-1);
 830         for(i=0;i<m_clusters.size();++i)
 831                 {
 832                 for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
 833                         {
 834                         cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i;
 835                         }
 836                 }
 837         for(i=0;i<m_faces.size();++i)
 838                 {
 839                 const int idx[]={       int(m_faces[i].m_n[0]-&m_nodes[0]),
 840                                                         int(m_faces[i].m_n[1]-&m_nodes[0]),
 841                                                         int(m_faces[i].m_n[2]-&m_nodes[0])};
 842                 for(int j=0;j<3;++j)
 843                         {
 844                         const int cid=cids[idx[j]];
 845                         for(int q=1;q<3;++q)
 846                                 {
 847                                 const int kid=idx[(j+q)%3];
 848                                 if(cids[kid]!=cid)
 849                                         {
 850                                         if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size())
 851                                                 {
 852                                                 m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
 853                                                 }
 854                                         }
 855                                 }
 856                         }
 857                 }
 858         /* Master               */
 859         if(m_clusters.size()>1)
 860                 {
 861                 Cluster*        pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
 862                 pmaster->m_collide      =       false;
 863                 pmaster->m_nodes.reserve(m_nodes.size());
 864                 for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]);
 865                 m_clusters.push_back(pmaster);
 866                 btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]);
 867                 }
 868         /* Terminate    */
 869         for(i=0;i<m_clusters.size();++i)
 870                 {
 871                 if(m_clusters[i]->m_nodes.size()==0)
 872                         {
 873                         btAlignedFree(m_clusters[i]);
 874                         btSwap(m_clusters[i],m_clusters[m_clusters.size()-1]);
 875                         m_clusters.pop_back();
 876                         --i;
 877                         }
 878                 }
 879
 880         initializeClusters();
 881         updateClusters();
 882         return(m_clusters.size());
 883         }
 884 return(0);
 885 }
 886
 887 //
 888 void                    btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut)
 889 {
 890 const Node*                     nbase = &m_nodes[0];
 891 int                                     ncount = m_nodes.size();
 892 btSymMatrix<int>        edges(ncount,-2);
 893 int                                     newnodes=0;
 894 int i,j,k,ni;
 895
 896 /* Filter out           */
 897 for(i=0;i<m_links.size();++i)
 898         {
 899         Link&   l=m_links[i];
 900         if(l.m_bbending)
 901                 {
 902                 if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
 903                         {
 904                         btSwap(m_links[i],m_links[m_links.size()-1]);
 905                         m_links.pop_back();--i;
 906                         }
 907                 }
 908         }
 909 /* Fill edges           */
 910 for(i=0;i<m_links.size();++i)
 911         {
 912         Link&   l=m_links[i];
 913         edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1;
 914         }
 915 for(i=0;i<m_faces.size();++i)
 916         {
 917         Face&   f=m_faces[i];
 918         edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1;
 919         edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
 920         edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
 921         }
 922 /* Intersect            */
 923 for(i=0;i<ncount;++i)
 924         {
 925         for(j=i+1;j<ncount;++j)
 926                 {
 927                 if(edges(i,j)==-1)
 928                         {
 929                         Node&                   a=m_nodes[i];
 930                         Node&                   b=m_nodes[j];
 931                         const btScalar  t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary);
 932                         if(t>0)
 933                                 {
 934                                 const btVector3 x=Lerp(a.m_x,b.m_x,t);
 935                                 const btVector3 v=Lerp(a.m_v,b.m_v,t);
 936                                 btScalar                m=0;
 937                                 if(a.m_im>0)
 938                                         {
 939                                         if(b.m_im>0)
 940                                                 {
 941                                                 const btScalar  ma=1/a.m_im;
 942                                                 const btScalar  mb=1/b.m_im;
 943                                                 const btScalar  mc=Lerp(ma,mb,t);
 944                                                 const btScalar  f=(ma+mb)/(ma+mb+mc);
 945                                                 a.m_im=1/(ma*f);
 946                                                 b.m_im=1/(mb*f);
 947                                                 m=mc*f;
 948                                                 }
 949                                                 else
 950                                                 { a.m_im/=0.5;m=1/a.m_im; }
 951                                         }
 952                                         else
 953                                         {
 954                                         if(b.m_im>0)
 955                                                 { b.m_im/=0.5;m=1/b.m_im; }
 956                                                 else
 957                                                 m=0;
 958                                         }
 959                                 appendNode(x,m);
 960                                 edges(i,j)=m_nodes.size()-1;
 961                                 m_nodes[edges(i,j)].m_v=v;
 962                                 ++newnodes;
 963                                 }
 964                         }
 965                 }
 966         }
 967 nbase=&m_nodes[0];
 968 /* Refine links         */
 969 for(i=0,ni=m_links.size();i<ni;++i)
 970         {
 971         Link&           feat=m_links[i];
 972         const int       idx[]={ int(feat.m_n[0]-nbase),
 973                                                 int(feat.m_n[1]-nbase)};
 974         if((idx[0]<ncount)&&(idx[1]<ncount))
 975                 {
 976                 const int ni=edges(idx[0],idx[1]);
 977                 if(ni>0)
 978                         {
 979                         appendLink(i);
 980                         Link*           pft[]={ &m_links[i],
 981                                                                 &m_links[m_links.size()-1]};
 982                         pft[0]->m_n[0]=&m_nodes[idx[0]];
 983                         pft[0]->m_n[1]=&m_nodes[ni];
 984                         pft[1]->m_n[0]=&m_nodes[ni];
 985                         pft[1]->m_n[1]=&m_nodes[idx[1]];
 986                         }
 987                 }
 988         }
 989 /* Refine faces         */
 990 for(i=0;i<m_faces.size();++i)
 991         {
 992         const Face&     feat=m_faces[i];
 993         const int       idx[]={ int(feat.m_n[0]-nbase),
 994                                                 int(feat.m_n[1]-nbase),
 995                                                 int(feat.m_n[2]-nbase)};
 996         for(j=2,k=0;k<3;j=k++)
 997                 {
 998                 if((idx[j]<ncount)&&(idx[k]<ncount))
 999                         {
1000                         const int ni=edges(idx[j],idx[k]);
1001                         if(ni>0)
1002                                 {
1003                                 appendFace(i);
1004                                 const int       l=(k+1)%3;
1005                                 Face*           pft[]={ &m_faces[i],
1006                                                                         &m_faces[m_faces.size()-1]};
1007                                 pft[0]->m_n[0]=&m_nodes[idx[l]];
1008                                 pft[0]->m_n[1]=&m_nodes[idx[j]];
1009                                 pft[0]->m_n[2]=&m_nodes[ni];
1010                                 pft[1]->m_n[0]=&m_nodes[ni];
1011                                 pft[1]->m_n[1]=&m_nodes[idx[k]];
1012                                 pft[1]->m_n[2]=&m_nodes[idx[l]];
1013                                 appendLink(ni,idx[l],pft[0]->m_material);
1014                                 --i;break;
1015                                 }
1016                         }
1017                 }
1018         }
1019 /* Cut                          */
1020 if(cut)
1021         {
1022         btAlignedObjectArray<int>       cnodes;
1023         const int                                       pcount=ncount;
1024         int                                                     i;
1025         ncount=m_nodes.size();
1026         cnodes.resize(ncount,0);
1027         /* Nodes                */
1028         for(i=0;i<ncount;++i)
1029                 {
1030                 const btVector3 x=m_nodes[i].m_x;
1031                 if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary))
1032                         {
1033                         const btVector3 v=m_nodes[i].m_v;
1034                         btScalar                m=getMass(i);
1035                         if(m>0) { m*=0.5;m_nodes[i].m_im/=0.5; }
1036                         appendNode(x,m);
1037                         cnodes[i]=m_nodes.size()-1;
1038                         m_nodes[cnodes[i]].m_v=v;
1039                         }
1040                 }
1041         nbase=&m_nodes[0];
1042         /* Links                */
1043         for(i=0,ni=m_links.size();i<ni;++i)
1044                 {
1045                 const int               id[]={  int(m_links[i].m_n[0]-nbase),
1046                                                                 int(m_links[i].m_n[1]-nbase)};
1047                 int                             todetach=0;
1048                 if(cnodes[id[0]]&&cnodes[id[1]])
1049                         {
1050                         appendLink(i);
1051                         todetach=m_links.size()-1;
1052                         }
1053                 else
1054                         {
1055                         if((    (ifn->Eval(m_nodes[id[0]].m_x)<accurary)&&
1056                                         (ifn->Eval(m_nodes[id[1]].m_x)<accurary)))
1057                                 todetach=i;
1058                         }
1059                 if(todetach)
1060                         {
1061                         Link&   l=m_links[todetach];
1062                         for(int j=0;j<2;++j)
1063                                 {
1064                                 int cn=cnodes[int(l.m_n[j]-nbase)];
1065                                 if(cn) l.m_n[j]=&m_nodes[cn];
1066                                 }
1067                         }
1068                 }
1069         /* Faces                */
1070         for(i=0,ni=m_faces.size();i<ni;++i)
1071                 {
1072                 Node**                  n=      m_faces[i].m_n;
1073                 if(     (ifn->Eval(n[0]->m_x)<accurary)&&
1074                         (ifn->Eval(n[1]->m_x)<accurary)&&
1075                         (ifn->Eval(n[2]->m_x)<accurary))
1076                         {
1077                         for(int j=0;j<3;++j)
1078                                 {
1079                                 int cn=cnodes[int(n[j]-nbase)];
1080                                 if(cn) n[j]=&m_nodes[cn];
1081                                 }
1082                         }
1083                 }
1084         /* Clean orphans        */
1085         int                                                     nnodes=m_nodes.size();
1086         btAlignedObjectArray<int>       ranks;
1087         btAlignedObjectArray<int>       todelete;
1088         ranks.resize(nnodes,0);
1089         for(i=0,ni=m_links.size();i<ni;++i)
1090                 {
1091                 for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++;
1092                 }
1093         for(i=0,ni=m_faces.size();i<ni;++i)
1094                 {
1095                 for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
1096                 }
1097         for(i=0;i<m_links.size();++i)
1098                 {
1099                 const int       id[]={  int(m_links[i].m_n[0]-nbase),
1100                                                         int(m_links[i].m_n[1]-nbase)};
1101                 const bool      sg[]={  ranks[id[0]]==1,
1102                                                         ranks[id[1]]==1};
1103                 if(sg[0]||sg[1])
1104                         {
1105                         --ranks[id[0]];
1106                         --ranks[id[1]];
1107                         btSwap(m_links[i],m_links[m_links.size()-1]);
1108                         m_links.pop_back();--i;
1109                         }
1110                 }
1111         #if 0
1112         for(i=nnodes-1;i>=0;--i)
1113                 {
1114                 if(!ranks[i]) todelete.push_back(i);
1115                 }
1116         if(todelete.size())
1117                 {
1118                 btAlignedObjectArray<int>&      map=ranks;
1119                 for(int i=0;i<nnodes;++i) map[i]=i;
1120                 PointersToIndices(this);
1121                 for(int i=0,ni=todelete.size();i<ni;++i)
1122                         {
1123                         int             j=todelete[i];
1124                         int&    a=map[j];
1125                         int&    b=map[--nnodes];
1126                         m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
1127                         btSwap(m_nodes[a],m_nodes[b]);
1128                         j=a;a=b;b=j;
1129                         }
1130                 IndicesToPointers(this,&map[0]);
1131                 m_nodes.resize(nnodes);
1132                 }
1133         #endif
1134         }
1135 m_bUpdateRtCst=true;
1136 }
1137
1138 //
1139 bool                    btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position)
1140 {
1141 return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position));
1142 }
1143
1144 //
1145 bool                    btSoftBody::cutLink(int node0,int node1,btScalar position)
1146 {
1147 bool                    done=false;
1148 int i,ni;
1149 const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x;
1150 const btVector3 x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position);
1151 const btVector3 v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position);
1152 const btScalar  m=1;
1153 appendNode(x,m);
1154 appendNode(x,m);
1155 Node*                   pa=&m_nodes[node0];
1156 Node*                   pb=&m_nodes[node1];
1157 Node*                   pn[2]={ &m_nodes[m_nodes.size()-2],
1158                                                 &m_nodes[m_nodes.size()-1]};
1159 pn[0]->m_v=v;
1160 pn[1]->m_v=v;
1161 for(i=0,ni=m_links.size();i<ni;++i)
1162         {
1163         const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb);
1164         if(mtch!=-1)
1165                 {
1166                 appendLink(i);
1167                 Link*   pft[]={&m_links[i],&m_links[m_links.size()-1]};
1168                 pft[0]->m_n[1]=pn[mtch];
1169                 pft[1]->m_n[0]=pn[1-mtch];
1170                 done=true;
1171                 }
1172         }
1173 for(i=0,ni=m_faces.size();i<ni;++i)
1174         {
1175         for(int k=2,l=0;l<3;k=l++)
1176                 {
1177                 const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb);
1178                 if(mtch!=-1)
1179                         {
1180                         appendFace(i);
1181                         Face*   pft[]={&m_faces[i],&m_faces[m_faces.size()-1]};
1182                         pft[0]->m_n[l]=pn[mtch];
1183                         pft[1]->m_n[k]=pn[1-mtch];
1184                         appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
1185                         appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
1186                         }
1187                 }
1188         }
1189 if(!done)
1190         {
1191         m_ndbvt.remove(pn[0]->m_leaf);
1192         m_ndbvt.remove(pn[1]->m_leaf);
1193         m_nodes.pop_back();
1194         m_nodes.pop_back();
1195         }
1196 return(done);
1197 }
1198
1199 //
1200 bool                    btSoftBody::rayCast(const btVector3& org,
1201                                                                         const btVector3& dir,
1202                                                                         sRayCast& results,
1203                                                                         btScalar maxtime)
1204 {
1205 if(m_faces.size()&&m_fdbvt.empty()) initializeFaceTree();
1206 results.body    =       this;
1207 results.time    =       maxtime;
1208 results.feature =       eFeature::None;
1209 results.index   =       -1;
1210 return(rayCast(org,dir,results.time,results.feature,results.index,false)!=0);
1211 }
1212
1213 //
1214 void                    btSoftBody::setSolver(eSolverPresets::_ preset)
1215 {
1216 m_cfg.m_vsequence.clear();
1217 m_cfg.m_psequence.clear();
1218 m_cfg.m_dsequence.clear();
1219 switch(preset)
1220         {
1221         case    eSolverPresets::Positions:
1222         m_cfg.m_psequence.push_back(ePSolver::Anchors);
1223         m_cfg.m_psequence.push_back(ePSolver::RContacts);
1224         m_cfg.m_psequence.push_back(ePSolver::SContacts);
1225         m_cfg.m_psequence.push_back(ePSolver::Linear);
1226         break;
1227         case    eSolverPresets::Velocities:
1228         m_cfg.m_vsequence.push_back(eVSolver::Linear);
1229
1230         m_cfg.m_psequence.push_back(ePSolver::Anchors);
1231         m_cfg.m_psequence.push_back(ePSolver::RContacts);
1232         m_cfg.m_psequence.push_back(ePSolver::SContacts);
1233
1234         m_cfg.m_dsequence.push_back(ePSolver::Linear);
1235         break;
1236         }
1237 }
1238
1239 //
1240 void                    btSoftBody::predictMotion(btScalar dt)
1241 {
1242         int i,ni;
1243
1244         /* Update                               */
1245         if(m_bUpdateRtCst)
1246         {
1247                 m_bUpdateRtCst=false;
1248                 updateConstants();
1249                 m_fdbvt.clear();
1250                 if(m_cfg.collisions&fCollision::VF_SS)
1251                         {
1252                         initializeFaceTree();
1253                         }
1254         }
1255
1256         /* Prepare                              */
1257         m_sst.sdt               =       dt*m_cfg.timescale;
1258         m_sst.isdt              =       1/m_sst.sdt;
1259         m_sst.velmrg    =       m_sst.sdt*3;
1260         m_sst.radmrg    =       getCollisionShape()->getMargin();
1261         m_sst.updmrg    =       m_sst.radmrg*(btScalar)0.25;
1262         /* Forces                               */
1263         addVelocity(m_worldInfo->m_gravity*m_sst.sdt);
1264         applyForces();
1265         /* Integrate                    */
1266         for(i=0,ni=m_nodes.size();i<ni;++i)
1267         {
1268                 Node&   n=m_nodes[i];
1269                 n.m_q   =       n.m_x;
1270                 n.m_v   +=      n.m_f*n.m_im*m_sst.sdt;
1271                 n.m_x   +=      n.m_v*m_sst.sdt;
1272                 n.m_f   =       btVector3(0,0,0);
1273         }
1274         /* Clusters                             */
1275         updateClusters();
1276         /* Bounds                               */
1277         updateBounds();
1278         /* Nodes                                */
1279         for(i=0,ni=m_nodes.size();i<ni;++i)
1280         {
1281                 Node&   n=m_nodes[i];
1282                 m_ndbvt.update( n.m_leaf,
1283                                                 btDbvtVolume::FromCR(n.m_x,m_sst.radmrg),
1284                                                 n.m_v*m_sst.velmrg,
1285                                                 m_sst.updmrg);
1286         }
1287         /* Faces                                */
1288         if(!m_fdbvt.empty())
1289                 {
1290                 for(int i=0;i<m_faces.size();++i)
1291                         {
1292                         Face&                   f=m_faces[i];
1293                         const btVector3 v=(     f.m_n[0]->m_v+
1294                                                                 f.m_n[1]->m_v+
1295                                                                 f.m_n[2]->m_v)/3;
1296                         m_fdbvt.update( f.m_leaf,
1297                                                         VolumeOf(f,m_sst.radmrg),
1298                                                         v*m_sst.velmrg,
1299                                                         m_sst.updmrg);
1300                         }
1301                 }
1302         /* Pose                                 */
1303         updatePose();
1304         /* Match                                */
1305         if(m_pose.m_bframe&&(m_cfg.kMT>0))
1306                 {
1307                 const btMatrix3x3       posetrs=m_pose.m_rot;
1308                 for(int i=0,ni=m_nodes.size();i<ni;++i)
1309                         {
1310                         Node&   n=m_nodes[i];
1311                         if(n.m_im>0)
1312                                 {
1313                                 const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com;
1314                                 n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
1315                                 }
1316                         }
1317                 }
1318         /* Clear contacts               */
1319         m_rcontacts.resize(0);
1320         m_scontacts.resize(0);
1321         /* Optimize dbvt's              */
1322         m_ndbvt.optimizeIncremental(1);
1323         m_fdbvt.optimizeIncremental(1);
1324         m_cdbvt.optimizeIncremental(1);
1325 }
1326
1327 //
1328 void                    btSoftBody::solveConstraints()
1329 {
1330 /* Apply clusters               */
1331 applyClusters(false);
1332 /* Prepare links                */
1333
1334 int i,ni;
1335
1336 for(i=0,ni=m_links.size();i<ni;++i)
1337         {
1338         Link&   l=m_links[i];
1339         l.m_c3          =       l.m_n[1]->m_q-l.m_n[0]->m_q;
1340         l.m_c2          =       1/(l.m_c3.length2()*l.m_c0);
1341         }
1342 /* Prepare anchors              */
1343 for(i=0,ni=m_anchors.size();i<ni;++i)
1344         {
1345         Anchor&                 a=m_anchors[i];
1346         const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
1347         a.m_c0  =       ImpulseMatrix(  m_sst.sdt,
1348                                                                 a.m_node->m_im,
1349                                                                 a.m_body->getInvMass(),
1350                                                                 a.m_body->getInvInertiaTensorWorld(),
1351                                                                 ra);
1352         a.m_c1  =       ra;
1353         a.m_c2  =       m_sst.sdt*a.m_node->m_im;
1354         a.m_body->activate();
1355         }
1356 /* Solve velocities             */
1357 if(m_cfg.viterations>0)
1358         {
1359         /* Solve                        */
1360         for(int isolve=0;isolve<m_cfg.viterations;++isolve)
1361                 {
1362                 for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
1363                         {
1364                         getSolver(m_cfg.m_vsequence[iseq])(this,1);
1365                         }
1366                 }
1367         /* Update                       */
1368         for(i=0,ni=m_nodes.size();i<ni;++i)
1369                 {
1370                 Node&   n=m_nodes[i];
1371                 n.m_x   =       n.m_q+n.m_v*m_sst.sdt;
1372                 }
1373         }
1374 /* Solve positions              */
1375 if(m_cfg.piterations>0)
1376         {
1377         for(int isolve=0;isolve<m_cfg.piterations;++isolve)
1378                 {
1379                 const btScalar ti=isolve/(btScalar)m_cfg.piterations;
1380                 for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
1381                         {
1382                         getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
1383                         }
1384                 }
1385         const btScalar  vc=m_sst.isdt*(1-m_cfg.kDP);
1386         for(i=0,ni=m_nodes.size();i<ni;++i)
1387                 {
1388                 Node&   n=m_nodes[i];
1389                 n.m_v   =       (n.m_x-n.m_q)*vc;
1390                 n.m_f   =       btVector3(0,0,0);
1391                 }
1392         }
1393 /* Solve drift                  */
1394 if(m_cfg.diterations>0)
1395         {
1396         const btScalar  vcf=m_cfg.kVCF*m_sst.isdt;
1397         for(i=0,ni=m_nodes.size();i<ni;++i)
1398                 {
1399                 Node&   n=m_nodes[i];
1400                 n.m_q   =       n.m_x;
1401                 }
1402         for(int idrift=0;idrift<m_cfg.diterations;++idrift)
1403                 {
1404                 for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
1405                         {
1406                         getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
1407                         }
1408                 }
1409         for(int i=0,ni=m_nodes.size();i<ni;++i)
1410                 {
1411                 Node&   n=m_nodes[i];
1412                 n.m_v   +=      (n.m_x-n.m_q)*vcf;
1413                 }
1414         }
1415 /* Apply clusters               */
1416 dampClusters();
1417 applyClusters(true);
1418 }
1419
1420 //
1421 void                    btSoftBody::staticSolve(int iterations)
1422 {
1423 for(int isolve=0;isolve<iterations;++isolve)
1424         {
1425         for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
1426                 {
1427                 getSolver(m_cfg.m_psequence[iseq])(this,1,0);
1428                 }
1429         }
1430 }
1431
1432 //
1433 void                    btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/)
1434 {
1435 /// placeholder
1436 }
1437
1438 //
1439 void                    btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
1440 {
1441 const int       nb=bodies.size();
1442 int                     iterations=0;
1443 int i;
1444
1445 for(i=0;i<nb;++i)
1446         {
1447         iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
1448         }
1449 for(i=0;i<nb;++i)
1450         {
1451         bodies[i]->prepareClusters(iterations);
1452         }
1453 for(i=0;i<iterations;++i)
1454         {
1455         const btScalar sor=1;
1456         for(int j=0;j<nb;++j)
1457                 {
1458                 bodies[j]->solveClusters(sor);
1459                 }
1460         }
1461 for(i=0;i<nb;++i)
1462         {
1463         bodies[i]->cleanupClusters();
1464         }
1465 }
1466
1467 //
1468 void                    btSoftBody::integrateMotion()
1469 {
1470         /* Update                       */
1471         updateNormals();
1472 }
1473
1474 //
1475                                         btSoftBody::RayCaster::RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt)
1476 {
1477 o               =       org;
1478 d               =       dir;
1479 mint    =       mxt;
1480 face    =       0;
1481 tests   =       0;
1482 }
1483
1484 //
1485 void                            btSoftBody::RayCaster::Process(const btDbvtNode* leaf)
1486 {
1487 btSoftBody::Face&       f=*(btSoftBody::Face*)leaf->data;
1488 const btScalar          t=rayTriangle(  o,d,
1489                                                                         f.m_n[0]->m_x,
1490                                                                         f.m_n[1]->m_x,
1491                                                                         f.m_n[2]->m_x,
1492                                                                         mint);
1493 if((t>0)&&(t<mint)) { mint=t;face=&f; }
1494 ++tests;
1495 }
1496
1497 //
1498 btScalar                        btSoftBody::RayCaster::rayTriangle(     const btVector3& org,
1499                                                                                                                 const btVector3& dir,
1500                                                                                                                 const btVector3& a,
1501                                                                                                                 const btVector3& b,
1502                                                                                                                 const btVector3& c,
1503                                                                                                                 btScalar maxt)
1504 {
1505         static const btScalar   ceps=-SIMD_EPSILON*10;
1506         static const btScalar   teps=SIMD_EPSILON*10;
1507         const btVector3                 n=cross(b-a,c-a);
1508         const btScalar                  d=dot(a,n);
1509         const btScalar                  den=dot(dir,n);
1510         if(!btFuzzyZero(den))
1511         {
1512                 const btScalar          num=dot(org,n)-d;
1513                 const btScalar          t=-num/den;
1514                 if((t>teps)&&(t<maxt))
1515                 {
1516                         const btVector3 hit=org+dir*t;
1517                         if(     (dot(n,cross(a-hit,b-hit))>ceps)        &&
1518                                 (dot(n,cross(b-hit,c-hit))>ceps)        &&
1519                                 (dot(n,cross(c-hit,a-hit))>ceps))
1520                         {
1521                                 return(t);
1522                         }
1523                 }
1524         }
1525         return(-1);
1526 }
1527
1528 //
1529 void                            btSoftBody::pointersToIndices()
1530 {
1531 #define PTR2IDX(_p_,_b_)        reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
1532         btSoftBody::Node*       base=&m_nodes[0];
1533         int i,ni;
1534
1535         for(i=0,ni=m_nodes.size();i<ni;++i)
1536         {
1537                 if(m_nodes[i].m_leaf)
1538                         {
1539                         m_nodes[i].m_leaf->data=*(void**)&i;
1540                         }
1541         }
1542         for(i=0,ni=m_links.size();i<ni;++i)
1543         {
1544                 m_links[i].m_n[0]=PTR2IDX(m_links[i].m_n[0],base);
1545                 m_links[i].m_n[1]=PTR2IDX(m_links[i].m_n[1],base);
1546         }
1547         for(i=0,ni=m_faces.size();i<ni;++i)
1548         {
1549                 m_faces[i].m_n[0]=PTR2IDX(m_faces[i].m_n[0],base);
1550                 m_faces[i].m_n[1]=PTR2IDX(m_faces[i].m_n[1],base);
1551                 m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base);
1552                 if(m_faces[i].m_leaf)
1553                         {
1554                         m_faces[i].m_leaf->data=*(void**)&i;
1555                         }
1556         }
1557         for(i=0,ni=m_anchors.size();i<ni;++i)
1558                 {
1559                 m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base);
1560                 }
1561         for(i=0,ni=m_notes.size();i<ni;++i)
1562                 {
1563                 for(int j=0;j<m_notes[i].m_rank;++j)
1564                         {
1565                         m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base);
1566                         }
1567                 }
1568 #undef  PTR2IDX
1569 }
1570
1571 //
1572 void                            btSoftBody::indicesToPointers(const int* map)
1573 {
1574 #define IDX2PTR(_p_,_b_)        map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]):     \
1575                                                                 (&(_b_)[(((char*)_p_)-(char*)0)])
1576         btSoftBody::Node*       base=&m_nodes[0];
1577         int i,ni;
1578
1579         for(i=0,ni=m_nodes.size();i<ni;++i)
1580         {
1581                 if(m_nodes[i].m_leaf)
1582                         {
1583                         m_nodes[i].m_leaf->data=&m_nodes[i];
1584                         }
1585         }
1586         for(i=0,ni=m_links.size();i<ni;++i)
1587         {
1588                 m_links[i].m_n[0]=IDX2PTR(m_links[i].m_n[0],base);
1589                 m_links[i].m_n[1]=IDX2PTR(m_links[i].m_n[1],base);
1590         }
1591         for(i=0,ni=m_faces.size();i<ni;++i)
1592         {
1593                 m_faces[i].m_n[0]=IDX2PTR(m_faces[i].m_n[0],base);
1594                 m_faces[i].m_n[1]=IDX2PTR(m_faces[i].m_n[1],base);
1595                 m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base);
1596                 if(m_faces[i].m_leaf)
1597                         {
1598                         m_faces[i].m_leaf->data=&m_faces[i];
1599                         }
1600         }
1601         for(i=0,ni=m_anchors.size();i<ni;++i)
1602                 {
1603                 m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base);
1604                 }
1605         for(i=0,ni=m_notes.size();i<ni;++i)
1606                 {
1607                 for(int j=0;j<m_notes[i].m_rank;++j)
1608                         {
1609                         m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base);
1610                         }
1611                 }
1612 #undef  IDX2PTR
1613 }
1614
1615 //
1616 int                                     btSoftBody::rayCast(const btVector3& org,const btVector3& dir,
1617                                                                                 btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const
1618 {
1619         int     cnt=0;
1620         if(bcountonly||m_fdbvt.empty())
1621                 {/* Full search */
1622                 for(int i=0,ni=m_faces.size();i<ni;++i)
1623                         {
1624                         const btSoftBody::Face& f=m_faces[i];
1625                         const btScalar                  t=RayCaster::rayTriangle(       org,dir,
1626                                                                                                                                 f.m_n[0]->m_x,
1627                                                                                                                                 f.m_n[1]->m_x,
1628                                                                                                                                 f.m_n[2]->m_x,
1629                                                                                                                                 mint);
1630                         if(t>0)
1631                                 {
1632                                 ++cnt;
1633                                 if(!bcountonly)
1634                                         {
1635                                         feature=btSoftBody::eFeature::Face;
1636                                         index=i;
1637                                         mint=t;
1638                                         }
1639                                 }
1640                         }
1641                 }
1642                 else
1643                 {/* Use dbvt    */
1644                 RayCaster       collider(org,dir,mint);
1645                 btDbvt::collideRAY(m_fdbvt.m_root,org,dir,collider);
1646                 if(collider.face)
1647                         {
1648                         mint=collider.mint;
1649                         feature=btSoftBody::eFeature::Face;
1650                         index=(int)(collider.face-&m_faces[0]);
1651                         cnt=1;
1652                         }
1653                 }
1654         return(cnt);
1655 }
1656
1657 //
1658 void                    btSoftBody::initializeFaceTree()
1659 {
1660 m_fdbvt.clear();
1661 for(int i=0;i<m_faces.size();++i)
1662         {
1663         Face&   f=m_faces[i];
1664         f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
1665         }
1666 }
1667
1668 //
1669 btVector3               btSoftBody::evaluateCom() const
1670 {
1671 btVector3       com(0,0,0);
1672 if(m_pose.m_bframe)
1673         {
1674         for(int i=0,ni=m_nodes.size();i<ni;++i)
1675                 {
1676                 com+=m_nodes[i].m_x*m_pose.m_wgh[i];
1677                 }
1678         }
1679         return(com);
1680 }
1681
1682 //
1683 bool                            btSoftBody::checkContact(       btRigidBody* prb,
1684                                                                                                 const btVector3& x,
1685                                                                                                 btScalar margin,
1686                                                                                                 btSoftBody::sCti& cti) const
1687 {
1688         btVector3                       nrm;
1689         btCollisionShape*       shp=prb->getCollisionShape();
1690         const btTransform&      wtr=prb->getInterpolationWorldTransform();
1691         btScalar                        dst=m_worldInfo->m_sparsesdf.Evaluate(  wtr.invXform(x),
1692                                                                                                                                 shp,
1693                                                                                                                                 nrm,
1694                                                                                                                                 margin);
1695         if(dst<0)
1696         {
1697                 cti.m_body              =       prb;
1698                 cti.m_normal    =       wtr.getBasis()*nrm;
1699                 cti.m_offset    =       -dot(   cti.m_normal,
1700                                                                         x-cti.m_normal*dst);
1701                 return(true);
1702         }
1703         return(false);
1704 }
1705
1706 //
1707 void                                    btSoftBody::updateNormals()
1708 {
1709         const btVector3 zv(0,0,0);
1710         int i,ni;
1711
1712         for(i=0,ni=m_nodes.size();i<ni;++i)
1713         {
1714                 m_nodes[i].m_n=zv;
1715         }
1716         for(i=0,ni=m_faces.size();i<ni;++i)
1717         {
1718                 btSoftBody::Face&       f=m_faces[i];
1719                 const btVector3         n=cross(f.m_n[1]->m_x-f.m_n[0]->m_x,
1720                                                                         f.m_n[2]->m_x-f.m_n[0]->m_x);
1721                 f.m_normal=n.normalized();
1722                 f.m_n[0]->m_n+=n;
1723                 f.m_n[1]->m_n+=n;
1724                 f.m_n[2]->m_n+=n;
1725         }
1726         for(i=0,ni=m_nodes.size();i<ni;++i)
1727         {
1728                 btScalar len = m_nodes[i].m_n.length();
1729                 if (len>SIMD_EPSILON)
1730                         m_nodes[i].m_n /= len;
1731         }
1732 }
1733
1734 //
1735 void                                    btSoftBody::updateBounds()
1736 {
1737         if(m_ndbvt.m_root)
1738                 {
1739                 const btVector3&        mins=m_ndbvt.m_root->volume.Mins();
1740                 const btVector3&        maxs=m_ndbvt.m_root->volume.Maxs();
1741                 const btScalar          csm=getCollisionShape()->getMargin();
1742                 const btVector3         mrg=btVector3(  csm,
1743                                                                                         csm,
1744                                                                                         csm)*1; // ??? to investigate...
1745                 m_bounds[0]=mins-mrg;
1746                 m_bounds[1]=maxs+mrg;
1747                         if(0!=getBroadphaseHandle())
1748                         {
1749                                 m_worldInfo->m_broadphase->setAabb(     getBroadphaseHandle(),
1750                                                                                                         m_bounds[0],
1751                                                                                                         m_bounds[1],
1752                                                                                                         m_worldInfo->m_dispatcher);
1753                         }
1754                 }
1755                 else
1756                 {
1757                 m_bounds[0]=
1758                 m_bounds[1]=btVector3(0,0,0);
1759                 }
1760 }
1761
1762
1763 //
1764 void                                    btSoftBody::updatePose()
1765 {
1766         if(m_pose.m_bframe)
1767         {
1768                 btSoftBody::Pose&       pose=m_pose;
1769                 const btVector3         com=evaluateCom();
1770                 /* Com                  */
1771                 pose.m_com      =       com;
1772                 /* Rotation             */
1773                 btMatrix3x3             Apq;
1774                 const btScalar  eps=SIMD_EPSILON;
1775                 Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0);
1776                 Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3);
1777                 for(int i=0,ni=m_nodes.size();i<ni;++i)
1778                 {
1779                         const btVector3         a=pose.m_wgh[i]*(m_nodes[i].m_x-com);
1780                         const btVector3&        b=pose.m_pos[i];
1781                         Apq[0]+=a.x()*b;
1782                         Apq[1]+=a.y()*b;
1783                         Apq[2]+=a.z()*b;
1784                 }
1785                 btMatrix3x3             r,s;
1786                 PolarDecompose(Apq,r,s);
1787                 pose.m_rot=r;
1788                 pose.m_scl=pose.m_aqq*r.transpose()*Apq;
1789                 if(m_cfg.maxvolume>1)
1790                         {
1791                         const btScalar  idet=Clamp<btScalar>(   1/pose.m_scl.determinant(),
1792                                                                                                         1,m_cfg.maxvolume);
1793                         pose.m_scl=Mul(pose.m_scl,idet);
1794                         }
1795
1796         }
1797 }
1798
1799 //
1800 void                            btSoftBody::updateConstants()
1801 {
1802         int i,ni;
1803
1804         /* Links                */
1805         for(i=0,ni=m_links.size();i<ni;++i)
1806         {
1807                 Link&           l=m_links[i];
1808                 Material&       m=*l.m_material;
1809                 l.m_rl  =       (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
1810                 l.m_c0  =       (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST;
1811                 l.m_c1  =       l.m_rl*l.m_rl;
1812         }
1813         /* Faces                */
1814         for(i=0,ni=m_faces.size();i<ni;++i)
1815         {
1816                 Face&           f=m_faces[i];
1817                 f.m_ra  =       AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
1818         }
1819         /* Area's               */
1820         btAlignedObjectArray<int>       counts;
1821         counts.resize(m_nodes.size(),0);
1822         for(i=0,ni=m_nodes.size();i<ni;++i)
1823         {
1824                 m_nodes[i].m_area       =       0;
1825         }
1826         for(i=0,ni=m_faces.size();i<ni;++i)
1827         {
1828                 btSoftBody::Face&       f=m_faces[i];
1829                 for(int j=0;j<3;++j)
1830                 {
1831                         const int index=(int)(f.m_n[j]-&m_nodes[0]);
1832                         counts[index]++;
1833                         f.m_n[j]->m_area+=btFabs(f.m_ra);
1834                 }
1835         }
1836         for(i=0,ni=m_nodes.size();i<ni;++i)
1837         {
1838                 if(counts[i]>0)
1839                         m_nodes[i].m_area/=(btScalar)counts[i];
1840                 else
1841                         m_nodes[i].m_area=0;
1842         }
1843 }
1844
1845 //
1846 void                                    btSoftBody::initializeClusters()
1847 {
1848         int i;
1849
1850 for( i=0;i<m_clusters.size();++i)
1851         {
1852         Cluster&        c=*m_clusters[i];
1853         c.m_imass=0;
1854         c.m_masses.resize(c.m_nodes.size());
1855         for(int j=0;j<c.m_nodes.size();++j)
1856                 {
1857                 c.m_masses[j]   =       c.m_nodes[j]->m_im>0?1/c.m_nodes[j]->m_im:0;
1858                 c.m_imass               +=      c.m_masses[j];
1859                 }
1860         c.m_imass               =       1/c.m_imass;
1861         c.m_com                 =       btSoftBody::clusterCom(&c);
1862         c.m_lv                  =       btVector3(0,0,0);
1863         c.m_av                  =       btVector3(0,0,0);
1864         c.m_leaf                =       0;
1865         /* Inertia      */
1866         btMatrix3x3&    ii=c.m_locii;
1867         ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
1868         {
1869                 int i,ni;
1870
1871                 for(i=0,ni=c.m_nodes.size();i<ni;++i)
1872                 {
1873                         const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
1874                         const btVector3 q=k*k;
1875                         const btScalar  m=c.m_masses[i];
1876                         ii[0][0]        +=      m*(q[1]+q[2]);
1877                         ii[1][1]        +=      m*(q[0]+q[2]);
1878                         ii[2][2]        +=      m*(q[0]+q[1]);
1879                         ii[0][1]        -=      m*k[0]*k[1];
1880                         ii[0][2]        -=      m*k[0]*k[2];
1881                         ii[1][2]        -=      m*k[1]*k[2];
1882                 }
1883         }
1884         ii[1][0]=ii[0][1];
1885         ii[2][0]=ii[0][2];
1886         ii[2][1]=ii[1][2];
1887         ii=ii.inverse();
1888         /* Frame        */
1889         c.m_framexform.setIdentity();
1890         c.m_framexform.setOrigin(c.m_com);
1891         c.m_framerefs.resize(c.m_nodes.size());
1892         {
1893                 int i;
1894                 for(i=0;i<c.m_framerefs.size();++i)
1895                         {
1896                         c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
1897                         }
1898                 }
1899         }
1900 }
1901
1902 //
1903 void                                    btSoftBody::updateClusters()
1904 {
1905 BT_PROFILE("UpdateClusters");
1906 int i;
1907
1908 for(i=0;i<m_clusters.size();++i)
1909         {
1910         btSoftBody::Cluster&    c=*m_clusters[i];
1911         const int                               n=c.m_nodes.size();
1912         const btScalar                  invn=1/(btScalar)n;
1913         if(n)
1914                 {
1915                 /* Frame                                */
1916                 const btScalar  eps=btScalar(0.0001);
1917                 btMatrix3x3             m,r,s;
1918                 m[0]=m[1]=m[2]=btVector3(0,0,0);
1919                 m[0][0]=eps*1;
1920                 m[1][1]=eps*2;
1921                 m[2][2]=eps*3;
1922                 c.m_com=clusterCom(&c);
1923                 for(int i=0;i<c.m_nodes.size();++i)
1924                         {
1925                         const btVector3         a=c.m_nodes[i]->m_x-c.m_com;
1926                         const btVector3&        b=c.m_framerefs[i];
1927                         m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
1928                         }
1929                 PolarDecompose(m,r,s);
1930                 c.m_framexform.setOrigin(c.m_com);
1931                 c.m_framexform.setBasis(r);
1932                 /* Inertia                      */
1933                 #if 1/* Constant        */
1934                 c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
1935                 #else
1936                         #if 0/* Sphere  */
1937                         const btScalar  rk=(2*c.m_extents.length2())/(5*c.m_imass);
1938                         const btVector3 inertia(rk,rk,rk);
1939                         const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
1940                                                                 btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
1941                                                                 btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
1942
1943                         c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
1944                         #else/* Actual  */
1945                         c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0);
1946                         for(int i=0;i<n;++i)
1947                                 {
1948                                 const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
1949                                 const btVector3         q=k*k;
1950                                 const btScalar          m=1/c.m_nodes[i]->m_im;
1951                                 c.m_invwi[0][0] +=      m*(q[1]+q[2]);
1952                                 c.m_invwi[1][1] +=      m*(q[0]+q[2]);
1953                                 c.m_invwi[2][2] +=      m*(q[0]+q[1]);
1954                                 c.m_invwi[0][1] -=      m*k[0]*k[1];
1955                                 c.m_invwi[0][2] -=      m*k[0]*k[2];
1956                                 c.m_invwi[1][2] -=      m*k[1]*k[2];
1957                                 }
1958                         c.m_invwi[1][0]=c.m_invwi[0][1];
1959                         c.m_invwi[2][0]=c.m_invwi[0][2];
1960                         c.m_invwi[2][1]=c.m_invwi[1][2];
1961                         c.m_invwi=c.m_invwi.inverse();
1962                         #endif
1963                 #endif
1964                 /* Velocities                   */
1965                 c.m_lv=btVector3(0,0,0);
1966                 c.m_av=btVector3(0,0,0);
1967                 {
1968                         int i;
1969
1970                         for(i=0;i<n;++i)
1971                         {
1972                                 const btVector3 v=c.m_nodes[i]->m_v*c.m_masses[i];
1973                                 c.m_lv  +=      v;
1974                                 c.m_av  +=      cross(c.m_nodes[i]->m_x-c.m_com,v);
1975                         }
1976                 }
1977                 c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
1978                 c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
1979                 c.m_vimpulses[0]        =
1980                 c.m_vimpulses[1]        = btVector3(0,0,0);
1981                 c.m_dimpulses[0]        =
1982                 c.m_dimpulses[1]        = btVector3(0,0,0);
1983                 c.m_nvimpulses          = 0;
1984                 c.m_ndimpulses          = 0;
1985                 /* Matching                             */
1986                 if(c.m_matching>0)
1987                         {
1988                         for(int j=0;j<c.m_nodes.size();++j)
1989                                 {
1990                                 Node&                   n=*c.m_nodes[j];
1991                                 const btVector3 x=c.m_framexform*c.m_framerefs[j];
1992                                 n.m_x=Lerp(n.m_x,x,c.m_matching);
1993                                 }
1994                         }
1995                 /* Dbvt                                 */
1996                 if(c.m_collide)
1997                         {
1998                         btVector3       mi=c.m_nodes[0]->m_x;
1999                         btVector3       mx=mi;
2000                         for(int j=1;j<n;++j)
2001                                 {
2002                                 mi.setMin(c.m_nodes[j]->m_x);
2003                                 mx.setMax(c.m_nodes[j]->m_x);
2004                                 }
2005                         const btDbvtVolume      bounds=btDbvtVolume::FromMM(mi,mx);
2006                         if(c.m_leaf)
2007                                 m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
2008                                 else
2009                                 c.m_leaf=m_cdbvt.insert(bounds,&c);
2010                         }
2011                 }
2012         }
2013 }
2014
2015 //
2016 void                                    btSoftBody::cleanupClusters()
2017 {
2018 for(int i=0;i<m_joints.size();++i)
2019         {
2020         m_joints[i]->Terminate(m_sst.sdt);
2021         if(m_joints[i]->m_delete)
2022                 {
2023                 btAlignedFree(m_joints[i]);
2024                 m_joints.remove(m_joints[i--]);
2025                 }
2026         }
2027 }
2028
2029 //
2030 void                                    btSoftBody::prepareClusters(int iterations)
2031 {
2032 for(int i=0;i<m_joints.size();++i)
2033         {
2034         m_joints[i]->Prepare(m_sst.sdt,iterations);
2035         }
2036 }
2037
2038
2039 //
2040 void                                    btSoftBody::solveClusters(btScalar sor)
2041 {
2042 for(int i=0,ni=m_joints.size();i<ni;++i)
2043         {
2044         m_joints[i]->Solve(m_sst.sdt,sor);
2045         }
2046 }
2047
2048 //
2049 void                                    btSoftBody::applyClusters(bool drift)
2050 {
2051 BT_PROFILE("ApplyClusters");
2052 const btScalar                                  f0=m_sst.sdt;
2053 const btScalar                                  f1=f0/2;
2054 btAlignedObjectArray<btVector3> deltas;
2055 btAlignedObjectArray<btScalar>  weights;
2056 deltas.resize(m_nodes.size(),btVector3(0,0,0));
2057 weights.resize(m_nodes.size(),0);
2058 int i;
2059
2060 if(drift)
2061         {
2062         for(i=0;i<m_clusters.size();++i)
2063                 {
2064                 Cluster&        c=*m_clusters[i];
2065                 if(c.m_ndimpulses)
2066                         {
2067                         c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
2068                         c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
2069                         }
2070                 }
2071         }
2072 for(i=0;i<m_clusters.size();++i)
2073         {
2074         Cluster&        c=*m_clusters[i];
2075         if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
2076                 {
2077                 const btVector3         v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
2078                 const btVector3         w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
2079                 for(int j=0;j<c.m_nodes.size();++j)
2080                         {
2081                         const int                       idx=int(c.m_nodes[j]-&m_nodes[0]);
2082                         const btVector3&        x=c.m_nodes[j]->m_x;
2083                         const btScalar          q=c.m_masses[j];
2084                         deltas[idx]             +=      (v+cross(w,x-c.m_com))*q;
2085                         weights[idx]    +=      q;
2086                         }
2087                 }
2088         }
2089         for(i=0;i<deltas.size();++i)
2090         {
2091                 if(weights[i]>0) m_nodes[i].m_x+=deltas[i]/weights[i];
2092         }
2093 }
2094
2095 //
2096 void                                    btSoftBody::dampClusters()
2097 {
2098         int i;
2099
2100 for(i=0;i<m_clusters.size();++i)
2101         {
2102         Cluster&        c=*m_clusters[i];
2103         if(c.m_ndamping>0)
2104                 {
2105                 for(int j=0;j<c.m_nodes.size();++j)
2106                         {
2107                         Node&                   n=*c.m_nodes[j];
2108                         if(n.m_im>0)
2109                                 {
2110                                 const btVector3 vx=c.m_lv+cross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
2111                                 n.m_v   +=      c.m_ndamping*(vx-n.m_v);
2112                                 }
2113                         }
2114                 }
2115         }
2116 }
2117
2118 //
2119 void                            btSoftBody::Joint::Prepare(btScalar dt,int)
2120 {
2121 m_bodies[0].activate();
2122 m_bodies[1].activate();
2123 }
2124
2125 //
2126 void                            btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
2127 {
2128 static const btScalar   maxdrift=4;
2129 Joint::Prepare(dt,iterations);
2130 m_rpos[0]               =       m_bodies[0].xform()*m_refs[0];
2131 m_rpos[1]               =       m_bodies[1].xform()*m_refs[1];
2132 m_drift                 =       Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
2133 m_rpos[0]               -=      m_bodies[0].xform().getOrigin();
2134 m_rpos[1]               -=      m_bodies[1].xform().getOrigin();
2135 m_massmatrix    =       ImpulseMatrix(  m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
2136                                                                         m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
2137 if(m_split>0)
2138         {
2139         m_sdrift        =       m_massmatrix*(m_drift*m_split);
2140         m_drift         *=      1-m_split;
2141         }
2142 m_drift /=(btScalar)iterations;
2143 }
2144
2145 //
2146 void                            btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
2147 {
2148 const btVector3         va=m_bodies[0].velocity(m_rpos[0]);
2149 const btVector3         vb=m_bodies[1].velocity(m_rpos[1]);
2150 const btVector3         vr=va-vb;
2151 btSoftBody::Impulse     impulse;
2152 impulse.m_asVelocity    =       1;
2153 impulse.m_velocity              =       m_massmatrix*(m_drift+vr*m_cfm)*sor;
2154 m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
2155 m_bodies[1].applyImpulse( impulse,m_rpos[1]);
2156 }
2157
2158 //
2159 void                            btSoftBody::LJoint::Terminate(btScalar dt)
2160 {
2161 if(m_split>0)
2162         {
2163         m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
2164         m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
2165         }
2166 }
2167
2168 //
2169 void                            btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
2170 {
2171 static const btScalar   maxdrift=SIMD_PI/16;
2172 m_icontrol->Prepare(this);
2173 Joint::Prepare(dt,iterations);
2174 m_axis[0]       =       m_bodies[0].xform().getBasis()*m_refs[0];
2175 m_axis[1]       =       m_bodies[1].xform().getBasis()*m_refs[1];
2176 m_drift         =       NormalizeAny(cross(m_axis[1],m_axis[0]));
2177 m_drift         *=      btMin(maxdrift,btAcos(Clamp<btScalar>(dot(m_axis[0],m_axis[1]),-1,+1)));
2178 m_drift         *=      m_erp/dt;
2179 m_massmatrix=   AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
2180 if(m_split>0)
2181         {
2182         m_sdrift        =       m_massmatrix*(m_drift*m_split);
2183         m_drift         *=      1-m_split;
2184         }
2185 m_drift /=(btScalar)iterations;
2186 }
2187
2188 //
2189 void                            btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
2190 {
2191 const btVector3         va=m_bodies[0].angularVelocity();
2192 const btVector3         vb=m_bodies[1].angularVelocity();
2193 const btVector3         vr=va-vb;
2194 const btScalar          sp=dot(vr,m_axis[0]);
2195 const btVector3         vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
2196 btSoftBody::Impulse     impulse;
2197 impulse.m_asVelocity    =       1;
2198 impulse.m_velocity              =       m_massmatrix*(m_drift+vc*m_cfm)*sor;
2199 m_bodies[0].applyAImpulse(-impulse);
2200 m_bodies[1].applyAImpulse( impulse);
2201 }
2202
2203 //
2204 void                            btSoftBody::AJoint::Terminate(btScalar dt)
2205 {
2206 if(m_split>0)
2207         {
2208         m_bodies[0].applyDAImpulse(-m_sdrift);
2209         m_bodies[1].applyDAImpulse( m_sdrift);
2210         }
2211 }
2212
2213 //
2214 void                            btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
2215 {
2216 Joint::Prepare(dt,iterations);
2217 const bool      dodrift=(m_life==0);
2218 m_delete=(++m_life)>m_maxlife;
2219 if(dodrift)
2220         {
2221         m_drift=m_drift*m_erp/dt;
2222         if(m_split>0)
2223                 {
2224                 m_sdrift        =       m_massmatrix*(m_drift*m_split);
2225                 m_drift         *=      1-m_split;
2226                 }
2227         m_drift/=(btScalar)iterations;
2228         }
2229         else
2230         {
2231         m_drift=m_sdrift=btVector3(0,0,0);
2232         }
2233 }
2234
2235 //
2236 void                            btSoftBody::CJoint::Solve(btScalar dt,btScalar sor)
2237 {
2238 const btVector3         va=m_bodies[0].velocity(m_rpos[0]);
2239 const btVector3         vb=m_bodies[1].velocity(m_rpos[1]);
2240 const btVector3         vrel=va-vb;
2241 const btScalar          rvac=dot(vrel,m_normal);
2242 btSoftBody::Impulse     impulse;
2243 impulse.m_asVelocity    =       1;
2244 impulse.m_velocity              =       m_drift;
2245 if(rvac<0)
2246         {
2247         const btVector3 iv=m_normal*rvac;
2248         const btVector3 fv=vrel-iv;
2249         impulse.m_velocity      +=      iv+fv*m_friction;
2250         }
2251 impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;
2252 m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
2253 m_bodies[1].applyImpulse( impulse,m_rpos[1]);
2254 }
2255
2256 //
2257 void                            btSoftBody::CJoint::Terminate(btScalar dt)
2258 {
2259 if(m_split>0)
2260         {
2261         m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
2262         m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
2263         }
2264 }
2265
2266 //
2267 void                            btSoftBody::applyForces()
2268 {
2269         BT_PROFILE("SoftBody applyForces");
2270         const btScalar                                  dt=m_sst.sdt;
2271         const btScalar                                  kLF=m_cfg.kLF;
2272         const btScalar                                  kDG=m_cfg.kDG;
2273         const btScalar                                  kPR=m_cfg.kPR;
2274         const btScalar                                  kVC=m_cfg.kVC;
2275         const bool                                              as_lift=kLF>0;
2276         const bool                                              as_drag=kDG>0;
2277         const bool                                              as_pressure=kPR!=0;
2278         const bool                                              as_volume=kVC>0;
2279         const bool                                              as_aero=        as_lift         ||
2280                                                                                                 as_drag         ;
2281         const bool                                              as_vaero=       as_aero         &&
2282                                                                                                 (m_cfg.aeromodel<btSoftBody::eAeroModel::F_TwoSided);
2283         const bool                                              as_faero=       as_aero         &&
2284                                                                                                 (m_cfg.aeromodel>=btSoftBody::eAeroModel::F_TwoSided);
2285         const bool                                              use_medium=     as_aero;
2286         const bool                                              use_volume=     as_pressure     ||
2287                                                                                                 as_volume       ;
2288         btScalar                                                volume=0;
2289         btScalar                                                ivolumetp=0;
2290         btScalar                                                dvolumetv=0;
2291         btSoftBody::sMedium     medium;
2292         if(use_volume)
2293         {
2294                 volume          =       getVolume();
2295                 ivolumetp       =       1/btFabs(volume)*kPR;
2296                 dvolumetv       =       (m_pose.m_volume-volume)*kVC;
2297         }
2298         /* Per vertex forces                    */
2299         int i,ni;
2300
2301         for(i=0,ni=m_nodes.size();i<ni;++i)
2302         {
2303                 btSoftBody::Node&       n=m_nodes[i];
2304                 if(n.m_im>0)
2305                 {
2306                         if(use_medium)
2307                         {
2308                                 EvaluateMedium(m_worldInfo,n.m_x,medium);
2309                                 /* Aerodynamics                 */
2310                                 if(as_vaero)
2311                                 {
2312                                         const btVector3 rel_v=n.m_v-medium.m_velocity;
2313                                         const btScalar  rel_v2=rel_v.length2();
2314                                         if(rel_v2>SIMD_EPSILON)
2315                                         {
2316                                                 btVector3       nrm=n.m_n;
2317                                                 /* Setup normal         */
2318                                                 switch(m_cfg.aeromodel)
2319                                                 {
2320                                                 case    btSoftBody::eAeroModel::V_Point:
2321                                                         nrm=NormalizeAny(rel_v);break;
2322                                                 case    btSoftBody::eAeroModel::V_TwoSided:
2323                                                         nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break;
2324                                                 }
2325                                                 const btScalar  dvn=dot(rel_v,nrm);
2326                                                 /* Compute forces       */
2327                                                 if(dvn>0)
2328                                                 {
2329                                                         btVector3               force(0,0,0);
2330                                                         const btScalar  c0      =       n.m_area*dvn*rel_v2/2;
2331                                                         const btScalar  c1      =       c0*medium.m_density;
2332                                                         force   +=      nrm*(-c1*kLF);
2333                                                         force   +=      rel_v.normalized()*(-c1*kDG);
2334                                                         ApplyClampedForce(n,force,dt);
2335                                                 }
2336                                         }
2337                                 }
2338                         }
2339                         /* Pressure                             */
2340                         if(as_pressure)
2341                         {
2342                                 n.m_f   +=      n.m_n*(n.m_area*ivolumetp);
2343                         }
2344                         /* Volume                               */
2345                         if(as_volume)
2346                         {
2347                                 n.m_f   +=      n.m_n*(n.m_area*dvolumetv);
2348                         }
2349                 }
2350         }
2351         /* Per face forces                              */
2352         for(i=0,ni=m_faces.size();i<ni;++i)
2353         {
2354                 btSoftBody::Face&       f=m_faces[i];
2355                 if(as_faero)
2356                 {
2357                         const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3;
2358                         const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3;
2359                         EvaluateMedium(m_worldInfo,x,medium);
2360                         const btVector3 rel_v=v-medium.m_velocity;
2361                         const btScalar  rel_v2=rel_v.length2();
2362                         if(rel_v2>SIMD_EPSILON)
2363                         {
2364                                 btVector3       nrm=f.m_normal;
2365                                 /* Setup normal         */
2366                                 switch(m_cfg.aeromodel)
2367                                 {
2368                                 case    btSoftBody::eAeroModel::F_TwoSided:
2369                                         nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break;
2370                                 }
2371                                 const btScalar  dvn=dot(rel_v,nrm);
2372                                 /* Compute forces       */
2373                                 if(dvn>0)
2374                                 {
2375                                         btVector3               force(0,0,0);
2376                                         const btScalar  c0      =       f.m_ra*dvn*rel_v2;
2377                                         const btScalar  c1      =       c0*medium.m_density;
2378                                         force   +=      nrm*(-c1*kLF);
2379                                         force   +=      rel_v.normalized()*(-c1*kDG);
2380                                         force   /=      3;
2381                                         for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt);
2382                                 }
2383                         }
2384                 }
2385         }
2386 }
2387
2388 //
2389 void                            btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti)
2390 {
2391         const btScalar  kAHR=psb->m_cfg.kAHR*kst;
2392         const btScalar  dt=psb->m_sst.sdt;
2393         for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
2394         {
2395                 const Anchor&           a=psb->m_anchors[i];
2396                 const btTransform&      t=a.m_body->getInterpolationWorldTransform();
2397                 Node&                           n=*a.m_node;
2398                 const btVector3         wa=t*a.m_local;
2399                 const btVector3         va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt;
2400                 const btVector3         vb=n.m_x-n.m_q;
2401                 const btVector3         vr=(va-vb)+(wa-n.m_x)*kAHR;
2402                 const btVector3         impulse=a.m_c0*vr;
2403                 n.m_x+=impulse*a.m_c2;
2404                 a.m_body->applyImpulse(-impulse,a.m_c1);
2405         }
2406 }
2407
2408 //
2409 void                            btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti)
2410 {
2411         const btScalar  dt=psb->m_sst.sdt;
2412         const btScalar  mrg=psb->getCollisionShape()->getMargin();
2413         for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
2414         {
2415                 const RContact&         c=psb->m_rcontacts[i];
2416                 const sCti&                     cti=c.m_cti;
2417                 const btVector3         va=cti.m_body->getVelocityInLocalPoint(c.m_c1)*dt;
2418                 const btVector3         vb=c.m_node->m_x-c.m_node->m_q;
2419                 const btVector3         vr=vb-va;
2420                 const btScalar          dn=dot(vr,cti.m_normal);
2421                 if(dn<=SIMD_EPSILON)
2422                 {
2423                         const btScalar          dp=btMin(dot(c.m_node->m_x,cti.m_normal)+cti.m_offset,mrg);
2424                         const btVector3         fv=vr-cti.m_normal*dn;
2425                         const btVector3         impulse=c.m_c0*((vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4))*kst);
2426                         c.m_node->m_x-=impulse*c.m_c2;
2427                         c.m_cti.m_body->applyImpulse(impulse,c.m_c1);
2428                 }
2429         }
2430 }
2431
2432 //
2433 void                            btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti)
2434 {
2435 for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
2436         {
2437         const SContact&         c=psb->m_scontacts[i];
2438         const btVector3&        nr=c.m_normal;
2439         Node&                           n=*c.m_node;
2440         Face&                           f=*c.m_face;
2441         const btVector3         p=BaryEval(     f.m_n[0]->m_x,
2442                                                                         f.m_n[1]->m_x,
2443                                                                         f.m_n[2]->m_x,
2444                                                                         c.m_weights);
2445         const btVector3         q=BaryEval(     f.m_n[0]->m_q,
2446                                                                         f.m_n[1]->m_q,
2447                                                                         f.m_n[2]->m_q,
2448                                                                         c.m_weights);
2449         const btVector3         vr=(n.m_x-n.m_q)-(p-q);
2450         btVector3                       corr(0,0,0);
2451         if(dot(vr,nr)<0)
2452                 {
2453                 const btScalar  j=c.m_margin-(dot(nr,n.m_x)-dot(nr,p));
2454                 corr+=c.m_normal*j;
2455                 }
2456         corr                    -=      ProjectOnPlane(vr,nr)*c.m_friction;
2457         n.m_x                   +=      corr*c.m_cfm[0];
2458         f.m_n[0]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.x());
2459         f.m_n[1]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.y());
2460         f.m_n[2]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.z());
2461         }
2462 }
2463
2464 //
2465 void                            btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti)
2466 {
2467         for(int i=0,ni=psb->m_links.size();i<ni;++i)
2468         {
2469                 Link&   l=psb->m_links[i];
2470                 if(l.m_c0>0)
2471                 {
2472                         Node&                   a=*l.m_n[0];
2473                         Node&                   b=*l.m_n[1];
2474                         const btVector3 del=b.m_x-a.m_x;
2475                         const btScalar  len=del.length2();
2476                         const btScalar  k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst;
2477                         const btScalar  t=k*a.m_im;
2478                         a.m_x-=del*(k*a.m_im);
2479                         b.m_x+=del*(k*b.m_im);
2480                 }
2481         }
2482 }
2483
2484 //
2485 void                            btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst)
2486 {
2487 for(int i=0,ni=psb->m_links.size();i<ni;++i)
2488         {
2489         Link&                   l=psb->m_links[i];
2490         Node**                  n=l.m_n;
2491         const btScalar  j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
2492         n[0]->m_v+=     l.m_c3*(j*n[0]->m_im);
2493         n[1]->m_v-=     l.m_c3*(j*n[1]->m_im);
2494         }
2495 }
2496
2497 //
2498 btSoftBody::psolver_t   btSoftBody::getSolver(ePSolver::_ solver)
2499 {
2500 switch(solver)
2501         {
2502         case    ePSolver::Anchors:              return(&btSoftBody::PSolve_Anchors);
2503         case    ePSolver::Linear:               return(&btSoftBody::PSolve_Links);
2504         case    ePSolver::RContacts:    return(&btSoftBody::PSolve_RContacts);
2505         case    ePSolver::SContacts:    return(&btSoftBody::PSolve_SContacts);
2506         }
2507 return(0);
2508 }
2509
2510 //
2511 btSoftBody::vsolver_t   btSoftBody::getSolver(eVSolver::_ solver)
2512 {
2513 switch(solver)
2514         {
2515         case    eVSolver::Linear:               return(&btSoftBody::VSolve_Links);
2516         }
2517 return(0);
2518 }
2519
2520 //
2521 void                    btSoftBody::defaultCollisionHandler(btCollisionObject* pco)
2522 {
2523 switch(m_cfg.collisions&fCollision::RVSmask)
2524         {
2525         case    fCollision::SDF_RS:
2526                 {
2527                 btSoftColliders::CollideSDF_RS  docollide;
2528                 btRigidBody*            prb=btRigidBody::upcast(pco);
2529                 const btTransform       wtr=prb->getInterpolationWorldTransform();
2530                 const btTransform       ctr=prb->getWorldTransform();
2531                 const btScalar          timemargin=(wtr.getOrigin()-ctr.getOrigin()).length();
2532                 const btScalar          basemargin=getCollisionShape()->getMargin();
2533                 btVector3                       mins;
2534                 btVector3                       maxs;
2535                 btDbvtVolume            volume;
2536                 pco->getCollisionShape()->getAabb(      pco->getInterpolationWorldTransform(),
2537                                                                                         mins,
2538                                                                                         maxs);
2539                 volume=btDbvtVolume::FromMM(mins,maxs);
2540                 volume.Expand(btVector3(basemargin,basemargin,basemargin));
2541                 docollide.psb           =       this;
2542                 docollide.prb           =       prb;
2543                 docollide.dynmargin     =       basemargin+timemargin;
2544                 docollide.stamargin     =       basemargin;
2545                 btDbvt::collideTV(m_ndbvt.m_root,volume,docollide);
2546                 }
2547         break;
2548         case    fCollision::CL_RS:
2549                 {
2550                 btSoftColliders::CollideCL_RS   collider;
2551                 collider.Process(this,btRigidBody::upcast(pco));
2552                 }
2553         break;
2554         }
2555 }
2556
2557 //
2558 void                    btSoftBody::defaultCollisionHandler(btSoftBody* psb)
2559 {
2560 const int cf=m_cfg.collisions&psb->m_cfg.collisions;
2561 switch(cf&fCollision::SVSmask)
2562         {
2563         case    fCollision::CL_SS:
2564                 {
2565                 btSoftColliders::CollideCL_SS   docollide;
2566                 docollide.Process(this,psb);
2567                 }
2568         break;
2569         case    fCollision::VF_SS:
2570                 {
2571                 btSoftColliders::CollideVF_SS   docollide;
2572                 /* common                                       */
2573                 docollide.mrg=  getCollisionShape()->getMargin()+
2574                                                 psb->getCollisionShape()->getMargin();
2575                 /* psb0 nodes vs psb1 faces     */
2576                 docollide.psb[0]=this;
2577                 docollide.psb[1]=psb;
2578                 btDbvt::collideTT(      docollide.psb[0]->m_ndbvt.m_root,
2579                                                         docollide.psb[1]->m_fdbvt.m_root,
2580                                                         docollide);
2581                 /* psb1 nodes vs psb0 faces     */
2582                 docollide.psb[0]=psb;
2583                 docollide.psb[1]=this;
2584                 btDbvt::collideTT(      docollide.psb[0]->m_ndbvt.m_root,
2585                                                         docollide.psb[1]->m_fdbvt.m_root,
2586                                                         docollide);
2587                 }
2588         break;
2589         }
2590 }