extern/bullet2/src/BulletSoftBody/btSoftBody.cpp

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