extern/bullet2/src/BulletSoftBody/btSoftBody.cpp

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