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