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 //
  24 btSoftBody::btSoftBody(btSoftBodyWorldInfo*     worldInfo,int node_count,  const btVector3* x,  const btScalar* m)
  25 :m_softBodySolver(0),m_worldInfo(worldInfo)
  26 {
  27         /* Init         */
  28         initDefaults();
  29
  30         /* Default material     */
  31         Material*       pm=appendMaterial();
  32         pm->m_kLST      =       1;
  33         pm->m_kAST      =       1;
  34         pm->m_kVST      =       1;
  35         pm->m_flags     =       fMaterial::Default;
  36
  37         /* Nodes                        */
  38         const btScalar          margin=getCollisionShape()->getMargin();
  39         m_nodes.resize(node_count);
  40         for(int i=0,ni=node_count;i<ni;++i)
  41         {
  42                 Node&   n=m_nodes[i];
  43                 ZeroInitialize(n);
  44                 n.m_x           =       x?*x++:btVector3(0,0,0);
  45                 n.m_q           =       n.m_x;
  46                 n.m_im          =       m?*m++:1;
  47                 n.m_im          =       n.m_im>0?1/n.m_im:0;
  48                 n.m_leaf        =       m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
  49                 n.m_material=   pm;
  50         }
  51         updateBounds();
  52
  53 }
  54
  55 btSoftBody::btSoftBody(btSoftBodyWorldInfo*     worldInfo)
  56 :m_worldInfo(worldInfo)
  57 {
  58         initDefaults();
  59 }
  60
  61
  62 void    btSoftBody::initDefaults()
  63 {
  64         m_internalType          =       CO_SOFT_BODY;
  65         m_cfg.aeromodel         =       eAeroModel::V_Point;
  66         m_cfg.kVCF                      =       1;
  67         m_cfg.kDG                       =       0;
  68         m_cfg.kLF                       =       0;
  69         m_cfg.kDP                       =       0;
  70         m_cfg.kPR                       =       0;
  71         m_cfg.kVC                       =       0;
  72         m_cfg.kDF                       =       (btScalar)0.2;
  73         m_cfg.kMT                       =       0;
  74         m_cfg.kCHR                      =       (btScalar)1.0;
  75         m_cfg.kKHR                      =       (btScalar)0.1;
  76         m_cfg.kSHR                      =       (btScalar)1.0;
  77         m_cfg.kAHR                      =       (btScalar)0.7;
  78         m_cfg.kSRHR_CL          =       (btScalar)0.1;
  79         m_cfg.kSKHR_CL          =       (btScalar)1;
  80         m_cfg.kSSHR_CL          =       (btScalar)0.5;
  81         m_cfg.kSR_SPLT_CL       =       (btScalar)0.5;
  82         m_cfg.kSK_SPLT_CL       =       (btScalar)0.5;
  83         m_cfg.kSS_SPLT_CL       =       (btScalar)0.5;
  84         m_cfg.maxvolume         =       (btScalar)1;
  85         m_cfg.timescale         =       1;
  86         m_cfg.viterations       =       0;
  87         m_cfg.piterations       =       1;
  88         m_cfg.diterations       =       0;
  89         m_cfg.citerations       =       4;
  90         m_cfg.collisions        =       fCollision::Default;
  91         m_pose.m_bvolume        =       false;
  92         m_pose.m_bframe         =       false;
  93         m_pose.m_volume         =       0;
  94         m_pose.m_com            =       btVector3(0,0,0);
  95         m_pose.m_rot.setIdentity();
  96         m_pose.m_scl.setIdentity();
  97         m_tag                           =       0;
  98         m_timeacc                       =       0;
  99         m_bUpdateRtCst          =       true;
 100         m_bounds[0]                     =       btVector3(0,0,0);
 101         m_bounds[1]                     =       btVector3(0,0,0);
 102         m_worldTransform.setIdentity();
 103         setSolver(eSolverPresets::Positions);
 104
 105         /* Collision shape      */
 106         ///for now, create a collision shape internally
 107         m_collisionShape = new btSoftBodyCollisionShape(this);
 108         m_collisionShape->setMargin(0.25f);
 109
 110         m_initialWorldTransform.setIdentity();
 111
 112         m_windVelocity = btVector3(0,0,0);
 113         m_restLengthScale = btScalar(1.0);
 114 }
 115
 116 //
 117 btSoftBody::~btSoftBody()
 118 {
 119         //for now, delete the internal shape
 120         delete m_collisionShape;
 121         int i;
 122
 123         releaseClusters();
 124         for(i=0;i<m_materials.size();++i)
 125                 btAlignedFree(m_materials[i]);
 126         for(i=0;i<m_joints.size();++i)
 127                 btAlignedFree(m_joints[i]);
 128 }
 129
 130 //
 131 bool                    btSoftBody::checkLink(int node0,int node1) const
 132 {
 133         return(checkLink(&m_nodes[node0],&m_nodes[node1]));
 134 }
 135
 136 //
 137 bool                    btSoftBody::checkLink(const Node* node0,const Node* node1) const
 138 {
 139         const Node*     n[]={node0,node1};
 140         for(int i=0,ni=m_links.size();i<ni;++i)
 141         {
 142                 const Link&     l=m_links[i];
 143                 if(     (l.m_n[0]==n[0]&&l.m_n[1]==n[1])||
 144                         (l.m_n[0]==n[1]&&l.m_n[1]==n[0]))
 145                 {
 146                         return(true);
 147                 }
 148         }
 149         return(false);
 150 }
 151
 152 //
 153 bool                    btSoftBody::checkFace(int node0,int node1,int node2) const
 154 {
 155         const Node*     n[]={   &m_nodes[node0],
 156                 &m_nodes[node1],
 157                 &m_nodes[node2]};
 158         for(int i=0,ni=m_faces.size();i<ni;++i)
 159         {
 160                 const Face&     f=m_faces[i];
 161                 int                     c=0;
 162                 for(int j=0;j<3;++j)
 163                 {
 164                         if(     (f.m_n[j]==n[0])||
 165                                 (f.m_n[j]==n[1])||
 166                                 (f.m_n[j]==n[2])) c|=1<<j; else break;
 167                 }
 168                 if(c==7) return(true);
 169         }
 170         return(false);
 171 }
 172
 173 //
 174 btSoftBody::Material*           btSoftBody::appendMaterial()
 175 {
 176         Material*       pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
 177         if(m_materials.size()>0)
 178                 *pm=*m_materials[0];
 179         else
 180                 ZeroInitialize(*pm);
 181         m_materials.push_back(pm);
 182         return(pm);
 183 }
 184
 185 //
 186 void                    btSoftBody::appendNote( const char* text,
 187                                                                            const btVector3& o,
 188                                                                            const btVector4& c,
 189                                                                            Node* n0,
 190                                                                            Node* n1,
 191                                                                            Node* n2,
 192                                                                            Node* n3)
 193 {
 194         Note    n;
 195         ZeroInitialize(n);
 196         n.m_rank                =       0;
 197         n.m_text                =       text;
 198         n.m_offset              =       o;
 199         n.m_coords[0]   =       c.x();
 200         n.m_coords[1]   =       c.y();
 201         n.m_coords[2]   =       c.z();
 202         n.m_coords[3]   =       c.w();
 203         n.m_nodes[0]    =       n0;n.m_rank+=n0?1:0;
 204         n.m_nodes[1]    =       n1;n.m_rank+=n1?1:0;
 205         n.m_nodes[2]    =       n2;n.m_rank+=n2?1:0;
 206         n.m_nodes[3]    =       n3;n.m_rank+=n3?1:0;
 207         m_notes.push_back(n);
 208 }
 209
 210 //
 211 void                    btSoftBody::appendNote( const char* text,
 212                                                                            const btVector3& o,
 213                                                                            Node* feature)
 214 {
 215         appendNote(text,o,btVector4(1,0,0,0),feature);
 216 }
 217
 218 //
 219 void                    btSoftBody::appendNote( const char* text,
 220                                                                            const btVector3& o,
 221                                                                            Link* feature)
 222 {
 223         static const btScalar   w=1/(btScalar)2;
 224         appendNote(text,o,btVector4(w,w,0,0),   feature->m_n[0],
 225                 feature->m_n[1]);
 226 }
 227
 228 //
 229 void                    btSoftBody::appendNote( const char* text,
 230                                                                            const btVector3& o,
 231                                                                            Face* feature)
 232 {
 233         static const btScalar   w=1/(btScalar)3;
 234         appendNote(text,o,btVector4(w,w,w,0),   feature->m_n[0],
 235                 feature->m_n[1],
 236                 feature->m_n[2]);
 237 }
 238
 239 //
 240 void                    btSoftBody::appendNode( const btVector3& x,btScalar m)
 241 {
 242         if(m_nodes.capacity()==m_nodes.size())
 243         {
 244                 pointersToIndices();
 245                 m_nodes.reserve(m_nodes.size()*2+1);
 246                 indicesToPointers();
 247         }
 248         const btScalar  margin=getCollisionShape()->getMargin();
 249         m_nodes.push_back(Node());
 250         Node&                   n=m_nodes[m_nodes.size()-1];
 251         ZeroInitialize(n);
 252         n.m_x                   =       x;
 253         n.m_q                   =       n.m_x;
 254         n.m_im                  =       m>0?1/m:0;
 255         n.m_material    =       m_materials[0];
 256         n.m_leaf                =       m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
 257 }
 258
 259 //
 260 void                    btSoftBody::appendLink(int model,Material* mat)
 261 {
 262         Link    l;
 263         if(model>=0)
 264                 l=m_links[model];
 265         else
 266         { ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; }
 267         m_links.push_back(l);
 268 }
 269
 270 //
 271 void                    btSoftBody::appendLink( int node0,
 272                                                                            int node1,
 273                                                                            Material* mat,
 274                                                                            bool bcheckexist)
 275 {
 276         appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist);
 277 }
 278
 279 //
 280 void                    btSoftBody::appendLink( Node* node0,
 281                                                                            Node* node1,
 282                                                                            Material* mat,
 283                                                                            bool bcheckexist)
 284 {
 285         if((!bcheckexist)||(!checkLink(node0,node1)))
 286         {
 287                 appendLink(-1,mat);
 288                 Link&   l=m_links[m_links.size()-1];
 289                 l.m_n[0]                =       node0;
 290                 l.m_n[1]                =       node1;
 291                 l.m_rl                  =       (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
 292                 m_bUpdateRtCst=true;
 293         }
 294 }
 295
 296 //
 297 void                    btSoftBody::appendFace(int model,Material* mat)
 298 {
 299         Face    f;
 300         if(model>=0)
 301         { f=m_faces[model]; }
 302         else
 303         { ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; }
 304         m_faces.push_back(f);
 305 }
 306
 307 //
 308 void                    btSoftBody::appendFace(int node0,int node1,int node2,Material* mat)
 309 {
 310         if (node0==node1)
 311                 return;
 312         if (node1==node2)
 313                 return;
 314         if (node2==node0)
 315                 return;
 316
 317         appendFace(-1,mat);
 318         Face&   f=m_faces[m_faces.size()-1];
 319         btAssert(node0!=node1);
 320         btAssert(node1!=node2);
 321         btAssert(node2!=node0);
 322         f.m_n[0]        =       &m_nodes[node0];
 323         f.m_n[1]        =       &m_nodes[node1];
 324         f.m_n[2]        =       &m_nodes[node2];
 325         f.m_ra          =       AreaOf( f.m_n[0]->m_x,
 326                 f.m_n[1]->m_x,
 327                 f.m_n[2]->m_x);
 328         m_bUpdateRtCst=true;
 329 }
 330
 331 //
 332 void                    btSoftBody::appendTetra(int model,Material* mat)
 333 {
 334 Tetra   t;
 335 if(model>=0)
 336         t=m_tetras[model];
 337         else
 338         { ZeroInitialize(t);t.m_material=mat?mat:m_materials[0]; }
 339 m_tetras.push_back(t);
 340 }
 341
 342 //
 343 void                    btSoftBody::appendTetra(int node0,
 344                                                                                 int node1,
 345                                                                                 int node2,
 346                                                                                 int node3,
 347                                                                                 Material* mat)
 348 {
 349         appendTetra(-1,mat);
 350         Tetra&  t=m_tetras[m_tetras.size()-1];
 351         t.m_n[0]        =       &m_nodes[node0];
 352         t.m_n[1]        =       &m_nodes[node1];
 353         t.m_n[2]        =       &m_nodes[node2];
 354         t.m_n[3]        =       &m_nodes[node3];
 355         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);
 356         m_bUpdateRtCst=true;
 357 }
 358
 359 //
 360
 361 void                    btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisionBetweenLinkedBodies,btScalar influence)
 362 {
 363         btVector3 local = body->getWorldTransform().inverse()*m_nodes[node].m_x;
 364         appendAnchor(node,body,local,disableCollisionBetweenLinkedBodies,influence);
 365 }
 366
 367 //
 368 void                    btSoftBody::appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies,btScalar influence)
 369 {
 370         if (disableCollisionBetweenLinkedBodies)
 371         {
 372                 if (m_collisionDisabledObjects.findLinearSearch(body)==m_collisionDisabledObjects.size())
 373                 {
 374                         m_collisionDisabledObjects.push_back(body);
 375                 }
 376         }
 377
 378         Anchor  a;
 379         a.m_node                        =       &m_nodes[node];
 380         a.m_body                        =       body;
 381         a.m_local                       =       localPivot;
 382         a.m_node->m_battach     =       1;
 383         a.m_influence = influence;
 384         m_anchors.push_back(a);
 385 }
 386
 387 //
 388 void                    btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1)
 389 {
 390         LJoint*         pj      =       new(btAlignedAlloc(sizeof(LJoint),16)) LJoint();
 391         pj->m_bodies[0] =       body0;
 392         pj->m_bodies[1] =       body1;
 393         pj->m_refs[0]   =       pj->m_bodies[0].xform().inverse()*specs.position;
 394         pj->m_refs[1]   =       pj->m_bodies[1].xform().inverse()*specs.position;
 395         pj->m_cfm               =       specs.cfm;
 396         pj->m_erp               =       specs.erp;
 397         pj->m_split             =       specs.split;
 398         m_joints.push_back(pj);
 399 }
 400
 401 //
 402 void                    btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body)
 403 {
 404         appendLinearJoint(specs,m_clusters[0],body);
 405 }
 406
 407 //
 408 void                    btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body)
 409 {
 410         appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]);
 411 }
 412
 413 //
 414 void                    btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1)
 415 {
 416         AJoint*         pj      =       new(btAlignedAlloc(sizeof(AJoint),16)) AJoint();
 417         pj->m_bodies[0] =       body0;
 418         pj->m_bodies[1] =       body1;
 419         pj->m_refs[0]   =       pj->m_bodies[0].xform().inverse().getBasis()*specs.axis;
 420         pj->m_refs[1]   =       pj->m_bodies[1].xform().inverse().getBasis()*specs.axis;
 421         pj->m_cfm               =       specs.cfm;
 422         pj->m_erp               =       specs.erp;
 423         pj->m_split             =       specs.split;
 424         pj->m_icontrol  =       specs.icontrol;
 425         m_joints.push_back(pj);
 426 }
 427
 428 //
 429 void                    btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body)
 430 {
 431         appendAngularJoint(specs,m_clusters[0],body);
 432 }
 433
 434 //
 435 void                    btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body)
 436 {
 437         appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]);
 438 }
 439
 440 //
 441 void                    btSoftBody::addForce(const btVector3& force)
 442 {
 443         for(int i=0,ni=m_nodes.size();i<ni;++i) addForce(force,i);
 444 }
 445
 446 //
 447 void                    btSoftBody::addForce(const btVector3& force,int node)
 448 {
 449         Node&   n=m_nodes[node];
 450         if(n.m_im>0)
 451         {
 452                 n.m_f   +=      force;
 453         }
 454 }
 455
 456 void                    btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeIndex)
 457 {
 458         btAssert(nodeIndex >= 0 && nodeIndex < m_nodes.size());
 459
 460         const btScalar dt = m_sst.sdt;
 461         const btScalar kLF = m_cfg.kLF;
 462         const btScalar kDG = m_cfg.kDG;
 463         //const btScalar kPR = m_cfg.kPR;
 464         //const btScalar kVC = m_cfg.kVC;
 465         const bool as_lift = kLF>0;
 466         const bool as_drag = kDG>0;
 467         const bool as_aero = as_lift || as_drag;
 468         const bool as_vaero = as_aero && (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
 469
 470         Node& n = m_nodes[nodeIndex];
 471
 472         if( n.m_im>0 )
 473         {
 474                 btSoftBody::sMedium     medium;
 475
 476                 EvaluateMedium(m_worldInfo, n.m_x, medium);
 477                 medium.m_velocity = windVelocity;
 478                 medium.m_density = m_worldInfo->air_density;
 479
 480                 /* Aerodynamics                 */
 481                 if(as_vaero)
 482                 {
 483                         const btVector3 rel_v = n.m_v - medium.m_velocity;
 484                         const btScalar rel_v_len = rel_v.length();
 485                         const btScalar  rel_v2 = rel_v.length2();
 486
 487                         if(rel_v2>SIMD_EPSILON)
 488                         {
 489                                 const btVector3 rel_v_nrm = rel_v.normalized();
 490                                 btVector3       nrm = n.m_n;
 491
 492                                 if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSidedLiftDrag)
 493                                 {
 494                                         nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);
 495                                         btVector3 fDrag(0, 0, 0);
 496                                         btVector3 fLift(0, 0, 0);
 497
 498                                         btScalar n_dot_v = nrm.dot(rel_v_nrm);
 499                                         btScalar tri_area = 0.5f * n.m_area;
 500
 501                                         fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
 502
 503                                         // Check angle of attack
 504                                         // cos(10º) = 0.98480
 505                                         if ( 0 < n_dot_v && n_dot_v < 0.98480f)
 506                                                 fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f-n_dot_v*n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));
 507
 508                                         // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node.
 509                                         btVector3 del_v_by_fDrag = fDrag*n.m_im*m_sst.sdt;
 510                                         btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
 511                                         btScalar v_len2 = n.m_v.length2();
 512
 513                                         if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
 514                                         {
 515                                                 btScalar del_v_by_fDrag_len = del_v_by_fDrag.length();
 516                                                 btScalar v_len = n.m_v.length();
 517                                                 fDrag *= btScalar(0.8)*(v_len / del_v_by_fDrag_len);
 518                                         }
 519
 520                                         n.m_f += fDrag;
 521                                         n.m_f += fLift;
 522                                 }
 523                                 else if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_Point || m_cfg.aeromodel == btSoftBody::eAeroModel::V_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided)
 524                                 {
 525                                         if (btSoftBody::eAeroModel::V_TwoSided)
 526                                                 nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);
 527
 528                                         const btScalar dvn = btDot(rel_v,nrm);
 529                                         /* Compute forces       */
 530                                         if(dvn>0)
 531                                         {
 532                                                 btVector3               force(0,0,0);
 533                                                 const btScalar  c0      =       n.m_area * dvn * rel_v2/2;
 534                                                 const btScalar  c1      =       c0 * medium.m_density;
 535                                                 force   +=      nrm*(-c1*kLF);
 536                                                 force   +=      rel_v.normalized() * (-c1 * kDG);
 537                                                 ApplyClampedForce(n, force, dt);
 538                                         }
 539                                 }
 540                         }
 541                 }
 542         }
 543 }
 544
 545 void                    btSoftBody::addAeroForceToFace(const btVector3& windVelocity,int faceIndex)
 546 {
 547         const btScalar dt = m_sst.sdt;
 548         const btScalar kLF = m_cfg.kLF;
 549         const btScalar kDG = m_cfg.kDG;
 550 //      const btScalar kPR = m_cfg.kPR;
 551 //      const btScalar kVC = m_cfg.kVC;
 552         const bool as_lift = kLF>0;
 553         const bool as_drag = kDG>0;
 554         const bool as_aero = as_lift || as_drag;
 555         const bool as_faero = as_aero && (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
 556
 557         if(as_faero)
 558         {
 559                 btSoftBody::Face&       f=m_faces[faceIndex];
 560
 561                 btSoftBody::sMedium     medium;
 562
 563                 const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3;
 564                 const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3;
 565                 EvaluateMedium(m_worldInfo,x,medium);
 566                 medium.m_velocity = windVelocity;
 567                 medium.m_density = m_worldInfo->air_density;
 568                 const btVector3 rel_v=v-medium.m_velocity;
 569                 const btScalar rel_v_len = rel_v.length();
 570                 const btScalar  rel_v2=rel_v.length2();
 571
 572                 if(rel_v2>SIMD_EPSILON)
 573                 {
 574                         const btVector3 rel_v_nrm = rel_v.normalized();
 575                         btVector3       nrm = f.m_normal;
 576
 577                         if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSidedLiftDrag)
 578                         {
 579                                 nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);
 580
 581                                 btVector3 fDrag(0, 0, 0);
 582                                 btVector3 fLift(0, 0, 0);
 583
 584                                 btScalar n_dot_v = nrm.dot(rel_v_nrm);
 585                                 btScalar tri_area = 0.5f * f.m_ra;
 586
 587                                 fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
 588
 589                                 // Check angle of attack
 590                                 // cos(10º) = 0.98480
 591                                 if ( 0 < n_dot_v && n_dot_v < 0.98480f)
 592                                         fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f-n_dot_v*n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));
 593
 594                                 fDrag /= 3;
 595                                 fLift /= 3;
 596
 597                                 for(int j=0;j<3;++j)
 598                                 {
 599                                         if (f.m_n[j]->m_im>0)
 600                                         {
 601                                                 // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node.
 602                                                 btVector3 del_v_by_fDrag = fDrag*f.m_n[j]->m_im*m_sst.sdt;
 603                                                 btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
 604                                                 btScalar v_len2 = f.m_n[j]->m_v.length2();
 605
 606                                                 if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
 607                                                 {
 608                                                         btScalar del_v_by_fDrag_len = del_v_by_fDrag.length();
 609                                                         btScalar v_len = f.m_n[j]->m_v.length();
 610                                                         fDrag *= btScalar(0.8)*(v_len / del_v_by_fDrag_len);
 611                                                 }
 612
 613                                                 f.m_n[j]->m_f += fDrag;
 614                                                 f.m_n[j]->m_f += fLift;
 615                                         }
 616                                 }
 617                         }
 618                         else if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided)
 619                         {
 620                                 if (btSoftBody::eAeroModel::F_TwoSided)
 621                                         nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);
 622
 623                                 const btScalar  dvn=btDot(rel_v,nrm);
 624                                 /* Compute forces       */
 625                                 if(dvn>0)
 626                                 {
 627                                         btVector3               force(0,0,0);
 628                                         const btScalar  c0      =       f.m_ra*dvn*rel_v2;
 629                                         const btScalar  c1      =       c0*medium.m_density;
 630                                         force   +=      nrm*(-c1*kLF);
 631                                         force   +=      rel_v.normalized()*(-c1*kDG);
 632                                         force   /=      3;
 633                                         for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt);
 634                                 }
 635                         }
 636                 }
 637         }
 638
 639 }
 640
 641 //
 642 void                    btSoftBody::addVelocity(const btVector3& velocity)
 643 {
 644         for(int i=0,ni=m_nodes.size();i<ni;++i) addVelocity(velocity,i);
 645 }
 646
 647 /* Set velocity for the entire body                                                                             */
 648 void                            btSoftBody::setVelocity(        const btVector3& velocity)
 649 {
 650         for(int i=0,ni=m_nodes.size();i<ni;++i)
 651         {
 652                 Node&   n=m_nodes[i];
 653                 if(n.m_im>0)
 654                 {
 655                         n.m_v   =       velocity;
 656                 }
 657         }
 658 }
 659
 660
 661 //
 662 void                    btSoftBody::addVelocity(const btVector3& velocity,int node)
 663 {
 664         Node&   n=m_nodes[node];
 665         if(n.m_im>0)
 666         {
 667                 n.m_v   +=      velocity;
 668         }
 669 }
 670
 671 //
 672 void                    btSoftBody::setMass(int node,btScalar mass)
 673 {
 674         m_nodes[node].m_im=mass>0?1/mass:0;
 675         m_bUpdateRtCst=true;
 676 }
 677
 678 //
 679 btScalar                btSoftBody::getMass(int node) const
 680 {
 681         return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0);
 682 }
 683
 684 //
 685 btScalar                btSoftBody::getTotalMass() const
 686 {
 687         btScalar        mass=0;
 688         for(int i=0;i<m_nodes.size();++i)
 689         {
 690                 mass+=getMass(i);
 691         }
 692         return(mass);
 693 }
 694
 695 //
 696 void                    btSoftBody::setTotalMass(btScalar mass,bool fromfaces)
 697 {
 698         int i;
 699
 700         if(fromfaces)
 701         {
 702
 703                 for(i=0;i<m_nodes.size();++i)
 704                 {
 705                         m_nodes[i].m_im=0;
 706                 }
 707                 for(i=0;i<m_faces.size();++i)
 708                 {
 709                         const Face&             f=m_faces[i];
 710                         const btScalar  twicearea=AreaOf(       f.m_n[0]->m_x,
 711                                 f.m_n[1]->m_x,
 712                                 f.m_n[2]->m_x);
 713                         for(int j=0;j<3;++j)
 714                         {
 715                                 f.m_n[j]->m_im+=twicearea;
 716                         }
 717                 }
 718                 for( i=0;i<m_nodes.size();++i)
 719                 {
 720                         m_nodes[i].m_im=1/m_nodes[i].m_im;
 721                 }
 722         }
 723         const btScalar  tm=getTotalMass();
 724         const btScalar  itm=1/tm;
 725         for( i=0;i<m_nodes.size();++i)
 726         {
 727                 m_nodes[i].m_im/=itm*mass;
 728         }
 729         m_bUpdateRtCst=true;
 730 }
 731
 732 //
 733 void                    btSoftBody::setTotalDensity(btScalar density)
 734 {
 735         setTotalMass(getVolume()*density,true);
 736 }
 737
 738 //
 739 void                    btSoftBody::setVolumeMass(btScalar mass)
 740 {
 741 btAlignedObjectArray<btScalar>  ranks;
 742 ranks.resize(m_nodes.size(),0);
 743 int i;
 744
 745 for(i=0;i<m_nodes.size();++i)
 746         {
 747         m_nodes[i].m_im=0;
 748         }
 749 for(i=0;i<m_tetras.size();++i)
 750         {
 751         const Tetra& t=m_tetras[i];
 752         for(int j=0;j<4;++j)
 753                 {
 754                 t.m_n[j]->m_im+=btFabs(t.m_rv);
 755                 ranks[int(t.m_n[j]-&m_nodes[0])]+=1;
 756                 }
 757         }
 758 for( i=0;i<m_nodes.size();++i)
 759         {
 760         if(m_nodes[i].m_im>0)
 761                 {
 762                 m_nodes[i].m_im=ranks[i]/m_nodes[i].m_im;
 763                 }
 764         }
 765 setTotalMass(mass,false);
 766 }
 767
 768 //
 769 void                    btSoftBody::setVolumeDensity(btScalar density)
 770 {
 771 btScalar        volume=0;
 772 for(int i=0;i<m_tetras.size();++i)
 773         {
 774         const Tetra& t=m_tetras[i];
 775         for(int j=0;j<4;++j)
 776                 {
 777                 volume+=btFabs(t.m_rv);
 778                 }
 779         }
 780 setVolumeMass(volume*density/6);
 781 }
 782
 783 //
 784 void                    btSoftBody::transform(const btTransform& trs)
 785 {
 786         const btScalar  margin=getCollisionShape()->getMargin();
 787         ATTRIBUTE_ALIGNED16(btDbvtVolume)       vol;
 788
 789         for(int i=0,ni=m_nodes.size();i<ni;++i)
 790         {
 791                 Node&   n=m_nodes[i];
 792                 n.m_x=trs*n.m_x;
 793                 n.m_q=trs*n.m_q;
 794                 n.m_n=trs.getBasis()*n.m_n;
 795                 vol = btDbvtVolume::FromCR(n.m_x,margin);
 796
 797                 m_ndbvt.update(n.m_leaf,vol);
 798         }
 799         updateNormals();
 800         updateBounds();
 801         updateConstants();
 802         m_initialWorldTransform = trs;
 803 }
 804
 805 //
 806 void                    btSoftBody::translate(const btVector3& trs)
 807 {
 808         btTransform     t;
 809         t.setIdentity();
 810         t.setOrigin(trs);
 811         transform(t);
 812 }
 813
 814 //
 815 void                    btSoftBody::rotate(     const btQuaternion& rot)
 816 {
 817         btTransform     t;
 818         t.setIdentity();
 819         t.setRotation(rot);
 820         transform(t);
 821 }
 822
 823 //
 824 void                    btSoftBody::scale(const btVector3& scl)
 825 {
 826
 827         const btScalar  margin=getCollisionShape()->getMargin();
 828         ATTRIBUTE_ALIGNED16(btDbvtVolume)       vol;
 829
 830         for(int i=0,ni=m_nodes.size();i<ni;++i)
 831         {
 832                 Node&   n=m_nodes[i];
 833                 n.m_x*=scl;
 834                 n.m_q*=scl;
 835                 vol = btDbvtVolume::FromCR(n.m_x,margin);
 836                 m_ndbvt.update(n.m_leaf,vol);
 837         }
 838         updateNormals();
 839         updateBounds();
 840         updateConstants();
 841 }
 842
 843 //
 844 btScalar btSoftBody::getRestLengthScale()
 845 {
 846         return m_restLengthScale;
 847 }
 848
 849 //
 850 void btSoftBody::setRestLengthScale(btScalar restLengthScale)
 851 {
 852         for(int i=0, ni=m_links.size(); i<ni; ++i)
 853         {
 854                 Link&           l=m_links[i];
 855                 l.m_rl  =       l.m_rl / m_restLengthScale * restLengthScale;
 856                 l.m_c1  =       l.m_rl*l.m_rl;
 857         }
 858         m_restLengthScale = restLengthScale;
 859
 860         if (getActivationState() == ISLAND_SLEEPING)
 861                 activate();
 862 }
 863
 864 //
 865 void                    btSoftBody::setPose(bool bvolume,bool bframe)
 866 {
 867         m_pose.m_bvolume        =       bvolume;
 868         m_pose.m_bframe         =       bframe;
 869         int i,ni;
 870
 871         /* Weights              */
 872         const btScalar  omass=getTotalMass();
 873         const btScalar  kmass=omass*m_nodes.size()*1000;
 874         btScalar                tmass=omass;
 875         m_pose.m_wgh.resize(m_nodes.size());
 876         for(i=0,ni=m_nodes.size();i<ni;++i)
 877         {
 878                 if(m_nodes[i].m_im<=0) tmass+=kmass;
 879         }
 880         for( i=0,ni=m_nodes.size();i<ni;++i)
 881         {
 882                 Node&   n=m_nodes[i];
 883                 m_pose.m_wgh[i]=        n.m_im>0                                        ?
 884                         1/(m_nodes[i].m_im*tmass)       :
 885                 kmass/tmass;
 886         }
 887         /* Pos          */
 888         const btVector3 com=evaluateCom();
 889         m_pose.m_pos.resize(m_nodes.size());
 890         for( i=0,ni=m_nodes.size();i<ni;++i)
 891         {
 892                 m_pose.m_pos[i]=m_nodes[i].m_x-com;
 893         }
 894         m_pose.m_volume =       bvolume?getVolume():0;
 895         m_pose.m_com    =       com;
 896         m_pose.m_rot.setIdentity();
 897         m_pose.m_scl.setIdentity();
 898         /* Aqq          */
 899         m_pose.m_aqq[0] =
 900                 m_pose.m_aqq[1] =
 901                 m_pose.m_aqq[2] =       btVector3(0,0,0);
 902         for( i=0,ni=m_nodes.size();i<ni;++i)
 903         {
 904                 const btVector3&        q=m_pose.m_pos[i];
 905                 const btVector3         mq=m_pose.m_wgh[i]*q;
 906                 m_pose.m_aqq[0]+=mq.x()*q;
 907                 m_pose.m_aqq[1]+=mq.y()*q;
 908                 m_pose.m_aqq[2]+=mq.z()*q;
 909         }
 910         m_pose.m_aqq=m_pose.m_aqq.inverse();
 911
 912         updateConstants();
 913 }
 914
 915 void                            btSoftBody::resetLinkRestLengths()
 916 {
 917         for(int i=0, ni=m_links.size();i<ni;++i)
 918         {
 919                 Link& l =       m_links[i];
 920                 l.m_rl  =       (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
 921                 l.m_c1  =       l.m_rl*l.m_rl;
 922         }
 923 }
 924
 925 //
 926 btScalar                btSoftBody::getVolume() const
 927 {
 928         btScalar        vol=0;
 929         if(m_nodes.size()>0)
 930         {
 931                 int i,ni;
 932
 933                 const btVector3 org=m_nodes[0].m_x;
 934                 for(i=0,ni=m_faces.size();i<ni;++i)
 935                 {
 936                         const Face&     f=m_faces[i];
 937                         vol+=btDot(f.m_n[0]->m_x-org,btCross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org));
 938                 }
 939                 vol/=(btScalar)6;
 940         }
 941         return(vol);
 942 }
 943
 944 //
 945 int                             btSoftBody::clusterCount() const
 946 {
 947         return(m_clusters.size());
 948 }
 949
 950 //
 951 btVector3               btSoftBody::clusterCom(const Cluster* cluster)
 952 {
 953         btVector3               com(0,0,0);
 954         for(int i=0,ni=cluster->m_nodes.size();i<ni;++i)
 955         {
 956                 com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i];
 957         }
 958         return(com*cluster->m_imass);
 959 }
 960
 961 //
 962 btVector3               btSoftBody::clusterCom(int cluster) const
 963 {
 964         return(clusterCom(m_clusters[cluster]));
 965 }
 966
 967 //
 968 btVector3               btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos)
 969 {
 970         return(cluster->m_lv+btCross(cluster->m_av,rpos));
 971 }
 972
 973 //
 974 void                    btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
 975 {
 976         const btVector3 li=cluster->m_imass*impulse;
 977         const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse);
 978         cluster->m_vimpulses[0]+=li;cluster->m_lv+=li;
 979         cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
 980         cluster->m_nvimpulses++;
 981 }
 982
 983 //
 984 void                    btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
 985 {
 986         const btVector3 li=cluster->m_imass*impulse;
 987         const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse);
 988         cluster->m_dimpulses[0]+=li;
 989         cluster->m_dimpulses[1]+=ai;
 990         cluster->m_ndimpulses++;
 991 }
 992
 993 //
 994 void                    btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse)
 995 {
 996         if(impulse.m_asVelocity)        clusterVImpulse(cluster,rpos,impulse.m_velocity);
 997         if(impulse.m_asDrift)           clusterDImpulse(cluster,rpos,impulse.m_drift);
 998 }
 999
