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