dali-physics/third-party/bullet3/src/BulletSoftBody/btSoftBody.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   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 #include "LinearMath/btImplicitQRSVD.h"
  22 #include "LinearMath/btAlignedAllocator.h"
  23 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
  24 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
  25 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
  26 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
  27 #include <iostream>
  28 //
  29 static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& leafNodes, btAlignedObjectArray<btAlignedObjectArray<int> >& adj)
  30 {
  31         int N = leafNodes.size();
  32         if (N == 0)
  33         {
  34                 return NULL;
  35         }
  36         while (N > 1)
  37         {
  38                 btAlignedObjectArray<bool> marked;
  39                 btAlignedObjectArray<btDbvtNode*> newLeafNodes;
  40                 btAlignedObjectArray<std::pair<int, int> > childIds;
  41                 btAlignedObjectArray<btAlignedObjectArray<int> > newAdj;
  42                 marked.resize(N);
  43                 for (int i = 0; i < N; ++i)
  44                         marked[i] = false;
  45
  46                 // pair adjacent nodes into new(parent) node
  47                 for (int i = 0; i < N; ++i)
  48                 {
  49                         if (marked[i])
  50                                 continue;
  51                         bool merged = false;
  52                         for (int j = 0; j < adj[i].size(); ++j)
  53                         {
  54                                 int n = adj[i][j];
  55                                 if (!marked[adj[i][j]])
  56                                 {
  57                                         btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
  58                                         node->parent = NULL;
  59                                         node->childs[0] = leafNodes[i];
  60                                         node->childs[1] = leafNodes[n];
  61                                         leafNodes[i]->parent = node;
  62                                         leafNodes[n]->parent = node;
  63                                         newLeafNodes.push_back(node);
  64                                         childIds.push_back(std::make_pair(i, n));
  65                                         merged = true;
  66                                         marked[n] = true;
  67                                         break;
  68                                 }
  69                         }
  70                         if (!merged)
  71                         {
  72                                 newLeafNodes.push_back(leafNodes[i]);
  73                                 childIds.push_back(std::make_pair(i, -1));
  74                         }
  75                         marked[i] = true;
  76                 }
  77                 // update adjacency matrix
  78                 newAdj.resize(newLeafNodes.size());
  79                 for (int i = 0; i < newLeafNodes.size(); ++i)
  80                 {
  81                         for (int j = i + 1; j < newLeafNodes.size(); ++j)
  82                         {
  83                                 bool neighbor = false;
  84                                 const btAlignedObjectArray<int>& leftChildNeighbors = adj[childIds[i].first];
  85                                 for (int k = 0; k < leftChildNeighbors.size(); ++k)
  86                                 {
  87                                         if (leftChildNeighbors[k] == childIds[j].first || leftChildNeighbors[k] == childIds[j].second)
  88                                         {
  89                                                 neighbor = true;
  90                                                 break;
  91                                         }
  92                                 }
  93                                 if (!neighbor && childIds[i].second != -1)
  94                                 {
  95                                         const btAlignedObjectArray<int>& rightChildNeighbors = adj[childIds[i].second];
  96                                         for (int k = 0; k < rightChildNeighbors.size(); ++k)
  97                                         {
  98                                                 if (rightChildNeighbors[k] == childIds[j].first || rightChildNeighbors[k] == childIds[j].second)
  99                                                 {
 100                                                         neighbor = true;
 101                                                         break;
 102                                                 }
 103                                         }
 104                                 }
 105                                 if (neighbor)
 106                                 {
 107                                         newAdj[i].push_back(j);
 108                                         newAdj[j].push_back(i);
 109                                 }
 110                         }
 111                 }
 112                 leafNodes = newLeafNodes;
 113                 //this assignment leaks memory, the assignment doesn't do a deep copy, for now a manual copy
 114                 //adj = newAdj;
 115                 adj.clear();
 116                 adj.resize(newAdj.size());
 117                 for (int i = 0; i < newAdj.size(); i++)
 118                 {
 119                         for (int j = 0; j < newAdj[i].size(); j++)
 120                         {
 121                                 adj[i].push_back(newAdj[i][j]);
 122                         }
 123                 }
 124                 N = leafNodes.size();
 125         }
 126         return leafNodes[0];
 127 }
 128
 129 //
 130 btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
 131         : m_softBodySolver(0), m_worldInfo(worldInfo)
 132 {
 133         /* Init         */
 134         initDefaults();
 135
 136         /* Default material     */
 137         Material* pm = appendMaterial();
 138         pm->m_kLST = 1;
 139         pm->m_kAST = 1;
 140         pm->m_kVST = 1;
 141         pm->m_flags = fMaterial::Default;
 142
 143         /* Nodes                        */
 144         const btScalar margin = getCollisionShape()->getMargin();
 145         m_nodes.resize(node_count);
 146         m_X.resize(node_count);
 147         for (int i = 0, ni = node_count; i < ni; ++i)
 148         {
 149                 Node& n = m_nodes[i];
 150                 ZeroInitialize(n);
 151                 n.m_x = x ? *x++ : btVector3(0, 0, 0);
 152                 n.m_q = n.m_x;
 153                 n.m_im = m ? *m++ : 1;
 154                 n.m_im = n.m_im > 0 ? 1 / n.m_im : 0;
 155                 n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n);
 156                 n.m_material = pm;
 157                 m_X[i] = n.m_x;
 158         }
 159         updateBounds();
 160         setCollisionQuadrature(3);
 161         m_fdbvnt = 0;
 162 }
 163
 164 btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo)
 165         : m_worldInfo(worldInfo)
 166 {
 167         initDefaults();
 168 }
 169
 170 void btSoftBody::initDefaults()
 171 {
 172         m_internalType = CO_SOFT_BODY;
 173         m_cfg.aeromodel = eAeroModel::V_Point;
 174         m_cfg.kVCF = 1;
 175         m_cfg.kDG = 0;
 176         m_cfg.kLF = 0;
 177         m_cfg.kDP = 0;
 178         m_cfg.kPR = 0;
 179         m_cfg.kVC = 0;
 180         m_cfg.kDF = (btScalar)0.2;
 181         m_cfg.kMT = 0;
 182         m_cfg.kCHR = (btScalar)1.0;
 183         m_cfg.kKHR = (btScalar)0.1;
 184         m_cfg.kSHR = (btScalar)1.0;
 185         m_cfg.kAHR = (btScalar)0.7;
 186         m_cfg.kSRHR_CL = (btScalar)0.1;
 187         m_cfg.kSKHR_CL = (btScalar)1;
 188         m_cfg.kSSHR_CL = (btScalar)0.5;
 189         m_cfg.kSR_SPLT_CL = (btScalar)0.5;
 190         m_cfg.kSK_SPLT_CL = (btScalar)0.5;
 191         m_cfg.kSS_SPLT_CL = (btScalar)0.5;
 192         m_cfg.maxvolume = (btScalar)1;
 193         m_cfg.timescale = 1;
 194         m_cfg.viterations = 0;
 195         m_cfg.piterations = 1;
 196         m_cfg.diterations = 0;
 197         m_cfg.citerations = 4;
 198         m_cfg.drag = 0;
 199         m_cfg.m_maxStress = 0;
 200         m_cfg.collisions = fCollision::Default;
 201         m_pose.m_bvolume = false;
 202         m_pose.m_bframe = false;
 203         m_pose.m_volume = 0;
 204         m_pose.m_com = btVector3(0, 0, 0);
 205         m_pose.m_rot.setIdentity();
 206         m_pose.m_scl.setIdentity();
 207         m_tag = 0;
 208         m_timeacc = 0;
 209         m_bUpdateRtCst = true;
 210         m_bounds[0] = btVector3(0, 0, 0);
 211         m_bounds[1] = btVector3(0, 0, 0);
 212         m_worldTransform.setIdentity();
 213         setSolver(eSolverPresets::Positions);
 214
 215         /* Collision shape      */
 216         ///for now, create a collision shape internally
 217         m_collisionShape = new btSoftBodyCollisionShape(this);
 218         m_collisionShape->setMargin(0.25f);
 219
 220         m_worldTransform.setIdentity();
 221
 222         m_windVelocity = btVector3(0, 0, 0);
 223         m_restLengthScale = btScalar(1.0);
 224         m_dampingCoefficient = 1.0;
 225         m_sleepingThreshold = .04;
 226         m_useSelfCollision = false;
 227         m_collisionFlags = 0;
 228         m_softSoftCollision = false;
 229         m_maxSpeedSquared = 0;
 230         m_repulsionStiffness = 0.5;
 231         m_gravityFactor = 1;
 232         m_cacheBarycenter = false;
 233         m_fdbvnt = 0;
 234
 235         // reduced flag
 236         m_reducedModel = false;
 237 }
 238
 239 //
 240 btSoftBody::~btSoftBody()
 241 {
 242         //for now, delete the internal shape
 243         delete m_collisionShape;
 244         int i;
 245
 246         releaseClusters();
 247         for (i = 0; i < m_materials.size(); ++i)
 248                 btAlignedFree(m_materials[i]);
 249         for (i = 0; i < m_joints.size(); ++i)
 250                 btAlignedFree(m_joints[i]);
 251         if (m_fdbvnt)
 252                 delete m_fdbvnt;
 253 }
 254
 255 //
 256 bool btSoftBody::checkLink(int node0, int node1) const
 257 {
 258         return (checkLink(&m_nodes[node0], &m_nodes[node1]));
 259 }
 260
 261 //
 262 bool btSoftBody::checkLink(const Node* node0, const Node* node1) const
 263 {
 264         const Node* n[] = {node0, node1};
 265         for (int i = 0, ni = m_links.size(); i < ni; ++i)
 266         {
 267                 const Link& l = m_links[i];
 268                 if ((l.m_n[0] == n[0] && l.m_n[1] == n[1]) ||
 269                         (l.m_n[0] == n[1] && l.m_n[1] == n[0]))
 270                 {
 271                         return (true);
 272                 }
 273         }
 274         return (false);
 275 }
 276
 277 //
 278 bool btSoftBody::checkFace(int node0, int node1, int node2) const
 279 {
 280         const Node* n[] = {&m_nodes[node0],
 281                                            &m_nodes[node1],
 282                                            &m_nodes[node2]};
 283         for (int i = 0, ni = m_faces.size(); i < ni; ++i)
 284         {
 285                 const Face& f = m_faces[i];
 286                 int c = 0;
 287                 for (int j = 0; j < 3; ++j)
 288                 {
 289                         if ((f.m_n[j] == n[0]) ||
 290                                 (f.m_n[j] == n[1]) ||
 291                                 (f.m_n[j] == n[2]))
 292                                 c |= 1 << j;
 293                         else
 294                                 break;
 295                 }
 296                 if (c == 7) return (true);
 297         }
 298         return (false);
 299 }
 300
 301 //
 302 btSoftBody::Material* btSoftBody::appendMaterial()
 303 {
 304         Material* pm = new (btAlignedAlloc(sizeof(Material), 16)) Material();
 305         if (m_materials.size() > 0)
 306                 *pm = *m_materials[0];
 307         else
 308                 ZeroInitialize(*pm);
 309         m_materials.push_back(pm);
 310         return (pm);
 311 }
 312
 313 //
 314 void btSoftBody::appendNote(const char* text,
 315                                                         const btVector3& o,
 316                                                         const btVector4& c,
 317                                                         Node* n0,
 318                                                         Node* n1,
 319                                                         Node* n2,
 320                                                         Node* n3)
 321 {
 322         Note n;
 323         ZeroInitialize(n);
 324         n.m_rank = 0;
 325         n.m_text = text;
 326         n.m_offset = o;
 327         n.m_coords[0] = c.x();
 328         n.m_coords[1] = c.y();
 329         n.m_coords[2] = c.z();
 330         n.m_coords[3] = c.w();
 331         n.m_nodes[0] = n0;
 332         n.m_rank += n0 ? 1 : 0;
 333         n.m_nodes[1] = n1;
 334         n.m_rank += n1 ? 1 : 0;
 335         n.m_nodes[2] = n2;
 336         n.m_rank += n2 ? 1 : 0;
 337         n.m_nodes[3] = n3;
 338         n.m_rank += n3 ? 1 : 0;
 339         m_notes.push_back(n);
 340 }
 341
 342 //
 343 void btSoftBody::appendNote(const char* text,
 344                                                         const btVector3& o,
 345                                                         Node* feature)
 346 {
 347         appendNote(text, o, btVector4(1, 0, 0, 0), feature);
 348 }
 349
 350 //
 351 void btSoftBody::appendNote(const char* text,
 352                                                         const btVector3& o,
 353                                                         Link* feature)
 354 {
 355         static const btScalar w = 1 / (btScalar)2;
 356         appendNote(text, o, btVector4(w, w, 0, 0), feature->m_n[0],
 357                            feature->m_n[1]);
 358 }
 359
 360 //
 361 void btSoftBody::appendNote(const char* text,
 362                                                         const btVector3& o,
 363                                                         Face* feature)
 364 {
 365         static const btScalar w = 1 / (btScalar)3;
 366         appendNote(text, o, btVector4(w, w, w, 0), feature->m_n[0],
 367                            feature->m_n[1],
 368                            feature->m_n[2]);
 369 }
 370
 371 //
 372 void btSoftBody::appendNode(const btVector3& x, btScalar m)
 373 {
 374         if (m_nodes.capacity() == m_nodes.size())
 375         {
 376                 pointersToIndices();
 377                 m_nodes.reserve(m_nodes.size() * 2 + 1);
 378                 indicesToPointers();
 379         }
 380         const btScalar margin = getCollisionShape()->getMargin();
 381         m_nodes.push_back(Node());
 382         Node& n = m_nodes[m_nodes.size() - 1];
 383         ZeroInitialize(n);
 384         n.m_x = x;
 385         n.m_q = n.m_x;
 386         n.m_im = m > 0 ? 1 / m : 0;
 387         n.m_material = m_materials[0];
 388         n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n);
 389 }
 390
 391 //
 392 void btSoftBody::appendLink(int model, Material* mat)
 393 {
 394         Link l;
 395         if (model >= 0)
 396                 l = m_links[model];
 397         else
 398         {
 399                 ZeroInitialize(l);
 400                 l.m_material = mat ? mat : m_materials[0];
 401         }
 402         m_links.push_back(l);
 403 }
 404
 405 //
 406 void btSoftBody::appendLink(int node0,
 407                                                         int node1,
 408                                                         Material* mat,
 409                                                         bool bcheckexist)
 410 {
 411         appendLink(&m_nodes[node0], &m_nodes[node1], mat, bcheckexist);
 412 }
 413
 414 //
 415 void btSoftBody::appendLink(Node* node0,
 416                                                         Node* node1,
 417                                                         Material* mat,
 418                                                         bool bcheckexist)
 419 {
 420         if ((!bcheckexist) || (!checkLink(node0, node1)))
 421         {
 422                 appendLink(-1, mat);
 423                 Link& l = m_links[m_links.size() - 1];
 424                 l.m_n[0] = node0;
 425                 l.m_n[1] = node1;
 426                 l.m_rl = (l.m_n[0]->m_x - l.m_n[1]->m_x).length();
 427                 m_bUpdateRtCst = true;
 428         }
 429 }
 430
 431 //
 432 void btSoftBody::appendFace(int model, Material* mat)
 433 {
 434         Face f;
 435         if (model >= 0)
 436         {
 437                 f = m_faces[model];
 438         }
 439         else
 440         {
 441                 ZeroInitialize(f);
 442                 f.m_material = mat ? mat : m_materials[0];
 443         }
 444         m_faces.push_back(f);
 445 }
 446
 447 //
 448 void btSoftBody::appendFace(int node0, int node1, int node2, Material* mat)
 449 {
 450         if (node0 == node1)
 451                 return;
 452         if (node1 == node2)
 453                 return;
 454         if (node2 == node0)
 455                 return;
 456
 457         appendFace(-1, mat);
 458         Face& f = m_faces[m_faces.size() - 1];
 459         btAssert(node0 != node1);
 460         btAssert(node1 != node2);
 461         btAssert(node2 != node0);
 462         f.m_n[0] = &m_nodes[node0];
 463         f.m_n[1] = &m_nodes[node1];
 464         f.m_n[2] = &m_nodes[node2];
 465         f.m_ra = AreaOf(f.m_n[0]->m_x,
 466                                         f.m_n[1]->m_x,
 467                                         f.m_n[2]->m_x);
 468         m_bUpdateRtCst = true;
 469 }
 470
 471 //
 472 void btSoftBody::appendTetra(int model, Material* mat)
 473 {
 474         Tetra t;
 475         if (model >= 0)
 476                 t = m_tetras[model];
 477         else
 478         {
 479                 ZeroInitialize(t);
 480                 t.m_material = mat ? mat : m_materials[0];
 481         }
 482         m_tetras.push_back(t);
 483 }
 484
 485 //
 486 void btSoftBody::appendTetra(int node0,
 487                                                          int node1,
 488                                                          int node2,
 489                                                          int node3,
 490                                                          Material* mat)
 491 {
 492         appendTetra(-1, mat);
 493         Tetra& t = m_tetras[m_tetras.size() - 1];
 494         t.m_n[0] = &m_nodes[node0];
 495         t.m_n[1] = &m_nodes[node1];
 496         t.m_n[2] = &m_nodes[node2];
 497         t.m_n[3] = &m_nodes[node3];
 498         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);
 499         m_bUpdateRtCst = true;
 500 }
 501
 502 //
 503
 504 void btSoftBody::appendAnchor(int node, btRigidBody* body, bool disableCollisionBetweenLinkedBodies, btScalar influence)
 505 {
 506         btVector3 local = body->getWorldTransform().inverse() * m_nodes[node].m_x;
 507         appendAnchor(node, body, local, disableCollisionBetweenLinkedBodies, influence);
 508 }
 509
 510 //
 511 void btSoftBody::appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies, btScalar influence)
 512 {
 513         if (disableCollisionBetweenLinkedBodies)
 514         {
 515                 if (m_collisionDisabledObjects.findLinearSearch(body) == m_collisionDisabledObjects.size())
 516                 {
 517                         m_collisionDisabledObjects.push_back(body);
 518                 }
 519         }
 520
 521         Anchor a;
 522         a.m_node = &m_nodes[node];
 523         a.m_body = body;
 524         a.m_local = localPivot;
 525         a.m_node->m_battach = 1;
 526         a.m_influence = influence;
 527         m_anchors.push_back(a);
 528 }
 529
 530 //
 531 void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body)
 532 {
 533         DeformableNodeRigidAnchor c;
 534         btSoftBody::Node& n = m_nodes[node];
 535         const btScalar ima = n.m_im;
 536         const btScalar imb = body->getInvMass();
 537         btVector3 nrm;
 538         const btCollisionShape* shp = body->getCollisionShape();
 539         const btTransform& wtr = body->getWorldTransform();
 540         btScalar dst =
 541                 m_worldInfo->m_sparsesdf.Evaluate(
 542                         wtr.invXform(m_nodes[node].m_x),
 543                         shp,
 544                         nrm,
 545                         0);
 546
 547         c.m_cti.m_colObj = body;
 548         c.m_cti.m_normal = wtr.getBasis() * nrm;
 549         c.m_cti.m_offset = dst;
 550         c.m_node = &m_nodes[node];
 551         const btScalar fc = m_cfg.kDF * body->getFriction();
 552         c.m_c2 = ima;
 553         c.m_c3 = fc;
 554         c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
 555         static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
 556         const btMatrix3x3& iwi = body->getInvInertiaTensorWorld();
 557         const btVector3 ra = n.m_x - wtr.getOrigin();
 558
 559         c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
 560         c.m_c1 = ra;
 561         c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x;
 562         c.m_node->m_battach = 1;
 563         m_deformableAnchors.push_back(c);
 564 }
 565
 566 void btSoftBody::removeAnchor(int node)
 567 {
 568         const btSoftBody::Node& n = m_nodes[node];
 569         for (int i = 0; i < m_deformableAnchors.size();)
 570         {
 571                 const DeformableNodeRigidAnchor& c = m_deformableAnchors[i];
 572                 if (c.m_node == &n)
 573                 {
 574                         m_deformableAnchors.removeAtIndex(i);
 575                 }
 576                 else
 577                 {
 578                         i++;
 579                 }
 580         }
 581 }
 582
 583 //
 584 void btSoftBody::appendDeformableAnchor(int node, btMultiBodyLinkCollider* link)
 585 {
 586         DeformableNodeRigidAnchor c;
 587         btSoftBody::Node& n = m_nodes[node];
 588         const btScalar ima = n.m_im;
 589         btVector3 nrm;
 590         const btCollisionShape* shp = link->getCollisionShape();
 591         const btTransform& wtr = link->getWorldTransform();
 592         btScalar dst =
 593                 m_worldInfo->m_sparsesdf.Evaluate(
 594                         wtr.invXform(m_nodes[node].m_x),
 595                         shp,
 596                         nrm,
 597                         0);
 598         c.m_cti.m_colObj = link;
 599         c.m_cti.m_normal = wtr.getBasis() * nrm;
 600         c.m_cti.m_offset = dst;
 601         c.m_node = &m_nodes[node];
 602         const btScalar fc = m_cfg.kDF * link->getFriction();
 603         c.m_c2 = ima;
 604         c.m_c3 = fc;
 605         c.m_c4 = link->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
 606         btVector3 normal = c.m_cti.m_normal;
 607         btVector3 t1 = generateUnitOrthogonalVector(normal);
 608         btVector3 t2 = btCross(normal, t1);
 609         btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
 610         findJacobian(link, jacobianData_normal, c.m_node->m_x, normal);
 611         findJacobian(link, jacobianData_t1, c.m_node->m_x, t1);
 612         findJacobian(link, jacobianData_t2, c.m_node->m_x, t2);
 613
 614         btScalar* J_n = &jacobianData_normal.m_jacobians[0];
 615         btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
 616         btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
 617
 618         btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
 619         btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
 620         btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
 621
 622         btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
 623                                         t1.getX(), t1.getY(), t1.getZ(),
 624                                         t2.getX(), t2.getY(), t2.getZ());  // world frame to local frame
 625         const int ndof = link->m_multiBody->getNumDofs() + 6;
 626         btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
 627         c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
 628         c.jacobianData_normal = jacobianData_normal;
 629         c.jacobianData_t1 = jacobianData_t1;
 630         c.jacobianData_t2 = jacobianData_t2;
 631         c.t1 = t1;
 632         c.t2 = t2;
 633         const btVector3 ra = n.m_x - wtr.getOrigin();
 634         c.m_c1 = ra;
 635         c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x;
 636         c.m_node->m_battach = 1;
 637         m_deformableAnchors.push_back(c);
 638 }
 639 //
 640 void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1)
 641 {
 642         LJoint* pj = new (btAlignedAlloc(sizeof(LJoint), 16)) LJoint();
 643         pj->m_bodies[0] = body0;
 644         pj->m_bodies[1] = body1;
 645         pj->m_refs[0] = pj->m_bodies[0].xform().inverse() * specs.position;
 646         pj->m_refs[1] = pj->m_bodies[1].xform().inverse() * specs.position;
 647         pj->m_cfm = specs.cfm;
 648         pj->m_erp = specs.erp;
 649         pj->m_split = specs.split;
 650         m_joints.push_back(pj);
 651 }
 652
 653 //
 654 void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Body body)
 655 {
 656         appendLinearJoint(specs, m_clusters[0], body);
 657 }
 658
 659 //
 660 void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, btSoftBody* body)
 661 {
 662         appendLinearJoint(specs, m_clusters[0], body->m_clusters[0]);
 663 }
 664
 665 //
 666 void btSoftBody::appendAngularJoint(const AJoint::Specs& specs, Cluster* body0, Body body1)
 667 {
 668         AJoint* pj = new (btAlignedAlloc(sizeof(AJoint), 16)) AJoint();
 669         pj->m_bodies[0] = body0;
 670         pj->m_bodies[1] = body1;
 671         pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis() * specs.axis;
 672         pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis() * specs.axis;
 673         pj->m_cfm = specs.cfm;
 674         pj->m_erp = specs.erp;
 675         pj->m_split = specs.split;
 676         pj->m_icontrol = specs.icontrol;
 677         m_joints.push_back(pj);
 678 }
 679
 680 //
 681 void btSoftBody::appendAngularJoint(const AJoint::Specs& specs, Body body)
 682 {
 683         appendAngularJoint(specs, m_clusters[0], body);
 684 }
 685
 686 //
 687 void btSoftBody::appendAngularJoint(const AJoint::Specs& specs, btSoftBody* body)
 688 {
 689         appendAngularJoint(specs, m_clusters[0], body->m_clusters[0]);
 690 }
 691
 692 //
 693 void btSoftBody::addForce(const btVector3& force)
 694 {
 695         for (int i = 0, ni = m_nodes.size(); i < ni; ++i) addForce(force, i);
 696 }
 697
 698 //
 699 void btSoftBody::addForce(const btVector3& force, int node)
 700 {
 701         Node& n = m_nodes[node];
 702         if (n.m_im > 0)
 703         {
 704                 n.m_f += force;
 705         }
 706 }
 707
 708 void btSoftBody::addAeroForceToNode(const btVector3& windVelocity, int nodeIndex)
 709 {
 710         btAssert(nodeIndex >= 0 && nodeIndex < m_nodes.size());
 711
 712         const btScalar dt = m_sst.sdt;
 713         const btScalar kLF = m_cfg.kLF;
 714         const btScalar kDG = m_cfg.kDG;
 715         //const btScalar kPR = m_cfg.kPR;
 716         //const btScalar kVC = m_cfg.kVC;
 717         const bool as_lift = kLF > 0;
 718         const bool as_drag = kDG > 0;
 719         const bool as_aero = as_lift || as_drag;
 720         const bool as_vaero = as_aero && (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
 721
 722         Node& n = m_nodes[nodeIndex];
 723
 724         if (n.m_im > 0)
 725         {
 726                 btSoftBody::sMedium medium;
 727
 728                 EvaluateMedium(m_worldInfo, n.m_x, medium);
 729                 medium.m_velocity = windVelocity;
 730                 medium.m_density = m_worldInfo->air_density;
 731
 732                 /* Aerodynamics                 */
 733                 if (as_vaero)
 734                 {
 735                         const btVector3 rel_v = n.m_v - medium.m_velocity;
 736                         const btScalar rel_v_len = rel_v.length();
 737                         const btScalar rel_v2 = rel_v.length2();
 738
 739                         if (rel_v2 > SIMD_EPSILON)
 740                         {
 741                                 const btVector3 rel_v_nrm = rel_v.normalized();
 742                                 btVector3 nrm = n.m_n;
 743
 744                                 if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSidedLiftDrag)
 745                                 {
 746                                         nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
 747                                         btVector3 fDrag(0, 0, 0);
 748                                         btVector3 fLift(0, 0, 0);
 749
 750                                         btScalar n_dot_v = nrm.dot(rel_v_nrm);
 751                                         btScalar tri_area = 0.5f * n.m_area;
 752
 753                                         fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
 754
 755                                         // Check angle of attack
 756                                         // cos(10º) = 0.98480
 757                                         if (0 < n_dot_v && n_dot_v < 0.98480f)
 758                                                 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));
 759
 760                                         // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node.
 761                                         btVector3 del_v_by_fDrag = fDrag * n.m_im * m_sst.sdt;
 762                                         btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
 763                                         btScalar v_len2 = n.m_v.length2();
 764
 765                                         if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
 766                                         {
 767                                                 btScalar del_v_by_fDrag_len = del_v_by_fDrag.length();
 768                                                 btScalar v_len = n.m_v.length();
 769                                                 fDrag *= btScalar(0.8) * (v_len / del_v_by_fDrag_len);
 770                                         }
 771
 772                                         n.m_f += fDrag;
 773                                         n.m_f += fLift;
 774                                 }
 775                                 else if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_Point || m_cfg.aeromodel == btSoftBody::eAeroModel::V_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided)
 776                                 {
 777                                         if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided)
 778                                                 nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
 779
 780                                         const btScalar dvn = btDot(rel_v, nrm);
 781                                         /* Compute forces       */
 782                                         if (dvn > 0)
 783                                         {
 784                                                 btVector3 force(0, 0, 0);
 785                                                 const btScalar c0 = n.m_area * dvn * rel_v2 / 2;
 786                                                 const btScalar c1 = c0 * medium.m_density;
 787                                                 force += nrm * (-c1 * kLF);
 788                                                 force += rel_v.normalized() * (-c1 * kDG);
 789                                                 ApplyClampedForce(n, force, dt);
 790                                         }
 791                                 }
 792                         }
 793                 }
 794         }
 795 }
 796
 797 void btSoftBody::addAeroForceToFace(const btVector3& windVelocity, int faceIndex)
 798 {
 799         const btScalar dt = m_sst.sdt;
 800         const btScalar kLF = m_cfg.kLF;
 801         const btScalar kDG = m_cfg.kDG;
 802         //      const btScalar kPR = m_cfg.kPR;
 803         //      const btScalar kVC = m_cfg.kVC;
 804         const bool as_lift = kLF > 0;
 805         const bool as_drag = kDG > 0;
 806         const bool as_aero = as_lift || as_drag;
 807         const bool as_faero = as_aero && (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
 808
 809         if (as_faero)
 810         {
 811                 btSoftBody::Face& f = m_faces[faceIndex];
 812
 813                 btSoftBody::sMedium medium;
 814
 815                 const btVector3 v = (f.m_n[0]->m_v + f.m_n[1]->m_v + f.m_n[2]->m_v) / 3;
 816                 const btVector3 x = (f.m_n[0]->m_x + f.m_n[1]->m_x + f.m_n[2]->m_x) / 3;
 817                 EvaluateMedium(m_worldInfo, x, medium);
 818                 medium.m_velocity = windVelocity;
 819                 medium.m_density = m_worldInfo->air_density;
 820                 const btVector3 rel_v = v - medium.m_velocity;
 821                 const btScalar rel_v_len = rel_v.length();
 822                 const btScalar rel_v2 = rel_v.length2();
 823
 824                 if (rel_v2 > SIMD_EPSILON)
 825                 {
 826                         const btVector3 rel_v_nrm = rel_v.normalized();
 827                         btVector3 nrm = f.m_normal;
 828
 829                         if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSidedLiftDrag)
 830                         {
 831                                 nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
 832
 833                                 btVector3 fDrag(0, 0, 0);
 834                                 btVector3 fLift(0, 0, 0);
 835
 836                                 btScalar n_dot_v = nrm.dot(rel_v_nrm);
 837                                 btScalar tri_area = 0.5f * f.m_ra;
 838
 839                                 fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
 840
 841                                 // Check angle of attack
 842                                 // cos(10º) = 0.98480
 843                                 if (0 < n_dot_v && n_dot_v < 0.98480f)
 844                                         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));
 845
 846                                 fDrag /= 3;
 847                                 fLift /= 3;
 848
 849                                 for (int j = 0; j < 3; ++j)
 850                                 {
 851                                         if (f.m_n[j]->m_im > 0)
 852                                         {
 853                                                 // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node.
 854                                                 btVector3 del_v_by_fDrag = fDrag * f.m_n[j]->m_im * m_sst.sdt;
 855                                                 btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
 856                                                 btScalar v_len2 = f.m_n[j]->m_v.length2();
 857
 858                                                 if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
 859                                                 {
 860                                                         btScalar del_v_by_fDrag_len = del_v_by_fDrag.length();
 861                                                         btScalar v_len = f.m_n[j]->m_v.length();
 862                                                         fDrag *= btScalar(0.8) * (v_len / del_v_by_fDrag_len);
 863                                                 }
 864
 865                                                 f.m_n[j]->m_f += fDrag;
 866                                                 f.m_n[j]->m_f += fLift;
 867                                         }
 868                                 }
 869                         }
 870                         else if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided)
 871                         {
 872                                 if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided)
 873                                         nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
 874
 875                                 const btScalar dvn = btDot(rel_v, nrm);
 876                                 /* Compute forces       */
 877                                 if (dvn > 0)
 878                                 {
 879                                         btVector3 force(0, 0, 0);
 880                                         const btScalar c0 = f.m_ra * dvn * rel_v2;
 881                                         const btScalar c1 = c0 * medium.m_density;
 882                                         force += nrm * (-c1 * kLF);
 883                                         force += rel_v.normalized() * (-c1 * kDG);
 884                                         force /= 3;
 885                                         for (int j = 0; j < 3; ++j) ApplyClampedForce(*f.m_n[j], force, dt);
 886                                 }
 887                         }
 888                 }
 889         }
 890 }
 891
 892 //
 893 void btSoftBody::addVelocity(const btVector3& velocity)
 894 {
 895         for (int i = 0, ni = m_nodes.size(); i < ni; ++i) addVelocity(velocity, i);
 896 }
 897
 898 /* Set velocity for the entire body                                                                             */
 899 void btSoftBody::setVelocity(const btVector3& velocity)
 900 {
 901         for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
 902         {
 903                 Node& n = m_nodes[i];
 904                 if (n.m_im > 0)
 905                 {
 906                         n.m_v = velocity;
 907                         n.m_vn = velocity;
 908                 }
 909         }
 910 }
 911
 912 //
 913 void btSoftBody::addVelocity(const btVector3& velocity, int node)
 914 {
 915         Node& n = m_nodes[node];
 916         if (n.m_im > 0)
 917         {
 918                 n.m_v += velocity;
 919         }
 920 }
 921
 922 //
 923 void btSoftBody::setMass(int node, btScalar mass)
 924 {
 925         m_nodes[node].m_im = mass > 0 ? 1 / mass : 0;
 926         m_bUpdateRtCst = true;
 927 }
 928
 929 //
 930 btScalar btSoftBody::getMass(int node) const
 931 {
 932         return (m_nodes[node].m_im > 0 ? 1 / m_nodes[node].m_im : 0);
 933 }
 934
 935 //
 936 btScalar btSoftBody::getTotalMass() const
 937 {
 938         btScalar mass = 0;
 939         for (int i = 0; i < m_nodes.size(); ++i)
 940         {
 941                 mass += getMass(i);
 942         }
 943         return (mass);
 944 }
 945
 946 //
 947 void btSoftBody::setTotalMass(btScalar mass, bool fromfaces)
 948 {
 949         int i;
 950
 951         if (fromfaces)
 952         {
 953                 for (i = 0; i < m_nodes.size(); ++i)
 954                 {
 955                         m_nodes[i].m_im = 0;
 956                 }
 957                 for (i = 0; i < m_faces.size(); ++i)
 958                 {
 959                         const Face& f = m_faces[i];
 960                         const btScalar twicearea = AreaOf(f.m_n[0]->m_x,
 961                                                                                           f.m_n[1]->m_x,
 962                                                                                           f.m_n[2]->m_x);
 963                         for (int j = 0; j < 3; ++j)
 964                         {
 965                                 f.m_n[j]->m_im += twicearea;
 966                         }
 967                 }
 968                 for (i = 0; i < m_nodes.size(); ++i)
 969                 {
 970                         m_nodes[i].m_im = 1 / m_nodes[i].m_im;
 971                 }
 972         }
 973         const btScalar tm = getTotalMass();
 974         const btScalar itm = 1 / tm;
 975         for (i = 0; i < m_nodes.size(); ++i)
 976         {
 977                 m_nodes[i].m_im /= itm * mass;
 978         }
 979         m_bUpdateRtCst = true;
 980 }
 981
 982 //
 983 void btSoftBody::setTotalDensity(btScalar density)
 984 {
 985         setTotalMass(getVolume() * density, true);
 986 }
 987
 988 //
 989 void btSoftBody::setVolumeMass(btScalar mass)
 990 {
 991         btAlignedObjectArray<btScalar> ranks;
 992         ranks.resize(m_nodes.size(), 0);
 993         int i;
 994
 995         for (i = 0; i < m_nodes.size(); ++i)
 996         {
 997                 m_nodes[i].m_im = 0;
 998         }
 999         for (i = 0; i < m_tetras.size(); ++i)
1000         {
1001                 const Tetra& t = m_tetras[i];
1002                 for (int j = 0; j < 4; ++j)
1003                 {
1004                         t.m_n[j]->m_im += btFabs(t.m_rv);
1005                         ranks[int(t.m_n[j] - &m_nodes[0])] += 1;
1006                 }
1007         }
1008         for (i = 0; i < m_nodes.size(); ++i)
1009         {
1010                 if (m_nodes[i].m_im > 0)
1011                 {
1012                         m_nodes[i].m_im = ranks[i] / m_nodes[i].m_im;
1013                 }
1014         }
1015         setTotalMass(mass, false);
1016 }
1017
1018 //
1019 void btSoftBody::setVolumeDensity(btScalar density)
1020 {
1021         btScalar volume = 0;
1022         for (int i = 0; i < m_tetras.size(); ++i)
1023         {
1024                 const Tetra& t = m_tetras[i];
1025                 for (int j = 0; j < 4; ++j)
1026                 {
1027                         volume += btFabs(t.m_rv);
1028                 }
1029         }
1030         setVolumeMass(volume * density / 6);
1031 }
1032
1033 //
1034 btVector3 btSoftBody::getLinearVelocity()
1035 {
1036         btVector3 total_momentum = btVector3(0, 0, 0);
1037         for (int i = 0; i < m_nodes.size(); ++i)
1038         {
1039                 btScalar mass = m_nodes[i].m_im == 0 ? 0 : 1.0 / m_nodes[i].m_im;
1040                 total_momentum += mass * m_nodes[i].m_v;
1041         }
1042         btScalar total_mass = getTotalMass();
1043         return total_mass == 0 ? total_momentum : total_momentum / total_mass;
1044 }
1045
1046 //
1047 void btSoftBody::setLinearVelocity(const btVector3& linVel)
1048 {
1049         btVector3 old_vel = getLinearVelocity();
1050         btVector3 diff = linVel - old_vel;
1051         for (int i = 0; i < m_nodes.size(); ++i)
1052                 m_nodes[i].m_v += diff;
1053 }
1054
1055 //
1056 void btSoftBody::setAngularVelocity(const btVector3& angVel)
1057 {
1058         btVector3 old_vel = getLinearVelocity();
1059         btVector3 com = getCenterOfMass();
1060         for (int i = 0; i < m_nodes.size(); ++i)
1061         {
1062                 m_nodes[i].m_v = angVel.cross(m_nodes[i].m_x - com) + old_vel;
1063         }
1064 }
1065
1066 //
1067 btTransform btSoftBody::getRigidTransform()
1068 {
1069         btVector3 t = getCenterOfMass();
1070         btMatrix3x3 S;
1071         S.setZero();
1072         // Get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i ||
1073         // It's important to make sure that S has the correct signs.
1074         // SVD is only unique up to the ordering of singular values.
1075         // SVD will manipulate U and V to ensure the ordering of singular values. If all three singular
1076         // vaues are negative, SVD will permute colums of U to make two of them positive.
1077         for (int i = 0; i < m_nodes.size(); ++i)
1078         {
1079                 S -= OuterProduct(m_X[i], t - m_nodes[i].m_x);
1080         }
1081         btVector3 sigma;
1082         btMatrix3x3 U, V;
1083         singularValueDecomposition(S, U, sigma, V);
1084         btMatrix3x3 R = V * U.transpose();
1085         btTransform trs;
1086         trs.setIdentity();
1087         trs.setOrigin(t);
1088         trs.setBasis(R);
1089         return trs;
1090 }
1091
1092 //
1093 void btSoftBody::transformTo(const btTransform& trs)
1094 {
1095         // get the current best rigid fit
1096         btTransform current_transform = getRigidTransform();
1097         // apply transform in material space
1098         btTransform new_transform = trs * current_transform.inverse();
1099         transform(new_transform);
1100 }
1101
1102 //
1103 void btSoftBody::transform(const btTransform& trs)
1104 {
1105         const btScalar margin = getCollisionShape()->getMargin();
1106         ATTRIBUTE_ALIGNED16(btDbvtVolume)
1107         vol;
1108
1109         for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
1110         {
1111                 Node& n = m_nodes[i];
1112                 n.m_x = trs * n.m_x;
1113                 n.m_q = trs * n.m_q;
1114                 n.m_n = trs.getBasis() * n.m_n;
1115                 vol = btDbvtVolume::FromCR(n.m_x, margin);
1116
1117                 m_ndbvt.update(n.m_leaf, vol);
1118         }
1119         updateNormals();
1120         updateBounds();
1121         updateConstants();
1122 }
1123
1124 //
1125 void btSoftBody::translate(const btVector3& trs)
1126 {
1127         btTransform t;
1128         t.setIdentity();
1129         t.setOrigin(trs);
1130         transform(t);
1131 }
1132
1133 //
1134 void btSoftBody::rotate(const btQuaternion& rot)
1135 {
1136         btTransform t;
1137         t.setIdentity();
1138         t.setRotation(rot);
1139         transform(t);
1140 }
1141
1142 //
1143 void btSoftBody::scale(const btVector3& scl)
1144 {
1145         const btScalar margin = getCollisionShape()->getMargin();
1146         ATTRIBUTE_ALIGNED16(btDbvtVolume)
1147         vol;
1148
1149         for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
1150         {
1151                 Node& n = m_nodes[i];
1152                 n.m_x *= scl;
1153                 n.m_q *= scl;
1154                 vol = btDbvtVolume::FromCR(n.m_x, margin);
1155                 m_ndbvt.update(n.m_leaf, vol);
1156         }
1157         updateNormals();
1158         updateBounds();
1159         updateConstants();
1160         initializeDmInverse();
1161 }
1162
1163 //
1164 btScalar btSoftBody::getRestLengthScale()
1165 {
1166         return m_restLengthScale;
1167 }
1168
1169 //
1170 void btSoftBody::setRestLengthScale(btScalar restLengthScale)
1171 {
1172         for (int i = 0, ni = m_links.size(); i < ni; ++i)
1173         {
1174                 Link& l = m_links[i];
1175                 l.m_rl = l.m_rl / m_restLengthScale * restLengthScale;
1176                 l.m_c1 = l.m_rl * l.m_rl;
1177         }
1178         m_restLengthScale = restLengthScale;
1179
1180         if (getActivationState() == ISLAND_SLEEPING)
1181                 activate();
1182 }
1183
1184 //
1185 void btSoftBody::setPose(bool bvolume, bool bframe)
1186 {
1187         m_pose.m_bvolume = bvolume;
1188         m_pose.m_bframe = bframe;
1189         int i, ni;
1190
1191         /* Weights              */
1192         const btScalar omass = getTotalMass();
1193         const btScalar kmass = omass * m_nodes.size() * 1000;
1194         btScalar tmass = omass;
1195         m_pose.m_wgh.resize(m_nodes.size());
1196         for (i = 0, ni = m_nodes.size(); i < ni; ++i)
1197         {
1198                 if (m_nodes[i].m_im <= 0) tmass += kmass;
1199         }
1200         for (i = 0, ni = m_nodes.size(); i < ni; ++i)
1201         {
1202                 Node& n = m_nodes[i];
1203                 m_pose.m_wgh[i] = n.m_im > 0 ? 1 / (m_nodes[i].m_im * tmass) : kmass / tmass;
1204         }
1205         /* Pos          */
1206         const btVector3 com = evaluateCom();
1207         m_pose.m_pos.resize(m_nodes.size());
1208         for (i = 0, ni = m_nodes.size(); i < ni; ++i)
1209         {
1210                 m_pose.m_pos[i] = m_nodes[i].m_x - com;
1211         }
1212         m_pose.m_volume = bvolume ? getVolume() : 0;
1213         m_pose.m_com = com;
1214         m_pose.m_rot.setIdentity();
1215         m_pose.m_scl.setIdentity();
1216         /* Aqq          */
1217         m_pose.m_aqq[0] =
1218                 m_pose.m_aqq[1] =
1219                         m_pose.m_aqq[2] = btVector3(0, 0, 0);
1220         for (i = 0, ni = m_nodes.size(); i < ni; ++i)
1221         {
1222                 const btVector3& q = m_pose.m_pos[i];
1223                 const btVector3 mq = m_pose.m_wgh[i] * q;
1224                 m_pose.m_aqq[0] += mq.x() * q;
1225                 m_pose.m_aqq[1] += mq.y() * q;
1226                 m_pose.m_aqq[2] += mq.z() * q;
1227         }
1228         m_pose.m_aqq = m_pose.m_aqq.inverse();
1229
1230         updateConstants();
1231 }
1232
1233 void btSoftBody::resetLinkRestLengths()
1234 {
1235         for (int i = 0, ni = m_links.size(); i < ni; ++i)
1236         {
1237                 Link& l = m_links[i];
1238                 l.m_rl = (l.m_n[0]->m_x - l.m_n[1]->m_x).length();
1239                 l.m_c1 = l.m_rl * l.m_rl;
1240         }
1241 }
1242
1243 //
1244 btScalar btSoftBody::getVolume() const
1245 {
1246         btScalar vol = 0;
1247         if (m_nodes.size() > 0)
1248         {
1249                 int i, ni;
1250
1251                 const btVector3 org = m_nodes[0].m_x;
1252                 for (i = 0, ni = m_faces.size(); i < ni; ++i)
1253                 {
1254                         const Face& f = m_faces[i];
1255                         vol += btDot(f.m_n[0]->m_x - org, btCross(f.m_n[1]->m_x - org, f.m_n[2]->m_x - org));
1256                 }
1257                 vol /= (btScalar)6;
1258         }
1259         return (vol);
1260 }
1261
1262 //
1263 int btSoftBody::clusterCount() const
1264 {
1265         return (m_clusters.size());
1266 }
1267
1268 //
1269 btVector3 btSoftBody::clusterCom(const Cluster* cluster)
1270 {
1271         btVector3 com(0, 0, 0);
1272         for (int i = 0, ni = cluster->m_nodes.size(); i < ni; ++i)
1273         {
1274                 com += cluster->m_nodes[i]->m_x * cluster->m_masses[i];
1275         }
1276         return (com * cluster->m_imass);
1277 }
1278
1279 //
1280 btVector3 btSoftBody::clusterCom(int cluster) const
1281 {
1282         return (clusterCom(m_clusters[cluster]));
1283 }
1284
1285 //
1286 btVector3 btSoftBody::clusterVelocity(const Cluster* cluster, const btVector3& rpos)
1287 {
1288         return (cluster->m_lv + btCross(cluster->m_av, rpos));
1289 }
1290
1291 //
1292 void btSoftBody::clusterVImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse)
1293 {
1294         const btVector3 li = cluster->m_imass * impulse;
1295         const btVector3 ai = cluster->m_invwi * btCross(rpos, impulse);
1296         cluster->m_vimpulses[0] += li;
1297         cluster->m_lv += li;
1298         cluster->m_vimpulses[1] += ai;
1299         cluster->m_av += ai;
1300         cluster->m_nvimpulses++;
1301 }
1302
1303 //
1304 void btSoftBody::clusterDImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse)
1305 {
1306         const btVector3 li = cluster->m_imass * impulse;
1307         const btVector3 ai = cluster->m_invwi * btCross(rpos, impulse);
1308         cluster->m_dimpulses[0] += li;
1309         cluster->m_dimpulses[1] += ai;
1310         cluster->m_ndimpulses++;
1311 }
1312
1313 //
1314 void btSoftBody::clusterImpulse(Cluster* cluster, const btVector3& rpos, const Impulse& impulse)
1315 {
1316         if (impulse.m_asVelocity) clusterVImpulse(cluster, rpos, impulse.m_velocity);
1317         if (impulse.m_asDrift) clusterDImpulse(cluster, rpos, impulse.m_drift);
1318 }
1319
1320 //
1321 void btSoftBody::clusterVAImpulse(Cluster* cluster, const btVector3& impulse)
1322 {
1323         const btVector3 ai = cluster->m_invwi * impulse;
1324         cluster->m_vimpulses[1] += ai;
1325         cluster->m_av += ai;
1326         cluster->m_nvimpulses++;
1327 }
1328
1329 //
1330 void btSoftBody::clusterDAImpulse(Cluster* cluster, const btVector3& impulse)
1331 {
1332         const btVector3 ai = cluster->m_invwi * impulse;
1333         cluster->m_dimpulses[1] += ai;
1334         cluster->m_ndimpulses++;
1335 }
1336
1337 //
1338 void btSoftBody::clusterAImpulse(Cluster* cluster, const Impulse& impulse)
1339 {
1340         if (impulse.m_asVelocity) clusterVAImpulse(cluster, impulse.m_velocity);
1341         if (impulse.m_asDrift) clusterDAImpulse(cluster, impulse.m_drift);
1342 }
1343
1344 //
1345 void btSoftBody::clusterDCImpulse(Cluster* cluster, const btVector3& impulse)
1346 {
1347         cluster->m_dimpulses[0] += impulse * cluster->m_imass;
1348         cluster->m_ndimpulses++;
1349 }
1350
1351 struct NodeLinks
1352 {
1353         btAlignedObjectArray<int> m_links;
1354 };
1355
1356 //
1357 int btSoftBody::generateBendingConstraints(int distance, Material* mat)
1358 {
1359         int i, j;
1360
1361         if (distance > 1)
1362         {
1363                 /* Build graph  */
1364                 const int n = m_nodes.size();
1365                 const unsigned inf = (~(unsigned)0) >> 1;
1366                 unsigned* adj = new unsigned[n * n];
1367
1368 #define IDX(_x_, _y_) ((_y_)*n + (_x_))
1369                 for (j = 0; j < n; ++j)
1370                 {
1371                         for (i = 0; i < n; ++i)
1372                         {
1373                                 if (i != j)
1374                                 {
1375                                         adj[IDX(i, j)] = adj[IDX(j, i)] = inf;
1376                                 }
1377                                 else
1378                                 {
1379                                         adj[IDX(i, j)] = adj[IDX(j, i)] = 0;
1380                                 }
1381                         }
1382                 }
1383                 for (i = 0; i < m_links.size(); ++i)
1384                 {
1385                         const int ia = (int)(m_links[i].m_n[0] - &m_nodes[0]);
1386                         const int ib = (int)(m_links[i].m_n[1] - &m_nodes[0]);
1387                         adj[IDX(ia, ib)] = 1;
1388                         adj[IDX(ib, ia)] = 1;
1389                 }
1390
1391                 //special optimized case for distance == 2
1392                 if (distance == 2)
1393                 {
1394                         btAlignedObjectArray<NodeLinks> nodeLinks;
1395
1396                         /* Build node links */
1397                         nodeLinks.resize(m_nodes.size());
1398
1399                         for (i = 0; i < m_links.size(); ++i)
1400                         {
1401                                 const int ia = (int)(m_links[i].m_n[0] - &m_nodes[0]);
1402                                 const int ib = (int)(m_links[i].m_n[1] - &m_nodes[0]);
1403                                 if (nodeLinks[ia].m_links.findLinearSearch(ib) == nodeLinks[ia].m_links.size())
1404                                         nodeLinks[ia].m_links.push_back(ib);
1405
1406                                 if (nodeLinks[ib].m_links.findLinearSearch(ia) == nodeLinks[ib].m_links.size())
1407                                         nodeLinks[ib].m_links.push_back(ia);
1408                         }
1409                         for (int ii = 0; ii < nodeLinks.size(); ii++)
1410                         {
1411                                 int i = ii;
1412
1413                                 for (int jj = 0; jj < nodeLinks[ii].m_links.size(); jj++)
1414                                 {
1415                                         int k = nodeLinks[ii].m_links[jj];
1416                                         for (int kk = 0; kk < nodeLinks[k].m_links.size(); kk++)
1417                                         {
1418                                                 int j = nodeLinks[k].m_links[kk];
1419                                                 if (i != j)
1420                                                 {
1421                                                         const unsigned sum = adj[IDX(i, k)] + adj[IDX(k, j)];
1422                                                         btAssert(sum == 2);
1423                                                         if (adj[IDX(i, j)] > sum)
1424                                                         {
1425                                                                 adj[IDX(i, j)] = adj[IDX(j, i)] = sum;
1426                                                         }
1427                                                 }
1428                                         }
1429                                 }
1430                         }
1431                 }
1432                 else
1433                 {
1434                         ///generic Floyd's algorithm
1435                         for (int k = 0; k < n; ++k)
1436                         {
1437                                 for (j = 0; j < n; ++j)
1438                                 {
1439                                         for (i = j + 1; i < n; ++i)
1440                                         {
1441                                                 const unsigned sum = adj[IDX(i, k)] + adj[IDX(k, j)];
1442                                                 if (adj[IDX(i, j)] > sum)
1443                                                 {
1444                                                         adj[IDX(i, j)] = adj[IDX(j, i)] = sum;
1445                                                 }
1446                                         }
1447                                 }
1448                         }
1449                 }
1450
1451                 /* Build links  */
1452                 int nlinks = 0;
1453                 for (j = 0; j < n; ++j)
1454                 {
1455                         for (i = j + 1; i < n; ++i)
1456                         {
1457                                 if (adj[IDX(i, j)] == (unsigned)distance)
1458                                 {
1459                                         appendLink(i, j, mat);
1460                                         m_links[m_links.size() - 1].m_bbending = 1;
1461                                         ++nlinks;
1462                                 }
1463                         }
1464                 }
1465                 delete[] adj;
1466                 return (nlinks);
1467         }
1468         return (0);
1469 }
1470
1471 //
1472 void btSoftBody::randomizeConstraints()
1473 {
1474         unsigned long seed = 243703;
1475 #define NEXTRAND (seed = (1664525L * seed + 1013904223L) & 0xffffffff)
1476         int i, ni;
1477
1478         for (i = 0, ni = m_links.size(); i < ni; ++i)
1479         {
1480                 btSwap(m_links[i], m_links[NEXTRAND % ni]);
1481         }
1482         for (i = 0, ni = m_faces.size(); i < ni; ++i)
1483         {
1484                 btSwap(m_faces[i], m_faces[NEXTRAND % ni]);
1485         }
1486 #undef NEXTRAND
1487 }
1488
1489 void btSoftBody::updateState(const btAlignedObjectArray<btVector3>& q, const btAlignedObjectArray<btVector3>& v)
1490 {
1491         int node_count = m_nodes.size();
1492         btAssert(node_count == q.size());
1493         btAssert(node_count == v.size());
1494         for (int i = 0; i < node_count; i++)
1495         {
1496                 Node& n = m_nodes[i];
1497                 n.m_x = q[i];
1498                 n.m_q = q[i];
1499                 n.m_v = v[i];
1500                 n.m_vn = v[i];
1501         }
1502 }
1503
1504 //
1505 void btSoftBody::releaseCluster(int index)
1506 {
1507         Cluster* c = m_clusters[index];
1508         if (c->m_leaf) m_cdbvt.remove(c->m_leaf);
1509         c->~Cluster();
1510         btAlignedFree(c);
1511         m_clusters.remove(c);
1512 }
1513
1514 //
1515 void btSoftBody::releaseClusters()
1516 {
1517         while (m_clusters.size() > 0) releaseCluster(0);
1518 }
1519
1520 //
1521 int btSoftBody::generateClusters(int k, int maxiterations)
1522 {
1523         int i;
1524         releaseClusters();
1525         m_clusters.resize(btMin(k, m_nodes.size()));
1526         for (i = 0; i < m_clusters.size(); ++i)
1527         {
1528                 m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
1529                 m_clusters[i]->m_collide = true;
1530         }
1531         k = m_clusters.size();
1532         if (k > 0)
1533         {
1534                 /* Initialize           */
1535                 btAlignedObjectArray<btVector3> centers;
1536                 btVector3 cog(0, 0, 0);
1537                 int i;
1538                 for (i = 0; i < m_nodes.size(); ++i)
1539                 {
1540                         cog += m_nodes[i].m_x;
1541                         m_clusters[(i * 29873) % m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
1542                 }
1543                 cog /= (btScalar)m_nodes.size();
1544                 centers.resize(k, cog);
1545                 /* Iterate                      */
1546                 const btScalar slope = 16;
1547                 bool changed;
1548                 int iterations = 0;
1549                 do
1550                 {
1551                         const btScalar w = 2 - btMin<btScalar>(1, iterations / slope);
1552                         changed = false;
1553                         iterations++;
1554                         int i;
1555
1556                         for (i = 0; i < k; ++i)
1557                         {
1558                                 btVector3 c(0, 0, 0);
1559                                 for (int j = 0; j < m_clusters[i]->m_nodes.size(); ++j)
1560                                 {
1561                                         c += m_clusters[i]->m_nodes[j]->m_x;
1562                                 }
1563                                 if (m_clusters[i]->m_nodes.size())
1564                                 {
1565                                         c /= (btScalar)m_clusters[i]->m_nodes.size();
1566                                         c = centers[i] + (c - centers[i]) * w;
1567                                         changed |= ((c - centers[i]).length2() > SIMD_EPSILON);
1568                                         centers[i] = c;
1569                                         m_clusters[i]->m_nodes.resize(0);
1570                                 }
1571                         }
1572                         for (i = 0; i < m_nodes.size(); ++i)
1573                         {
1574                                 const btVector3 nx = m_nodes[i].m_x;
1575                                 int kbest = 0;
1576                                 btScalar kdist = ClusterMetric(centers[0], nx);
1577                                 for (int j = 1; j < k; ++j)
1578                                 {
1579                                         const btScalar d = ClusterMetric(centers[j], nx);
1580                                         if (d < kdist)
1581                                         {
1582                                                 kbest = j;
1583                                                 kdist = d;
1584                                         }
1585                                 }
1586                                 m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
1587                         }
1588                 } while (changed && (iterations < maxiterations));
1589                 /* Merge                */
1590                 btAlignedObjectArray<int> cids;
1591                 cids.resize(m_nodes.size(), -1);
1592                 for (i = 0; i < m_clusters.size(); ++i)
1593                 {
1594                         for (int j = 0; j < m_clusters[i]->m_nodes.size(); ++j)
1595                         {
1596                                 cids[int(m_clusters[i]->m_nodes[j] - &m_nodes[0])] = i;
1597                         }
1598                 }
1599                 for (i = 0; i < m_faces.size(); ++i)
1600                 {
1601                         const int idx[] = {int(m_faces[i].m_n[0] - &m_nodes[0]),
1602                                                            int(m_faces[i].m_n[1] - &m_nodes[0]),
1603                                                            int(m_faces[i].m_n[2] - &m_nodes[0])};
1604                         for (int j = 0; j < 3; ++j)
1605                         {
1606                                 const int cid = cids[idx[j]];
1607                                 for (int q = 1; q < 3; ++q)
1608                                 {
1609                                         const int kid = idx[(j + q) % 3];
1610                                         if (cids[kid] != cid)
1611                                         {
1612                                                 if (m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid]) == m_clusters[cid]->m_nodes.size())
1613                                                 {
1614                                                         m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
1615                                                 }
1616                                         }
1617                                 }
1618                         }
1619                 }
1620                 /* Master               */
1621                 if (m_clusters.size() > 1)
1622                 {
1623                         Cluster* pmaster = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
1624                         pmaster->m_collide = false;
1625                         pmaster->m_nodes.reserve(m_nodes.size());
1626                         for (int i = 0; i < m_nodes.size(); ++i) pmaster->m_nodes.push_back(&m_nodes[i]);
1627                         m_clusters.push_back(pmaster);
1628                         btSwap(m_clusters[0], m_clusters[m_clusters.size() - 1]);
1629                 }
1630                 /* Terminate    */
1631                 for (i = 0; i < m_clusters.size(); ++i)
1632                 {
1633                         if (m_clusters[i]->m_nodes.size() == 0)
1634                         {
1635                                 releaseCluster(i--);
1636                         }
1637                 }
1638         }
1639         else
1640         {
1641                 //create a cluster for each tetrahedron (if tetrahedra exist) or each face
1642                 if (m_tetras.size())
1643                 {
1644                         m_clusters.resize(m_tetras.size());
1645                         for (i = 0; i < m_clusters.size(); ++i)
1646                         {
1647                                 m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
1648                                 m_clusters[i]->m_collide = true;
1649                         }
1650                         for (i = 0; i < m_tetras.size(); i++)
1651                         {
1652                                 for (int j = 0; j < 4; j++)
1653                                 {
1654                                         m_clusters[i]->m_nodes.push_back(m_tetras[i].m_n[j]);
1655                                 }
1656                         }
1657                 }
1658                 else
1659                 {
1660                         m_clusters.resize(m_faces.size());
1661                         for (i = 0; i < m_clusters.size(); ++i)
1662                         {
1663                                 m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
1664                                 m_clusters[i]->m_collide = true;
1665                         }
1666
1667                         for (i = 0; i < m_faces.size(); ++i)
1668                         {
1669                                 for (int j = 0; j < 3; ++j)
1670                                 {
1671                                         m_clusters[i]->m_nodes.push_back(m_faces[i].m_n[j]);
1672                                 }
1673                         }
1674                 }
1675         }
1676
1677         if (m_clusters.size())
1678         {
1679                 initializeClusters();
1680                 updateClusters();
1681
1682                 //for self-collision
1683                 m_clusterConnectivity.resize(m_clusters.size() * m_clusters.size());
1684                 {
1685                         for (int c0 = 0; c0 < m_clusters.size(); c0++)
1686                         {
1687                                 m_clusters[c0]->m_clusterIndex = c0;
1688                                 for (int c1 = 0; c1 < m_clusters.size(); c1++)
1689                                 {
1690                                         bool connected = false;
1691                                         Cluster* cla = m_clusters[c0];
1692                                         Cluster* clb = m_clusters[c1];
1693                                         for (int i = 0; !connected && i < cla->m_nodes.size(); i++)
1694                                         {
1695                                                 for (int j = 0; j < clb->m_nodes.size(); j++)
1696                                                 {
1697                                                         if (cla->m_nodes[i] == clb->m_nodes[j])
1698                                                         {
1699                                                                 connected = true;
1700                                                                 break;
1701                                                         }
1702                                                 }
1703                                         }
1704                                         m_clusterConnectivity[c0 + c1 * m_clusters.size()] = connected;
1705                                 }
1706                         }
1707                 }
1708         }
1709
1710         return (m_clusters.size());
1711 }
1712
1713 //
1714 void btSoftBody::refine(ImplicitFn* ifn, btScalar accurary, bool cut)
1715 {
1716         const Node* nbase = &m_nodes[0];
1717         int ncount = m_nodes.size();
1718         btSymMatrix<int> edges(ncount, -2);
1719         int newnodes = 0;
1720         int i, j, k, ni;
1721
1722         /* Filter out           */
1723         for (i = 0; i < m_links.size(); ++i)
1724         {
1725                 Link& l = m_links[i];
1726                 if (l.m_bbending)
1727                 {
1728                         if (!SameSign(ifn->Eval(l.m_n[0]->m_x), ifn->Eval(l.m_n[1]->m_x)))
1729                         {
1730                                 btSwap(m_links[i], m_links[m_links.size() - 1]);
1731                                 m_links.pop_back();
1732                                 --i;
1733                         }
1734                 }
1735         }
1736         /* Fill edges           */
1737         for (i = 0; i < m_links.size(); ++i)
1738         {
1739                 Link& l = m_links[i];
1740                 edges(int(l.m_n[0] - nbase), int(l.m_n[1] - nbase)) = -1;
1741         }
1742         for (i = 0; i < m_faces.size(); ++i)
1743         {
1744                 Face& f = m_faces[i];
1745                 edges(int(f.m_n[0] - nbase), int(f.m_n[1] - nbase)) = -1;
1746                 edges(int(f.m_n[1] - nbase), int(f.m_n[2] - nbase)) = -1;
1747                 edges(int(f.m_n[2] - nbase), int(f.m_n[0] - nbase)) = -1;
1748         }
1749         /* Intersect            */
1750         for (i = 0; i < ncount; ++i)
1751         {
1752                 for (j = i + 1; j < ncount; ++j)
1753                 {
1754                         if (edges(i, j) == -1)
1755                         {
1756                                 Node& a = m_nodes[i];
1757                                 Node& b = m_nodes[j];
1758                                 const btScalar t = ImplicitSolve(ifn, a.m_x, b.m_x, accurary);
1759                                 if (t > 0)
1760                                 {
1761                                         const btVector3 x = Lerp(a.m_x, b.m_x, t);
1762                                         const btVector3 v = Lerp(a.m_v, b.m_v, t);
1763                                         btScalar m = 0;
1764                                         if (a.m_im > 0)
1765                                         {
1766                                                 if (b.m_im > 0)
1767                                                 {
1768                                                         const btScalar ma = 1 / a.m_im;
1769                                                         const btScalar mb = 1 / b.m_im;
1770                                                         const btScalar mc = Lerp(ma, mb, t);
1771                                                         const btScalar f = (ma + mb) / (ma + mb + mc);
1772                                                         a.m_im = 1 / (ma * f);
1773                                                         b.m_im = 1 / (mb * f);
1774                                                         m = mc * f;
1775                                                 }
1776                                                 else
1777                                                 {
1778                                                         a.m_im /= 0.5f;
1779                                                         m = 1 / a.m_im;
1780                                                 }
1781                                         }
1782                                         else
1783                                         {
1784                                                 if (b.m_im > 0)
1785                                                 {
1786                                                         b.m_im /= 0.5f;
1787                                                         m = 1 / b.m_im;
1788                                                 }
1789                                                 else
1790                                                         m = 0;
1791                                         }
1792                                         appendNode(x, m);
1793                                         edges(i, j) = m_nodes.size() - 1;
1794                                         m_nodes[edges(i, j)].m_v = v;
1795                                         ++newnodes;
1796                                 }
1797                         }
1798                 }
1799         }
1800         nbase = &m_nodes[0];
1801         /* Refine links         */
1802         for (i = 0, ni = m_links.size(); i < ni; ++i)
1803         {
1804                 Link& feat = m_links[i];
1805                 const int idx[] = {int(feat.m_n[0] - nbase),
1806                                                    int(feat.m_n[1] - nbase)};
1807                 if ((idx[0] < ncount) && (idx[1] < ncount))
1808                 {
1809                         const int ni = edges(idx[0], idx[1]);
1810                         if (ni > 0)
1811                         {
1812                                 appendLink(i);
1813                                 Link* pft[] = {&m_links[i],
1814                                                            &m_links[m_links.size() - 1]};
1815                                 pft[0]->m_n[0] = &m_nodes[idx[0]];
1816                                 pft[0]->m_n[1] = &m_nodes[ni];
1817                                 pft[1]->m_n[0] = &m_nodes[ni];
1818                                 pft[1]->m_n[1] = &m_nodes[idx[1]];
1819                         }
1820                 }
1821         }
1822         /* Refine faces         */
1823         for (i = 0; i < m_faces.size(); ++i)
1824         {
1825                 const Face& feat = m_faces[i];
1826                 const int idx[] = {int(feat.m_n[0] - nbase),
1827                                                    int(feat.m_n[1] - nbase),
1828                                                    int(feat.m_n[2] - nbase)};
1829                 for (j = 2, k = 0; k < 3; j = k++)
1830                 {
1831                         if ((idx[j] < ncount) && (idx[k] < ncount))
1832                         {
1833                                 const int ni = edges(idx[j], idx[k]);
1834                                 if (ni > 0)
1835                                 {
1836                                         appendFace(i);
1837                                         const int l = (k + 1) % 3;
1838                                         Face* pft[] = {&m_faces[i],
1839                                                                    &m_faces[m_faces.size() - 1]};
1840                                         pft[0]->m_n[0] = &m_nodes[idx[l]];
1841                                         pft[0]->m_n[1] = &m_nodes[idx[j]];
1842                                         pft[0]->m_n[2] = &m_nodes[ni];
1843                                         pft[1]->m_n[0] = &m_nodes[ni];
1844                                         pft[1]->m_n[1] = &m_nodes[idx[k]];
1845                                         pft[1]->m_n[2] = &m_nodes[idx[l]];
1846                                         appendLink(ni, idx[l], pft[0]->m_material);
1847                                         --i;
1848                                         break;
1849                                 }
1850                         }
1851                 }
1852         }
1853         /* Cut                          */
1854         if (cut)
1855         {
1856                 btAlignedObjectArray<int> cnodes;
1857                 const int pcount = ncount;
1858                 int i;
1859                 ncount = m_nodes.size();
1860                 cnodes.resize(ncount, 0);
1861                 /* Nodes                */
1862                 for (i = 0; i < ncount; ++i)
1863                 {
1864                         const btVector3 x = m_nodes[i].m_x;
1865                         if ((i >= pcount) || (btFabs(ifn->Eval(x)) < accurary))
1866                         {
1867                                 const btVector3 v = m_nodes[i].m_v;
1868                                 btScalar m = getMass(i);
1869                                 if (m > 0)
1870                                 {
1871                                         m *= 0.5f;
1872                                         m_nodes[i].m_im /= 0.5f;
1873                                 }
1874                                 appendNode(x, m);
1875                                 cnodes[i] = m_nodes.size() - 1;
1876                                 m_nodes[cnodes[i]].m_v = v;
1877                         }
1878                 }
1879                 nbase = &m_nodes[0];
1880                 /* Links                */
1881                 for (i = 0, ni = m_links.size(); i < ni; ++i)
1882                 {
1883                         const int id[] = {int(m_links[i].m_n[0] - nbase),
1884                                                           int(m_links[i].m_n[1] - nbase)};
1885                         int todetach = 0;
1886                         if (cnodes[id[0]] && cnodes[id[1]])
1887                         {
1888                                 appendLink(i);
1889                                 todetach = m_links.size() - 1;
1890                         }
1891                         else
1892                         {
1893                                 if (((ifn->Eval(m_nodes[id[0]].m_x) < accurary) &&
1894                                          (ifn->Eval(m_nodes[id[1]].m_x) < accurary)))
1895                                         todetach = i;
1896                         }
1897                         if (todetach)
1898                         {
1899                                 Link& l = m_links[todetach];
1900                                 for (int j = 0; j < 2; ++j)
1901                                 {
1902                                         int cn = cnodes[int(l.m_n[j] - nbase)];
1903                                         if (cn) l.m_n[j] = &m_nodes[cn];
1904                                 }
1905                         }
1906                 }
1907                 /* Faces                */
1908                 for (i = 0, ni = m_faces.size(); i < ni; ++i)
1909                 {
1910                         Node** n = m_faces[i].m_n;
1911                         if ((ifn->Eval(n[0]->m_x) < accurary) &&
1912                                 (ifn->Eval(n[1]->m_x) < accurary) &&
1913                                 (ifn->Eval(n[2]->m_x) < accurary))
1914                         {
1915                                 for (int j = 0; j < 3; ++j)
1916                                 {
1917                                         int cn = cnodes[int(n[j] - nbase)];
1918                                         if (cn) n[j] = &m_nodes[cn];
1919                                 }
1920                         }
1921                 }
1922                 /* Clean orphans        */
1923                 int nnodes = m_nodes.size();
1924                 btAlignedObjectArray<int> ranks;
1925                 btAlignedObjectArray<int> todelete;
1926                 ranks.resize(nnodes, 0);
1927                 for (i = 0, ni = m_links.size(); i < ni; ++i)
1928                 {
1929                         for (int j = 0; j < 2; ++j) ranks[int(m_links[i].m_n[j] - nbase)]++;
1930                 }
1931                 for (i = 0, ni = m_faces.size(); i < ni; ++i)
1932                 {
1933                         for (int j = 0; j < 3; ++j) ranks[int(m_faces[i].m_n[j] - nbase)]++;
1934                 }
1935                 for (i = 0; i < m_links.size(); ++i)
1936                 {
1937                         const int id[] = {int(m_links[i].m_n[0] - nbase),
1938                                                           int(m_links[i].m_n[1] - nbase)};
1939                         const bool sg[] = {ranks[id[0]] == 1,
1940                                                            ranks[id[1]] == 1};
1941                         if (sg[0] || sg[1])
1942                         {
1943                                 --ranks[id[0]];
1944                                 --ranks[id[1]];
1945                                 btSwap(m_links[i], m_links[m_links.size() - 1]);
1946                                 m_links.pop_back();
1947                                 --i;
1948                         }
1949                 }
1950 #if 0
1951                 for(i=nnodes-1;i>=0;--i)
1952                 {
1953                         if(!ranks[i]) todelete.push_back(i);
1954                 }
1955                 if(todelete.size())
1956                 {
1957                         btAlignedObjectArray<int>&      map=ranks;
1958                         for(int i=0;i<nnodes;++i) map[i]=i;
1959                         PointersToIndices(this);
1960                         for(int i=0,ni=todelete.size();i<ni;++i)
1961                         {
1962                                 int             j=todelete[i];
1963                                 int&    a=map[j];
1964                                 int&    b=map[--nnodes];
1965                                 m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
1966                                 btSwap(m_nodes[a],m_nodes[b]);
1967                                 j=a;a=b;b=j;
1968                         }
1969                         IndicesToPointers(this,&map[0]);
1970                         m_nodes.resize(nnodes);
1971                 }
1972 #endif
1973         }
1974         m_bUpdateRtCst = true;
1975 }
1976
1977 //
1978 bool btSoftBody::cutLink(const Node* node0, const Node* node1, btScalar position)
1979 {
1980         return (cutLink(int(node0 - &m_nodes[0]), int(node1 - &m_nodes[0]), position));
1981 }
1982
1983 //
1984 bool btSoftBody::cutLink(int node0, int node1, btScalar position)
1985 {
1986         bool done = false;
1987         int i, ni;
1988         //      const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x;
1989         const btVector3 x = Lerp(m_nodes[node0].m_x, m_nodes[node1].m_x, position);
1990         const btVector3 v = Lerp(m_nodes[node0].m_v, m_nodes[node1].m_v, position);
1991         const btScalar m = 1;
1992         appendNode(x, m);
1993         appendNode(x, m);
1994         Node* pa = &m_nodes[node0];
1995         Node* pb = &m_nodes[node1];
1996         Node* pn[2] = {&m_nodes[m_nodes.size() - 2],
1997                                    &m_nodes[m_nodes.size() - 1]};
1998         pn[0]->m_v = v;
1999         pn[1]->m_v = v;
2000         for (i = 0, ni = m_links.size(); i < ni; ++i)
2001         {
2002                 const int mtch = MatchEdge(m_links[i].m_n[0], m_links[i].m_n[1], pa, pb);
2003                 if (mtch != -1)
2004                 {
2005                         appendLink(i);
2006                         Link* pft[] = {&m_links[i], &m_links[m_links.size() - 1]};
2007                         pft[0]->m_n[1] = pn[mtch];
2008                         pft[1]->m_n[0] = pn[1 - mtch];
2009                         done = true;
2010                 }
2011         }
2012         for (i = 0, ni = m_faces.size(); i < ni; ++i)
2013         {
2014                 for (int k = 2, l = 0; l < 3; k = l++)
2015                 {
2016                         const int mtch = MatchEdge(m_faces[i].m_n[k], m_faces[i].m_n[l], pa, pb);
2017                         if (mtch != -1)
2018                         {
2019                                 appendFace(i);
2020                                 Face* pft[] = {&m_faces[i], &m_faces[m_faces.size() - 1]};
2021                                 pft[0]->m_n[l] = pn[mtch];
2022                                 pft[1]->m_n[k] = pn[1 - mtch];
2023                                 appendLink(pn[0], pft[0]->m_n[(l + 1) % 3], pft[0]->m_material, true);
2024                                 appendLink(pn[1], pft[0]->m_n[(l + 1) % 3], pft[0]->m_material, true);
2025                         }
2026                 }
2027         }
2028         if (!done)
2029         {
2030                 m_ndbvt.remove(pn[0]->m_leaf);
2031                 m_ndbvt.remove(pn[1]->m_leaf);
2032                 m_nodes.pop_back();
2033                 m_nodes.pop_back();
2034         }
2035         return (done);
2036 }
2037
2038 //
2039 bool btSoftBody::rayTest(const btVector3& rayFrom,
2040                                                  const btVector3& rayTo,
2041                                                  sRayCast& results)
2042 {
2043         if (m_faces.size() && m_fdbvt.empty())
2044                 initializeFaceTree();
2045
2046         results.body = this;
2047         results.fraction = 1.f;
2048         results.feature = eFeature::None;
2049         results.index = -1;
2050
2051         return (rayTest(rayFrom, rayTo, results.fraction, results.feature, results.index, false) != 0);
2052 }
2053
2054 bool btSoftBody::rayFaceTest(const btVector3& rayFrom,
2055                                                          const btVector3& rayTo,
2056                                                          sRayCast& results)
2057 {
2058         if (m_faces.size() == 0)
2059                 return false;
2060         else
2061         {
2062                 if (m_fdbvt.empty())
2063                         initializeFaceTree();
2064         }
2065
2066         results.body = this;
2067         results.fraction = 1.f;
2068         results.index = -1;
2069
2070         return (rayFaceTest(rayFrom, rayTo, results.fraction, results.index) != 0);
2071 }
2072
2073 //
2074 void btSoftBody::setSolver(eSolverPresets::_ preset)
2075 {
2076         m_cfg.m_vsequence.clear();
2077         m_cfg.m_psequence.clear();
2078         m_cfg.m_dsequence.clear();
2079         switch (preset)
2080         {
2081                 case eSolverPresets::Positions:
2082                         m_cfg.m_psequence.push_back(ePSolver::Anchors);
2083                         m_cfg.m_psequence.push_back(ePSolver::RContacts);
2084                         m_cfg.m_psequence.push_back(ePSolver::SContacts);
2085                         m_cfg.m_psequence.push_back(ePSolver::Linear);
2086                         break;
2087                 case eSolverPresets::Velocities:
2088                         m_cfg.m_vsequence.push_back(eVSolver::Linear);
2089
2090                         m_cfg.m_psequence.push_back(ePSolver::Anchors);
2091                         m_cfg.m_psequence.push_back(ePSolver::RContacts);
2092                         m_cfg.m_psequence.push_back(ePSolver::SContacts);
2093
2094                         m_cfg.m_dsequence.push_back(ePSolver::Linear);
2095                         break;
2096         }
2097 }
2098
2099 void btSoftBody::predictMotion(btScalar dt)
2100 {
2101         int i, ni;
2102
2103         /* Update                */
2104         if (m_bUpdateRtCst)
2105         {
2106                 m_bUpdateRtCst = false;
2107                 updateConstants();
2108                 m_fdbvt.clear();
2109                 if (m_cfg.collisions & fCollision::VF_SS)
2110                 {
2111                         initializeFaceTree();
2112                 }
2113         }
2114
2115         /* Prepare                */
2116         m_sst.sdt = dt * m_cfg.timescale;
2117         m_sst.isdt = 1 / m_sst.sdt;
2118         m_sst.velmrg = m_sst.sdt * 3;
2119         m_sst.radmrg = getCollisionShape()->getMargin();
2120         m_sst.updmrg = m_sst.radmrg * (btScalar)0.25;
2121         /* Forces                */
2122         addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
2123         applyForces();
2124         /* Integrate            */
2125         for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2126         {
2127                 Node& n = m_nodes[i];
2128                 n.m_q = n.m_x;
2129                 btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt;
2130                 {
2131                         btScalar maxDisplacement = m_worldInfo->m_maxDisplacement;
2132                         btScalar clampDeltaV = maxDisplacement / m_sst.sdt;
2133                         for (int c = 0; c < 3; c++)
2134                         {
2135                                 if (deltaV[c] > clampDeltaV)
2136                                 {
2137                                         deltaV[c] = clampDeltaV;
2138                                 }
2139                                 if (deltaV[c] < -clampDeltaV)
2140                                 {
2141                                         deltaV[c] = -clampDeltaV;
2142                                 }
2143                         }
2144                 }
2145                 n.m_v += deltaV;
2146                 n.m_x += n.m_v * m_sst.sdt;
2147                 n.m_f = btVector3(0, 0, 0);
2148         }
2149         /* Clusters                */
2150         updateClusters();
2151         /* Bounds                */
2152         updateBounds();
2153         /* Nodes                */
2154         ATTRIBUTE_ALIGNED16(btDbvtVolume)
2155         vol;
2156         for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2157         {
2158                 Node& n = m_nodes[i];
2159                 vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg);
2160                 m_ndbvt.update(n.m_leaf,
2161                                            vol,
2162                                            n.m_v * m_sst.velmrg,
2163                                            m_sst.updmrg);
2164         }
2165         /* Faces                */
2166         if (!m_fdbvt.empty())
2167         {
2168                 for (int i = 0; i < m_faces.size(); ++i)
2169                 {
2170                         Face& f = m_faces[i];
2171                         const btVector3 v = (f.m_n[0]->m_v +
2172                                                                  f.m_n[1]->m_v +
2173                                                                  f.m_n[2]->m_v) /
2174                                                                 3;
2175                         vol = VolumeOf(f, m_sst.radmrg);
2176                         m_fdbvt.update(f.m_leaf,
2177                                                    vol,
2178                                                    v * m_sst.velmrg,
2179                                                    m_sst.updmrg);
2180                 }
2181         }
2182         /* Pose                    */
2183         updatePose();
2184         /* Match                */
2185         if (m_pose.m_bframe && (m_cfg.kMT > 0))
2186         {
2187                 const btMatrix3x3 posetrs = m_pose.m_rot;
2188                 for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
2189                 {
2190                         Node& n = m_nodes[i];
2191                         if (n.m_im > 0)
2192                         {
2193                                 const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com;
2194                                 n.m_x = Lerp(n.m_x, x, m_cfg.kMT);
2195                         }
2196                 }
2197         }
2198         /* Clear contacts        */
2199         m_rcontacts.resize(0);
2200         m_scontacts.resize(0);
2201         /* Optimize dbvt's        */
2202         m_ndbvt.optimizeIncremental(1);
2203         m_fdbvt.optimizeIncremental(1);
2204         m_cdbvt.optimizeIncremental(1);
2205 }
2206
2207 //
2208 void btSoftBody::solveConstraints()
2209 {
2210         /* Apply clusters               */
2211         applyClusters(false);
2212         /* Prepare links                */
2213
2214         int i, ni;
2215
2216         for (i = 0, ni = m_links.size(); i < ni; ++i)
2217         {
2218                 Link& l = m_links[i];
2219                 l.m_c3 = l.m_n[1]->m_q - l.m_n[0]->m_q;
2220                 l.m_c2 = 1 / (l.m_c3.length2() * l.m_c0);
2221         }
2222         /* Prepare anchors              */
2223         for (i = 0, ni = m_anchors.size(); i < ni; ++i)
2224         {
2225                 Anchor& a = m_anchors[i];
2226                 const btVector3 ra = a.m_body->getWorldTransform().getBasis() * a.m_local;
2227                 a.m_c0 = ImpulseMatrix(m_sst.sdt,
2228                                                            a.m_node->m_im,
2229                                                            a.m_body->getInvMass(),
2230                                                            a.m_body->getInvInertiaTensorWorld(),
2231                                                            ra);
2232                 a.m_c1 = ra;
2233                 a.m_c2 = m_sst.sdt * a.m_node->m_im;
2234                 a.m_body->activate();
2235         }
2236         /* Solve velocities             */
2237         if (m_cfg.viterations > 0)
2238         {
2239                 /* Solve                        */
2240                 for (int isolve = 0; isolve < m_cfg.viterations; ++isolve)
2241                 {
2242                         for (int iseq = 0; iseq < m_cfg.m_vsequence.size(); ++iseq)
2243                         {
2244                                 getSolver(m_cfg.m_vsequence[iseq])(this, 1);
2245                         }
2246                 }
2247                 /* Update                       */
2248                 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2249                 {
2250                         Node& n = m_nodes[i];
2251                         n.m_x = n.m_q + n.m_v * m_sst.sdt;
2252                 }
2253         }
2254         /* Solve positions              */
2255         if (m_cfg.piterations > 0)
2256         {
2257                 for (int isolve = 0; isolve < m_cfg.piterations; ++isolve)
2258                 {
2259                         const btScalar ti = isolve / (btScalar)m_cfg.piterations;
2260                         for (int iseq = 0; iseq < m_cfg.m_psequence.size(); ++iseq)
2261                         {
2262                                 getSolver(m_cfg.m_psequence[iseq])(this, 1, ti);
2263                         }
2264                 }
2265                 const btScalar vc = m_sst.isdt * (1 - m_cfg.kDP);
2266                 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2267                 {
2268                         Node& n = m_nodes[i];
2269                         n.m_v = (n.m_x - n.m_q) * vc;
2270                         n.m_f = btVector3(0, 0, 0);
2271                 }
2272         }
2273         /* Solve drift                  */
2274         if (m_cfg.diterations > 0)
2275         {
2276                 const btScalar vcf = m_cfg.kVCF * m_sst.isdt;
2277                 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2278                 {
2279                         Node& n = m_nodes[i];
2280                         n.m_q = n.m_x;
2281                 }
2282                 for (int idrift = 0; idrift < m_cfg.diterations; ++idrift)
2283                 {
2284                         for (int iseq = 0; iseq < m_cfg.m_dsequence.size(); ++iseq)
2285                         {
2286                                 getSolver(m_cfg.m_dsequence[iseq])(this, 1, 0);
2287                         }
2288                 }
2289                 for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
2290                 {
2291                         Node& n = m_nodes[i];
2292                         n.m_v += (n.m_x - n.m_q) * vcf;
2293                 }
2294         }
2295         /* Apply clusters               */
2296         dampClusters();
2297         applyClusters(true);
2298 }
2299
2300 //
2301 void btSoftBody::staticSolve(int iterations)
2302 {
2303         for (int isolve = 0; isolve < iterations; ++isolve)
2304         {
2305                 for (int iseq = 0; iseq < m_cfg.m_psequence.size(); ++iseq)
2306                 {
2307                         getSolver(m_cfg.m_psequence[iseq])(this, 1, 0);
2308                 }
2309         }
2310 }
2311
2312 //
2313 void btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/, int /*count*/, int /*iterations*/)
2314 {
2315         /// placeholder
2316 }
2317
2318 //
2319 void btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
2320 {
2321         const int nb = bodies.size();
2322         int iterations = 0;
2323         int i;
2324
2325         for (i = 0; i < nb; ++i)
2326         {
2327                 iterations = btMax(iterations, bodies[i]->m_cfg.citerations);
2328         }
2329         for (i = 0; i < nb; ++i)
2330         {
2331                 bodies[i]->prepareClusters(iterations);
2332         }
2333         for (i = 0; i < iterations; ++i)
2334         {
2335                 const btScalar sor = 1;
2336                 for (int j = 0; j < nb; ++j)
2337                 {
2338                         bodies[j]->solveClusters(sor);
2339                 }
2340         }
2341         for (i = 0; i < nb; ++i)
2342         {
2343                 bodies[i]->cleanupClusters();
2344         }
2345 }
2346
2347 //
2348 void btSoftBody::integrateMotion()
2349 {
2350         /* Update                       */
2351         updateNormals();
2352 }
2353
2354 //
2355 btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom, const btVector3& rayTo, btScalar mxt)
2356 {
2357         m_rayFrom = rayFrom;
2358         m_rayNormalizedDirection = (rayTo - rayFrom);
2359         m_rayTo = rayTo;
2360         m_mint = mxt;
2361         m_face = 0;
2362         m_tests = 0;
2363 }
2364
2365 //
2366 void btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf)
2367 {
2368         btSoftBody::Face& f = *(btSoftBody::Face*)leaf->data;
2369         const btScalar t = rayFromToTriangle(m_rayFrom, m_rayTo, m_rayNormalizedDirection,
2370                                                                                  f.m_n[0]->m_x,
2371                                                                                  f.m_n[1]->m_x,
2372                                                                                  f.m_n[2]->m_x,
2373                                                                                  m_mint);
2374         if ((t > 0) && (t < m_mint))
2375         {
2376                 m_mint = t;
2377                 m_face = &f;
2378         }
2379         ++m_tests;
2380 }
2381
2382 //
2383 btScalar btSoftBody::RayFromToCaster::rayFromToTriangle(const btVector3& rayFrom,
2384                                                                                                                 const btVector3& rayTo,
2385                                                                                                                 const btVector3& rayNormalizedDirection,
2386                                                                                                                 const btVector3& a,
2387                                                                                                                 const btVector3& b,
2388                                                                                                                 const btVector3& c,
2389                                                                                                                 btScalar maxt)
2390 {
2391         static const btScalar ceps = -SIMD_EPSILON * 10;
2392         static const btScalar teps = SIMD_EPSILON * 10;
2393
2394         const btVector3 n = btCross(b - a, c - a);
2395         const btScalar d = btDot(a, n);
2396         const btScalar den = btDot(rayNormalizedDirection, n);
2397         if (!btFuzzyZero(den))
2398         {
2399                 const btScalar num = btDot(rayFrom, n) - d;
2400                 const btScalar t = -num / den;
2401                 if ((t > teps) && (t < maxt))
2402                 {
2403                         const btVector3 hit = rayFrom + rayNormalizedDirection * t;
2404                         if ((btDot(n, btCross(a - hit, b - hit)) > ceps) &&
2405                                 (btDot(n, btCross(b - hit, c - hit)) > ceps) &&
2406                                 (btDot(n, btCross(c - hit, a - hit)) > ceps))
2407                         {
2408                                 return (t);
2409                         }
2410                 }
2411         }
2412         return (-1);
2413 }
2414
2415 //
2416 void btSoftBody::pointersToIndices()
2417 {
2418 #define PTR2IDX(_p_, _b_) reinterpret_cast<btSoftBody::Node*>((_p_) - (_b_))
2419         btSoftBody::Node* base = m_nodes.size() ? &m_nodes[0] : 0;
2420         int i, ni;
2421
2422         for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2423         {
2424                 if (m_nodes[i].m_leaf)
2425                 {
2426                         m_nodes[i].m_leaf->data = *(void**)&i;
2427                 }
2428         }
2429         for (i = 0, ni = m_links.size(); i < ni; ++i)
2430         {
2431                 m_links[i].m_n[0] = PTR2IDX(m_links[i].m_n[0], base);
2432                 m_links[i].m_n[1] = PTR2IDX(m_links[i].m_n[1], base);
2433         }
2434         for (i = 0, ni = m_faces.size(); i < ni; ++i)
2435         {
2436                 m_faces[i].m_n[0] = PTR2IDX(m_faces[i].m_n[0], base);
2437                 m_faces[i].m_n[1] = PTR2IDX(m_faces[i].m_n[1], base);
2438                 m_faces[i].m_n[2] = PTR2IDX(m_faces[i].m_n[2], base);
2439                 if (m_faces[i].m_leaf)
2440                 {
2441                         m_faces[i].m_leaf->data = *(void**)&i;
2442                 }
2443         }
2444         for (i = 0, ni = m_anchors.size(); i < ni; ++i)
2445         {
2446                 m_anchors[i].m_node = PTR2IDX(m_anchors[i].m_node, base);
2447         }
2448         for (i = 0, ni = m_notes.size(); i < ni; ++i)
2449         {
2450                 for (int j = 0; j < m_notes[i].m_rank; ++j)
2451                 {
2452                         m_notes[i].m_nodes[j] = PTR2IDX(m_notes[i].m_nodes[j], base);
2453                 }
2454         }
2455 #undef PTR2IDX
2456 }
2457
2458 //
2459 void btSoftBody::indicesToPointers(const int* map)
2460 {
2461 #define IDX2PTR(_p_, _b_) map ? (&(_b_)[map[(((char*)_p_) - (char*)0)]]) : (&(_b_)[(((char*)_p_) - (char*)0)])
2462         btSoftBody::Node* base = m_nodes.size() ? &m_nodes[0] : 0;
2463         int i, ni;
2464
2465         for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2466         {
2467                 if (m_nodes[i].m_leaf)
2468                 {
2469                         m_nodes[i].m_leaf->data = &m_nodes[i];
2470                 }
2471         }
2472         for (i = 0, ni = m_links.size(); i < ni; ++i)
2473         {
2474                 m_links[i].m_n[0] = IDX2PTR(m_links[i].m_n[0], base);
2475                 m_links[i].m_n[1] = IDX2PTR(m_links[i].m_n[1], base);
2476         }
2477         for (i = 0, ni = m_faces.size(); i < ni; ++i)
2478         {
2479                 m_faces[i].m_n[0] = IDX2PTR(m_faces[i].m_n[0], base);
2480                 m_faces[i].m_n[1] = IDX2PTR(m_faces[i].m_n[1], base);
2481                 m_faces[i].m_n[2] = IDX2PTR(m_faces[i].m_n[2], base);
2482                 if (m_faces[i].m_leaf)
2483                 {
2484                         m_faces[i].m_leaf->data = &m_faces[i];
2485                 }
2486         }
2487         for (i = 0, ni = m_anchors.size(); i < ni; ++i)
2488         {
2489                 m_anchors[i].m_node = IDX2PTR(m_anchors[i].m_node, base);
2490         }
2491         for (i = 0, ni = m_notes.size(); i < ni; ++i)
2492         {
2493                 for (int j = 0; j < m_notes[i].m_rank; ++j)
2494                 {
2495                         m_notes[i].m_nodes[j] = IDX2PTR(m_notes[i].m_nodes[j], base);
2496                 }
2497         }
2498 #undef IDX2PTR
2499 }
2500
2501 //
2502 int btSoftBody::rayTest(const btVector3& rayFrom, const btVector3& rayTo,
2503                                                 btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const
2504 {
2505         int cnt = 0;
2506         btVector3 dir = rayTo - rayFrom;
2507
2508         if (bcountonly || m_fdbvt.empty())
2509         { /* Full search        */
2510
2511                 for (int i = 0, ni = m_faces.size(); i < ni; ++i)
2512                 {
2513                         const btSoftBody::Face& f = m_faces[i];
2514
2515                         const btScalar t = RayFromToCaster::rayFromToTriangle(rayFrom, rayTo, dir,
2516                                                                                                                                   f.m_n[0]->m_x,
2517                                                                                                                                   f.m_n[1]->m_x,
2518                                                                                                                                   f.m_n[2]->m_x,
2519                                                                                                                                   mint);
2520                         if (t > 0)
2521                         {
2522                                 ++cnt;
2523                                 if (!bcountonly)
2524                                 {
2525                                         feature = btSoftBody::eFeature::Face;
2526                                         index = i;
2527                                         mint = t;
2528                                 }
2529                         }
2530                 }
2531         }
2532         else
2533         { /* Use dbvt   */
2534                 RayFromToCaster collider(rayFrom, rayTo, mint);
2535
2536                 btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider);
2537                 if (collider.m_face)
2538                 {
2539                         mint = collider.m_mint;
2540                         feature = btSoftBody::eFeature::Face;
2541                         index = (int)(collider.m_face - &m_faces[0]);
2542                         cnt = 1;
2543                 }
2544         }
2545
2546         for (int i = 0; i < m_tetras.size(); i++)
2547         {
2548                 const btSoftBody::Tetra& tet = m_tetras[i];
2549                 int tetfaces[4][3] = {{0, 1, 2}, {0, 1, 3}, {1, 2, 3}, {0, 2, 3}};
2550                 for (int f = 0; f < 4; f++)
2551                 {
2552                         int index0 = tetfaces[f][0];
2553                         int index1 = tetfaces[f][1];
2554                         int index2 = tetfaces[f][2];
2555                         btVector3 v0 = tet.m_n[index0]->m_x;
2556                         btVector3 v1 = tet.m_n[index1]->m_x;
2557                         btVector3 v2 = tet.m_n[index2]->m_x;
2558
2559                         const btScalar t = RayFromToCaster::rayFromToTriangle(rayFrom, rayTo, dir,
2560                                                                                                                                   v0, v1, v2,
2561                                                                                                                                   mint);
2562                         if (t > 0)
2563                         {
2564                                 ++cnt;
2565                                 if (!bcountonly)
2566                                 {
2567                                         feature = btSoftBody::eFeature::Tetra;
2568                                         index = i;
2569                                         mint = t;
2570                                 }
2571                         }
2572                 }
2573         }
2574         return (cnt);
2575 }
2576
2577 int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo,
2578                                                         btScalar& mint, int& index) const
2579 {
2580         int cnt = 0;
2581         { /* Use dbvt   */
2582                 RayFromToCaster collider(rayFrom, rayTo, mint);
2583
2584                 btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider);
2585                 if (collider.m_face)
2586                 {
2587                         mint = collider.m_mint;
2588                         index = (int)(collider.m_face - &m_faces[0]);
2589                         cnt = 1;
2590                 }
2591         }
2592         return (cnt);
2593 }
2594
2595 //
2596 static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n)
2597 {
2598         if (n == 0)
2599                 return 0;
2600         btDbvntNode* root = new btDbvntNode(n);
2601         if (n->isinternal())
2602         {
2603                 btDbvntNode* c0 = copyToDbvnt(n->childs[0]);
2604                 root->childs[0] = c0;
2605                 btDbvntNode* c1 = copyToDbvnt(n->childs[1]);
2606                 root->childs[1] = c1;
2607         }
2608         return root;
2609 }
2610
2611 static inline void calculateNormalCone(btDbvntNode* root)
2612 {
2613         if (!root)
2614                 return;
2615         if (root->isleaf())
2616         {
2617                 const btSoftBody::Face* face = (btSoftBody::Face*)root->data;
2618                 root->normal = face->m_normal;
2619                 root->angle = 0;
2620         }
2621         else
2622         {
2623                 btVector3 n0(0, 0, 0), n1(0, 0, 0);
2624                 btScalar a0 = 0, a1 = 0;
2625                 if (root->childs[0])
2626                 {
2627                         calculateNormalCone(root->childs[0]);
2628                         n0 = root->childs[0]->normal;
2629                         a0 = root->childs[0]->angle;
2630                 }
2631                 if (root->childs[1])
2632                 {
2633                         calculateNormalCone(root->childs[1]);
2634                         n1 = root->childs[1]->normal;
2635                         a1 = root->childs[1]->angle;
2636                 }
2637                 root->normal = (n0 + n1).safeNormalize();
2638                 root->angle = btMax(a0, a1) + btAngle(n0, n1) * 0.5;
2639         }
2640 }
2641
2642 void btSoftBody::initializeFaceTree()
2643 {
2644         BT_PROFILE("btSoftBody::initializeFaceTree");
2645         m_fdbvt.clear();
2646         // create leaf nodes;
2647         btAlignedObjectArray<btDbvtNode*> leafNodes;
2648         leafNodes.resize(m_faces.size());
2649         for (int i = 0; i < m_faces.size(); ++i)
2650         {
2651                 Face& f = m_faces[i];
2652                 ATTRIBUTE_ALIGNED16(btDbvtVolume)
2653                 vol = VolumeOf(f, 0);
2654                 btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
2655                 node->parent = NULL;
2656                 node->data = &f;
2657                 node->childs[1] = 0;
2658                 node->volume = vol;
2659                 leafNodes[i] = node;
2660                 f.m_leaf = node;
2661         }
2662         btAlignedObjectArray<btAlignedObjectArray<int> > adj;
2663         adj.resize(m_faces.size());
2664         // construct the adjacency list for triangles
2665         for (int i = 0; i < adj.size(); ++i)
2666         {
2667                 for (int j = i + 1; j < adj.size(); ++j)
2668                 {
2669                         int dup = 0;
2670                         for (int k = 0; k < 3; ++k)
2671                         {
2672                                 for (int l = 0; l < 3; ++l)
2673                                 {
2674                                         if (m_faces[i].m_n[k] == m_faces[j].m_n[l])
2675                                         {
2676                                                 ++dup;
2677                                                 break;
2678                                         }
2679                                 }
2680                                 if (dup == 2)
2681                                 {
2682                                         adj[i].push_back(j);
2683                                         adj[j].push_back(i);
2684                                 }
2685                         }
2686                 }
2687         }
2688         m_fdbvt.m_root = buildTreeBottomUp(leafNodes, adj);
2689         if (m_fdbvnt)
2690                 delete m_fdbvnt;
2691         m_fdbvnt = copyToDbvnt(m_fdbvt.m_root);
2692         updateFaceTree(false, false);
2693         rebuildNodeTree();
2694 }
2695
2696 //
2697 void btSoftBody::rebuildNodeTree()
2698 {
2699         m_ndbvt.clear();
2700         btAlignedObjectArray<btDbvtNode*> leafNodes;
2701         leafNodes.resize(m_nodes.size());
2702         for (int i = 0; i < m_nodes.size(); ++i)
2703         {
2704                 Node& n = m_nodes[i];
2705                 ATTRIBUTE_ALIGNED16(btDbvtVolume)
2706                 vol = btDbvtVolume::FromCR(n.m_x, 0);
2707                 btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
2708                 node->parent = NULL;
2709                 node->data = &n;
2710                 node->childs[1] = 0;
2711                 node->volume = vol;
2712                 leafNodes[i] = node;
2713                 n.m_leaf = node;
2714         }
2715         btAlignedObjectArray<btAlignedObjectArray<int> > adj;
2716         adj.resize(m_nodes.size());
2717         btAlignedObjectArray<int> old_id;
2718         old_id.resize(m_nodes.size());
2719         for (int i = 0; i < m_nodes.size(); ++i)
2720                 old_id[i] = m_nodes[i].index;
2721         for (int i = 0; i < m_nodes.size(); ++i)
2722                 m_nodes[i].index = i;
2723         for (int i = 0; i < m_links.size(); ++i)
2724         {
2725                 Link& l = m_links[i];
2726                 adj[l.m_n[0]->index].push_back(l.m_n[1]->index);
2727                 adj[l.m_n[1]->index].push_back(l.m_n[0]->index);
2728         }
2729         m_ndbvt.m_root = buildTreeBottomUp(leafNodes, adj);
2730         for (int i = 0; i < m_nodes.size(); ++i)
2731                 m_nodes[i].index = old_id[i];
2732 }
2733
2734 //
2735 btVector3 btSoftBody::evaluateCom() const
2736 {
2737         btVector3 com(0, 0, 0);
2738         if (m_pose.m_bframe)
2739         {
2740                 for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
2741                 {
2742                         com += m_nodes[i].m_x * m_pose.m_wgh[i];
2743                 }
2744         }
2745         return (com);
2746 }
2747
2748 bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
2749                                                           const btVector3& x,
2750                                                           btScalar margin,
2751                                                           btSoftBody::sCti& cti) const
2752 {
2753         btVector3 nrm;
2754         const btCollisionShape* shp = colObjWrap->getCollisionShape();
2755         //    const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
2756         //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
2757         const btTransform& wtr = colObjWrap->getWorldTransform();
2758         //todo: check which transform is needed here
2759
2760         btScalar dst =
2761                 m_worldInfo->m_sparsesdf.Evaluate(
2762                         wtr.invXform(x),
2763                         shp,
2764                         nrm,
2765                         margin);
2766         if (dst < 0)
2767         {
2768                 cti.m_colObj = colObjWrap->getCollisionObject();
2769                 cti.m_normal = wtr.getBasis() * nrm;
2770                 cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst);
2771                 return (true);
2772         }
2773         return (false);
2774 }
2775
2776 //
2777 bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap,
2778                                                                                 const btVector3& x,
2779                                                                                 btScalar margin,
2780                                                                                 btSoftBody::sCti& cti, bool predict) const
2781 {
2782         btVector3 nrm;
2783         const btCollisionShape* shp = colObjWrap->getCollisionShape();
2784         const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
2785         // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
2786         // but resolve contact at x_n
2787         btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
2788                                                                 : colObjWrap->getWorldTransform();
2789         btScalar dst =
2790                 m_worldInfo->m_sparsesdf.Evaluate(
2791                         wtr.invXform(x),
2792                         shp,
2793                         nrm,
2794                         margin);
2795
2796         if (!predict)
2797         {
2798                 cti.m_colObj = colObjWrap->getCollisionObject();
2799                 cti.m_normal = wtr.getBasis() * nrm;
2800                 cti.m_offset = dst;
2801         }
2802         if (dst < 0)
2803                 return true;
2804         return (false);
2805 }
2806
2807 //
2808 // Compute barycentric coordinates (u, v, w) for
2809 // point p with respect to triangle (a, b, c)
2810 static void getBarycentric(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, btVector3& bary)
2811 {
2812         btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
2813         btScalar d00 = v0.dot(v0);
2814         btScalar d01 = v0.dot(v1);
2815         btScalar d11 = v1.dot(v1);
2816         btScalar d20 = v2.dot(v0);
2817         btScalar d21 = v2.dot(v1);
2818         btScalar denom = d00 * d11 - d01 * d01;
2819         // In the case of a degenerate triangle, pick a vertex.
2820         if (btFabs(denom) < SIMD_EPSILON)
2821         {
2822                 bary.setY(btScalar(0.0));
2823                 bary.setZ(btScalar(0.0));
2824         }
2825         else
2826         {
2827                 bary.setY((d11 * d20 - d01 * d21) / denom);
2828                 bary.setZ((d00 * d21 - d01 * d20) / denom);
2829         }
2830         bary.setX(btScalar(1) - bary.getY() - bary.getZ());
2831 }
2832
2833 //
2834 bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap,
2835                                                                                         Face& f,
2836                                                                                         btVector3& contact_point,
2837                                                                                         btVector3& bary,
2838                                                                                         btScalar margin,
2839                                                                                         btSoftBody::sCti& cti, bool predict) const
2840 {
2841         btVector3 nrm;
2842         const btCollisionShape* shp = colObjWrap->getCollisionShape();
2843         const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
2844         // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
2845         // but resolve contact at x_n
2846         btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
2847                                                                 : colObjWrap->getWorldTransform();
2848         btScalar dst;
2849         btGjkEpaSolver2::sResults results;
2850
2851         //      #define USE_QUADRATURE 1
2852
2853         // use collision quadrature point
2854 #ifdef USE_QUADRATURE
2855         {
2856                 dst = SIMD_INFINITY;
2857                 btVector3 local_nrm;
2858                 for (int q = 0; q < m_quads.size(); ++q)
2859                 {
2860                         btVector3 p;
2861                         if (predict)
2862                                 p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]);
2863                         else
2864                                 p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
2865                         btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate(
2866                                 wtr.invXform(p),
2867                                 shp,
2868                                 local_nrm,
2869                                 margin);
2870                         if (local_dst < dst)
2871                         {
2872                                 if (local_dst < 0 && predict)
2873                                         return true;
2874                                 dst = local_dst;
2875                                 contact_point = p;
2876                                 bary = m_quads[q];
2877                                 nrm = local_nrm;
2878                         }
2879                         if (!predict)
2880                         {
2881                                 cti.m_colObj = colObjWrap->getCollisionObject();
2882                                 cti.m_normal = wtr.getBasis() * nrm;
2883                                 cti.m_offset = dst;
2884                         }
2885                 }
2886                 return (dst < 0);
2887         }
2888 #endif
2889
2890         // collision detection using x*
2891         btTransform triangle_transform;
2892         triangle_transform.setIdentity();
2893         triangle_transform.setOrigin(f.m_n[0]->m_q);
2894         btTriangleShape triangle(btVector3(0, 0, 0), f.m_n[1]->m_q - f.m_n[0]->m_q, f.m_n[2]->m_q - f.m_n[0]->m_q);
2895         btVector3 guess(0, 0, 0);
2896         const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
2897         btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
2898         dst = results.distance - 2.0 * csh->getMargin() - margin;  // margin padding so that the distance = the actual distance between face and rigid - margin of rigid - margin of deformable
2899         if (dst >= 0)
2900                 return false;
2901
2902         // Use consistent barycenter to recalculate distance.
2903         if (this->m_cacheBarycenter)
2904         {
2905                 if (f.m_pcontact[3] != 0)
2906                 {
2907                         for (int i = 0; i < 3; ++i)
2908                                 bary[i] = f.m_pcontact[i];
2909                         contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
2910                         const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
2911                         btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr, results);
2912                         cti.m_colObj = colObjWrap->getCollisionObject();
2913                         dst = results.distance;
2914                         cti.m_normal = results.normal;
2915                         cti.m_offset = dst;
2916
2917                         //point-convex CD
2918                         wtr = colObjWrap->getWorldTransform();
2919                         btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x);
2920                         triangle_transform.setOrigin(f.m_n[0]->m_x);
2921                         btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
2922
2923                         dst = results.distance - csh->getMargin() - margin;
2924                         return true;
2925                 }
2926         }
2927
2928         // Use triangle-convex CD.
2929         wtr = colObjWrap->getWorldTransform();
2930         btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x);
2931         triangle_transform.setOrigin(f.m_n[0]->m_x);
2932         btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
2933         contact_point = results.witnesses[0];
2934         getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
2935
2936         for (int i = 0; i < 3; ++i)
2937                 f.m_pcontact[i] = bary[i];
2938
2939         dst = results.distance - csh->getMargin() - margin;
2940         cti.m_colObj = colObjWrap->getCollisionObject();
2941         cti.m_normal = results.normal;
2942         cti.m_offset = dst;
2943         return true;
2944 }
2945
2946 void btSoftBody::updateNormals()
2947 {
2948         const btVector3 zv(0, 0, 0);
2949         int i, ni;
2950
2951         for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2952         {
2953                 m_nodes[i].m_n = zv;
2954         }
2955         for (i = 0, ni = m_faces.size(); i < ni; ++i)
2956         {
2957                 btSoftBody::Face& f = m_faces[i];
2958                 const btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x,
2959                                                                         f.m_n[2]->m_x - f.m_n[0]->m_x);
2960                 f.m_normal = n;
2961                 f.m_normal.safeNormalize();
2962                 f.m_n[0]->m_n += n;
2963                 f.m_n[1]->m_n += n;
2964                 f.m_n[2]->m_n += n;
2965         }
2966         for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2967         {
2968                 btScalar len = m_nodes[i].m_n.length();
2969                 if (len > SIMD_EPSILON)
2970                         m_nodes[i].m_n /= len;
2971         }
2972 }
2973
2974 //
2975 void btSoftBody::updateBounds()
2976 {
2977         /*if( m_acceleratedSoftBody )
2978         {
2979                 // If we have an accelerated softbody we need to obtain the bounds correctly
2980                 // For now (slightly hackily) just have a very large AABB
2981                 // TODO: Write get bounds kernel
2982                 // If that is updating in place, atomic collisions might be low (when the cloth isn't perfectly aligned to an axis) and we could
2983                 // probably do a test and exchange reasonably efficiently.
2984
2985                 m_bounds[0] = btVector3(-1000, -1000, -1000);
2986                 m_bounds[1] = btVector3(1000, 1000, 1000);
2987
2988         } else {*/
2989         //    if (m_ndbvt.m_root)
2990         //    {
2991         //        const btVector3& mins = m_ndbvt.m_root->volume.Mins();
2992         //        const btVector3& maxs = m_ndbvt.m_root->volume.Maxs();
2993         //        const btScalar csm = getCollisionShape()->getMargin();
2994         //        const btVector3 mrg = btVector3(csm,
2995         //                                        csm,
2996         //                                        csm) *
2997         //                              1;  // ??? to investigate...
2998         //        m_bounds[0] = mins - mrg;
2999         //        m_bounds[1] = maxs + mrg;
3000         //        if (0 != getBroadphaseHandle())
3001         //        {
3002         //            m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(),
3003         //                                               m_bounds[0],
3004         //                                               m_bounds[1],
3005         //                                               m_worldInfo->m_dispatcher);
3006         //        }
3007         //    }
3008         //    else
3009         //    {
3010         //        m_bounds[0] =
3011         //            m_bounds[1] = btVector3(0, 0, 0);
3012         //    }
3013         if (m_nodes.size())
3014         {
3015                 btVector3 mins = m_nodes[0].m_x;
3016                 btVector3 maxs = m_nodes[0].m_x;
3017                 for (int i = 1; i < m_nodes.size(); ++i)
3018                 {
3019                         for (int d = 0; d < 3; ++d)
3020                         {
3021                                 if (m_nodes[i].m_x[d] > maxs[d])
3022                                         maxs[d] = m_nodes[i].m_x[d];
3023                                 if (m_nodes[i].m_x[d] < mins[d])
3024                                         mins[d] = m_nodes[i].m_x[d];
3025                         }
3026                 }
3027                 const btScalar csm = getCollisionShape()->getMargin();
3028                 const btVector3 mrg = btVector3(csm,
3029                                                                                 csm,
3030                                                                                 csm);
3031                 m_bounds[0] = mins - mrg;
3032                 m_bounds[1] = maxs + mrg;
3033                 if (0 != getBroadphaseHandle())
3034                 {
3035                         m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(),
3036                                                                                            m_bounds[0],
3037                                                                                            m_bounds[1],
3038                                                                                            m_worldInfo->m_dispatcher);
3039                 }
3040         }
3041         else
3042         {
3043                 m_bounds[0] =
3044                         m_bounds[1] = btVector3(0, 0, 0);
3045         }
3046 }
3047
3048 //
3049 void btSoftBody::updatePose()
3050 {
3051         if (m_pose.m_bframe)
3052         {
3053                 btSoftBody::Pose& pose = m_pose;
3054                 const btVector3 com = evaluateCom();
3055                 /* Com                  */
3056                 pose.m_com = com;
3057                 /* Rotation             */
3058                 btMatrix3x3 Apq;
3059                 const btScalar eps = SIMD_EPSILON;
3060                 Apq[0] = Apq[1] = Apq[2] = btVector3(0, 0, 0);
3061                 Apq[0].setX(eps);
3062                 Apq[1].setY(eps * 2);
3063                 Apq[2].setZ(eps * 3);
3064                 for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
3065                 {
3066                         const btVector3 a = pose.m_wgh[i] * (m_nodes[i].m_x - com);
3067                         const btVector3& b = pose.m_pos[i];
3068                         Apq[0] += a.x() * b;
3069                         Apq[1] += a.y() * b;
3070                         Apq[2] += a.z() * b;
3071                 }
3072                 btMatrix3x3 r, s;
3073                 PolarDecompose(Apq, r, s);
3074                 pose.m_rot = r;
3075                 pose.m_scl = pose.m_aqq * r.transpose() * Apq;
3076                 if (m_cfg.maxvolume > 1)
3077                 {
3078                         const btScalar idet = Clamp<btScalar>(1 / pose.m_scl.determinant(),
3079                                                                                                   1, m_cfg.maxvolume);
3080                         pose.m_scl = Mul(pose.m_scl, idet);
3081                 }
3082         }
3083 }
3084
3085 //
3086 void btSoftBody::updateArea(bool averageArea)
3087 {
3088         int i, ni;
3089
3090         /* Face area            */
3091         for (i = 0, ni = m_faces.size(); i < ni; ++i)
3092         {
3093                 Face& f = m_faces[i];
3094                 f.m_ra = AreaOf(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x);
3095         }
3096
3097         /* Node area            */
3098
3099         if (averageArea)
3100         {
3101                 btAlignedObjectArray<int> counts;
3102                 counts.resize(m_nodes.size(), 0);
3103                 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
3104                 {
3105                         m_nodes[i].m_area = 0;
3106                 }
3107                 for (i = 0, ni = m_faces.size(); i < ni; ++i)
3108                 {
3109                         btSoftBody::Face& f = m_faces[i];
3110                         for (int j = 0; j < 3; ++j)
3111                         {
3112                                 const int index = (int)(f.m_n[j] - &m_nodes[0]);
3113                                 counts[index]++;
3114                                 f.m_n[j]->m_area += btFabs(f.m_ra);
3115                         }
3116                 }
3117                 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
3118                 {
3119                         if (counts[i] > 0)
3120                                 m_nodes[i].m_area /= (btScalar)counts[i];
3121                         else
3122                                 m_nodes[i].m_area = 0;
3123                 }
3124         }
3125         else
3126         {
3127                 // initialize node area as zero
3128                 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
3129                 {
3130                         m_nodes[i].m_area = 0;
3131                 }
3132
3133                 for (i = 0, ni = m_faces.size(); i < ni; ++i)
3134                 {
3135                         btSoftBody::Face& f = m_faces[i];
3136
3137                         for (int j = 0; j < 3; ++j)
3138                         {
3139                                 f.m_n[j]->m_area += f.m_ra;
3140                         }
3141                 }
3142
3143                 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
3144                 {
3145                         m_nodes[i].m_area *= 0.3333333f;
3146                 }
3147         }
3148 }
3149
3150 void btSoftBody::updateLinkConstants()
3151 {
3152         int i, ni;
3153
3154         /* Links                */
3155         for (i = 0, ni = m_links.size(); i < ni; ++i)
3156         {
3157                 Link& l = m_links[i];
3158                 Material& m = *l.m_material;
3159                 l.m_c0 = (l.m_n[0]->m_im + l.m_n[1]->m_im) / m.m_kLST;
3160         }
3161 }
3162
3163 void btSoftBody::updateConstants()
3164 {
3165         resetLinkRestLengths();
3166         updateLinkConstants();
3167         updateArea();
3168 }
3169
3170 //
3171 void btSoftBody::initializeClusters()
3172 {
3173         int i;
3174
3175         for (i = 0; i < m_clusters.size(); ++i)
3176         {
3177                 Cluster& c = *m_clusters[i];
3178                 c.m_imass = 0;
3179                 c.m_masses.resize(c.m_nodes.size());
3180                 for (int j = 0; j < c.m_nodes.size(); ++j)
3181                 {
3182                         if (c.m_nodes[j]->m_im == 0)
3183                         {
3184                                 c.m_containsAnchor = true;
3185                                 c.m_masses[j] = BT_LARGE_FLOAT;
3186                         }
3187                         else
3188                         {
3189                                 c.m_masses[j] = btScalar(1.) / c.m_nodes[j]->m_im;
3190                         }
3191                         c.m_imass += c.m_masses[j];
3192                 }
3193                 c.m_imass = btScalar(1.) / c.m_imass;
3194                 c.m_com = btSoftBody::clusterCom(&c);
3195                 c.m_lv = btVector3(0, 0, 0);
3196                 c.m_av = btVector3(0, 0, 0);
3197                 c.m_leaf = 0;
3198                 /* Inertia      */
3199                 btMatrix3x3& ii = c.m_locii;
3200                 ii[0] = ii[1] = ii[2] = btVector3(0, 0, 0);
3201                 {
3202                         int i, ni;
3203
3204                         for (i = 0, ni = c.m_nodes.size(); i < ni; ++i)
3205                         {
3206                                 const btVector3 k = c.m_nodes[i]->m_x - c.m_com;
3207                                 const btVector3 q = k * k;
3208                                 const btScalar m = c.m_masses[i];
3209                                 ii[0][0] += m * (q[1] + q[2]);
3210                                 ii[1][1] += m * (q[0] + q[2]);
3211                                 ii[2][2] += m * (q[0] + q[1]);
3212                                 ii[0][1] -= m * k[0] * k[1];
3213                                 ii[0][2] -= m * k[0] * k[2];
3214                                 ii[1][2] -= m * k[1] * k[2];
3215                         }
3216                 }
3217                 ii[1][0] = ii[0][1];
3218                 ii[2][0] = ii[0][2];
3219                 ii[2][1] = ii[1][2];
3220
3221                 ii = ii.inverse();
3222
3223                 /* Frame        */
3224                 c.m_framexform.setIdentity();
3225                 c.m_framexform.setOrigin(c.m_com);
3226                 c.m_framerefs.resize(c.m_nodes.size());
3227                 {
3228                         int i;
3229                         for (i = 0; i < c.m_framerefs.size(); ++i)
3230                         {
3231                                 c.m_framerefs[i] = c.m_nodes[i]->m_x - c.m_com;
3232                         }
3233                 }
3234         }
3235 }
3236
3237 //
3238 void btSoftBody::updateClusters()
3239 {
3240         BT_PROFILE("UpdateClusters");
3241         int i;
3242
3243         for (i = 0; i < m_clusters.size(); ++i)
3244         {
3245                 btSoftBody::Cluster& c = *m_clusters[i];
3246                 const int n = c.m_nodes.size();
3247                 //const btScalar                        invn=1/(btScalar)n;
3248                 if (n)
3249                 {
3250                         /* Frame                                */
3251                         const btScalar eps = btScalar(0.0001);
3252                         btMatrix3x3 m, r, s;
3253                         m[0] = m[1] = m[2] = btVector3(0, 0, 0);
3254                         m[0][0] = eps * 1;
3255                         m[1][1] = eps * 2;
3256                         m[2][2] = eps * 3;
3257                         c.m_com = clusterCom(&c);
3258                         for (int i = 0; i < c.m_nodes.size(); ++i)
3259                         {
3260                                 const btVector3 a = c.m_nodes[i]->m_x - c.m_com;
3261                                 const btVector3& b = c.m_framerefs[i];
3262                                 m[0] += a[0] * b;
3263                                 m[1] += a[1] * b;
3264                                 m[2] += a[2] * b;
3265                         }
3266                         PolarDecompose(m, r, s);
3267                         c.m_framexform.setOrigin(c.m_com);
3268                         c.m_framexform.setBasis(r);
3269                         /* Inertia                      */
3270 #if 1 /* Constant       */
3271                         c.m_invwi = c.m_framexform.getBasis() * c.m_locii * c.m_framexform.getBasis().transpose();
3272 #else
3273 #if 0 /* Sphere */
3274                         const btScalar  rk=(2*c.m_extents.length2())/(5*c.m_imass);
3275                         const btVector3 inertia(rk,rk,rk);
3276                         const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
3277                                 btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
3278                                 btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
3279
3280                         c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
3281 #else /* Actual */
3282                         c.m_invwi[0] = c.m_invwi[1] = c.m_invwi[2] = btVector3(0, 0, 0);
3283                         for (int i = 0; i < n; ++i)
3284                         {
3285                                 const btVector3 k = c.m_nodes[i]->m_x - c.m_com;
3286                                 const btVector3 q = k * k;
3287                                 const btScalar m = 1 / c.m_nodes[i]->m_im;
3288                                 c.m_invwi[0][0] += m * (q[1] + q[2]);
3289                                 c.m_invwi[1][1] += m * (q[0] + q[2]);
3290                                 c.m_invwi[2][2] += m * (q[0] + q[1]);
3291                                 c.m_invwi[0][1] -= m * k[0] * k[1];
3292                                 c.m_invwi[0][2] -= m * k[0] * k[2];
3293                                 c.m_invwi[1][2] -= m * k[1] * k[2];
3294                         }
3295                         c.m_invwi[1][0] = c.m_invwi[0][1];
3296                         c.m_invwi[2][0] = c.m_invwi[0][2];
3297                         c.m_invwi[2][1] = c.m_invwi[1][2];
3298                         c.m_invwi = c.m_invwi.inverse();
3299 #endif
3300 #endif
3301                         /* Velocities                   */
3302                         c.m_lv = btVector3(0, 0, 0);
3303                         c.m_av = btVector3(0, 0, 0);
3304                         {
3305                                 int i;
3306
3307                                 for (i = 0; i < n; ++i)
3308                                 {
3309                                         const btVector3 v = c.m_nodes[i]->m_v * c.m_masses[i];
3310                                         c.m_lv += v;
3311                                         c.m_av += btCross(c.m_nodes[i]->m_x - c.m_com, v);
3312                                 }
3313                         }
3314                         c.m_lv = c.m_imass * c.m_lv * (1 - c.m_ldamping);
3315                         c.m_av = c.m_invwi * c.m_av * (1 - c.m_adamping);
3316                         c.m_vimpulses[0] =
3317                                 c.m_vimpulses[1] = btVector3(0, 0, 0);
3318                         c.m_dimpulses[0] =
3319                                 c.m_dimpulses[1] = btVector3(0, 0, 0);
3320                         c.m_nvimpulses = 0;
3321                         c.m_ndimpulses = 0;
3322                         /* Matching                             */
3323                         if (c.m_matching > 0)
3324                         {
3325                                 for (int j = 0; j < c.m_nodes.size(); ++j)
3326                                 {
3327                                         Node& n = *c.m_nodes[j];
3328                                         const btVector3 x = c.m_framexform * c.m_framerefs[j];
3329                                         n.m_x = Lerp(n.m_x, x, c.m_matching);
3330                                 }
3331                         }
3332                         /* Dbvt                                 */
3333                         if (c.m_collide)
3334                         {
3335                                 btVector3 mi = c.m_nodes[0]->m_x;
3336                                 btVector3 mx = mi;
3337                                 for (int j = 1; j < n; ++j)
3338                                 {
3339                                         mi.setMin(c.m_nodes[j]->m_x);
3340                                         mx.setMax(c.m_nodes[j]->m_x);
3341                                 }
3342                                 ATTRIBUTE_ALIGNED16(btDbvtVolume)
3343                                 bounds = btDbvtVolume::FromMM(mi, mx);
3344                                 if (c.m_leaf)
3345                                         m_cdbvt.update(c.m_leaf, bounds, c.m_lv * m_sst.sdt * 3, m_sst.radmrg);
3346                                 else
3347                                         c.m_leaf = m_cdbvt.insert(bounds, &c);
3348                         }
3349                 }
3350         }
3351 }
3352
3353 //
3354 void btSoftBody::cleanupClusters()
3355 {
3356         for (int i = 0; i < m_joints.size(); ++i)
3357         {
3358                 m_joints[i]->Terminate(m_sst.sdt);
3359                 if (m_joints[i]->m_delete)
3360                 {
3361                         btAlignedFree(m_joints[i]);
3362                         m_joints.remove(m_joints[i--]);
3363                 }
3364         }
3365 }
3366
3367 //
3368 void btSoftBody::prepareClusters(int iterations)
3369 {
3370         for (int i = 0; i < m_joints.size(); ++i)
3371         {
3372                 m_joints[i]->Prepare(m_sst.sdt, iterations);
3373         }
3374 }
3375
3376 //
3377 void btSoftBody::solveClusters(btScalar sor)
3378 {
3379         for (int i = 0, ni = m_joints.size(); i < ni; ++i)
3380         {
3381                 m_joints[i]->Solve(m_sst.sdt, sor);
3382         }
3383 }
3384
3385 //
3386 void btSoftBody::applyClusters(bool drift)
3387 {
3388         BT_PROFILE("ApplyClusters");
3389         //      const btScalar                                  f0=m_sst.sdt;
3390         //const btScalar                                        f1=f0/2;
3391         btAlignedObjectArray<btVector3> deltas;
3392         btAlignedObjectArray<btScalar> weights;
3393         deltas.resize(m_nodes.size(), btVector3(0, 0, 0));
3394         weights.resize(m_nodes.size(), 0);
3395         int i;
3396
3397         if (drift)
3398         {
3399                 for (i = 0; i < m_clusters.size(); ++i)
3400                 {
3401                         Cluster& c = *m_clusters[i];
3402                         if (c.m_ndimpulses)
3403                         {
3404                                 c.m_dimpulses[0] /= (btScalar)c.m_ndimpulses;
3405                                 c.m_dimpulses[1] /= (btScalar)c.m_ndimpulses;
3406                         }
3407                 }
3408         }
3409
3410         for (i = 0; i < m_clusters.size(); ++i)
3411         {
3412                 Cluster& c = *m_clusters[i];
3413                 if (0 < (drift ? c.m_ndimpulses : c.m_nvimpulses))
3414                 {
3415                         const btVector3 v = (drift ? c.m_dimpulses[0] : c.m_vimpulses[0]) * m_sst.sdt;
3416                         const btVector3 w = (drift ? c.m_dimpulses[1] : c.m_vimpulses[1]) * m_sst.sdt;
3417                         for (int j = 0; j < c.m_nodes.size(); ++j)
3418                         {
3419                                 const int idx = int(c.m_nodes[j] - &m_nodes[0]);
3420                                 const btVector3& x = c.m_nodes[j]->m_x;
3421                                 const btScalar q = c.m_masses[j];
3422                                 deltas[idx] += (v + btCross(w, x - c.m_com)) * q;
3423                                 weights[idx] += q;
3424                         }
3425                 }
3426         }
3427         for (i = 0; i < deltas.size(); ++i)
3428         {
3429                 if (weights[i] > 0)
3430                 {
3431                         m_nodes[i].m_x += deltas[i] / weights[i];
3432                 }
3433         }
3434 }
3435
3436 //
3437 void btSoftBody::dampClusters()
3438 {
3439         int i;
3440
3441         for (i = 0; i < m_clusters.size(); ++i)
3442         {
3443                 Cluster& c = *m_clusters[i];
3444                 if (c.m_ndamping > 0)
3445                 {
3446                         for (int j = 0; j < c.m_nodes.size(); ++j)
3447                         {
3448                                 Node& n = *c.m_nodes[j];
3449                                 if (n.m_im > 0)
3450                                 {
3451                                         const btVector3 vx = c.m_lv + btCross(c.m_av, c.m_nodes[j]->m_q - c.m_com);
3452                                         if (vx.length2() <= n.m_v.length2())
3453                                         {
3454                                                 n.m_v += c.m_ndamping * (vx - n.m_v);
3455                                         }
3456                                 }
3457                         }
3458                 }
3459         }
3460 }
3461
3462 void btSoftBody::setSpringStiffness(btScalar k)
3463 {
3464         for (int i = 0; i < m_links.size(); ++i)
3465         {
3466                 m_links[i].Feature::m_material->m_kLST = k;
3467         }
3468         m_repulsionStiffness = k;
3469 }
3470
3471 void btSoftBody::setGravityFactor(btScalar gravFactor)
3472 {
3473         m_gravityFactor = gravFactor;
3474 }
3475
3476 void btSoftBody::setCacheBarycenter(bool cacheBarycenter)
3477 {
3478         m_cacheBarycenter = cacheBarycenter;
3479 }
3480
3481 void btSoftBody::initializeDmInverse()
3482 {
3483         btScalar unit_simplex_measure = 1. / 6.;
3484
3485         for (int i = 0; i < m_tetras.size(); ++i)
3486         {
3487                 Tetra& t = m_tetras[i];
3488                 btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x;
3489                 btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x;
3490                 btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x;
3491                 btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(),
3492                                            c1.getY(), c2.getY(), c3.getY(),
3493                                            c1.getZ(), c2.getZ(), c3.getZ());
3494                 t.m_element_measure = Dm.determinant() * unit_simplex_measure;
3495                 t.m_Dm_inverse = Dm.inverse();
3496
3497                 // calculate the first three columns of P^{-1}
3498                 btVector3 a = t.m_n[0]->m_x;
3499                 btVector3 b = t.m_n[1]->m_x;
3500                 btVector3 c = t.m_n[2]->m_x;
3501                 btVector3 d = t.m_n[3]->m_x;
3502
3503                 btScalar det = 1 / (a[0] * b[1] * c[2] - a[0] * b[1] * d[2] - a[0] * b[2] * c[1] + a[0] * b[2] * d[1] + a[0] * c[1] * d[2] - a[0] * c[2] * d[1] + a[1] * (-b[0] * c[2] + b[0] * d[2] + b[2] * c[0] - b[2] * d[0] - c[0] * d[2] + c[2] * d[0]) + a[2] * (b[0] * c[1] - b[0] * d[1] + b[1] * (d[0] - c[0]) + c[0] * d[1] - c[1] * d[0]) - b[0] * c[1] * d[2] + b[0] * c[2] * d[1] + b[1] * c[0] * d[2] - b[1] * c[2] * d[0] - b[2] * c[0] * d[1] + b[2] * c[1] * d[0]);
3504
3505                 btScalar P11 = -b[2] * c[1] + d[2] * c[1] + b[1] * c[2] + b[2] * d[1] - c[2] * d[1] - b[1] * d[2];
3506                 btScalar P12 = b[2] * c[0] - d[2] * c[0] - b[0] * c[2] - b[2] * d[0] + c[2] * d[0] + b[0] * d[2];
3507                 btScalar P13 = -b[1] * c[0] + d[1] * c[0] + b[0] * c[1] + b[1] * d[0] - c[1] * d[0] - b[0] * d[1];
3508                 btScalar P21 = a[2] * c[1] - d[2] * c[1] - a[1] * c[2] - a[2] * d[1] + c[2] * d[1] + a[1] * d[2];
3509                 btScalar P22 = -a[2] * c[0] + d[2] * c[0] + a[0] * c[2] + a[2] * d[0] - c[2] * d[0] - a[0] * d[2];
3510                 btScalar P23 = a[1] * c[0] - d[1] * c[0] - a[0] * c[1] - a[1] * d[0] + c[1] * d[0] + a[0] * d[1];
3511                 btScalar P31 = -a[2] * b[1] + d[2] * b[1] + a[1] * b[2] + a[2] * d[1] - b[2] * d[1] - a[1] * d[2];
3512                 btScalar P32 = a[2] * b[0] - d[2] * b[0] - a[0] * b[2] - a[2] * d[0] + b[2] * d[0] + a[0] * d[2];
3513                 btScalar P33 = -a[1] * b[0] + d[1] * b[0] + a[0] * b[1] + a[1] * d[0] - b[1] * d[0] - a[0] * d[1];
3514                 btScalar P41 = a[2] * b[1] - c[2] * b[1] - a[1] * b[2] - a[2] * c[1] + b[2] * c[1] + a[1] * c[2];
3515                 btScalar P42 = -a[2] * b[0] + c[2] * b[0] + a[0] * b[2] + a[2] * c[0] - b[2] * c[0] - a[0] * c[2];
3516                 btScalar P43 = a[1] * b[0] - c[1] * b[0] - a[0] * b[1] - a[1] * c[0] + b[1] * c[0] + a[0] * c[1];
3517
3518                 btVector4 p1(P11 * det, P21 * det, P31 * det, P41 * det);
3519                 btVector4 p2(P12 * det, P22 * det, P32 * det, P42 * det);
3520                 btVector4 p3(P13 * det, P23 * det, P33 * det, P43 * det);
3521
3522                 t.m_P_inv[0] = p1;
3523                 t.m_P_inv[1] = p2;
3524                 t.m_P_inv[2] = p3;
3525         }
3526 }
3527
3528 static btScalar Dot4(const btVector4& a, const btVector4& b)
3529 {
3530         return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
3531 }
3532
3533 void btSoftBody::updateDeformation()
3534 {
3535         btQuaternion q;
3536         for (int i = 0; i < m_tetras.size(); ++i)
3537         {
3538                 btSoftBody::Tetra& t = m_tetras[i];
3539                 btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q;
3540                 btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q;
3541                 btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q;
3542                 btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(),
3543                                            c1.getY(), c2.getY(), c3.getY(),
3544                                            c1.getZ(), c2.getZ(), c3.getZ());
3545                 t.m_F = Ds * t.m_Dm_inverse;
3546
3547                 btSoftBody::TetraScratch& s = m_tetraScratches[i];
3548                 s.m_F = t.m_F;
3549                 s.m_J = t.m_F.determinant();
3550                 btMatrix3x3 C = t.m_F.transpose() * t.m_F;
3551                 s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ();
3552                 s.m_cofF = t.m_F.adjoint().transpose();
3553
3554                 btVector3 a = t.m_n[0]->m_q;
3555                 btVector3 b = t.m_n[1]->m_q;
3556                 btVector3 c = t.m_n[2]->m_q;
3557                 btVector3 d = t.m_n[3]->m_q;
3558                 btVector4 q1(a[0], b[0], c[0], d[0]);
3559                 btVector4 q2(a[1], b[1], c[1], d[1]);
3560                 btVector4 q3(a[2], b[2], c[2], d[2]);
3561                 btMatrix3x3 B(Dot4(q1, t.m_P_inv[0]), Dot4(q1, t.m_P_inv[1]), Dot4(q1, t.m_P_inv[2]),
3562                                           Dot4(q2, t.m_P_inv[0]), Dot4(q2, t.m_P_inv[1]), Dot4(q2, t.m_P_inv[2]),
3563                                           Dot4(q3, t.m_P_inv[0]), Dot4(q3, t.m_P_inv[1]), Dot4(q3, t.m_P_inv[2]));
3564                 q.setRotation(btVector3(0, 0, 1), 0);
3565                 B.extractRotation(q, 0.01);  // precision of the rotation is not very important for visual correctness.
3566                 btMatrix3x3 Q(q);
3567                 s.m_corotation = Q;
3568         }
3569 }
3570
3571 void btSoftBody::advanceDeformation()
3572 {
3573         updateDeformation();
3574         for (int i = 0; i < m_tetras.size(); ++i)
3575         {
3576                 m_tetraScratchesTn[i] = m_tetraScratches[i];
3577         }
3578 }
3579 //
3580 void btSoftBody::Joint::Prepare(btScalar dt, int)
3581 {
3582         m_bodies[0].activate();
3583         m_bodies[1].activate();
3584 }
3585
3586 //
3587 void btSoftBody::LJoint::Prepare(btScalar dt, int iterations)
3588 {
3589         static const btScalar maxdrift = 4;
3590         Joint::Prepare(dt, iterations);
3591         m_rpos[0] = m_bodies[0].xform() * m_refs[0];
3592         m_rpos[1] = m_bodies[1].xform() * m_refs[1];
3593         m_drift = Clamp(m_rpos[0] - m_rpos[1], maxdrift) * m_erp / dt;
3594         m_rpos[0] -= m_bodies[0].xform().getOrigin();
3595         m_rpos[1] -= m_bodies[1].xform().getOrigin();
3596         m_massmatrix = ImpulseMatrix(m_bodies[0].invMass(), m_bodies[0].invWorldInertia(), m_rpos[0],
3597                                                                  m_bodies[1].invMass(), m_bodies[1].invWorldInertia(), m_rpos[1]);
3598         if (m_split > 0)
3599         {
3600                 m_sdrift = m_massmatrix * (m_drift * m_split);
3601                 m_drift *= 1 - m_split;
3602         }
3603         m_drift /= (btScalar)iterations;
3604 }
3605
3606 //
3607 void btSoftBody::LJoint::Solve(btScalar dt, btScalar sor)
3608 {
3609         const btVector3 va = m_bodies[0].velocity(m_rpos[0]);
3610         const btVector3 vb = m_bodies[1].velocity(m_rpos[1]);
3611         const btVector3 vr = va - vb;
3612         btSoftBody::Impulse impulse;
3613         impulse.m_asVelocity = 1;
3614         impulse.m_velocity = m_massmatrix * (m_drift + vr * m_cfm) * sor;
3615         m_bodies[0].applyImpulse(-impulse, m_rpos[0]);
3616         m_bodies[1].applyImpulse(impulse, m_rpos[1]);
3617 }
3618
3619 //
3620 void btSoftBody::LJoint::Terminate(btScalar dt)
3621 {
3622         if (m_split > 0)
3623         {
3624                 m_bodies[0].applyDImpulse(-m_sdrift, m_rpos[0]);
3625                 m_bodies[1].applyDImpulse(m_sdrift, m_rpos[1]);
3626         }
3627 }
3628
3629 //
3630 void btSoftBody::AJoint::Prepare(btScalar dt, int iterations)
3631 {
3632         static const btScalar maxdrift = SIMD_PI / 16;
3633         m_icontrol->Prepare(this);
3634         Joint::Prepare(dt, iterations);
3635         m_axis[0] = m_bodies[0].xform().getBasis() * m_refs[0];
3636         m_axis[1] = m_bodies[1].xform().getBasis() * m_refs[1];
3637         m_drift = NormalizeAny(btCross(m_axis[1], m_axis[0]));
3638         m_drift *= btMin(maxdrift, btAcos(Clamp<btScalar>(btDot(m_axis[0], m_axis[1]), -1, +1)));
3639         m_drift *= m_erp / dt;
3640         m_massmatrix = AngularImpulseMatrix(m_bodies[0].invWorldInertia(), m_bodies[1].invWorldInertia());
3641         if (m_split > 0)
3642         {
3643                 m_sdrift = m_massmatrix * (m_drift * m_split);
3644                 m_drift *= 1 - m_split;
3645         }
3646         m_drift /= (btScalar)iterations;
3647 }
3648
3649 //
3650 void btSoftBody::AJoint::Solve(btScalar dt, btScalar sor)
3651 {
3652         const btVector3 va = m_bodies[0].angularVelocity();
3653         const btVector3 vb = m_bodies[1].angularVelocity();
3654         const btVector3 vr = va - vb;
3655         const btScalar sp = btDot(vr, m_axis[0]);
3656         const btVector3 vc = vr - m_axis[0] * m_icontrol->Speed(this, sp);
3657         btSoftBody::Impulse impulse;
3658         impulse.m_asVelocity = 1;
3659         impulse.m_velocity = m_massmatrix * (m_drift + vc * m_cfm) * sor;
3660         m_bodies[0].applyAImpulse(-impulse);
3661         m_bodies[1].applyAImpulse(impulse);
3662 }
3663
3664 //
3665 void btSoftBody::AJoint::Terminate(btScalar dt)
3666 {
3667         if (m_split > 0)
3668         {
3669                 m_bodies[0].applyDAImpulse(-m_sdrift);
3670                 m_bodies[1].applyDAImpulse(m_sdrift);
3671         }
3672 }
3673
3674 //
3675 void btSoftBody::CJoint::Prepare(btScalar dt, int iterations)
3676 {
3677         Joint::Prepare(dt, iterations);
3678         const bool dodrift = (m_life == 0);
3679         m_delete = (++m_life) > m_maxlife;
3680         if (dodrift)
3681         {
3682                 m_drift = m_drift * m_erp / dt;
3683                 if (m_split > 0)
3684                 {
3685                         m_sdrift = m_massmatrix * (m_drift * m_split);
3686                         m_drift *= 1 - m_split;
3687                 }
3688                 m_drift /= (btScalar)iterations;
3689         }
3690         else
3691         {
3692                 m_drift = m_sdrift = btVector3(0, 0, 0);
3693         }
3694 }
3695
3696 //
3697 void btSoftBody::CJoint::Solve(btScalar dt, btScalar sor)
3698 {
3699         const btVector3 va = m_bodies[0].velocity(m_rpos[0]);
3700         const btVector3 vb = m_bodies[1].velocity(m_rpos[1]);
3701         const btVector3 vrel = va - vb;
3702         const btScalar rvac = btDot(vrel, m_normal);
3703         btSoftBody::Impulse impulse;
3704         impulse.m_asVelocity = 1;
3705         impulse.m_velocity = m_drift;
3706         if (rvac < 0)
3707         {
3708                 const btVector3 iv = m_normal * rvac;
3709                 const btVector3 fv = vrel - iv;
3710                 impulse.m_velocity += iv + fv * m_friction;
3711         }
3712         impulse.m_velocity = m_massmatrix * impulse.m_velocity * sor;
3713
3714         if (m_bodies[0].m_soft == m_bodies[1].m_soft)
3715         {
3716                 if ((impulse.m_velocity.getX() == impulse.m_velocity.getX()) && (impulse.m_velocity.getY() == impulse.m_velocity.getY()) &&
3717                         (impulse.m_velocity.getZ() == impulse.m_velocity.getZ()))
3718                 {
3719                         if (impulse.m_asVelocity)
3720                         {
3721                                 if (impulse.m_velocity.length() < m_bodies[0].m_soft->m_maxSelfCollisionImpulse)
3722                                 {
3723                                 }
3724                                 else
3725                                 {
3726                                         m_bodies[0].applyImpulse(-impulse * m_bodies[0].m_soft->m_selfCollisionImpulseFactor, m_rpos[0]);
3727                                         m_bodies[1].applyImpulse(impulse * m_bodies[0].m_soft->m_selfCollisionImpulseFactor, m_rpos[1]);
3728                                 }
3729                         }
3730                 }
3731         }
3732         else
3733         {
3734                 m_bodies[0].applyImpulse(-impulse, m_rpos[0]);
3735                 m_bodies[1].applyImpulse(impulse, m_rpos[1]);
3736         }
3737 }
3738
3739 //
3740 void btSoftBody::CJoint::Terminate(btScalar dt)
3741 {
3742         if (m_split > 0)
3743         {
3744                 m_bodies[0].applyDImpulse(-m_sdrift, m_rpos[0]);
3745                 m_bodies[1].applyDImpulse(m_sdrift, m_rpos[1]);
3746         }
3747 }
3748
3749 //
3750 void btSoftBody::applyForces()
3751 {
3752         BT_PROFILE("SoftBody applyForces");
3753         //      const btScalar                                  dt =                    m_sst.sdt;
3754         const btScalar kLF = m_cfg.kLF;
3755         const btScalar kDG = m_cfg.kDG;
3756         const btScalar kPR = m_cfg.kPR;
3757         const btScalar kVC = m_cfg.kVC;
3758         const bool as_lift = kLF > 0;
3759         const bool as_drag = kDG > 0;
3760         const bool as_pressure = kPR != 0;
3761         const bool as_volume = kVC > 0;
3762         const bool as_aero = as_lift ||
3763                                                  as_drag;
3764         //const bool                                            as_vaero =              as_aero &&
3765         //                                                                                              (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
3766         //const bool                                            as_faero =              as_aero &&
3767         //                                                                                              (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
3768         const bool use_medium = as_aero;
3769         const bool use_volume = as_pressure ||
3770                                                         as_volume;
3771         btScalar volume = 0;
3772         btScalar ivolumetp = 0;
3773         btScalar dvolumetv = 0;
3774         btSoftBody::sMedium medium;
3775         if (use_volume)
3776         {
3777                 volume = getVolume();
3778                 ivolumetp = 1 / btFabs(volume) * kPR;
3779                 dvolumetv = (m_pose.m_volume - volume) * kVC;
3780         }
3781         /* Per vertex forces                    */
3782         int i, ni;
3783
3784         for (i = 0, ni = m_nodes.size(); i < ni; ++i)
3785         {
3786                 btSoftBody::Node& n = m_nodes[i];
3787                 if (n.m_im > 0)
3788                 {
3789                         if (use_medium)
3790                         {
3791                                 /* Aerodynamics                 */
3792                                 addAeroForceToNode(m_windVelocity, i);
3793                         }
3794                         /* Pressure                             */
3795                         if (as_pressure)
3796                         {
3797                                 n.m_f += n.m_n * (n.m_area * ivolumetp);
3798                         }
3799                         /* Volume                               */
3800                         if (as_volume)
3801                         {
3802                                 n.m_f += n.m_n * (n.m_area * dvolumetv);
3803                         }
3804                 }
3805         }
3806
3807         /* Per face forces                              */
3808         for (i = 0, ni = m_faces.size(); i < ni; ++i)
3809         {
3810                 //      btSoftBody::Face&       f=m_faces[i];
3811
3812                 /* Aerodynamics                 */
3813                 addAeroForceToFace(m_windVelocity, i);
3814         }
3815 }
3816
3817 //
3818 void btSoftBody::setMaxStress(btScalar maxStress)
3819 {
3820         m_cfg.m_maxStress = maxStress;
3821 }
3822
3823 //
3824 void btSoftBody::interpolateRenderMesh()
3825 {
3826         if (m_z.size() > 0)
3827         {
3828                 for (int i = 0; i < m_renderNodes.size(); ++i)
3829                 {
3830                         const Node* p0 = m_renderNodesParents[i][0];
3831                         const Node* p1 = m_renderNodesParents[i][1];
3832                         const Node* p2 = m_renderNodesParents[i][2];
3833                         btVector3 normal = btCross(p1->m_x - p0->m_x, p2->m_x - p0->m_x);
3834                         btVector3 unit_normal = normal.normalized();
3835                         RenderNode& n = m_renderNodes[i];
3836                         n.m_x.setZero();
3837                         for (int j = 0; j < 3; ++j)
3838                         {
3839                                 n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j];
3840                         }
3841                         n.m_x += m_z[i] * unit_normal;
3842                 }
3843         }
3844         else
3845         {
3846                 for (int i = 0; i < m_renderNodes.size(); ++i)
3847                 {
3848                         RenderNode& n = m_renderNodes[i];
3849                         n.m_x.setZero();
3850                         for (int j = 0; j < 4; ++j)
3851                         {
3852                                 if (m_renderNodesParents[i].size())
3853                                 {
3854                                         n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j];
3855                                 }
3856                         }
3857                 }
3858         }
3859 }
3860
3861 void btSoftBody::setCollisionQuadrature(int N)
3862 {
3863         for (int i = 0; i <= N; ++i)
3864         {
3865                 for (int j = 0; i + j <= N; ++j)
3866                 {
3867                         m_quads.push_back(btVector3(btScalar(i) / btScalar(N), btScalar(j) / btScalar(N), btScalar(N - i - j) / btScalar(N)));
3868                 }
3869         }
3870 }
3871
3872 //
3873 void btSoftBody::PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti)
3874 {
3875         BT_PROFILE("PSolve_Anchors");
3876         const btScalar kAHR = psb->m_cfg.kAHR * kst;
3877         const btScalar dt = psb->m_sst.sdt;
3878         for (int i = 0, ni = psb->m_anchors.size(); i < ni; ++i)
3879         {
3880                 const Anchor& a = psb->m_anchors[i];
3881                 const btTransform& t = a.m_body->getWorldTransform();
3882                 Node& n = *a.m_node;
3883                 const btVector3 wa = t * a.m_local;
3884                 const btVector3 va = a.m_body->getVelocityInLocalPoint(a.m_c1) * dt;
3885                 const btVector3 vb = n.m_x - n.m_q;
3886                 const btVector3 vr = (va - vb) + (wa - n.m_x) * kAHR;
3887                 const btVector3 impulse = a.m_c0 * vr * a.m_influence;
3888                 n.m_x += impulse * a.m_c2;
3889                 a.m_body->applyImpulse(-impulse, a.m_c1);
3890         }
3891 }
3892
3893 //
3894 void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti)
3895 {
3896         BT_PROFILE("PSolve_RContacts");
3897         const btScalar dt = psb->m_sst.sdt;
3898         const btScalar mrg = psb->getCollisionShape()->getMargin();
3899         btMultiBodyJacobianData jacobianData;
3900         for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i)
3901         {
3902                 const RContact& c = psb->m_rcontacts[i];
3903                 const sCti& cti = c.m_cti;
3904                 if (cti.m_colObj->hasContactResponse())
3905                 {
3906                         btVector3 va(0, 0, 0);
3907                         btRigidBody* rigidCol = 0;
3908                         btMultiBodyLinkCollider* multibodyLinkCol = 0;
3909                         btScalar* deltaV = NULL;
3910
3911                         if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
3912                         {
3913                                 rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
3914                                 va = rigidCol ? rigidCol->getVelocityInLocalPoint(c.m_c1) * dt : btVector3(0, 0, 0);
3915                         }
3916                         else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
3917                         {
3918                                 multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
3919                                 if (multibodyLinkCol)
3920                                 {
3921                                         const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
3922                                         jacobianData.m_jacobians.resize(ndof);
3923                                         jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
3924                                         btScalar* jac = &jacobianData.m_jacobians[0];
3925
3926                                         multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, c.m_node->m_x, cti.m_normal, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
3927                                         deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
3928                                         multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
3929
3930                                         btScalar vel = 0.0;
3931                                         for (int j = 0; j < ndof; ++j)
3932                                         {
3933                                                 vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
3934                                         }
3935                                         va = cti.m_normal * vel * dt;
3936                                 }
3937                         }
3938
3939                         const btVector3 vb = c.m_node->m_x - c.m_node->m_q;
3940                         const btVector3 vr = vb - va;
3941                         const btScalar dn = btDot(vr, cti.m_normal);
3942                         if (dn <= SIMD_EPSILON)
3943                         {
3944                                 const btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg);
3945                                 const btVector3 fv = vr - (cti.m_normal * dn);
3946                                 // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
3947                                 const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst);
3948                                 c.m_node->m_x -= impulse * c.m_c2;
3949
3950                                 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
3951                                 {
3952                                         if (rigidCol)
3953                                                 rigidCol->applyImpulse(impulse, c.m_c1);
3954                                 }
3955                                 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
3956                                 {
3957                                         if (multibodyLinkCol)
3958                                         {
3959                                                 double multiplier = 0.5;
3960                                                 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
3961                                         }
3962                                 }
3963                         }
3964                 }
3965         }
3966 }
3967
3968 //
3969 void btSoftBody::PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti)
3970 {
3971         BT_PROFILE("PSolve_SContacts");
3972
3973         for (int i = 0, ni = psb->m_scontacts.size(); i < ni; ++i)
3974         {
3975                 const SContact& c = psb->m_scontacts[i];
3976                 const btVector3& nr = c.m_normal;
3977                 Node& n = *c.m_node;
3978                 Face& f = *c.m_face;
3979                 const btVector3 p = BaryEval(f.m_n[0]->m_x,
3980                                                                          f.m_n[1]->m_x,
3981                                                                          f.m_n[2]->m_x,
3982                                                                          c.m_weights);
3983                 const btVector3 q = BaryEval(f.m_n[0]->m_q,
3984                                                                          f.m_n[1]->m_q,
3985                                                                          f.m_n[2]->m_q,
3986                                                                          c.m_weights);
3987                 const btVector3 vr = (n.m_x - n.m_q) - (p - q);
3988                 btVector3 corr(0, 0, 0);
3989                 btScalar dot = btDot(vr, nr);
3990                 if (dot < 0)
3991                 {
3992                         const btScalar j = c.m_margin - (btDot(nr, n.m_x) - btDot(nr, p));
3993                         corr += c.m_normal * j;
3994                 }
3995                 corr -= ProjectOnPlane(vr, nr) * c.m_friction;
3996                 n.m_x += corr * c.m_cfm[0];
3997                 f.m_n[0]->m_x -= corr * (c.m_cfm[1] * c.m_weights.x());
3998                 f.m_n[1]->m_x -= corr * (c.m_cfm[1] * c.m_weights.y());
3999                 f.m_n[2]->m_x -= corr * (c.m_cfm[1] * c.m_weights.z());
4000         }
4001 }
4002
4003 //
4004 void btSoftBody::PSolve_Links(btSoftBody* psb, btScalar kst, btScalar ti)
4005 {
4006         BT_PROFILE("PSolve_Links");
4007         for (int i = 0, ni = psb->m_links.size(); i < ni; ++i)
4008         {
4009                 Link& l = psb->m_links[i];
4010                 if (l.m_c0 > 0)
4011                 {
4012                         Node& a = *l.m_n[0];
4013                         Node& b = *l.m_n[1];
4014                         const btVector3 del = b.m_x - a.m_x;
4015                         const btScalar len = del.length2();
4016                         if (l.m_c1 + len > SIMD_EPSILON)
4017                         {
4018                                 const btScalar k = ((l.m_c1 - len) / (l.m_c0 * (l.m_c1 + len))) * kst;
4019                                 a.m_x -= del * (k * a.m_im);
4020                                 b.m_x += del * (k * b.m_im);
4021                         }
4022                 }
4023         }
4024 }
4025
4026 //
4027 void btSoftBody::VSolve_Links(btSoftBody* psb, btScalar kst)
4028 {
4029         BT_PROFILE("VSolve_Links");
4030         for (int i = 0, ni = psb->m_links.size(); i < ni; ++i)
4031         {
4032                 Link& l = psb->m_links[i];
4033                 Node** n = l.m_n;
4034                 const btScalar j = -btDot(l.m_c3, n[0]->m_v - n[1]->m_v) * l.m_c2 * kst;
4035                 n[0]->m_v += l.m_c3 * (j * n[0]->m_im);
4036                 n[1]->m_v -= l.m_c3 * (j * n[1]->m_im);
4037         }
4038 }
4039
4040 //
4041 btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver)
4042 {
4043         switch (solver)
4044         {
4045                 case ePSolver::Anchors:
4046                         return (&btSoftBody::PSolve_Anchors);
4047                 case ePSolver::Linear:
4048                         return (&btSoftBody::PSolve_Links);
4049                 case ePSolver::RContacts:
4050                         return (&btSoftBody::PSolve_RContacts);
4051                 case ePSolver::SContacts:
4052                         return (&btSoftBody::PSolve_SContacts);
4053                 default:
4054                 {
4055                 }
4056         }
4057         return (0);
4058 }
4059
4060 //
4061 btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver)
4062 {
4063         switch (solver)
4064         {
4065                 case eVSolver::Linear:
4066                         return (&btSoftBody::VSolve_Links);
4067                 default:
4068                 {
4069                 }
4070         }
4071         return (0);
4072 }
4073
4074 void btSoftBody::setSelfCollision(bool useSelfCollision)
4075 {
4076         m_useSelfCollision = useSelfCollision;
4077 }
4078
4079 bool btSoftBody::useSelfCollision()
4080 {
4081         return m_useSelfCollision;
4082 }
4083
4084 //
4085 void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap)
4086 {
4087         switch (m_cfg.collisions & fCollision::RVSmask)
4088         {
4089                 case fCollision::SDF_RS:
4090                 {
4091                         btSoftColliders::CollideSDF_RS docollide;
4092                         btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject());
4093                         btTransform wtr = pcoWrap->getWorldTransform();
4094
4095                         const btTransform ctr = pcoWrap->getWorldTransform();
4096                         const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length();
4097                         const btScalar basemargin = getCollisionShape()->getMargin();
4098                         btVector3 mins;
4099                         btVector3 maxs;
4100                         ATTRIBUTE_ALIGNED16(btDbvtVolume)
4101                         volume;
4102                         pcoWrap->getCollisionShape()->getAabb(pcoWrap->getWorldTransform(),
4103                                                                                                   mins,
4104                                                                                                   maxs);
4105                         volume = btDbvtVolume::FromMM(mins, maxs);
4106                         volume.Expand(btVector3(basemargin, basemargin, basemargin));
4107                         docollide.psb = this;
4108                         docollide.m_colObj1Wrap = pcoWrap;
4109                         docollide.m_rigidBody = prb1;
4110
4111                         docollide.dynmargin = basemargin + timemargin;
4112                         docollide.stamargin = basemargin;
4113                         m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollide);
4114                 }
4115                 break;
4116                 case fCollision::CL_RS:
4117                 {
4118                         btSoftColliders::CollideCL_RS collider;
4119                         collider.ProcessColObj(this, pcoWrap);
4120                 }
4121                 break;
4122                 case fCollision::SDF_RD:
4123                 {
4124                         btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject());
4125                         if (this->isActive())
4126                         {
4127                                 const btTransform wtr = pcoWrap->getWorldTransform();
4128                                 const btScalar timemargin = 0;
4129                                 const btScalar basemargin = getCollisionShape()->getMargin();
4130                                 btVector3 mins;
4131                                 btVector3 maxs;
4132                                 ATTRIBUTE_ALIGNED16(btDbvtVolume)
4133                                 volume;
4134                                 pcoWrap->getCollisionShape()->getAabb(wtr,
4135                                                                                                           mins,
4136                                                                                                           maxs);
4137                                 volume = btDbvtVolume::FromMM(mins, maxs);
4138                                 volume.Expand(btVector3(basemargin, basemargin, basemargin));
4139                                 if (m_cfg.collisions & fCollision::SDF_RDN)
4140                                 {
4141                                         btSoftColliders::CollideSDF_RD docollideNode;
4142                                         docollideNode.psb = this;
4143                                         docollideNode.m_colObj1Wrap = pcoWrap;
4144                                         docollideNode.m_rigidBody = prb1;
4145                                         docollideNode.dynmargin = basemargin + timemargin;
4146                                         docollideNode.stamargin = basemargin;
4147                                         m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
4148                                 }
4149
4150                                 if (((pcoWrap->getCollisionObject()->getInternalType() == CO_RIGID_BODY) && (m_cfg.collisions & fCollision::SDF_RDF)) || ((pcoWrap->getCollisionObject()->getInternalType() == CO_FEATHERSTONE_LINK) && (m_cfg.collisions & fCollision::SDF_MDF)))
4151                                 {
4152                                         btSoftColliders::CollideSDF_RDF docollideFace;
4153                                         docollideFace.psb = this;
4154                                         docollideFace.m_colObj1Wrap = pcoWrap;
4155                                         docollideFace.m_rigidBody = prb1;
4156                                         docollideFace.dynmargin = basemargin + timemargin;
4157                                         docollideFace.stamargin = basemargin;
4158                                         m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
4159                                 }
4160                         }
4161                 }
4162                 break;
4163         }
4164 }
4165
4166 //
4167 void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
4168 {
4169         BT_PROFILE("Deformable Collision");
4170         const int cf = m_cfg.collisions & psb->m_cfg.collisions;
4171         switch (cf & fCollision::SVSmask)
4172         {
4173                 case fCollision::CL_SS:
4174                 {
4175                         //support self-collision if CL_SELF flag set
4176                         if (this != psb || psb->m_cfg.collisions & fCollision::CL_SELF)
4177                         {
4178                                 btSoftColliders::CollideCL_SS docollide;
4179                                 docollide.ProcessSoftSoft(this, psb);
4180                         }
4181                 }
4182                 break;
4183                 case fCollision::VF_SS:
4184                 {
4185                         //only self-collision for Cluster, not Vertex-Face yet
4186                         if (this != psb)
4187                         {
4188                                 btSoftColliders::CollideVF_SS docollide;
4189                                 /* common                                       */
4190                                 docollide.mrg = getCollisionShape()->getMargin() +
4191                                                                 psb->getCollisionShape()->getMargin();
4192                                 /* psb0 nodes vs psb1 faces     */
4193                                 docollide.psb[0] = this;
4194                                 docollide.psb[1] = psb;
4195                                 docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
4196                                                                                                         docollide.psb[1]->m_fdbvt.m_root,
4197                                                                                                         docollide);
4198                                 /* psb1 nodes vs psb0 faces     */
4199                                 docollide.psb[0] = psb;
4200                                 docollide.psb[1] = this;
4201                                 docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
4202                                                                                                         docollide.psb[1]->m_fdbvt.m_root,
4203                                                                                                         docollide);
4204                         }
4205                 }
4206                 break;
4207                 case fCollision::VF_DD:
4208                 {
4209                         if (!psb->m_softSoftCollision)
4210                                 return;
4211                         if (psb->isActive() || this->isActive())
4212                         {
4213                                 if (this != psb)
4214                                 {
4215                                         btSoftColliders::CollideVF_DD docollide;
4216                                         /* common                    */
4217                                         docollide.mrg = getCollisionShape()->getMargin() +
4218                                                                         psb->getCollisionShape()->getMargin();
4219                                         /* psb0 nodes vs psb1 faces    */
4220                                         if (psb->m_tetras.size() > 0)
4221                                                 docollide.useFaceNormal = true;
4222                                         else
4223                                                 docollide.useFaceNormal = false;
4224                                         docollide.psb[0] = this;
4225                                         docollide.psb[1] = psb;
4226                                         docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
4227                                                                                                                 docollide.psb[1]->m_fdbvt.m_root,
4228                                                                                                                 docollide);
4229
4230                                         /* psb1 nodes vs psb0 faces    */
4231                                         if (this->m_tetras.size() > 0)
4232                                                 docollide.useFaceNormal = true;
4233                                         else
4234                                                 docollide.useFaceNormal = false;
4235                                         docollide.psb[0] = psb;
4236                                         docollide.psb[1] = this;
4237                                         docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
4238                                                                                                                 docollide.psb[1]->m_fdbvt.m_root,
4239                                                                                                                 docollide);
4240                                 }
4241                                 else
4242                                 {
4243                                         if (psb->useSelfCollision())
4244                                         {
4245                                                 btSoftColliders::CollideFF_DD docollide;
4246                                                 docollide.mrg = 2 * getCollisionShape()->getMargin();
4247                                                 docollide.psb[0] = this;
4248                                                 docollide.psb[1] = psb;
4249                                                 if (this->m_tetras.size() > 0)
4250                                                         docollide.useFaceNormal = true;
4251                                                 else
4252                                                         docollide.useFaceNormal = false;
4253                                                 /* psb0 faces vs psb0 faces    */
4254                                                 calculateNormalCone(this->m_fdbvnt);
4255                                                 this->m_fdbvt.selfCollideT(m_fdbvnt, docollide);
4256                                         }
4257                                 }
4258                         }
4259                 }
4260                 break;
4261                 default:
4262                 {
4263                 }
4264         }
4265 }
4266
4267 void btSoftBody::geometricCollisionHandler(btSoftBody* psb)
4268 {
4269         if (psb->isActive() || this->isActive())
4270         {
4271                 if (this != psb)
4272                 {
4273                         btSoftColliders::CollideCCD docollide;
4274                         /* common                    */
4275                         docollide.mrg = SAFE_EPSILON;  // for rounding error instead of actual margin
4276                         docollide.dt = psb->m_sst.sdt;
4277                         /* psb0 nodes vs psb1 faces    */
4278                         if (psb->m_tetras.size() > 0)
4279                                 docollide.useFaceNormal = true;
4280                         else
4281                                 docollide.useFaceNormal = false;
4282                         docollide.psb[0] = this;
4283                         docollide.psb[1] = psb;
4284                         docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
4285                                                                                                 docollide.psb[1]->m_fdbvt.m_root,
4286                                                                                                 docollide);
4287                         /* psb1 nodes vs psb0 faces    */
4288                         if (this->m_tetras.size() > 0)
4289                                 docollide.useFaceNormal = true;
4290                         else
4291                                 docollide.useFaceNormal = false;
4292                         docollide.psb[0] = psb;
4293                         docollide.psb[1] = this;
4294                         docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
4295                                                                                                 docollide.psb[1]->m_fdbvt.m_root,
4296                                                                                                 docollide);
4297                 }
4298                 else
4299                 {
4300                         if (psb->useSelfCollision())
4301                         {
4302                                 btSoftColliders::CollideCCD docollide;
4303                                 docollide.mrg = SAFE_EPSILON;
4304                                 docollide.psb[0] = this;
4305                                 docollide.psb[1] = psb;
4306                                 docollide.dt = psb->m_sst.sdt;
4307                                 if (this->m_tetras.size() > 0)
4308                                         docollide.useFaceNormal = true;
4309                                 else
4310                                         docollide.useFaceNormal = false;
4311                                 /* psb0 faces vs psb0 faces    */
4312                                 calculateNormalCone(this->m_fdbvnt);  // should compute this outside of this scope
4313                                 this->m_fdbvt.selfCollideT(m_fdbvnt, docollide);
4314                         }
4315                 }
4316         }
4317 }
4318
4319 void btSoftBody::setWindVelocity(const btVector3& velocity)
4320 {
4321         m_windVelocity = velocity;
4322 }
4323
4324 const btVector3& btSoftBody::getWindVelocity()
4325 {
4326         return m_windVelocity;
4327 }
4328
4329 int btSoftBody::calculateSerializeBufferSize() const
4330 {
4331         int sz = sizeof(btSoftBodyData);
4332         return sz;
4333 }
4334
4335 ///fills the dataBuffer and returns the struct name (and 0 on failure)
4336 const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const
4337 {
4338         btSoftBodyData* sbd = (btSoftBodyData*)dataBuffer;
4339
4340         btCollisionObject::serialize(&sbd->m_collisionObjectData, serializer);
4341
4342         btHashMap<btHashPtr, int> m_nodeIndexMap;
4343
4344         sbd->m_numMaterials = m_materials.size();
4345         sbd->m_materials = sbd->m_numMaterials ? (SoftBodyMaterialData**)serializer->getUniquePointer((void*)&m_materials) : 0;
4346
4347         if (sbd->m_materials)
4348         {
4349                 int sz = sizeof(SoftBodyMaterialData*);
4350                 int numElem = sbd->m_numMaterials;
4351                 btChunk* chunk = serializer->allocate(sz, numElem);
4352                 //SoftBodyMaterialData** memPtr = chunk->m_oldPtr;
4353                 SoftBodyMaterialData** memPtr = (SoftBodyMaterialData**)chunk->m_oldPtr;
4354                 for (int i = 0; i < numElem; i++, memPtr++)
4355                 {
4356                         btSoftBody::Material* mat = m_materials[i];
4357                         *memPtr = mat ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)mat) : 0;
4358                         if (!serializer->findPointer(mat))
4359                         {
4360                                 //serialize it here
4361                                 btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData), 1);
4362                                 SoftBodyMaterialData* memPtr = (SoftBodyMaterialData*)chunk->m_oldPtr;
4363                                 memPtr->m_flags = mat->m_flags;
4364                                 memPtr->m_angularStiffness = mat->m_kAST;
4365                                 memPtr->m_linearStiffness = mat->m_kLST;
4366                                 memPtr->m_volumeStiffness = mat->m_kVST;
4367                                 serializer->finalizeChunk(chunk, "SoftBodyMaterialData", BT_SBMATERIAL_CODE, mat);
4368                         }
4369                 }
4370                 serializer->finalizeChunk(chunk, "SoftBodyMaterialData", BT_ARRAY_CODE, (void*)&m_materials);
4371         }
4372
4373         sbd->m_numNodes = m_nodes.size();
4374         sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes) : 0;
4375         if (sbd->m_nodes)
4376         {
4377                 int sz = sizeof(SoftBodyNodeData);
4378                 int numElem = sbd->m_numNodes;
4379                 btChunk* chunk = serializer->allocate(sz, numElem);
4380                 SoftBodyNodeData* memPtr = (SoftBodyNodeData*)chunk->m_oldPtr;
4381                 for (int i = 0; i < numElem; i++, memPtr++)
4382                 {
4383                         m_nodes[i].m_f.serializeFloat(memPtr->m_accumulatedForce);
4384                         memPtr->m_area = m_nodes[i].m_area;
4385                         memPtr->m_attach = m_nodes[i].m_battach;
4386                         memPtr->m_inverseMass = m_nodes[i].m_im;
4387                         memPtr->m_material = m_nodes[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_nodes[i].m_material) : 0;
4388                         m_nodes[i].m_n.serializeFloat(memPtr->m_normal);
4389                         m_nodes[i].m_x.serializeFloat(memPtr->m_position);
4390                         m_nodes[i].m_q.serializeFloat(memPtr->m_previousPosition);
4391                         m_nodes[i].m_v.serializeFloat(memPtr->m_velocity);
4392                         m_nodeIndexMap.insert(&m_nodes[i], i);
4393                 }
4394                 serializer->finalizeChunk(chunk, "SoftBodyNodeData", BT_SBNODE_CODE, (void*)&m_nodes);
4395         }
4396
4397         sbd->m_numLinks = m_links.size();
4398         sbd->m_links = sbd->m_numLinks ? (SoftBodyLinkData*)serializer->getUniquePointer((void*)&m_links[0]) : 0;
4399         if (sbd->m_links)
4400         {
4401                 int sz = sizeof(SoftBodyLinkData);
4402                 int numElem = sbd->m_numLinks;
4403                 btChunk* chunk = serializer->allocate(sz, numElem);
4404                 SoftBodyLinkData* memPtr = (SoftBodyLinkData*)chunk->m_oldPtr;
4405                 for (int i = 0; i < numElem; i++, memPtr++)
4406                 {
4407                         memPtr->m_bbending = m_links[i].m_bbending;
4408                         memPtr->m_material = m_links[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_links[i].m_material) : 0;
4409                         memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0] : -1;
4410                         memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0] : -1;
4411                         btAssert(memPtr->m_nodeIndices[0] < m_nodes.size());
4412                         btAssert(memPtr->m_nodeIndices[1] < m_nodes.size());
4413                         memPtr->m_restLength = m_links[i].m_rl;
4414                 }
4415                 serializer->finalizeChunk(chunk, "SoftBodyLinkData", BT_ARRAY_CODE, (void*)&m_links[0]);
4416         }
4417
4418         sbd->m_numFaces = m_faces.size();
4419         sbd->m_faces = sbd->m_numFaces ? (SoftBodyFaceData*)serializer->getUniquePointer((void*)&m_faces[0]) : 0;
4420         if (sbd->m_faces)
4421         {
4422                 int sz = sizeof(SoftBodyFaceData);
4423                 int numElem = sbd->m_numFaces;
4424                 btChunk* chunk = serializer->allocate(sz, numElem);
4425                 SoftBodyFaceData* memPtr = (SoftBodyFaceData*)chunk->m_oldPtr;
4426                 for (int i = 0; i < numElem; i++, memPtr++)
4427                 {
4428                         memPtr->m_material = m_faces[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_faces[i].m_material) : 0;
4429                         m_faces[i].m_normal.serializeFloat(memPtr->m_normal);
4430                         for (int j = 0; j < 3; j++)
4431                         {
4432                                 memPtr->m_nodeIndices[j] = m_faces[i].m_n[j] ? m_faces[i].m_n[j] - &m_nodes[0] : -1;
4433                         }
4434                         memPtr->m_restArea = m_faces[i].m_ra;
4435                 }
4436                 serializer->finalizeChunk(chunk, "SoftBodyFaceData", BT_ARRAY_CODE, (void*)&m_faces[0]);
4437         }
4438
4439         sbd->m_numTetrahedra = m_tetras.size();
4440         sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*)serializer->getUniquePointer((void*)&m_tetras[0]) : 0;
4441         if (sbd->m_tetrahedra)
4442         {
4443                 int sz = sizeof(SoftBodyTetraData);
4444                 int numElem = sbd->m_numTetrahedra;
4445                 btChunk* chunk = serializer->allocate(sz, numElem);
4446                 SoftBodyTetraData* memPtr = (SoftBodyTetraData*)chunk->m_oldPtr;
4447                 for (int i = 0; i < numElem; i++, memPtr++)
4448                 {
4449                         for (int j = 0; j < 4; j++)
4450                         {
4451                                 m_tetras[i].m_c0[j].serializeFloat(memPtr->m_c0[j]);
4452                                 memPtr->m_nodeIndices[j] = m_tetras[i].m_n[j] ? m_tetras[i].m_n[j] - &m_nodes[0] : -1;
4453                         }
4454                         memPtr->m_c1 = m_tetras[i].m_c1;
4455                         memPtr->m_c2 = m_tetras[i].m_c2;
4456                         memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_tetras[i].m_material) : 0;
4457                         memPtr->m_restVolume = m_tetras[i].m_rv;
4458                 }
4459                 serializer->finalizeChunk(chunk, "SoftBodyTetraData", BT_ARRAY_CODE, (void*)&m_tetras[0]);
4460         }
4461
4462         sbd->m_numAnchors = m_anchors.size();
4463         sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*)serializer->getUniquePointer((void*)&m_anchors[0]) : 0;
4464         if (sbd->m_anchors)
4465         {
4466                 int sz = sizeof(SoftRigidAnchorData);
4467                 int numElem = sbd->m_numAnchors;
4468                 btChunk* chunk = serializer->allocate(sz, numElem);
4469                 SoftRigidAnchorData* memPtr = (SoftRigidAnchorData*)chunk->m_oldPtr;
4470                 for (int i = 0; i < numElem; i++, memPtr++)
4471                 {
4472                         m_anchors[i].m_c0.serializeFloat(memPtr->m_c0);
4473                         m_anchors[i].m_c1.serializeFloat(memPtr->m_c1);
4474                         memPtr->m_c2 = m_anchors[i].m_c2;
4475                         m_anchors[i].m_local.serializeFloat(memPtr->m_localFrame);
4476                         memPtr->m_nodeIndex = m_anchors[i].m_node ? m_anchors[i].m_node - &m_nodes[0] : -1;
4477
4478                         memPtr->m_rigidBody = m_anchors[i].m_body ? (btRigidBodyData*)serializer->getUniquePointer((void*)m_anchors[i].m_body) : 0;
4479                         btAssert(memPtr->m_nodeIndex < m_nodes.size());
4480                 }
4481                 serializer->finalizeChunk(chunk, "SoftRigidAnchorData", BT_ARRAY_CODE, (void*)&m_anchors[0]);
4482         }
4483
4484         sbd->m_config.m_dynamicFriction = m_cfg.kDF;
4485         sbd->m_config.m_baumgarte = m_cfg.kVCF;
4486         sbd->m_config.m_pressure = m_cfg.kPR;
4487         sbd->m_config.m_aeroModel = this->m_cfg.aeromodel;
4488         sbd->m_config.m_lift = m_cfg.kLF;
4489         sbd->m_config.m_drag = m_cfg.kDG;
4490         sbd->m_config.m_positionIterations = m_cfg.piterations;
4491         sbd->m_config.m_driftIterations = m_cfg.diterations;
4492         sbd->m_config.m_clusterIterations = m_cfg.citerations;
4493         sbd->m_config.m_velocityIterations = m_cfg.viterations;
4494         sbd->m_config.m_maxVolume = m_cfg.maxvolume;
4495         sbd->m_config.m_damping = m_cfg.kDP;
4496         sbd->m_config.m_poseMatch = m_cfg.kMT;
4497         sbd->m_config.m_collisionFlags = m_cfg.collisions;
4498         sbd->m_config.m_volume = m_cfg.kVC;
4499         sbd->m_config.m_rigidContactHardness = m_cfg.kCHR;
4500         sbd->m_config.m_kineticContactHardness = m_cfg.kKHR;
4501         sbd->m_config.m_softContactHardness = m_cfg.kSHR;
4502         sbd->m_config.m_anchorHardness = m_cfg.kAHR;
4503         sbd->m_config.m_timeScale = m_cfg.timescale;
4504         sbd->m_config.m_maxVolume = m_cfg.maxvolume;
4505         sbd->m_config.m_softRigidClusterHardness = m_cfg.kSRHR_CL;
4506         sbd->m_config.m_softKineticClusterHardness = m_cfg.kSKHR_CL;
4507         sbd->m_config.m_softSoftClusterHardness = m_cfg.kSSHR_CL;
4508         sbd->m_config.m_softRigidClusterImpulseSplit = m_cfg.kSR_SPLT_CL;
4509         sbd->m_config.m_softKineticClusterImpulseSplit = m_cfg.kSK_SPLT_CL;
4510         sbd->m_config.m_softSoftClusterImpulseSplit = m_cfg.kSS_SPLT_CL;
4511
4512         //pose for shape matching
4513         {
4514                 sbd->m_pose = (SoftBodyPoseData*)serializer->getUniquePointer((void*)&m_pose);
4515
4516                 int sz = sizeof(SoftBodyPoseData);
4517                 btChunk* chunk = serializer->allocate(sz, 1);
4518                 SoftBodyPoseData* memPtr = (SoftBodyPoseData*)chunk->m_oldPtr;
4519
4520                 m_pose.m_aqq.serializeFloat(memPtr->m_aqq);
4521                 memPtr->m_bframe = m_pose.m_bframe;
4522                 memPtr->m_bvolume = m_pose.m_bvolume;
4523                 m_pose.m_com.serializeFloat(memPtr->m_com);
4524
4525                 memPtr->m_numPositions = m_pose.m_pos.size();
4526                 memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]) : 0;
4527                 if (memPtr->m_numPositions)
4528                 {
4529                         int numElem = memPtr->m_numPositions;
4530                         int sz = sizeof(btVector3Data);
4531                         btChunk* chunk = serializer->allocate(sz, numElem);
4532                         btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr;
4533                         for (int i = 0; i < numElem; i++, memPtr++)
4534                         {
4535                                 m_pose.m_pos[i].serializeFloat(*memPtr);
4536                         }
4537                         serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)&m_pose.m_pos[0]);
4538                 }
4539                 memPtr->m_restVolume = m_pose.m_volume;
4540                 m_pose.m_rot.serializeFloat(memPtr->m_rot);
4541                 m_pose.m_scl.serializeFloat(memPtr->m_scale);
4542
4543                 memPtr->m_numWeigts = m_pose.m_wgh.size();
4544                 memPtr->m_weights = memPtr->m_numWeigts ? (float*)serializer->getUniquePointer((void*)&m_pose.m_wgh[0]) : 0;
4545                 if (memPtr->m_numWeigts)
4546                 {
4547                         int numElem = memPtr->m_numWeigts;
4548                         int sz = sizeof(float);
4549                         btChunk* chunk = serializer->allocate(sz, numElem);
4550                         float* memPtr = (float*)chunk->m_oldPtr;
4551                         for (int i = 0; i < numElem; i++, memPtr++)
4552                         {
4553                                 *memPtr = m_pose.m_wgh[i];
4554                         }
4555                         serializer->finalizeChunk(chunk, "float", BT_ARRAY_CODE, (void*)&m_pose.m_wgh[0]);
4556                 }
4557
4558                 serializer->finalizeChunk(chunk, "SoftBodyPoseData", BT_ARRAY_CODE, (void*)&m_pose);
4559         }
4560
4561         //clusters for convex-cluster collision detection
4562
4563         sbd->m_numClusters = m_clusters.size();
4564         sbd->m_clusters = sbd->m_numClusters ? (SoftBodyClusterData*)serializer->getUniquePointer((void*)m_clusters[0]) : 0;
4565         if (sbd->m_numClusters)
4566         {
4567                 int numElem = sbd->m_numClusters;
4568                 int sz = sizeof(SoftBodyClusterData);
4569                 btChunk* chunk = serializer->allocate(sz, numElem);
4570                 SoftBodyClusterData* memPtr = (SoftBodyClusterData*)chunk->m_oldPtr;
4571                 for (int i = 0; i < numElem; i++, memPtr++)
4572                 {
4573                         memPtr->m_adamping = m_clusters[i]->m_adamping;
4574                         m_clusters[i]->m_av.serializeFloat(memPtr->m_av);
4575                         memPtr->m_clusterIndex = m_clusters[i]->m_clusterIndex;
4576                         memPtr->m_collide = m_clusters[i]->m_collide;
4577                         m_clusters[i]->m_com.serializeFloat(memPtr->m_com);
4578                         memPtr->m_containsAnchor = m_clusters[i]->m_containsAnchor;
4579                         m_clusters[i]->m_dimpulses[0].serializeFloat(memPtr->m_dimpulses[0]);
4580                         m_clusters[i]->m_dimpulses[1].serializeFloat(memPtr->m_dimpulses[1]);
4581                         m_clusters[i]->m_framexform.serializeFloat(memPtr->m_framexform);
4582                         memPtr->m_idmass = m_clusters[i]->m_idmass;
4583                         memPtr->m_imass = m_clusters[i]->m_imass;
4584                         m_clusters[i]->m_invwi.serializeFloat(memPtr->m_invwi);
4585                         memPtr->m_ldamping = m_clusters[i]->m_ldamping;
4586                         m_clusters[i]->m_locii.serializeFloat(memPtr->m_locii);
4587                         m_clusters[i]->m_lv.serializeFloat(memPtr->m_lv);
4588                         memPtr->m_matching = m_clusters[i]->m_matching;
4589                         memPtr->m_maxSelfCollisionImpulse = m_clusters[i]->m_maxSelfCollisionImpulse;
4590                         memPtr->m_ndamping = m_clusters[i]->m_ndamping;
4591                         memPtr->m_ldamping = m_clusters[i]->m_ldamping;
4592                         memPtr->m_adamping = m_clusters[i]->m_adamping;
4593                         memPtr->m_selfCollisionImpulseFactor = m_clusters[i]->m_selfCollisionImpulseFactor;
4594
4595                         memPtr->m_numFrameRefs = m_clusters[i]->m_framerefs.size();
4596                         memPtr->m_numMasses = m_clusters[i]->m_masses.size();
4597                         memPtr->m_numNodes = m_clusters[i]->m_nodes.size();
4598
4599                         memPtr->m_nvimpulses = m_clusters[i]->m_nvimpulses;
4600                         m_clusters[i]->m_vimpulses[0].serializeFloat(memPtr->m_vimpulses[0]);
4601                         m_clusters[i]->m_vimpulses[1].serializeFloat(memPtr->m_vimpulses[1]);
4602                         memPtr->m_ndimpulses = m_clusters[i]->m_ndimpulses;
4603
4604                         memPtr->m_framerefs = memPtr->m_numFrameRefs ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0;
4605                         if (memPtr->m_framerefs)
4606                         {
4607                                 int numElem = memPtr->m_numFrameRefs;
4608                                 int sz = sizeof(btVector3FloatData);
4609                                 btChunk* chunk = serializer->allocate(sz, numElem);
4610                                 btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr;
4611                                 for (int j = 0; j < numElem; j++, memPtr++)
4612                                 {
4613                                         m_clusters[i]->m_framerefs[j].serializeFloat(*memPtr);
4614                                 }
4615                                 serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_framerefs[0]);
4616                         }
4617
4618                         memPtr->m_masses = memPtr->m_numMasses ? (float*)serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]) : 0;
4619                         if (memPtr->m_masses)
4620                         {
4621                                 int numElem = memPtr->m_numMasses;
4622                                 int sz = sizeof(float);
4623                                 btChunk* chunk = serializer->allocate(sz, numElem);
4624                                 float* memPtr = (float*)chunk->m_oldPtr;
4625                                 for (int j = 0; j < numElem; j++, memPtr++)
4626                                 {
4627                                         *memPtr = m_clusters[i]->m_masses[j];
4628                                 }
4629                                 serializer->finalizeChunk(chunk, "float", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_masses[0]);
4630                         }
4631
4632                         memPtr->m_nodeIndices = memPtr->m_numNodes ? (int*)serializer->getUniquePointer((void*)&m_clusters[i]->m_nodes) : 0;
4633                         if (memPtr->m_nodeIndices)
4634                         {
4635                                 int numElem = memPtr->m_numMasses;
4636                                 int sz = sizeof(int);
4637                                 btChunk* chunk = serializer->allocate(sz, numElem);
4638                                 int* memPtr = (int*)chunk->m_oldPtr;
4639                                 for (int j = 0; j < numElem; j++, memPtr++)
4640                                 {
4641                                         int* indexPtr = m_nodeIndexMap.find(m_clusters[i]->m_nodes[j]);
4642                                         btAssert(indexPtr);
4643                                         *memPtr = *indexPtr;
4644                                 }
4645                                 serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_nodes);
4646                         }
4647                 }
4648                 serializer->finalizeChunk(chunk, "SoftBodyClusterData", BT_ARRAY_CODE, (void*)m_clusters[0]);
4649         }
4650
4651         sbd->m_numJoints = m_joints.size();
4652         sbd->m_joints = m_joints.size() ? (btSoftBodyJointData*)serializer->getUniquePointer((void*)&m_joints[0]) : 0;
4653
4654         if (sbd->m_joints)
4655         {
4656                 int sz = sizeof(btSoftBodyJointData);
4657                 int numElem = m_joints.size();
4658                 btChunk* chunk = serializer->allocate(sz, numElem);
4659                 btSoftBodyJointData* memPtr = (btSoftBodyJointData*)chunk->m_oldPtr;
4660
4661                 for (int i = 0; i < numElem; i++, memPtr++)
4662                 {
4663                         memPtr->m_jointType = (int)m_joints[i]->Type();
4664                         m_joints[i]->m_refs[0].serializeFloat(memPtr->m_refs[0]);
4665                         m_joints[i]->m_refs[1].serializeFloat(memPtr->m_refs[1]);
4666                         memPtr->m_cfm = m_joints[i]->m_cfm;
4667                         memPtr->m_erp = float(m_joints[i]->m_erp);
4668                         memPtr->m_split = float(m_joints[i]->m_split);
4669                         memPtr->m_delete = m_joints[i]->m_delete;
4670
4671                         for (int j = 0; j < 4; j++)
4672                         {
4673                                 memPtr->m_relPosition[0].m_floats[j] = 0.f;
4674                                 memPtr->m_relPosition[1].m_floats[j] = 0.f;
4675                         }
4676                         memPtr->m_bodyA = 0;
4677                         memPtr->m_bodyB = 0;
4678                         if (m_joints[i]->m_bodies[0].m_soft)
4679                         {
4680                                 memPtr->m_bodyAtype = BT_JOINT_SOFT_BODY_CLUSTER;
4681                                 memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_soft);
4682                         }
4683                         if (m_joints[i]->m_bodies[0].m_collisionObject)
4684                         {
4685                                 memPtr->m_bodyAtype = BT_JOINT_COLLISION_OBJECT;
4686                                 memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_collisionObject);
4687                         }
4688                         if (m_joints[i]->m_bodies[0].m_rigid)
4689                         {
4690                                 memPtr->m_bodyAtype = BT_JOINT_RIGID_BODY;
4691                                 memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_rigid);
4692                         }
4693
4694                         if (m_joints[i]->m_bodies[1].m_soft)
4695                         {
4696                                 memPtr->m_bodyBtype = BT_JOINT_SOFT_BODY_CLUSTER;
4697                                 memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_soft);
4698                         }
4699                         if (m_joints[i]->m_bodies[1].m_collisionObject)
4700                         {
4701                                 memPtr->m_bodyBtype = BT_JOINT_COLLISION_OBJECT;
4702                                 memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_collisionObject);
4703                         }
4704                         if (m_joints[i]->m_bodies[1].m_rigid)
4705                         {
4706                                 memPtr->m_bodyBtype = BT_JOINT_RIGID_BODY;
4707                                 memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_rigid);
4708                         }
4709                 }
4710                 serializer->finalizeChunk(chunk, "btSoftBodyJointData", BT_ARRAY_CODE, (void*)&m_joints[0]);
4711         }
4712
4713         return btSoftBodyDataName;
4714 }
4715
4716 void btSoftBody::updateDeactivation(btScalar timeStep)
4717 {
4718         if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
4719                 return;
4720
4721         if (m_maxSpeedSquared < m_sleepingThreshold * m_sleepingThreshold)
4722         {
4723                 m_deactivationTime += timeStep;
4724         }
4725         else
4726         {
4727                 m_deactivationTime = btScalar(0.);
4728                 setActivationState(0);
4729         }
4730 }
4731
4732 void btSoftBody::setZeroVelocity()
4733 {
4734         for (int i = 0; i < m_nodes.size(); ++i)
4735         {
4736                 m_nodes[i].m_v.setZero();
4737         }
4738 }
4739
4740 bool btSoftBody::wantsSleeping()
4741 {
4742         if (getActivationState() == DISABLE_DEACTIVATION)
4743                 return false;
4744
4745         //disable deactivation
4746         if (gDisableDeactivation || (gDeactivationTime == btScalar(0.)))
4747                 return false;
4748
4749         if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
4750                 return true;
4751
4752         if (m_deactivationTime > gDeactivationTime)
4753         {
4754                 return true;
4755         }
4756         return false;
4757 }