1000 //
1001 void                    btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse)
1002 {
1003         const btVector3 ai=cluster->m_invwi*impulse;
1004         cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
1005         cluster->m_nvimpulses++;
1006 }
1007
1008 //
1009 void                    btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse)
1010 {
1011         const btVector3 ai=cluster->m_invwi*impulse;
1012         cluster->m_dimpulses[1]+=ai;
1013         cluster->m_ndimpulses++;
1014 }
1015
1016 //
1017 void                    btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse)
1018 {
1019         if(impulse.m_asVelocity)        clusterVAImpulse(cluster,impulse.m_velocity);
1020         if(impulse.m_asDrift)           clusterDAImpulse(cluster,impulse.m_drift);
1021 }
1022
1023 //
1024 void                    btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse)
1025 {
1026         cluster->m_dimpulses[0]+=impulse*cluster->m_imass;
1027         cluster->m_ndimpulses++;
1028 }
1029
1030 struct NodeLinks
1031 {
1032     btAlignedObjectArray<int> m_links;
1033 };
1034
1035
1036
1037 //
1038 int                             btSoftBody::generateBendingConstraints(int distance,Material* mat)
1039 {
1040         int i,j;
1041
1042         if(distance>1)
1043         {
1044                 /* Build graph  */
1045                 const int               n=m_nodes.size();
1046                 const unsigned  inf=(~(unsigned)0)>>1;
1047                 unsigned*               adj=new unsigned[n*n];
1048
1049
1050 #define IDX(_x_,_y_)    ((_y_)*n+(_x_))
1051                 for(j=0;j<n;++j)
1052                 {
1053                         for(i=0;i<n;++i)
1054                         {
1055                                 if(i!=j)
1056                                 {
1057                                         adj[IDX(i,j)]=adj[IDX(j,i)]=inf;
1058                                 }
1059                                 else
1060                                 {
1061                                         adj[IDX(i,j)]=adj[IDX(j,i)]=0;
1062                                 }
1063                         }
1064                 }
1065                 for( i=0;i<m_links.size();++i)
1066                 {
1067                         const int       ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
1068                         const int       ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
1069                         adj[IDX(ia,ib)]=1;
1070                         adj[IDX(ib,ia)]=1;
1071                 }
1072
1073
1074                 //special optimized case for distance == 2
1075                 if (distance == 2)
1076                 {
1077
1078                         btAlignedObjectArray<NodeLinks> nodeLinks;
1079
1080
1081                         /* Build node links */
1082                         nodeLinks.resize(m_nodes.size());
1083
1084                         for( i=0;i<m_links.size();++i)
1085                         {
1086                                 const int       ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
1087                                 const int       ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
1088                                 if (nodeLinks[ia].m_links.findLinearSearch(ib)==nodeLinks[ia].m_links.size())
1089                                         nodeLinks[ia].m_links.push_back(ib);
1090
1091                                 if (nodeLinks[ib].m_links.findLinearSearch(ia)==nodeLinks[ib].m_links.size())
1092                                         nodeLinks[ib].m_links.push_back(ia);
1093                         }
1094                         for (int ii=0;ii<nodeLinks.size();ii++)
1095                         {
1096                                 int i=ii;
1097
1098                                 for (int jj=0;jj<nodeLinks[ii].m_links.size();jj++)
1099                                 {
1100                                         int k = nodeLinks[ii].m_links[jj];
1101                                         for (int kk=0;kk<nodeLinks[k].m_links.size();kk++)
1102                                         {
1103                                                 int j = nodeLinks[k].m_links[kk];
1104                                                 if (i!=j)
1105                                                 {
1106                                                         const unsigned  sum=adj[IDX(i,k)]+adj[IDX(k,j)];
1107                                                         btAssert(sum==2);
1108                                                         if(adj[IDX(i,j)]>sum)
1109                                                         {
1110                                                                 adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
1111                                                         }
1112                                                 }
1113
1114                                         }
1115                                 }
1116                         }
1117                 } else
1118                 {
1119                         ///generic Floyd's algorithm
1120                         for(int k=0;k<n;++k)
1121                         {
1122                                 for(j=0;j<n;++j)
1123                                 {
1124                                         for(i=j+1;i<n;++i)
1125                                         {
1126                                                 const unsigned  sum=adj[IDX(i,k)]+adj[IDX(k,j)];
1127                                                 if(adj[IDX(i,j)]>sum)
1128                                                 {
1129                                                         adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
1130                                                 }
1131                                         }
1132                                 }
1133                         }
1134                 }
1135
1136
1137                 /* Build links  */
1138                 int     nlinks=0;
1139                 for(j=0;j<n;++j)
1140                 {
1141                         for(i=j+1;i<n;++i)
1142                         {
1143                                 if(adj[IDX(i,j)]==(unsigned)distance)
1144                                 {
1145                                         appendLink(i,j,mat);
1146                                         m_links[m_links.size()-1].m_bbending=1;
1147                                         ++nlinks;
1148                                 }
1149                         }
1150                 }
1151                 delete[] adj;
1152                 return(nlinks);
1153         }
1154         return(0);
1155 }
1156
1157 //
1158 void                    btSoftBody::randomizeConstraints()
1159 {
1160         unsigned long   seed=243703;
1161 #define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff)
1162         int i,ni;
1163
1164         for(i=0,ni=m_links.size();i<ni;++i)
1165         {
1166                 btSwap(m_links[i],m_links[NEXTRAND%ni]);
1167         }
1168         for(i=0,ni=m_faces.size();i<ni;++i)
1169         {
1170                 btSwap(m_faces[i],m_faces[NEXTRAND%ni]);
1171         }
1172 #undef NEXTRAND
1173 }
1174
1175 //
1176 void                    btSoftBody::releaseCluster(int index)
1177 {
1178         Cluster*        c=m_clusters[index];
1179         if(c->m_leaf) m_cdbvt.remove(c->m_leaf);
1180         c->~Cluster();
1181         btAlignedFree(c);
1182         m_clusters.remove(c);
1183 }
1184
1185 //
1186 void                    btSoftBody::releaseClusters()
1187 {
1188         while(m_clusters.size()>0) releaseCluster(0);
1189 }
1190
1191 //
1192 int                             btSoftBody::generateClusters(int k,int maxiterations)
1193 {
1194         int i;
1195         releaseClusters();
1196         m_clusters.resize(btMin(k,m_nodes.size()));
1197         for(i=0;i<m_clusters.size();++i)
1198         {
1199                 m_clusters[i]                   =       new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
1200                 m_clusters[i]->m_collide=       true;
1201         }
1202         k=m_clusters.size();
1203         if(k>0)
1204         {
1205                 /* Initialize           */
1206                 btAlignedObjectArray<btVector3> centers;
1207                 btVector3                                               cog(0,0,0);
1208                 int                                                             i;
1209                 for(i=0;i<m_nodes.size();++i)
1210                 {
1211                         cog+=m_nodes[i].m_x;
1212                         m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
1213                 }
1214                 cog/=(btScalar)m_nodes.size();
1215                 centers.resize(k,cog);
1216                 /* Iterate                      */
1217                 const btScalar  slope=16;
1218                 bool                    changed;
1219                 int                             iterations=0;
1220                 do      {
1221                         const btScalar  w=2-btMin<btScalar>(1,iterations/slope);
1222                         changed=false;
1223                         iterations++;
1224                         int i;
1225
1226                         for(i=0;i<k;++i)
1227                         {
1228                                 btVector3       c(0,0,0);
1229                                 for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
1230                                 {
1231                                         c+=m_clusters[i]->m_nodes[j]->m_x;
1232                                 }
1233                                 if(m_clusters[i]->m_nodes.size())
1234                                 {
1235                                         c                       /=      (btScalar)m_clusters[i]->m_nodes.size();
1236                                         c                       =       centers[i]+(c-centers[i])*w;
1237                                         changed         |=      ((c-centers[i]).length2()>SIMD_EPSILON);
1238                                         centers[i]      =       c;
1239                                         m_clusters[i]->m_nodes.resize(0);
1240                                 }
1241                         }
1242                         for(i=0;i<m_nodes.size();++i)
1243                         {
1244                                 const btVector3 nx=m_nodes[i].m_x;
1245                                 int                             kbest=0;
1246                                 btScalar                kdist=ClusterMetric(centers[0],nx);
1247                                 for(int j=1;j<k;++j)
1248                                 {
1249                                         const btScalar  d=ClusterMetric(centers[j],nx);
1250                                         if(d<kdist)
1251                                         {
1252                                                 kbest=j;
1253                                                 kdist=d;
1254                                         }
1255                                 }
1256                                 m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
1257                         }
1258                 } while(changed&&(iterations<maxiterations));
1259                 /* Merge                */
1260                 btAlignedObjectArray<int>       cids;
1261                 cids.resize(m_nodes.size(),-1);
1262                 for(i=0;i<m_clusters.size();++i)
1263                 {
1264                         for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
1265                         {
1266                                 cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i;
1267                         }
1268                 }
1269                 for(i=0;i<m_faces.size();++i)
1270                 {
1271                         const int idx[]={       int(m_faces[i].m_n[0]-&m_nodes[0]),
1272                                 int(m_faces[i].m_n[1]-&m_nodes[0]),
1273                                 int(m_faces[i].m_n[2]-&m_nodes[0])};
1274                         for(int j=0;j<3;++j)
1275                         {
1276                                 const int cid=cids[idx[j]];
1277                                 for(int q=1;q<3;++q)
1278                                 {
1279                                         const int kid=idx[(j+q)%3];
1280                                         if(cids[kid]!=cid)
1281                                         {
1282                                                 if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size())
1283                                                 {
1284                                                         m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
1285                                                 }
1286                                         }
1287                                 }
1288                         }
1289                 }
1290                 /* Master               */
1291                 if(m_clusters.size()>1)
1292                 {
1293                         Cluster*        pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
1294                         pmaster->m_collide      =       false;
1295                         pmaster->m_nodes.reserve(m_nodes.size());
1296                         for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]);
1297                         m_clusters.push_back(pmaster);
1298                         btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]);
1299                 }
1300                 /* Terminate    */
1301                 for(i=0;i<m_clusters.size();++i)
1302                 {
1303                         if(m_clusters[i]->m_nodes.size()==0)
1304                         {
1305                                 releaseCluster(i--);
1306                         }
1307                 }
1308         } else
1309         {
1310                 //create a cluster for each tetrahedron (if tetrahedra exist) or each face
1311                 if (m_tetras.size())
1312                 {
1313                         m_clusters.resize(m_tetras.size());
1314                         for(i=0;i<m_clusters.size();++i)
1315                         {
1316                                 m_clusters[i]                   =       new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
1317                                 m_clusters[i]->m_collide=       true;
1318                         }
1319                         for (i=0;i<m_tetras.size();i++)
1320                         {
1321                                 for (int j=0;j<4;j++)
1322                                 {
1323                                         m_clusters[i]->m_nodes.push_back(m_tetras[i].m_n[j]);
1324                                 }
1325                         }
1326
1327                 } else
1328                 {
1329                         m_clusters.resize(m_faces.size());
1330                         for(i=0;i<m_clusters.size();++i)
1331                         {
1332                                 m_clusters[i]                   =       new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
1333                                 m_clusters[i]->m_collide=       true;
1334                         }
1335
1336                         for(i=0;i<m_faces.size();++i)
1337                         {
1338                                 for(int j=0;j<3;++j)
1339                                 {
1340                                         m_clusters[i]->m_nodes.push_back(m_faces[i].m_n[j]);
1341                                 }
1342                         }
1343                 }
1344         }
1345
1346         if (m_clusters.size())
1347         {
1348                 initializeClusters();
1349                 updateClusters();
1350
1351
1352                 //for self-collision
1353                 m_clusterConnectivity.resize(m_clusters.size()*m_clusters.size());
1354                 {
1355                         for (int c0=0;c0<m_clusters.size();c0++)
1356                         {
1357                                 m_clusters[c0]->m_clusterIndex=c0;
1358                                 for (int c1=0;c1<m_clusters.size();c1++)
1359                                 {
1360
1361                                         bool connected=false;
1362                                         Cluster* cla = m_clusters[c0];
1363                                         Cluster* clb = m_clusters[c1];
1364                                         for (int i=0;!connected&&i<cla->m_nodes.size();i++)
1365                                         {
1366                                                 for (int j=0;j<clb->m_nodes.size();j++)
1367                                                 {
1368                                                         if (cla->m_nodes[i] == clb->m_nodes[j])
1369                                                         {
1370                                                                 connected=true;
1371                                                                 break;
1372                                                         }
1373                                                 }
1374                                         }
1375                                         m_clusterConnectivity[c0+c1*m_clusters.size()]=connected;
1376                                 }
1377                         }
1378                 }
1379         }
1380
1381         return(m_clusters.size());
1382 }
1383
1384 //
1385 void                    btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut)
1386 {
1387         const Node*                     nbase = &m_nodes[0];
1388         int                                     ncount = m_nodes.size();
1389         btSymMatrix<int>        edges(ncount,-2);
1390         int                                     newnodes=0;
1391         int i,j,k,ni;
1392
1393         /* Filter out           */
1394         for(i=0;i<m_links.size();++i)
1395         {
1396                 Link&   l=m_links[i];
1397                 if(l.m_bbending)
1398                 {
1399                         if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
1400                         {
1401                                 btSwap(m_links[i],m_links[m_links.size()-1]);
1402                                 m_links.pop_back();--i;
1403                         }
1404                 }
1405         }
1406         /* Fill edges           */
1407         for(i=0;i<m_links.size();++i)
1408         {
1409                 Link&   l=m_links[i];
1410                 edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1;
1411         }
1412         for(i=0;i<m_faces.size();++i)
1413         {
1414                 Face&   f=m_faces[i];
1415                 edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1;
1416                 edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
1417                 edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
1418         }
1419         /* Intersect            */
1420         for(i=0;i<ncount;++i)
1421         {
1422                 for(j=i+1;j<ncount;++j)
1423                 {
1424                         if(edges(i,j)==-1)
1425                         {
1426                                 Node&                   a=m_nodes[i];
1427                                 Node&                   b=m_nodes[j];
1428                                 const btScalar  t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary);
1429                                 if(t>0)
1430                                 {
1431                                         const btVector3 x=Lerp(a.m_x,b.m_x,t);
1432                                         const btVector3 v=Lerp(a.m_v,b.m_v,t);
1433                                         btScalar                m=0;
1434                                         if(a.m_im>0)
1435                                         {
1436                                                 if(b.m_im>0)
1437                                                 {
1438                                                         const btScalar  ma=1/a.m_im;
1439                                                         const btScalar  mb=1/b.m_im;
1440                                                         const btScalar  mc=Lerp(ma,mb,t);
1441                                                         const btScalar  f=(ma+mb)/(ma+mb+mc);
1442                                                         a.m_im=1/(ma*f);
1443                                                         b.m_im=1/(mb*f);
1444                                                         m=mc*f;
1445                                                 }
1446                                                 else
1447                                                 { a.m_im/=0.5f;m=1/a.m_im; }
1448                                         }
1449                                         else
1450                                         {
1451                                                 if(b.m_im>0)
1452                                                 { b.m_im/=0.5f;m=1/b.m_im; }
1453                                                 else
1454                                                         m=0;
1455                                         }
1456                                         appendNode(x,m);
1457                                         edges(i,j)=m_nodes.size()-1;
1458                                         m_nodes[edges(i,j)].m_v=v;
1459                                         ++newnodes;
1460                                 }
1461                         }
1462                 }
1463         }
1464         nbase=&m_nodes[0];
1465         /* Refine links         */
1466         for(i=0,ni=m_links.size();i<ni;++i)
1467         {
1468                 Link&           feat=m_links[i];
1469                 const int       idx[]={ int(feat.m_n[0]-nbase),
1470                         int(feat.m_n[1]-nbase)};
1471                 if((idx[0]<ncount)&&(idx[1]<ncount))
1472                 {
1473                         const int ni=edges(idx[0],idx[1]);
1474                         if(ni>0)
1475                         {
1476                                 appendLink(i);
1477                                 Link*           pft[]={ &m_links[i],
1478                                         &m_links[m_links.size()-1]};
1479                                 pft[0]->m_n[0]=&m_nodes[idx[0]];
1480                                 pft[0]->m_n[1]=&m_nodes[ni];
1481                                 pft[1]->m_n[0]=&m_nodes[ni];
1482                                 pft[1]->m_n[1]=&m_nodes[idx[1]];
1483                         }
1484                 }
1485         }
1486         /* Refine faces         */
1487         for(i=0;i<m_faces.size();++i)
1488         {
1489                 const Face&     feat=m_faces[i];
1490                 const int       idx[]={ int(feat.m_n[0]-nbase),
1491                         int(feat.m_n[1]-nbase),
1492                         int(feat.m_n[2]-nbase)};
1493                 for(j=2,k=0;k<3;j=k++)
1494                 {
1495                         if((idx[j]<ncount)&&(idx[k]<ncount))
1496                         {
1497                                 const int ni=edges(idx[j],idx[k]);
1498                                 if(ni>0)
1499                                 {
1500                                         appendFace(i);
1501                                         const int       l=(k+1)%3;
1502                                         Face*           pft[]={ &m_faces[i],
1503                                                 &m_faces[m_faces.size()-1]};
1504                                         pft[0]->m_n[0]=&m_nodes[idx[l]];
1505                                         pft[0]->m_n[1]=&m_nodes[idx[j]];
1506                                         pft[0]->m_n[2]=&m_nodes[ni];
1507                                         pft[1]->m_n[0]=&m_nodes[ni];
1508                                         pft[1]->m_n[1]=&m_nodes[idx[k]];
1509                                         pft[1]->m_n[2]=&m_nodes[idx[l]];
1510                                         appendLink(ni,idx[l],pft[0]->m_material);
1511                                         --i;break;
1512                                 }
1513                         }
1514                 }
1515         }
1516         /* Cut                          */
1517         if(cut)
1518         {
1519                 btAlignedObjectArray<int>       cnodes;
1520                 const int                                       pcount=ncount;
1521                 int                                                     i;
1522                 ncount=m_nodes.size();
1523                 cnodes.resize(ncount,0);
1524                 /* Nodes                */
1525                 for(i=0;i<ncount;++i)
1526                 {
1527                         const btVector3 x=m_nodes[i].m_x;
1528                         if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary))
1529                         {
1530                                 const btVector3 v=m_nodes[i].m_v;
1531                                 btScalar                m=getMass(i);
1532                                 if(m>0) { m*=0.5f;m_nodes[i].m_im/=0.5f; }
1533                                 appendNode(x,m);
1534                                 cnodes[i]=m_nodes.size()-1;
1535                                 m_nodes[cnodes[i]].m_v=v;
1536                         }
1537                 }
1538                 nbase=&m_nodes[0];
1539                 /* Links                */
1540                 for(i=0,ni=m_links.size();i<ni;++i)
1541                 {
1542                         const int               id[]={  int(m_links[i].m_n[0]-nbase),
1543                                 int(m_links[i].m_n[1]-nbase)};
1544                         int                             todetach=0;
1545                         if(cnodes[id[0]]&&cnodes[id[1]])
1546                         {
1547                                 appendLink(i);
1548                                 todetach=m_links.size()-1;
1549                         }
1550                         else
1551                         {
1552                                 if((    (ifn->Eval(m_nodes[id[0]].m_x)<accurary)&&
1553                                         (ifn->Eval(m_nodes[id[1]].m_x)<accurary)))
1554                                         todetach=i;
1555                         }
1556                         if(todetach)
1557                         {
1558                                 Link&   l=m_links[todetach];
1559                                 for(int j=0;j<2;++j)
1560                                 {
1561                                         int cn=cnodes[int(l.m_n[j]-nbase)];
1562                                         if(cn) l.m_n[j]=&m_nodes[cn];
1563                                 }
1564                         }
1565                 }
1566                 /* Faces                */
1567                 for(i=0,ni=m_faces.size();i<ni;++i)
1568                 {
1569                         Node**                  n=      m_faces[i].m_n;
1570                         if(     (ifn->Eval(n[0]->m_x)<accurary)&&
1571                                 (ifn->Eval(n[1]->m_x)<accurary)&&
1572                                 (ifn->Eval(n[2]->m_x)<accurary))
1573                         {
1574                                 for(int j=0;j<3;++j)
1575                                 {
1576                                         int cn=cnodes[int(n[j]-nbase)];
1577                                         if(cn) n[j]=&m_nodes[cn];
1578                                 }
1579                         }
1580                 }
1581                 /* Clean orphans        */
1582                 int                                                     nnodes=m_nodes.size();
1583                 btAlignedObjectArray<int>       ranks;
1584                 btAlignedObjectArray<int>       todelete;
1585                 ranks.resize(nnodes,0);
1586                 for(i=0,ni=m_links.size();i<ni;++i)
1587                 {
1588                         for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++;
1589                 }
1590                 for(i=0,ni=m_faces.size();i<ni;++i)
1591                 {
1592                         for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
1593                 }
1594                 for(i=0;i<m_links.size();++i)
1595                 {
1596                         const int       id[]={  int(m_links[i].m_n[0]-nbase),
1597                                 int(m_links[i].m_n[1]-nbase)};
1598                         const bool      sg[]={  ranks[id[0]]==1,
1599                                 ranks[id[1]]==1};
1600                         if(sg[0]||sg[1])
1601                         {
1602                                 --ranks[id[0]];
1603                                 --ranks[id[1]];
1604                                 btSwap(m_links[i],m_links[m_links.size()-1]);
1605                                 m_links.pop_back();--i;
1606                         }
1607                 }
1608 #if 0
1609                 for(i=nnodes-1;i>=0;--i)
1610                 {
1611                         if(!ranks[i]) todelete.push_back(i);
1612                 }
1613                 if(todelete.size())
1614                 {
1615                         btAlignedObjectArray<int>&      map=ranks;
1616                         for(int i=0;i<nnodes;++i) map[i]=i;
1617                         PointersToIndices(this);
1618                         for(int i=0,ni=todelete.size();i<ni;++i)
1619                         {
1620                                 int             j=todelete[i];
1621                                 int&    a=map[j];
1622                                 int&    b=map[--nnodes];
1623                                 m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
1624                                 btSwap(m_nodes[a],m_nodes[b]);
1625                                 j=a;a=b;b=j;
1626                         }
1627                         IndicesToPointers(this,&map[0]);
1628                         m_nodes.resize(nnodes);
1629                 }
1630 #endif
1631         }
1632         m_bUpdateRtCst=true;
1633 }
1634
1635 //
1636 bool                    btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position)
1637 {
1638         return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position));
1639 }
1640
1641 //
1642 bool                    btSoftBody::cutLink(int node0,int node1,btScalar position)
1643 {
1644         bool                    done=false;
1645         int i,ni;
1646 //      const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x;
1647         const btVector3 x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position);
1648         const btVector3 v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position);
1649         const btScalar  m=1;
1650         appendNode(x,m);
1651         appendNode(x,m);
1652         Node*                   pa=&m_nodes[node0];
1653         Node*                   pb=&m_nodes[node1];
1654         Node*                   pn[2]={ &m_nodes[m_nodes.size()-2],
1655                 &m_nodes[m_nodes.size()-1]};
1656         pn[0]->m_v=v;
1657         pn[1]->m_v=v;
1658         for(i=0,ni=m_links.size();i<ni;++i)
1659         {
1660                 const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb);
1661                 if(mtch!=-1)
1662                 {
1663                         appendLink(i);
1664                         Link*   pft[]={&m_links[i],&m_links[m_links.size()-1]};
1665                         pft[0]->m_n[1]=pn[mtch];
1666                         pft[1]->m_n[0]=pn[1-mtch];
1667                         done=true;
1668                 }
1669         }
1670         for(i=0,ni=m_faces.size();i<ni;++i)
1671         {
1672                 for(int k=2,l=0;l<3;k=l++)
1673                 {
1674                         const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb);
1675                         if(mtch!=-1)
1676                         {
1677                                 appendFace(i);
1678                                 Face*   pft[]={&m_faces[i],&m_faces[m_faces.size()-1]};
1679                                 pft[0]->m_n[l]=pn[mtch];
1680                                 pft[1]->m_n[k]=pn[1-mtch];
1681                                 appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
1682                                 appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
1683                         }
1684                 }
1685         }
1686         if(!done)
1687         {
1688                 m_ndbvt.remove(pn[0]->m_leaf);
1689                 m_ndbvt.remove(pn[1]->m_leaf);
1690                 m_nodes.pop_back();
1691                 m_nodes.pop_back();
1692         }
1693         return(done);
1694 }
1695
1696 //
1697 bool                    btSoftBody::rayTest(const btVector3& rayFrom,
1698                                                                         const btVector3& rayTo,
1699                                                                         sRayCast& results)
1700 {
1701         if(m_faces.size()&&m_fdbvt.empty())
1702                 initializeFaceTree();
1703
1704         results.body    =       this;
1705         results.fraction = 1.f;
1706         results.feature =       eFeature::None;
1707         results.index   =       -1;
1708
1709         return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0);
1710 }
1711
1712 //
1713 void                    btSoftBody::setSolver(eSolverPresets::_ preset)
1714 {
1715         m_cfg.m_vsequence.clear();
1716         m_cfg.m_psequence.clear();
1717         m_cfg.m_dsequence.clear();
1718         switch(preset)
1719         {
1720         case    eSolverPresets::Positions:
1721                 m_cfg.m_psequence.push_back(ePSolver::Anchors);
1722                 m_cfg.m_psequence.push_back(ePSolver::RContacts);
1723                 m_cfg.m_psequence.push_back(ePSolver::SContacts);
1724                 m_cfg.m_psequence.push_back(ePSolver::Linear);
1725                 break;
1726         case    eSolverPresets::Velocities:
1727                 m_cfg.m_vsequence.push_back(eVSolver::Linear);
1728
1729                 m_cfg.m_psequence.push_back(ePSolver::Anchors);
1730                 m_cfg.m_psequence.push_back(ePSolver::RContacts);
1731                 m_cfg.m_psequence.push_back(ePSolver::SContacts);
1732
1733                 m_cfg.m_dsequence.push_back(ePSolver::Linear);
1734                 break;
1735         }
1736 }
1737
1738 //
1739 void                    btSoftBody::predictMotion(btScalar dt)
1740 {
1741
1742         int i,ni;
1743
1744         /* Update                               */
1745         if(m_bUpdateRtCst)
1746         {
1747                 m_bUpdateRtCst=false;
1748                 updateConstants();
1749                 m_fdbvt.clear();
1750                 if(m_cfg.collisions&fCollision::VF_SS)
1751                 {
1752                         initializeFaceTree();
1753                 }
1754         }
1755
1756         /* Prepare                              */
1757         m_sst.sdt               =       dt*m_cfg.timescale;
1758         m_sst.isdt              =       1/m_sst.sdt;
1759         m_sst.velmrg    =       m_sst.sdt*3;
1760         m_sst.radmrg    =       getCollisionShape()->getMargin();
1761         m_sst.updmrg    =       m_sst.radmrg*(btScalar)0.25;
1762         /* Forces                               */
1763         addVelocity(m_worldInfo->m_gravity*m_sst.sdt);
1764         applyForces();
1765         /* Integrate                    */
1766         for(i=0,ni=m_nodes.size();i<ni;++i)
1767         {
1768                 Node&   n=m_nodes[i];
1769                 n.m_q   =       n.m_x;
1770                 n.m_v   +=      n.m_f*n.m_im*m_sst.sdt;
1771                 n.m_x   +=      n.m_v*m_sst.sdt;
1772                 n.m_f   =       btVector3(0,0,0);
1773         }
1774         /* Clusters                             */
1775         updateClusters();
1776         /* Bounds                               */
1777         updateBounds();
1778         /* Nodes                                */
1779         ATTRIBUTE_ALIGNED16(btDbvtVolume)       vol;
1780         for(i=0,ni=m_nodes.size();i<ni;++i)
1781         {
1782                 Node&   n=m_nodes[i];
1783                 vol = btDbvtVolume::FromCR(n.m_x,m_sst.radmrg);
1784                 m_ndbvt.update( n.m_leaf,
1785                         vol,
1786                         n.m_v*m_sst.velmrg,
1787                         m_sst.updmrg);
1788         }
1789         /* Faces                                */
1790         if(!m_fdbvt.empty())
1791         {
1792                 for(int i=0;i<m_faces.size();++i)
1793                 {
1794                         Face&                   f=m_faces[i];
1795                         const btVector3 v=(     f.m_n[0]->m_v+
1796                                 f.m_n[1]->m_v+
1797                                 f.m_n[2]->m_v)/3;
1798                         vol = VolumeOf(f,m_sst.radmrg);
1799                         m_fdbvt.update( f.m_leaf,
1800                                 vol,
1801                                 v*m_sst.velmrg,
1802                                 m_sst.updmrg);
1803                 }
1804         }
1805         /* Pose                                 */
1806         updatePose();
1807         /* Match                                */
1808         if(m_pose.m_bframe&&(m_cfg.kMT>0))
1809         {
1810                 const btMatrix3x3       posetrs=m_pose.m_rot;
1811                 for(int i=0,ni=m_nodes.size();i<ni;++i)
1812                 {
1813                         Node&   n=m_nodes[i];
1814                         if(n.m_im>0)
1815                         {
1816                                 const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com;
1817                                 n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
1818                         }
1819                 }
1820         }
1821         /* Clear contacts               */
1822         m_rcontacts.resize(0);
1823         m_scontacts.resize(0);
1824         /* Optimize dbvt's              */
1825         m_ndbvt.optimizeIncremental(1);
1826         m_fdbvt.optimizeIncremental(1);
1827         m_cdbvt.optimizeIncremental(1);
1828 }
1829
1830 //
1831 void                    btSoftBody::solveConstraints()
1832 {
1833
1834         /* Apply clusters               */
1835         applyClusters(false);
1836         /* Prepare links                */
1837
1838         int i,ni;
1839
1840         for(i=0,ni=m_links.size();i<ni;++i)
1841         {
1842                 Link&   l=m_links[i];
1843                 l.m_c3          =       l.m_n[1]->m_q-l.m_n[0]->m_q;
1844                 l.m_c2          =       1/(l.m_c3.length2()*l.m_c0);
1845         }
1846         /* Prepare anchors              */
1847         for(i=0,ni=m_anchors.size();i<ni;++i)
1848         {
1849                 Anchor&                 a=m_anchors[i];
1850                 const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
1851                 a.m_c0  =       ImpulseMatrix(  m_sst.sdt,
1852                         a.m_node->m_im,
1853                         a.m_body->getInvMass(),
1854                         a.m_body->getInvInertiaTensorWorld(),
1855                         ra);
1856                 a.m_c1  =       ra;
1857                 a.m_c2  =       m_sst.sdt*a.m_node->m_im;
1858                 a.m_body->activate();
1859         }
1860         /* Solve velocities             */
1861         if(m_cfg.viterations>0)
1862         {
1863                 /* Solve                        */
1864                 for(int isolve=0;isolve<m_cfg.viterations;++isolve)
1865                 {
1866                         for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
1867                         {
1868                                 getSolver(m_cfg.m_vsequence[iseq])(this,1);
1869                         }
1870                 }
1871                 /* Update                       */
1872                 for(i=0,ni=m_nodes.size();i<ni;++i)
1873                 {
1874                         Node&   n=m_nodes[i];
1875                         n.m_x   =       n.m_q+n.m_v*m_sst.sdt;
1876                 }
1877         }
1878         /* Solve positions              */
1879         if(m_cfg.piterations>0)
1880         {
1881                 for(int isolve=0;isolve<m_cfg.piterations;++isolve)
1882                 {
1883                         const btScalar ti=isolve/(btScalar)m_cfg.piterations;
1884                         for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
1885                         {
1886                                 getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
1887                         }
1888                 }
1889                 const btScalar  vc=m_sst.isdt*(1-m_cfg.kDP);
1890                 for(i=0,ni=m_nodes.size();i<ni;++i)
1891                 {
1892                         Node&   n=m_nodes[i];
1893                         n.m_v   =       (n.m_x-n.m_q)*vc;
1894                         n.m_f   =       btVector3(0,0,0);
1895                 }
1896         }
1897         /* Solve drift                  */
1898         if(m_cfg.diterations>0)
1899         {
1900                 const btScalar  vcf=m_cfg.kVCF*m_sst.isdt;
1901                 for(i=0,ni=m_nodes.size();i<ni;++i)
1902                 {
1903                         Node&   n=m_nodes[i];
1904                         n.m_q   =       n.m_x;
1905                 }
1906                 for(int idrift=0;idrift<m_cfg.diterations;++idrift)
1907                 {
1908                         for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
1909                         {
1910                                 getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
1911                         }
1912                 }
1913                 for(int i=0,ni=m_nodes.size();i<ni;++i)
1914                 {
1915                         Node&   n=m_nodes[i];
1916                         n.m_v   +=      (n.m_x-n.m_q)*vcf;
1917                 }
1918         }
1919         /* Apply clusters               */
1920         dampClusters();
1921         applyClusters(true);
1922 }
1923
1924 //
1925 void                    btSoftBody::staticSolve(int iterations)
1926 {
1927         for(int isolve=0;isolve<iterations;++isolve)
1928         {
1929                 for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
1930                 {
1931                         getSolver(m_cfg.m_psequence[iseq])(this,1,0);
1932                 }
1933         }
1934 }
1935
1936 //
1937 void                    btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/)
1938 {
1939         /// placeholder
1940 }
1941
1942 //
1943 void                    btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
1944 {
1945         const int       nb=bodies.size();
1946         int                     iterations=0;
1947         int i;
1948
1949         for(i=0;i<nb;++i)
1950         {
1951                 iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
1952         }
1953         for(i=0;i<nb;++i)
1954         {
1955                 bodies[i]->prepareClusters(iterations);
1956         }
1957         for(i=0;i<iterations;++i)
1958         {
1959                 const btScalar sor=1;
1960                 for(int j=0;j<nb;++j)
1961                 {
1962                         bodies[j]->solveClusters(sor);
1963                 }
1964         }
1965         for(i=0;i<nb;++i)
1966         {
1967                 bodies[i]->cleanupClusters();
1968         }
1969 }
1970
1971 //
1972 void                    btSoftBody::integrateMotion()
1973 {
1974         /* Update                       */
1975         updateNormals();
1976 }
1977
1978 //
1979 btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt)
1980 {
1981         m_rayFrom = rayFrom;
1982         m_rayNormalizedDirection = (rayTo-rayFrom);
1983         m_rayTo = rayTo;
1984         m_mint  =       mxt;
1985         m_face  =       0;
1986         m_tests =       0;
1987 }
1988
1989 //
1990 void                            btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf)
1991 {
1992         btSoftBody::Face&       f=*(btSoftBody::Face*)leaf->data;
1993         const btScalar          t=rayFromToTriangle(    m_rayFrom,m_rayTo,m_rayNormalizedDirection,
1994                 f.m_n[0]->m_x,
1995                 f.m_n[1]->m_x,
1996                 f.m_n[2]->m_x,
1997                 m_mint);
1998         if((t>0)&&(t<m_mint))
1999         {
2000                 m_mint=t;m_face=&f;
2001         }
2002         ++m_tests;
2003 }
2004
2005 //
2006 btScalar                        btSoftBody::RayFromToCaster::rayFromToTriangle( const btVector3& rayFrom,
2007                                                                                                                                    const btVector3& rayTo,
2008                                                                                                                                    const btVector3& rayNormalizedDirection,
2009                                                                                                                                    const btVector3& a,
2010                                                                                                                                    const btVector3& b,
2011                                                                                                                                    const btVector3& c,
2012                                                                                                                                    btScalar maxt)
2013 {
2014         static const btScalar   ceps=-SIMD_EPSILON*10;
2015         static const btScalar   teps=SIMD_EPSILON*10;
2016
2017         const btVector3                 n=btCross(b-a,c-a);
2018         const btScalar                  d=btDot(a,n);
2019         const btScalar                  den=btDot(rayNormalizedDirection,n);
2020         if(!btFuzzyZero(den))
2021         {
2022                 const btScalar          num=btDot(rayFrom,n)-d;
2023                 const btScalar          t=-num/den;
2024                 if((t>teps)&&(t<maxt))
2025                 {
2026                         const btVector3 hit=rayFrom+rayNormalizedDirection*t;
2027                         if(     (btDot(n,btCross(a-hit,b-hit))>ceps)    &&
2028                                 (btDot(n,btCross(b-hit,c-hit))>ceps)    &&
2029                                 (btDot(n,btCross(c-hit,a-hit))>ceps))
2030                         {
2031                                 return(t);
2032                         }
2033                 }
2034         }
2035         return(-1);
2036 }
2037
2038 //
2039 void                            btSoftBody::pointersToIndices()
2040 {
2041 #define PTR2IDX(_p_,_b_)        reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
2042         btSoftBody::Node*       base=m_nodes.size() ? &m_nodes[0] : 0;
2043         int i,ni;
2044
2045         for(i=0,ni=m_nodes.size();i<ni;++i)
2046         {
2047                 if(m_nodes[i].m_leaf)
2048                 {
2049                         m_nodes[i].m_leaf->data=*(void**)&i;
2050                 }
2051         }
2052         for(i=0,ni=m_links.size();i<ni;++i)
2053         {
2054                 m_links[i].m_n[0]=PTR2IDX(m_links[i].m_n[0],base);
2055                 m_links[i].m_n[1]=PTR2IDX(m_links[i].m_n[1],base);
2056         }
2057         for(i=0,ni=m_faces.size();i<ni;++i)
2058         {
2059                 m_faces[i].m_n[0]=PTR2IDX(m_faces[i].m_n[0],base);
2060                 m_faces[i].m_n[1]=PTR2IDX(m_faces[i].m_n[1],base);
2061                 m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base);
2062                 if(m_faces[i].m_leaf)
2063                 {
2064                         m_faces[i].m_leaf->data=*(void**)&i;
2065                 }
2066         }
2067         for(i=0,ni=m_anchors.size();i<ni;++i)
2068         {
2069                 m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base);
2070         }
2071         for(i=0,ni=m_notes.size();i<ni;++i)
2072         {
2073                 for(int j=0;j<m_notes[i].m_rank;++j)
2074                 {
2075                         m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base);
2076                 }
2077         }
2078 #undef  PTR2IDX
2079 }
2080
2081 //
2082 void                            btSoftBody::indicesToPointers(const int* map)
2083 {
2084 #define IDX2PTR(_p_,_b_)        map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]):     \
2085         (&(_b_)[(((char*)_p_)-(char*)0)])
2086         btSoftBody::Node*       base=m_nodes.size() ? &m_nodes[0]:0;
2087         int i,ni;
2088
2089         for(i=0,ni=m_nodes.size();i<ni;++i)
2090         {
2091                 if(m_nodes[i].m_leaf)
2092                 {
2093                         m_nodes[i].m_leaf->data=&m_nodes[i];
2094                 }
2095         }
2096         for(i=0,ni=m_links.size();i<ni;++i)
2097         {
2098                 m_links[i].m_n[0]=IDX2PTR(m_links[i].m_n[0],base);
2099                 m_links[i].m_n[1]=IDX2PTR(m_links[i].m_n[1],base);
2100         }
2101         for(i=0,ni=m_faces.size();i<ni;++i)
2102         {
2103                 m_faces[i].m_n[0]=IDX2PTR(m_faces[i].m_n[0],base);
2104                 m_faces[i].m_n[1]=IDX2PTR(m_faces[i].m_n[1],base);
2105                 m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base);
2106                 if(m_faces[i].m_leaf)
2107                 {
2108                         m_faces[i].m_leaf->data=&m_faces[i];
2109                 }
2110         }
2111         for(i=0,ni=m_anchors.size();i<ni;++i)
2112         {
2113                 m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base);
2114         }
2115         for(i=0,ni=m_notes.size();i<ni;++i)
2116         {
2117                 for(int j=0;j<m_notes[i].m_rank;++j)
2118                 {
2119                         m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base);
2120                 }
2121         }
2122 #undef  IDX2PTR
2123 }
2124
2125 //
2126 int                                     btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
2127                                                                                 btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const
2128 {
2129         int     cnt=0;
2130         btVector3 dir = rayTo-rayFrom;
2131
2132
2133         if(bcountonly||m_fdbvt.empty())
2134         {/* Full search */
2135
2136                 for(int i=0,ni=m_faces.size();i<ni;++i)
2137                 {
2138                         const btSoftBody::Face& f=m_faces[i];
2139
2140                         const btScalar                  t=RayFromToCaster::rayFromToTriangle(   rayFrom,rayTo,dir,
2141                                 f.m_n[0]->m_x,
2142                                 f.m_n[1]->m_x,
2143                                 f.m_n[2]->m_x,
2144                                 mint);
2145                         if(t>0)
2146                         {
2147                                 ++cnt;
2148                                 if(!bcountonly)
2149                                 {
2150                                         feature=btSoftBody::eFeature::Face;
2151                                         index=i;
2152                                         mint=t;
2153                                 }
2154                         }
2155                 }
2156         }
2157         else
2158         {/* Use dbvt    */
2159                 RayFromToCaster collider(rayFrom,rayTo,mint);
2160
2161                 btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider);
2162                 if(collider.m_face)
2163                 {
2164                         mint=collider.m_mint;
2165                         feature=btSoftBody::eFeature::Face;
2166                         index=(int)(collider.m_face-&m_faces[0]);
2167                         cnt=1;
2168                 }
2169         }
2170
2171         for (int i=0;i<m_tetras.size();i++)
2172         {
2173                 const btSoftBody::Tetra& tet = m_tetras[i];
2174                 int tetfaces[4][3] = {{0,1,2},{0,1,3},{1,2,3},{0,2,3}};
2175                 for (int f=0;f<4;f++)
2176                 {
2177
2178                         int index0=tetfaces[f][0];
2179                         int index1=tetfaces[f][1];
2180                         int index2=tetfaces[f][2];
2181                         btVector3 v0=tet.m_n[index0]->m_x;
2182                         btVector3 v1=tet.m_n[index1]->m_x;
2183                         btVector3 v2=tet.m_n[index2]->m_x;
2184
2185
2186                 const btScalar                  t=RayFromToCaster::rayFromToTriangle(   rayFrom,rayTo,dir,
2187                         v0,v1,v2,
2188                                 mint);
2189                 if(t>0)
2190                         {
2191                                 ++cnt;
2192                                 if(!bcountonly)
2193                                 {
2194                                         feature=btSoftBody::eFeature::Tetra;
2195                                         index=i;
2196                                         mint=t;
2197                                 }
2198                         }
2199                 }
2200         }
2201         return(cnt);
2202 }
2203
2204 //
2205 void                    btSoftBody::initializeFaceTree()
2206 {
2207         m_fdbvt.clear();
2208         for(int i=0;i<m_faces.size();++i)
2209         {
2210                 Face&   f=m_faces[i];
2211                 f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
2212         }
2213 }
2214
2215 //
2216 btVector3               btSoftBody::evaluateCom() const
2217 {
2218         btVector3       com(0,0,0);
2219         if(m_pose.m_bframe)
2220         {
2221                 for(int i=0,ni=m_nodes.size();i<ni;++i)
2222                 {
2223                         com+=m_nodes[i].m_x*m_pose.m_wgh[i];
2224                 }
2225         }
2226         return(com);
2227 }
2228
2229 //
2230 bool                            btSoftBody::checkContact(       const btCollisionObjectWrapper* colObjWrap,
2231                                                                                          const btVector3& x,
2232                                                                                          btScalar margin,
2233                                                                                          btSoftBody::sCti& cti) const
2234 {
2235         btVector3 nrm;
2236         const btCollisionShape *shp = colObjWrap->getCollisionShape();
2237 //      const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
2238         //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
2239         const btTransform &wtr = colObjWrap->getWorldTransform();
2240         //todo: check which transform is needed here
2241
2242         btScalar dst =
2243                 m_worldInfo->m_sparsesdf.Evaluate(
2244                         wtr.invXform(x),
2245                         shp,
2246                         nrm,
2247                         margin);
2248         if(dst<0)
2249         {
2250                 cti.m_colObj = colObjWrap->getCollisionObject();
2251                 cti.m_normal = wtr.getBasis()*nrm;
2252                 cti.m_offset = -btDot( cti.m_normal, x - cti.m_normal * dst );
2253                 return(true);
2254         }
2255         return(false);
2256 }
2257
2258 //
2259 void                                    btSoftBody::updateNormals()
2260 {
2261
2262         const btVector3 zv(0,0,0);
2263         int i,ni;
2264
2265         for(i=0,ni=m_nodes.size();i<ni;++i)
2266         {
2267                 m_nodes[i].m_n=zv;
2268         }
2269         for(i=0,ni=m_faces.size();i<ni;++i)
2270         {
2271                 btSoftBody::Face&       f=m_faces[i];
2272                 const btVector3         n=btCross(f.m_n[1]->m_x-f.m_n[0]->m_x,
2273                         f.m_n[2]->m_x-f.m_n[0]->m_x);
2274                 f.m_normal=n.normalized();
2275                 f.m_n[0]->m_n+=n;
2276                 f.m_n[1]->m_n+=n;
2277                 f.m_n[2]->m_n+=n;
2278         }
2279         for(i=0,ni=m_nodes.size();i<ni;++i)
2280         {
2281                 btScalar len = m_nodes[i].m_n.length();
2282                 if (len>SIMD_EPSILON)
2283                         m_nodes[i].m_n /= len;
2284         }
2285 }
2286
2287 //
2288 void                                    btSoftBody::updateBounds()
2289 {
2290         /*if( m_acceleratedSoftBody )
2291         {
2292                 // If we have an accelerated softbody we need to obtain the bounds correctly
2293                 // For now (slightly hackily) just have a very large AABB
2294                 // TODO: Write get bounds kernel
2295                 // If that is updating in place, atomic collisions might be low (when the cloth isn't perfectly aligned to an axis) and we could
2296                 // probably do a test and exchange reasonably efficiently.
2297
2298                 m_bounds[0] = btVector3(-1000, -1000, -1000);
2299                 m_bounds[1] = btVector3(1000, 1000, 1000);
2300
2301         } else {*/
2302                 if(m_ndbvt.m_root)
2303                 {
2304                         const btVector3&        mins=m_ndbvt.m_root->volume.Mins();
2305                         const btVector3&        maxs=m_ndbvt.m_root->volume.Maxs();
2306                         const btScalar          csm=getCollisionShape()->getMargin();
2307                         const btVector3         mrg=btVector3(  csm,
2308                                 csm,
2309                                 csm)*1; // ??? to investigate...
2310                         m_bounds[0]=mins-mrg;
2311                         m_bounds[1]=maxs+mrg;
2312                         if(0!=getBroadphaseHandle())
2313                         {
2314                                 m_worldInfo->m_broadphase->setAabb(     getBroadphaseHandle(),
2315                                         m_bounds[0],
2316                                         m_bounds[1],
2317                                         m_worldInfo->m_dispatcher);
2318                         }
2319                 }
2320                 else
2321                 {
2322                         m_bounds[0]=
2323                                 m_bounds[1]=btVector3(0,0,0);
2324                 }
2325         //}
2326 }
2327
2328
2329 //
2330 void                                    btSoftBody::updatePose()
2331 {
2332         if(m_pose.m_bframe)
2333         {
2334                 btSoftBody::Pose&       pose=m_pose;
2335                 const btVector3         com=evaluateCom();
2336                 /* Com                  */
2337                 pose.m_com      =       com;
2338                 /* Rotation             */
2339                 btMatrix3x3             Apq;
2340                 const btScalar  eps=SIMD_EPSILON;
2341                 Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0);
2342                 Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3);
2343                 for(int i=0,ni=m_nodes.size();i<ni;++i)
2344                 {
2345                         const btVector3         a=pose.m_wgh[i]*(m_nodes[i].m_x-com);
2346                         const btVector3&        b=pose.m_pos[i];
2347                         Apq[0]+=a.x()*b;
2348                         Apq[1]+=a.y()*b;
2349                         Apq[2]+=a.z()*b;
2350                 }
2351                 btMatrix3x3             r,s;
2352                 PolarDecompose(Apq,r,s);
2353                 pose.m_rot=r;
2354                 pose.m_scl=pose.m_aqq*r.transpose()*Apq;
2355                 if(m_cfg.maxvolume>1)
2356                 {
2357                         const btScalar  idet=Clamp<btScalar>(   1/pose.m_scl.determinant(),
2358                                 1,m_cfg.maxvolume);
2359                         pose.m_scl=Mul(pose.m_scl,idet);
2360                 }
2361
2362         }
2363 }
2364
2365 //
2366 void                            btSoftBody::updateArea(bool averageArea)
2367 {
2368         int i,ni;
2369
2370         /* Face area            */
2371         for(i=0,ni=m_faces.size();i<ni;++i)
2372         {
2373                 Face&           f=m_faces[i];
2374                 f.m_ra  =       AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
2375         }
2376
2377         /* Node area            */
2378
2379         if (averageArea)
2380         {
2381                 btAlignedObjectArray<int>       counts;
2382                 counts.resize(m_nodes.size(),0);
2383                 for(i=0,ni=m_nodes.size();i<ni;++i)
2384                 {
2385                         m_nodes[i].m_area       =       0;
2386                 }
2387                 for(i=0,ni=m_faces.size();i<ni;++i)
2388                 {
2389                         btSoftBody::Face&       f=m_faces[i];
2390                         for(int j=0;j<3;++j)
2391                         {
2392                                 const int index=(int)(f.m_n[j]-&m_nodes[0]);
2393                                 counts[index]++;
2394                                 f.m_n[j]->m_area+=btFabs(f.m_ra);
2395                         }
2396                 }
2397                 for(i=0,ni=m_nodes.size();i<ni;++i)
2398                 {
2399                         if(counts[i]>0)
2400                                 m_nodes[i].m_area/=(btScalar)counts[i];
2401                         else
2402                                 m_nodes[i].m_area=0;
2403                 }
2404         }
2405         else
2406         {
2407                 // initialize node area as zero
2408                 for(i=0,ni=m_nodes.size();i<ni;++i)
2409                 {
2410                         m_nodes[i].m_area=0;
2411                 }
2412
2413                 for(i=0,ni=m_faces.size();i<ni;++i)
2414                 {
2415                         btSoftBody::Face&       f=m_faces[i];
2416
2417                         for(int j=0;j<3;++j)
2418                         {
2419                                 f.m_n[j]->m_area += f.m_ra;
2420                         }
2421                 }
2422
2423                 for(i=0,ni=m_nodes.size();i<ni;++i)
2424                 {
2425                         m_nodes[i].m_area *= 0.3333333f;
2426                 }
2427         }
2428 }
2429
2430
2431 void                            btSoftBody::updateLinkConstants()
2432 {
2433         int i,ni;
2434
2435         /* Links                */
2436         for(i=0,ni=m_links.size();i<ni;++i)
2437         {
2438                 Link&           l=m_links[i];
2439                 Material&       m=*l.m_material;
2440                 l.m_c0  =       (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST;
2441         }
2442 }
2443
2444 void                            btSoftBody::updateConstants()
2445 {
2446         resetLinkRestLengths();
2447         updateLinkConstants();
2448         updateArea();
2449 }
2450
2451
2452
2453 //
2454 void                                    btSoftBody::initializeClusters()
2455 {
2456         int i;
2457
2458         for( i=0;i<m_clusters.size();++i)
2459         {
2460                 Cluster&        c=*m_clusters[i];
2461                 c.m_imass=0;
2462                 c.m_masses.resize(c.m_nodes.size());
2463                 for(int j=0;j<c.m_nodes.size();++j)
2464                 {
2465                         if (c.m_nodes[j]->m_im==0)
2466                         {
2467                                 c.m_containsAnchor = true;
2468                                 c.m_masses[j]   =       BT_LARGE_FLOAT;
2469                         } else
2470                         {
2471                                 c.m_masses[j]   =       btScalar(1.)/c.m_nodes[j]->m_im;
2472                         }
2473                         c.m_imass               +=      c.m_masses[j];
2474                 }
2475                 c.m_imass               =       btScalar(1.)/c.m_imass;
2476                 c.m_com                 =       btSoftBody::clusterCom(&c);
2477                 c.m_lv                  =       btVector3(0,0,0);
2478                 c.m_av                  =       btVector3(0,0,0);
2479                 c.m_leaf                =       0;
2480                 /* Inertia      */
2481                 btMatrix3x3&    ii=c.m_locii;
2482                 ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
2483                 {
2484                         int i,ni;
2485
2486                         for(i=0,ni=c.m_nodes.size();i<ni;++i)
2487                         {
2488                                 const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
2489                                 const btVector3 q=k*k;
2490                                 const btScalar  m=c.m_masses[i];
2491                                 ii[0][0]        +=      m*(q[1]+q[2]);
2492                                 ii[1][1]        +=      m*(q[0]+q[2]);
2493                                 ii[2][2]        +=      m*(q[0]+q[1]);
2494                                 ii[0][1]        -=      m*k[0]*k[1];
2495                                 ii[0][2]        -=      m*k[0]*k[2];
2496                                 ii[1][2]        -=      m*k[1]*k[2];
2497                         }
2498                 }
2499                 ii[1][0]=ii[0][1];
2500                 ii[2][0]=ii[0][2];
2501                 ii[2][1]=ii[1][2];
2502
2503                 ii = ii.inverse();
2504
2505                 /* Frame        */
2506                 c.m_framexform.setIdentity();
2507                 c.m_framexform.setOrigin(c.m_com);
2508                 c.m_framerefs.resize(c.m_nodes.size());
2509                 {
2510                         int i;
2511                         for(i=0;i<c.m_framerefs.size();++i)
2512                         {
2513                                 c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
2514                         }
2515                 }
2516         }
2517 }
2518
2519 //
2520 void                                    btSoftBody::updateClusters()
2521 {
2522         BT_PROFILE("UpdateClusters");
2523         int i;
2524
2525         for(i=0;i<m_clusters.size();++i)
2526         {
2527                 btSoftBody::Cluster&    c=*m_clusters[i];
2528                 const int                               n=c.m_nodes.size();
2529                 //const btScalar                        invn=1/(btScalar)n;
2530                 if(n)
2531                 {
2532                         /* Frame                                */
2533                         const btScalar  eps=btScalar(0.0001);
2534                         btMatrix3x3             m,r,s;
2535                         m[0]=m[1]=m[2]=btVector3(0,0,0);
2536                         m[0][0]=eps*1;
2537                         m[1][1]=eps*2;
2538                         m[2][2]=eps*3;
2539                         c.m_com=clusterCom(&c);
2540                         for(int i=0;i<c.m_nodes.size();++i)
2541                         {
2542                                 const btVector3         a=c.m_nodes[i]->m_x-c.m_com;
2543                                 const btVector3&        b=c.m_framerefs[i];
2544                                 m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
2545                         }
2546                         PolarDecompose(m,r,s);
2547                         c.m_framexform.setOrigin(c.m_com);
2548                         c.m_framexform.setBasis(r);
2549                         /* Inertia                      */
2550 #if 1/* Constant        */
2551                         c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
2552 #else
2553 #if 0/* Sphere  */
2554                         const btScalar  rk=(2*c.m_extents.length2())/(5*c.m_imass);
2555                         const btVector3 inertia(rk,rk,rk);
2556                         const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
2557                                 btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
2558                                 btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
2559
2560                         c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
2561 #else/* Actual  */
2562                         c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0);
2563                         for(int i=0;i<n;++i)
2564                         {
2565                                 const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
2566                                 const btVector3         q=k*k;
2567                                 const btScalar          m=1/c.m_nodes[i]->m_im;
2568                                 c.m_invwi[0][0] +=      m*(q[1]+q[2]);
2569                                 c.m_invwi[1][1] +=      m*(q[0]+q[2]);
2570                                 c.m_invwi[2][2] +=      m*(q[0]+q[1]);
2571                                 c.m_invwi[0][1] -=      m*k[0]*k[1];
2572                                 c.m_invwi[0][2] -=      m*k[0]*k[2];
2573                                 c.m_invwi[1][2] -=      m*k[1]*k[2];
2574                         }
2575                         c.m_invwi[1][0]=c.m_invwi[0][1];
2576                         c.m_invwi[2][0]=c.m_invwi[0][2];
2577                         c.m_invwi[2][1]=c.m_invwi[1][2];
2578                         c.m_invwi=c.m_invwi.inverse();
2579 #endif
2580 #endif
2581                         /* Velocities                   */
2582                         c.m_lv=btVector3(0,0,0);
2583                         c.m_av=btVector3(0,0,0);
2584                         {
2585                                 int i;
2586
2587                                 for(i=0;i<n;++i)
2588                                 {
2589                                         const btVector3 v=c.m_nodes[i]->m_v*c.m_masses[i];
2590                                         c.m_lv  +=      v;
2591                                         c.m_av  +=      btCross(c.m_nodes[i]->m_x-c.m_com,v);
2592                                 }
2593                         }
2594                         c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
2595                         c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
2596                         c.m_vimpulses[0]        =
2597                                 c.m_vimpulses[1]        = btVector3(0,0,0);
2598                         c.m_dimpulses[0]        =
2599                                 c.m_dimpulses[1]        = btVector3(0,0,0);
2600                         c.m_nvimpulses          = 0;
2601                         c.m_ndimpulses          = 0;
2602                         /* Matching                             */
2603                         if(c.m_matching>0)
2604                         {
2605                                 for(int j=0;j<c.m_nodes.size();++j)
2606                                 {
2607                                         Node&                   n=*c.m_nodes[j];
2608                                         const btVector3 x=c.m_framexform*c.m_framerefs[j];
2609                                         n.m_x=Lerp(n.m_x,x,c.m_matching);
2610                                 }
2611                         }
2612                         /* Dbvt                                 */
2613                         if(c.m_collide)
2614                         {
2615                                 btVector3       mi=c.m_nodes[0]->m_x;
2616                                 btVector3       mx=mi;
2617                                 for(int j=1;j<n;++j)
2618                                 {
2619                                         mi.setMin(c.m_nodes[j]->m_x);
2620                                         mx.setMax(c.m_nodes[j]->m_x);
2621                                 }
2622                                 ATTRIBUTE_ALIGNED16(btDbvtVolume)       bounds=btDbvtVolume::FromMM(mi,mx);
2623                                 if(c.m_leaf)
2624                                         m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
2625                                 else
2626                                         c.m_leaf=m_cdbvt.insert(bounds,&c);
2627                         }
2628                 }
2629         }
2630
2631
2632 }
2633
2634
2635
2636
2637 //
2638 void                                    btSoftBody::cleanupClusters()
2639 {
2640         for(int i=0;i<m_joints.size();++i)
2641         {
2642                 m_joints[i]->Terminate(m_sst.sdt);
2643                 if(m_joints[i]->m_delete)
2644                 {
2645                         btAlignedFree(m_joints[i]);
2646                         m_joints.remove(m_joints[i--]);
2647                 }
2648         }
2649 }
2650
2651 //
2652 void                                    btSoftBody::prepareClusters(int iterations)
2653 {
2654         for(int i=0;i<m_joints.size();++i)
2655         {
2656                 m_joints[i]->Prepare(m_sst.sdt,iterations);
2657         }
2658 }
2659
2660
2661 //
2662 void                                    btSoftBody::solveClusters(btScalar sor)
2663 {
2664         for(int i=0,ni=m_joints.size();i<ni;++i)
2665         {
2666                 m_joints[i]->Solve(m_sst.sdt,sor);
2667         }
2668 }
2669
2670 //
2671 void                                    btSoftBody::applyClusters(bool drift)
2672 {
2673         BT_PROFILE("ApplyClusters");
2674 //      const btScalar                                  f0=m_sst.sdt;
2675         //const btScalar                                        f1=f0/2;
2676         btAlignedObjectArray<btVector3> deltas;
2677         btAlignedObjectArray<btScalar> weights;
2678         deltas.resize(m_nodes.size(),btVector3(0,0,0));
2679         weights.resize(m_nodes.size(),0);
2680         int i;
2681
2682         if(drift)
2683         {
2684                 for(i=0;i<m_clusters.size();++i)
2685                 {
2686                         Cluster&        c=*m_clusters[i];
2687                         if(c.m_ndimpulses)
2688                         {
2689                                 c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
2690                                 c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
2691                         }
2692                 }
2693         }
2694
2695         for(i=0;i<m_clusters.size();++i)
2696         {
2697                 Cluster&        c=*m_clusters[i];
2698                 if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
2699                 {
2700                         const btVector3         v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
2701                         const btVector3         w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
2702                         for(int j=0;j<c.m_nodes.size();++j)
2703                         {
2704                                 const int                       idx=int(c.m_nodes[j]-&m_nodes[0]);
2705                                 const btVector3&        x=c.m_nodes[j]->m_x;
2706                                 const btScalar          q=c.m_masses[j];
2707                                 deltas[idx]             +=      (v+btCross(w,x-c.m_com))*q;
2708                                 weights[idx]    +=      q;
2709                         }
2710                 }
2711         }
2712         for(i=0;i<deltas.size();++i)
2713         {
2714                 if(weights[i]>0)
2715                 {
2716                         m_nodes[i].m_x+=deltas[i]/weights[i];
2717                 }
2718         }
2719 }
2720
2721 //
2722 void                                    btSoftBody::dampClusters()
2723 {
2724         int i;
2725
2726         for(i=0;i<m_clusters.size();++i)
2727         {
2728                 Cluster&        c=*m_clusters[i];
2729                 if(c.m_ndamping>0)
2730                 {
2731                         for(int j=0;j<c.m_nodes.size();++j)
2732                         {
2733                                 Node&                   n=*c.m_nodes[j];
2734                                 if(n.m_im>0)
2735                                 {
2736                                         const btVector3 vx=c.m_lv+btCross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
2737                                         if(vx.length2()<=n.m_v.length2())
2738                                                 {
2739                                                 n.m_v   +=      c.m_ndamping*(vx-n.m_v);
2740                                                 }
2741                                 }
2742                         }
2743                 }
2744         }
2745 }
2746
2747 //
2748 void                            btSoftBody::Joint::Prepare(btScalar dt,int)
2749 {
2750         m_bodies[0].activate();
2751         m_bodies[1].activate();
2752 }
2753
2754 //
2755 void                            btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
2756 {
2757         static const btScalar   maxdrift=4;
2758         Joint::Prepare(dt,iterations);
2759         m_rpos[0]               =       m_bodies[0].xform()*m_refs[0];
2760         m_rpos[1]               =       m_bodies[1].xform()*m_refs[1];
2761         m_drift                 =       Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
2762         m_rpos[0]               -=      m_bodies[0].xform().getOrigin();
2763         m_rpos[1]               -=      m_bodies[1].xform().getOrigin();
2764         m_massmatrix    =       ImpulseMatrix(  m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
2765                 m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
2766         if(m_split>0)
2767         {
2768                 m_sdrift        =       m_massmatrix*(m_drift*m_split);
2769                 m_drift         *=      1-m_split;
2770         }
2771         m_drift /=(btScalar)iterations;
2772 }
2773
2774 //
2775 void                            btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
2776 {
2777         const btVector3         va=m_bodies[0].velocity(m_rpos[0]);
2778         const btVector3         vb=m_bodies[1].velocity(m_rpos[1]);
2779         const btVector3         vr=va-vb;
2780         btSoftBody::Impulse     impulse;
2781         impulse.m_asVelocity    =       1;
2782         impulse.m_velocity              =       m_massmatrix*(m_drift+vr*m_cfm)*sor;
2783         m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
2784         m_bodies[1].applyImpulse( impulse,m_rpos[1]);
2785 }
2786
2787 //
2788 void                            btSoftBody::LJoint::Terminate(btScalar dt)
2789 {
2790         if(m_split>0)
2791         {
2792                 m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
2793                 m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
2794         }
2795 }
2796
2797 //
2798 void                            btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
2799 {
2800         static const btScalar   maxdrift=SIMD_PI/16;
2801         m_icontrol->Prepare(this);
2802         Joint::Prepare(dt,iterations);
2803         m_axis[0]       =       m_bodies[0].xform().getBasis()*m_refs[0];
2804         m_axis[1]       =       m_bodies[1].xform().getBasis()*m_refs[1];
2805         m_drift         =       NormalizeAny(btCross(m_axis[1],m_axis[0]));
2806         m_drift         *=      btMin(maxdrift,btAcos(Clamp<btScalar>(btDot(m_axis[0],m_axis[1]),-1,+1)));
2807         m_drift         *=      m_erp/dt;
2808         m_massmatrix=   AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
2809         if(m_split>0)
2810         {
2811                 m_sdrift        =       m_massmatrix*(m_drift*m_split);
2812                 m_drift         *=      1-m_split;
2813         }
2814         m_drift /=(btScalar)iterations;
2815 }
2816
2817 //
2818 void                            btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
2819 {
2820         const btVector3         va=m_bodies[0].angularVelocity();
2821         const btVector3         vb=m_bodies[1].angularVelocity();
2822         const btVector3         vr=va-vb;
2823         const btScalar          sp=btDot(vr,m_axis[0]);
2824         const btVector3         vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
2825         btSoftBody::Impulse     impulse;
2826         impulse.m_asVelocity    =       1;
2827         impulse.m_velocity              =       m_massmatrix*(m_drift+vc*m_cfm)*sor;
2828         m_bodies[0].applyAImpulse(-impulse);
2829         m_bodies[1].applyAImpulse( impulse);
2830 }
2831
2832 //
2833 void                            btSoftBody::AJoint::Terminate(btScalar dt)
2834 {
2835         if(m_split>0)
2836         {
2837                 m_bodies[0].applyDAImpulse(-m_sdrift);
2838                 m_bodies[1].applyDAImpulse( m_sdrift);
2839         }
2840 }
2841
2842 //
2843 void                            btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
2844 {
2845         Joint::Prepare(dt,iterations);
2846         const bool      dodrift=(m_life==0);
2847         m_delete=(++m_life)>m_maxlife;
2848         if(dodrift)
2849         {
2850                 m_drift=m_drift*m_erp/dt;
2851                 if(m_split>0)
2852                 {
2853                         m_sdrift        =       m_massmatrix*(m_drift*m_split);
2854                         m_drift         *=      1-m_split;
2855                 }
2856                 m_drift/=(btScalar)iterations;
2857         }
2858         else
2859         {
2860                 m_drift=m_sdrift=btVector3(0,0,0);
2861         }
2862 }
2863
2864 //
2865 void                            btSoftBody::CJoint::Solve(btScalar dt,btScalar sor)
2866 {
2867         const btVector3         va=m_bodies[0].velocity(m_rpos[0]);
2868         const btVector3         vb=m_bodies[1].velocity(m_rpos[1]);
2869         const btVector3         vrel=va-vb;
2870         const btScalar          rvac=btDot(vrel,m_normal);
2871         btSoftBody::Impulse     impulse;
2872         impulse.m_asVelocity    =       1;
2873         impulse.m_velocity              =       m_drift;
2874         if(rvac<0)
2875         {
2876                 const btVector3 iv=m_normal*rvac;
2877                 const btVector3 fv=vrel-iv;
2878                 impulse.m_velocity      +=      iv+fv*m_friction;
2879         }
2880         impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;
2881
2882         if (m_bodies[0].m_soft==m_bodies[1].m_soft)
2883         {
2884                 if ((impulse.m_velocity.getX() ==impulse.m_velocity.getX())&&(impulse.m_velocity.getY() ==impulse.m_velocity.getY())&&
2885                         (impulse.m_velocity.getZ() ==impulse.m_velocity.getZ()))
2886                 {
2887                         if (impulse.m_asVelocity)
2888                         {
2889                                 if (impulse.m_velocity.length() <m_bodies[0].m_soft->m_maxSelfCollisionImpulse)
2890                                 {
2891
2892                                 } else
2893                                 {
2894                                         m_bodies[0].applyImpulse(-impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[0]);
2895                                         m_bodies[1].applyImpulse( impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[1]);
2896                                 }
2897                         }
2898                 }
2899         } else
2900         {
2901                 m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
2902                 m_bodies[1].applyImpulse( impulse,m_rpos[1]);
2903         }
2904 }
2905
2906 //
2907 void                            btSoftBody::CJoint::Terminate(btScalar dt)
2908 {
2909         if(m_split>0)
2910         {
2911                 m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
2912                 m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
2913         }
2914 }
2915
2916 //
2917 void                            btSoftBody::applyForces()
2918 {
2919
2920         BT_PROFILE("SoftBody applyForces");
2921 //      const btScalar                                  dt =                    m_sst.sdt;
2922         const btScalar                                  kLF =                   m_cfg.kLF;
2923         const btScalar                                  kDG =                   m_cfg.kDG;
2924         const btScalar                                  kPR =                   m_cfg.kPR;
2925         const btScalar                                  kVC =                   m_cfg.kVC;
2926         const bool                                              as_lift =               kLF>0;
2927         const bool                                              as_drag =               kDG>0;
2928         const bool                                              as_pressure =   kPR!=0;
2929         const bool                                              as_volume =             kVC>0;
2930         const bool                                              as_aero =               as_lift ||
2931                                                                                                         as_drag         ;
2932         //const bool                                            as_vaero =              as_aero &&
2933         //                                                                                              (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
2934         //const bool                                            as_faero =              as_aero &&
2935         //                                                                                              (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
2936         const bool                                              use_medium =    as_aero;
2937         const bool                                              use_volume =    as_pressure     ||
2938                 as_volume       ;
2939         btScalar                                                volume =                0;
2940         btScalar                                                ivolumetp =             0;
2941         btScalar                                                dvolumetv =             0;
2942         btSoftBody::sMedium     medium;
2943         if(use_volume)
2944         {
2945                 volume          =       getVolume();
2946                 ivolumetp       =       1/btFabs(volume)*kPR;
2947                 dvolumetv       =       (m_pose.m_volume-volume)*kVC;
2948         }
2949         /* Per vertex forces                    */
2950         int i,ni;
2951
2952         for(i=0,ni=m_nodes.size();i<ni;++i)
2953         {
2954                 btSoftBody::Node&       n=m_nodes[i];
2955                 if(n.m_im>0)
2956                 {
2957                         if(use_medium)
2958                         {
2959                                 /* Aerodynamics                 */
2960                                 addAeroForceToNode(m_windVelocity, i);
2961                         }
2962                         /* Pressure                             */
2963                         if(as_pressure)
2964                         {
2965                                 n.m_f   +=      n.m_n*(n.m_area*ivolumetp);
2966                         }
2967                         /* Volume                               */
2968                         if(as_volume)
2969                         {
2970                                 n.m_f   +=      n.m_n*(n.m_area*dvolumetv);
2971                         }
2972                 }
2973         }
2974
2975         /* Per face forces                              */
2976         for(i=0,ni=m_faces.size();i<ni;++i)
2977         {
2978         //      btSoftBody::Face&       f=m_faces[i];
2979
2980                 /* Aerodynamics                 */
2981                 addAeroForceToFace(m_windVelocity, i);
2982         }
2983 }
2984
2985 //
2986 void                            btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti)
2987 {
2988         const btScalar  kAHR=psb->m_cfg.kAHR*kst;
2989         const btScalar  dt=psb->m_sst.sdt;
2990         for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
2991         {
2992                 const Anchor&           a=psb->m_anchors[i];
2993                 const btTransform&      t=a.m_body->getWorldTransform();
2994                 Node&                           n=*a.m_node;
2995                 const btVector3         wa=t*a.m_local;
2996                 const btVector3         va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt;
2997                 const btVector3         vb=n.m_x-n.m_q;
2998                 const btVector3         vr=(va-vb)+(wa-n.m_x)*kAHR;
2999                 const btVector3         impulse=a.m_c0*vr*a.m_influence;
3000                 n.m_x+=impulse*a.m_c2;
3001                 a.m_body->applyImpulse(-impulse,a.m_c1);
3002         }
3003 }
3004
3005 //
3006 void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti)
3007 {
3008         const btScalar  dt = psb->m_sst.sdt;
3009         const btScalar  mrg = psb->getCollisionShape()->getMargin();
3010         for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
3011         {
3012                 const RContact&         c = psb->m_rcontacts[i];
3013                 const sCti&                     cti = c.m_cti;
3014                 btRigidBody* tmpRigid = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
3015
3016                 const btVector3         va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0);
3017                 const btVector3         vb = c.m_node->m_x-c.m_node->m_q;
3018                 const btVector3         vr = vb-va;
3019                 const btScalar          dn = btDot(vr, cti.m_normal);
3020                 if(dn<=SIMD_EPSILON)
3021                 {
3022                         const btScalar          dp = btMin( (btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg );
3023                         const btVector3         fv = vr - (cti.m_normal * dn);
3024                         // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
3025                         const btVector3         impulse = c.m_c0 * ( (vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst );
3026                         c.m_node->m_x -= impulse * c.m_c2;
3027                         if (tmpRigid)
3028                                 tmpRigid->applyImpulse(impulse,c.m_c1);
3029                 }
3030         }
3031 }
3032
3033 //
3034 void                            btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti)
3035 {
3036         for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
3037         {
3038                 const SContact&         c=psb->m_scontacts[i];
3039                 const btVector3&        nr=c.m_normal;
3040                 Node&                           n=*c.m_node;
3041                 Face&                           f=*c.m_face;
3042                 const btVector3         p=BaryEval(     f.m_n[0]->m_x,
3043                         f.m_n[1]->m_x,
3044                         f.m_n[2]->m_x,
3045                         c.m_weights);
3046                 const btVector3         q=BaryEval(     f.m_n[0]->m_q,
3047                         f.m_n[1]->m_q,
3048                         f.m_n[2]->m_q,
3049                         c.m_weights);
3050                 const btVector3         vr=(n.m_x-n.m_q)-(p-q);
3051                 btVector3                       corr(0,0,0);
3052                 btScalar dot = btDot(vr,nr);
3053                 if(dot<0)
3054                 {
3055                         const btScalar  j=c.m_margin-(btDot(nr,n.m_x)-btDot(nr,p));
3056                         corr+=c.m_normal*j;
3057                 }
3058                 corr                    -=      ProjectOnPlane(vr,nr)*c.m_friction;
3059                 n.m_x                   +=      corr*c.m_cfm[0];
3060                 f.m_n[0]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.x());
3061                 f.m_n[1]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.y());
3062                 f.m_n[2]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.z());
3063         }
3064 }
3065
3066 //
3067 void                            btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti)
3068 {
3069         for(int i=0,ni=psb->m_links.size();i<ni;++i)
3070         {
3071                 Link&   l=psb->m_links[i];
3072                 if(l.m_c0>0)
3073                 {
3074                         Node&                   a=*l.m_n[0];
3075                         Node&                   b=*l.m_n[1];
3076                         const btVector3 del=b.m_x-a.m_x;
3077                         const btScalar  len=del.length2();
3078                         if (l.m_c1+len > SIMD_EPSILON)
3079                         {
3080                                 const btScalar  k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst;
3081                                 a.m_x-=del*(k*a.m_im);
3082                                 b.m_x+=del*(k*b.m_im);
3083                         }
3084                 }
3085         }
3086 }
3087
3088 //
3089 void                            btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst)
3090 {
3091         for(int i=0,ni=psb->m_links.size();i<ni;++i)
3092         {
3093                 Link&                   l=psb->m_links[i];
3094                 Node**                  n=l.m_n;
3095                 const btScalar  j=-btDot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
3096                 n[0]->m_v+=     l.m_c3*(j*n[0]->m_im);
3097                 n[1]->m_v-=     l.m_c3*(j*n[1]->m_im);
3098         }
3099 }
3100
3101 //
3102 btSoftBody::psolver_t   btSoftBody::getSolver(ePSolver::_ solver)
3103 {
3104         switch(solver)
3105         {
3106         case    ePSolver::Anchors:
3107                 return(&btSoftBody::PSolve_Anchors);
3108         case    ePSolver::Linear:
3109                 return(&btSoftBody::PSolve_Links);
3110         case    ePSolver::RContacts:
3111                 return(&btSoftBody::PSolve_RContacts);
3112         case    ePSolver::SContacts:
3113                 return(&btSoftBody::PSolve_SContacts);
3114                 default:
3115                 {
3116                 }
3117         }
3118         return(0);
3119 }
3120
3121 //
3122 btSoftBody::vsolver_t   btSoftBody::getSolver(eVSolver::_ solver)
3123 {
3124         switch(solver)
3125         {
3126         case    eVSolver::Linear:               return(&btSoftBody::VSolve_Links);
3127                 default:
3128                 {
3129                 }
3130         }
3131         return(0);
3132 }
3133
3134 //
3135 void                    btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap)
3136 {
3137
3138         switch(m_cfg.collisions&fCollision::RVSmask)
3139         {
3140         case    fCollision::SDF_RS:
3141                 {
3142                         btSoftColliders::CollideSDF_RS  docollide;
3143                         btRigidBody*            prb1=(btRigidBody*) btRigidBody::upcast(pcoWrap->getCollisionObject());
3144                         btTransform     wtr=pcoWrap->getWorldTransform();
3145
3146                         const btTransform       ctr=pcoWrap->getWorldTransform();
3147                         const btScalar          timemargin=(wtr.getOrigin()-ctr.getOrigin()).length();
3148                         const btScalar          basemargin=getCollisionShape()->getMargin();
3149                         btVector3                       mins;
3150                         btVector3                       maxs;
3151                         ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume;
3152                         pcoWrap->getCollisionShape()->getAabb(  pcoWrap->getWorldTransform(),
3153                                 mins,
3154                                 maxs);
3155                         volume=btDbvtVolume::FromMM(mins,maxs);
3156                         volume.Expand(btVector3(basemargin,basemargin,basemargin));
3157                         docollide.psb           =       this;
3158                         docollide.m_colObj1Wrap = pcoWrap;
3159                         docollide.m_rigidBody = prb1;
3160
3161                         docollide.dynmargin     =       basemargin+timemargin;
3162                         docollide.stamargin     =       basemargin;
3163                         m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide);
3164                 }
3165                 break;
3166         case    fCollision::CL_RS:
3167                 {
3168                         btSoftColliders::CollideCL_RS   collider;
3169                         collider.ProcessColObj(this,pcoWrap);
3170                 }
3171                 break;
3172         }
3173 }
3174
3175 //
3176 void                    btSoftBody::defaultCollisionHandler(btSoftBody* psb)
3177 {
3178         const int cf=m_cfg.collisions&psb->m_cfg.collisions;
3179         switch(cf&fCollision::SVSmask)
3180         {
3181         case    fCollision::CL_SS:
3182                 {
3183
3184                         //support self-collision if CL_SELF flag set
3185                         if (this!=psb || psb->m_cfg.collisions&fCollision::CL_SELF)
3186                         {
3187                                 btSoftColliders::CollideCL_SS   docollide;
3188                                 docollide.ProcessSoftSoft(this,psb);
3189                         }
3190
3191                 }
3192                 break;
3193         case    fCollision::VF_SS:
3194                 {
3195                         //only self-collision for Cluster, not Vertex-Face yet
3196                         if (this!=psb)
3197                         {
3198                                 btSoftColliders::CollideVF_SS   docollide;
3199                                 /* common                                       */
3200                                 docollide.mrg=  getCollisionShape()->getMargin()+
3201                                         psb->getCollisionShape()->getMargin();
3202                                 /* psb0 nodes vs psb1 faces     */
3203                                 docollide.psb[0]=this;
3204                                 docollide.psb[1]=psb;
3205                                 docollide.psb[0]->m_ndbvt.collideTT(    docollide.psb[0]->m_ndbvt.m_root,
3206                                         docollide.psb[1]->m_fdbvt.m_root,
3207                                         docollide);
3208                                 /* psb1 nodes vs psb0 faces     */
3209                                 docollide.psb[0]=psb;
3210                                 docollide.psb[1]=this;
3211                                 docollide.psb[0]->m_ndbvt.collideTT(    docollide.psb[0]->m_ndbvt.m_root,
3212                                         docollide.psb[1]->m_fdbvt.m_root,
3213                                         docollide);
3214                         }
3215                 }
3216                 break;
3217         default:
3218                 {
3219
3220                 }
3221         }
3222 }
3223
3224
3225
3226 void btSoftBody::setWindVelocity( const btVector3 &velocity )
3227 {
3228         m_windVelocity = velocity;
3229 }
3230
3231
3232 const btVector3& btSoftBody::getWindVelocity()
3233 {
3234         return m_windVelocity;
3235 }
3236
3237
3238
3239 int     btSoftBody::calculateSerializeBufferSize()      const
3240 {
3241         int sz = sizeof(btSoftBodyData);
3242         return sz;
3243 }
3244
3245         ///fills the dataBuffer and returns the struct name (and 0 on failure)
3246 const char*     btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const
3247 {
3248         btSoftBodyData* sbd = (btSoftBodyData*) dataBuffer;
3249
3250         btCollisionObject::serialize(&sbd->m_collisionObjectData, serializer);
3251
3252         btHashMap<btHashPtr,int>        m_nodeIndexMap;
3253
3254         sbd->m_numMaterials = m_materials.size();
3255         sbd->m_materials = sbd->m_numMaterials? (SoftBodyMaterialData**) serializer->getUniquePointer((void*)&m_materials): 0;
3256
3257         if (sbd->m_materials)
3258         {
3259                 int sz = sizeof(SoftBodyMaterialData*);
3260                 int numElem = sbd->m_numMaterials;
3261                 btChunk* chunk = serializer->allocate(sz,numElem);
3262                 //SoftBodyMaterialData** memPtr = chunk->m_oldPtr;
3263                 SoftBodyMaterialData** memPtr = (SoftBodyMaterialData**)chunk->m_oldPtr;
3264                 for (int i=0;i<numElem;i++,memPtr++)
3265                 {
3266                         btSoftBody::Material* mat = m_materials[i];
3267                         *memPtr = mat ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)mat) : 0;
3268                         if (!serializer->findPointer(mat))
3269                         {
3270                                 //serialize it here
3271                                 btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData),1);
3272                                 SoftBodyMaterialData* memPtr = (SoftBodyMaterialData*)chunk->m_oldPtr;
3273                                 memPtr->m_flags = mat->m_flags;
3274                                 memPtr->m_angularStiffness = mat->m_kAST;
3275                                 memPtr->m_linearStiffness = mat->m_kLST;
3276                                 memPtr->m_volumeStiffness = mat->m_kVST;
3277                                 serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_SBMATERIAL_CODE,mat);
3278                         }
3279                 }
3280                 serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_ARRAY_CODE,(void*) &m_materials);
3281         }
3282
3283
3284
3285
3286         sbd->m_numNodes = m_nodes.size();
3287         sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes): 0;
3288         if (sbd->m_nodes)
3289         {
3290                 int sz = sizeof(SoftBodyNodeData);
3291                 int numElem = sbd->m_numNodes;
3292                 btChunk* chunk = serializer->allocate(sz,numElem);
3293                 SoftBodyNodeData* memPtr = (SoftBodyNodeData*)chunk->m_oldPtr;
3294                 for (int i=0;i<numElem;i++,memPtr++)
3295                 {
3296                         m_nodes[i].m_f.serializeFloat( memPtr->m_accumulatedForce);
3297                         memPtr->m_area = m_nodes[i].m_area;
3298                         memPtr->m_attach = m_nodes[i].m_battach;
3299                         memPtr->m_inverseMass = m_nodes[i].m_im;
3300                         memPtr->m_material = m_nodes[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_nodes[i].m_material):0;
3301                         m_nodes[i].m_n.serializeFloat(memPtr->m_normal);
3302                         m_nodes[i].m_x.serializeFloat(memPtr->m_position);
3303                         m_nodes[i].m_q.serializeFloat(memPtr->m_previousPosition);
3304                         m_nodes[i].m_v.serializeFloat(memPtr->m_velocity);
3305                         m_nodeIndexMap.insert(&m_nodes[i],i);
3306                 }
3307                 serializer->finalizeChunk(chunk,"SoftBodyNodeData",BT_SBNODE_CODE,(void*) &m_nodes);
3308         }
3309
3310         sbd->m_numLinks = m_links.size();
3311         sbd->m_links = sbd->m_numLinks? (SoftBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0;
3312         if (sbd->m_links)
3313         {
3314                 int sz = sizeof(SoftBodyLinkData);
3315                 int numElem = sbd->m_numLinks;
3316                 btChunk* chunk = serializer->allocate(sz,numElem);
3317                 SoftBodyLinkData* memPtr = (SoftBodyLinkData*)chunk->m_oldPtr;
3318                 for (int i=0;i<numElem;i++,memPtr++)
3319                 {
3320                         memPtr->m_bbending = m_links[i].m_bbending;
3321                         memPtr->m_material = m_links[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_links[i].m_material):0;
3322                         memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0]: -1;
3323                         memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0]: -1;
3324                         btAssert(memPtr->m_nodeIndices[0]<m_nodes.size());
3325                         btAssert(memPtr->m_nodeIndices[1]<m_nodes.size());
3326                         memPtr->m_restLength = m_links[i].m_rl;
3327                 }
3328                 serializer->finalizeChunk(chunk,"SoftBodyLinkData",BT_ARRAY_CODE,(void*) &m_links[0]);
3329
3330         }
3331
3332
3333         sbd->m_numFaces = m_faces.size();
3334         sbd->m_faces = sbd->m_numFaces? (SoftBodyFaceData*) serializer->getUniquePointer((void*)&m_faces[0]):0;
3335         if (sbd->m_faces)
3336         {
3337                 int sz = sizeof(SoftBodyFaceData);
3338                 int numElem = sbd->m_numFaces;
3339                 btChunk* chunk = serializer->allocate(sz,numElem);
3340                 SoftBodyFaceData* memPtr = (SoftBodyFaceData*)chunk->m_oldPtr;
3341                 for (int i=0;i<numElem;i++,memPtr++)
3342                 {
3343                         memPtr->m_material = m_faces[i].m_material ?  (SoftBodyMaterialData*) serializer->getUniquePointer((void*)m_faces[i].m_material): 0;
3344                         m_faces[i].m_normal.serializeFloat(     memPtr->m_normal);
3345                         for (int j=0;j<3;j++)
3346                         {
3347                                 memPtr->m_nodeIndices[j] = m_faces[i].m_n[j]? m_faces[i].m_n[j] - &m_nodes[0]: -1;
3348                         }
3349                         memPtr->m_restArea = m_faces[i].m_ra;
3350                 }
3351                 serializer->finalizeChunk(chunk,"SoftBodyFaceData",BT_ARRAY_CODE,(void*) &m_faces[0]);
3352         }
3353
3354
3355         sbd->m_numTetrahedra = m_tetras.size();
3356         sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*) serializer->getUniquePointer((void*)&m_tetras[0]):0;
3357         if (sbd->m_tetrahedra)
3358         {
3359                 int sz = sizeof(SoftBodyTetraData);
3360                 int numElem = sbd->m_numTetrahedra;
3361                 btChunk* chunk = serializer->allocate(sz,numElem);
3362                 SoftBodyTetraData* memPtr = (SoftBodyTetraData*)chunk->m_oldPtr;
3363                 for (int i=0;i<numElem;i++,memPtr++)
3364                 {
3365                         for (int j=0;j<4;j++)
3366                         {
3367                                 m_tetras[i].m_c0[j].serializeFloat(     memPtr->m_c0[j] );
3368                                 memPtr->m_nodeIndices[j] = m_tetras[j].m_n[j]? m_tetras[j].m_n[j]-&m_nodes[0] : -1;
3369                         }
3370                         memPtr->m_c1 = m_tetras[i].m_c1;
3371                         memPtr->m_c2 = m_tetras[i].m_c2;
3372                         memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_tetras[i].m_material): 0;
3373                         memPtr->m_restVolume = m_tetras[i].m_rv;
3374                 }
3375                 serializer->finalizeChunk(chunk,"SoftBodyTetraData",BT_ARRAY_CODE,(void*) &m_tetras[0]);
3376         }
3377
3378         sbd->m_numAnchors = m_anchors.size();
3379         sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*) serializer->getUniquePointer((void*)&m_anchors[0]):0;
3380         if (sbd->m_anchors)
3381         {
3382                 int sz = sizeof(SoftRigidAnchorData);
3383                 int numElem = sbd->m_numAnchors;
3384                 btChunk* chunk = serializer->allocate(sz,numElem);
3385                 SoftRigidAnchorData* memPtr = (SoftRigidAnchorData*)chunk->m_oldPtr;
3386                 for (int i=0;i<numElem;i++,memPtr++)
3387                 {
3388                         m_anchors[i].m_c0.serializeFloat(memPtr->m_c0);
3389                         m_anchors[i].m_c1.serializeFloat(memPtr->m_c1);
3390                         memPtr->m_c2 = m_anchors[i].m_c2;
3391                         m_anchors[i].m_local.serializeFloat(memPtr->m_localFrame);
3392                         memPtr->m_nodeIndex = m_anchors[i].m_node? m_anchors[i].m_node-&m_nodes[0]: -1;
3393
3394                         memPtr->m_rigidBody = m_anchors[i].m_body? (btRigidBodyData*)  serializer->getUniquePointer((void*)m_anchors[i].m_body): 0;
3395                         btAssert(memPtr->m_nodeIndex < m_nodes.size());
3396                 }
3397                 serializer->finalizeChunk(chunk,"SoftRigidAnchorData",BT_ARRAY_CODE,(void*) &m_anchors[0]);
3398         }
3399
3400
3401         sbd->m_config.m_dynamicFriction = m_cfg.kDF;
3402         sbd->m_config.m_baumgarte = m_cfg.kVCF;
3403         sbd->m_config.m_pressure = m_cfg.kPR;
3404         sbd->m_config.m_aeroModel = this->m_cfg.aeromodel;
3405         sbd->m_config.m_lift = m_cfg.kLF;
3406         sbd->m_config.m_drag = m_cfg.kDG;
3407         sbd->m_config.m_positionIterations = m_cfg.piterations;
3408         sbd->m_config.m_driftIterations = m_cfg.diterations;
3409         sbd->m_config.m_clusterIterations = m_cfg.citerations;
3410         sbd->m_config.m_velocityIterations = m_cfg.viterations;
3411         sbd->m_config.m_maxVolume = m_cfg.maxvolume;
3412         sbd->m_config.m_damping = m_cfg.kDP;
3413         sbd->m_config.m_poseMatch = m_cfg.kMT;
3414         sbd->m_config.m_collisionFlags = m_cfg.collisions;
3415         sbd->m_config.m_volume = m_cfg.kVC;
3416         sbd->m_config.m_rigidContactHardness = m_cfg.kCHR;
3417         sbd->m_config.m_kineticContactHardness = m_cfg.kKHR;
3418         sbd->m_config.m_softContactHardness = m_cfg.kSHR;
3419         sbd->m_config.m_anchorHardness = m_cfg.kAHR;
3420         sbd->m_config.m_timeScale = m_cfg.timescale;
3421         sbd->m_config.m_maxVolume = m_cfg.maxvolume;
3422         sbd->m_config.m_softRigidClusterHardness = m_cfg.kSRHR_CL;
3423         sbd->m_config.m_softKineticClusterHardness = m_cfg.kSKHR_CL;
3424         sbd->m_config.m_softSoftClusterHardness = m_cfg.kSSHR_CL;
3425         sbd->m_config.m_softRigidClusterImpulseSplit = m_cfg.kSR_SPLT_CL;
3426         sbd->m_config.m_softKineticClusterImpulseSplit = m_cfg.kSK_SPLT_CL;
3427         sbd->m_config.m_softSoftClusterImpulseSplit = m_cfg.kSS_SPLT_CL;
3428
3429         //pose for shape matching
3430         {
3431                 sbd->m_pose = (SoftBodyPoseData*)serializer->getUniquePointer((void*)&m_pose);
3432
3433                 int sz = sizeof(SoftBodyPoseData);
3434                 btChunk* chunk = serializer->allocate(sz,1);
3435                 SoftBodyPoseData* memPtr = (SoftBodyPoseData*)chunk->m_oldPtr;
3436
3437                 m_pose.m_aqq.serializeFloat(memPtr->m_aqq);
3438                 memPtr->m_bframe = m_pose.m_bframe;
3439                 memPtr->m_bvolume = m_pose.m_bvolume;
3440                 m_pose.m_com.serializeFloat(memPtr->m_com);
3441
3442                 memPtr->m_numPositions = m_pose.m_pos.size();
3443                 memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]): 0;
3444                 if (memPtr->m_numPositions)
3445                 {
3446                         int numElem = memPtr->m_numPositions;
3447                         int sz = sizeof(btVector3Data);
3448                         btChunk* chunk = serializer->allocate(sz,numElem);
3449                         btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr;
3450                         for (int i=0;i<numElem;i++,memPtr++)
3451                         {
3452                                 m_pose.m_pos[i].serializeFloat(*memPtr);
3453                         }
3454                         serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_pose.m_pos[0]);
3455                 }
3456                 memPtr->m_restVolume = m_pose.m_volume;
3457                 m_pose.m_rot.serializeFloat(memPtr->m_rot);
3458                 m_pose.m_scl.serializeFloat(memPtr->m_scale);
3459
3460                 memPtr->m_numWeigts = m_pose.m_wgh.size();
3461                 memPtr->m_weights = memPtr->m_numWeigts? (float*) serializer->getUniquePointer((void*) &m_pose.m_wgh[0]) : 0;
3462                 if (memPtr->m_numWeigts)
3463                 {
3464
3465                         int numElem = memPtr->m_numWeigts;
3466                         int sz = sizeof(float);
3467                         btChunk* chunk = serializer->allocate(sz,numElem);
3468                         float* memPtr = (float*) chunk->m_oldPtr;
3469                         for (int i=0;i<numElem;i++,memPtr++)
3470                         {
3471                                 *memPtr = m_pose.m_wgh[i];
3472                         }
3473                         serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_pose.m_wgh[0]);
3474                 }
3475
3476                 serializer->finalizeChunk(chunk,"SoftBodyPoseData",BT_ARRAY_CODE,(void*)&m_pose);
3477         }
3478
3479         //clusters for convex-cluster collision detection
3480
3481         sbd->m_numClusters = m_clusters.size();
3482         sbd->m_clusters = sbd->m_numClusters? (SoftBodyClusterData*) serializer->getUniquePointer((void*)m_clusters[0]) : 0;
3483         if (sbd->m_numClusters)
3484         {
3485                 int numElem = sbd->m_numClusters;
3486                 int sz = sizeof(SoftBodyClusterData);
3487                 btChunk* chunk = serializer->allocate(sz,numElem);
3488                 SoftBodyClusterData* memPtr = (SoftBodyClusterData*) chunk->m_oldPtr;
3489                 for (int i=0;i<numElem;i++,memPtr++)
3490                 {
3491                         memPtr->m_adamping= m_clusters[i]->m_adamping;
3492                         m_clusters[i]->m_av.serializeFloat(memPtr->m_av);
3493                         memPtr->m_clusterIndex = m_clusters[i]->m_clusterIndex;
3494                         memPtr->m_collide = m_clusters[i]->m_collide;
3495                         m_clusters[i]->m_com.serializeFloat(memPtr->m_com);
3496                         memPtr->m_containsAnchor = m_clusters[i]->m_containsAnchor;
3497                         m_clusters[i]->m_dimpulses[0].serializeFloat(memPtr->m_dimpulses[0]);
3498                         m_clusters[i]->m_dimpulses[1].serializeFloat(memPtr->m_dimpulses[1]);
3499                         m_clusters[i]->m_framexform.serializeFloat(memPtr->m_framexform);
3500                         memPtr->m_idmass = m_clusters[i]->m_idmass;
3501                         memPtr->m_imass = m_clusters[i]->m_imass;
3502                         m_clusters[i]->m_invwi.serializeFloat(memPtr->m_invwi);
3503                         memPtr->m_ldamping = m_clusters[i]->m_ldamping;
3504                         m_clusters[i]->m_locii.serializeFloat(memPtr->m_locii);
3505                         m_clusters[i]->m_lv.serializeFloat(memPtr->m_lv);
3506                         memPtr->m_matching = m_clusters[i]->m_matching;
3507                         memPtr->m_maxSelfCollisionImpulse = m_clusters[i]->m_maxSelfCollisionImpulse;
3508                         memPtr->m_ndamping = m_clusters[i]->m_ndamping;
3509                         memPtr->m_ldamping = m_clusters[i]->m_ldamping;
3510                         memPtr->m_adamping = m_clusters[i]->m_adamping;
3511                         memPtr->m_selfCollisionImpulseFactor = m_clusters[i]->m_selfCollisionImpulseFactor;
3512
3513                         memPtr->m_numFrameRefs = m_clusters[i]->m_framerefs.size();
3514                         memPtr->m_numMasses = m_clusters[i]->m_masses.size();
3515                         memPtr->m_numNodes = m_clusters[i]->m_nodes.size();
3516
3517                         memPtr->m_nvimpulses = m_clusters[i]->m_nvimpulses;
3518                         m_clusters[i]->m_vimpulses[0].serializeFloat(memPtr->m_vimpulses[0]);
3519                         m_clusters[i]->m_vimpulses[1].serializeFloat(memPtr->m_vimpulses[1]);
3520                         memPtr->m_ndimpulses = m_clusters[i]->m_ndimpulses;
3521
3522
3523
3524                         memPtr->m_framerefs = memPtr->m_numFrameRefs? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0;
3525                         if (memPtr->m_framerefs)
3526                         {
3527                                 int numElem = memPtr->m_numFrameRefs;
3528                                 int sz = sizeof(btVector3FloatData);
3529                                 btChunk* chunk = serializer->allocate(sz,numElem);
3530                                 btVector3FloatData* memPtr = (btVector3FloatData*) chunk->m_oldPtr;
3531                                 for (int j=0;j<numElem;j++,memPtr++)
3532                                 {
3533                                         m_clusters[i]->m_framerefs[j].serializeFloat(*memPtr);
3534                                 }
3535                                 serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_framerefs[0]);
3536                         }
3537
3538                         memPtr->m_masses = memPtr->m_numMasses ? (float*) serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]): 0;
3539                         if (memPtr->m_masses)
3540                         {
3541                                 int numElem = memPtr->m_numMasses;
3542                                 int sz = sizeof(float);
3543                                 btChunk* chunk = serializer->allocate(sz,numElem);
3544                                 float* memPtr = (float*) chunk->m_oldPtr;
3545                                 for (int j=0;j<numElem;j++,memPtr++)
3546                                 {
3547                                         *memPtr = m_clusters[i]->m_masses[j];
3548                                 }
3549                                 serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_masses[0]);
3550                         }
3551
3552                         memPtr->m_nodeIndices  = memPtr->m_numNodes ? (int*) serializer->getUniquePointer((void*) &m_clusters[i]->m_nodes) : 0;
3553                         if (memPtr->m_nodeIndices )
3554                         {
3555                                 int numElem = memPtr->m_numMasses;
3556                                 int sz = sizeof(int);
3557                                 btChunk* chunk = serializer->allocate(sz,numElem);
3558                                 int* memPtr = (int*) chunk->m_oldPtr;
3559                                 for (int j=0;j<numElem;j++,memPtr++)
3560                                 {
3561                                         int* indexPtr = m_nodeIndexMap.find(m_clusters[i]->m_nodes[j]);
3562                                         btAssert(indexPtr);
3563                                         *memPtr = *indexPtr;
3564                                 }
3565                                 serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_nodes);
3566                         }
3567                 }
3568                 serializer->finalizeChunk(chunk,"SoftBodyClusterData",BT_ARRAY_CODE,(void*)m_clusters[0]);
3569
3570         }
3571
3572
3573
3574         sbd->m_numJoints = m_joints.size();
3575         sbd->m_joints = m_joints.size()? (btSoftBodyJointData*) serializer->getUniquePointer((void*)&m_joints[0]) : 0;
3576
3577         if (sbd->m_joints)
3578         {
3579                 int sz = sizeof(btSoftBodyJointData);
3580                 int numElem = m_joints.size();
3581                 btChunk* chunk = serializer->allocate(sz,numElem);
3582                 btSoftBodyJointData* memPtr = (btSoftBodyJointData*)chunk->m_oldPtr;
3583
3584                 for (int i=0;i<numElem;i++,memPtr++)
3585                 {
3586                         memPtr->m_jointType = (int)m_joints[i]->Type();
3587                         m_joints[i]->m_refs[0].serializeFloat(memPtr->m_refs[0]);
3588                         m_joints[i]->m_refs[1].serializeFloat(memPtr->m_refs[1]);
3589                         memPtr->m_cfm = m_joints[i]->m_cfm;
3590                         memPtr->m_erp = m_joints[i]->m_erp;
3591                         memPtr->m_split = m_joints[i]->m_split;
3592                         memPtr->m_delete = m_joints[i]->m_delete;
3593
3594                         for (int j=0;j<4;j++)
3595                         {
3596                                 memPtr->m_relPosition[0].m_floats[j] = 0.f;
3597                                 memPtr->m_relPosition[1].m_floats[j] = 0.f;
3598                         }
3599                         memPtr->m_bodyA = 0;
3600                         memPtr->m_bodyB = 0;
3601                         if (m_joints[i]->m_bodies[0].m_soft)
3602                         {
3603                                 memPtr->m_bodyAtype = BT_JOINT_SOFT_BODY_CLUSTER;
3604                                 memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_soft);
3605                         }
3606                         if (m_joints[i]->m_bodies[0].m_collisionObject)
3607                         {
3608                                 memPtr->m_bodyAtype = BT_JOINT_COLLISION_OBJECT;
3609                                 memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_collisionObject);
3610                         }
3611                         if (m_joints[i]->m_bodies[0].m_rigid)
3612                         {
3613                                 memPtr->m_bodyAtype = BT_JOINT_RIGID_BODY;
3614                                 memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_rigid);
3615                         }
3616
3617                         if (m_joints[i]->m_bodies[1].m_soft)
3618                         {
3619                                 memPtr->m_bodyBtype = BT_JOINT_SOFT_BODY_CLUSTER;
3620                                 memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_soft);
3621                         }
3622                         if (m_joints[i]->m_bodies[1].m_collisionObject)
3623                         {
3624                                 memPtr->m_bodyBtype = BT_JOINT_COLLISION_OBJECT;
3625                                 memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_collisionObject);
3626                         }
3627                         if (m_joints[i]->m_bodies[1].m_rigid)
3628                         {
3629                                 memPtr->m_bodyBtype = BT_JOINT_RIGID_BODY;
3630                                 memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_rigid);
3631                         }
3632                 }
3633                 serializer->finalizeChunk(chunk,"btSoftBodyJointData",BT_ARRAY_CODE,(void*) &m_joints[0]);
3634         }
3635
3636
3637         return btSoftBodyDataName;
3638 }
3639