extern/bullet2/src/BulletSoftBody/btSoftBody.cpp

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