diff options
Diffstat (limited to 'extern/bullet2/src/BulletSoftBody/btSoftBody.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletSoftBody/btSoftBody.cpp | 4761 |
1 files changed, 2914 insertions, 1847 deletions
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp index 51f4b33d034..0597a8016cd 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp @@ -18,442 +18,690 @@ subject to the following restrictions: #include "BulletSoftBody/btSoftBodySolvers.h" #include "btSoftBodyData.h" #include "LinearMath/btSerializer.h" - +#include "LinearMath/btImplicitQRSVD.h" +#include "LinearMath/btAlignedAllocator.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include <iostream> +// +static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& leafNodes, btAlignedObjectArray<btAlignedObjectArray<int> >& adj) +{ + int N = leafNodes.size(); + if (N == 0) + { + return NULL; + } + while (N > 1) + { + btAlignedObjectArray<bool> marked; + btAlignedObjectArray<btDbvtNode*> newLeafNodes; + btAlignedObjectArray<std::pair<int, int> > childIds; + btAlignedObjectArray<btAlignedObjectArray<int> > newAdj; + marked.resize(N); + for (int i = 0; i < N; ++i) + marked[i] = false; + + // pair adjacent nodes into new(parent) node + for (int i = 0; i < N; ++i) + { + if (marked[i]) + continue; + bool merged = false; + for (int j = 0; j < adj[i].size(); ++j) + { + int n = adj[i][j]; + if (!marked[adj[i][j]]) + { + btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode(); + node->parent = NULL; + node->childs[0] = leafNodes[i]; + node->childs[1] = leafNodes[n]; + leafNodes[i]->parent = node; + leafNodes[n]->parent = node; + newLeafNodes.push_back(node); + childIds.push_back(std::make_pair(i, n)); + merged = true; + marked[n] = true; + break; + } + } + if (!merged) + { + newLeafNodes.push_back(leafNodes[i]); + childIds.push_back(std::make_pair(i, -1)); + } + marked[i] = true; + } + // update adjacency matrix + newAdj.resize(newLeafNodes.size()); + for (int i = 0; i < newLeafNodes.size(); ++i) + { + for (int j = i + 1; j < newLeafNodes.size(); ++j) + { + bool neighbor = false; + const btAlignedObjectArray<int>& leftChildNeighbors = adj[childIds[i].first]; + for (int k = 0; k < leftChildNeighbors.size(); ++k) + { + if (leftChildNeighbors[k] == childIds[j].first || leftChildNeighbors[k] == childIds[j].second) + { + neighbor = true; + break; + } + } + if (!neighbor && childIds[i].second != -1) + { + const btAlignedObjectArray<int>& rightChildNeighbors = adj[childIds[i].second]; + for (int k = 0; k < rightChildNeighbors.size(); ++k) + { + if (rightChildNeighbors[k] == childIds[j].first || rightChildNeighbors[k] == childIds[j].second) + { + neighbor = true; + break; + } + } + } + if (neighbor) + { + newAdj[i].push_back(j); + newAdj[j].push_back(i); + } + } + } + leafNodes = newLeafNodes; + //this assignment leaks memory, the assignment doesn't do a deep copy, for now a manual copy + //adj = newAdj; + adj.clear(); + adj.resize(newAdj.size()); + for (int i = 0; i < newAdj.size(); i++) + { + for (int j = 0; j < newAdj[i].size(); j++) + { + adj[i].push_back(newAdj[i][j]); + } + } + N = leafNodes.size(); + } + return leafNodes[0]; +} // -btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m) -:m_softBodySolver(0),m_worldInfo(worldInfo) -{ - /* Init */ +btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m) + : m_softBodySolver(0), m_worldInfo(worldInfo) +{ + /* Init */ initDefaults(); - /* Default material */ - Material* pm=appendMaterial(); - pm->m_kLST = 1; - pm->m_kAST = 1; - pm->m_kVST = 1; - pm->m_flags = fMaterial::Default; + /* Default material */ + Material* pm = appendMaterial(); + pm->m_kLST = 1; + pm->m_kAST = 1; + pm->m_kVST = 1; + pm->m_flags = fMaterial::Default; - /* Nodes */ - const btScalar margin=getCollisionShape()->getMargin(); + /* Nodes */ + const btScalar margin = getCollisionShape()->getMargin(); m_nodes.resize(node_count); - for(int i=0,ni=node_count;i<ni;++i) - { - Node& n=m_nodes[i]; + m_X.resize(node_count); + for (int i = 0, ni = node_count; i < ni; ++i) + { + Node& n = m_nodes[i]; ZeroInitialize(n); - n.m_x = x?*x++:btVector3(0,0,0); - n.m_q = n.m_x; - n.m_im = m?*m++:1; - n.m_im = n.m_im>0?1/n.m_im:0; - n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n); - n.m_material= pm; + n.m_x = x ? *x++ : btVector3(0, 0, 0); + n.m_q = n.m_x; + n.m_im = m ? *m++ : 1; + n.m_im = n.m_im > 0 ? 1 / n.m_im : 0; + n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n); + n.m_material = pm; + m_X[i] = n.m_x; } - updateBounds(); - + updateBounds(); + setCollisionQuadrature(3); + m_fdbvnt = 0; } -btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo) -:m_worldInfo(worldInfo) +btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo) + : m_worldInfo(worldInfo) { initDefaults(); } - -void btSoftBody::initDefaults() -{ - m_internalType = CO_SOFT_BODY; - m_cfg.aeromodel = eAeroModel::V_Point; - m_cfg.kVCF = 1; - m_cfg.kDG = 0; - m_cfg.kLF = 0; - m_cfg.kDP = 0; - m_cfg.kPR = 0; - m_cfg.kVC = 0; - m_cfg.kDF = (btScalar)0.2; - m_cfg.kMT = 0; - m_cfg.kCHR = (btScalar)1.0; - m_cfg.kKHR = (btScalar)0.1; - m_cfg.kSHR = (btScalar)1.0; - m_cfg.kAHR = (btScalar)0.7; - m_cfg.kSRHR_CL = (btScalar)0.1; - m_cfg.kSKHR_CL = (btScalar)1; - m_cfg.kSSHR_CL = (btScalar)0.5; - m_cfg.kSR_SPLT_CL = (btScalar)0.5; - m_cfg.kSK_SPLT_CL = (btScalar)0.5; - m_cfg.kSS_SPLT_CL = (btScalar)0.5; - m_cfg.maxvolume = (btScalar)1; - m_cfg.timescale = 1; - m_cfg.viterations = 0; - m_cfg.piterations = 1; - m_cfg.diterations = 0; - m_cfg.citerations = 4; - m_cfg.collisions = fCollision::Default; - m_pose.m_bvolume = false; - m_pose.m_bframe = false; - m_pose.m_volume = 0; - m_pose.m_com = btVector3(0,0,0); +void btSoftBody::initDefaults() +{ + m_internalType = CO_SOFT_BODY; + m_cfg.aeromodel = eAeroModel::V_Point; + m_cfg.kVCF = 1; + m_cfg.kDG = 0; + m_cfg.kLF = 0; + m_cfg.kDP = 0; + m_cfg.kPR = 0; + m_cfg.kVC = 0; + m_cfg.kDF = (btScalar)0.2; + m_cfg.kMT = 0; + m_cfg.kCHR = (btScalar)1.0; + m_cfg.kKHR = (btScalar)0.1; + m_cfg.kSHR = (btScalar)1.0; + m_cfg.kAHR = (btScalar)0.7; + m_cfg.kSRHR_CL = (btScalar)0.1; + m_cfg.kSKHR_CL = (btScalar)1; + m_cfg.kSSHR_CL = (btScalar)0.5; + m_cfg.kSR_SPLT_CL = (btScalar)0.5; + m_cfg.kSK_SPLT_CL = (btScalar)0.5; + m_cfg.kSS_SPLT_CL = (btScalar)0.5; + m_cfg.maxvolume = (btScalar)1; + m_cfg.timescale = 1; + m_cfg.viterations = 0; + m_cfg.piterations = 1; + m_cfg.diterations = 0; + m_cfg.citerations = 4; + m_cfg.drag = 0; + m_cfg.m_maxStress = 0; + m_cfg.collisions = fCollision::Default; + m_pose.m_bvolume = false; + m_pose.m_bframe = false; + m_pose.m_volume = 0; + m_pose.m_com = btVector3(0, 0, 0); m_pose.m_rot.setIdentity(); m_pose.m_scl.setIdentity(); - m_tag = 0; - m_timeacc = 0; - m_bUpdateRtCst = true; - m_bounds[0] = btVector3(0,0,0); - m_bounds[1] = btVector3(0,0,0); + m_tag = 0; + m_timeacc = 0; + m_bUpdateRtCst = true; + m_bounds[0] = btVector3(0, 0, 0); + m_bounds[1] = btVector3(0, 0, 0); m_worldTransform.setIdentity(); setSolver(eSolverPresets::Positions); - - /* Collision shape */ + + /* Collision shape */ ///for now, create a collision shape internally m_collisionShape = new btSoftBodyCollisionShape(this); m_collisionShape->setMargin(0.25f); - - m_initialWorldTransform.setIdentity(); - m_windVelocity = btVector3(0,0,0); + m_worldTransform.setIdentity(); + + m_windVelocity = btVector3(0, 0, 0); m_restLengthScale = btScalar(1.0); + m_dampingCoefficient = 1.0; + m_sleepingThreshold = .04; + m_useSelfCollision = false; + m_collisionFlags = 0; + m_softSoftCollision = false; + m_maxSpeedSquared = 0; + m_repulsionStiffness = 0.5; + m_gravityFactor = 1; + m_fdbvnt = 0; } // btSoftBody::~btSoftBody() { //for now, delete the internal shape - delete m_collisionShape; + delete m_collisionShape; int i; releaseClusters(); - for(i=0;i<m_materials.size();++i) + for (i = 0; i < m_materials.size(); ++i) btAlignedFree(m_materials[i]); - for(i=0;i<m_joints.size();++i) + for (i = 0; i < m_joints.size(); ++i) btAlignedFree(m_joints[i]); + if (m_fdbvnt) + delete m_fdbvnt; } // -bool btSoftBody::checkLink(int node0,int node1) const +bool btSoftBody::checkLink(int node0, int node1) const { - return(checkLink(&m_nodes[node0],&m_nodes[node1])); + return (checkLink(&m_nodes[node0], &m_nodes[node1])); } // -bool btSoftBody::checkLink(const Node* node0,const Node* node1) const +bool btSoftBody::checkLink(const Node* node0, const Node* node1) const { - const Node* n[]={node0,node1}; - for(int i=0,ni=m_links.size();i<ni;++i) + const Node* n[] = {node0, node1}; + for (int i = 0, ni = m_links.size(); i < ni; ++i) { - const Link& l=m_links[i]; - if( (l.m_n[0]==n[0]&&l.m_n[1]==n[1])|| - (l.m_n[0]==n[1]&&l.m_n[1]==n[0])) + const Link& l = m_links[i]; + if ((l.m_n[0] == n[0] && l.m_n[1] == n[1]) || + (l.m_n[0] == n[1] && l.m_n[1] == n[0])) { - return(true); + return (true); } } - return(false); + return (false); } // -bool btSoftBody::checkFace(int node0,int node1,int node2) const +bool btSoftBody::checkFace(int node0, int node1, int node2) const { - const Node* n[]={ &m_nodes[node0], - &m_nodes[node1], - &m_nodes[node2]}; - for(int i=0,ni=m_faces.size();i<ni;++i) + const Node* n[] = {&m_nodes[node0], + &m_nodes[node1], + &m_nodes[node2]}; + for (int i = 0, ni = m_faces.size(); i < ni; ++i) { - const Face& f=m_faces[i]; - int c=0; - for(int j=0;j<3;++j) + const Face& f = m_faces[i]; + int c = 0; + for (int j = 0; j < 3; ++j) { - if( (f.m_n[j]==n[0])|| - (f.m_n[j]==n[1])|| - (f.m_n[j]==n[2])) c|=1<<j; else break; + if ((f.m_n[j] == n[0]) || + (f.m_n[j] == n[1]) || + (f.m_n[j] == n[2])) + c |= 1 << j; + else + break; } - if(c==7) return(true); + if (c == 7) return (true); } - return(false); + return (false); } // -btSoftBody::Material* btSoftBody::appendMaterial() +btSoftBody::Material* btSoftBody::appendMaterial() { - Material* pm=new(btAlignedAlloc(sizeof(Material),16)) Material(); - if(m_materials.size()>0) - *pm=*m_materials[0]; + Material* pm = new (btAlignedAlloc(sizeof(Material), 16)) Material(); + if (m_materials.size() > 0) + *pm = *m_materials[0]; else ZeroInitialize(*pm); m_materials.push_back(pm); - return(pm); + return (pm); } // -void btSoftBody::appendNote( const char* text, - const btVector3& o, - const btVector4& c, - Node* n0, - Node* n1, - Node* n2, - Node* n3) +void btSoftBody::appendNote(const char* text, + const btVector3& o, + const btVector4& c, + Node* n0, + Node* n1, + Node* n2, + Node* n3) { - Note n; + Note n; ZeroInitialize(n); - n.m_rank = 0; - n.m_text = text; - n.m_offset = o; - n.m_coords[0] = c.x(); - n.m_coords[1] = c.y(); - n.m_coords[2] = c.z(); - n.m_coords[3] = c.w(); - n.m_nodes[0] = n0;n.m_rank+=n0?1:0; - n.m_nodes[1] = n1;n.m_rank+=n1?1:0; - n.m_nodes[2] = n2;n.m_rank+=n2?1:0; - n.m_nodes[3] = n3;n.m_rank+=n3?1:0; + n.m_rank = 0; + n.m_text = text; + n.m_offset = o; + n.m_coords[0] = c.x(); + n.m_coords[1] = c.y(); + n.m_coords[2] = c.z(); + n.m_coords[3] = c.w(); + n.m_nodes[0] = n0; + n.m_rank += n0 ? 1 : 0; + n.m_nodes[1] = n1; + n.m_rank += n1 ? 1 : 0; + n.m_nodes[2] = n2; + n.m_rank += n2 ? 1 : 0; + n.m_nodes[3] = n3; + n.m_rank += n3 ? 1 : 0; m_notes.push_back(n); } // -void btSoftBody::appendNote( const char* text, - const btVector3& o, - Node* feature) +void btSoftBody::appendNote(const char* text, + const btVector3& o, + Node* feature) { - appendNote(text,o,btVector4(1,0,0,0),feature); + appendNote(text, o, btVector4(1, 0, 0, 0), feature); } // -void btSoftBody::appendNote( const char* text, - const btVector3& o, - Link* feature) +void btSoftBody::appendNote(const char* text, + const btVector3& o, + Link* feature) { - static const btScalar w=1/(btScalar)2; - appendNote(text,o,btVector4(w,w,0,0), feature->m_n[0], - feature->m_n[1]); + static const btScalar w = 1 / (btScalar)2; + appendNote(text, o, btVector4(w, w, 0, 0), feature->m_n[0], + feature->m_n[1]); } // -void btSoftBody::appendNote( const char* text, - const btVector3& o, - Face* feature) +void btSoftBody::appendNote(const char* text, + const btVector3& o, + Face* feature) { - static const btScalar w=1/(btScalar)3; - appendNote(text,o,btVector4(w,w,w,0), feature->m_n[0], - feature->m_n[1], - feature->m_n[2]); + static const btScalar w = 1 / (btScalar)3; + appendNote(text, o, btVector4(w, w, w, 0), feature->m_n[0], + feature->m_n[1], + feature->m_n[2]); } // -void btSoftBody::appendNode( const btVector3& x,btScalar m) +void btSoftBody::appendNode(const btVector3& x, btScalar m) { - if(m_nodes.capacity()==m_nodes.size()) + if (m_nodes.capacity() == m_nodes.size()) { pointersToIndices(); - m_nodes.reserve(m_nodes.size()*2+1); + m_nodes.reserve(m_nodes.size() * 2 + 1); indicesToPointers(); } - const btScalar margin=getCollisionShape()->getMargin(); + const btScalar margin = getCollisionShape()->getMargin(); m_nodes.push_back(Node()); - Node& n=m_nodes[m_nodes.size()-1]; + Node& n = m_nodes[m_nodes.size() - 1]; ZeroInitialize(n); - n.m_x = x; - n.m_q = n.m_x; - n.m_im = m>0?1/m:0; - n.m_material = m_materials[0]; - n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n); + n.m_x = x; + n.m_q = n.m_x; + n.m_im = m > 0 ? 1 / m : 0; + n.m_material = m_materials[0]; + n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n); } // -void btSoftBody::appendLink(int model,Material* mat) +void btSoftBody::appendLink(int model, Material* mat) { - Link l; - if(model>=0) - l=m_links[model]; + Link l; + if (model >= 0) + l = m_links[model]; else - { ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; } + { + ZeroInitialize(l); + l.m_material = mat ? mat : m_materials[0]; + } m_links.push_back(l); } // -void btSoftBody::appendLink( int node0, - int node1, - Material* mat, - bool bcheckexist) +void btSoftBody::appendLink(int node0, + int node1, + Material* mat, + bool bcheckexist) { - appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist); + appendLink(&m_nodes[node0], &m_nodes[node1], mat, bcheckexist); } // -void btSoftBody::appendLink( Node* node0, - Node* node1, - Material* mat, - bool bcheckexist) +void btSoftBody::appendLink(Node* node0, + Node* node1, + Material* mat, + bool bcheckexist) { - if((!bcheckexist)||(!checkLink(node0,node1))) + if ((!bcheckexist) || (!checkLink(node0, node1))) { - appendLink(-1,mat); - Link& l=m_links[m_links.size()-1]; - l.m_n[0] = node0; - l.m_n[1] = node1; - l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length(); - m_bUpdateRtCst=true; + appendLink(-1, mat); + Link& l = m_links[m_links.size() - 1]; + l.m_n[0] = node0; + l.m_n[1] = node1; + l.m_rl = (l.m_n[0]->m_x - l.m_n[1]->m_x).length(); + m_bUpdateRtCst = true; } } // -void btSoftBody::appendFace(int model,Material* mat) +void btSoftBody::appendFace(int model, Material* mat) { - Face f; - if(model>=0) - { f=m_faces[model]; } + Face f; + if (model >= 0) + { + f = m_faces[model]; + } else - { ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; } + { + ZeroInitialize(f); + f.m_material = mat ? mat : m_materials[0]; + } m_faces.push_back(f); } // -void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat) +void btSoftBody::appendFace(int node0, int node1, int node2, Material* mat) { - if (node0==node1) + if (node0 == node1) return; - if (node1==node2) + if (node1 == node2) return; - if (node2==node0) + if (node2 == node0) return; - appendFace(-1,mat); - Face& f=m_faces[m_faces.size()-1]; - btAssert(node0!=node1); - btAssert(node1!=node2); - btAssert(node2!=node0); - f.m_n[0] = &m_nodes[node0]; - f.m_n[1] = &m_nodes[node1]; - f.m_n[2] = &m_nodes[node2]; - f.m_ra = AreaOf( f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x); - m_bUpdateRtCst=true; + appendFace(-1, mat); + Face& f = m_faces[m_faces.size() - 1]; + btAssert(node0 != node1); + btAssert(node1 != node2); + btAssert(node2 != node0); + f.m_n[0] = &m_nodes[node0]; + f.m_n[1] = &m_nodes[node1]; + f.m_n[2] = &m_nodes[node2]; + f.m_ra = AreaOf(f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x); + m_bUpdateRtCst = true; } // -void btSoftBody::appendTetra(int model,Material* mat) +void btSoftBody::appendTetra(int model, Material* mat) { -Tetra t; -if(model>=0) - t=m_tetras[model]; + Tetra t; + if (model >= 0) + t = m_tetras[model]; else - { ZeroInitialize(t);t.m_material=mat?mat:m_materials[0]; } -m_tetras.push_back(t); + { + ZeroInitialize(t); + t.m_material = mat ? mat : m_materials[0]; + } + m_tetras.push_back(t); } // -void btSoftBody::appendTetra(int node0, - int node1, - int node2, - int node3, - Material* mat) +void btSoftBody::appendTetra(int node0, + int node1, + int node2, + int node3, + Material* mat) { - appendTetra(-1,mat); - Tetra& t=m_tetras[m_tetras.size()-1]; - t.m_n[0] = &m_nodes[node0]; - t.m_n[1] = &m_nodes[node1]; - t.m_n[2] = &m_nodes[node2]; - t.m_n[3] = &m_nodes[node3]; - 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); - m_bUpdateRtCst=true; + appendTetra(-1, mat); + Tetra& t = m_tetras[m_tetras.size() - 1]; + t.m_n[0] = &m_nodes[node0]; + t.m_n[1] = &m_nodes[node1]; + t.m_n[2] = &m_nodes[node2]; + t.m_n[3] = &m_nodes[node3]; + 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); + m_bUpdateRtCst = true; } // -void btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisionBetweenLinkedBodies,btScalar influence) +void btSoftBody::appendAnchor(int node, btRigidBody* body, bool disableCollisionBetweenLinkedBodies, btScalar influence) { - btVector3 local = body->getWorldTransform().inverse()*m_nodes[node].m_x; - appendAnchor(node,body,local,disableCollisionBetweenLinkedBodies,influence); + btVector3 local = body->getWorldTransform().inverse() * m_nodes[node].m_x; + appendAnchor(node, body, local, disableCollisionBetweenLinkedBodies, influence); } // -void btSoftBody::appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies,btScalar influence) +void btSoftBody::appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies, btScalar influence) { if (disableCollisionBetweenLinkedBodies) { - if (m_collisionDisabledObjects.findLinearSearch(body)==m_collisionDisabledObjects.size()) + if (m_collisionDisabledObjects.findLinearSearch(body) == m_collisionDisabledObjects.size()) { m_collisionDisabledObjects.push_back(body); } } - Anchor a; - a.m_node = &m_nodes[node]; - a.m_body = body; - a.m_local = localPivot; - a.m_node->m_battach = 1; + Anchor a; + a.m_node = &m_nodes[node]; + a.m_body = body; + a.m_local = localPivot; + a.m_node->m_battach = 1; a.m_influence = influence; m_anchors.push_back(a); } // -void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1) +void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body) +{ + DeformableNodeRigidAnchor c; + btSoftBody::Node& n = m_nodes[node]; + const btScalar ima = n.m_im; + const btScalar imb = body->getInvMass(); + btVector3 nrm; + const btCollisionShape* shp = body->getCollisionShape(); + const btTransform& wtr = body->getWorldTransform(); + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(m_nodes[node].m_x), + shp, + nrm, + 0); + + c.m_cti.m_colObj = body; + c.m_cti.m_normal = wtr.getBasis() * nrm; + c.m_cti.m_offset = dst; + c.m_node = &m_nodes[node]; + const btScalar fc = m_cfg.kDF * body->getFriction(); + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR; + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = body->getInvInertiaTensorWorld(); + const btVector3 ra = n.m_x - wtr.getOrigin(); + + c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + c.m_c1 = ra; + c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x; + c.m_node->m_battach = 1; + m_deformableAnchors.push_back(c); +} + +void btSoftBody::removeAnchor(int node) +{ + const btSoftBody::Node& n = m_nodes[node]; + for (int i = 0; i < m_deformableAnchors.size();) + { + const DeformableNodeRigidAnchor& c = m_deformableAnchors[i]; + if (c.m_node == &n) + { + m_deformableAnchors.removeAtIndex(i); + } + else + { + i++; + } + } +} + +// +void btSoftBody::appendDeformableAnchor(int node, btMultiBodyLinkCollider* link) +{ + DeformableNodeRigidAnchor c; + btSoftBody::Node& n = m_nodes[node]; + const btScalar ima = n.m_im; + btVector3 nrm; + const btCollisionShape* shp = link->getCollisionShape(); + const btTransform& wtr = link->getWorldTransform(); + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(m_nodes[node].m_x), + shp, + nrm, + 0); + c.m_cti.m_colObj = link; + c.m_cti.m_normal = wtr.getBasis() * nrm; + c.m_cti.m_offset = dst; + c.m_node = &m_nodes[node]; + const btScalar fc = m_cfg.kDF * link->getFriction(); + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = link->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR; + btVector3 normal = c.m_cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(link, jacobianData_normal, c.m_node->m_x, normal); + findJacobian(link, jacobianData_t1, c.m_node->m_x, t1); + findJacobian(link, jacobianData_t2, c.m_node->m_x, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = link->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + c.m_c0 = rot.transpose() * local_impulse_matrix * rot; + c.jacobianData_normal = jacobianData_normal; + c.jacobianData_t1 = jacobianData_t1; + c.jacobianData_t2 = jacobianData_t2; + c.t1 = t1; + c.t2 = t2; + const btVector3 ra = n.m_x - wtr.getOrigin(); + c.m_c1 = ra; + c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x; + c.m_node->m_battach = 1; + m_deformableAnchors.push_back(c); +} +// +void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1) { - LJoint* pj = new(btAlignedAlloc(sizeof(LJoint),16)) LJoint(); - pj->m_bodies[0] = body0; - pj->m_bodies[1] = body1; - pj->m_refs[0] = pj->m_bodies[0].xform().inverse()*specs.position; - pj->m_refs[1] = pj->m_bodies[1].xform().inverse()*specs.position; - pj->m_cfm = specs.cfm; - pj->m_erp = specs.erp; - pj->m_split = specs.split; + LJoint* pj = new (btAlignedAlloc(sizeof(LJoint), 16)) LJoint(); + pj->m_bodies[0] = body0; + pj->m_bodies[1] = body1; + pj->m_refs[0] = pj->m_bodies[0].xform().inverse() * specs.position; + pj->m_refs[1] = pj->m_bodies[1].xform().inverse() * specs.position; + pj->m_cfm = specs.cfm; + pj->m_erp = specs.erp; + pj->m_split = specs.split; m_joints.push_back(pj); } // -void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body) +void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Body body) { - appendLinearJoint(specs,m_clusters[0],body); + appendLinearJoint(specs, m_clusters[0], body); } // -void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body) +void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, btSoftBody* body) { - appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]); + appendLinearJoint(specs, m_clusters[0], body->m_clusters[0]); } // -void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1) +void btSoftBody::appendAngularJoint(const AJoint::Specs& specs, Cluster* body0, Body body1) { - AJoint* pj = new(btAlignedAlloc(sizeof(AJoint),16)) AJoint(); - pj->m_bodies[0] = body0; - pj->m_bodies[1] = body1; - pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis()*specs.axis; - pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis()*specs.axis; - pj->m_cfm = specs.cfm; - pj->m_erp = specs.erp; - pj->m_split = specs.split; - pj->m_icontrol = specs.icontrol; + AJoint* pj = new (btAlignedAlloc(sizeof(AJoint), 16)) AJoint(); + pj->m_bodies[0] = body0; + pj->m_bodies[1] = body1; + pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis() * specs.axis; + pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis() * specs.axis; + pj->m_cfm = specs.cfm; + pj->m_erp = specs.erp; + pj->m_split = specs.split; + pj->m_icontrol = specs.icontrol; m_joints.push_back(pj); } // -void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body) +void btSoftBody::appendAngularJoint(const AJoint::Specs& specs, Body body) { - appendAngularJoint(specs,m_clusters[0],body); + appendAngularJoint(specs, m_clusters[0], body); } // -void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body) +void btSoftBody::appendAngularJoint(const AJoint::Specs& specs, btSoftBody* body) { - appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]); + appendAngularJoint(specs, m_clusters[0], body->m_clusters[0]); } // -void btSoftBody::addForce(const btVector3& force) +void btSoftBody::addForce(const btVector3& force) { - for(int i=0,ni=m_nodes.size();i<ni;++i) addForce(force,i); + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) addForce(force, i); } // -void btSoftBody::addForce(const btVector3& force,int node) +void btSoftBody::addForce(const btVector3& force, int node) { - Node& n=m_nodes[node]; - if(n.m_im>0) + Node& n = m_nodes[node]; + if (n.m_im > 0) { - n.m_f += force; + n.m_f += force; } } -void btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeIndex) +void btSoftBody::addAeroForceToNode(const btVector3& windVelocity, int nodeIndex) { btAssert(nodeIndex >= 0 && nodeIndex < m_nodes.size()); @@ -462,51 +710,51 @@ void btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeInde const btScalar kDG = m_cfg.kDG; //const btScalar kPR = m_cfg.kPR; //const btScalar kVC = m_cfg.kVC; - const bool as_lift = kLF>0; - const bool as_drag = kDG>0; + const bool as_lift = kLF > 0; + const bool as_drag = kDG > 0; const bool as_aero = as_lift || as_drag; const bool as_vaero = as_aero && (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided); Node& n = m_nodes[nodeIndex]; - if( n.m_im>0 ) + if (n.m_im > 0) { - btSoftBody::sMedium medium; + btSoftBody::sMedium medium; EvaluateMedium(m_worldInfo, n.m_x, medium); medium.m_velocity = windVelocity; medium.m_density = m_worldInfo->air_density; - /* Aerodynamics */ - if(as_vaero) - { - const btVector3 rel_v = n.m_v - medium.m_velocity; + /* Aerodynamics */ + if (as_vaero) + { + const btVector3 rel_v = n.m_v - medium.m_velocity; const btScalar rel_v_len = rel_v.length(); - const btScalar rel_v2 = rel_v.length2(); + const btScalar rel_v2 = rel_v.length2(); - if(rel_v2>SIMD_EPSILON) + if (rel_v2 > SIMD_EPSILON) { const btVector3 rel_v_nrm = rel_v.normalized(); - btVector3 nrm = n.m_n; + btVector3 nrm = n.m_n; if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSidedLiftDrag) { - nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); + nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1); btVector3 fDrag(0, 0, 0); btVector3 fLift(0, 0, 0); btScalar n_dot_v = nrm.dot(rel_v_nrm); btScalar tri_area = 0.5f * n.m_area; - + fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm); - + // Check angle of attack // cos(10º) = 0.98480 - if ( 0 < n_dot_v && n_dot_v < 0.98480f) - 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)); + if (0 < n_dot_v && n_dot_v < 0.98480f) + 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)); // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node. - btVector3 del_v_by_fDrag = fDrag*n.m_im*m_sst.sdt; + btVector3 del_v_by_fDrag = fDrag * n.m_im * m_sst.sdt; btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2(); btScalar v_len2 = n.m_v.length2(); @@ -514,7 +762,7 @@ void btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeInde { btScalar del_v_by_fDrag_len = del_v_by_fDrag.length(); btScalar v_len = n.m_v.length(); - fDrag *= btScalar(0.8)*(v_len / del_v_by_fDrag_len); + fDrag *= btScalar(0.8) * (v_len / del_v_by_fDrag_len); } n.m_f += fDrag; @@ -522,84 +770,84 @@ void btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeInde } else if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_Point || m_cfg.aeromodel == btSoftBody::eAeroModel::V_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided) { - if (btSoftBody::eAeroModel::V_TwoSided) - nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); + if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided) + nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1); - const btScalar dvn = btDot(rel_v,nrm); - /* Compute forces */ - if(dvn>0) + const btScalar dvn = btDot(rel_v, nrm); + /* Compute forces */ + if (dvn > 0) { - btVector3 force(0,0,0); - const btScalar c0 = n.m_area * dvn * rel_v2/2; - const btScalar c1 = c0 * medium.m_density; - force += nrm*(-c1*kLF); - force += rel_v.normalized() * (-c1 * kDG); + btVector3 force(0, 0, 0); + const btScalar c0 = n.m_area * dvn * rel_v2 / 2; + const btScalar c1 = c0 * medium.m_density; + force += nrm * (-c1 * kLF); + force += rel_v.normalized() * (-c1 * kDG); ApplyClampedForce(n, force, dt); } - } + } } } } } -void btSoftBody::addAeroForceToFace(const btVector3& windVelocity,int faceIndex) +void btSoftBody::addAeroForceToFace(const btVector3& windVelocity, int faceIndex) { const btScalar dt = m_sst.sdt; const btScalar kLF = m_cfg.kLF; const btScalar kDG = m_cfg.kDG; -// const btScalar kPR = m_cfg.kPR; -// const btScalar kVC = m_cfg.kVC; - const bool as_lift = kLF>0; - const bool as_drag = kDG>0; + // const btScalar kPR = m_cfg.kPR; + // const btScalar kVC = m_cfg.kVC; + const bool as_lift = kLF > 0; + const bool as_drag = kDG > 0; const bool as_aero = as_lift || as_drag; const bool as_faero = as_aero && (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided); - if(as_faero) + if (as_faero) { - btSoftBody::Face& f=m_faces[faceIndex]; + btSoftBody::Face& f = m_faces[faceIndex]; - btSoftBody::sMedium medium; - - const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3; - const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3; - EvaluateMedium(m_worldInfo,x,medium); + btSoftBody::sMedium medium; + + const btVector3 v = (f.m_n[0]->m_v + f.m_n[1]->m_v + f.m_n[2]->m_v) / 3; + const btVector3 x = (f.m_n[0]->m_x + f.m_n[1]->m_x + f.m_n[2]->m_x) / 3; + EvaluateMedium(m_worldInfo, x, medium); medium.m_velocity = windVelocity; medium.m_density = m_worldInfo->air_density; - const btVector3 rel_v=v-medium.m_velocity; + const btVector3 rel_v = v - medium.m_velocity; const btScalar rel_v_len = rel_v.length(); - const btScalar rel_v2=rel_v.length2(); + const btScalar rel_v2 = rel_v.length2(); - if(rel_v2>SIMD_EPSILON) + if (rel_v2 > SIMD_EPSILON) { const btVector3 rel_v_nrm = rel_v.normalized(); - btVector3 nrm = f.m_normal; + btVector3 nrm = f.m_normal; if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSidedLiftDrag) { - nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); + nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1); btVector3 fDrag(0, 0, 0); btVector3 fLift(0, 0, 0); btScalar n_dot_v = nrm.dot(rel_v_nrm); btScalar tri_area = 0.5f * f.m_ra; - + fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm); // Check angle of attack // cos(10º) = 0.98480 - if ( 0 < n_dot_v && n_dot_v < 0.98480f) - 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)); + if (0 < n_dot_v && n_dot_v < 0.98480f) + 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)); fDrag /= 3; fLift /= 3; - for(int j=0;j<3;++j) + for (int j = 0; j < 3; ++j) { - if (f.m_n[j]->m_im>0) + if (f.m_n[j]->m_im > 0) { // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node. - btVector3 del_v_by_fDrag = fDrag*f.m_n[j]->m_im*m_sst.sdt; + btVector3 del_v_by_fDrag = fDrag * f.m_n[j]->m_im * m_sst.sdt; btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2(); btScalar v_len2 = f.m_n[j]->m_v.length2(); @@ -607,237 +855,305 @@ void btSoftBody::addAeroForceToFace(const btVector3& windVelocity,int faceInde { btScalar del_v_by_fDrag_len = del_v_by_fDrag.length(); btScalar v_len = f.m_n[j]->m_v.length(); - fDrag *= btScalar(0.8)*(v_len / del_v_by_fDrag_len); + fDrag *= btScalar(0.8) * (v_len / del_v_by_fDrag_len); } - f.m_n[j]->m_f += fDrag; + f.m_n[j]->m_f += fDrag; f.m_n[j]->m_f += fLift; } } } else if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided) { - if (btSoftBody::eAeroModel::F_TwoSided) - nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); + if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided) + nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1); - const btScalar dvn=btDot(rel_v,nrm); - /* Compute forces */ - if(dvn>0) + const btScalar dvn = btDot(rel_v, nrm); + /* Compute forces */ + if (dvn > 0) { - btVector3 force(0,0,0); - const btScalar c0 = f.m_ra*dvn*rel_v2; - const btScalar c1 = c0*medium.m_density; - force += nrm*(-c1*kLF); - force += rel_v.normalized()*(-c1*kDG); - force /= 3; - for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt); + btVector3 force(0, 0, 0); + const btScalar c0 = f.m_ra * dvn * rel_v2; + const btScalar c1 = c0 * medium.m_density; + force += nrm * (-c1 * kLF); + force += rel_v.normalized() * (-c1 * kDG); + force /= 3; + for (int j = 0; j < 3; ++j) ApplyClampedForce(*f.m_n[j], force, dt); } } } } - } // -void btSoftBody::addVelocity(const btVector3& velocity) +void btSoftBody::addVelocity(const btVector3& velocity) { - for(int i=0,ni=m_nodes.size();i<ni;++i) addVelocity(velocity,i); + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) addVelocity(velocity, i); } -/* Set velocity for the entire body */ -void btSoftBody::setVelocity( const btVector3& velocity) +/* Set velocity for the entire body */ +void btSoftBody::setVelocity(const btVector3& velocity) { - for(int i=0,ni=m_nodes.size();i<ni;++i) + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - if(n.m_im>0) + Node& n = m_nodes[i]; + if (n.m_im > 0) { - n.m_v = velocity; + n.m_v = velocity; + n.m_vn = velocity; } } } - // -void btSoftBody::addVelocity(const btVector3& velocity,int node) +void btSoftBody::addVelocity(const btVector3& velocity, int node) { - Node& n=m_nodes[node]; - if(n.m_im>0) + Node& n = m_nodes[node]; + if (n.m_im > 0) { - n.m_v += velocity; + n.m_v += velocity; } } // -void btSoftBody::setMass(int node,btScalar mass) +void btSoftBody::setMass(int node, btScalar mass) { - m_nodes[node].m_im=mass>0?1/mass:0; - m_bUpdateRtCst=true; + m_nodes[node].m_im = mass > 0 ? 1 / mass : 0; + m_bUpdateRtCst = true; } // -btScalar btSoftBody::getMass(int node) const +btScalar btSoftBody::getMass(int node) const { - return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0); + return (m_nodes[node].m_im > 0 ? 1 / m_nodes[node].m_im : 0); } // -btScalar btSoftBody::getTotalMass() const +btScalar btSoftBody::getTotalMass() const { - btScalar mass=0; - for(int i=0;i<m_nodes.size();++i) + btScalar mass = 0; + for (int i = 0; i < m_nodes.size(); ++i) { - mass+=getMass(i); + mass += getMass(i); } - return(mass); + return (mass); } // -void btSoftBody::setTotalMass(btScalar mass,bool fromfaces) +void btSoftBody::setTotalMass(btScalar mass, bool fromfaces) { int i; - if(fromfaces) + if (fromfaces) { - - for(i=0;i<m_nodes.size();++i) + for (i = 0; i < m_nodes.size(); ++i) { - m_nodes[i].m_im=0; + m_nodes[i].m_im = 0; } - for(i=0;i<m_faces.size();++i) + for (i = 0; i < m_faces.size(); ++i) { - const Face& f=m_faces[i]; - const btScalar twicearea=AreaOf( f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x); - for(int j=0;j<3;++j) + const Face& f = m_faces[i]; + const btScalar twicearea = AreaOf(f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x); + for (int j = 0; j < 3; ++j) { - f.m_n[j]->m_im+=twicearea; + f.m_n[j]->m_im += twicearea; } } - for( i=0;i<m_nodes.size();++i) + for (i = 0; i < m_nodes.size(); ++i) { - m_nodes[i].m_im=1/m_nodes[i].m_im; + m_nodes[i].m_im = 1 / m_nodes[i].m_im; } } - const btScalar tm=getTotalMass(); - const btScalar itm=1/tm; - for( i=0;i<m_nodes.size();++i) + const btScalar tm = getTotalMass(); + const btScalar itm = 1 / tm; + for (i = 0; i < m_nodes.size(); ++i) { - m_nodes[i].m_im/=itm*mass; + m_nodes[i].m_im /= itm * mass; } - m_bUpdateRtCst=true; + m_bUpdateRtCst = true; } // -void btSoftBody::setTotalDensity(btScalar density) +void btSoftBody::setTotalDensity(btScalar density) { - setTotalMass(getVolume()*density,true); + setTotalMass(getVolume() * density, true); } // -void btSoftBody::setVolumeMass(btScalar mass) +void btSoftBody::setVolumeMass(btScalar mass) { -btAlignedObjectArray<btScalar> ranks; -ranks.resize(m_nodes.size(),0); -int i; + btAlignedObjectArray<btScalar> ranks; + ranks.resize(m_nodes.size(), 0); + int i; -for(i=0;i<m_nodes.size();++i) + for (i = 0; i < m_nodes.size(); ++i) { - m_nodes[i].m_im=0; + m_nodes[i].m_im = 0; } -for(i=0;i<m_tetras.size();++i) + for (i = 0; i < m_tetras.size(); ++i) { - const Tetra& t=m_tetras[i]; - for(int j=0;j<4;++j) + const Tetra& t = m_tetras[i]; + for (int j = 0; j < 4; ++j) { - t.m_n[j]->m_im+=btFabs(t.m_rv); - ranks[int(t.m_n[j]-&m_nodes[0])]+=1; + t.m_n[j]->m_im += btFabs(t.m_rv); + ranks[int(t.m_n[j] - &m_nodes[0])] += 1; } } -for( i=0;i<m_nodes.size();++i) + for (i = 0; i < m_nodes.size(); ++i) { - if(m_nodes[i].m_im>0) + if (m_nodes[i].m_im > 0) { - m_nodes[i].m_im=ranks[i]/m_nodes[i].m_im; + m_nodes[i].m_im = ranks[i] / m_nodes[i].m_im; } } -setTotalMass(mass,false); + setTotalMass(mass, false); } // -void btSoftBody::setVolumeDensity(btScalar density) +void btSoftBody::setVolumeDensity(btScalar density) { -btScalar volume=0; -for(int i=0;i<m_tetras.size();++i) + btScalar volume = 0; + for (int i = 0; i < m_tetras.size(); ++i) { - const Tetra& t=m_tetras[i]; - for(int j=0;j<4;++j) + const Tetra& t = m_tetras[i]; + for (int j = 0; j < 4; ++j) { - volume+=btFabs(t.m_rv); + volume += btFabs(t.m_rv); } } -setVolumeMass(volume*density/6); + setVolumeMass(volume * density / 6); +} + +// +btVector3 btSoftBody::getLinearVelocity() +{ + btVector3 total_momentum = btVector3(0, 0, 0); + for (int i = 0; i < m_nodes.size(); ++i) + { + btScalar mass = m_nodes[i].m_im == 0 ? 0 : 1.0 / m_nodes[i].m_im; + total_momentum += mass * m_nodes[i].m_v; + } + btScalar total_mass = getTotalMass(); + return total_mass == 0 ? total_momentum : total_momentum / total_mass; +} + +// +void btSoftBody::setLinearVelocity(const btVector3& linVel) +{ + btVector3 old_vel = getLinearVelocity(); + btVector3 diff = linVel - old_vel; + for (int i = 0; i < m_nodes.size(); ++i) + m_nodes[i].m_v += diff; +} + +// +void btSoftBody::setAngularVelocity(const btVector3& angVel) +{ + btVector3 old_vel = getLinearVelocity(); + btVector3 com = getCenterOfMass(); + for (int i = 0; i < m_nodes.size(); ++i) + { + m_nodes[i].m_v = angVel.cross(m_nodes[i].m_x - com) + old_vel; + } +} + +// +btTransform btSoftBody::getRigidTransform() +{ + btVector3 t = getCenterOfMass(); + btMatrix3x3 S; + S.setZero(); + // Get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i || + // It's important to make sure that S has the correct signs. + // SVD is only unique up to the ordering of singular values. + // SVD will manipulate U and V to ensure the ordering of singular values. If all three singular + // vaues are negative, SVD will permute colums of U to make two of them positive. + for (int i = 0; i < m_nodes.size(); ++i) + { + S -= OuterProduct(m_X[i], t - m_nodes[i].m_x); + } + btVector3 sigma; + btMatrix3x3 U, V; + singularValueDecomposition(S, U, sigma, V); + btMatrix3x3 R = V * U.transpose(); + btTransform trs; + trs.setIdentity(); + trs.setOrigin(t); + trs.setBasis(R); + return trs; +} + +// +void btSoftBody::transformTo(const btTransform& trs) +{ + // get the current best rigid fit + btTransform current_transform = getRigidTransform(); + // apply transform in material space + btTransform new_transform = trs * current_transform.inverse(); + transform(new_transform); } // -void btSoftBody::transform(const btTransform& trs) +void btSoftBody::transform(const btTransform& trs) { - const btScalar margin=getCollisionShape()->getMargin(); - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; - - for(int i=0,ni=m_nodes.size();i<ni;++i) + const btScalar margin = getCollisionShape()->getMargin(); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - n.m_x=trs*n.m_x; - n.m_q=trs*n.m_q; - n.m_n=trs.getBasis()*n.m_n; - vol = btDbvtVolume::FromCR(n.m_x,margin); - - m_ndbvt.update(n.m_leaf,vol); + Node& n = m_nodes[i]; + n.m_x = trs * n.m_x; + n.m_q = trs * n.m_q; + n.m_n = trs.getBasis() * n.m_n; + vol = btDbvtVolume::FromCR(n.m_x, margin); + + m_ndbvt.update(n.m_leaf, vol); } updateNormals(); updateBounds(); updateConstants(); - m_initialWorldTransform = trs; } // -void btSoftBody::translate(const btVector3& trs) +void btSoftBody::translate(const btVector3& trs) { - btTransform t; + btTransform t; t.setIdentity(); t.setOrigin(trs); transform(t); } // -void btSoftBody::rotate( const btQuaternion& rot) +void btSoftBody::rotate(const btQuaternion& rot) { - btTransform t; + btTransform t; t.setIdentity(); t.setRotation(rot); transform(t); } // -void btSoftBody::scale(const btVector3& scl) +void btSoftBody::scale(const btVector3& scl) { + const btScalar margin = getCollisionShape()->getMargin(); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; - const btScalar margin=getCollisionShape()->getMargin(); - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; - - for(int i=0,ni=m_nodes.size();i<ni;++i) + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - n.m_x*=scl; - n.m_q*=scl; - vol = btDbvtVolume::FromCR(n.m_x,margin); - m_ndbvt.update(n.m_leaf,vol); + Node& n = m_nodes[i]; + n.m_x *= scl; + n.m_q *= scl; + vol = btDbvtVolume::FromCR(n.m_x, margin); + m_ndbvt.update(n.m_leaf, vol); } updateNormals(); updateBounds(); updateConstants(); + initializeDmInverse(); } // @@ -849,437 +1165,432 @@ btScalar btSoftBody::getRestLengthScale() // void btSoftBody::setRestLengthScale(btScalar restLengthScale) { - for(int i=0, ni=m_links.size(); i<ni; ++i) + for (int i = 0, ni = m_links.size(); i < ni; ++i) { - Link& l=m_links[i]; - l.m_rl = l.m_rl / m_restLengthScale * restLengthScale; - l.m_c1 = l.m_rl*l.m_rl; + Link& l = m_links[i]; + l.m_rl = l.m_rl / m_restLengthScale * restLengthScale; + l.m_c1 = l.m_rl * l.m_rl; } m_restLengthScale = restLengthScale; - + if (getActivationState() == ISLAND_SLEEPING) activate(); } // -void btSoftBody::setPose(bool bvolume,bool bframe) +void btSoftBody::setPose(bool bvolume, bool bframe) { - m_pose.m_bvolume = bvolume; - m_pose.m_bframe = bframe; - int i,ni; - - /* Weights */ - const btScalar omass=getTotalMass(); - const btScalar kmass=omass*m_nodes.size()*1000; - btScalar tmass=omass; + m_pose.m_bvolume = bvolume; + m_pose.m_bframe = bframe; + int i, ni; + + /* Weights */ + const btScalar omass = getTotalMass(); + const btScalar kmass = omass * m_nodes.size() * 1000; + btScalar tmass = omass; m_pose.m_wgh.resize(m_nodes.size()); - for(i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - if(m_nodes[i].m_im<=0) tmass+=kmass; + if (m_nodes[i].m_im <= 0) tmass += kmass; } - for( i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - m_pose.m_wgh[i]= n.m_im>0 ? - 1/(m_nodes[i].m_im*tmass) : - kmass/tmass; + Node& n = m_nodes[i]; + m_pose.m_wgh[i] = n.m_im > 0 ? 1 / (m_nodes[i].m_im * tmass) : kmass / tmass; } - /* Pos */ - const btVector3 com=evaluateCom(); + /* Pos */ + const btVector3 com = evaluateCom(); m_pose.m_pos.resize(m_nodes.size()); - for( i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - m_pose.m_pos[i]=m_nodes[i].m_x-com; + m_pose.m_pos[i] = m_nodes[i].m_x - com; } - m_pose.m_volume = bvolume?getVolume():0; - m_pose.m_com = com; + m_pose.m_volume = bvolume ? getVolume() : 0; + m_pose.m_com = com; m_pose.m_rot.setIdentity(); m_pose.m_scl.setIdentity(); - /* Aqq */ - m_pose.m_aqq[0] = - m_pose.m_aqq[1] = - m_pose.m_aqq[2] = btVector3(0,0,0); - for( i=0,ni=m_nodes.size();i<ni;++i) - { - const btVector3& q=m_pose.m_pos[i]; - const btVector3 mq=m_pose.m_wgh[i]*q; - m_pose.m_aqq[0]+=mq.x()*q; - m_pose.m_aqq[1]+=mq.y()*q; - m_pose.m_aqq[2]+=mq.z()*q; - } - m_pose.m_aqq=m_pose.m_aqq.inverse(); - + /* Aqq */ + m_pose.m_aqq[0] = + m_pose.m_aqq[1] = + m_pose.m_aqq[2] = btVector3(0, 0, 0); + for (i = 0, ni = m_nodes.size(); i < ni; ++i) + { + const btVector3& q = m_pose.m_pos[i]; + const btVector3 mq = m_pose.m_wgh[i] * q; + m_pose.m_aqq[0] += mq.x() * q; + m_pose.m_aqq[1] += mq.y() * q; + m_pose.m_aqq[2] += mq.z() * q; + } + m_pose.m_aqq = m_pose.m_aqq.inverse(); + updateConstants(); } -void btSoftBody::resetLinkRestLengths() +void btSoftBody::resetLinkRestLengths() { - for(int i=0, ni=m_links.size();i<ni;++i) + for (int i = 0, ni = m_links.size(); i < ni; ++i) { - Link& l = m_links[i]; - l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length(); - l.m_c1 = l.m_rl*l.m_rl; + Link& l = m_links[i]; + l.m_rl = (l.m_n[0]->m_x - l.m_n[1]->m_x).length(); + l.m_c1 = l.m_rl * l.m_rl; } } // -btScalar btSoftBody::getVolume() const +btScalar btSoftBody::getVolume() const { - btScalar vol=0; - if(m_nodes.size()>0) + btScalar vol = 0; + if (m_nodes.size() > 0) { - int i,ni; + int i, ni; - const btVector3 org=m_nodes[0].m_x; - for(i=0,ni=m_faces.size();i<ni;++i) + const btVector3 org = m_nodes[0].m_x; + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - const Face& f=m_faces[i]; - vol+=btDot(f.m_n[0]->m_x-org,btCross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org)); + const Face& f = m_faces[i]; + vol += btDot(f.m_n[0]->m_x - org, btCross(f.m_n[1]->m_x - org, f.m_n[2]->m_x - org)); } - vol/=(btScalar)6; + vol /= (btScalar)6; } - return(vol); + return (vol); } // -int btSoftBody::clusterCount() const +int btSoftBody::clusterCount() const { - return(m_clusters.size()); + return (m_clusters.size()); } // -btVector3 btSoftBody::clusterCom(const Cluster* cluster) +btVector3 btSoftBody::clusterCom(const Cluster* cluster) { - btVector3 com(0,0,0); - for(int i=0,ni=cluster->m_nodes.size();i<ni;++i) + btVector3 com(0, 0, 0); + for (int i = 0, ni = cluster->m_nodes.size(); i < ni; ++i) { - com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i]; + com += cluster->m_nodes[i]->m_x * cluster->m_masses[i]; } - return(com*cluster->m_imass); + return (com * cluster->m_imass); } // -btVector3 btSoftBody::clusterCom(int cluster) const +btVector3 btSoftBody::clusterCom(int cluster) const { - return(clusterCom(m_clusters[cluster])); + return (clusterCom(m_clusters[cluster])); } // -btVector3 btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos) +btVector3 btSoftBody::clusterVelocity(const Cluster* cluster, const btVector3& rpos) { - return(cluster->m_lv+btCross(cluster->m_av,rpos)); + return (cluster->m_lv + btCross(cluster->m_av, rpos)); } // -void btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse) +void btSoftBody::clusterVImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse) { - const btVector3 li=cluster->m_imass*impulse; - const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse); - cluster->m_vimpulses[0]+=li;cluster->m_lv+=li; - cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; + const btVector3 li = cluster->m_imass * impulse; + const btVector3 ai = cluster->m_invwi * btCross(rpos, impulse); + cluster->m_vimpulses[0] += li; + cluster->m_lv += li; + cluster->m_vimpulses[1] += ai; + cluster->m_av += ai; cluster->m_nvimpulses++; } // -void btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse) +void btSoftBody::clusterDImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse) { - const btVector3 li=cluster->m_imass*impulse; - const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse); - cluster->m_dimpulses[0]+=li; - cluster->m_dimpulses[1]+=ai; + const btVector3 li = cluster->m_imass * impulse; + const btVector3 ai = cluster->m_invwi * btCross(rpos, impulse); + cluster->m_dimpulses[0] += li; + cluster->m_dimpulses[1] += ai; cluster->m_ndimpulses++; } // -void btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse) +void btSoftBody::clusterImpulse(Cluster* cluster, const btVector3& rpos, const Impulse& impulse) { - if(impulse.m_asVelocity) clusterVImpulse(cluster,rpos,impulse.m_velocity); - if(impulse.m_asDrift) clusterDImpulse(cluster,rpos,impulse.m_drift); + if (impulse.m_asVelocity) clusterVImpulse(cluster, rpos, impulse.m_velocity); + if (impulse.m_asDrift) clusterDImpulse(cluster, rpos, impulse.m_drift); } // -void btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse) +void btSoftBody::clusterVAImpulse(Cluster* cluster, const btVector3& impulse) { - const btVector3 ai=cluster->m_invwi*impulse; - cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; + const btVector3 ai = cluster->m_invwi * impulse; + cluster->m_vimpulses[1] += ai; + cluster->m_av += ai; cluster->m_nvimpulses++; } // -void btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse) +void btSoftBody::clusterDAImpulse(Cluster* cluster, const btVector3& impulse) { - const btVector3 ai=cluster->m_invwi*impulse; - cluster->m_dimpulses[1]+=ai; + const btVector3 ai = cluster->m_invwi * impulse; + cluster->m_dimpulses[1] += ai; cluster->m_ndimpulses++; } // -void btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse) +void btSoftBody::clusterAImpulse(Cluster* cluster, const Impulse& impulse) { - if(impulse.m_asVelocity) clusterVAImpulse(cluster,impulse.m_velocity); - if(impulse.m_asDrift) clusterDAImpulse(cluster,impulse.m_drift); + if (impulse.m_asVelocity) clusterVAImpulse(cluster, impulse.m_velocity); + if (impulse.m_asDrift) clusterDAImpulse(cluster, impulse.m_drift); } // -void btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse) +void btSoftBody::clusterDCImpulse(Cluster* cluster, const btVector3& impulse) { - cluster->m_dimpulses[0]+=impulse*cluster->m_imass; + cluster->m_dimpulses[0] += impulse * cluster->m_imass; cluster->m_ndimpulses++; } struct NodeLinks { - btAlignedObjectArray<int> m_links; + btAlignedObjectArray<int> m_links; }; - - // -int btSoftBody::generateBendingConstraints(int distance,Material* mat) +int btSoftBody::generateBendingConstraints(int distance, Material* mat) { - int i,j; + int i, j; - if(distance>1) + if (distance > 1) { - /* Build graph */ - const int n=m_nodes.size(); - const unsigned inf=(~(unsigned)0)>>1; - unsigned* adj=new unsigned[n*n]; - + /* Build graph */ + const int n = m_nodes.size(); + const unsigned inf = (~(unsigned)0) >> 1; + unsigned* adj = new unsigned[n * n]; -#define IDX(_x_,_y_) ((_y_)*n+(_x_)) - for(j=0;j<n;++j) +#define IDX(_x_, _y_) ((_y_)*n + (_x_)) + for (j = 0; j < n; ++j) { - for(i=0;i<n;++i) + for (i = 0; i < n; ++i) { - if(i!=j) + if (i != j) { - adj[IDX(i,j)]=adj[IDX(j,i)]=inf; + adj[IDX(i, j)] = adj[IDX(j, i)] = inf; } else { - adj[IDX(i,j)]=adj[IDX(j,i)]=0; + adj[IDX(i, j)] = adj[IDX(j, i)] = 0; } } } - for( i=0;i<m_links.size();++i) + for (i = 0; i < m_links.size(); ++i) { - const int ia=(int)(m_links[i].m_n[0]-&m_nodes[0]); - const int ib=(int)(m_links[i].m_n[1]-&m_nodes[0]); - adj[IDX(ia,ib)]=1; - adj[IDX(ib,ia)]=1; + const int ia = (int)(m_links[i].m_n[0] - &m_nodes[0]); + const int ib = (int)(m_links[i].m_n[1] - &m_nodes[0]); + adj[IDX(ia, ib)] = 1; + adj[IDX(ib, ia)] = 1; } - //special optimized case for distance == 2 if (distance == 2) { - btAlignedObjectArray<NodeLinks> nodeLinks; - /* Build node links */ nodeLinks.resize(m_nodes.size()); - for( i=0;i<m_links.size();++i) + for (i = 0; i < m_links.size(); ++i) { - const int ia=(int)(m_links[i].m_n[0]-&m_nodes[0]); - const int ib=(int)(m_links[i].m_n[1]-&m_nodes[0]); - if (nodeLinks[ia].m_links.findLinearSearch(ib)==nodeLinks[ia].m_links.size()) + const int ia = (int)(m_links[i].m_n[0] - &m_nodes[0]); + const int ib = (int)(m_links[i].m_n[1] - &m_nodes[0]); + if (nodeLinks[ia].m_links.findLinearSearch(ib) == nodeLinks[ia].m_links.size()) nodeLinks[ia].m_links.push_back(ib); - if (nodeLinks[ib].m_links.findLinearSearch(ia)==nodeLinks[ib].m_links.size()) + if (nodeLinks[ib].m_links.findLinearSearch(ia) == nodeLinks[ib].m_links.size()) nodeLinks[ib].m_links.push_back(ia); } - for (int ii=0;ii<nodeLinks.size();ii++) + for (int ii = 0; ii < nodeLinks.size(); ii++) { - int i=ii; + int i = ii; - for (int jj=0;jj<nodeLinks[ii].m_links.size();jj++) + for (int jj = 0; jj < nodeLinks[ii].m_links.size(); jj++) { int k = nodeLinks[ii].m_links[jj]; - for (int kk=0;kk<nodeLinks[k].m_links.size();kk++) + for (int kk = 0; kk < nodeLinks[k].m_links.size(); kk++) { int j = nodeLinks[k].m_links[kk]; - if (i!=j) + if (i != j) { - const unsigned sum=adj[IDX(i,k)]+adj[IDX(k,j)]; - btAssert(sum==2); - if(adj[IDX(i,j)]>sum) + const unsigned sum = adj[IDX(i, k)] + adj[IDX(k, j)]; + btAssert(sum == 2); + if (adj[IDX(i, j)] > sum) { - adj[IDX(i,j)]=adj[IDX(j,i)]=sum; + adj[IDX(i, j)] = adj[IDX(j, i)] = sum; } } - } } } - } else + } + else { ///generic Floyd's algorithm - for(int k=0;k<n;++k) + for (int k = 0; k < n; ++k) { - for(j=0;j<n;++j) + for (j = 0; j < n; ++j) { - for(i=j+1;i<n;++i) + for (i = j + 1; i < n; ++i) { - const unsigned sum=adj[IDX(i,k)]+adj[IDX(k,j)]; - if(adj[IDX(i,j)]>sum) + const unsigned sum = adj[IDX(i, k)] + adj[IDX(k, j)]; + if (adj[IDX(i, j)] > sum) { - adj[IDX(i,j)]=adj[IDX(j,i)]=sum; + adj[IDX(i, j)] = adj[IDX(j, i)] = sum; } } } } } - - /* Build links */ - int nlinks=0; - for(j=0;j<n;++j) + /* Build links */ + int nlinks = 0; + for (j = 0; j < n; ++j) { - for(i=j+1;i<n;++i) + for (i = j + 1; i < n; ++i) { - if(adj[IDX(i,j)]==(unsigned)distance) + if (adj[IDX(i, j)] == (unsigned)distance) { - appendLink(i,j,mat); - m_links[m_links.size()-1].m_bbending=1; + appendLink(i, j, mat); + m_links[m_links.size() - 1].m_bbending = 1; ++nlinks; } } } - delete[] adj; - return(nlinks); + delete[] adj; + return (nlinks); } - return(0); + return (0); } // -void btSoftBody::randomizeConstraints() +void btSoftBody::randomizeConstraints() { - unsigned long seed=243703; -#define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff) - int i,ni; + unsigned long seed = 243703; +#define NEXTRAND (seed = (1664525L * seed + 1013904223L) & 0xffffffff) + int i, ni; - for(i=0,ni=m_links.size();i<ni;++i) + for (i = 0, ni = m_links.size(); i < ni; ++i) { - btSwap(m_links[i],m_links[NEXTRAND%ni]); + btSwap(m_links[i], m_links[NEXTRAND % ni]); } - for(i=0,ni=m_faces.size();i<ni;++i) + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - btSwap(m_faces[i],m_faces[NEXTRAND%ni]); + btSwap(m_faces[i], m_faces[NEXTRAND % ni]); } #undef NEXTRAND } // -void btSoftBody::releaseCluster(int index) +void btSoftBody::releaseCluster(int index) { - Cluster* c=m_clusters[index]; - if(c->m_leaf) m_cdbvt.remove(c->m_leaf); + Cluster* c = m_clusters[index]; + if (c->m_leaf) m_cdbvt.remove(c->m_leaf); c->~Cluster(); btAlignedFree(c); m_clusters.remove(c); } // -void btSoftBody::releaseClusters() +void btSoftBody::releaseClusters() { - while(m_clusters.size()>0) releaseCluster(0); + while (m_clusters.size() > 0) releaseCluster(0); } // -int btSoftBody::generateClusters(int k,int maxiterations) +int btSoftBody::generateClusters(int k, int maxiterations) { int i; releaseClusters(); - m_clusters.resize(btMin(k,m_nodes.size())); - for(i=0;i<m_clusters.size();++i) - { - m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); - m_clusters[i]->m_collide= true; - } - k=m_clusters.size(); - if(k>0) - { - /* Initialize */ - btAlignedObjectArray<btVector3> centers; - btVector3 cog(0,0,0); - int i; - for(i=0;i<m_nodes.size();++i) - { - cog+=m_nodes[i].m_x; - m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]); - } - cog/=(btScalar)m_nodes.size(); - centers.resize(k,cog); - /* Iterate */ - const btScalar slope=16; - bool changed; - int iterations=0; - do { - const btScalar w=2-btMin<btScalar>(1,iterations/slope); - changed=false; - iterations++; + m_clusters.resize(btMin(k, m_nodes.size())); + for (i = 0; i < m_clusters.size(); ++i) + { + m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster(); + m_clusters[i]->m_collide = true; + } + k = m_clusters.size(); + if (k > 0) + { + /* Initialize */ + btAlignedObjectArray<btVector3> centers; + btVector3 cog(0, 0, 0); + int i; + for (i = 0; i < m_nodes.size(); ++i) + { + cog += m_nodes[i].m_x; + m_clusters[(i * 29873) % m_clusters.size()]->m_nodes.push_back(&m_nodes[i]); + } + cog /= (btScalar)m_nodes.size(); + centers.resize(k, cog); + /* Iterate */ + const btScalar slope = 16; + bool changed; + int iterations = 0; + do + { + const btScalar w = 2 - btMin<btScalar>(1, iterations / slope); + changed = false; + iterations++; int i; - for(i=0;i<k;++i) + for (i = 0; i < k; ++i) { - btVector3 c(0,0,0); - for(int j=0;j<m_clusters[i]->m_nodes.size();++j) + btVector3 c(0, 0, 0); + for (int j = 0; j < m_clusters[i]->m_nodes.size(); ++j) { - c+=m_clusters[i]->m_nodes[j]->m_x; + c += m_clusters[i]->m_nodes[j]->m_x; } - if(m_clusters[i]->m_nodes.size()) + if (m_clusters[i]->m_nodes.size()) { - c /= (btScalar)m_clusters[i]->m_nodes.size(); - c = centers[i]+(c-centers[i])*w; - changed |= ((c-centers[i]).length2()>SIMD_EPSILON); - centers[i] = c; + c /= (btScalar)m_clusters[i]->m_nodes.size(); + c = centers[i] + (c - centers[i]) * w; + changed |= ((c - centers[i]).length2() > SIMD_EPSILON); + centers[i] = c; m_clusters[i]->m_nodes.resize(0); - } + } } - for(i=0;i<m_nodes.size();++i) + for (i = 0; i < m_nodes.size(); ++i) { - const btVector3 nx=m_nodes[i].m_x; - int kbest=0; - btScalar kdist=ClusterMetric(centers[0],nx); - for(int j=1;j<k;++j) + const btVector3 nx = m_nodes[i].m_x; + int kbest = 0; + btScalar kdist = ClusterMetric(centers[0], nx); + for (int j = 1; j < k; ++j) { - const btScalar d=ClusterMetric(centers[j],nx); - if(d<kdist) + const btScalar d = ClusterMetric(centers[j], nx); + if (d < kdist) { - kbest=j; - kdist=d; + kbest = j; + kdist = d; } } m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]); - } - } while(changed&&(iterations<maxiterations)); - /* Merge */ - btAlignedObjectArray<int> cids; - cids.resize(m_nodes.size(),-1); - for(i=0;i<m_clusters.size();++i) + } + } while (changed && (iterations < maxiterations)); + /* Merge */ + btAlignedObjectArray<int> cids; + cids.resize(m_nodes.size(), -1); + for (i = 0; i < m_clusters.size(); ++i) { - for(int j=0;j<m_clusters[i]->m_nodes.size();++j) + for (int j = 0; j < m_clusters[i]->m_nodes.size(); ++j) { - cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i; + cids[int(m_clusters[i]->m_nodes[j] - &m_nodes[0])] = i; } } - for(i=0;i<m_faces.size();++i) + for (i = 0; i < m_faces.size(); ++i) { - const int idx[]={ int(m_faces[i].m_n[0]-&m_nodes[0]), - int(m_faces[i].m_n[1]-&m_nodes[0]), - int(m_faces[i].m_n[2]-&m_nodes[0])}; - for(int j=0;j<3;++j) + const int idx[] = {int(m_faces[i].m_n[0] - &m_nodes[0]), + int(m_faces[i].m_n[1] - &m_nodes[0]), + int(m_faces[i].m_n[2] - &m_nodes[0])}; + for (int j = 0; j < 3; ++j) { - const int cid=cids[idx[j]]; - for(int q=1;q<3;++q) + const int cid = cids[idx[j]]; + for (int q = 1; q < 3; ++q) { - const int kid=idx[(j+q)%3]; - if(cids[kid]!=cid) + const int kid = idx[(j + q) % 3]; + if (cids[kid] != cid) { - if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size()) + if (m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid]) == m_clusters[cid]->m_nodes.size()) { m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]); } @@ -1287,55 +1598,56 @@ int btSoftBody::generateClusters(int k,int maxiterations) } } } - /* Master */ - if(m_clusters.size()>1) + /* Master */ + if (m_clusters.size() > 1) { - Cluster* pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); - pmaster->m_collide = false; + Cluster* pmaster = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster(); + pmaster->m_collide = false; pmaster->m_nodes.reserve(m_nodes.size()); - for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]); + for (int i = 0; i < m_nodes.size(); ++i) pmaster->m_nodes.push_back(&m_nodes[i]); m_clusters.push_back(pmaster); - btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]); + btSwap(m_clusters[0], m_clusters[m_clusters.size() - 1]); } - /* Terminate */ - for(i=0;i<m_clusters.size();++i) + /* Terminate */ + for (i = 0; i < m_clusters.size(); ++i) { - if(m_clusters[i]->m_nodes.size()==0) + if (m_clusters[i]->m_nodes.size() == 0) { releaseCluster(i--); } } - } else + } + else { //create a cluster for each tetrahedron (if tetrahedra exist) or each face if (m_tetras.size()) { m_clusters.resize(m_tetras.size()); - for(i=0;i<m_clusters.size();++i) + for (i = 0; i < m_clusters.size(); ++i) { - m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); - m_clusters[i]->m_collide= true; + m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster(); + m_clusters[i]->m_collide = true; } - for (i=0;i<m_tetras.size();i++) + for (i = 0; i < m_tetras.size(); i++) { - for (int j=0;j<4;j++) + for (int j = 0; j < 4; j++) { m_clusters[i]->m_nodes.push_back(m_tetras[i].m_n[j]); } } - - } else + } + else { m_clusters.resize(m_faces.size()); - for(i=0;i<m_clusters.size();++i) + for (i = 0; i < m_clusters.size(); ++i) { - m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); - m_clusters[i]->m_collide= true; + m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster(); + m_clusters[i]->m_collide = true; } - for(i=0;i<m_faces.size();++i) + for (i = 0; i < m_faces.size(); ++i) { - for(int j=0;j<3;++j) + for (int j = 0; j < 3; ++j) { m_clusters[i]->m_nodes.push_back(m_faces[i].m_n[j]); } @@ -1348,261 +1660,272 @@ int btSoftBody::generateClusters(int k,int maxiterations) initializeClusters(); updateClusters(); - //for self-collision - m_clusterConnectivity.resize(m_clusters.size()*m_clusters.size()); + m_clusterConnectivity.resize(m_clusters.size() * m_clusters.size()); { - for (int c0=0;c0<m_clusters.size();c0++) + for (int c0 = 0; c0 < m_clusters.size(); c0++) { - m_clusters[c0]->m_clusterIndex=c0; - for (int c1=0;c1<m_clusters.size();c1++) + m_clusters[c0]->m_clusterIndex = c0; + for (int c1 = 0; c1 < m_clusters.size(); c1++) { - - bool connected=false; + bool connected = false; Cluster* cla = m_clusters[c0]; Cluster* clb = m_clusters[c1]; - for (int i=0;!connected&&i<cla->m_nodes.size();i++) + for (int i = 0; !connected && i < cla->m_nodes.size(); i++) { - for (int j=0;j<clb->m_nodes.size();j++) + for (int j = 0; j < clb->m_nodes.size(); j++) { if (cla->m_nodes[i] == clb->m_nodes[j]) { - connected=true; + connected = true; break; } } } - m_clusterConnectivity[c0+c1*m_clusters.size()]=connected; + m_clusterConnectivity[c0 + c1 * m_clusters.size()] = connected; } } } } - return(m_clusters.size()); + return (m_clusters.size()); } // -void btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut) +void btSoftBody::refine(ImplicitFn* ifn, btScalar accurary, bool cut) { - const Node* nbase = &m_nodes[0]; - int ncount = m_nodes.size(); - btSymMatrix<int> edges(ncount,-2); - int newnodes=0; - int i,j,k,ni; - - /* Filter out */ - for(i=0;i<m_links.size();++i) + const Node* nbase = &m_nodes[0]; + int ncount = m_nodes.size(); + btSymMatrix<int> edges(ncount, -2); + int newnodes = 0; + int i, j, k, ni; + + /* Filter out */ + for (i = 0; i < m_links.size(); ++i) { - Link& l=m_links[i]; - if(l.m_bbending) + Link& l = m_links[i]; + if (l.m_bbending) { - if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x))) + if (!SameSign(ifn->Eval(l.m_n[0]->m_x), ifn->Eval(l.m_n[1]->m_x))) { - btSwap(m_links[i],m_links[m_links.size()-1]); - m_links.pop_back();--i; + btSwap(m_links[i], m_links[m_links.size() - 1]); + m_links.pop_back(); + --i; } - } + } } - /* Fill edges */ - for(i=0;i<m_links.size();++i) + /* Fill edges */ + for (i = 0; i < m_links.size(); ++i) { - Link& l=m_links[i]; - edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1; + Link& l = m_links[i]; + edges(int(l.m_n[0] - nbase), int(l.m_n[1] - nbase)) = -1; } - for(i=0;i<m_faces.size();++i) - { - Face& f=m_faces[i]; - edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1; - edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1; - edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1; + for (i = 0; i < m_faces.size(); ++i) + { + Face& f = m_faces[i]; + edges(int(f.m_n[0] - nbase), int(f.m_n[1] - nbase)) = -1; + edges(int(f.m_n[1] - nbase), int(f.m_n[2] - nbase)) = -1; + edges(int(f.m_n[2] - nbase), int(f.m_n[0] - nbase)) = -1; } - /* Intersect */ - for(i=0;i<ncount;++i) + /* Intersect */ + for (i = 0; i < ncount; ++i) { - for(j=i+1;j<ncount;++j) + for (j = i + 1; j < ncount; ++j) { - if(edges(i,j)==-1) + if (edges(i, j) == -1) { - Node& a=m_nodes[i]; - Node& b=m_nodes[j]; - const btScalar t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary); - if(t>0) + Node& a = m_nodes[i]; + Node& b = m_nodes[j]; + const btScalar t = ImplicitSolve(ifn, a.m_x, b.m_x, accurary); + if (t > 0) { - const btVector3 x=Lerp(a.m_x,b.m_x,t); - const btVector3 v=Lerp(a.m_v,b.m_v,t); - btScalar m=0; - if(a.m_im>0) + const btVector3 x = Lerp(a.m_x, b.m_x, t); + const btVector3 v = Lerp(a.m_v, b.m_v, t); + btScalar m = 0; + if (a.m_im > 0) { - if(b.m_im>0) + if (b.m_im > 0) { - const btScalar ma=1/a.m_im; - const btScalar mb=1/b.m_im; - const btScalar mc=Lerp(ma,mb,t); - const btScalar f=(ma+mb)/(ma+mb+mc); - a.m_im=1/(ma*f); - b.m_im=1/(mb*f); - m=mc*f; + const btScalar ma = 1 / a.m_im; + const btScalar mb = 1 / b.m_im; + const btScalar mc = Lerp(ma, mb, t); + const btScalar f = (ma + mb) / (ma + mb + mc); + a.m_im = 1 / (ma * f); + b.m_im = 1 / (mb * f); + m = mc * f; } else - { a.m_im/=0.5f;m=1/a.m_im; } + { + a.m_im /= 0.5f; + m = 1 / a.m_im; + } } else { - if(b.m_im>0) - { b.m_im/=0.5f;m=1/b.m_im; } + if (b.m_im > 0) + { + b.m_im /= 0.5f; + m = 1 / b.m_im; + } else - m=0; + m = 0; } - appendNode(x,m); - edges(i,j)=m_nodes.size()-1; - m_nodes[edges(i,j)].m_v=v; + appendNode(x, m); + edges(i, j) = m_nodes.size() - 1; + m_nodes[edges(i, j)].m_v = v; ++newnodes; } } } } - nbase=&m_nodes[0]; - /* Refine links */ - for(i=0,ni=m_links.size();i<ni;++i) + nbase = &m_nodes[0]; + /* Refine links */ + for (i = 0, ni = m_links.size(); i < ni; ++i) { - Link& feat=m_links[i]; - const int idx[]={ int(feat.m_n[0]-nbase), - int(feat.m_n[1]-nbase)}; - if((idx[0]<ncount)&&(idx[1]<ncount)) + Link& feat = m_links[i]; + const int idx[] = {int(feat.m_n[0] - nbase), + int(feat.m_n[1] - nbase)}; + if ((idx[0] < ncount) && (idx[1] < ncount)) { - const int ni=edges(idx[0],idx[1]); - if(ni>0) + const int ni = edges(idx[0], idx[1]); + if (ni > 0) { appendLink(i); - Link* pft[]={ &m_links[i], - &m_links[m_links.size()-1]}; - pft[0]->m_n[0]=&m_nodes[idx[0]]; - pft[0]->m_n[1]=&m_nodes[ni]; - pft[1]->m_n[0]=&m_nodes[ni]; - pft[1]->m_n[1]=&m_nodes[idx[1]]; + Link* pft[] = {&m_links[i], + &m_links[m_links.size() - 1]}; + pft[0]->m_n[0] = &m_nodes[idx[0]]; + pft[0]->m_n[1] = &m_nodes[ni]; + pft[1]->m_n[0] = &m_nodes[ni]; + pft[1]->m_n[1] = &m_nodes[idx[1]]; } } } - /* Refine faces */ - for(i=0;i<m_faces.size();++i) + /* Refine faces */ + for (i = 0; i < m_faces.size(); ++i) { - const Face& feat=m_faces[i]; - const int idx[]={ int(feat.m_n[0]-nbase), - int(feat.m_n[1]-nbase), - int(feat.m_n[2]-nbase)}; - for(j=2,k=0;k<3;j=k++) + const Face& feat = m_faces[i]; + const int idx[] = {int(feat.m_n[0] - nbase), + int(feat.m_n[1] - nbase), + int(feat.m_n[2] - nbase)}; + for (j = 2, k = 0; k < 3; j = k++) { - if((idx[j]<ncount)&&(idx[k]<ncount)) + if ((idx[j] < ncount) && (idx[k] < ncount)) { - const int ni=edges(idx[j],idx[k]); - if(ni>0) + const int ni = edges(idx[j], idx[k]); + if (ni > 0) { appendFace(i); - const int l=(k+1)%3; - Face* pft[]={ &m_faces[i], - &m_faces[m_faces.size()-1]}; - pft[0]->m_n[0]=&m_nodes[idx[l]]; - pft[0]->m_n[1]=&m_nodes[idx[j]]; - pft[0]->m_n[2]=&m_nodes[ni]; - pft[1]->m_n[0]=&m_nodes[ni]; - pft[1]->m_n[1]=&m_nodes[idx[k]]; - pft[1]->m_n[2]=&m_nodes[idx[l]]; - appendLink(ni,idx[l],pft[0]->m_material); - --i;break; + const int l = (k + 1) % 3; + Face* pft[] = {&m_faces[i], + &m_faces[m_faces.size() - 1]}; + pft[0]->m_n[0] = &m_nodes[idx[l]]; + pft[0]->m_n[1] = &m_nodes[idx[j]]; + pft[0]->m_n[2] = &m_nodes[ni]; + pft[1]->m_n[0] = &m_nodes[ni]; + pft[1]->m_n[1] = &m_nodes[idx[k]]; + pft[1]->m_n[2] = &m_nodes[idx[l]]; + appendLink(ni, idx[l], pft[0]->m_material); + --i; + break; } } } } - /* Cut */ - if(cut) - { - btAlignedObjectArray<int> cnodes; - const int pcount=ncount; - int i; - ncount=m_nodes.size(); - cnodes.resize(ncount,0); - /* Nodes */ - for(i=0;i<ncount;++i) + /* Cut */ + if (cut) + { + btAlignedObjectArray<int> cnodes; + const int pcount = ncount; + int i; + ncount = m_nodes.size(); + cnodes.resize(ncount, 0); + /* Nodes */ + for (i = 0; i < ncount; ++i) { - const btVector3 x=m_nodes[i].m_x; - if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary)) + const btVector3 x = m_nodes[i].m_x; + if ((i >= pcount) || (btFabs(ifn->Eval(x)) < accurary)) { - const btVector3 v=m_nodes[i].m_v; - btScalar m=getMass(i); - if(m>0) { m*=0.5f;m_nodes[i].m_im/=0.5f; } - appendNode(x,m); - cnodes[i]=m_nodes.size()-1; - m_nodes[cnodes[i]].m_v=v; + const btVector3 v = m_nodes[i].m_v; + btScalar m = getMass(i); + if (m > 0) + { + m *= 0.5f; + m_nodes[i].m_im /= 0.5f; + } + appendNode(x, m); + cnodes[i] = m_nodes.size() - 1; + m_nodes[cnodes[i]].m_v = v; } } - nbase=&m_nodes[0]; - /* Links */ - for(i=0,ni=m_links.size();i<ni;++i) + nbase = &m_nodes[0]; + /* Links */ + for (i = 0, ni = m_links.size(); i < ni; ++i) { - const int id[]={ int(m_links[i].m_n[0]-nbase), - int(m_links[i].m_n[1]-nbase)}; - int todetach=0; - if(cnodes[id[0]]&&cnodes[id[1]]) + const int id[] = {int(m_links[i].m_n[0] - nbase), + int(m_links[i].m_n[1] - nbase)}; + int todetach = 0; + if (cnodes[id[0]] && cnodes[id[1]]) { appendLink(i); - todetach=m_links.size()-1; + todetach = m_links.size() - 1; } else { - if(( (ifn->Eval(m_nodes[id[0]].m_x)<accurary)&& - (ifn->Eval(m_nodes[id[1]].m_x)<accurary))) - todetach=i; + if (((ifn->Eval(m_nodes[id[0]].m_x) < accurary) && + (ifn->Eval(m_nodes[id[1]].m_x) < accurary))) + todetach = i; } - if(todetach) + if (todetach) { - Link& l=m_links[todetach]; - for(int j=0;j<2;++j) + Link& l = m_links[todetach]; + for (int j = 0; j < 2; ++j) { - int cn=cnodes[int(l.m_n[j]-nbase)]; - if(cn) l.m_n[j]=&m_nodes[cn]; - } + int cn = cnodes[int(l.m_n[j] - nbase)]; + if (cn) l.m_n[j] = &m_nodes[cn]; + } } } - /* Faces */ - for(i=0,ni=m_faces.size();i<ni;++i) + /* Faces */ + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - Node** n= m_faces[i].m_n; - if( (ifn->Eval(n[0]->m_x)<accurary)&& - (ifn->Eval(n[1]->m_x)<accurary)&& - (ifn->Eval(n[2]->m_x)<accurary)) + Node** n = m_faces[i].m_n; + if ((ifn->Eval(n[0]->m_x) < accurary) && + (ifn->Eval(n[1]->m_x) < accurary) && + (ifn->Eval(n[2]->m_x) < accurary)) { - for(int j=0;j<3;++j) + for (int j = 0; j < 3; ++j) { - int cn=cnodes[int(n[j]-nbase)]; - if(cn) n[j]=&m_nodes[cn]; + int cn = cnodes[int(n[j] - nbase)]; + if (cn) n[j] = &m_nodes[cn]; } } } - /* Clean orphans */ - int nnodes=m_nodes.size(); - btAlignedObjectArray<int> ranks; - btAlignedObjectArray<int> todelete; - ranks.resize(nnodes,0); - for(i=0,ni=m_links.size();i<ni;++i) + /* Clean orphans */ + int nnodes = m_nodes.size(); + btAlignedObjectArray<int> ranks; + btAlignedObjectArray<int> todelete; + ranks.resize(nnodes, 0); + for (i = 0, ni = m_links.size(); i < ni; ++i) { - for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++; + for (int j = 0; j < 2; ++j) ranks[int(m_links[i].m_n[j] - nbase)]++; } - for(i=0,ni=m_faces.size();i<ni;++i) + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++; + for (int j = 0; j < 3; ++j) ranks[int(m_faces[i].m_n[j] - nbase)]++; } - for(i=0;i<m_links.size();++i) + for (i = 0; i < m_links.size(); ++i) { - const int id[]={ int(m_links[i].m_n[0]-nbase), - int(m_links[i].m_n[1]-nbase)}; - const bool sg[]={ ranks[id[0]]==1, - ranks[id[1]]==1}; - if(sg[0]||sg[1]) + const int id[] = {int(m_links[i].m_n[0] - nbase), + int(m_links[i].m_n[1] - nbase)}; + const bool sg[] = {ranks[id[0]] == 1, + ranks[id[1]] == 1}; + if (sg[0] || sg[1]) { --ranks[id[0]]; --ranks[id[1]]; - btSwap(m_links[i],m_links[m_links.size()-1]); - m_links.pop_back();--i; + btSwap(m_links[i], m_links[m_links.size() - 1]); + m_links.pop_back(); + --i; } } #if 0 @@ -1629,679 +1952,999 @@ void btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut) } #endif } - m_bUpdateRtCst=true; + m_bUpdateRtCst = true; } // -bool btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position) +bool btSoftBody::cutLink(const Node* node0, const Node* node1, btScalar position) { - return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position)); + return (cutLink(int(node0 - &m_nodes[0]), int(node1 - &m_nodes[0]), position)); } // -bool btSoftBody::cutLink(int node0,int node1,btScalar position) +bool btSoftBody::cutLink(int node0, int node1, btScalar position) { - bool done=false; - int i,ni; -// const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x; - const btVector3 x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position); - const btVector3 v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position); - const btScalar m=1; - appendNode(x,m); - appendNode(x,m); - Node* pa=&m_nodes[node0]; - Node* pb=&m_nodes[node1]; - Node* pn[2]={ &m_nodes[m_nodes.size()-2], - &m_nodes[m_nodes.size()-1]}; - pn[0]->m_v=v; - pn[1]->m_v=v; - for(i=0,ni=m_links.size();i<ni;++i) + bool done = false; + int i, ni; + // const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x; + const btVector3 x = Lerp(m_nodes[node0].m_x, m_nodes[node1].m_x, position); + const btVector3 v = Lerp(m_nodes[node0].m_v, m_nodes[node1].m_v, position); + const btScalar m = 1; + appendNode(x, m); + appendNode(x, m); + Node* pa = &m_nodes[node0]; + Node* pb = &m_nodes[node1]; + Node* pn[2] = {&m_nodes[m_nodes.size() - 2], + &m_nodes[m_nodes.size() - 1]}; + pn[0]->m_v = v; + pn[1]->m_v = v; + for (i = 0, ni = m_links.size(); i < ni; ++i) { - const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb); - if(mtch!=-1) + const int mtch = MatchEdge(m_links[i].m_n[0], m_links[i].m_n[1], pa, pb); + if (mtch != -1) { appendLink(i); - Link* pft[]={&m_links[i],&m_links[m_links.size()-1]}; - pft[0]->m_n[1]=pn[mtch]; - pft[1]->m_n[0]=pn[1-mtch]; - done=true; + Link* pft[] = {&m_links[i], &m_links[m_links.size() - 1]}; + pft[0]->m_n[1] = pn[mtch]; + pft[1]->m_n[0] = pn[1 - mtch]; + done = true; } } - for(i=0,ni=m_faces.size();i<ni;++i) + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - for(int k=2,l=0;l<3;k=l++) + for (int k = 2, l = 0; l < 3; k = l++) { - const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb); - if(mtch!=-1) + const int mtch = MatchEdge(m_faces[i].m_n[k], m_faces[i].m_n[l], pa, pb); + if (mtch != -1) { appendFace(i); - Face* pft[]={&m_faces[i],&m_faces[m_faces.size()-1]}; - pft[0]->m_n[l]=pn[mtch]; - pft[1]->m_n[k]=pn[1-mtch]; - appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); - appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); + Face* pft[] = {&m_faces[i], &m_faces[m_faces.size() - 1]}; + pft[0]->m_n[l] = pn[mtch]; + pft[1]->m_n[k] = pn[1 - mtch]; + appendLink(pn[0], pft[0]->m_n[(l + 1) % 3], pft[0]->m_material, true); + appendLink(pn[1], pft[0]->m_n[(l + 1) % 3], pft[0]->m_material, true); } } } - if(!done) + if (!done) { m_ndbvt.remove(pn[0]->m_leaf); m_ndbvt.remove(pn[1]->m_leaf); m_nodes.pop_back(); m_nodes.pop_back(); } - return(done); + return (done); } // -bool btSoftBody::rayTest(const btVector3& rayFrom, - const btVector3& rayTo, - sRayCast& results) +bool btSoftBody::rayTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results) { - if(m_faces.size()&&m_fdbvt.empty()) + if (m_faces.size() && m_fdbvt.empty()) initializeFaceTree(); - results.body = this; + results.body = this; results.fraction = 1.f; - results.feature = eFeature::None; - results.index = -1; + results.feature = eFeature::None; + results.index = -1; - return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0); + return (rayTest(rayFrom, rayTo, results.fraction, results.feature, results.index, false) != 0); +} + +bool btSoftBody::rayFaceTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results) +{ + if (m_faces.size() == 0) + return false; + else + { + if (m_fdbvt.empty()) + initializeFaceTree(); + } + + results.body = this; + results.fraction = 1.f; + results.index = -1; + + return (rayFaceTest(rayFrom, rayTo, results.fraction, results.index) != 0); } // -void btSoftBody::setSolver(eSolverPresets::_ preset) +void btSoftBody::setSolver(eSolverPresets::_ preset) { m_cfg.m_vsequence.clear(); m_cfg.m_psequence.clear(); m_cfg.m_dsequence.clear(); - switch(preset) - { - case eSolverPresets::Positions: - m_cfg.m_psequence.push_back(ePSolver::Anchors); - m_cfg.m_psequence.push_back(ePSolver::RContacts); - m_cfg.m_psequence.push_back(ePSolver::SContacts); - m_cfg.m_psequence.push_back(ePSolver::Linear); - break; - case eSolverPresets::Velocities: - m_cfg.m_vsequence.push_back(eVSolver::Linear); - - m_cfg.m_psequence.push_back(ePSolver::Anchors); - m_cfg.m_psequence.push_back(ePSolver::RContacts); - m_cfg.m_psequence.push_back(ePSolver::SContacts); - - m_cfg.m_dsequence.push_back(ePSolver::Linear); - break; + switch (preset) + { + case eSolverPresets::Positions: + m_cfg.m_psequence.push_back(ePSolver::Anchors); + m_cfg.m_psequence.push_back(ePSolver::RContacts); + m_cfg.m_psequence.push_back(ePSolver::SContacts); + m_cfg.m_psequence.push_back(ePSolver::Linear); + break; + case eSolverPresets::Velocities: + m_cfg.m_vsequence.push_back(eVSolver::Linear); + + m_cfg.m_psequence.push_back(ePSolver::Anchors); + m_cfg.m_psequence.push_back(ePSolver::RContacts); + m_cfg.m_psequence.push_back(ePSolver::SContacts); + + m_cfg.m_dsequence.push_back(ePSolver::Linear); + break; } } -// -void btSoftBody::predictMotion(btScalar dt) +void btSoftBody::predictMotion(btScalar dt) { + int i, ni; - int i,ni; - - /* Update */ - if(m_bUpdateRtCst) + /* Update */ + if (m_bUpdateRtCst) { - m_bUpdateRtCst=false; + m_bUpdateRtCst = false; updateConstants(); m_fdbvt.clear(); - if(m_cfg.collisions&fCollision::VF_SS) + if (m_cfg.collisions & fCollision::VF_SS) { - initializeFaceTree(); + initializeFaceTree(); } } - /* Prepare */ - m_sst.sdt = dt*m_cfg.timescale; - m_sst.isdt = 1/m_sst.sdt; - m_sst.velmrg = m_sst.sdt*3; - m_sst.radmrg = getCollisionShape()->getMargin(); - m_sst.updmrg = m_sst.radmrg*(btScalar)0.25; - /* Forces */ - addVelocity(m_worldInfo->m_gravity*m_sst.sdt); + /* Prepare */ + m_sst.sdt = dt * m_cfg.timescale; + m_sst.isdt = 1 / m_sst.sdt; + m_sst.velmrg = m_sst.sdt * 3; + m_sst.radmrg = getCollisionShape()->getMargin(); + m_sst.updmrg = m_sst.radmrg * (btScalar)0.25; + /* Forces */ + addVelocity(m_worldInfo->m_gravity * m_sst.sdt); applyForces(); - /* Integrate */ - for(i=0,ni=m_nodes.size();i<ni;++i) + /* Integrate */ + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - n.m_q = n.m_x; - btVector3 deltaV = n.m_f*n.m_im*m_sst.sdt; + Node& n = m_nodes[i]; + n.m_q = n.m_x; + btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt; { btScalar maxDisplacement = m_worldInfo->m_maxDisplacement; - btScalar clampDeltaV = maxDisplacement/m_sst.sdt; - for (int c=0;c<3;c++) + btScalar clampDeltaV = maxDisplacement / m_sst.sdt; + for (int c = 0; c < 3; c++) { - if (deltaV[c]>clampDeltaV) + if (deltaV[c] > clampDeltaV) { deltaV[c] = clampDeltaV; } - if (deltaV[c]<-clampDeltaV) + if (deltaV[c] < -clampDeltaV) { - deltaV[c]=-clampDeltaV; + deltaV[c] = -clampDeltaV; } } } - n.m_v += deltaV; - n.m_x += n.m_v*m_sst.sdt; - n.m_f = btVector3(0,0,0); + n.m_v += deltaV; + n.m_x += n.m_v * m_sst.sdt; + n.m_f = btVector3(0, 0, 0); } - /* Clusters */ + /* Clusters */ updateClusters(); - /* Bounds */ - updateBounds(); - /* Nodes */ - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; - for(i=0,ni=m_nodes.size();i<ni;++i) - { - Node& n=m_nodes[i]; - vol = btDbvtVolume::FromCR(n.m_x,m_sst.radmrg); - m_ndbvt.update( n.m_leaf, - vol, - n.m_v*m_sst.velmrg, - m_sst.updmrg); - } - /* Faces */ - if(!m_fdbvt.empty()) - { - for(int i=0;i<m_faces.size();++i) - { - Face& f=m_faces[i]; - const btVector3 v=( f.m_n[0]->m_v+ - f.m_n[1]->m_v+ - f.m_n[2]->m_v)/3; - vol = VolumeOf(f,m_sst.radmrg); - m_fdbvt.update( f.m_leaf, - vol, - v*m_sst.velmrg, - m_sst.updmrg); - } - } - /* Pose */ + /* Bounds */ + updateBounds(); + /* Nodes */ + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + for (i = 0, ni = m_nodes.size(); i < ni; ++i) + { + Node& n = m_nodes[i]; + vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg); + m_ndbvt.update(n.m_leaf, + vol, + n.m_v * m_sst.velmrg, + m_sst.updmrg); + } + /* Faces */ + if (!m_fdbvt.empty()) + { + for (int i = 0; i < m_faces.size(); ++i) + { + Face& f = m_faces[i]; + const btVector3 v = (f.m_n[0]->m_v + + f.m_n[1]->m_v + + f.m_n[2]->m_v) / + 3; + vol = VolumeOf(f, m_sst.radmrg); + m_fdbvt.update(f.m_leaf, + vol, + v * m_sst.velmrg, + m_sst.updmrg); + } + } + /* Pose */ updatePose(); - /* Match */ - if(m_pose.m_bframe&&(m_cfg.kMT>0)) + /* Match */ + if (m_pose.m_bframe && (m_cfg.kMT > 0)) { - const btMatrix3x3 posetrs=m_pose.m_rot; - for(int i=0,ni=m_nodes.size();i<ni;++i) + const btMatrix3x3 posetrs = m_pose.m_rot; + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - if(n.m_im>0) + Node& n = m_nodes[i]; + if (n.m_im > 0) { - const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com; - n.m_x=Lerp(n.m_x,x,m_cfg.kMT); + const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com; + n.m_x = Lerp(n.m_x, x, m_cfg.kMT); } } } - /* Clear contacts */ + /* Clear contacts */ m_rcontacts.resize(0); m_scontacts.resize(0); - /* Optimize dbvt's */ + /* Optimize dbvt's */ m_ndbvt.optimizeIncremental(1); m_fdbvt.optimizeIncremental(1); m_cdbvt.optimizeIncremental(1); } // -void btSoftBody::solveConstraints() +void btSoftBody::solveConstraints() { - - /* Apply clusters */ + /* Apply clusters */ applyClusters(false); - /* Prepare links */ + /* Prepare links */ - int i,ni; + int i, ni; - for(i=0,ni=m_links.size();i<ni;++i) + for (i = 0, ni = m_links.size(); i < ni; ++i) { - Link& l=m_links[i]; - l.m_c3 = l.m_n[1]->m_q-l.m_n[0]->m_q; - l.m_c2 = 1/(l.m_c3.length2()*l.m_c0); + Link& l = m_links[i]; + l.m_c3 = l.m_n[1]->m_q - l.m_n[0]->m_q; + l.m_c2 = 1 / (l.m_c3.length2() * l.m_c0); } - /* Prepare anchors */ - for(i=0,ni=m_anchors.size();i<ni;++i) + /* Prepare anchors */ + for (i = 0, ni = m_anchors.size(); i < ni; ++i) { - Anchor& a=m_anchors[i]; - const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local; - a.m_c0 = ImpulseMatrix( m_sst.sdt, - a.m_node->m_im, - a.m_body->getInvMass(), - a.m_body->getInvInertiaTensorWorld(), - ra); - a.m_c1 = ra; - a.m_c2 = m_sst.sdt*a.m_node->m_im; + Anchor& a = m_anchors[i]; + const btVector3 ra = a.m_body->getWorldTransform().getBasis() * a.m_local; + a.m_c0 = ImpulseMatrix(m_sst.sdt, + a.m_node->m_im, + a.m_body->getInvMass(), + a.m_body->getInvInertiaTensorWorld(), + ra); + a.m_c1 = ra; + a.m_c2 = m_sst.sdt * a.m_node->m_im; a.m_body->activate(); } - /* Solve velocities */ - if(m_cfg.viterations>0) + /* Solve velocities */ + if (m_cfg.viterations > 0) { - /* Solve */ - for(int isolve=0;isolve<m_cfg.viterations;++isolve) + /* Solve */ + for (int isolve = 0; isolve < m_cfg.viterations; ++isolve) { - for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq) + for (int iseq = 0; iseq < m_cfg.m_vsequence.size(); ++iseq) { - getSolver(m_cfg.m_vsequence[iseq])(this,1); + getSolver(m_cfg.m_vsequence[iseq])(this, 1); } } - /* Update */ - for(i=0,ni=m_nodes.size();i<ni;++i) + /* Update */ + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - n.m_x = n.m_q+n.m_v*m_sst.sdt; + Node& n = m_nodes[i]; + n.m_x = n.m_q + n.m_v * m_sst.sdt; } } - /* Solve positions */ - if(m_cfg.piterations>0) + /* Solve positions */ + if (m_cfg.piterations > 0) { - for(int isolve=0;isolve<m_cfg.piterations;++isolve) + for (int isolve = 0; isolve < m_cfg.piterations; ++isolve) { - const btScalar ti=isolve/(btScalar)m_cfg.piterations; - for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq) + const btScalar ti = isolve / (btScalar)m_cfg.piterations; + for (int iseq = 0; iseq < m_cfg.m_psequence.size(); ++iseq) { - getSolver(m_cfg.m_psequence[iseq])(this,1,ti); + getSolver(m_cfg.m_psequence[iseq])(this, 1, ti); } } - const btScalar vc=m_sst.isdt*(1-m_cfg.kDP); - for(i=0,ni=m_nodes.size();i<ni;++i) + const btScalar vc = m_sst.isdt * (1 - m_cfg.kDP); + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - n.m_v = (n.m_x-n.m_q)*vc; - n.m_f = btVector3(0,0,0); + Node& n = m_nodes[i]; + n.m_v = (n.m_x - n.m_q) * vc; + n.m_f = btVector3(0, 0, 0); } } - /* Solve drift */ - if(m_cfg.diterations>0) + /* Solve drift */ + if (m_cfg.diterations > 0) { - const btScalar vcf=m_cfg.kVCF*m_sst.isdt; - for(i=0,ni=m_nodes.size();i<ni;++i) + const btScalar vcf = m_cfg.kVCF * m_sst.isdt; + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - n.m_q = n.m_x; + Node& n = m_nodes[i]; + n.m_q = n.m_x; } - for(int idrift=0;idrift<m_cfg.diterations;++idrift) + for (int idrift = 0; idrift < m_cfg.diterations; ++idrift) { - for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq) + for (int iseq = 0; iseq < m_cfg.m_dsequence.size(); ++iseq) { - getSolver(m_cfg.m_dsequence[iseq])(this,1,0); + getSolver(m_cfg.m_dsequence[iseq])(this, 1, 0); } } - for(int i=0,ni=m_nodes.size();i<ni;++i) + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - n.m_v += (n.m_x-n.m_q)*vcf; + Node& n = m_nodes[i]; + n.m_v += (n.m_x - n.m_q) * vcf; } } - /* Apply clusters */ + /* Apply clusters */ dampClusters(); applyClusters(true); } // -void btSoftBody::staticSolve(int iterations) +void btSoftBody::staticSolve(int iterations) { - for(int isolve=0;isolve<iterations;++isolve) + for (int isolve = 0; isolve < iterations; ++isolve) { - for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq) + for (int iseq = 0; iseq < m_cfg.m_psequence.size(); ++iseq) { - getSolver(m_cfg.m_psequence[iseq])(this,1,0); + getSolver(m_cfg.m_psequence[iseq])(this, 1, 0); } } } // -void btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/) +void btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/, int /*count*/, int /*iterations*/) { /// placeholder } // -void btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies) +void btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies) { - const int nb=bodies.size(); - int iterations=0; + const int nb = bodies.size(); + int iterations = 0; int i; - for(i=0;i<nb;++i) + for (i = 0; i < nb; ++i) { - iterations=btMax(iterations,bodies[i]->m_cfg.citerations); + iterations = btMax(iterations, bodies[i]->m_cfg.citerations); } - for(i=0;i<nb;++i) + for (i = 0; i < nb; ++i) { bodies[i]->prepareClusters(iterations); } - for(i=0;i<iterations;++i) + for (i = 0; i < iterations; ++i) { - const btScalar sor=1; - for(int j=0;j<nb;++j) + const btScalar sor = 1; + for (int j = 0; j < nb; ++j) { bodies[j]->solveClusters(sor); } } - for(i=0;i<nb;++i) + for (i = 0; i < nb; ++i) { bodies[i]->cleanupClusters(); } } // -void btSoftBody::integrateMotion() +void btSoftBody::integrateMotion() { - /* Update */ + /* Update */ updateNormals(); } // -btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt) +btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom, const btVector3& rayTo, btScalar mxt) { m_rayFrom = rayFrom; - m_rayNormalizedDirection = (rayTo-rayFrom); + m_rayNormalizedDirection = (rayTo - rayFrom); m_rayTo = rayTo; - m_mint = mxt; - m_face = 0; - m_tests = 0; + m_mint = mxt; + m_face = 0; + m_tests = 0; } // -void btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf) +void btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf) { - btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data; - const btScalar t=rayFromToTriangle( m_rayFrom,m_rayTo,m_rayNormalizedDirection, - f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - m_mint); - if((t>0)&&(t<m_mint)) - { - m_mint=t;m_face=&f; + btSoftBody::Face& f = *(btSoftBody::Face*)leaf->data; + const btScalar t = rayFromToTriangle(m_rayFrom, m_rayTo, m_rayNormalizedDirection, + f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + m_mint); + if ((t > 0) && (t < m_mint)) + { + m_mint = t; + m_face = &f; } ++m_tests; } // -btScalar btSoftBody::RayFromToCaster::rayFromToTriangle( const btVector3& rayFrom, - const btVector3& rayTo, - const btVector3& rayNormalizedDirection, - const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar maxt) +btScalar btSoftBody::RayFromToCaster::rayFromToTriangle(const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& rayNormalizedDirection, + const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar maxt) { - static const btScalar ceps=-SIMD_EPSILON*10; - static const btScalar teps=SIMD_EPSILON*10; + static const btScalar ceps = -SIMD_EPSILON * 10; + static const btScalar teps = SIMD_EPSILON * 10; - const btVector3 n=btCross(b-a,c-a); - const btScalar d=btDot(a,n); - const btScalar den=btDot(rayNormalizedDirection,n); - if(!btFuzzyZero(den)) + const btVector3 n = btCross(b - a, c - a); + const btScalar d = btDot(a, n); + const btScalar den = btDot(rayNormalizedDirection, n); + if (!btFuzzyZero(den)) { - const btScalar num=btDot(rayFrom,n)-d; - const btScalar t=-num/den; - if((t>teps)&&(t<maxt)) + const btScalar num = btDot(rayFrom, n) - d; + const btScalar t = -num / den; + if ((t > teps) && (t < maxt)) { - const btVector3 hit=rayFrom+rayNormalizedDirection*t; - if( (btDot(n,btCross(a-hit,b-hit))>ceps) && - (btDot(n,btCross(b-hit,c-hit))>ceps) && - (btDot(n,btCross(c-hit,a-hit))>ceps)) + const btVector3 hit = rayFrom + rayNormalizedDirection * t; + if ((btDot(n, btCross(a - hit, b - hit)) > ceps) && + (btDot(n, btCross(b - hit, c - hit)) > ceps) && + (btDot(n, btCross(c - hit, a - hit)) > ceps)) { - return(t); + return (t); } } } - return(-1); + return (-1); } // -void btSoftBody::pointersToIndices() +void btSoftBody::pointersToIndices() { -#define PTR2IDX(_p_,_b_) reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_)) - btSoftBody::Node* base=m_nodes.size() ? &m_nodes[0] : 0; - int i,ni; +#define PTR2IDX(_p_, _b_) reinterpret_cast<btSoftBody::Node*>((_p_) - (_b_)) + btSoftBody::Node* base = m_nodes.size() ? &m_nodes[0] : 0; + int i, ni; - for(i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - if(m_nodes[i].m_leaf) + if (m_nodes[i].m_leaf) { - m_nodes[i].m_leaf->data=*(void**)&i; + m_nodes[i].m_leaf->data = *(void**)&i; } } - for(i=0,ni=m_links.size();i<ni;++i) + for (i = 0, ni = m_links.size(); i < ni; ++i) { - m_links[i].m_n[0]=PTR2IDX(m_links[i].m_n[0],base); - m_links[i].m_n[1]=PTR2IDX(m_links[i].m_n[1],base); + m_links[i].m_n[0] = PTR2IDX(m_links[i].m_n[0], base); + m_links[i].m_n[1] = PTR2IDX(m_links[i].m_n[1], base); } - for(i=0,ni=m_faces.size();i<ni;++i) + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - m_faces[i].m_n[0]=PTR2IDX(m_faces[i].m_n[0],base); - m_faces[i].m_n[1]=PTR2IDX(m_faces[i].m_n[1],base); - m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base); - if(m_faces[i].m_leaf) + m_faces[i].m_n[0] = PTR2IDX(m_faces[i].m_n[0], base); + m_faces[i].m_n[1] = PTR2IDX(m_faces[i].m_n[1], base); + m_faces[i].m_n[2] = PTR2IDX(m_faces[i].m_n[2], base); + if (m_faces[i].m_leaf) { - m_faces[i].m_leaf->data=*(void**)&i; + m_faces[i].m_leaf->data = *(void**)&i; } } - for(i=0,ni=m_anchors.size();i<ni;++i) + for (i = 0, ni = m_anchors.size(); i < ni; ++i) { - m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base); + m_anchors[i].m_node = PTR2IDX(m_anchors[i].m_node, base); } - for(i=0,ni=m_notes.size();i<ni;++i) + for (i = 0, ni = m_notes.size(); i < ni; ++i) { - for(int j=0;j<m_notes[i].m_rank;++j) + for (int j = 0; j < m_notes[i].m_rank; ++j) { - m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base); + m_notes[i].m_nodes[j] = PTR2IDX(m_notes[i].m_nodes[j], base); } } -#undef PTR2IDX +#undef PTR2IDX } // -void btSoftBody::indicesToPointers(const int* map) +void btSoftBody::indicesToPointers(const int* map) { -#define IDX2PTR(_p_,_b_) map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]): \ - (&(_b_)[(((char*)_p_)-(char*)0)]) - btSoftBody::Node* base=m_nodes.size() ? &m_nodes[0]:0; - int i,ni; +#define IDX2PTR(_p_, _b_) map ? (&(_b_)[map[(((char*)_p_) - (char*)0)]]) : (&(_b_)[(((char*)_p_) - (char*)0)]) + btSoftBody::Node* base = m_nodes.size() ? &m_nodes[0] : 0; + int i, ni; - for(i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - if(m_nodes[i].m_leaf) + if (m_nodes[i].m_leaf) { - m_nodes[i].m_leaf->data=&m_nodes[i]; + m_nodes[i].m_leaf->data = &m_nodes[i]; } } - for(i=0,ni=m_links.size();i<ni;++i) + for (i = 0, ni = m_links.size(); i < ni; ++i) { - m_links[i].m_n[0]=IDX2PTR(m_links[i].m_n[0],base); - m_links[i].m_n[1]=IDX2PTR(m_links[i].m_n[1],base); + m_links[i].m_n[0] = IDX2PTR(m_links[i].m_n[0], base); + m_links[i].m_n[1] = IDX2PTR(m_links[i].m_n[1], base); } - for(i=0,ni=m_faces.size();i<ni;++i) + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - m_faces[i].m_n[0]=IDX2PTR(m_faces[i].m_n[0],base); - m_faces[i].m_n[1]=IDX2PTR(m_faces[i].m_n[1],base); - m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base); - if(m_faces[i].m_leaf) + m_faces[i].m_n[0] = IDX2PTR(m_faces[i].m_n[0], base); + m_faces[i].m_n[1] = IDX2PTR(m_faces[i].m_n[1], base); + m_faces[i].m_n[2] = IDX2PTR(m_faces[i].m_n[2], base); + if (m_faces[i].m_leaf) { - m_faces[i].m_leaf->data=&m_faces[i]; + m_faces[i].m_leaf->data = &m_faces[i]; } } - for(i=0,ni=m_anchors.size();i<ni;++i) + for (i = 0, ni = m_anchors.size(); i < ni; ++i) { - m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base); + m_anchors[i].m_node = IDX2PTR(m_anchors[i].m_node, base); } - for(i=0,ni=m_notes.size();i<ni;++i) + for (i = 0, ni = m_notes.size(); i < ni; ++i) { - for(int j=0;j<m_notes[i].m_rank;++j) + for (int j = 0; j < m_notes[i].m_rank; ++j) { - m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base); + m_notes[i].m_nodes[j] = IDX2PTR(m_notes[i].m_nodes[j], base); } } -#undef IDX2PTR +#undef IDX2PTR } // -int btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo, - btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const +int btSoftBody::rayTest(const btVector3& rayFrom, const btVector3& rayTo, + btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const { - int cnt=0; - btVector3 dir = rayTo-rayFrom; - + int cnt = 0; + btVector3 dir = rayTo - rayFrom; - if(bcountonly||m_fdbvt.empty()) - {/* Full search */ - - for(int i=0,ni=m_faces.size();i<ni;++i) - { - const btSoftBody::Face& f=m_faces[i]; + if (bcountonly || m_fdbvt.empty()) + { /* Full search */ - const btScalar t=RayFromToCaster::rayFromToTriangle( rayFrom,rayTo,dir, - f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - mint); - if(t>0) + for (int i = 0, ni = m_faces.size(); i < ni; ++i) + { + const btSoftBody::Face& f = m_faces[i]; + + const btScalar t = RayFromToCaster::rayFromToTriangle(rayFrom, rayTo, dir, + f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + mint); + if (t > 0) { ++cnt; - if(!bcountonly) + if (!bcountonly) { - feature=btSoftBody::eFeature::Face; - index=i; - mint=t; + feature = btSoftBody::eFeature::Face; + index = i; + mint = t; } } } } else - {/* Use dbvt */ - RayFromToCaster collider(rayFrom,rayTo,mint); + { /* Use dbvt */ + RayFromToCaster collider(rayFrom, rayTo, mint); - btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider); - if(collider.m_face) + btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider); + if (collider.m_face) { - mint=collider.m_mint; - feature=btSoftBody::eFeature::Face; - index=(int)(collider.m_face-&m_faces[0]); - cnt=1; + mint = collider.m_mint; + feature = btSoftBody::eFeature::Face; + index = (int)(collider.m_face - &m_faces[0]); + cnt = 1; } } - for (int i=0;i<m_tetras.size();i++) + for (int i = 0; i < m_tetras.size(); i++) { const btSoftBody::Tetra& tet = m_tetras[i]; - int tetfaces[4][3] = {{0,1,2},{0,1,3},{1,2,3},{0,2,3}}; - for (int f=0;f<4;f++) + int tetfaces[4][3] = {{0, 1, 2}, {0, 1, 3}, {1, 2, 3}, {0, 2, 3}}; + for (int f = 0; f < 4; f++) { + int index0 = tetfaces[f][0]; + int index1 = tetfaces[f][1]; + int index2 = tetfaces[f][2]; + btVector3 v0 = tet.m_n[index0]->m_x; + btVector3 v1 = tet.m_n[index1]->m_x; + btVector3 v2 = tet.m_n[index2]->m_x; + + const btScalar t = RayFromToCaster::rayFromToTriangle(rayFrom, rayTo, dir, + v0, v1, v2, + mint); + if (t > 0) + { + ++cnt; + if (!bcountonly) + { + feature = btSoftBody::eFeature::Tetra; + index = i; + mint = t; + } + } + } + } + return (cnt); +} - int index0=tetfaces[f][0]; - int index1=tetfaces[f][1]; - int index2=tetfaces[f][2]; - btVector3 v0=tet.m_n[index0]->m_x; - btVector3 v1=tet.m_n[index1]->m_x; - btVector3 v2=tet.m_n[index2]->m_x; +int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo, + btScalar& mint, int& index) const +{ + int cnt = 0; + { /* Use dbvt */ + RayFromToCaster collider(rayFrom, rayTo, mint); + btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider); + if (collider.m_face) + { + mint = collider.m_mint; + index = (int)(collider.m_face - &m_faces[0]); + cnt = 1; + } + } + return (cnt); +} + +// +static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n) +{ + if (n == 0) + return 0; + btDbvntNode* root = new btDbvntNode(n); + if (n->isinternal()) + { + btDbvntNode* c0 = copyToDbvnt(n->childs[0]); + root->childs[0] = c0; + btDbvntNode* c1 = copyToDbvnt(n->childs[1]); + root->childs[1] = c1; + } + return root; +} - const btScalar t=RayFromToCaster::rayFromToTriangle( rayFrom,rayTo,dir, - v0,v1,v2, - mint); - if(t>0) +static inline void calculateNormalCone(btDbvntNode* root) +{ + if (!root) + return; + if (root->isleaf()) + { + const btSoftBody::Face* face = (btSoftBody::Face*)root->data; + root->normal = face->m_normal; + root->angle = 0; + } + else + { + btVector3 n0(0, 0, 0), n1(0, 0, 0); + btScalar a0 = 0, a1 = 0; + if (root->childs[0]) + { + calculateNormalCone(root->childs[0]); + n0 = root->childs[0]->normal; + a0 = root->childs[0]->angle; + } + if (root->childs[1]) + { + calculateNormalCone(root->childs[1]); + n1 = root->childs[1]->normal; + a1 = root->childs[1]->angle; + } + root->normal = (n0 + n1).safeNormalize(); + root->angle = btMax(a0, a1) + btAngle(n0, n1) * 0.5; + } +} + +void btSoftBody::initializeFaceTree() +{ + BT_PROFILE("btSoftBody::initializeFaceTree"); + m_fdbvt.clear(); + // create leaf nodes; + btAlignedObjectArray<btDbvtNode*> leafNodes; + leafNodes.resize(m_faces.size()); + for (int i = 0; i < m_faces.size(); ++i) + { + Face& f = m_faces[i]; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol = VolumeOf(f, 0); + btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode(); + node->parent = NULL; + node->data = &f; + node->childs[1] = 0; + node->volume = vol; + leafNodes[i] = node; + f.m_leaf = node; + } + btAlignedObjectArray<btAlignedObjectArray<int> > adj; + adj.resize(m_faces.size()); + // construct the adjacency list for triangles + for (int i = 0; i < adj.size(); ++i) + { + for (int j = i + 1; j < adj.size(); ++j) + { + int dup = 0; + for (int k = 0; k < 3; ++k) { - ++cnt; - if(!bcountonly) + for (int l = 0; l < 3; ++l) { - feature=btSoftBody::eFeature::Tetra; - index=i; - mint=t; + if (m_faces[i].m_n[k] == m_faces[j].m_n[l]) + { + ++dup; + break; + } + } + if (dup == 2) + { + adj[i].push_back(j); + adj[j].push_back(i); } } } } - return(cnt); + m_fdbvt.m_root = buildTreeBottomUp(leafNodes, adj); + if (m_fdbvnt) + delete m_fdbvnt; + m_fdbvnt = copyToDbvnt(m_fdbvt.m_root); + updateFaceTree(false, false); + rebuildNodeTree(); } // -void btSoftBody::initializeFaceTree() +void btSoftBody::rebuildNodeTree() { - m_fdbvt.clear(); - for(int i=0;i<m_faces.size();++i) + m_ndbvt.clear(); + btAlignedObjectArray<btDbvtNode*> leafNodes; + leafNodes.resize(m_nodes.size()); + for (int i = 0; i < m_nodes.size(); ++i) + { + Node& n = m_nodes[i]; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol = btDbvtVolume::FromCR(n.m_x, 0); + btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode(); + node->parent = NULL; + node->data = &n; + node->childs[1] = 0; + node->volume = vol; + leafNodes[i] = node; + n.m_leaf = node; + } + btAlignedObjectArray<btAlignedObjectArray<int> > adj; + adj.resize(m_nodes.size()); + btAlignedObjectArray<int> old_id; + old_id.resize(m_nodes.size()); + for (int i = 0; i < m_nodes.size(); ++i) + old_id[i] = m_nodes[i].index; + for (int i = 0; i < m_nodes.size(); ++i) + m_nodes[i].index = i; + for (int i = 0; i < m_links.size(); ++i) { - Face& f=m_faces[i]; - f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f); + Link& l = m_links[i]; + adj[l.m_n[0]->index].push_back(l.m_n[1]->index); + adj[l.m_n[1]->index].push_back(l.m_n[0]->index); } + m_ndbvt.m_root = buildTreeBottomUp(leafNodes, adj); + for (int i = 0; i < m_nodes.size(); ++i) + m_nodes[i].index = old_id[i]; } // -btVector3 btSoftBody::evaluateCom() const +btVector3 btSoftBody::evaluateCom() const { - btVector3 com(0,0,0); - if(m_pose.m_bframe) + btVector3 com(0, 0, 0); + if (m_pose.m_bframe) { - for(int i=0,ni=m_nodes.size();i<ni;++i) + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) { - com+=m_nodes[i].m_x*m_pose.m_wgh[i]; + com += m_nodes[i].m_x * m_pose.m_wgh[i]; } } - return(com); + return (com); } -// -bool btSoftBody::checkContact( const btCollisionObjectWrapper* colObjWrap, - const btVector3& x, - btScalar margin, - btSoftBody::sCti& cti) const +bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, + const btVector3& x, + btScalar margin, + btSoftBody::sCti& cti) const { btVector3 nrm; - const btCollisionShape *shp = colObjWrap->getCollisionShape(); -// const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); + const btCollisionShape* shp = colObjWrap->getCollisionShape(); + // const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform(); - const btTransform &wtr = colObjWrap->getWorldTransform(); + const btTransform& wtr = colObjWrap->getWorldTransform(); //todo: check which transform is needed here - btScalar dst = - m_worldInfo->m_sparsesdf.Evaluate( + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), shp, nrm, margin); - if(dst<0) + if (dst < 0) { cti.m_colObj = colObjWrap->getCollisionObject(); - cti.m_normal = wtr.getBasis()*nrm; - cti.m_offset = -btDot( cti.m_normal, x - cti.m_normal * dst ); - return(true); + cti.m_normal = wtr.getBasis() * nrm; + cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst); + return (true); } - return(false); + return (false); } // -void btSoftBody::updateNormals() +bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, + const btVector3& x, + btScalar margin, + btSoftBody::sCti& cti, bool predict) const { + btVector3 nrm; + const btCollisionShape* shp = colObjWrap->getCollisionShape(); + const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); + // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect + // but resolve contact at x_n + btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) + : colObjWrap->getWorldTransform(); + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(x), + shp, + nrm, + margin); + + if (!predict) + { + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = wtr.getBasis() * nrm; + cti.m_offset = dst; + } + if (dst < 0) + return true; + return (false); +} - const btVector3 zv(0,0,0); - int i,ni; +// +// Compute barycentric coordinates (u, v, w) for +// point p with respect to triangle (a, b, c) +static void getBarycentric(const btVector3& p, btVector3& a, btVector3& b, btVector3& c, btVector3& bary) +{ + btVector3 v0 = b - a, v1 = c - a, v2 = p - a; + btScalar d00 = v0.dot(v0); + btScalar d01 = v0.dot(v1); + btScalar d11 = v1.dot(v1); + btScalar d20 = v2.dot(v0); + btScalar d21 = v2.dot(v1); + btScalar denom = d00 * d11 - d01 * d01; + bary.setY((d11 * d20 - d01 * d21) / denom); + bary.setZ((d00 * d21 - d01 * d20) / denom); + bary.setX(btScalar(1) - bary.getY() - bary.getZ()); +} - for(i=0,ni=m_nodes.size();i<ni;++i) +// +bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, + Face& f, + btVector3& contact_point, + btVector3& bary, + btScalar margin, + btSoftBody::sCti& cti, bool predict) const +{ + btVector3 nrm; + const btCollisionShape* shp = colObjWrap->getCollisionShape(); + const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); + // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect + // but resolve contact at x_n + btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) + : colObjWrap->getWorldTransform(); + btScalar dst; + btGjkEpaSolver2::sResults results; + +// #define USE_QUADRATURE 1 +//#define CACHE_PREV_COLLISION + + // use collision quadrature point +#ifdef USE_QUADRATURE + { + dst = SIMD_INFINITY; + btVector3 local_nrm; + for (int q = 0; q < m_quads.size(); ++q) + { + btVector3 p; + if (predict) + p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]); + else + p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]); + btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(p), + shp, + local_nrm, + margin); + if (local_dst < dst) + { + if (local_dst < 0 && predict) + return true; + dst = local_dst; + contact_point = p; + bary = m_quads[q]; + nrm = local_nrm; + } + if (!predict) + { + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = wtr.getBasis() * nrm; + cti.m_offset = dst; + } + } + return (dst < 0); + } +#endif + + // collision detection using x* + btTransform triangle_transform; + triangle_transform.setIdentity(); + triangle_transform.setOrigin(f.m_n[0]->m_q); + 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); + btVector3 guess(0, 0, 0); + const btConvexShape* csh = static_cast<const btConvexShape*>(shp); + btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); + 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 + if (dst >= 0) + return false; + +// Use consistent barycenter to recalculate distance. +#ifdef CACHE_PREV_COLLISION + if (f.m_pcontact[3] != 0) + { + for (int i = 0; i < 3; ++i) + bary[i] = f.m_pcontact[i]; + contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); + const btConvexShape* csh = static_cast<const btConvexShape*>(shp); + btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr, results); + cti.m_colObj = colObjWrap->getCollisionObject(); + dst = results.distance; + cti.m_normal = results.normal; + cti.m_offset = dst; + + //point-convex CD + wtr = colObjWrap->getWorldTransform(); + 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); + triangle_transform.setOrigin(f.m_n[0]->m_x); + btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results); + + dst = results.distance - csh->getMargin() - margin; + return true; + } +#endif + + // Use triangle-convex CD. + wtr = colObjWrap->getWorldTransform(); + 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); + triangle_transform.setOrigin(f.m_n[0]->m_x); + btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results); + contact_point = results.witnesses[0]; + getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); + + for (int i = 0; i < 3; ++i) + f.m_pcontact[i] = bary[i]; + + dst = results.distance - csh->getMargin() - margin; + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = results.normal; + cti.m_offset = dst; + return true; +} + +void btSoftBody::updateNormals() +{ + const btVector3 zv(0, 0, 0); + int i, ni; + + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - m_nodes[i].m_n=zv; + m_nodes[i].m_n = zv; } - for(i=0,ni=m_faces.size();i<ni;++i) + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - btSoftBody::Face& f=m_faces[i]; - const btVector3 n=btCross(f.m_n[1]->m_x-f.m_n[0]->m_x, - f.m_n[2]->m_x-f.m_n[0]->m_x); - f.m_normal=n.normalized(); - f.m_n[0]->m_n+=n; - f.m_n[1]->m_n+=n; - f.m_n[2]->m_n+=n; + btSoftBody::Face& f = m_faces[i]; + const btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, + f.m_n[2]->m_x - f.m_n[0]->m_x); + f.m_normal = n; + f.m_normal.safeNormalize(); + f.m_n[0]->m_n += n; + f.m_n[1]->m_n += n; + f.m_n[2]->m_n += n; } - for(i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { btScalar len = m_nodes[i].m_n.length(); - if (len>SIMD_EPSILON) + if (len > SIMD_EPSILON) m_nodes[i].m_n /= len; } } // -void btSoftBody::updateBounds() +void btSoftBody::updateBounds() { /*if( m_acceleratedSoftBody ) { @@ -2315,258 +2958,291 @@ void btSoftBody::updateBounds() m_bounds[1] = btVector3(1000, 1000, 1000); } else {*/ - if(m_ndbvt.m_root) - { - const btVector3& mins=m_ndbvt.m_root->volume.Mins(); - const btVector3& maxs=m_ndbvt.m_root->volume.Maxs(); - const btScalar csm=getCollisionShape()->getMargin(); - const btVector3 mrg=btVector3( csm, - csm, - csm)*1; // ??? to investigate... - m_bounds[0]=mins-mrg; - m_bounds[1]=maxs+mrg; - if(0!=getBroadphaseHandle()) - { - m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(), - m_bounds[0], - m_bounds[1], - m_worldInfo->m_dispatcher); + // if (m_ndbvt.m_root) + // { + // const btVector3& mins = m_ndbvt.m_root->volume.Mins(); + // const btVector3& maxs = m_ndbvt.m_root->volume.Maxs(); + // const btScalar csm = getCollisionShape()->getMargin(); + // const btVector3 mrg = btVector3(csm, + // csm, + // csm) * + // 1; // ??? to investigate... + // m_bounds[0] = mins - mrg; + // m_bounds[1] = maxs + mrg; + // if (0 != getBroadphaseHandle()) + // { + // m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), + // m_bounds[0], + // m_bounds[1], + // m_worldInfo->m_dispatcher); + // } + // } + // else + // { + // m_bounds[0] = + // m_bounds[1] = btVector3(0, 0, 0); + // } + if (m_nodes.size()) + { + btVector3 mins = m_nodes[0].m_x; + btVector3 maxs = m_nodes[0].m_x; + for (int i = 1; i < m_nodes.size(); ++i) + { + for (int d = 0; d < 3; ++d) + { + if (m_nodes[i].m_x[d] > maxs[d]) + maxs[d] = m_nodes[i].m_x[d]; + if (m_nodes[i].m_x[d] < mins[d]) + mins[d] = m_nodes[i].m_x[d]; } } - else + const btScalar csm = getCollisionShape()->getMargin(); + const btVector3 mrg = btVector3(csm, + csm, + csm); + m_bounds[0] = mins - mrg; + m_bounds[1] = maxs + mrg; + if (0 != getBroadphaseHandle()) { - m_bounds[0]= - m_bounds[1]=btVector3(0,0,0); - } - //} + m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), + m_bounds[0], + m_bounds[1], + m_worldInfo->m_dispatcher); + } + } + else + { + m_bounds[0] = + m_bounds[1] = btVector3(0, 0, 0); + } } - // -void btSoftBody::updatePose() +void btSoftBody::updatePose() { - if(m_pose.m_bframe) + if (m_pose.m_bframe) { - btSoftBody::Pose& pose=m_pose; - const btVector3 com=evaluateCom(); - /* Com */ - pose.m_com = com; - /* Rotation */ - btMatrix3x3 Apq; - const btScalar eps=SIMD_EPSILON; - Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0); - Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3); - for(int i=0,ni=m_nodes.size();i<ni;++i) + btSoftBody::Pose& pose = m_pose; + const btVector3 com = evaluateCom(); + /* Com */ + pose.m_com = com; + /* Rotation */ + btMatrix3x3 Apq; + const btScalar eps = SIMD_EPSILON; + Apq[0] = Apq[1] = Apq[2] = btVector3(0, 0, 0); + Apq[0].setX(eps); + Apq[1].setY(eps * 2); + Apq[2].setZ(eps * 3); + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) { - const btVector3 a=pose.m_wgh[i]*(m_nodes[i].m_x-com); - const btVector3& b=pose.m_pos[i]; - Apq[0]+=a.x()*b; - Apq[1]+=a.y()*b; - Apq[2]+=a.z()*b; + const btVector3 a = pose.m_wgh[i] * (m_nodes[i].m_x - com); + const btVector3& b = pose.m_pos[i]; + Apq[0] += a.x() * b; + Apq[1] += a.y() * b; + Apq[2] += a.z() * b; } - btMatrix3x3 r,s; - PolarDecompose(Apq,r,s); - pose.m_rot=r; - pose.m_scl=pose.m_aqq*r.transpose()*Apq; - if(m_cfg.maxvolume>1) + btMatrix3x3 r, s; + PolarDecompose(Apq, r, s); + pose.m_rot = r; + pose.m_scl = pose.m_aqq * r.transpose() * Apq; + if (m_cfg.maxvolume > 1) { - const btScalar idet=Clamp<btScalar>( 1/pose.m_scl.determinant(), - 1,m_cfg.maxvolume); - pose.m_scl=Mul(pose.m_scl,idet); + const btScalar idet = Clamp<btScalar>(1 / pose.m_scl.determinant(), + 1, m_cfg.maxvolume); + pose.m_scl = Mul(pose.m_scl, idet); } - } } // -void btSoftBody::updateArea(bool averageArea) +void btSoftBody::updateArea(bool averageArea) { - int i,ni; + int i, ni; - /* Face area */ - for(i=0,ni=m_faces.size();i<ni;++i) + /* Face area */ + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - Face& f=m_faces[i]; - f.m_ra = AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x); + Face& f = m_faces[i]; + f.m_ra = AreaOf(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x); } - - /* Node area */ + + /* Node area */ if (averageArea) { - btAlignedObjectArray<int> counts; - counts.resize(m_nodes.size(),0); - for(i=0,ni=m_nodes.size();i<ni;++i) + btAlignedObjectArray<int> counts; + counts.resize(m_nodes.size(), 0); + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - m_nodes[i].m_area = 0; + m_nodes[i].m_area = 0; } - for(i=0,ni=m_faces.size();i<ni;++i) + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - btSoftBody::Face& f=m_faces[i]; - for(int j=0;j<3;++j) + btSoftBody::Face& f = m_faces[i]; + for (int j = 0; j < 3; ++j) { - const int index=(int)(f.m_n[j]-&m_nodes[0]); + const int index = (int)(f.m_n[j] - &m_nodes[0]); counts[index]++; - f.m_n[j]->m_area+=btFabs(f.m_ra); + f.m_n[j]->m_area += btFabs(f.m_ra); } } - for(i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - if(counts[i]>0) - m_nodes[i].m_area/=(btScalar)counts[i]; + if (counts[i] > 0) + m_nodes[i].m_area /= (btScalar)counts[i]; else - m_nodes[i].m_area=0; + m_nodes[i].m_area = 0; } } else { // initialize node area as zero - for(i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - m_nodes[i].m_area=0; + m_nodes[i].m_area = 0; } - for(i=0,ni=m_faces.size();i<ni;++i) + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - btSoftBody::Face& f=m_faces[i]; + btSoftBody::Face& f = m_faces[i]; - for(int j=0;j<3;++j) + for (int j = 0; j < 3; ++j) { f.m_n[j]->m_area += f.m_ra; } } - for(i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { m_nodes[i].m_area *= 0.3333333f; } } } +void btSoftBody::updateLinkConstants() +{ + int i, ni; -void btSoftBody::updateLinkConstants() -{ - int i,ni; - - /* Links */ - for(i=0,ni=m_links.size();i<ni;++i) + /* Links */ + for (i = 0, ni = m_links.size(); i < ni; ++i) { - Link& l=m_links[i]; - Material& m=*l.m_material; - l.m_c0 = (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST; + Link& l = m_links[i]; + Material& m = *l.m_material; + l.m_c0 = (l.m_n[0]->m_im + l.m_n[1]->m_im) / m.m_kLST; } } -void btSoftBody::updateConstants() +void btSoftBody::updateConstants() { resetLinkRestLengths(); updateLinkConstants(); updateArea(); } - - // -void btSoftBody::initializeClusters() +void btSoftBody::initializeClusters() { int i; - for( i=0;i<m_clusters.size();++i) + for (i = 0; i < m_clusters.size(); ++i) { - Cluster& c=*m_clusters[i]; - c.m_imass=0; + Cluster& c = *m_clusters[i]; + c.m_imass = 0; c.m_masses.resize(c.m_nodes.size()); - for(int j=0;j<c.m_nodes.size();++j) + for (int j = 0; j < c.m_nodes.size(); ++j) { - if (c.m_nodes[j]->m_im==0) + if (c.m_nodes[j]->m_im == 0) { c.m_containsAnchor = true; - c.m_masses[j] = BT_LARGE_FLOAT; - } else + c.m_masses[j] = BT_LARGE_FLOAT; + } + else { - c.m_masses[j] = btScalar(1.)/c.m_nodes[j]->m_im; + c.m_masses[j] = btScalar(1.) / c.m_nodes[j]->m_im; } - c.m_imass += c.m_masses[j]; + c.m_imass += c.m_masses[j]; } - c.m_imass = btScalar(1.)/c.m_imass; - c.m_com = btSoftBody::clusterCom(&c); - c.m_lv = btVector3(0,0,0); - c.m_av = btVector3(0,0,0); - c.m_leaf = 0; - /* Inertia */ - btMatrix3x3& ii=c.m_locii; - ii[0]=ii[1]=ii[2]=btVector3(0,0,0); + c.m_imass = btScalar(1.) / c.m_imass; + c.m_com = btSoftBody::clusterCom(&c); + c.m_lv = btVector3(0, 0, 0); + c.m_av = btVector3(0, 0, 0); + c.m_leaf = 0; + /* Inertia */ + btMatrix3x3& ii = c.m_locii; + ii[0] = ii[1] = ii[2] = btVector3(0, 0, 0); { - int i,ni; + int i, ni; - for(i=0,ni=c.m_nodes.size();i<ni;++i) + for (i = 0, ni = c.m_nodes.size(); i < ni; ++i) { - const btVector3 k=c.m_nodes[i]->m_x-c.m_com; - const btVector3 q=k*k; - const btScalar m=c.m_masses[i]; - ii[0][0] += m*(q[1]+q[2]); - ii[1][1] += m*(q[0]+q[2]); - ii[2][2] += m*(q[0]+q[1]); - ii[0][1] -= m*k[0]*k[1]; - ii[0][2] -= m*k[0]*k[2]; - ii[1][2] -= m*k[1]*k[2]; + const btVector3 k = c.m_nodes[i]->m_x - c.m_com; + const btVector3 q = k * k; + const btScalar m = c.m_masses[i]; + ii[0][0] += m * (q[1] + q[2]); + ii[1][1] += m * (q[0] + q[2]); + ii[2][2] += m * (q[0] + q[1]); + ii[0][1] -= m * k[0] * k[1]; + ii[0][2] -= m * k[0] * k[2]; + ii[1][2] -= m * k[1] * k[2]; } } - ii[1][0]=ii[0][1]; - ii[2][0]=ii[0][2]; - ii[2][1]=ii[1][2]; - + ii[1][0] = ii[0][1]; + ii[2][0] = ii[0][2]; + ii[2][1] = ii[1][2]; + ii = ii.inverse(); - /* Frame */ + /* Frame */ c.m_framexform.setIdentity(); c.m_framexform.setOrigin(c.m_com); c.m_framerefs.resize(c.m_nodes.size()); { int i; - for(i=0;i<c.m_framerefs.size();++i) + for (i = 0; i < c.m_framerefs.size(); ++i) { - c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com; + c.m_framerefs[i] = c.m_nodes[i]->m_x - c.m_com; } } } } // -void btSoftBody::updateClusters() +void btSoftBody::updateClusters() { BT_PROFILE("UpdateClusters"); int i; - for(i=0;i<m_clusters.size();++i) + for (i = 0; i < m_clusters.size(); ++i) { - btSoftBody::Cluster& c=*m_clusters[i]; - const int n=c.m_nodes.size(); + btSoftBody::Cluster& c = *m_clusters[i]; + const int n = c.m_nodes.size(); //const btScalar invn=1/(btScalar)n; - if(n) - { - /* Frame */ - const btScalar eps=btScalar(0.0001); - btMatrix3x3 m,r,s; - m[0]=m[1]=m[2]=btVector3(0,0,0); - m[0][0]=eps*1; - m[1][1]=eps*2; - m[2][2]=eps*3; - c.m_com=clusterCom(&c); - for(int i=0;i<c.m_nodes.size();++i) - { - const btVector3 a=c.m_nodes[i]->m_x-c.m_com; - const btVector3& b=c.m_framerefs[i]; - m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b; - } - PolarDecompose(m,r,s); + if (n) + { + /* Frame */ + const btScalar eps = btScalar(0.0001); + btMatrix3x3 m, r, s; + m[0] = m[1] = m[2] = btVector3(0, 0, 0); + m[0][0] = eps * 1; + m[1][1] = eps * 2; + m[2][2] = eps * 3; + c.m_com = clusterCom(&c); + for (int i = 0; i < c.m_nodes.size(); ++i) + { + const btVector3 a = c.m_nodes[i]->m_x - c.m_com; + const btVector3& b = c.m_framerefs[i]; + m[0] += a[0] * b; + m[1] += a[1] * b; + m[2] += a[2] * b; + } + PolarDecompose(m, r, s); c.m_framexform.setOrigin(c.m_com); - c.m_framexform.setBasis(r); - /* Inertia */ -#if 1/* Constant */ - c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose(); + c.m_framexform.setBasis(r); + /* Inertia */ +#if 1 /* Constant */ + c.m_invwi = c.m_framexform.getBasis() * c.m_locii * c.m_framexform.getBasis().transpose(); #else -#if 0/* Sphere */ +#if 0 /* Sphere */ const btScalar rk=(2*c.m_extents.length2())/(5*c.m_imass); const btVector3 inertia(rk,rk,rk); const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0, @@ -2574,847 +3250,1203 @@ void btSoftBody::updateClusters() btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0); c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose(); -#else/* Actual */ - c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0); - for(int i=0;i<n;++i) - { - const btVector3 k=c.m_nodes[i]->m_x-c.m_com; - const btVector3 q=k*k; - const btScalar m=1/c.m_nodes[i]->m_im; - c.m_invwi[0][0] += m*(q[1]+q[2]); - c.m_invwi[1][1] += m*(q[0]+q[2]); - c.m_invwi[2][2] += m*(q[0]+q[1]); - c.m_invwi[0][1] -= m*k[0]*k[1]; - c.m_invwi[0][2] -= m*k[0]*k[2]; - c.m_invwi[1][2] -= m*k[1]*k[2]; - } - c.m_invwi[1][0]=c.m_invwi[0][1]; - c.m_invwi[2][0]=c.m_invwi[0][2]; - c.m_invwi[2][1]=c.m_invwi[1][2]; - c.m_invwi=c.m_invwi.inverse(); +#else /* Actual */ + c.m_invwi[0] = c.m_invwi[1] = c.m_invwi[2] = btVector3(0, 0, 0); + for (int i = 0; i < n; ++i) + { + const btVector3 k = c.m_nodes[i]->m_x - c.m_com; + const btVector3 q = k * k; + const btScalar m = 1 / c.m_nodes[i]->m_im; + c.m_invwi[0][0] += m * (q[1] + q[2]); + c.m_invwi[1][1] += m * (q[0] + q[2]); + c.m_invwi[2][2] += m * (q[0] + q[1]); + c.m_invwi[0][1] -= m * k[0] * k[1]; + c.m_invwi[0][2] -= m * k[0] * k[2]; + c.m_invwi[1][2] -= m * k[1] * k[2]; + } + c.m_invwi[1][0] = c.m_invwi[0][1]; + c.m_invwi[2][0] = c.m_invwi[0][2]; + c.m_invwi[2][1] = c.m_invwi[1][2]; + c.m_invwi = c.m_invwi.inverse(); #endif #endif - /* Velocities */ - c.m_lv=btVector3(0,0,0); - c.m_av=btVector3(0,0,0); + /* Velocities */ + c.m_lv = btVector3(0, 0, 0); + c.m_av = btVector3(0, 0, 0); { int i; - for(i=0;i<n;++i) + for (i = 0; i < n; ++i) { - const btVector3 v=c.m_nodes[i]->m_v*c.m_masses[i]; - c.m_lv += v; - c.m_av += btCross(c.m_nodes[i]->m_x-c.m_com,v); + const btVector3 v = c.m_nodes[i]->m_v * c.m_masses[i]; + c.m_lv += v; + c.m_av += btCross(c.m_nodes[i]->m_x - c.m_com, v); } } - c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping); - c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping); - c.m_vimpulses[0] = - c.m_vimpulses[1] = btVector3(0,0,0); - c.m_dimpulses[0] = - c.m_dimpulses[1] = btVector3(0,0,0); - c.m_nvimpulses = 0; - c.m_ndimpulses = 0; - /* Matching */ - if(c.m_matching>0) - { - for(int j=0;j<c.m_nodes.size();++j) + c.m_lv = c.m_imass * c.m_lv * (1 - c.m_ldamping); + c.m_av = c.m_invwi * c.m_av * (1 - c.m_adamping); + c.m_vimpulses[0] = + c.m_vimpulses[1] = btVector3(0, 0, 0); + c.m_dimpulses[0] = + c.m_dimpulses[1] = btVector3(0, 0, 0); + c.m_nvimpulses = 0; + c.m_ndimpulses = 0; + /* Matching */ + if (c.m_matching > 0) + { + for (int j = 0; j < c.m_nodes.size(); ++j) { - Node& n=*c.m_nodes[j]; - const btVector3 x=c.m_framexform*c.m_framerefs[j]; - n.m_x=Lerp(n.m_x,x,c.m_matching); + Node& n = *c.m_nodes[j]; + const btVector3 x = c.m_framexform * c.m_framerefs[j]; + n.m_x = Lerp(n.m_x, x, c.m_matching); } - } - /* Dbvt */ - if(c.m_collide) + } + /* Dbvt */ + if (c.m_collide) { - btVector3 mi=c.m_nodes[0]->m_x; - btVector3 mx=mi; - for(int j=1;j<n;++j) + btVector3 mi = c.m_nodes[0]->m_x; + btVector3 mx = mi; + for (int j = 1; j < n; ++j) { mi.setMin(c.m_nodes[j]->m_x); mx.setMax(c.m_nodes[j]->m_x); - } - ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx); - if(c.m_leaf) - m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg); + } + ATTRIBUTE_ALIGNED16(btDbvtVolume) + bounds = btDbvtVolume::FromMM(mi, mx); + if (c.m_leaf) + m_cdbvt.update(c.m_leaf, bounds, c.m_lv * m_sst.sdt * 3, m_sst.radmrg); else - c.m_leaf=m_cdbvt.insert(bounds,&c); + c.m_leaf = m_cdbvt.insert(bounds, &c); } } } - - } - - - // -void btSoftBody::cleanupClusters() +void btSoftBody::cleanupClusters() { - for(int i=0;i<m_joints.size();++i) + for (int i = 0; i < m_joints.size(); ++i) { m_joints[i]->Terminate(m_sst.sdt); - if(m_joints[i]->m_delete) + if (m_joints[i]->m_delete) { btAlignedFree(m_joints[i]); m_joints.remove(m_joints[i--]); - } + } } } // -void btSoftBody::prepareClusters(int iterations) +void btSoftBody::prepareClusters(int iterations) { - for(int i=0;i<m_joints.size();++i) + for (int i = 0; i < m_joints.size(); ++i) { - m_joints[i]->Prepare(m_sst.sdt,iterations); + m_joints[i]->Prepare(m_sst.sdt, iterations); } } - // -void btSoftBody::solveClusters(btScalar sor) +void btSoftBody::solveClusters(btScalar sor) { - for(int i=0,ni=m_joints.size();i<ni;++i) + for (int i = 0, ni = m_joints.size(); i < ni; ++i) { - m_joints[i]->Solve(m_sst.sdt,sor); + m_joints[i]->Solve(m_sst.sdt, sor); } } // -void btSoftBody::applyClusters(bool drift) +void btSoftBody::applyClusters(bool drift) { BT_PROFILE("ApplyClusters"); -// const btScalar f0=m_sst.sdt; + // const btScalar f0=m_sst.sdt; //const btScalar f1=f0/2; btAlignedObjectArray<btVector3> deltas; btAlignedObjectArray<btScalar> weights; - deltas.resize(m_nodes.size(),btVector3(0,0,0)); - weights.resize(m_nodes.size(),0); + deltas.resize(m_nodes.size(), btVector3(0, 0, 0)); + weights.resize(m_nodes.size(), 0); int i; - if(drift) + if (drift) { - for(i=0;i<m_clusters.size();++i) + for (i = 0; i < m_clusters.size(); ++i) { - Cluster& c=*m_clusters[i]; - if(c.m_ndimpulses) + Cluster& c = *m_clusters[i]; + if (c.m_ndimpulses) { - c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses; - c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses; + c.m_dimpulses[0] /= (btScalar)c.m_ndimpulses; + c.m_dimpulses[1] /= (btScalar)c.m_ndimpulses; } } } - - for(i=0;i<m_clusters.size();++i) + + for (i = 0; i < m_clusters.size(); ++i) { - Cluster& c=*m_clusters[i]; - if(0<(drift?c.m_ndimpulses:c.m_nvimpulses)) + Cluster& c = *m_clusters[i]; + if (0 < (drift ? c.m_ndimpulses : c.m_nvimpulses)) { - const btVector3 v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt; - const btVector3 w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt; - for(int j=0;j<c.m_nodes.size();++j) + const btVector3 v = (drift ? c.m_dimpulses[0] : c.m_vimpulses[0]) * m_sst.sdt; + const btVector3 w = (drift ? c.m_dimpulses[1] : c.m_vimpulses[1]) * m_sst.sdt; + for (int j = 0; j < c.m_nodes.size(); ++j) { - const int idx=int(c.m_nodes[j]-&m_nodes[0]); - const btVector3& x=c.m_nodes[j]->m_x; - const btScalar q=c.m_masses[j]; - deltas[idx] += (v+btCross(w,x-c.m_com))*q; - weights[idx] += q; + const int idx = int(c.m_nodes[j] - &m_nodes[0]); + const btVector3& x = c.m_nodes[j]->m_x; + const btScalar q = c.m_masses[j]; + deltas[idx] += (v + btCross(w, x - c.m_com)) * q; + weights[idx] += q; } } } - for(i=0;i<deltas.size();++i) + for (i = 0; i < deltas.size(); ++i) { - if(weights[i]>0) + if (weights[i] > 0) { - m_nodes[i].m_x+=deltas[i]/weights[i]; + m_nodes[i].m_x += deltas[i] / weights[i]; } } } // -void btSoftBody::dampClusters() +void btSoftBody::dampClusters() { int i; - for(i=0;i<m_clusters.size();++i) + for (i = 0; i < m_clusters.size(); ++i) { - Cluster& c=*m_clusters[i]; - if(c.m_ndamping>0) + Cluster& c = *m_clusters[i]; + if (c.m_ndamping > 0) { - for(int j=0;j<c.m_nodes.size();++j) + for (int j = 0; j < c.m_nodes.size(); ++j) { - Node& n=*c.m_nodes[j]; - if(n.m_im>0) + Node& n = *c.m_nodes[j]; + if (n.m_im > 0) { - const btVector3 vx=c.m_lv+btCross(c.m_av,c.m_nodes[j]->m_q-c.m_com); - if(vx.length2()<=n.m_v.length2()) - { - n.m_v += c.m_ndamping*(vx-n.m_v); - } + const btVector3 vx = c.m_lv + btCross(c.m_av, c.m_nodes[j]->m_q - c.m_com); + if (vx.length2() <= n.m_v.length2()) + { + n.m_v += c.m_ndamping * (vx - n.m_v); + } } } } } } +void btSoftBody::setSpringStiffness(btScalar k) +{ + for (int i = 0; i < m_links.size(); ++i) + { + m_links[i].Feature::m_material->m_kLST = k; + } + m_repulsionStiffness = k; +} + +void btSoftBody::setGravityFactor(btScalar gravFactor) +{ + m_gravityFactor = gravFactor; +} + +void btSoftBody::initializeDmInverse() +{ + btScalar unit_simplex_measure = 1. / 6.; + + for (int i = 0; i < m_tetras.size(); ++i) + { + Tetra& t = m_tetras[i]; + btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x; + btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x; + btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x; + btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(), + c1.getY(), c2.getY(), c3.getY(), + c1.getZ(), c2.getZ(), c3.getZ()); + t.m_element_measure = Dm.determinant() * unit_simplex_measure; + t.m_Dm_inverse = Dm.inverse(); + + // calculate the first three columns of P^{-1} + btVector3 a = t.m_n[0]->m_x; + btVector3 b = t.m_n[1]->m_x; + btVector3 c = t.m_n[2]->m_x; + btVector3 d = t.m_n[3]->m_x; + + 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]); + + 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]; + 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]; + 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]; + 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]; + 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]; + 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]; + 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]; + 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]; + 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]; + 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]; + 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]; + 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]; + + btVector4 p1(P11 * det, P21 * det, P31 * det, P41 * det); + btVector4 p2(P12 * det, P22 * det, P32 * det, P42 * det); + btVector4 p3(P13 * det, P23 * det, P33 * det, P43 * det); + + t.m_P_inv[0] = p1; + t.m_P_inv[1] = p2; + t.m_P_inv[2] = p3; + } +} + +static btScalar Dot4(const btVector4& a, const btVector4& b) +{ + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]; +} + +void btSoftBody::updateDeformation() +{ + btQuaternion q; + for (int i = 0; i < m_tetras.size(); ++i) + { + btSoftBody::Tetra& t = m_tetras[i]; + btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q; + btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q; + btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q; + btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(), + c1.getY(), c2.getY(), c3.getY(), + c1.getZ(), c2.getZ(), c3.getZ()); + t.m_F = Ds * t.m_Dm_inverse; + + btSoftBody::TetraScratch& s = m_tetraScratches[i]; + s.m_F = t.m_F; + s.m_J = t.m_F.determinant(); + btMatrix3x3 C = t.m_F.transpose() * t.m_F; + s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ(); + s.m_cofF = t.m_F.adjoint().transpose(); + + btVector3 a = t.m_n[0]->m_q; + btVector3 b = t.m_n[1]->m_q; + btVector3 c = t.m_n[2]->m_q; + btVector3 d = t.m_n[3]->m_q; + btVector4 q1(a[0], b[0], c[0], d[0]); + btVector4 q2(a[1], b[1], c[1], d[1]); + btVector4 q3(a[2], b[2], c[2], d[2]); + btMatrix3x3 B(Dot4(q1, t.m_P_inv[0]), Dot4(q1, t.m_P_inv[1]), Dot4(q1, t.m_P_inv[2]), + Dot4(q2, t.m_P_inv[0]), Dot4(q2, t.m_P_inv[1]), Dot4(q2, t.m_P_inv[2]), + Dot4(q3, t.m_P_inv[0]), Dot4(q3, t.m_P_inv[1]), Dot4(q3, t.m_P_inv[2])); + q.setRotation(btVector3(0, 0, 1), 0); + B.extractRotation(q, 0.01); // precision of the rotation is not very important for visual correctness. + btMatrix3x3 Q(q); + s.m_corotation = Q; + } +} + +void btSoftBody::advanceDeformation() +{ + updateDeformation(); + for (int i = 0; i < m_tetras.size(); ++i) + { + m_tetraScratchesTn[i] = m_tetraScratches[i]; + } +} // -void btSoftBody::Joint::Prepare(btScalar dt,int) +void btSoftBody::Joint::Prepare(btScalar dt, int) { m_bodies[0].activate(); m_bodies[1].activate(); } // -void btSoftBody::LJoint::Prepare(btScalar dt,int iterations) +void btSoftBody::LJoint::Prepare(btScalar dt, int iterations) { - static const btScalar maxdrift=4; - Joint::Prepare(dt,iterations); - m_rpos[0] = m_bodies[0].xform()*m_refs[0]; - m_rpos[1] = m_bodies[1].xform()*m_refs[1]; - m_drift = Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt; - m_rpos[0] -= m_bodies[0].xform().getOrigin(); - m_rpos[1] -= m_bodies[1].xform().getOrigin(); - m_massmatrix = ImpulseMatrix( m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0], - m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]); - if(m_split>0) + static const btScalar maxdrift = 4; + Joint::Prepare(dt, iterations); + m_rpos[0] = m_bodies[0].xform() * m_refs[0]; + m_rpos[1] = m_bodies[1].xform() * m_refs[1]; + m_drift = Clamp(m_rpos[0] - m_rpos[1], maxdrift) * m_erp / dt; + m_rpos[0] -= m_bodies[0].xform().getOrigin(); + m_rpos[1] -= m_bodies[1].xform().getOrigin(); + m_massmatrix = ImpulseMatrix(m_bodies[0].invMass(), m_bodies[0].invWorldInertia(), m_rpos[0], + m_bodies[1].invMass(), m_bodies[1].invWorldInertia(), m_rpos[1]); + if (m_split > 0) { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; + m_sdrift = m_massmatrix * (m_drift * m_split); + m_drift *= 1 - m_split; } - m_drift /=(btScalar)iterations; + m_drift /= (btScalar)iterations; } // -void btSoftBody::LJoint::Solve(btScalar dt,btScalar sor) +void btSoftBody::LJoint::Solve(btScalar dt, btScalar sor) { - const btVector3 va=m_bodies[0].velocity(m_rpos[0]); - const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); - const btVector3 vr=va-vb; - btSoftBody::Impulse impulse; - impulse.m_asVelocity = 1; - impulse.m_velocity = m_massmatrix*(m_drift+vr*m_cfm)*sor; - m_bodies[0].applyImpulse(-impulse,m_rpos[0]); - m_bodies[1].applyImpulse( impulse,m_rpos[1]); + const btVector3 va = m_bodies[0].velocity(m_rpos[0]); + const btVector3 vb = m_bodies[1].velocity(m_rpos[1]); + const btVector3 vr = va - vb; + btSoftBody::Impulse impulse; + impulse.m_asVelocity = 1; + impulse.m_velocity = m_massmatrix * (m_drift + vr * m_cfm) * sor; + m_bodies[0].applyImpulse(-impulse, m_rpos[0]); + m_bodies[1].applyImpulse(impulse, m_rpos[1]); } // -void btSoftBody::LJoint::Terminate(btScalar dt) +void btSoftBody::LJoint::Terminate(btScalar dt) { - if(m_split>0) + if (m_split > 0) { - m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); - m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); + m_bodies[0].applyDImpulse(-m_sdrift, m_rpos[0]); + m_bodies[1].applyDImpulse(m_sdrift, m_rpos[1]); } } // -void btSoftBody::AJoint::Prepare(btScalar dt,int iterations) +void btSoftBody::AJoint::Prepare(btScalar dt, int iterations) { - static const btScalar maxdrift=SIMD_PI/16; + static const btScalar maxdrift = SIMD_PI / 16; m_icontrol->Prepare(this); - Joint::Prepare(dt,iterations); - m_axis[0] = m_bodies[0].xform().getBasis()*m_refs[0]; - m_axis[1] = m_bodies[1].xform().getBasis()*m_refs[1]; - m_drift = NormalizeAny(btCross(m_axis[1],m_axis[0])); - m_drift *= btMin(maxdrift,btAcos(Clamp<btScalar>(btDot(m_axis[0],m_axis[1]),-1,+1))); - m_drift *= m_erp/dt; - m_massmatrix= AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia()); - if(m_split>0) + Joint::Prepare(dt, iterations); + m_axis[0] = m_bodies[0].xform().getBasis() * m_refs[0]; + m_axis[1] = m_bodies[1].xform().getBasis() * m_refs[1]; + m_drift = NormalizeAny(btCross(m_axis[1], m_axis[0])); + m_drift *= btMin(maxdrift, btAcos(Clamp<btScalar>(btDot(m_axis[0], m_axis[1]), -1, +1))); + m_drift *= m_erp / dt; + m_massmatrix = AngularImpulseMatrix(m_bodies[0].invWorldInertia(), m_bodies[1].invWorldInertia()); + if (m_split > 0) { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; + m_sdrift = m_massmatrix * (m_drift * m_split); + m_drift *= 1 - m_split; } - m_drift /=(btScalar)iterations; + m_drift /= (btScalar)iterations; } // -void btSoftBody::AJoint::Solve(btScalar dt,btScalar sor) +void btSoftBody::AJoint::Solve(btScalar dt, btScalar sor) { - const btVector3 va=m_bodies[0].angularVelocity(); - const btVector3 vb=m_bodies[1].angularVelocity(); - const btVector3 vr=va-vb; - const btScalar sp=btDot(vr,m_axis[0]); - const btVector3 vc=vr-m_axis[0]*m_icontrol->Speed(this,sp); - btSoftBody::Impulse impulse; - impulse.m_asVelocity = 1; - impulse.m_velocity = m_massmatrix*(m_drift+vc*m_cfm)*sor; + const btVector3 va = m_bodies[0].angularVelocity(); + const btVector3 vb = m_bodies[1].angularVelocity(); + const btVector3 vr = va - vb; + const btScalar sp = btDot(vr, m_axis[0]); + const btVector3 vc = vr - m_axis[0] * m_icontrol->Speed(this, sp); + btSoftBody::Impulse impulse; + impulse.m_asVelocity = 1; + impulse.m_velocity = m_massmatrix * (m_drift + vc * m_cfm) * sor; m_bodies[0].applyAImpulse(-impulse); - m_bodies[1].applyAImpulse( impulse); + m_bodies[1].applyAImpulse(impulse); } // -void btSoftBody::AJoint::Terminate(btScalar dt) +void btSoftBody::AJoint::Terminate(btScalar dt) { - if(m_split>0) + if (m_split > 0) { m_bodies[0].applyDAImpulse(-m_sdrift); - m_bodies[1].applyDAImpulse( m_sdrift); + m_bodies[1].applyDAImpulse(m_sdrift); } } // -void btSoftBody::CJoint::Prepare(btScalar dt,int iterations) +void btSoftBody::CJoint::Prepare(btScalar dt, int iterations) { - Joint::Prepare(dt,iterations); - const bool dodrift=(m_life==0); - m_delete=(++m_life)>m_maxlife; - if(dodrift) + Joint::Prepare(dt, iterations); + const bool dodrift = (m_life == 0); + m_delete = (++m_life) > m_maxlife; + if (dodrift) { - m_drift=m_drift*m_erp/dt; - if(m_split>0) + m_drift = m_drift * m_erp / dt; + if (m_split > 0) { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; + m_sdrift = m_massmatrix * (m_drift * m_split); + m_drift *= 1 - m_split; } - m_drift/=(btScalar)iterations; + m_drift /= (btScalar)iterations; } else { - m_drift=m_sdrift=btVector3(0,0,0); + m_drift = m_sdrift = btVector3(0, 0, 0); } } // -void btSoftBody::CJoint::Solve(btScalar dt,btScalar sor) +void btSoftBody::CJoint::Solve(btScalar dt, btScalar sor) { - const btVector3 va=m_bodies[0].velocity(m_rpos[0]); - const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); - const btVector3 vrel=va-vb; - const btScalar rvac=btDot(vrel,m_normal); - btSoftBody::Impulse impulse; - impulse.m_asVelocity = 1; - impulse.m_velocity = m_drift; - if(rvac<0) + const btVector3 va = m_bodies[0].velocity(m_rpos[0]); + const btVector3 vb = m_bodies[1].velocity(m_rpos[1]); + const btVector3 vrel = va - vb; + const btScalar rvac = btDot(vrel, m_normal); + btSoftBody::Impulse impulse; + impulse.m_asVelocity = 1; + impulse.m_velocity = m_drift; + if (rvac < 0) { - const btVector3 iv=m_normal*rvac; - const btVector3 fv=vrel-iv; - impulse.m_velocity += iv+fv*m_friction; + const btVector3 iv = m_normal * rvac; + const btVector3 fv = vrel - iv; + impulse.m_velocity += iv + fv * m_friction; } - impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor; - - if (m_bodies[0].m_soft==m_bodies[1].m_soft) + impulse.m_velocity = m_massmatrix * impulse.m_velocity * sor; + + if (m_bodies[0].m_soft == m_bodies[1].m_soft) { - if ((impulse.m_velocity.getX() ==impulse.m_velocity.getX())&&(impulse.m_velocity.getY() ==impulse.m_velocity.getY())&& - (impulse.m_velocity.getZ() ==impulse.m_velocity.getZ())) + if ((impulse.m_velocity.getX() == impulse.m_velocity.getX()) && (impulse.m_velocity.getY() == impulse.m_velocity.getY()) && + (impulse.m_velocity.getZ() == impulse.m_velocity.getZ())) { if (impulse.m_asVelocity) { - if (impulse.m_velocity.length() <m_bodies[0].m_soft->m_maxSelfCollisionImpulse) + if (impulse.m_velocity.length() < m_bodies[0].m_soft->m_maxSelfCollisionImpulse) { - - } else + } + else { - m_bodies[0].applyImpulse(-impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[0]); - m_bodies[1].applyImpulse( impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[1]); + m_bodies[0].applyImpulse(-impulse * m_bodies[0].m_soft->m_selfCollisionImpulseFactor, m_rpos[0]); + m_bodies[1].applyImpulse(impulse * m_bodies[0].m_soft->m_selfCollisionImpulseFactor, m_rpos[1]); } } } - } else + } + else { - m_bodies[0].applyImpulse(-impulse,m_rpos[0]); - m_bodies[1].applyImpulse( impulse,m_rpos[1]); + m_bodies[0].applyImpulse(-impulse, m_rpos[0]); + m_bodies[1].applyImpulse(impulse, m_rpos[1]); } } // -void btSoftBody::CJoint::Terminate(btScalar dt) +void btSoftBody::CJoint::Terminate(btScalar dt) { - if(m_split>0) + if (m_split > 0) { - m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); - m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); + m_bodies[0].applyDImpulse(-m_sdrift, m_rpos[0]); + m_bodies[1].applyDImpulse(m_sdrift, m_rpos[1]); } } // -void btSoftBody::applyForces() +void btSoftBody::applyForces() { - BT_PROFILE("SoftBody applyForces"); -// const btScalar dt = m_sst.sdt; - const btScalar kLF = m_cfg.kLF; - const btScalar kDG = m_cfg.kDG; - const btScalar kPR = m_cfg.kPR; - const btScalar kVC = m_cfg.kVC; - const bool as_lift = kLF>0; - const bool as_drag = kDG>0; - const bool as_pressure = kPR!=0; - const bool as_volume = kVC>0; - const bool as_aero = as_lift || - as_drag ; + // const btScalar dt = m_sst.sdt; + const btScalar kLF = m_cfg.kLF; + const btScalar kDG = m_cfg.kDG; + const btScalar kPR = m_cfg.kPR; + const btScalar kVC = m_cfg.kVC; + const bool as_lift = kLF > 0; + const bool as_drag = kDG > 0; + const bool as_pressure = kPR != 0; + const bool as_volume = kVC > 0; + const bool as_aero = as_lift || + as_drag; //const bool as_vaero = as_aero && // (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided); //const bool as_faero = as_aero && // (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided); - const bool use_medium = as_aero; - const bool use_volume = as_pressure || - as_volume ; - btScalar volume = 0; - btScalar ivolumetp = 0; - btScalar dvolumetv = 0; - btSoftBody::sMedium medium; - if(use_volume) + const bool use_medium = as_aero; + const bool use_volume = as_pressure || + as_volume; + btScalar volume = 0; + btScalar ivolumetp = 0; + btScalar dvolumetv = 0; + btSoftBody::sMedium medium; + if (use_volume) { - volume = getVolume(); - ivolumetp = 1/btFabs(volume)*kPR; - dvolumetv = (m_pose.m_volume-volume)*kVC; + volume = getVolume(); + ivolumetp = 1 / btFabs(volume) * kPR; + dvolumetv = (m_pose.m_volume - volume) * kVC; } - /* Per vertex forces */ - int i,ni; + /* Per vertex forces */ + int i, ni; - for(i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - btSoftBody::Node& n=m_nodes[i]; - if(n.m_im>0) + btSoftBody::Node& n = m_nodes[i]; + if (n.m_im > 0) { - if(use_medium) + if (use_medium) { - /* Aerodynamics */ + /* Aerodynamics */ addAeroForceToNode(m_windVelocity, i); } - /* Pressure */ - if(as_pressure) + /* Pressure */ + if (as_pressure) { - n.m_f += n.m_n*(n.m_area*ivolumetp); + n.m_f += n.m_n * (n.m_area * ivolumetp); } - /* Volume */ - if(as_volume) + /* Volume */ + if (as_volume) { - n.m_f += n.m_n*(n.m_area*dvolumetv); + n.m_f += n.m_n * (n.m_area * dvolumetv); } } } - /* Per face forces */ - for(i=0,ni=m_faces.size();i<ni;++i) + /* Per face forces */ + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - // btSoftBody::Face& f=m_faces[i]; + // btSoftBody::Face& f=m_faces[i]; + + /* Aerodynamics */ + addAeroForceToFace(m_windVelocity, i); + } +} + +// +void btSoftBody::setMaxStress(btScalar maxStress) +{ + m_cfg.m_maxStress = maxStress; +} + +// +void btSoftBody::interpolateRenderMesh() +{ + if (m_z.size() > 0) + { + for (int i = 0; i < m_renderNodes.size(); ++i) + { + const Node* p0 = m_renderNodesParents[i][0]; + const Node* p1 = m_renderNodesParents[i][1]; + const Node* p2 = m_renderNodesParents[i][2]; + btVector3 normal = btCross(p1->m_x - p0->m_x, p2->m_x - p0->m_x); + btVector3 unit_normal = normal.normalized(); + Node& n = m_renderNodes[i]; + n.m_x.setZero(); + for (int j = 0; j < 3; ++j) + { + n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j]; + } + n.m_x += m_z[i] * unit_normal; + } + } + else + { + for (int i = 0; i < m_renderNodes.size(); ++i) + { + Node& n = m_renderNodes[i]; + n.m_x.setZero(); + for (int j = 0; j < 4; ++j) + { + if (m_renderNodesParents[i].size()) + { + n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j]; + } + } + } + } +} - /* Aerodynamics */ - addAeroForceToFace(m_windVelocity, i); +void btSoftBody::setCollisionQuadrature(int N) +{ + for (int i = 0; i <= N; ++i) + { + for (int j = 0; i + j <= N; ++j) + { + m_quads.push_back(btVector3(btScalar(i) / btScalar(N), btScalar(j) / btScalar(N), btScalar(N - i - j) / btScalar(N))); + } } } // -void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti) +void btSoftBody::PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti) { - const btScalar kAHR=psb->m_cfg.kAHR*kst; - const btScalar dt=psb->m_sst.sdt; - for(int i=0,ni=psb->m_anchors.size();i<ni;++i) + BT_PROFILE("PSolve_Anchors"); + const btScalar kAHR = psb->m_cfg.kAHR * kst; + const btScalar dt = psb->m_sst.sdt; + for (int i = 0, ni = psb->m_anchors.size(); i < ni; ++i) { - const Anchor& a=psb->m_anchors[i]; - const btTransform& t=a.m_body->getWorldTransform(); - Node& n=*a.m_node; - const btVector3 wa=t*a.m_local; - const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt; - const btVector3 vb=n.m_x-n.m_q; - const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR; - const btVector3 impulse=a.m_c0*vr*a.m_influence; - n.m_x+=impulse*a.m_c2; - a.m_body->applyImpulse(-impulse,a.m_c1); + const Anchor& a = psb->m_anchors[i]; + const btTransform& t = a.m_body->getWorldTransform(); + Node& n = *a.m_node; + const btVector3 wa = t * a.m_local; + const btVector3 va = a.m_body->getVelocityInLocalPoint(a.m_c1) * dt; + const btVector3 vb = n.m_x - n.m_q; + const btVector3 vr = (va - vb) + (wa - n.m_x) * kAHR; + const btVector3 impulse = a.m_c0 * vr * a.m_influence; + n.m_x += impulse * a.m_c2; + a.m_body->applyImpulse(-impulse, a.m_c1); } } // void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti) { - const btScalar dt = psb->m_sst.sdt; - const btScalar mrg = psb->getCollisionShape()->getMargin(); - for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i) + BT_PROFILE("PSolve_RContacts"); + const btScalar dt = psb->m_sst.sdt; + const btScalar mrg = psb->getCollisionShape()->getMargin(); + btMultiBodyJacobianData jacobianData; + for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i) { - const RContact& c = psb->m_rcontacts[i]; - const sCti& cti = c.m_cti; - if (cti.m_colObj->hasContactResponse()) + const RContact& c = psb->m_rcontacts[i]; + const sCti& cti = c.m_cti; + if (cti.m_colObj->hasContactResponse()) { - btRigidBody* tmpRigid = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - const btVector3 va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); - const btVector3 vb = c.m_node->m_x-c.m_node->m_q; - const btVector3 vr = vb-va; - const btScalar dn = btDot(vr, cti.m_normal); - if(dn<=SIMD_EPSILON) + btVector3 va(0, 0, 0); + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + btScalar* deltaV; + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? rigidCol->getVelocityInLocalPoint(c.m_c1) * dt : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + jacobianData.m_jacobians.resize(ndof); + jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof); + btScalar* jac = &jacobianData.m_jacobians[0]; + + 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); + deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v); + + btScalar vel = 0.0; + for (int j = 0; j < ndof; ++j) + { + vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j]; + } + va = cti.m_normal * vel * dt; + } + } + + const btVector3 vb = c.m_node->m_x - c.m_node->m_q; + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, cti.m_normal); + if (dn <= SIMD_EPSILON) { - const btScalar dp = btMin( (btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg ); - const btVector3 fv = vr - (cti.m_normal * dn); + const btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg); + const btVector3 fv = vr - (cti.m_normal * dn); // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient - const btVector3 impulse = c.m_c0 * ( (vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst ); + const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst); c.m_node->m_x -= impulse * c.m_c2; - if (tmpRigid) - tmpRigid->applyImpulse(impulse,c.m_c1); + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + if (rigidCol) + rigidCol->applyImpulse(impulse, c.m_c1); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + if (multibodyLinkCol) + { + double multiplier = 0.5; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier); + } + } } } } } // -void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti) +void btSoftBody::PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti) { - for(int i=0,ni=psb->m_scontacts.size();i<ni;++i) + BT_PROFILE("PSolve_SContacts"); + + for (int i = 0, ni = psb->m_scontacts.size(); i < ni; ++i) { - const SContact& c=psb->m_scontacts[i]; - const btVector3& nr=c.m_normal; - Node& n=*c.m_node; - Face& f=*c.m_face; - const btVector3 p=BaryEval( f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - c.m_weights); - const btVector3 q=BaryEval( f.m_n[0]->m_q, - f.m_n[1]->m_q, - f.m_n[2]->m_q, - c.m_weights); - const btVector3 vr=(n.m_x-n.m_q)-(p-q); - btVector3 corr(0,0,0); - btScalar dot = btDot(vr,nr); - if(dot<0) + const SContact& c = psb->m_scontacts[i]; + const btVector3& nr = c.m_normal; + Node& n = *c.m_node; + Face& f = *c.m_face; + const btVector3 p = BaryEval(f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + c.m_weights); + const btVector3 q = BaryEval(f.m_n[0]->m_q, + f.m_n[1]->m_q, + f.m_n[2]->m_q, + c.m_weights); + const btVector3 vr = (n.m_x - n.m_q) - (p - q); + btVector3 corr(0, 0, 0); + btScalar dot = btDot(vr, nr); + if (dot < 0) { - const btScalar j=c.m_margin-(btDot(nr,n.m_x)-btDot(nr,p)); - corr+=c.m_normal*j; + const btScalar j = c.m_margin - (btDot(nr, n.m_x) - btDot(nr, p)); + corr += c.m_normal * j; } - corr -= ProjectOnPlane(vr,nr)*c.m_friction; - n.m_x += corr*c.m_cfm[0]; - f.m_n[0]->m_x -= corr*(c.m_cfm[1]*c.m_weights.x()); - f.m_n[1]->m_x -= corr*(c.m_cfm[1]*c.m_weights.y()); - f.m_n[2]->m_x -= corr*(c.m_cfm[1]*c.m_weights.z()); + corr -= ProjectOnPlane(vr, nr) * c.m_friction; + n.m_x += corr * c.m_cfm[0]; + f.m_n[0]->m_x -= corr * (c.m_cfm[1] * c.m_weights.x()); + f.m_n[1]->m_x -= corr * (c.m_cfm[1] * c.m_weights.y()); + f.m_n[2]->m_x -= corr * (c.m_cfm[1] * c.m_weights.z()); } } // -void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti) +void btSoftBody::PSolve_Links(btSoftBody* psb, btScalar kst, btScalar ti) { - for(int i=0,ni=psb->m_links.size();i<ni;++i) - { - Link& l=psb->m_links[i]; - if(l.m_c0>0) + BT_PROFILE("PSolve_Links"); + for (int i = 0, ni = psb->m_links.size(); i < ni; ++i) + { + Link& l = psb->m_links[i]; + if (l.m_c0 > 0) { - Node& a=*l.m_n[0]; - Node& b=*l.m_n[1]; - const btVector3 del=b.m_x-a.m_x; - const btScalar len=del.length2(); - if (l.m_c1+len > SIMD_EPSILON) + Node& a = *l.m_n[0]; + Node& b = *l.m_n[1]; + const btVector3 del = b.m_x - a.m_x; + const btScalar len = del.length2(); + if (l.m_c1 + len > SIMD_EPSILON) { - const btScalar k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst; - a.m_x-=del*(k*a.m_im); - b.m_x+=del*(k*b.m_im); + const btScalar k = ((l.m_c1 - len) / (l.m_c0 * (l.m_c1 + len))) * kst; + a.m_x -= del * (k * a.m_im); + b.m_x += del * (k * b.m_im); } } } } // -void btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst) +void btSoftBody::VSolve_Links(btSoftBody* psb, btScalar kst) { - for(int i=0,ni=psb->m_links.size();i<ni;++i) - { - Link& l=psb->m_links[i]; - Node** n=l.m_n; - const btScalar j=-btDot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst; - n[0]->m_v+= l.m_c3*(j*n[0]->m_im); - n[1]->m_v-= l.m_c3*(j*n[1]->m_im); + BT_PROFILE("VSolve_Links"); + for (int i = 0, ni = psb->m_links.size(); i < ni; ++i) + { + Link& l = psb->m_links[i]; + Node** n = l.m_n; + const btScalar j = -btDot(l.m_c3, n[0]->m_v - n[1]->m_v) * l.m_c2 * kst; + n[0]->m_v += l.m_c3 * (j * n[0]->m_im); + n[1]->m_v -= l.m_c3 * (j * n[1]->m_im); } } // -btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver) +btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver) { - switch(solver) + switch (solver) { - case ePSolver::Anchors: - return(&btSoftBody::PSolve_Anchors); - case ePSolver::Linear: - return(&btSoftBody::PSolve_Links); - case ePSolver::RContacts: - return(&btSoftBody::PSolve_RContacts); - case ePSolver::SContacts: - return(&btSoftBody::PSolve_SContacts); + case ePSolver::Anchors: + return (&btSoftBody::PSolve_Anchors); + case ePSolver::Linear: + return (&btSoftBody::PSolve_Links); + case ePSolver::RContacts: + return (&btSoftBody::PSolve_RContacts); + case ePSolver::SContacts: + return (&btSoftBody::PSolve_SContacts); default: { } } - return(0); + return (0); } // -btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) +btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) { - switch(solver) + switch (solver) { - case eVSolver::Linear: return(&btSoftBody::VSolve_Links); + case eVSolver::Linear: + return (&btSoftBody::VSolve_Links); default: { } } - return(0); + return (0); } -// -void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap) +void btSoftBody::setSelfCollision(bool useSelfCollision) +{ + m_useSelfCollision = useSelfCollision; +} + +bool btSoftBody::useSelfCollision() { + return m_useSelfCollision; +} - switch(m_cfg.collisions&fCollision::RVSmask) +// +void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap) +{ + switch (m_cfg.collisions & fCollision::RVSmask) { - case fCollision::SDF_RS: + case fCollision::SDF_RS: { - btSoftColliders::CollideSDF_RS docollide; - btRigidBody* prb1=(btRigidBody*) btRigidBody::upcast(pcoWrap->getCollisionObject()); - btTransform wtr=pcoWrap->getWorldTransform(); - - const btTransform ctr=pcoWrap->getWorldTransform(); - const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length(); - const btScalar basemargin=getCollisionShape()->getMargin(); - btVector3 mins; - btVector3 maxs; - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; - pcoWrap->getCollisionShape()->getAabb( pcoWrap->getWorldTransform(), - mins, - maxs); - volume=btDbvtVolume::FromMM(mins,maxs); - volume.Expand(btVector3(basemargin,basemargin,basemargin)); - docollide.psb = this; + btSoftColliders::CollideSDF_RS docollide; + btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject()); + btTransform wtr = pcoWrap->getWorldTransform(); + + const btTransform ctr = pcoWrap->getWorldTransform(); + const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length(); + const btScalar basemargin = getCollisionShape()->getMargin(); + btVector3 mins; + btVector3 maxs; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume; + pcoWrap->getCollisionShape()->getAabb(pcoWrap->getWorldTransform(), + mins, + maxs); + volume = btDbvtVolume::FromMM(mins, maxs); + volume.Expand(btVector3(basemargin, basemargin, basemargin)); + docollide.psb = this; docollide.m_colObj1Wrap = pcoWrap; docollide.m_rigidBody = prb1; - docollide.dynmargin = basemargin+timemargin; - docollide.stamargin = basemargin; - m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide); + docollide.dynmargin = basemargin + timemargin; + docollide.stamargin = basemargin; + m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollide); } break; - case fCollision::CL_RS: + case fCollision::CL_RS: { - btSoftColliders::CollideCL_RS collider; - collider.ProcessColObj(this,pcoWrap); + btSoftColliders::CollideCL_RS collider; + collider.ProcessColObj(this, pcoWrap); + } + break; + case fCollision::SDF_RD: + { + btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject()); + if (pcoWrap->getCollisionObject()->isActive() || this->isActive()) + { + const btTransform wtr = pcoWrap->getWorldTransform(); + const btScalar timemargin = 0; + const btScalar basemargin = getCollisionShape()->getMargin(); + btVector3 mins; + btVector3 maxs; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume; + pcoWrap->getCollisionShape()->getAabb(wtr, + mins, + maxs); + volume = btDbvtVolume::FromMM(mins, maxs); + volume.Expand(btVector3(basemargin, basemargin, basemargin)); + if (m_cfg.collisions & fCollision::SDF_RDN) + { + btSoftColliders::CollideSDF_RD docollideNode; + docollideNode.psb = this; + docollideNode.m_colObj1Wrap = pcoWrap; + docollideNode.m_rigidBody = prb1; + docollideNode.dynmargin = basemargin + timemargin; + docollideNode.stamargin = basemargin; + m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode); + } + + 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))) + { + btSoftColliders::CollideSDF_RDF docollideFace; + docollideFace.psb = this; + docollideFace.m_colObj1Wrap = pcoWrap; + docollideFace.m_rigidBody = prb1; + docollideFace.dynmargin = basemargin + timemargin; + docollideFace.stamargin = basemargin; + m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); + } + } } break; } } // -void btSoftBody::defaultCollisionHandler(btSoftBody* psb) +void btSoftBody::defaultCollisionHandler(btSoftBody* psb) { - const int cf=m_cfg.collisions&psb->m_cfg.collisions; - switch(cf&fCollision::SVSmask) + BT_PROFILE("Deformable Collision"); + const int cf = m_cfg.collisions & psb->m_cfg.collisions; + switch (cf & fCollision::SVSmask) { - case fCollision::CL_SS: + case fCollision::CL_SS: { - //support self-collision if CL_SELF flag set - if (this!=psb || psb->m_cfg.collisions&fCollision::CL_SELF) + if (this != psb || psb->m_cfg.collisions & fCollision::CL_SELF) { - btSoftColliders::CollideCL_SS docollide; - docollide.ProcessSoftSoft(this,psb); + btSoftColliders::CollideCL_SS docollide; + docollide.ProcessSoftSoft(this, psb); } - } break; - case fCollision::VF_SS: + case fCollision::VF_SS: { //only self-collision for Cluster, not Vertex-Face yet - if (this!=psb) - { - btSoftColliders::CollideVF_SS docollide; - /* common */ - docollide.mrg= getCollisionShape()->getMargin()+ - psb->getCollisionShape()->getMargin(); - /* psb0 nodes vs psb1 faces */ - docollide.psb[0]=this; - docollide.psb[1]=psb; - docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); - /* psb1 nodes vs psb0 faces */ - docollide.psb[0]=psb; - docollide.psb[1]=this; - docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); + if (this != psb) + { + btSoftColliders::CollideVF_SS docollide; + /* common */ + docollide.mrg = getCollisionShape()->getMargin() + + psb->getCollisionShape()->getMargin(); + /* psb0 nodes vs psb1 faces */ + docollide.psb[0] = this; + docollide.psb[1] = psb; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + /* psb1 nodes vs psb0 faces */ + docollide.psb[0] = psb; + docollide.psb[1] = this; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); } } break; - default: + case fCollision::VF_DD: + { + if (!psb->m_softSoftCollision) + return; + if (psb->isActive() || this->isActive()) + { + if (this != psb) + { + btSoftColliders::CollideVF_DD docollide; + /* common */ + docollide.mrg = getCollisionShape()->getMargin() + + psb->getCollisionShape()->getMargin(); + /* psb0 nodes vs psb1 faces */ + if (psb->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + docollide.psb[0] = this; + docollide.psb[1] = psb; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + + /* psb1 nodes vs psb0 faces */ + if (this->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + docollide.psb[0] = psb; + docollide.psb[1] = this; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + } + else + { + if (psb->useSelfCollision()) + { + btSoftColliders::CollideFF_DD docollide; + docollide.mrg = 2 * getCollisionShape()->getMargin(); + docollide.psb[0] = this; + docollide.psb[1] = psb; + if (this->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + /* psb0 faces vs psb0 faces */ + calculateNormalCone(this->m_fdbvnt); + this->m_fdbvt.selfCollideT(m_fdbvnt, docollide); + } + } + } + } + break; + default: { - } } } +void btSoftBody::geometricCollisionHandler(btSoftBody* psb) +{ + if (psb->isActive() || this->isActive()) + { + if (this != psb) + { + btSoftColliders::CollideCCD docollide; + /* common */ + docollide.mrg = SAFE_EPSILON; // for rounding error instead of actual margin + docollide.dt = psb->m_sst.sdt; + /* psb0 nodes vs psb1 faces */ + if (psb->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + docollide.psb[0] = this; + docollide.psb[1] = psb; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + /* psb1 nodes vs psb0 faces */ + if (this->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + docollide.psb[0] = psb; + docollide.psb[1] = this; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + } + else + { + if (psb->useSelfCollision()) + { + btSoftColliders::CollideCCD docollide; + docollide.mrg = SAFE_EPSILON; + docollide.psb[0] = this; + docollide.psb[1] = psb; + docollide.dt = psb->m_sst.sdt; + if (this->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + /* psb0 faces vs psb0 faces */ + calculateNormalCone(this->m_fdbvnt); // should compute this outside of this scope + this->m_fdbvt.selfCollideT(m_fdbvnt, docollide); + } + } + } +} - -void btSoftBody::setWindVelocity( const btVector3 &velocity ) +void btSoftBody::setWindVelocity(const btVector3& velocity) { m_windVelocity = velocity; } - const btVector3& btSoftBody::getWindVelocity() { return m_windVelocity; } - - -int btSoftBody::calculateSerializeBufferSize() const +int btSoftBody::calculateSerializeBufferSize() const { int sz = sizeof(btSoftBodyData); return sz; } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const +///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const { - btSoftBodyData* sbd = (btSoftBodyData*) dataBuffer; + btSoftBodyData* sbd = (btSoftBodyData*)dataBuffer; btCollisionObject::serialize(&sbd->m_collisionObjectData, serializer); - btHashMap<btHashPtr,int> m_nodeIndexMap; + btHashMap<btHashPtr, int> m_nodeIndexMap; sbd->m_numMaterials = m_materials.size(); - sbd->m_materials = sbd->m_numMaterials? (SoftBodyMaterialData**) serializer->getUniquePointer((void*)&m_materials): 0; + sbd->m_materials = sbd->m_numMaterials ? (SoftBodyMaterialData**)serializer->getUniquePointer((void*)&m_materials) : 0; if (sbd->m_materials) { int sz = sizeof(SoftBodyMaterialData*); int numElem = sbd->m_numMaterials; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); //SoftBodyMaterialData** memPtr = chunk->m_oldPtr; SoftBodyMaterialData** memPtr = (SoftBodyMaterialData**)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { btSoftBody::Material* mat = m_materials[i]; *memPtr = mat ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)mat) : 0; if (!serializer->findPointer(mat)) { //serialize it here - btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData),1); + btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData), 1); SoftBodyMaterialData* memPtr = (SoftBodyMaterialData*)chunk->m_oldPtr; memPtr->m_flags = mat->m_flags; memPtr->m_angularStiffness = mat->m_kAST; memPtr->m_linearStiffness = mat->m_kLST; memPtr->m_volumeStiffness = mat->m_kVST; - serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_SBMATERIAL_CODE,mat); + serializer->finalizeChunk(chunk, "SoftBodyMaterialData", BT_SBMATERIAL_CODE, mat); } } - serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_ARRAY_CODE,(void*) &m_materials); + serializer->finalizeChunk(chunk, "SoftBodyMaterialData", BT_ARRAY_CODE, (void*)&m_materials); } - - - sbd->m_numNodes = m_nodes.size(); - sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes): 0; + sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes) : 0; if (sbd->m_nodes) { int sz = sizeof(SoftBodyNodeData); int numElem = sbd->m_numNodes; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); SoftBodyNodeData* memPtr = (SoftBodyNodeData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { - m_nodes[i].m_f.serializeFloat( memPtr->m_accumulatedForce); + m_nodes[i].m_f.serializeFloat(memPtr->m_accumulatedForce); memPtr->m_area = m_nodes[i].m_area; memPtr->m_attach = m_nodes[i].m_battach; memPtr->m_inverseMass = m_nodes[i].m_im; - memPtr->m_material = m_nodes[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_nodes[i].m_material):0; + memPtr->m_material = m_nodes[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_nodes[i].m_material) : 0; m_nodes[i].m_n.serializeFloat(memPtr->m_normal); m_nodes[i].m_x.serializeFloat(memPtr->m_position); m_nodes[i].m_q.serializeFloat(memPtr->m_previousPosition); m_nodes[i].m_v.serializeFloat(memPtr->m_velocity); - m_nodeIndexMap.insert(&m_nodes[i],i); + m_nodeIndexMap.insert(&m_nodes[i], i); } - serializer->finalizeChunk(chunk,"SoftBodyNodeData",BT_SBNODE_CODE,(void*) &m_nodes); + serializer->finalizeChunk(chunk, "SoftBodyNodeData", BT_SBNODE_CODE, (void*)&m_nodes); } sbd->m_numLinks = m_links.size(); - sbd->m_links = sbd->m_numLinks? (SoftBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0; + sbd->m_links = sbd->m_numLinks ? (SoftBodyLinkData*)serializer->getUniquePointer((void*)&m_links[0]) : 0; if (sbd->m_links) { int sz = sizeof(SoftBodyLinkData); int numElem = sbd->m_numLinks; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); SoftBodyLinkData* memPtr = (SoftBodyLinkData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { memPtr->m_bbending = m_links[i].m_bbending; - memPtr->m_material = m_links[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_links[i].m_material):0; - memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0]: -1; - memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0]: -1; - btAssert(memPtr->m_nodeIndices[0]<m_nodes.size()); - btAssert(memPtr->m_nodeIndices[1]<m_nodes.size()); + memPtr->m_material = m_links[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_links[i].m_material) : 0; + memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0] : -1; + memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0] : -1; + btAssert(memPtr->m_nodeIndices[0] < m_nodes.size()); + btAssert(memPtr->m_nodeIndices[1] < m_nodes.size()); memPtr->m_restLength = m_links[i].m_rl; } - serializer->finalizeChunk(chunk,"SoftBodyLinkData",BT_ARRAY_CODE,(void*) &m_links[0]); - + serializer->finalizeChunk(chunk, "SoftBodyLinkData", BT_ARRAY_CODE, (void*)&m_links[0]); } - sbd->m_numFaces = m_faces.size(); - sbd->m_faces = sbd->m_numFaces? (SoftBodyFaceData*) serializer->getUniquePointer((void*)&m_faces[0]):0; + sbd->m_faces = sbd->m_numFaces ? (SoftBodyFaceData*)serializer->getUniquePointer((void*)&m_faces[0]) : 0; if (sbd->m_faces) { int sz = sizeof(SoftBodyFaceData); int numElem = sbd->m_numFaces; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); SoftBodyFaceData* memPtr = (SoftBodyFaceData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { - memPtr->m_material = m_faces[i].m_material ? (SoftBodyMaterialData*) serializer->getUniquePointer((void*)m_faces[i].m_material): 0; - m_faces[i].m_normal.serializeFloat( memPtr->m_normal); - for (int j=0;j<3;j++) + memPtr->m_material = m_faces[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_faces[i].m_material) : 0; + m_faces[i].m_normal.serializeFloat(memPtr->m_normal); + for (int j = 0; j < 3; j++) { - memPtr->m_nodeIndices[j] = m_faces[i].m_n[j]? m_faces[i].m_n[j] - &m_nodes[0]: -1; + memPtr->m_nodeIndices[j] = m_faces[i].m_n[j] ? m_faces[i].m_n[j] - &m_nodes[0] : -1; } memPtr->m_restArea = m_faces[i].m_ra; } - serializer->finalizeChunk(chunk,"SoftBodyFaceData",BT_ARRAY_CODE,(void*) &m_faces[0]); + serializer->finalizeChunk(chunk, "SoftBodyFaceData", BT_ARRAY_CODE, (void*)&m_faces[0]); } - sbd->m_numTetrahedra = m_tetras.size(); - sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*) serializer->getUniquePointer((void*)&m_tetras[0]):0; + sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*)serializer->getUniquePointer((void*)&m_tetras[0]) : 0; if (sbd->m_tetrahedra) { int sz = sizeof(SoftBodyTetraData); int numElem = sbd->m_numTetrahedra; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); SoftBodyTetraData* memPtr = (SoftBodyTetraData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { - for (int j=0;j<4;j++) + for (int j = 0; j < 4; j++) { - m_tetras[i].m_c0[j].serializeFloat( memPtr->m_c0[j] ); - memPtr->m_nodeIndices[j] = m_tetras[j].m_n[j]? m_tetras[j].m_n[j]-&m_nodes[0] : -1; + m_tetras[i].m_c0[j].serializeFloat(memPtr->m_c0[j]); + memPtr->m_nodeIndices[j] = m_tetras[i].m_n[j] ? m_tetras[i].m_n[j] - &m_nodes[0] : -1; } memPtr->m_c1 = m_tetras[i].m_c1; memPtr->m_c2 = m_tetras[i].m_c2; - memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_tetras[i].m_material): 0; + memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_tetras[i].m_material) : 0; memPtr->m_restVolume = m_tetras[i].m_rv; } - serializer->finalizeChunk(chunk,"SoftBodyTetraData",BT_ARRAY_CODE,(void*) &m_tetras[0]); + serializer->finalizeChunk(chunk, "SoftBodyTetraData", BT_ARRAY_CODE, (void*)&m_tetras[0]); } sbd->m_numAnchors = m_anchors.size(); - sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*) serializer->getUniquePointer((void*)&m_anchors[0]):0; + sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*)serializer->getUniquePointer((void*)&m_anchors[0]) : 0; if (sbd->m_anchors) { int sz = sizeof(SoftRigidAnchorData); int numElem = sbd->m_numAnchors; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); SoftRigidAnchorData* memPtr = (SoftRigidAnchorData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { m_anchors[i].m_c0.serializeFloat(memPtr->m_c0); m_anchors[i].m_c1.serializeFloat(memPtr->m_c1); memPtr->m_c2 = m_anchors[i].m_c2; m_anchors[i].m_local.serializeFloat(memPtr->m_localFrame); - memPtr->m_nodeIndex = m_anchors[i].m_node? m_anchors[i].m_node-&m_nodes[0]: -1; - - memPtr->m_rigidBody = m_anchors[i].m_body? (btRigidBodyData*) serializer->getUniquePointer((void*)m_anchors[i].m_body): 0; + memPtr->m_nodeIndex = m_anchors[i].m_node ? m_anchors[i].m_node - &m_nodes[0] : -1; + + memPtr->m_rigidBody = m_anchors[i].m_body ? (btRigidBodyData*)serializer->getUniquePointer((void*)m_anchors[i].m_body) : 0; btAssert(memPtr->m_nodeIndex < m_nodes.size()); } - serializer->finalizeChunk(chunk,"SoftRigidAnchorData",BT_ARRAY_CODE,(void*) &m_anchors[0]); + serializer->finalizeChunk(chunk, "SoftRigidAnchorData", BT_ARRAY_CODE, (void*)&m_anchors[0]); } - sbd->m_config.m_dynamicFriction = m_cfg.kDF; sbd->m_config.m_baumgarte = m_cfg.kVCF; @@ -3449,64 +4481,63 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ sbd->m_pose = (SoftBodyPoseData*)serializer->getUniquePointer((void*)&m_pose); int sz = sizeof(SoftBodyPoseData); - btChunk* chunk = serializer->allocate(sz,1); + btChunk* chunk = serializer->allocate(sz, 1); SoftBodyPoseData* memPtr = (SoftBodyPoseData*)chunk->m_oldPtr; - + m_pose.m_aqq.serializeFloat(memPtr->m_aqq); memPtr->m_bframe = m_pose.m_bframe; memPtr->m_bvolume = m_pose.m_bvolume; m_pose.m_com.serializeFloat(memPtr->m_com); - + memPtr->m_numPositions = m_pose.m_pos.size(); - memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]): 0; + memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]) : 0; if (memPtr->m_numPositions) { int numElem = memPtr->m_numPositions; int sz = sizeof(btVector3Data); - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { m_pose.m_pos[i].serializeFloat(*memPtr); } - serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_pose.m_pos[0]); + serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)&m_pose.m_pos[0]); } memPtr->m_restVolume = m_pose.m_volume; m_pose.m_rot.serializeFloat(memPtr->m_rot); m_pose.m_scl.serializeFloat(memPtr->m_scale); memPtr->m_numWeigts = m_pose.m_wgh.size(); - memPtr->m_weights = memPtr->m_numWeigts? (float*) serializer->getUniquePointer((void*) &m_pose.m_wgh[0]) : 0; + memPtr->m_weights = memPtr->m_numWeigts ? (float*)serializer->getUniquePointer((void*)&m_pose.m_wgh[0]) : 0; if (memPtr->m_numWeigts) { - int numElem = memPtr->m_numWeigts; int sz = sizeof(float); - btChunk* chunk = serializer->allocate(sz,numElem); - float* memPtr = (float*) chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + btChunk* chunk = serializer->allocate(sz, numElem); + float* memPtr = (float*)chunk->m_oldPtr; + for (int i = 0; i < numElem; i++, memPtr++) { *memPtr = m_pose.m_wgh[i]; } - serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_pose.m_wgh[0]); + serializer->finalizeChunk(chunk, "float", BT_ARRAY_CODE, (void*)&m_pose.m_wgh[0]); } - serializer->finalizeChunk(chunk,"SoftBodyPoseData",BT_ARRAY_CODE,(void*)&m_pose); + serializer->finalizeChunk(chunk, "SoftBodyPoseData", BT_ARRAY_CODE, (void*)&m_pose); } //clusters for convex-cluster collision detection sbd->m_numClusters = m_clusters.size(); - sbd->m_clusters = sbd->m_numClusters? (SoftBodyClusterData*) serializer->getUniquePointer((void*)m_clusters[0]) : 0; + sbd->m_clusters = sbd->m_numClusters ? (SoftBodyClusterData*)serializer->getUniquePointer((void*)m_clusters[0]) : 0; if (sbd->m_numClusters) { int numElem = sbd->m_numClusters; int sz = sizeof(SoftBodyClusterData); - btChunk* chunk = serializer->allocate(sz,numElem); - SoftBodyClusterData* memPtr = (SoftBodyClusterData*) chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + btChunk* chunk = serializer->allocate(sz, numElem); + SoftBodyClusterData* memPtr = (SoftBodyClusterData*)chunk->m_oldPtr; + for (int i = 0; i < numElem; i++, memPtr++) { - memPtr->m_adamping= m_clusters[i]->m_adamping; + memPtr->m_adamping = m_clusters[i]->m_adamping; m_clusters[i]->m_av.serializeFloat(memPtr->m_av); memPtr->m_clusterIndex = m_clusters[i]->m_clusterIndex; memPtr->m_collide = m_clusters[i]->m_collide; @@ -3537,69 +4568,64 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ m_clusters[i]->m_vimpulses[1].serializeFloat(memPtr->m_vimpulses[1]); memPtr->m_ndimpulses = m_clusters[i]->m_ndimpulses; - - - memPtr->m_framerefs = memPtr->m_numFrameRefs? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0; + memPtr->m_framerefs = memPtr->m_numFrameRefs ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0; if (memPtr->m_framerefs) { int numElem = memPtr->m_numFrameRefs; int sz = sizeof(btVector3FloatData); - btChunk* chunk = serializer->allocate(sz,numElem); - btVector3FloatData* memPtr = (btVector3FloatData*) chunk->m_oldPtr; - for (int j=0;j<numElem;j++,memPtr++) + btChunk* chunk = serializer->allocate(sz, numElem); + btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr; + for (int j = 0; j < numElem; j++, memPtr++) { m_clusters[i]->m_framerefs[j].serializeFloat(*memPtr); } - serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_framerefs[0]); + serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_framerefs[0]); } - - memPtr->m_masses = memPtr->m_numMasses ? (float*) serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]): 0; + + memPtr->m_masses = memPtr->m_numMasses ? (float*)serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]) : 0; if (memPtr->m_masses) { int numElem = memPtr->m_numMasses; int sz = sizeof(float); - btChunk* chunk = serializer->allocate(sz,numElem); - float* memPtr = (float*) chunk->m_oldPtr; - for (int j=0;j<numElem;j++,memPtr++) + btChunk* chunk = serializer->allocate(sz, numElem); + float* memPtr = (float*)chunk->m_oldPtr; + for (int j = 0; j < numElem; j++, memPtr++) { *memPtr = m_clusters[i]->m_masses[j]; } - serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_masses[0]); + serializer->finalizeChunk(chunk, "float", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_masses[0]); } - memPtr->m_nodeIndices = memPtr->m_numNodes ? (int*) serializer->getUniquePointer((void*) &m_clusters[i]->m_nodes) : 0; - if (memPtr->m_nodeIndices ) + memPtr->m_nodeIndices = memPtr->m_numNodes ? (int*)serializer->getUniquePointer((void*)&m_clusters[i]->m_nodes) : 0; + if (memPtr->m_nodeIndices) { int numElem = memPtr->m_numMasses; int sz = sizeof(int); - btChunk* chunk = serializer->allocate(sz,numElem); - int* memPtr = (int*) chunk->m_oldPtr; - for (int j=0;j<numElem;j++,memPtr++) + btChunk* chunk = serializer->allocate(sz, numElem); + int* memPtr = (int*)chunk->m_oldPtr; + for (int j = 0; j < numElem; j++, memPtr++) { int* indexPtr = m_nodeIndexMap.find(m_clusters[i]->m_nodes[j]); btAssert(indexPtr); *memPtr = *indexPtr; } - serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_nodes); + serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_nodes); } } - serializer->finalizeChunk(chunk,"SoftBodyClusterData",BT_ARRAY_CODE,(void*)m_clusters[0]); - + serializer->finalizeChunk(chunk, "SoftBodyClusterData", BT_ARRAY_CODE, (void*)m_clusters[0]); } - - sbd->m_numJoints = m_joints.size(); - sbd->m_joints = m_joints.size()? (btSoftBodyJointData*) serializer->getUniquePointer((void*)&m_joints[0]) : 0; + sbd->m_joints = m_joints.size() ? (btSoftBodyJointData*)serializer->getUniquePointer((void*)&m_joints[0]) : 0; if (sbd->m_joints) { int sz = sizeof(btSoftBodyJointData); int numElem = m_joints.size(); - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); btSoftBodyJointData* memPtr = (btSoftBodyJointData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { memPtr->m_jointType = (int)m_joints[i]->Type(); m_joints[i]->m_refs[0].serializeFloat(memPtr->m_refs[0]); @@ -3608,8 +4634,8 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ memPtr->m_erp = float(m_joints[i]->m_erp); memPtr->m_split = float(m_joints[i]->m_split); memPtr->m_delete = m_joints[i]->m_delete; - - for (int j=0;j<4;j++) + + for (int j = 0; j < 4; j++) { memPtr->m_relPosition[0].m_floats[j] = 0.f; memPtr->m_relPosition[1].m_floats[j] = 0.f; @@ -3648,10 +4674,51 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_rigid); } } - serializer->finalizeChunk(chunk,"btSoftBodyJointData",BT_ARRAY_CODE,(void*) &m_joints[0]); + serializer->finalizeChunk(chunk, "btSoftBodyJointData", BT_ARRAY_CODE, (void*)&m_joints[0]); } - return btSoftBodyDataName; } +void btSoftBody::updateDeactivation(btScalar timeStep) +{ + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) + return; + + if (m_maxSpeedSquared < m_sleepingThreshold * m_sleepingThreshold) + { + m_deactivationTime += timeStep; + } + else + { + m_deactivationTime = btScalar(0.); + setActivationState(0); + } +} + +void btSoftBody::setZeroVelocity() +{ + for (int i = 0; i < m_nodes.size(); ++i) + { + m_nodes[i].m_v.setZero(); + } +} + +bool btSoftBody::wantsSleeping() +{ + if (getActivationState() == DISABLE_DEACTIVATION) + return false; + + //disable deactivation + if (gDisableDeactivation || (gDeactivationTime == btScalar(0.))) + return false; + + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) + return true; + + if (m_deactivationTime > gDeactivationTime) + { + return true; + } + return false; +} |