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