diff options
Diffstat (limited to 'thirdparty/bullet/src/BulletSoftBody')
25 files changed, 0 insertions, 10065 deletions
diff --git a/thirdparty/bullet/src/BulletSoftBody/CMakeLists.txt b/thirdparty/bullet/src/BulletSoftBody/CMakeLists.txt deleted file mode 100644 index d43df1c67b..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/CMakeLists.txt +++ /dev/null @@ -1,69 +0,0 @@ - -INCLUDE_DIRECTORIES( -${BULLET_PHYSICS_SOURCE_DIR}/src - -) - -#SUBDIRS( Solvers ) - -SET(BulletSoftBody_SRCS - btSoftBody.cpp - btSoftBodyConcaveCollisionAlgorithm.cpp - btSoftBodyHelpers.cpp - btSoftBodyRigidBodyCollisionConfiguration.cpp - btSoftRigidCollisionAlgorithm.cpp - btSoftRigidDynamicsWorld.cpp - btSoftMultiBodyDynamicsWorld.cpp - btSoftSoftCollisionAlgorithm.cpp - btDefaultSoftBodySolver.cpp - -) - -SET(BulletSoftBody_HDRS - btSoftBody.h - btSoftBodyData.h - btSoftBodyConcaveCollisionAlgorithm.h - btSoftBodyHelpers.h - btSoftBodyRigidBodyCollisionConfiguration.h - btSoftRigidCollisionAlgorithm.h - btSoftRigidDynamicsWorld.h - btSoftMultiBodyDynamicsWorld.h - btSoftSoftCollisionAlgorithm.h - btSparseSDF.h - - btSoftBodySolvers.h - btDefaultSoftBodySolver.h - - btSoftBodySolverVertexBuffer.h -) - - - -ADD_LIBRARY(BulletSoftBody ${BulletSoftBody_SRCS} ${BulletSoftBody_HDRS}) -SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES VERSION ${BULLET_VERSION}) -SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES SOVERSION ${BULLET_VERSION}) -IF (BUILD_SHARED_LIBS) - TARGET_LINK_LIBRARIES(BulletSoftBody BulletDynamics) -ENDIF (BUILD_SHARED_LIBS) - -IF (INSTALL_LIBS) - IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) - IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) - IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) - INSTALL(TARGETS BulletSoftBody DESTINATION .) - ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) - INSTALL(TARGETS BulletSoftBody RUNTIME DESTINATION bin - LIBRARY DESTINATION lib${LIB_SUFFIX} - ARCHIVE DESTINATION lib${LIB_SUFFIX}) - INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} -DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN -".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE) - ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) - ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) - - IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) - SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES FRAMEWORK true) - SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES PUBLIC_HEADER "${BulletSoftBody_HDRS}") - ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) - ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) -ENDIF (INSTALL_LIBS) diff --git a/thirdparty/bullet/src/BulletSoftBody/btDefaultSoftBodySolver.cpp b/thirdparty/bullet/src/BulletSoftBody/btDefaultSoftBodySolver.cpp deleted file mode 100644 index 9c20403074..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btDefaultSoftBodySolver.cpp +++ /dev/null @@ -1,151 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionShapes/btCollisionShape.h" - -#include "btDefaultSoftBodySolver.h" -#include "BulletCollision/CollisionShapes/btCapsuleShape.h" -#include "BulletSoftBody/btSoftBody.h" - - -btDefaultSoftBodySolver::btDefaultSoftBodySolver() -{ - // Initial we will clearly need to update solver constants - // For now this is global for the cloths linked with this solver - we should probably make this body specific - // for performance in future once we understand more clearly when constants need to be updated - m_updateSolverConstants = true; -} - -btDefaultSoftBodySolver::~btDefaultSoftBodySolver() -{ -} - -// In this case the data is already in the soft bodies so there is no need for us to do anything -void btDefaultSoftBodySolver::copyBackToSoftBodies(bool bMove) -{ - -} - -void btDefaultSoftBodySolver::optimize( btAlignedObjectArray< btSoftBody * > &softBodies , bool forceUpdate) -{ - m_softBodySet.copyFromArray( softBodies ); -} - -void btDefaultSoftBodySolver::updateSoftBodies( ) -{ - for ( int i=0; i < m_softBodySet.size(); i++) - { - btSoftBody* psb=(btSoftBody*)m_softBodySet[i]; - if (psb->isActive()) - { - psb->integrateMotion(); - } - } -} // updateSoftBodies - -bool btDefaultSoftBodySolver::checkInitialized() -{ - return true; -} - -void btDefaultSoftBodySolver::solveConstraints( float solverdt ) -{ - // Solve constraints for non-solver softbodies - for(int i=0; i < m_softBodySet.size(); ++i) - { - btSoftBody* psb = static_cast<btSoftBody*>(m_softBodySet[i]); - if (psb->isActive()) - { - psb->solveConstraints(); - } - } -} // btDefaultSoftBodySolver::solveConstraints - - -void btDefaultSoftBodySolver::copySoftBodyToVertexBuffer( const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer ) -{ - // Currently only support CPU output buffers - // TODO: check for DX11 buffers. Take all offsets into the same DX11 buffer - // and use them together on a single kernel call if possible by setting up a - // per-cloth target buffer array for the copy kernel. - - if( vertexBuffer->getBufferType() == btVertexBufferDescriptor::CPU_BUFFER ) - { - const btAlignedObjectArray<btSoftBody::Node> &clothVertices( softBody->m_nodes ); - int numVertices = clothVertices.size(); - - const btCPUVertexBufferDescriptor *cpuVertexBuffer = static_cast< btCPUVertexBufferDescriptor* >(vertexBuffer); - float *basePointer = cpuVertexBuffer->getBasePointer(); - - if( vertexBuffer->hasVertexPositions() ) - { - const int vertexOffset = cpuVertexBuffer->getVertexOffset(); - const int vertexStride = cpuVertexBuffer->getVertexStride(); - float *vertexPointer = basePointer + vertexOffset; - - for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex ) - { - btVector3 position = clothVertices[vertexIndex].m_x; - *(vertexPointer + 0) = (float)position.getX(); - *(vertexPointer + 1) = (float)position.getY(); - *(vertexPointer + 2) = (float)position.getZ(); - vertexPointer += vertexStride; - } - } - if( vertexBuffer->hasNormals() ) - { - const int normalOffset = cpuVertexBuffer->getNormalOffset(); - const int normalStride = cpuVertexBuffer->getNormalStride(); - float *normalPointer = basePointer + normalOffset; - - for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex ) - { - btVector3 normal = clothVertices[vertexIndex].m_n; - *(normalPointer + 0) = (float)normal.getX(); - *(normalPointer + 1) = (float)normal.getY(); - *(normalPointer + 2) = (float)normal.getZ(); - normalPointer += normalStride; - } - } - } -} // btDefaultSoftBodySolver::copySoftBodyToVertexBuffer - -void btDefaultSoftBodySolver::processCollision( btSoftBody* softBody, btSoftBody* otherSoftBody) -{ - softBody->defaultCollisionHandler( otherSoftBody); -} - -// For the default solver just leave the soft body to do its collision processing -void btDefaultSoftBodySolver::processCollision( btSoftBody *softBody, const btCollisionObjectWrapper* collisionObjectWrap ) -{ - softBody->defaultCollisionHandler( collisionObjectWrap ); -} // btDefaultSoftBodySolver::processCollision - - -void btDefaultSoftBodySolver::predictMotion( float timeStep ) -{ - for ( int i=0; i < m_softBodySet.size(); ++i) - { - btSoftBody* psb = m_softBodySet[i]; - - if (psb->isActive()) - { - psb->predictMotion(timeStep); - } - } -} - diff --git a/thirdparty/bullet/src/BulletSoftBody/btDefaultSoftBodySolver.h b/thirdparty/bullet/src/BulletSoftBody/btDefaultSoftBodySolver.h deleted file mode 100644 index 1c17ffcbb2..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btDefaultSoftBodySolver.h +++ /dev/null @@ -1,63 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFT_BODY_DEFAULT_SOLVER_H -#define BT_SOFT_BODY_DEFAULT_SOLVER_H - - -#include "BulletSoftBody/btSoftBodySolvers.h" -#include "btSoftBodySolverVertexBuffer.h" -struct btCollisionObjectWrapper; - -class btDefaultSoftBodySolver : public btSoftBodySolver -{ -protected: - /** Variable to define whether we need to update solver constants on the next iteration */ - bool m_updateSolverConstants; - - btAlignedObjectArray< btSoftBody * > m_softBodySet; - - -public: - btDefaultSoftBodySolver(); - - virtual ~btDefaultSoftBodySolver(); - - virtual SolverTypes getSolverType() const - { - return DEFAULT_SOLVER; - } - - virtual bool checkInitialized(); - - virtual void updateSoftBodies( ); - - virtual void optimize( btAlignedObjectArray< btSoftBody * > &softBodies,bool forceUpdate=false ); - - virtual void copyBackToSoftBodies(bool bMove = true); - - virtual void solveConstraints( float solverdt ); - - virtual void predictMotion( float solverdt ); - - virtual void copySoftBodyToVertexBuffer( const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer ); - - virtual void processCollision( btSoftBody *, const btCollisionObjectWrapper* ); - - virtual void processCollision( btSoftBody*, btSoftBody* ); - -}; - -#endif // #ifndef BT_ACCELERATED_SOFT_BODY_CPU_SOLVER_H diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftBody.cpp b/thirdparty/bullet/src/BulletSoftBody/btSoftBody.cpp deleted file mode 100644 index 48efb0d8d4..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftBody.cpp +++ /dev/null @@ -1,3709 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btSoftBody implementation by Nathanael Presson - -#include "btSoftBodyInternals.h" -#include "BulletSoftBody/btSoftBodySolvers.h" -#include "btSoftBodyData.h" -#include "LinearMath/btSerializer.h" -#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" -#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" - - -// -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; - - /* 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]; - 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; - } - updateBounds(); - -} - -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); - 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_worldTransform.setIdentity(); - setSolver(eSolverPresets::Positions); - - /* 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_restLengthScale = btScalar(1.0); -} - -// -btSoftBody::~btSoftBody() -{ - //for now, delete the internal shape - delete m_collisionShape; - int i; - - releaseClusters(); - for(i=0;i<m_materials.size();++i) - btAlignedFree(m_materials[i]); - for(i=0;i<m_joints.size();++i) - btAlignedFree(m_joints[i]); -} - -// -bool btSoftBody::checkLink(int node0,int node1) const -{ - return(checkLink(&m_nodes[node0],&m_nodes[node1])); -} - -// -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 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(false); -} - -// -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 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(c==7) return(true); - } - return(false); -} - -// -btSoftBody::Material* btSoftBody::appendMaterial() -{ - 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); -} - -// -void btSoftBody::appendNote( const char* text, - const btVector3& o, - const btVector4& c, - Node* n0, - Node* n1, - Node* n2, - Node* n3) -{ - 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; - m_notes.push_back(n); -} - -// -void btSoftBody::appendNote( const char* text, - const btVector3& o, - Node* feature) -{ - appendNote(text,o,btVector4(1,0,0,0),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]); -} - -// -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]); -} - -// -void btSoftBody::appendNode( const btVector3& x,btScalar m) -{ - if(m_nodes.capacity()==m_nodes.size()) - { - pointersToIndices(); - m_nodes.reserve(m_nodes.size()*2+1); - indicesToPointers(); - } - const btScalar margin=getCollisionShape()->getMargin(); - m_nodes.push_back(Node()); - 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); -} - -// -void btSoftBody::appendLink(int model,Material* mat) -{ - Link l; - if(model>=0) - l=m_links[model]; - else - { 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) -{ - appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist); -} - -// -void btSoftBody::appendLink( Node* node0, - Node* node1, - Material* mat, - bool bcheckexist) -{ - 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; - } -} - -// -void btSoftBody::appendFace(int model,Material* mat) -{ - Face f; - if(model>=0) - { f=m_faces[model]; } - else - { 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) -{ - if (node0==node1) - return; - if (node1==node2) - return; - 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; -} - -// -void btSoftBody::appendTetra(int model,Material* mat) -{ -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); -} - -// -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; -} - -// - -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); -} - -// -void btSoftBody::appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies,btScalar influence) -{ - if (disableCollisionBetweenLinkedBodies) - { - 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; - a.m_influence = influence; - m_anchors.push_back(a); -} - -// -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; - m_joints.push_back(pj); -} - -// -void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body) -{ - appendLinearJoint(specs,m_clusters[0],body); -} - -// -void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body) -{ - appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]); -} - -// -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; - m_joints.push_back(pj); -} - -// -void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body) -{ - appendAngularJoint(specs,m_clusters[0],body); -} - -// -void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body) -{ - appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]); -} - -// -void btSoftBody::addForce(const btVector3& force) -{ - for(int i=0,ni=m_nodes.size();i<ni;++i) addForce(force,i); -} - -// -void btSoftBody::addForce(const btVector3& force,int node) -{ - Node& n=m_nodes[node]; - if(n.m_im>0) - { - n.m_f += force; - } -} - -void btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeIndex) -{ - btAssert(nodeIndex >= 0 && nodeIndex < m_nodes.size()); - - 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_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 ) - { - 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; - const btScalar rel_v_len = rel_v.length(); - const btScalar rel_v2 = rel_v.length2(); - - if(rel_v2>SIMD_EPSILON) - { - const btVector3 rel_v_nrm = rel_v.normalized(); - btVector3 nrm = n.m_n; - - if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSidedLiftDrag) - { - 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)); - - // 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; - btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2(); - btScalar v_len2 = n.m_v.length2(); - - if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0) - { - 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); - } - - n.m_f += fDrag; - n.m_f += fLift; - } - else if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_Point || m_cfg.aeromodel == btSoftBody::eAeroModel::V_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided) - { - 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) - { - 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) -{ - 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_aero = as_lift || as_drag; - const bool as_faero = as_aero && (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided); - - if(as_faero) - { - 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); - medium.m_velocity = windVelocity; - medium.m_density = m_worldInfo->air_density; - const btVector3 rel_v=v-medium.m_velocity; - const btScalar rel_v_len = rel_v.length(); - const btScalar rel_v2=rel_v.length2(); - - if(rel_v2>SIMD_EPSILON) - { - const btVector3 rel_v_nrm = rel_v.normalized(); - btVector3 nrm = f.m_normal; - - if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSidedLiftDrag) - { - 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)); - - fDrag /= 3; - fLift /= 3; - - for(int j=0;j<3;++j) - { - 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; - btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2(); - btScalar v_len2 = f.m_n[j]->m_v.length2(); - - if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0) - { - 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); - } - - 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 (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) - { - 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) -{ - 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) -{ - for(int i=0,ni=m_nodes.size();i<ni;++i) - { - Node& n=m_nodes[i]; - if(n.m_im>0) - { - n.m_v = velocity; - } - } -} - - -// -void btSoftBody::addVelocity(const btVector3& velocity,int node) -{ - Node& n=m_nodes[node]; - if(n.m_im>0) - { - n.m_v += velocity; - } -} - -// -void btSoftBody::setMass(int node,btScalar mass) -{ - m_nodes[node].m_im=mass>0?1/mass:0; - m_bUpdateRtCst=true; -} - -// -btScalar btSoftBody::getMass(int node) const -{ - return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0); -} - -// -btScalar btSoftBody::getTotalMass() const -{ - btScalar mass=0; - for(int i=0;i<m_nodes.size();++i) - { - mass+=getMass(i); - } - return(mass); -} - -// -void btSoftBody::setTotalMass(btScalar mass,bool fromfaces) -{ - int i; - - if(fromfaces) - { - - for(i=0;i<m_nodes.size();++i) - { - m_nodes[i].m_im=0; - } - 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) - { - f.m_n[j]->m_im+=twicearea; - } - } - for( i=0;i<m_nodes.size();++i) - { - 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) - { - m_nodes[i].m_im/=itm*mass; - } - m_bUpdateRtCst=true; -} - -// -void btSoftBody::setTotalDensity(btScalar density) -{ - setTotalMass(getVolume()*density,true); -} - -// -void btSoftBody::setVolumeMass(btScalar mass) -{ -btAlignedObjectArray<btScalar> ranks; -ranks.resize(m_nodes.size(),0); -int i; - -for(i=0;i<m_nodes.size();++i) - { - m_nodes[i].m_im=0; - } -for(i=0;i<m_tetras.size();++i) - { - 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; - } - } -for( i=0;i<m_nodes.size();++i) - { - if(m_nodes[i].m_im>0) - { - m_nodes[i].m_im=ranks[i]/m_nodes[i].m_im; - } - } -setTotalMass(mass,false); -} - -// -void btSoftBody::setVolumeDensity(btScalar density) -{ -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) - { - volume+=btFabs(t.m_rv); - } - } -setVolumeMass(volume*density/6); -} - -// -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) - { - 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) -{ - btTransform t; - t.setIdentity(); - t.setOrigin(trs); - transform(t); -} - -// -void btSoftBody::rotate( const btQuaternion& rot) -{ - btTransform t; - t.setIdentity(); - t.setRotation(rot); - transform(t); -} - -// -void btSoftBody::scale(const btVector3& scl) -{ - - 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*=scl; - n.m_q*=scl; - vol = btDbvtVolume::FromCR(n.m_x,margin); - m_ndbvt.update(n.m_leaf,vol); - } - updateNormals(); - updateBounds(); - updateConstants(); -} - -// -btScalar btSoftBody::getRestLengthScale() -{ - return m_restLengthScale; -} - -// -void btSoftBody::setRestLengthScale(btScalar restLengthScale) -{ - 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; - } - m_restLengthScale = restLengthScale; - - if (getActivationState() == ISLAND_SLEEPING) - activate(); -} - -// -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_wgh.resize(m_nodes.size()); - for(i=0,ni=m_nodes.size();i<ni;++i) - { - if(m_nodes[i].m_im<=0) tmass+=kmass; - } - 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; - } - /* Pos */ - const btVector3 com=evaluateCom(); - m_pose.m_pos.resize(m_nodes.size()); - for( i=0,ni=m_nodes.size();i<ni;++i) - { - 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_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(); - - updateConstants(); -} - -void btSoftBody::resetLinkRestLengths() -{ - 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; - } -} - -// -btScalar btSoftBody::getVolume() const -{ - btScalar vol=0; - if(m_nodes.size()>0) - { - int i,ni; - - 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)); - } - vol/=(btScalar)6; - } - return(vol); -} - -// -int btSoftBody::clusterCount() const -{ - return(m_clusters.size()); -} - -// -btVector3 btSoftBody::clusterCom(const Cluster* cluster) -{ - 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]; - } - return(com*cluster->m_imass); -} - -// -btVector3 btSoftBody::clusterCom(int cluster) const -{ - return(clusterCom(m_clusters[cluster])); -} - -// -btVector3 btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos) -{ - return(cluster->m_lv+btCross(cluster->m_av,rpos)); -} - -// -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; - cluster->m_nvimpulses++; -} - -// -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; - cluster->m_ndimpulses++; -} - -// -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); -} - -// -void btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse) -{ - 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) -{ - const btVector3 ai=cluster->m_invwi*impulse; - cluster->m_dimpulses[1]+=ai; - cluster->m_ndimpulses++; -} - -// -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); -} - -// -void btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse) -{ - cluster->m_dimpulses[0]+=impulse*cluster->m_imass; - cluster->m_ndimpulses++; -} - -struct NodeLinks -{ - btAlignedObjectArray<int> m_links; -}; - - - -// -int btSoftBody::generateBendingConstraints(int distance,Material* mat) -{ - int i,j; - - if(distance>1) - { - /* 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) - { - for(i=0;i<n;++i) - { - if(i!=j) - { - adj[IDX(i,j)]=adj[IDX(j,i)]=inf; - } - else - { - adj[IDX(i,j)]=adj[IDX(j,i)]=0; - } - } - } - 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; - } - - - //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) - { - 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()) - nodeLinks[ib].m_links.push_back(ia); - } - for (int ii=0;ii<nodeLinks.size();ii++) - { - int i=ii; - - 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++) - { - int j = nodeLinks[k].m_links[kk]; - if (i!=j) - { - 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; - } - } - - } - } - } - } else - { - ///generic Floyd's algorithm - for(int k=0;k<n;++k) - { - for(j=0;j<n;++j) - { - for(i=j+1;i<n;++i) - { - 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; - } - } - } - } - } - - - /* Build links */ - int nlinks=0; - for(j=0;j<n;++j) - { - for(i=j+1;i<n;++i) - { - if(adj[IDX(i,j)]==(unsigned)distance) - { - appendLink(i,j,mat); - m_links[m_links.size()-1].m_bbending=1; - ++nlinks; - } - } - } - delete[] adj; - return(nlinks); - } - return(0); -} - -// -void btSoftBody::randomizeConstraints() -{ - unsigned long seed=243703; -#define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff) - int i,ni; - - for(i=0,ni=m_links.size();i<ni;++i) - { - btSwap(m_links[i],m_links[NEXTRAND%ni]); - } - for(i=0,ni=m_faces.size();i<ni;++i) - { - btSwap(m_faces[i],m_faces[NEXTRAND%ni]); - } -#undef NEXTRAND -} - -// -void btSoftBody::releaseCluster(int index) -{ - 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() -{ - while(m_clusters.size()>0) releaseCluster(0); -} - -// -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++; - int i; - - for(i=0;i<k;++i) - { - 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; - } - 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; - m_clusters[i]->m_nodes.resize(0); - } - } - 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 btScalar d=ClusterMetric(centers[j],nx); - if(d<kdist) - { - 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) - { - for(int j=0;j<m_clusters[i]->m_nodes.size();++j) - { - cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=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 cid=cids[idx[j]]; - for(int q=1;q<3;++q) - { - 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()) - { - m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]); - } - } - } - } - } - /* Master */ - if(m_clusters.size()>1) - { - 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]); - m_clusters.push_back(pmaster); - btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]); - } - /* Terminate */ - for(i=0;i<m_clusters.size();++i) - { - if(m_clusters[i]->m_nodes.size()==0) - { - releaseCluster(i--); - } - } - } 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) - { - m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); - m_clusters[i]->m_collide= true; - } - for (i=0;i<m_tetras.size();i++) - { - for (int j=0;j<4;j++) - { - m_clusters[i]->m_nodes.push_back(m_tetras[i].m_n[j]); - } - } - - } else - { - m_clusters.resize(m_faces.size()); - for(i=0;i<m_clusters.size();++i) - { - m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); - m_clusters[i]->m_collide= true; - } - - for(i=0;i<m_faces.size();++i) - { - for(int j=0;j<3;++j) - { - m_clusters[i]->m_nodes.push_back(m_faces[i].m_n[j]); - } - } - } - } - - if (m_clusters.size()) - { - initializeClusters(); - updateClusters(); - - - //for self-collision - m_clusterConnectivity.resize(m_clusters.size()*m_clusters.size()); - { - for (int c0=0;c0<m_clusters.size();c0++) - { - m_clusters[c0]->m_clusterIndex=c0; - for (int c1=0;c1<m_clusters.size();c1++) - { - - 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 j=0;j<clb->m_nodes.size();j++) - { - if (cla->m_nodes[i] == clb->m_nodes[j]) - { - connected=true; - break; - } - } - } - m_clusterConnectivity[c0+c1*m_clusters.size()]=connected; - } - } - } - } - - return(m_clusters.size()); -} - -// -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) - { - 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))) - { - 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) - { - 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; - } - /* Intersect */ - for(i=0;i<ncount;++i) - { - for(j=i+1;j<ncount;++j) - { - 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) - { - 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) - { - 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; } - } - else - { - if(b.m_im>0) - { b.m_im/=0.5f;m=1/b.m_im; } - else - m=0; - } - 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) - { - 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) - { - 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]]; - } - } - } - /* 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++) - { - if((idx[j]<ncount)&&(idx[k]<ncount)) - { - 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; - } - } - } - } - /* 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 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) - { - 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; - } - else - { - if(( (ifn->Eval(m_nodes[id[0]].m_x)<accurary)&& - (ifn->Eval(m_nodes[id[1]].m_x)<accurary))) - todetach=i; - } - if(todetach) - { - 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]; - } - } - } - /* 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)) - { - for(int j=0;j<3;++j) - { - 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) - { - 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(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++; - } - 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]) - { - --ranks[id[0]]; - --ranks[id[1]]; - btSwap(m_links[i],m_links[m_links.size()-1]); - m_links.pop_back();--i; - } - } -#if 0 - for(i=nnodes-1;i>=0;--i) - { - if(!ranks[i]) todelete.push_back(i); - } - if(todelete.size()) - { - btAlignedObjectArray<int>& map=ranks; - for(int i=0;i<nnodes;++i) map[i]=i; - PointersToIndices(this); - for(int i=0,ni=todelete.size();i<ni;++i) - { - int j=todelete[i]; - int& a=map[j]; - int& b=map[--nnodes]; - m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0; - btSwap(m_nodes[a],m_nodes[b]); - j=a;a=b;b=j; - } - IndicesToPointers(this,&map[0]); - m_nodes.resize(nnodes); - } -#endif - } - m_bUpdateRtCst=true; -} - -// -bool btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position) -{ - return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),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) - { - 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; - } - } - for(i=0,ni=m_faces.size();i<ni;++i) - { - 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) - { - 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); - } - } - } - 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); -} - -// -bool btSoftBody::rayTest(const btVector3& rayFrom, - const btVector3& rayTo, - sRayCast& results) -{ - if(m_faces.size()&&m_fdbvt.empty()) - initializeFaceTree(); - - results.body = this; - results.fraction = 1.f; - results.feature = eFeature::None; - results.index = -1; - - return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0); -} - -// -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; - } -} - -// -void btSoftBody::predictMotion(btScalar dt) -{ - - int i,ni; - - /* Update */ - if(m_bUpdateRtCst) - { - m_bUpdateRtCst=false; - updateConstants(); - m_fdbvt.clear(); - if(m_cfg.collisions&fCollision::VF_SS) - { - 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); - applyForces(); - /* 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; - { - btScalar maxDisplacement = m_worldInfo->m_maxDisplacement; - btScalar clampDeltaV = maxDisplacement/m_sst.sdt; - for (int c=0;c<3;c++) - { - if (deltaV[c]>clampDeltaV) - { - deltaV[c] = clampDeltaV; - } - if (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); - } - /* 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 */ - updatePose(); - /* 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) - { - 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); - } - } - } - /* Clear contacts */ - m_rcontacts.resize(0); - m_scontacts.resize(0); - /* Optimize dbvt's */ - m_ndbvt.optimizeIncremental(1); - m_fdbvt.optimizeIncremental(1); - m_cdbvt.optimizeIncremental(1); -} - -// -void btSoftBody::solveConstraints() -{ - - /* Apply clusters */ - applyClusters(false); - /* Prepare links */ - - int i,ni; - - 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); - } - /* 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; - a.m_body->activate(); - } - /* Solve velocities */ - if(m_cfg.viterations>0) - { - /* Solve */ - for(int isolve=0;isolve<m_cfg.viterations;++isolve) - { - for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq) - { - getSolver(m_cfg.m_vsequence[iseq])(this,1); - } - } - /* 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; - } - } - /* Solve positions */ - if(m_cfg.piterations>0) - { - 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) - { - 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) - { - 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) - { - 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; - } - for(int idrift=0;idrift<m_cfg.diterations;++idrift) - { - for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq) - { - getSolver(m_cfg.m_dsequence[iseq])(this,1,0); - } - } - 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; - } - } - /* Apply clusters */ - dampClusters(); - applyClusters(true); -} - -// -void btSoftBody::staticSolve(int iterations) -{ - for(int isolve=0;isolve<iterations;++isolve) - { - for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq) - { - getSolver(m_cfg.m_psequence[iseq])(this,1,0); - } - } -} - -// -void btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/) -{ - /// placeholder -} - -// -void btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies) -{ - const int nb=bodies.size(); - int iterations=0; - int i; - - for(i=0;i<nb;++i) - { - iterations=btMax(iterations,bodies[i]->m_cfg.citerations); - } - for(i=0;i<nb;++i) - { - bodies[i]->prepareClusters(iterations); - } - for(i=0;i<iterations;++i) - { - const btScalar sor=1; - for(int j=0;j<nb;++j) - { - bodies[j]->solveClusters(sor); - } - } - for(i=0;i<nb;++i) - { - bodies[i]->cleanupClusters(); - } -} - -// -void btSoftBody::integrateMotion() -{ - /* Update */ - updateNormals(); -} - -// -btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt) -{ - m_rayFrom = rayFrom; - m_rayNormalizedDirection = (rayTo-rayFrom); - m_rayTo = rayTo; - m_mint = mxt; - m_face = 0; - m_tests = 0; -} - -// -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; - } - ++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) -{ - 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 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)) - { - return(t); - } - } - } - return(-1); -} - -// -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; - - for(i=0,ni=m_nodes.size();i<ni;++i) - { - if(m_nodes[i].m_leaf) - { - m_nodes[i].m_leaf->data=*(void**)&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); - } - 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_leaf->data=*(void**)&i; - } - } - for(i=0,ni=m_anchors.size();i<ni;++i) - { - m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base); - } - for(i=0,ni=m_notes.size();i<ni;++i) - { - 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); - } - } -#undef PTR2IDX -} - -// -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; - - for(i=0,ni=m_nodes.size();i<ni;++i) - { - if(m_nodes[i].m_leaf) - { - m_nodes[i].m_leaf->data=&m_nodes[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); - } - 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_leaf->data=&m_faces[i]; - } - } - for(i=0,ni=m_anchors.size();i<ni;++i) - { - m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base); - } - for(i=0,ni=m_notes.size();i<ni;++i) - { - 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); - } - } -#undef IDX2PTR -} - -// -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; - - - 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]; - - 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) - { - feature=btSoftBody::eFeature::Face; - index=i; - mint=t; - } - } - } - } - else - {/* Use dbvt */ - RayFromToCaster collider(rayFrom,rayTo,mint); - - 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; - } - } - - 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 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); -} - -// -void btSoftBody::initializeFaceTree() -{ - m_fdbvt.clear(); - for(int i=0;i<m_faces.size();++i) - { - Face& f=m_faces[i]; - f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f); - } -} - -// -btVector3 btSoftBody::evaluateCom() const -{ - btVector3 com(0,0,0); - if(m_pose.m_bframe) - { - for(int i=0,ni=m_nodes.size();i<ni;++i) - { - com+=m_nodes[i].m_x*m_pose.m_wgh[i]; - } - } - return(com); -} - -// -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 btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform(); - const btTransform &wtr = colObjWrap->getWorldTransform(); - //todo: check which transform is needed here - - btScalar dst = - m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(x), - shp, - nrm, - margin); - 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); - } - return(false); -} - -// -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; - } - 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; - } - for(i=0,ni=m_nodes.size();i<ni;++i) - { - btScalar len = m_nodes[i].m_n.length(); - if (len>SIMD_EPSILON) - m_nodes[i].m_n /= len; - } -} - -// -void btSoftBody::updateBounds() -{ - /*if( m_acceleratedSoftBody ) - { - // If we have an accelerated softbody we need to obtain the bounds correctly - // For now (slightly hackily) just have a very large AABB - // TODO: Write get bounds kernel - // If that is updating in place, atomic collisions might be low (when the cloth isn't perfectly aligned to an axis) and we could - // probably do a test and exchange reasonably efficiently. - - m_bounds[0] = btVector3(-1000, -1000, -1000); - 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); - } - } - else - { - m_bounds[0]= - m_bounds[1]=btVector3(0,0,0); - } - //} -} - - -// -void btSoftBody::updatePose() -{ - 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) - { - 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) - { - 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) -{ - int i,ni; - - /* 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); - } - - /* Node area */ - - if (averageArea) - { - 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; - } - for(i=0,ni=m_faces.size();i<ni;++i) - { - btSoftBody::Face& f=m_faces[i]; - for(int j=0;j<3;++j) - { - const int index=(int)(f.m_n[j]-&m_nodes[0]); - counts[index]++; - f.m_n[j]->m_area+=btFabs(f.m_ra); - } - } - for(i=0,ni=m_nodes.size();i<ni;++i) - { - if(counts[i]>0) - m_nodes[i].m_area/=(btScalar)counts[i]; - else - m_nodes[i].m_area=0; - } - } - else - { - // initialize node area as zero - for(i=0,ni=m_nodes.size();i<ni;++i) - { - m_nodes[i].m_area=0; - } - - for(i=0,ni=m_faces.size();i<ni;++i) - { - btSoftBody::Face& f=m_faces[i]; - - 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) - { - m_nodes[i].m_area *= 0.3333333f; - } - } -} - - -void btSoftBody::updateLinkConstants() -{ - int i,ni; - - /* 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; - } -} - -void btSoftBody::updateConstants() -{ - resetLinkRestLengths(); - updateLinkConstants(); - updateArea(); -} - - - -// -void btSoftBody::initializeClusters() -{ - int i; - - for( i=0;i<m_clusters.size();++i) - { - 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) - { - if (c.m_nodes[j]->m_im==0) - { - c.m_containsAnchor = true; - c.m_masses[j] = BT_LARGE_FLOAT; - } else - { - c.m_masses[j] = btScalar(1.)/c.m_nodes[j]->m_im; - } - 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); - { - int i,ni; - - 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]; - } - } - ii[1][0]=ii[0][1]; - ii[2][0]=ii[0][2]; - ii[2][1]=ii[1][2]; - - ii = ii.inverse(); - - /* 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) - { - c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com; - } - } - } -} - -// -void btSoftBody::updateClusters() -{ - BT_PROFILE("UpdateClusters"); - int i; - - for(i=0;i<m_clusters.size();++i) - { - 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); - 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(); -#else -#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, - btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0, - 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(); -#endif -#endif - /* Velocities */ - c.m_lv=btVector3(0,0,0); - c.m_av=btVector3(0,0,0); - { - int 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); - } - } - 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); - } - } - /* Dbvt */ - if(c.m_collide) - { - 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); - else - c.m_leaf=m_cdbvt.insert(bounds,&c); - } - } - } - - -} - - - - -// -void btSoftBody::cleanupClusters() -{ - for(int i=0;i<m_joints.size();++i) - { - m_joints[i]->Terminate(m_sst.sdt); - if(m_joints[i]->m_delete) - { - btAlignedFree(m_joints[i]); - m_joints.remove(m_joints[i--]); - } - } -} - -// -void btSoftBody::prepareClusters(int iterations) -{ - for(int i=0;i<m_joints.size();++i) - { - m_joints[i]->Prepare(m_sst.sdt,iterations); - } -} - - -// -void btSoftBody::solveClusters(btScalar sor) -{ - for(int i=0,ni=m_joints.size();i<ni;++i) - { - m_joints[i]->Solve(m_sst.sdt,sor); - } -} - -// -void btSoftBody::applyClusters(bool drift) -{ - BT_PROFILE("ApplyClusters"); -// 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); - int i; - - if(drift) - { - for(i=0;i<m_clusters.size();++i) - { - 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; - } - } - } - - for(i=0;i<m_clusters.size();++i) - { - 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 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) - { - if(weights[i]>0) - { - m_nodes[i].m_x+=deltas[i]/weights[i]; - } - } -} - -// -void btSoftBody::dampClusters() -{ - int i; - - for(i=0;i<m_clusters.size();++i) - { - Cluster& c=*m_clusters[i]; - if(c.m_ndamping>0) - { - for(int j=0;j<c.m_nodes.size();++j) - { - 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); - } - } - } - } - } -} - -// -void btSoftBody::Joint::Prepare(btScalar dt,int) -{ - m_bodies[0].activate(); - m_bodies[1].activate(); -} - -// -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) - { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; - } - m_drift /=(btScalar)iterations; -} - -// -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]); -} - -// -void btSoftBody::LJoint::Terminate(btScalar dt) -{ - if(m_split>0) - { - 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) -{ - 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) - { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; - } - m_drift /=(btScalar)iterations; -} - -// -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; - m_bodies[0].applyAImpulse(-impulse); - m_bodies[1].applyAImpulse( impulse); -} - -// -void btSoftBody::AJoint::Terminate(btScalar dt) -{ - if(m_split>0) - { - m_bodies[0].applyDAImpulse(-m_sdrift); - m_bodies[1].applyDAImpulse( m_sdrift); - } -} - -// -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) - { - 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_drift/=(btScalar)iterations; - } - else - { - m_drift=m_sdrift=btVector3(0,0,0); - } -} - -// -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 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) - { - 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) - { - - } 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]); - } - } - } - } else - { - m_bodies[0].applyImpulse(-impulse,m_rpos[0]); - m_bodies[1].applyImpulse( impulse,m_rpos[1]); - } -} - -// -void btSoftBody::CJoint::Terminate(btScalar dt) -{ - if(m_split>0) - { - m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); - m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); - } -} - -// -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 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) - { - volume = getVolume(); - ivolumetp = 1/btFabs(volume)*kPR; - dvolumetv = (m_pose.m_volume-volume)*kVC; - } - /* Per vertex forces */ - int i,ni; - - for(i=0,ni=m_nodes.size();i<ni;++i) - { - btSoftBody::Node& n=m_nodes[i]; - if(n.m_im>0) - { - if(use_medium) - { - /* Aerodynamics */ - addAeroForceToNode(m_windVelocity, i); - } - /* Pressure */ - if(as_pressure) - { - n.m_f += n.m_n*(n.m_area*ivolumetp); - } - /* Volume */ - if(as_volume) - { - n.m_f += n.m_n*(n.m_area*dvolumetv); - } - } - } - - /* Per face forces */ - for(i=0,ni=m_faces.size();i<ni;++i) - { - // btSoftBody::Face& f=m_faces[i]; - - /* Aerodynamics */ - addAeroForceToFace(m_windVelocity, i); - } -} - -// -void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti) -{ - 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); - } -} - - -// -void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti) -{ - 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()) - { - 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); - // 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 ); - c.m_node->m_x -= impulse * c.m_c2; - - 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) -{ - 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 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()); - } -} - -// -void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti) -{ -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) - { - 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) -{ - 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) -{ - 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); - default: - { - } - } - return(0); -} - -// -btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) -{ - switch(solver) - { - case eVSolver::Linear: return(&btSoftBody::VSolve_Links); - default: - { - } - } - return(0); -} - -// -void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap) -{ - - switch(m_cfg.collisions&fCollision::RVSmask) - { - 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; - docollide.m_colObj1Wrap = pcoWrap; - docollide.m_rigidBody = prb1; - - docollide.dynmargin = basemargin+timemargin; - docollide.stamargin = basemargin; - m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide); - } - break; - case fCollision::CL_RS: - { - btSoftColliders::CollideCL_RS collider; - collider.ProcessColObj(this,pcoWrap); - } - break; - } -} - -// -void btSoftBody::defaultCollisionHandler(btSoftBody* psb) -{ - const int cf=m_cfg.collisions&psb->m_cfg.collisions; - switch(cf&fCollision::SVSmask) - { - case fCollision::CL_SS: - { - - //support self-collision if CL_SELF flag set - if (this!=psb || psb->m_cfg.collisions&fCollision::CL_SELF) - { - btSoftColliders::CollideCL_SS docollide; - docollide.ProcessSoftSoft(this,psb); - } - - } - break; - 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); - } - } - break; - default: - { - - } - } -} - - - -void btSoftBody::setWindVelocity( const btVector3 &velocity ) -{ - m_windVelocity = velocity; -} - - -const btVector3& btSoftBody::getWindVelocity() -{ - return m_windVelocity; -} - - - -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 -{ - btSoftBodyData* sbd = (btSoftBodyData*) dataBuffer; - - btCollisionObject::serialize(&sbd->m_collisionObjectData, serializer); - - btHashMap<btHashPtr,int> m_nodeIndexMap; - - sbd->m_numMaterials = m_materials.size(); - 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); - //SoftBodyMaterialData** memPtr = chunk->m_oldPtr; - SoftBodyMaterialData** memPtr = (SoftBodyMaterialData**)chunk->m_oldPtr; - 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); - 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_ARRAY_CODE,(void*) &m_materials); - } - - - - - sbd->m_numNodes = m_nodes.size(); - 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); - SoftBodyNodeData* memPtr = (SoftBodyNodeData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) - { - 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; - 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); - } - 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; - if (sbd->m_links) - { - int sz = sizeof(SoftBodyLinkData); - int numElem = sbd->m_numLinks; - btChunk* chunk = serializer->allocate(sz,numElem); - SoftBodyLinkData* memPtr = (SoftBodyLinkData*)chunk->m_oldPtr; - 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_restLength = m_links[i].m_rl; - } - 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; - if (sbd->m_faces) - { - int sz = sizeof(SoftBodyFaceData); - int numElem = sbd->m_numFaces; - btChunk* chunk = serializer->allocate(sz,numElem); - SoftBodyFaceData* memPtr = (SoftBodyFaceData*)chunk->m_oldPtr; - 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_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]); - } - - - sbd->m_numTetrahedra = m_tetras.size(); - 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); - SoftBodyTetraData* memPtr = (SoftBodyTetraData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) - { - 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; - } - 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_restVolume = m_tetras[i].m_rv; - } - 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; - if (sbd->m_anchors) - { - int sz = sizeof(SoftRigidAnchorData); - int numElem = sbd->m_numAnchors; - btChunk* chunk = serializer->allocate(sz,numElem); - SoftRigidAnchorData* memPtr = (SoftRigidAnchorData*)chunk->m_oldPtr; - 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; - btAssert(memPtr->m_nodeIndex < m_nodes.size()); - } - 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; - sbd->m_config.m_pressure = m_cfg.kPR; - sbd->m_config.m_aeroModel = this->m_cfg.aeromodel; - sbd->m_config.m_lift = m_cfg.kLF; - sbd->m_config.m_drag = m_cfg.kDG; - sbd->m_config.m_positionIterations = m_cfg.piterations; - sbd->m_config.m_driftIterations = m_cfg.diterations; - sbd->m_config.m_clusterIterations = m_cfg.citerations; - sbd->m_config.m_velocityIterations = m_cfg.viterations; - sbd->m_config.m_maxVolume = m_cfg.maxvolume; - sbd->m_config.m_damping = m_cfg.kDP; - sbd->m_config.m_poseMatch = m_cfg.kMT; - sbd->m_config.m_collisionFlags = m_cfg.collisions; - sbd->m_config.m_volume = m_cfg.kVC; - sbd->m_config.m_rigidContactHardness = m_cfg.kCHR; - sbd->m_config.m_kineticContactHardness = m_cfg.kKHR; - sbd->m_config.m_softContactHardness = m_cfg.kSHR; - sbd->m_config.m_anchorHardness = m_cfg.kAHR; - sbd->m_config.m_timeScale = m_cfg.timescale; - sbd->m_config.m_maxVolume = m_cfg.maxvolume; - sbd->m_config.m_softRigidClusterHardness = m_cfg.kSRHR_CL; - sbd->m_config.m_softKineticClusterHardness = m_cfg.kSKHR_CL; - sbd->m_config.m_softSoftClusterHardness = m_cfg.kSSHR_CL; - sbd->m_config.m_softRigidClusterImpulseSplit = m_cfg.kSR_SPLT_CL; - sbd->m_config.m_softKineticClusterImpulseSplit = m_cfg.kSK_SPLT_CL; - sbd->m_config.m_softSoftClusterImpulseSplit = m_cfg.kSS_SPLT_CL; - - //pose for shape matching - { - sbd->m_pose = (SoftBodyPoseData*)serializer->getUniquePointer((void*)&m_pose); - - int sz = sizeof(SoftBodyPoseData); - 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; - if (memPtr->m_numPositions) - { - int numElem = memPtr->m_numPositions; - int sz = sizeof(btVector3Data); - btChunk* chunk = serializer->allocate(sz,numElem); - btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr; - 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]); - } - 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; - 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++) - { - *memPtr = m_pose.m_wgh[i]; - } - serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_pose.m_wgh[0]); - } - - 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; - 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++) - { - 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; - m_clusters[i]->m_com.serializeFloat(memPtr->m_com); - memPtr->m_containsAnchor = m_clusters[i]->m_containsAnchor; - m_clusters[i]->m_dimpulses[0].serializeFloat(memPtr->m_dimpulses[0]); - m_clusters[i]->m_dimpulses[1].serializeFloat(memPtr->m_dimpulses[1]); - m_clusters[i]->m_framexform.serializeFloat(memPtr->m_framexform); - memPtr->m_idmass = m_clusters[i]->m_idmass; - memPtr->m_imass = m_clusters[i]->m_imass; - m_clusters[i]->m_invwi.serializeFloat(memPtr->m_invwi); - memPtr->m_ldamping = m_clusters[i]->m_ldamping; - m_clusters[i]->m_locii.serializeFloat(memPtr->m_locii); - m_clusters[i]->m_lv.serializeFloat(memPtr->m_lv); - memPtr->m_matching = m_clusters[i]->m_matching; - memPtr->m_maxSelfCollisionImpulse = m_clusters[i]->m_maxSelfCollisionImpulse; - memPtr->m_ndamping = m_clusters[i]->m_ndamping; - memPtr->m_ldamping = m_clusters[i]->m_ldamping; - memPtr->m_adamping = m_clusters[i]->m_adamping; - memPtr->m_selfCollisionImpulseFactor = m_clusters[i]->m_selfCollisionImpulseFactor; - - memPtr->m_numFrameRefs = m_clusters[i]->m_framerefs.size(); - memPtr->m_numMasses = m_clusters[i]->m_masses.size(); - memPtr->m_numNodes = m_clusters[i]->m_nodes.size(); - - memPtr->m_nvimpulses = m_clusters[i]->m_nvimpulses; - m_clusters[i]->m_vimpulses[0].serializeFloat(memPtr->m_vimpulses[0]); - 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; - 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++) - { - m_clusters[i]->m_framerefs[j].serializeFloat(*memPtr); - } - 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; - 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++) - { - *memPtr = m_clusters[i]->m_masses[j]; - } - 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 ) - { - 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++) - { - 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,"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; - - if (sbd->m_joints) - { - int sz = sizeof(btSoftBodyJointData); - int numElem = m_joints.size(); - btChunk* chunk = serializer->allocate(sz,numElem); - btSoftBodyJointData* memPtr = (btSoftBodyJointData*)chunk->m_oldPtr; - - 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]); - m_joints[i]->m_refs[1].serializeFloat(memPtr->m_refs[1]); - memPtr->m_cfm = m_joints[i]->m_cfm; - 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++) - { - memPtr->m_relPosition[0].m_floats[j] = 0.f; - memPtr->m_relPosition[1].m_floats[j] = 0.f; - } - memPtr->m_bodyA = 0; - memPtr->m_bodyB = 0; - if (m_joints[i]->m_bodies[0].m_soft) - { - memPtr->m_bodyAtype = BT_JOINT_SOFT_BODY_CLUSTER; - memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_soft); - } - if (m_joints[i]->m_bodies[0].m_collisionObject) - { - memPtr->m_bodyAtype = BT_JOINT_COLLISION_OBJECT; - memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_collisionObject); - } - if (m_joints[i]->m_bodies[0].m_rigid) - { - memPtr->m_bodyAtype = BT_JOINT_RIGID_BODY; - memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_rigid); - } - - if (m_joints[i]->m_bodies[1].m_soft) - { - memPtr->m_bodyBtype = BT_JOINT_SOFT_BODY_CLUSTER; - memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_soft); - } - if (m_joints[i]->m_bodies[1].m_collisionObject) - { - memPtr->m_bodyBtype = BT_JOINT_COLLISION_OBJECT; - memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_collisionObject); - } - if (m_joints[i]->m_bodies[1].m_rigid) - { - memPtr->m_bodyBtype = BT_JOINT_RIGID_BODY; - 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]); - } - - - return btSoftBodyDataName; -} - diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftBody.h b/thirdparty/bullet/src/BulletSoftBody/btSoftBody.h deleted file mode 100644 index ada0dfd1a5..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftBody.h +++ /dev/null @@ -1,1005 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btSoftBody implementation by Nathanael Presson - -#ifndef _BT_SOFT_BODY_H -#define _BT_SOFT_BODY_H - -#include "LinearMath/btAlignedObjectArray.h" -#include "LinearMath/btTransform.h" -#include "LinearMath/btIDebugDraw.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" - -#include "BulletCollision/CollisionShapes/btConcaveShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -#include "btSparseSDF.h" -#include "BulletCollision/BroadphaseCollision/btDbvt.h" - -//#ifdef BT_USE_DOUBLE_PRECISION -//#define btRigidBodyData btRigidBodyDoubleData -//#define btRigidBodyDataName "btRigidBodyDoubleData" -//#else -#define btSoftBodyData btSoftBodyFloatData -#define btSoftBodyDataName "btSoftBodyFloatData" -//#endif //BT_USE_DOUBLE_PRECISION - -class btBroadphaseInterface; -class btDispatcher; -class btSoftBodySolver; - -/* btSoftBodyWorldInfo */ -struct btSoftBodyWorldInfo -{ - btScalar air_density; - btScalar water_density; - btScalar water_offset; - btScalar m_maxDisplacement; - btVector3 water_normal; - btBroadphaseInterface* m_broadphase; - btDispatcher* m_dispatcher; - btVector3 m_gravity; - btSparseSdf<3> m_sparsesdf; - - btSoftBodyWorldInfo() - :air_density((btScalar)1.2), - water_density(0), - water_offset(0), - m_maxDisplacement(1000.f),//avoid soft body from 'exploding' so use some upper threshold of maximum motion that a node can travel per frame - water_normal(0,0,0), - m_broadphase(0), - m_dispatcher(0), - m_gravity(0,-10,0) - { - } -}; - - -///The btSoftBody is an class to simulate cloth and volumetric soft bodies. -///There is two-way interaction between btSoftBody and btRigidBody/btCollisionObject. -class btSoftBody : public btCollisionObject -{ -public: - btAlignedObjectArray<const class btCollisionObject*> m_collisionDisabledObjects; - - // The solver object that handles this soft body - btSoftBodySolver *m_softBodySolver; - - // - // Enumerations - // - - ///eAeroModel - struct eAeroModel { enum _ { - V_Point, ///Vertex normals are oriented toward velocity - V_TwoSided, ///Vertex normals are flipped to match velocity - V_TwoSidedLiftDrag, ///Vertex normals are flipped to match velocity and lift and drag forces are applied - V_OneSided, ///Vertex normals are taken as it is - F_TwoSided, ///Face normals are flipped to match velocity - F_TwoSidedLiftDrag, ///Face normals are flipped to match velocity and lift and drag forces are applied - F_OneSided, ///Face normals are taken as it is - END - };}; - - ///eVSolver : velocities solvers - struct eVSolver { enum _ { - Linear, ///Linear solver - END - };}; - - ///ePSolver : positions solvers - struct ePSolver { enum _ { - Linear, ///Linear solver - Anchors, ///Anchor solver - RContacts, ///Rigid contacts solver - SContacts, ///Soft contacts solver - END - };}; - - ///eSolverPresets - struct eSolverPresets { enum _ { - Positions, - Velocities, - Default = Positions, - END - };}; - - ///eFeature - struct eFeature { enum _ { - None, - Node, - Link, - Face, - Tetra, - END - };}; - - typedef btAlignedObjectArray<eVSolver::_> tVSolverArray; - typedef btAlignedObjectArray<ePSolver::_> tPSolverArray; - - // - // Flags - // - - ///fCollision - struct fCollision { enum _ { - RVSmask = 0x000f, ///Rigid versus soft mask - SDF_RS = 0x0001, ///SDF based rigid vs soft - CL_RS = 0x0002, ///Cluster vs convex rigid vs soft - - SVSmask = 0x0030, ///Rigid versus soft mask - VF_SS = 0x0010, ///Vertex vs face soft vs soft handling - CL_SS = 0x0020, ///Cluster vs cluster soft vs soft handling - CL_SELF = 0x0040, ///Cluster soft body self collision - /* presets */ - Default = SDF_RS, - END - };}; - - ///fMaterial - struct fMaterial { enum _ { - DebugDraw = 0x0001, /// Enable debug draw - /* presets */ - Default = DebugDraw, - END - };}; - - // - // API Types - // - - /* sRayCast */ - struct sRayCast - { - btSoftBody* body; /// soft body - eFeature::_ feature; /// feature type - int index; /// feature index - btScalar fraction; /// time of impact fraction (rayorg+(rayto-rayfrom)*fraction) - }; - - /* ImplicitFn */ - struct ImplicitFn - { - virtual ~ImplicitFn() {} - virtual btScalar Eval(const btVector3& x)=0; - }; - - // - // Internal types - // - - typedef btAlignedObjectArray<btScalar> tScalarArray; - typedef btAlignedObjectArray<btVector3> tVector3Array; - - /* sCti is Softbody contact info */ - struct sCti - { - const btCollisionObject* m_colObj; /* Rigid body */ - btVector3 m_normal; /* Outward normal */ - btScalar m_offset; /* Offset from origin */ - }; - - /* sMedium */ - struct sMedium - { - btVector3 m_velocity; /* Velocity */ - btScalar m_pressure; /* Pressure */ - btScalar m_density; /* Density */ - }; - - /* Base type */ - struct Element - { - void* m_tag; // User data - Element() : m_tag(0) {} - }; - /* Material */ - struct Material : Element - { - btScalar m_kLST; // Linear stiffness coefficient [0,1] - btScalar m_kAST; // Area/Angular stiffness coefficient [0,1] - btScalar m_kVST; // Volume stiffness coefficient [0,1] - int m_flags; // Flags - }; - - /* Feature */ - struct Feature : Element - { - Material* m_material; // Material - }; - /* Node */ - struct Node : Feature - { - btVector3 m_x; // Position - btVector3 m_q; // Previous step position - btVector3 m_v; // Velocity - btVector3 m_f; // Force accumulator - btVector3 m_n; // Normal - btScalar m_im; // 1/mass - btScalar m_area; // Area - btDbvtNode* m_leaf; // Leaf data - int m_battach:1; // Attached - }; - /* Link */ - ATTRIBUTE_ALIGNED16(struct) Link : Feature - { - btVector3 m_c3; // gradient - Node* m_n[2]; // Node pointers - btScalar m_rl; // Rest length - int m_bbending:1; // Bending link - btScalar m_c0; // (ima+imb)*kLST - btScalar m_c1; // rl^2 - btScalar m_c2; // |gradient|^2/c0 - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - }; - /* Face */ - struct Face : Feature - { - Node* m_n[3]; // Node pointers - btVector3 m_normal; // Normal - btScalar m_ra; // Rest area - btDbvtNode* m_leaf; // Leaf data - }; - /* Tetra */ - struct Tetra : Feature - { - Node* m_n[4]; // Node pointers - btScalar m_rv; // Rest volume - btDbvtNode* m_leaf; // Leaf data - btVector3 m_c0[4]; // gradients - btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3) - btScalar m_c2; // m_c1/sum(|g0..3|^2) - }; - /* RContact */ - struct RContact - { - sCti m_cti; // Contact infos - Node* m_node; // Owner node - btMatrix3x3 m_c0; // Impulse matrix - btVector3 m_c1; // Relative anchor - btScalar m_c2; // ima*dt - btScalar m_c3; // Friction - btScalar m_c4; // Hardness - }; - /* SContact */ - struct SContact - { - Node* m_node; // Node - Face* m_face; // Face - btVector3 m_weights; // Weigths - btVector3 m_normal; // Normal - btScalar m_margin; // Margin - btScalar m_friction; // Friction - btScalar m_cfm[2]; // Constraint force mixing - }; - /* Anchor */ - struct Anchor - { - Node* m_node; // Node pointer - btVector3 m_local; // Anchor position in body space - btRigidBody* m_body; // Body - btScalar m_influence; - btMatrix3x3 m_c0; // Impulse matrix - btVector3 m_c1; // Relative anchor - btScalar m_c2; // ima*dt - }; - /* Note */ - struct Note : Element - { - const char* m_text; // Text - btVector3 m_offset; // Offset - int m_rank; // Rank - Node* m_nodes[4]; // Nodes - btScalar m_coords[4]; // Coordinates - }; - /* Pose */ - struct Pose - { - bool m_bvolume; // Is valid - bool m_bframe; // Is frame - btScalar m_volume; // Rest volume - tVector3Array m_pos; // Reference positions - tScalarArray m_wgh; // Weights - btVector3 m_com; // COM - btMatrix3x3 m_rot; // Rotation - btMatrix3x3 m_scl; // Scale - btMatrix3x3 m_aqq; // Base scaling - }; - /* Cluster */ - struct Cluster - { - tScalarArray m_masses; - btAlignedObjectArray<Node*> m_nodes; - tVector3Array m_framerefs; - btTransform m_framexform; - btScalar m_idmass; - btScalar m_imass; - btMatrix3x3 m_locii; - btMatrix3x3 m_invwi; - btVector3 m_com; - btVector3 m_vimpulses[2]; - btVector3 m_dimpulses[2]; - int m_nvimpulses; - int m_ndimpulses; - btVector3 m_lv; - btVector3 m_av; - btDbvtNode* m_leaf; - btScalar m_ndamping; /* Node damping */ - btScalar m_ldamping; /* Linear damping */ - btScalar m_adamping; /* Angular damping */ - btScalar m_matching; - btScalar m_maxSelfCollisionImpulse; - btScalar m_selfCollisionImpulseFactor; - bool m_containsAnchor; - bool m_collide; - int m_clusterIndex; - Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) - ,m_maxSelfCollisionImpulse(100.f), - m_selfCollisionImpulseFactor(0.01f), - m_containsAnchor(false) - {} - }; - /* Impulse */ - struct Impulse - { - btVector3 m_velocity; - btVector3 m_drift; - int m_asVelocity:1; - int m_asDrift:1; - Impulse() : m_velocity(0,0,0),m_drift(0,0,0),m_asVelocity(0),m_asDrift(0) {} - Impulse operator -() const - { - Impulse i=*this; - i.m_velocity=-i.m_velocity; - i.m_drift=-i.m_drift; - return(i); - } - Impulse operator*(btScalar x) const - { - Impulse i=*this; - i.m_velocity*=x; - i.m_drift*=x; - return(i); - } - }; - /* Body */ - struct Body - { - Cluster* m_soft; - btRigidBody* m_rigid; - const btCollisionObject* m_collisionObject; - - Body() : m_soft(0),m_rigid(0),m_collisionObject(0) {} - Body(Cluster* p) : m_soft(p),m_rigid(0),m_collisionObject(0) {} - Body(const btCollisionObject* colObj) : m_soft(0),m_collisionObject(colObj) - { - m_rigid = (btRigidBody*)btRigidBody::upcast(m_collisionObject); - } - - void activate() const - { - if(m_rigid) - m_rigid->activate(); - if (m_collisionObject) - m_collisionObject->activate(); - - } - const btMatrix3x3& invWorldInertia() const - { - static const btMatrix3x3 iwi(0,0,0,0,0,0,0,0,0); - if(m_rigid) return(m_rigid->getInvInertiaTensorWorld()); - if(m_soft) return(m_soft->m_invwi); - return(iwi); - } - btScalar invMass() const - { - if(m_rigid) return(m_rigid->getInvMass()); - if(m_soft) return(m_soft->m_imass); - return(0); - } - const btTransform& xform() const - { - static const btTransform identity=btTransform::getIdentity(); - if(m_collisionObject) return(m_collisionObject->getWorldTransform()); - if(m_soft) return(m_soft->m_framexform); - return(identity); - } - btVector3 linearVelocity() const - { - if(m_rigid) return(m_rigid->getLinearVelocity()); - if(m_soft) return(m_soft->m_lv); - return(btVector3(0,0,0)); - } - btVector3 angularVelocity(const btVector3& rpos) const - { - if(m_rigid) return(btCross(m_rigid->getAngularVelocity(),rpos)); - if(m_soft) return(btCross(m_soft->m_av,rpos)); - return(btVector3(0,0,0)); - } - btVector3 angularVelocity() const - { - if(m_rigid) return(m_rigid->getAngularVelocity()); - if(m_soft) return(m_soft->m_av); - return(btVector3(0,0,0)); - } - btVector3 velocity(const btVector3& rpos) const - { - return(linearVelocity()+angularVelocity(rpos)); - } - void applyVImpulse(const btVector3& impulse,const btVector3& rpos) const - { - if(m_rigid) m_rigid->applyImpulse(impulse,rpos); - if(m_soft) btSoftBody::clusterVImpulse(m_soft,rpos,impulse); - } - void applyDImpulse(const btVector3& impulse,const btVector3& rpos) const - { - if(m_rigid) m_rigid->applyImpulse(impulse,rpos); - if(m_soft) btSoftBody::clusterDImpulse(m_soft,rpos,impulse); - } - void applyImpulse(const Impulse& impulse,const btVector3& rpos) const - { - if(impulse.m_asVelocity) - { -// printf("impulse.m_velocity = %f,%f,%f\n",impulse.m_velocity.getX(),impulse.m_velocity.getY(),impulse.m_velocity.getZ()); - applyVImpulse(impulse.m_velocity,rpos); - } - if(impulse.m_asDrift) - { -// printf("impulse.m_drift = %f,%f,%f\n",impulse.m_drift.getX(),impulse.m_drift.getY(),impulse.m_drift.getZ()); - applyDImpulse(impulse.m_drift,rpos); - } - } - void applyVAImpulse(const btVector3& impulse) const - { - if(m_rigid) m_rigid->applyTorqueImpulse(impulse); - if(m_soft) btSoftBody::clusterVAImpulse(m_soft,impulse); - } - void applyDAImpulse(const btVector3& impulse) const - { - if(m_rigid) m_rigid->applyTorqueImpulse(impulse); - if(m_soft) btSoftBody::clusterDAImpulse(m_soft,impulse); - } - void applyAImpulse(const Impulse& impulse) const - { - if(impulse.m_asVelocity) applyVAImpulse(impulse.m_velocity); - if(impulse.m_asDrift) applyDAImpulse(impulse.m_drift); - } - void applyDCImpulse(const btVector3& impulse) const - { - if(m_rigid) m_rigid->applyCentralImpulse(impulse); - if(m_soft) btSoftBody::clusterDCImpulse(m_soft,impulse); - } - }; - /* Joint */ - struct Joint - { - struct eType { enum _ { - Linear=0, - Angular, - Contact - };}; - struct Specs - { - Specs() : erp(1),cfm(1),split(1) {} - btScalar erp; - btScalar cfm; - btScalar split; - }; - Body m_bodies[2]; - btVector3 m_refs[2]; - btScalar m_cfm; - btScalar m_erp; - btScalar m_split; - btVector3 m_drift; - btVector3 m_sdrift; - btMatrix3x3 m_massmatrix; - bool m_delete; - virtual ~Joint() {} - Joint() : m_delete(false) {} - virtual void Prepare(btScalar dt,int iterations); - virtual void Solve(btScalar dt,btScalar sor)=0; - virtual void Terminate(btScalar dt)=0; - virtual eType::_ Type() const=0; - }; - /* LJoint */ - struct LJoint : Joint - { - struct Specs : Joint::Specs - { - btVector3 position; - }; - btVector3 m_rpos[2]; - void Prepare(btScalar dt,int iterations); - void Solve(btScalar dt,btScalar sor); - void Terminate(btScalar dt); - eType::_ Type() const { return(eType::Linear); } - }; - /* AJoint */ - struct AJoint : Joint - { - struct IControl - { - virtual ~IControl() {} - virtual void Prepare(AJoint*) {} - virtual btScalar Speed(AJoint*,btScalar current) { return(current); } - static IControl* Default() { static IControl def;return(&def); } - }; - struct Specs : Joint::Specs - { - Specs() : icontrol(IControl::Default()) {} - btVector3 axis; - IControl* icontrol; - }; - btVector3 m_axis[2]; - IControl* m_icontrol; - void Prepare(btScalar dt,int iterations); - void Solve(btScalar dt,btScalar sor); - void Terminate(btScalar dt); - eType::_ Type() const { return(eType::Angular); } - }; - /* CJoint */ - struct CJoint : Joint - { - int m_life; - int m_maxlife; - btVector3 m_rpos[2]; - btVector3 m_normal; - btScalar m_friction; - void Prepare(btScalar dt,int iterations); - void Solve(btScalar dt,btScalar sor); - void Terminate(btScalar dt); - eType::_ Type() const { return(eType::Contact); } - }; - /* Config */ - struct Config - { - eAeroModel::_ aeromodel; // Aerodynamic model (default: V_Point) - btScalar kVCF; // Velocities correction factor (Baumgarte) - btScalar kDP; // Damping coefficient [0,1] - btScalar kDG; // Drag coefficient [0,+inf] - btScalar kLF; // Lift coefficient [0,+inf] - btScalar kPR; // Pressure coefficient [-inf,+inf] - btScalar kVC; // Volume conversation coefficient [0,+inf] - btScalar kDF; // Dynamic friction coefficient [0,1] - btScalar kMT; // Pose matching coefficient [0,1] - btScalar kCHR; // Rigid contacts hardness [0,1] - btScalar kKHR; // Kinetic contacts hardness [0,1] - btScalar kSHR; // Soft contacts hardness [0,1] - btScalar kAHR; // Anchors hardness [0,1] - btScalar kSRHR_CL; // Soft vs rigid hardness [0,1] (cluster only) - btScalar kSKHR_CL; // Soft vs kinetic hardness [0,1] (cluster only) - btScalar kSSHR_CL; // Soft vs soft hardness [0,1] (cluster only) - btScalar kSR_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) - btScalar kSK_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) - btScalar kSS_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) - btScalar maxvolume; // Maximum volume ratio for pose - btScalar timescale; // Time scale - int viterations; // Velocities solver iterations - int piterations; // Positions solver iterations - int diterations; // Drift solver iterations - int citerations; // Cluster solver iterations - int collisions; // Collisions flags - tVSolverArray m_vsequence; // Velocity solvers sequence - tPSolverArray m_psequence; // Position solvers sequence - tPSolverArray m_dsequence; // Drift solvers sequence - }; - /* SolverState */ - struct SolverState - { - btScalar sdt; // dt*timescale - btScalar isdt; // 1/sdt - btScalar velmrg; // velocity margin - btScalar radmrg; // radial margin - btScalar updmrg; // Update margin - }; - /// RayFromToCaster takes a ray from, ray to (instead of direction!) - struct RayFromToCaster : btDbvt::ICollide - { - btVector3 m_rayFrom; - btVector3 m_rayTo; - btVector3 m_rayNormalizedDirection; - btScalar m_mint; - Face* m_face; - int m_tests; - RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt); - void Process(const btDbvtNode* leaf); - - static /*inline*/ btScalar rayFromToTriangle(const btVector3& rayFrom, - const btVector3& rayTo, - const btVector3& rayNormalizedDirection, - const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar maxt=SIMD_INFINITY); - }; - - // - // Typedefs - // - - typedef void (*psolver_t)(btSoftBody*,btScalar,btScalar); - typedef void (*vsolver_t)(btSoftBody*,btScalar); - typedef btAlignedObjectArray<Cluster*> tClusterArray; - typedef btAlignedObjectArray<Note> tNoteArray; - typedef btAlignedObjectArray<Node> tNodeArray; - typedef btAlignedObjectArray<btDbvtNode*> tLeafArray; - typedef btAlignedObjectArray<Link> tLinkArray; - typedef btAlignedObjectArray<Face> tFaceArray; - typedef btAlignedObjectArray<Tetra> tTetraArray; - typedef btAlignedObjectArray<Anchor> tAnchorArray; - typedef btAlignedObjectArray<RContact> tRContactArray; - typedef btAlignedObjectArray<SContact> tSContactArray; - typedef btAlignedObjectArray<Material*> tMaterialArray; - typedef btAlignedObjectArray<Joint*> tJointArray; - typedef btAlignedObjectArray<btSoftBody*> tSoftBodyArray; - - // - // Fields - // - - Config m_cfg; // Configuration - SolverState m_sst; // Solver state - Pose m_pose; // Pose - void* m_tag; // User data - btSoftBodyWorldInfo* m_worldInfo; // World info - tNoteArray m_notes; // Notes - tNodeArray m_nodes; // Nodes - tLinkArray m_links; // Links - tFaceArray m_faces; // Faces - tTetraArray m_tetras; // Tetras - tAnchorArray m_anchors; // Anchors - tRContactArray m_rcontacts; // Rigid contacts - tSContactArray m_scontacts; // Soft contacts - tJointArray m_joints; // Joints - tMaterialArray m_materials; // Materials - btScalar m_timeacc; // Time accumulator - btVector3 m_bounds[2]; // Spatial bounds - bool m_bUpdateRtCst; // Update runtime constants - btDbvt m_ndbvt; // Nodes tree - btDbvt m_fdbvt; // Faces tree - btDbvt m_cdbvt; // Clusters tree - tClusterArray m_clusters; // Clusters - - btAlignedObjectArray<bool>m_clusterConnectivity;//cluster connectivity, for self-collision - - btTransform m_initialWorldTransform; - - btVector3 m_windVelocity; - - btScalar m_restLengthScale; - - // - // Api - // - - /* ctor */ - btSoftBody( btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m); - - /* ctor */ - btSoftBody( btSoftBodyWorldInfo* worldInfo); - - void initDefaults(); - - /* dtor */ - virtual ~btSoftBody(); - /* Check for existing link */ - - btAlignedObjectArray<int> m_userIndexMapping; - - btSoftBodyWorldInfo* getWorldInfo() - { - return m_worldInfo; - } - - ///@todo: avoid internal softbody shape hack and move collision code to collision library - virtual void setCollisionShape(btCollisionShape* collisionShape) - { - - } - - bool checkLink( int node0, - int node1) const; - bool checkLink( const Node* node0, - const Node* node1) const; - /* Check for existring face */ - bool checkFace( int node0, - int node1, - int node2) const; - /* Append material */ - Material* appendMaterial(); - /* Append note */ - void appendNote( const char* text, - const btVector3& o, - const btVector4& c=btVector4(1,0,0,0), - Node* n0=0, - Node* n1=0, - Node* n2=0, - Node* n3=0); - void appendNote( const char* text, - const btVector3& o, - Node* feature); - void appendNote( const char* text, - const btVector3& o, - Link* feature); - void appendNote( const char* text, - const btVector3& o, - Face* feature); - /* Append node */ - void appendNode( const btVector3& x,btScalar m); - /* Append link */ - void appendLink(int model=-1,Material* mat=0); - void appendLink( int node0, - int node1, - Material* mat=0, - bool bcheckexist=false); - void appendLink( Node* node0, - Node* node1, - Material* mat=0, - bool bcheckexist=false); - /* Append face */ - void appendFace(int model=-1,Material* mat=0); - void appendFace( int node0, - int node1, - int node2, - Material* mat=0); - void appendTetra(int model,Material* mat); - // - void appendTetra(int node0, - int node1, - int node2, - int node3, - Material* mat=0); - - - /* Append anchor */ - void appendAnchor( int node, - btRigidBody* body, bool disableCollisionBetweenLinkedBodies=false,btScalar influence = 1); - void appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies=false,btScalar influence = 1); - /* Append linear joint */ - void appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1); - void appendLinearJoint(const LJoint::Specs& specs,Body body=Body()); - void appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body); - /* Append linear joint */ - void appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1); - void appendAngularJoint(const AJoint::Specs& specs,Body body=Body()); - void appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body); - /* Add force (or gravity) to the entire body */ - void addForce( const btVector3& force); - /* Add force (or gravity) to a node of the body */ - void addForce( const btVector3& force, - int node); - /* Add aero force to a node of the body */ - void addAeroForceToNode(const btVector3& windVelocity,int nodeIndex); - - /* Add aero force to a face of the body */ - void addAeroForceToFace(const btVector3& windVelocity,int faceIndex); - - /* Add velocity to the entire body */ - void addVelocity( const btVector3& velocity); - - /* Set velocity for the entire body */ - void setVelocity( const btVector3& velocity); - - /* Add velocity to a node of the body */ - void addVelocity( const btVector3& velocity, - int node); - /* Set mass */ - void setMass( int node, - btScalar mass); - /* Get mass */ - btScalar getMass( int node) const; - /* Get total mass */ - btScalar getTotalMass() const; - /* Set total mass (weighted by previous masses) */ - void setTotalMass( btScalar mass, - bool fromfaces=false); - /* Set total density */ - void setTotalDensity(btScalar density); - /* Set volume mass (using tetrahedrons) */ - void setVolumeMass( btScalar mass); - /* Set volume density (using tetrahedrons) */ - void setVolumeDensity( btScalar density); - /* Transform */ - void transform( const btTransform& trs); - /* Translate */ - void translate( const btVector3& trs); - /* Rotate */ - void rotate( const btQuaternion& rot); - /* Scale */ - void scale( const btVector3& scl); - /* Get link resting lengths scale */ - btScalar getRestLengthScale(); - /* Scale resting length of all springs */ - void setRestLengthScale(btScalar restLength); - /* Set current state as pose */ - void setPose( bool bvolume, - bool bframe); - /* Set current link lengths as resting lengths */ - void resetLinkRestLengths(); - /* Return the volume */ - btScalar getVolume() const; - /* Cluster count */ - int clusterCount() const; - /* Cluster center of mass */ - static btVector3 clusterCom(const Cluster* cluster); - btVector3 clusterCom(int cluster) const; - /* Cluster velocity at rpos */ - static btVector3 clusterVelocity(const Cluster* cluster,const btVector3& rpos); - /* Cluster impulse */ - static void clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse); - static void clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse); - static void clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse); - static void clusterVAImpulse(Cluster* cluster,const btVector3& impulse); - static void clusterDAImpulse(Cluster* cluster,const btVector3& impulse); - static void clusterAImpulse(Cluster* cluster,const Impulse& impulse); - static void clusterDCImpulse(Cluster* cluster,const btVector3& impulse); - /* Generate bending constraints based on distance in the adjency graph */ - int generateBendingConstraints( int distance, - Material* mat=0); - /* Randomize constraints to reduce solver bias */ - void randomizeConstraints(); - /* Release clusters */ - void releaseCluster(int index); - void releaseClusters(); - /* Generate clusters (K-mean) */ - ///generateClusters with k=0 will create a convex cluster for each tetrahedron or triangle - ///otherwise an approximation will be used (better performance) - int generateClusters(int k,int maxiterations=8192); - /* Refine */ - void refine(ImplicitFn* ifn,btScalar accurary,bool cut); - /* CutLink */ - bool cutLink(int node0,int node1,btScalar position); - bool cutLink(const Node* node0,const Node* node1,btScalar position); - - ///Ray casting using rayFrom and rayTo in worldspace, (not direction!) - bool rayTest(const btVector3& rayFrom, - const btVector3& rayTo, - sRayCast& results); - /* Solver presets */ - void setSolver(eSolverPresets::_ preset); - /* predictMotion */ - void predictMotion(btScalar dt); - /* solveConstraints */ - void solveConstraints(); - /* staticSolve */ - void staticSolve(int iterations); - /* solveCommonConstraints */ - static void solveCommonConstraints(btSoftBody** bodies,int count,int iterations); - /* solveClusters */ - static void solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies); - /* integrateMotion */ - void integrateMotion(); - /* defaultCollisionHandlers */ - void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap); - void defaultCollisionHandler(btSoftBody* psb); - - - - // - // Functionality to deal with new accelerated solvers. - // - - /** - * Set a wind velocity for interaction with the air. - */ - void setWindVelocity( const btVector3 &velocity ); - - - /** - * Return the wind velocity for interaction with the air. - */ - const btVector3& getWindVelocity(); - - // - // Set the solver that handles this soft body - // Should not be allowed to get out of sync with reality - // Currently called internally on addition to the world - void setSoftBodySolver( btSoftBodySolver *softBodySolver ) - { - m_softBodySolver = softBodySolver; - } - - // - // Return the solver that handles this soft body - // - btSoftBodySolver *getSoftBodySolver() - { - return m_softBodySolver; - } - - // - // Return the solver that handles this soft body - // - btSoftBodySolver *getSoftBodySolver() const - { - return m_softBodySolver; - } - - - // - // Cast - // - - static const btSoftBody* upcast(const btCollisionObject* colObj) - { - if (colObj->getInternalType()==CO_SOFT_BODY) - return (const btSoftBody*)colObj; - return 0; - } - static btSoftBody* upcast(btCollisionObject* colObj) - { - if (colObj->getInternalType()==CO_SOFT_BODY) - return (btSoftBody*)colObj; - return 0; - } - - // - // ::btCollisionObject - // - - virtual void getAabb(btVector3& aabbMin,btVector3& aabbMax) const - { - aabbMin = m_bounds[0]; - aabbMax = m_bounds[1]; - } - // - // Private - // - void pointersToIndices(); - void indicesToPointers(const int* map=0); - - int rayTest(const btVector3& rayFrom,const btVector3& rayTo, - btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const; - void initializeFaceTree(); - btVector3 evaluateCom() const; - bool checkContact(const btCollisionObjectWrapper* colObjWrap,const btVector3& x,btScalar margin,btSoftBody::sCti& cti) const; - void updateNormals(); - void updateBounds(); - void updatePose(); - void updateConstants(); - void updateLinkConstants(); - void updateArea(bool averageArea = true); - void initializeClusters(); - void updateClusters(); - void cleanupClusters(); - void prepareClusters(int iterations); - void solveClusters(btScalar sor); - void applyClusters(bool drift); - void dampClusters(); - void applyForces(); - static void PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti); - static void PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti); - static void PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti); - static void PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti); - static void VSolve_Links(btSoftBody* psb,btScalar kst); - static psolver_t getSolver(ePSolver::_ solver); - static vsolver_t getSolver(eVSolver::_ solver); - - - virtual int calculateSerializeBufferSize() const; - - ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; - - //virtual void serializeSingleObject(class btSerializer* serializer) const; - - -}; - - - - -#endif //_BT_SOFT_BODY_H diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp b/thirdparty/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp deleted file mode 100644 index ab84bddf2a..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp +++ /dev/null @@ -1,358 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btSoftBodyConcaveCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionShapes/btConcaveShape.h" -#include "BulletCollision/CollisionDispatch/btManifoldResult.h" -#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" -#include "BulletCollision/CollisionShapes/btTriangleShape.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionShapes/btTetrahedronShape.h" -#include "BulletCollision/CollisionShapes/btConvexHullShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - - -#include "LinearMath/btIDebugDraw.h" -#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" -#include "BulletSoftBody/btSoftBody.h" - -#define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06)//make this configurable - -btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) -: btCollisionAlgorithm(ci), -m_isSwapped(isSwapped), -m_btSoftBodyTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped) -{ -} - - - -btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm() -{ -} - - - -btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped): -m_dispatcher(dispatcher), -m_dispatchInfoPtr(0) -{ - m_softBody = (isSwapped? (btSoftBody*)body1Wrap->getCollisionObject():(btSoftBody*)body0Wrap->getCollisionObject()); - m_triBody = isSwapped? body0Wrap->getCollisionObject():body1Wrap->getCollisionObject(); - - // - // create the manifold from the dispatcher 'manifold pool' - // - // m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody); - - clearCache(); -} - -btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback() -{ - clearCache(); - // m_dispatcher->releaseManifold( m_manifoldPtr ); - -} - - -void btSoftBodyTriangleCallback::clearCache() -{ - for (int i=0;i<m_shapeCache.size();i++) - { - btTriIndex* tmp = m_shapeCache.getAtIndex(i); - btAssert(tmp); - btAssert(tmp->m_childShape); - m_softBody->getWorldInfo()->m_sparsesdf.RemoveReferences(tmp->m_childShape);//necessary? - delete tmp->m_childShape; - } - m_shapeCache.clear(); -} - - -void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) -{ - //just for debugging purposes - //printf("triangle %d",m_triangleCount++); - - btCollisionAlgorithmConstructionInfo ci; - ci.m_dispatcher1 = m_dispatcher; - - ///debug drawing of the overlapping triangles - if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe)) - { - btVector3 color(1,1,0); - const btTransform& tr = m_triBody->getWorldTransform(); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color); - } - - btTriIndex triIndex(partId,triangleIndex,0); - btHashKey<btTriIndex> triKey(triIndex.getUid()); - - - btTriIndex* shapeIndex = m_shapeCache[triKey]; - if (shapeIndex) - { - btCollisionShape* tm = shapeIndex->m_childShape; - btAssert(tm); - - //copy over user pointers to temporary shape - tm->setUserPointer(m_triBody->getCollisionShape()->getUserPointer()); - - btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1); - //btCollisionObjectWrapper triBody(0,tm, ob, btTransform::getIdentity());//ob->getWorldTransform());//?? - btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex); - ebtDispatcherQueryType algoType = m_resultOut->m_closestPointDistanceThreshold > 0 ? BT_CLOSEST_POINT_ALGORITHMS : BT_CONTACT_POINT_ALGORITHMS; - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0, algoType);//m_manifoldPtr); - - colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut); - colAlgo->~btCollisionAlgorithm(); - ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); - - return; - } - - //aabb filter is already applied! - - //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject); - - // if (m_softBody->getCollisionShape()->getShapeType()== - { - // btVector3 other; - btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]); - normal.normalize(); - normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION; - // other=(triangle[0]+triangle[1]+triangle[2])*0.333333f; - // other+=normal*22.f; - btVector3 pts[6] = {triangle[0]+normal, - triangle[1]+normal, - triangle[2]+normal, - triangle[0]-normal, - triangle[1]-normal, - triangle[2]-normal}; - - btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6); - - - // btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other); - - //btTriangleShape tm(triangle[0],triangle[1],triangle[2]); - // tm.setMargin(m_collisionMarginTriangle); - - //copy over user pointers to temporary shape - tm->setUserPointer(m_triBody->getCollisionShape()->getUserPointer()); - - - btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1); - btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex);//btTransform::getIdentity());//?? - - ebtDispatcherQueryType algoType = m_resultOut->m_closestPointDistanceThreshold > 0 ? BT_CLOSEST_POINT_ALGORITHMS : BT_CONTACT_POINT_ALGORITHMS; - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0, algoType);//m_manifoldPtr); - - colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut); - colAlgo->~btCollisionAlgorithm(); - ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); - - triIndex.m_childShape = tm; - m_shapeCache.insert(triKey,triIndex); - - } - - - -} - - - -void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btCollisionObjectWrapper* triBodyWrap, const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - m_dispatchInfoPtr = &dispatchInfo; - m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION); - m_resultOut = resultOut; - - - btVector3 aabbWorldSpaceMin,aabbWorldSpaceMax; - m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax); - btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5); - btVector3 softBodyCenter = (aabbWorldSpaceMax+aabbWorldSpaceMin)*btScalar(0.5); - - btTransform softTransform; - softTransform.setIdentity(); - softTransform.setOrigin(softBodyCenter); - - btTransform convexInTriangleSpace; - convexInTriangleSpace = triBodyWrap->getWorldTransform().inverse() * softTransform; - btTransformAabb(halfExtents,m_collisionMarginTriangle,convexInTriangleSpace,m_aabbMin,m_aabbMax); -} - -void btSoftBodyConcaveCollisionAlgorithm::clearCache() -{ - m_btSoftBodyTriangleCallback.clearCache(); - -} - -void btSoftBodyConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - - - //btCollisionObject* convexBody = m_isSwapped ? body1 : body0; - const btCollisionObjectWrapper* triBody = m_isSwapped ? body0Wrap : body1Wrap; - - if (triBody->getCollisionShape()->isConcave()) - { - - - const btCollisionObject* triOb = triBody->getCollisionObject(); - const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triOb->getCollisionShape()); - - // if (convexBody->getCollisionShape()->isConvex()) - { - btScalar collisionMarginTriangle = concaveShape->getMargin(); - - // resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); - m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,triBody,dispatchInfo,resultOut); - - - concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax()); - - // resultOut->refreshContactPoints(); - - } - - } - -} - - -btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)resultOut; - (void)dispatchInfo; - btCollisionObject* convexbody = m_isSwapped ? body1 : body0; - btCollisionObject* triBody = m_isSwapped ? body0 : body1; - - - //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) - - //only perform CCD above a certain threshold, this prevents blocking on the long run - //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... - btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2(); - if (squareMot0 < convexbody->getCcdSquareMotionThreshold()) - { - return btScalar(1.); - } - - //const btVector3& from = convexbody->m_worldTransform.getOrigin(); - //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin(); - //todo: only do if the motion exceeds the 'radius' - - btTransform triInv = triBody->getWorldTransform().inverse(); - btTransform convexFromLocal = triInv * convexbody->getWorldTransform(); - btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform(); - - struct LocalTriangleSphereCastCallback : public btTriangleCallback - { - btTransform m_ccdSphereFromTrans; - btTransform m_ccdSphereToTrans; - btTransform m_meshTransform; - - btScalar m_ccdSphereRadius; - btScalar m_hitFraction; - - - LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction) - :m_ccdSphereFromTrans(from), - m_ccdSphereToTrans(to), - m_ccdSphereRadius(ccdSphereRadius), - m_hitFraction(hitFraction) - { - } - - - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) - { - (void)partId; - (void)triangleIndex; - //do a swept sphere for now - btTransform ident; - ident.setIdentity(); - btConvexCast::CastResult castResult; - castResult.m_fraction = m_hitFraction; - btSphereShape pointShape(m_ccdSphereRadius); - btTriangleShape triShape(triangle[0],triangle[1],triangle[2]); - btVoronoiSimplexSolver simplexSolver; - btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver); - //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); - //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); - //local space? - - if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans, - ident,ident,castResult)) - { - if (m_hitFraction > castResult.m_fraction) - m_hitFraction = castResult.m_fraction; - } - - } - - }; - - - - - - if (triBody->getCollisionShape()->isConcave()) - { - btVector3 rayAabbMin = convexFromLocal.getOrigin(); - rayAabbMin.setMin(convexToLocal.getOrigin()); - btVector3 rayAabbMax = convexFromLocal.getOrigin(); - rayAabbMax.setMax(convexToLocal.getOrigin()); - btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius(); - rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0); - rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0); - - btScalar curHitFraction = btScalar(1.); //is this available? - LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal, - convexbody->getCcdSweptSphereRadius(),curHitFraction); - - raycastCallback.m_hitFraction = convexbody->getHitFraction(); - - btCollisionObject* concavebody = triBody; - - btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape(); - - if (triangleMesh) - { - triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); - } - - - - if (raycastCallback.m_hitFraction < convexbody->getHitFraction()) - { - convexbody->setHitFraction( raycastCallback.m_hitFraction); - return raycastCallback.m_hitFraction; - } - } - - return btScalar(1.); - -} diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h b/thirdparty/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h deleted file mode 100644 index 11c7b88f98..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h +++ /dev/null @@ -1,155 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H -#define BT_SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" -#include "BulletCollision/CollisionShapes/btTriangleCallback.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -class btDispatcher; -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -class btSoftBody; -class btCollisionShape; - -#include "LinearMath/btHashMap.h" - -#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h" //for definition of MAX_NUM_PARTS_IN_BITS - -struct btTriIndex -{ - int m_PartIdTriangleIndex; - class btCollisionShape* m_childShape; - - btTriIndex(int partId,int triangleIndex,btCollisionShape* shape) - { - m_PartIdTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; - m_childShape = shape; - } - - int getTriangleIndex() const - { - // Get only the lower bits where the triangle index is stored - unsigned int x = 0; - unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS); - return (m_PartIdTriangleIndex&~(y)); - } - int getPartId() const - { - // Get only the highest bits where the part index is stored - return (m_PartIdTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS)); - } - int getUid() const - { - return m_PartIdTriangleIndex; - } -}; - - -///For each triangle in the concave mesh that overlaps with the AABB of a soft body (m_softBody), processTriangle is called. -class btSoftBodyTriangleCallback : public btTriangleCallback -{ - btSoftBody* m_softBody; - const btCollisionObject* m_triBody; - - btVector3 m_aabbMin; - btVector3 m_aabbMax ; - - btManifoldResult* m_resultOut; - - btDispatcher* m_dispatcher; - const btDispatcherInfo* m_dispatchInfoPtr; - btScalar m_collisionMarginTriangle; - - btHashMap<btHashKey<btTriIndex>,btTriIndex> m_shapeCache; - -public: - int m_triangleCount; - - // btPersistentManifold* m_manifoldPtr; - - btSoftBodyTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); - - void setTimeStepAndCounters(btScalar collisionMarginTriangle,const btCollisionObjectWrapper* triObjWrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual ~btSoftBodyTriangleCallback(); - - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - - void clearCache(); - - SIMD_FORCE_INLINE const btVector3& getAabbMin() const - { - return m_aabbMin; - } - SIMD_FORCE_INLINE const btVector3& getAabbMax() const - { - return m_aabbMax; - } - -}; - - - - -/// btSoftBodyConcaveCollisionAlgorithm supports collision between soft body shapes and (concave) trianges meshes. -class btSoftBodyConcaveCollisionAlgorithm : public btCollisionAlgorithm -{ - - bool m_isSwapped; - - btSoftBodyTriangleCallback m_btSoftBodyTriangleCallback; - -public: - - btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); - - virtual ~btSoftBodyConcaveCollisionAlgorithm(); - - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - //we don't add any manifolds - } - - void clearCache(); - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) - { - void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftBodyConcaveCollisionAlgorithm)); - return new(mem) btSoftBodyConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,false); - } - }; - - struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc - { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) - { - void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftBodyConcaveCollisionAlgorithm)); - return new(mem) btSoftBodyConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,true); - } - }; - -}; - -#endif //BT_SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftBodyData.h b/thirdparty/bullet/src/BulletSoftBody/btSoftBodyData.h deleted file mode 100644 index 87d8841cfa..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftBodyData.h +++ /dev/null @@ -1,217 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFTBODY_FLOAT_DATA -#define BT_SOFTBODY_FLOAT_DATA - -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" - - -struct SoftBodyMaterialData -{ - float m_linearStiffness; - float m_angularStiffness; - float m_volumeStiffness; - int m_flags; -}; - -struct SoftBodyNodeData -{ - SoftBodyMaterialData *m_material; - btVector3FloatData m_position; - btVector3FloatData m_previousPosition; - btVector3FloatData m_velocity; - btVector3FloatData m_accumulatedForce; - btVector3FloatData m_normal; - float m_inverseMass; - float m_area; - int m_attach; - int m_pad; -}; - -struct SoftBodyLinkData -{ - SoftBodyMaterialData *m_material; - int m_nodeIndices[2]; // Node pointers - float m_restLength; // Rest length - int m_bbending; // Bending link -}; - -struct SoftBodyFaceData -{ - btVector3FloatData m_normal; // Normal - SoftBodyMaterialData *m_material; - int m_nodeIndices[3]; // Node pointers - float m_restArea; // Rest area -}; - -struct SoftBodyTetraData -{ - btVector3FloatData m_c0[4]; // gradients - SoftBodyMaterialData *m_material; - int m_nodeIndices[4]; // Node pointers - float m_restVolume; // Rest volume - float m_c1; // (4*kVST)/(im0+im1+im2+im3) - float m_c2; // m_c1/sum(|g0..3|^2) - int m_pad; -}; - -struct SoftRigidAnchorData -{ - btMatrix3x3FloatData m_c0; // Impulse matrix - btVector3FloatData m_c1; // Relative anchor - btVector3FloatData m_localFrame; // Anchor position in body space - btRigidBodyData *m_rigidBody; - int m_nodeIndex; // Node pointer - float m_c2; // ima*dt -}; - - - -struct SoftBodyConfigData -{ - int m_aeroModel; // Aerodynamic model (default: V_Point) - float m_baumgarte; // Velocities correction factor (Baumgarte) - float m_damping; // Damping coefficient [0,1] - float m_drag; // Drag coefficient [0,+inf] - float m_lift; // Lift coefficient [0,+inf] - float m_pressure; // Pressure coefficient [-inf,+inf] - float m_volume; // Volume conversation coefficient [0,+inf] - float m_dynamicFriction; // Dynamic friction coefficient [0,1] - float m_poseMatch; // Pose matching coefficient [0,1] - float m_rigidContactHardness; // Rigid contacts hardness [0,1] - float m_kineticContactHardness; // Kinetic contacts hardness [0,1] - float m_softContactHardness; // Soft contacts hardness [0,1] - float m_anchorHardness; // Anchors hardness [0,1] - float m_softRigidClusterHardness; // Soft vs rigid hardness [0,1] (cluster only) - float m_softKineticClusterHardness; // Soft vs kinetic hardness [0,1] (cluster only) - float m_softSoftClusterHardness; // Soft vs soft hardness [0,1] (cluster only) - float m_softRigidClusterImpulseSplit; // Soft vs rigid impulse split [0,1] (cluster only) - float m_softKineticClusterImpulseSplit; // Soft vs rigid impulse split [0,1] (cluster only) - float m_softSoftClusterImpulseSplit; // Soft vs rigid impulse split [0,1] (cluster only) - float m_maxVolume; // Maximum volume ratio for pose - float m_timeScale; // Time scale - int m_velocityIterations; // Velocities solver iterations - int m_positionIterations; // Positions solver iterations - int m_driftIterations; // Drift solver iterations - int m_clusterIterations; // Cluster solver iterations - int m_collisionFlags; // Collisions flags -}; - -struct SoftBodyPoseData -{ - btMatrix3x3FloatData m_rot; // Rotation - btMatrix3x3FloatData m_scale; // Scale - btMatrix3x3FloatData m_aqq; // Base scaling - btVector3FloatData m_com; // COM - - btVector3FloatData *m_positions; // Reference positions - float *m_weights; // Weights - int m_numPositions; - int m_numWeigts; - - int m_bvolume; // Is valid - int m_bframe; // Is frame - float m_restVolume; // Rest volume - int m_pad; -}; - -struct SoftBodyClusterData -{ - btTransformFloatData m_framexform; - btMatrix3x3FloatData m_locii; - btMatrix3x3FloatData m_invwi; - btVector3FloatData m_com; - btVector3FloatData m_vimpulses[2]; - btVector3FloatData m_dimpulses[2]; - btVector3FloatData m_lv; - btVector3FloatData m_av; - - btVector3FloatData *m_framerefs; - int *m_nodeIndices; - float *m_masses; - - int m_numFrameRefs; - int m_numNodes; - int m_numMasses; - - float m_idmass; - float m_imass; - int m_nvimpulses; - int m_ndimpulses; - float m_ndamping; - float m_ldamping; - float m_adamping; - float m_matching; - float m_maxSelfCollisionImpulse; - float m_selfCollisionImpulseFactor; - int m_containsAnchor; - int m_collide; - int m_clusterIndex; -}; - - -enum btSoftJointBodyType -{ - BT_JOINT_SOFT_BODY_CLUSTER=1, - BT_JOINT_RIGID_BODY, - BT_JOINT_COLLISION_OBJECT -}; - -struct btSoftBodyJointData -{ - void *m_bodyA; - void *m_bodyB; - btVector3FloatData m_refs[2]; - float m_cfm; - float m_erp; - float m_split; - int m_delete; - btVector3FloatData m_relPosition[2];//linear - int m_bodyAtype; - int m_bodyBtype; - int m_jointType; - int m_pad; -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btSoftBodyFloatData -{ - btCollisionObjectFloatData m_collisionObjectData; - - SoftBodyPoseData *m_pose; - SoftBodyMaterialData **m_materials; - SoftBodyNodeData *m_nodes; - SoftBodyLinkData *m_links; - SoftBodyFaceData *m_faces; - SoftBodyTetraData *m_tetrahedra; - SoftRigidAnchorData *m_anchors; - SoftBodyClusterData *m_clusters; - btSoftBodyJointData *m_joints; - - int m_numMaterials; - int m_numNodes; - int m_numLinks; - int m_numFaces; - int m_numTetrahedra; - int m_numAnchors; - int m_numClusters; - int m_numJoints; - SoftBodyConfigData m_config; -}; - -#endif //BT_SOFTBODY_FLOAT_DATA - diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftBodyHelpers.cpp b/thirdparty/bullet/src/BulletSoftBody/btSoftBodyHelpers.cpp deleted file mode 100644 index 51fcd16da4..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ /dev/null @@ -1,1219 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btSoftBodyHelpers.cpp by Nathanael Presson - -#include "btSoftBodyInternals.h" -#include <stdio.h> -#include <string.h> -#include "btSoftBodyHelpers.h" -#include "LinearMath/btConvexHull.h" -#include "LinearMath/btConvexHullComputer.h" - - -// -static void drawVertex( btIDebugDraw* idraw, - const btVector3& x,btScalar s,const btVector3& c) -{ - idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c); - idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c); - idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c); -} - -// -static void drawBox( btIDebugDraw* idraw, - const btVector3& mins, - const btVector3& maxs, - const btVector3& color) -{ - const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()), - btVector3(maxs.x(),mins.y(),mins.z()), - btVector3(maxs.x(),maxs.y(),mins.z()), - btVector3(mins.x(),maxs.y(),mins.z()), - btVector3(mins.x(),mins.y(),maxs.z()), - btVector3(maxs.x(),mins.y(),maxs.z()), - btVector3(maxs.x(),maxs.y(),maxs.z()), - btVector3(mins.x(),maxs.y(),maxs.z())}; - idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color); - idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color); - idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color); - idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color); - idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color); - idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color); -} - -// -static void drawTree( btIDebugDraw* idraw, - const btDbvtNode* node, - int depth, - const btVector3& ncolor, - const btVector3& lcolor, - int mindepth, - int maxdepth) -{ - if(node) - { - if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0))) - { - drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth); - drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth); - } - if(depth>=mindepth) - { - const btScalar scl=(btScalar)(node->isinternal()?1:1); - const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl; - const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl; - drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor); - } - } -} - -// -template <typename T> -static inline T sum(const btAlignedObjectArray<T>& items) -{ - T v; - if(items.size()) - { - v=items[0]; - for(int i=1,ni=items.size();i<ni;++i) - { - v+=items[i]; - } - } - return(v); -} - -// -template <typename T,typename Q> -static inline void add(btAlignedObjectArray<T>& items,const Q& value) -{ - for(int i=0,ni=items.size();i<ni;++i) - { - items[i]+=value; - } -} - -// -template <typename T,typename Q> -static inline void mul(btAlignedObjectArray<T>& items,const Q& value) -{ - for(int i=0,ni=items.size();i<ni;++i) - { - items[i]*=value; - } -} - -// -template <typename T> -static inline T average(const btAlignedObjectArray<T>& items) -{ - const btScalar n=(btScalar)(items.size()>0?items.size():1); - return(sum(items)/n); -} - -#if 0 -// - inline static btScalar tetravolume(const btVector3& x0, - const btVector3& x1, - const btVector3& x2, - const btVector3& x3) -{ - const btVector3 a=x1-x0; - const btVector3 b=x2-x0; - const btVector3 c=x3-x0; - return(btDot(a,btCross(b,c))); -} -#endif - -// -#if 0 -static btVector3 stresscolor(btScalar stress) -{ - static const btVector3 spectrum[]= { btVector3(1,0,1), - btVector3(0,0,1), - btVector3(0,1,1), - btVector3(0,1,0), - btVector3(1,1,0), - btVector3(1,0,0), - btVector3(1,0,0)}; - static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1; - static const btScalar one=1; - stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors; - const int sel=(int)stress; - const btScalar frc=stress-sel; - return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc); -} -#endif - -// -void btSoftBodyHelpers::Draw( btSoftBody* psb, - btIDebugDraw* idraw, - int drawflags) -{ - const btScalar scl=(btScalar)0.1; - const btScalar nscl=scl*5; - const btVector3 lcolor=btVector3(0,0,0); - const btVector3 ncolor=btVector3(1,1,1); - const btVector3 ccolor=btVector3(1,0,0); - int i,j,nj; - - /* Clusters */ - if(0!=(drawflags&fDrawFlags::Clusters)) - { - srand(1806); - for(i=0;i<psb->m_clusters.size();++i) - { - if(psb->m_clusters[i]->m_collide) - { - btVector3 color( rand()/(btScalar)RAND_MAX, - rand()/(btScalar)RAND_MAX, - rand()/(btScalar)RAND_MAX); - color=color.normalized()*0.75; - btAlignedObjectArray<btVector3> vertices; - vertices.resize(psb->m_clusters[i]->m_nodes.size()); - for(j=0,nj=vertices.size();j<nj;++j) - { - vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x; - } -#define USE_NEW_CONVEX_HULL_COMPUTER -#ifdef USE_NEW_CONVEX_HULL_COMPUTER - btConvexHullComputer computer; - int stride = sizeof(btVector3); - int count = vertices.size(); - btScalar shrink=0.f; - btScalar shrinkClamp=0.f; - computer.compute(&vertices[0].getX(),stride,count,shrink,shrinkClamp); - for (int i=0;i<computer.faces.size();i++) - { - - int face = computer.faces[i]; - //printf("face=%d\n",face); - const btConvexHullComputer::Edge* firstEdge = &computer.edges[face]; - const btConvexHullComputer::Edge* edge = firstEdge->getNextEdgeOfFace(); - - int v0 = firstEdge->getSourceVertex(); - int v1 = firstEdge->getTargetVertex(); - while (edge!=firstEdge) - { - int v2 = edge->getTargetVertex(); - idraw->drawTriangle(computer.vertices[v0],computer.vertices[v1],computer.vertices[v2],color,1); - edge = edge->getNextEdgeOfFace(); - v0=v1; - v1=v2; - }; - } -#else - - HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]); - HullResult hres; - HullLibrary hlib; - hdsc.mMaxVertices=vertices.size(); - hlib.CreateConvexHull(hdsc,hres); - const btVector3 center=average(hres.m_OutputVertices); - add(hres.m_OutputVertices,-center); - mul(hres.m_OutputVertices,(btScalar)1); - add(hres.m_OutputVertices,center); - for(j=0;j<(int)hres.mNumFaces;++j) - { - const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]}; - idraw->drawTriangle(hres.m_OutputVertices[idx[0]], - hres.m_OutputVertices[idx[1]], - hres.m_OutputVertices[idx[2]], - color,1); - } - hlib.ReleaseResult(hres); -#endif - - } - /* Velocities */ -#if 0 - for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j) - { - const btSoftBody::Cluster& c=psb->m_clusters[i]; - const btVector3 r=c.m_nodes[j]->m_x-c.m_com; - const btVector3 v=c.m_lv+btCross(c.m_av,r); - idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0)); - } -#endif - /* Frame */ - // btSoftBody::Cluster& c=*psb->m_clusters[i]; - // idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0)); - // idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0)); - // idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1)); - } - } - else - { - /* Nodes */ - if(0!=(drawflags&fDrawFlags::Nodes)) - { - for(i=0;i<psb->m_nodes.size();++i) - { - const btSoftBody::Node& n=psb->m_nodes[i]; - if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0)); - idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0)); - idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1)); - } - } - /* Links */ - if(0!=(drawflags&fDrawFlags::Links)) - { - for(i=0;i<psb->m_links.size();++i) - { - const btSoftBody::Link& l=psb->m_links[i]; - if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor); - } - } - /* Normals */ - if(0!=(drawflags&fDrawFlags::Normals)) - { - for(i=0;i<psb->m_nodes.size();++i) - { - const btSoftBody::Node& n=psb->m_nodes[i]; - if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - const btVector3 d=n.m_n*nscl; - idraw->drawLine(n.m_x,n.m_x+d,ncolor); - idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5); - } - } - /* Contacts */ - if(0!=(drawflags&fDrawFlags::Contacts)) - { - static const btVector3 axis[]={btVector3(1,0,0), - btVector3(0,1,0), - btVector3(0,0,1)}; - for(i=0;i<psb->m_rcontacts.size();++i) - { - const btSoftBody::RContact& c=psb->m_rcontacts[i]; - const btVector3 o= c.m_node->m_x-c.m_cti.m_normal* - (btDot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset); - const btVector3 x=btCross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized(); - const btVector3 y=btCross(x,c.m_cti.m_normal).normalized(); - idraw->drawLine(o-x*nscl,o+x*nscl,ccolor); - idraw->drawLine(o-y*nscl,o+y*nscl,ccolor); - idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0)); - } - } - /* Faces */ - if(0!=(drawflags&fDrawFlags::Faces)) - { - const btScalar scl=(btScalar)0.8; - const btScalar alp=(btScalar)1; - const btVector3 col(0,(btScalar)0.7,0); - for(i=0;i<psb->m_faces.size();++i) - { - const btSoftBody::Face& f=psb->m_faces[i]; - if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x}; - const btVector3 c=(x[0]+x[1]+x[2])/3; - idraw->drawTriangle((x[0]-c)*scl+c, - (x[1]-c)*scl+c, - (x[2]-c)*scl+c, - col,alp); - } - } - /* Tetras */ - if(0!=(drawflags&fDrawFlags::Tetras)) - { - const btScalar scl=(btScalar)0.8; - const btScalar alp=(btScalar)1; - const btVector3 col((btScalar)0.3,(btScalar)0.3,(btScalar)0.7); - for(int i=0;i<psb->m_tetras.size();++i) - { - const btSoftBody::Tetra& t=psb->m_tetras[i]; - if(0==(t.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - const btVector3 x[]={t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x}; - const btVector3 c=(x[0]+x[1]+x[2]+x[3])/4; - idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[2]-c)*scl+c,col,alp); - idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[3]-c)*scl+c,col,alp); - idraw->drawTriangle((x[1]-c)*scl+c,(x[2]-c)*scl+c,(x[3]-c)*scl+c,col,alp); - idraw->drawTriangle((x[2]-c)*scl+c,(x[0]-c)*scl+c,(x[3]-c)*scl+c,col,alp); - } - } - } - /* Anchors */ - if(0!=(drawflags&fDrawFlags::Anchors)) - { - for(i=0;i<psb->m_anchors.size();++i) - { - const btSoftBody::Anchor& a=psb->m_anchors[i]; - const btVector3 q=a.m_body->getWorldTransform()*a.m_local; - drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0)); - drawVertex(idraw,q,0.25,btVector3(0,1,0)); - idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1)); - } - for(i=0;i<psb->m_nodes.size();++i) - { - const btSoftBody::Node& n=psb->m_nodes[i]; - if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - if(n.m_im<=0) - { - drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0)); - } - } - } - - - /* Notes */ - if(0!=(drawflags&fDrawFlags::Notes)) - { - for(i=0;i<psb->m_notes.size();++i) - { - const btSoftBody::Note& n=psb->m_notes[i]; - btVector3 p=n.m_offset; - for(int j=0;j<n.m_rank;++j) - { - p+=n.m_nodes[j]->m_x*n.m_coords[j]; - } - idraw->draw3dText(p,n.m_text); - } - } - /* Node tree */ - if(0!=(drawflags&fDrawFlags::NodeTree)) DrawNodeTree(psb,idraw); - /* Face tree */ - if(0!=(drawflags&fDrawFlags::FaceTree)) DrawFaceTree(psb,idraw); - /* Cluster tree */ - if(0!=(drawflags&fDrawFlags::ClusterTree)) DrawClusterTree(psb,idraw); - /* Joints */ - if(0!=(drawflags&fDrawFlags::Joints)) - { - for(i=0;i<psb->m_joints.size();++i) - { - const btSoftBody::Joint* pj=psb->m_joints[i]; - switch(pj->Type()) - { - case btSoftBody::Joint::eType::Linear: - { - const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj; - const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0]; - const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1]; - idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0)); - idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1)); - drawVertex(idraw,a0,0.25,btVector3(1,1,0)); - drawVertex(idraw,a1,0.25,btVector3(0,1,1)); - } - break; - case btSoftBody::Joint::eType::Angular: - { - //const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj; - const btVector3 o0=pj->m_bodies[0].xform().getOrigin(); - const btVector3 o1=pj->m_bodies[1].xform().getOrigin(); - const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0]; - const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1]; - idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0)); - idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0)); - idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1)); - idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1)); - break; - } - default: - { - } - - } - } - } -} - -// -void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, - btIDebugDraw* idraw, - bool masses, - bool areas, - bool /*stress*/) -{ - for(int i=0;i<psb->m_nodes.size();++i) - { - const btSoftBody::Node& n=psb->m_nodes[i]; - char text[2048]={0}; - char buff[1024]; - if(masses) - { - sprintf(buff," M(%.2f)",1/n.m_im); - strcat(text,buff); - } - if(areas) - { - sprintf(buff," A(%.2f)",n.m_area); - strcat(text,buff); - } - if(text[0]) idraw->draw3dText(n.m_x,text); - } -} - -// -void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) -{ - drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth); -} - -// -void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) -{ - drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth); -} - -// -void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) -{ - drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth); -} - - -//The btSoftBody object from the BulletSDK includes an array of Nodes and Links. These links appear -// to be first set up to connect a node to between 5 and 6 of its neighbors [480 links], -//and then to the rest of the nodes after the execution of the Floyd-Warshall graph algorithm -//[another 930 links]. -//The way the links are stored by default, we have a number of cases where adjacent links share a node in common -// - this leads to the creation of a data dependency through memory. -//The PSolve_Links() function reads and writes nodes as it iterates over each link. -//So, we now have the possibility of a data dependency between iteration X -//that processes link L with iteration X+1 that processes link L+1 -//because L and L+1 have one node in common, and iteration X updates the positions of that node, -//and iteration X+1 reads in the position of that shared node. -// -//Such a memory dependency limits the ability of a modern CPU to speculate beyond -//a certain point because it has to respect a possible dependency -//- this prevents the CPU from making full use of its out-of-order resources. -//If we re-order the links such that we minimize the cases where a link L and L+1 share a common node, -//we create a temporal gap between when the node position is written, -//and when it is subsequently read. This in turn allows the CPU to continue execution without -//risking a dependency violation. Such a reordering would result in significant speedups on -//modern CPUs with lots of execution resources. -//In our testing, we see it have a tremendous impact not only on the A7, -//but also on all x86 cores that ship with modern Macs. -//The attached source file includes a single function (ReoptimizeLinkOrder) which can be called on a -//btSoftBody object in the solveConstraints() function before the actual solver is invoked, -//or right after generateBendingConstraints() once we have all 1410 links. - - -//=================================================================== -// -// -// This function takes in a list of interdependent Links and tries -// to maximize the distance between calculation -// of dependent links. This increases the amount of parallelism that can -// be exploited by out-of-order instruction processors with large but -// (inevitably) finite instruction windows. -// -//=================================================================== - -// A small structure to track lists of dependent link calculations -class LinkDeps_t { - public: - int value; // A link calculation that is dependent on this one - // Positive values = "input A" while negative values = "input B" - LinkDeps_t *next; // Next dependence in the list -}; -typedef LinkDeps_t *LinkDepsPtr_t; - -// Dependency list constants -#define REOP_NOT_DEPENDENT -1 -#define REOP_NODE_COMPLETE -2 // Must be less than REOP_NOT_DEPENDENT - - -void btSoftBodyHelpers::ReoptimizeLinkOrder(btSoftBody *psb /* This can be replaced by a btSoftBody pointer */) -{ - int i, nLinks=psb->m_links.size(), nNodes=psb->m_nodes.size(); - btSoftBody::Link *lr; - int ar, br; - btSoftBody::Node *node0 = &(psb->m_nodes[0]); - btSoftBody::Node *node1 = &(psb->m_nodes[1]); - LinkDepsPtr_t linkDep; - int readyListHead, readyListTail, linkNum, linkDepFrees, depLink; - - // Allocate temporary buffers - int *nodeWrittenAt = new int[nNodes+1]; // What link calculation produced this node's current values? - int *linkDepA = new int[nLinks]; // Link calculation input is dependent upon prior calculation #N - int *linkDepB = new int[nLinks]; - int *readyList = new int[nLinks]; // List of ready-to-process link calculations (# of links, maximum) - LinkDeps_t *linkDepFreeList = new LinkDeps_t[2*nLinks]; // Dependent-on-me list elements (2x# of links, maximum) - LinkDepsPtr_t *linkDepListStarts = new LinkDepsPtr_t[nLinks]; // Start nodes of dependent-on-me lists, one for each link - - // Copy the original, unsorted links to a side buffer - btSoftBody::Link *linkBuffer = new btSoftBody::Link[nLinks]; - memcpy(linkBuffer, &(psb->m_links[0]), sizeof(btSoftBody::Link)*nLinks); - - // Clear out the node setup and ready list - for (i=0; i < nNodes+1; i++) { - nodeWrittenAt[i] = REOP_NOT_DEPENDENT; - } - for (i=0; i < nLinks; i++) { - linkDepListStarts[i] = NULL; - } - readyListHead = readyListTail = linkDepFrees = 0; - - // Initial link analysis to set up data structures - for (i=0; i < nLinks; i++) { - - // Note which prior link calculations we are dependent upon & build up dependence lists - lr = &(psb->m_links[i]); - ar = (lr->m_n[0] - node0)/(node1 - node0); - br = (lr->m_n[1] - node0)/(node1 - node0); - if (nodeWrittenAt[ar] > REOP_NOT_DEPENDENT) { - linkDepA[i] = nodeWrittenAt[ar]; - linkDep = &linkDepFreeList[linkDepFrees++]; - linkDep->value = i; - linkDep->next = linkDepListStarts[nodeWrittenAt[ar]]; - linkDepListStarts[nodeWrittenAt[ar]] = linkDep; - } else { - linkDepA[i] = REOP_NOT_DEPENDENT; - } - if (nodeWrittenAt[br] > REOP_NOT_DEPENDENT) { - linkDepB[i] = nodeWrittenAt[br]; - linkDep = &linkDepFreeList[linkDepFrees++]; - linkDep->value = -(i+1); - linkDep->next = linkDepListStarts[nodeWrittenAt[br]]; - linkDepListStarts[nodeWrittenAt[br]] = linkDep; - } else { - linkDepB[i] = REOP_NOT_DEPENDENT; - } - - // Add this link to the initial ready list, if it is not dependent on any other links - if ((linkDepA[i] == REOP_NOT_DEPENDENT) && (linkDepB[i] == REOP_NOT_DEPENDENT)) { - readyList[readyListTail++] = i; - linkDepA[i] = linkDepB[i] = REOP_NODE_COMPLETE; // Probably not needed now - } - - // Update the nodes to mark which ones are calculated by this link - nodeWrittenAt[ar] = nodeWrittenAt[br] = i; - } - - // Process the ready list and create the sorted list of links - // -- By treating the ready list as a queue, we maximize the distance between any - // inter-dependent node calculations - // -- All other (non-related) nodes in the ready list will automatically be inserted - // in between each set of inter-dependent link calculations by this loop - i = 0; - while (readyListHead != readyListTail) { - // Use ready list to select the next link to process - linkNum = readyList[readyListHead++]; - // Copy the next-to-calculate link back into the original link array - psb->m_links[i++] = linkBuffer[linkNum]; - - // Free up any link inputs that are dependent on this one - linkDep = linkDepListStarts[linkNum]; - while (linkDep) { - depLink = linkDep->value; - if (depLink >= 0) { - linkDepA[depLink] = REOP_NOT_DEPENDENT; - } else { - depLink = -depLink - 1; - linkDepB[depLink] = REOP_NOT_DEPENDENT; - } - // Add this dependent link calculation to the ready list if *both* inputs are clear - if ((linkDepA[depLink] == REOP_NOT_DEPENDENT) && (linkDepB[depLink] == REOP_NOT_DEPENDENT)) { - readyList[readyListTail++] = depLink; - linkDepA[depLink] = linkDepB[depLink] = REOP_NODE_COMPLETE; // Probably not needed now - } - linkDep = linkDep->next; - } - } - - // Delete the temporary buffers - delete [] nodeWrittenAt; - delete [] linkDepA; - delete [] linkDepB; - delete [] readyList; - delete [] linkDepFreeList; - delete [] linkDepListStarts; - delete [] linkBuffer; -} - - -// -void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, - btIDebugDraw* idraw) -{ - if(psb->m_pose.m_bframe) - { - static const btScalar ascl=10; - static const btScalar nscl=(btScalar)0.1; - const btVector3 com=psb->m_pose.m_com; - const btMatrix3x3 trs=psb->m_pose.m_rot*psb->m_pose.m_scl; - const btVector3 Xaxis=(trs*btVector3(1,0,0)).normalized(); - const btVector3 Yaxis=(trs*btVector3(0,1,0)).normalized(); - const btVector3 Zaxis=(trs*btVector3(0,0,1)).normalized(); - idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0)); - idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0)); - idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1)); - for(int i=0;i<psb->m_pose.m_pos.size();++i) - { - const btVector3 x=com+trs*psb->m_pose.m_pos[i]; - drawVertex(idraw,x,nscl,btVector3(1,0,1)); - } - } -} - -// -btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, const btVector3& from, - const btVector3& to, - int res, - int fixeds) -{ - /* Create nodes */ - const int r=res+2; - btVector3* x=new btVector3[r]; - btScalar* m=new btScalar[r]; - int i; - - for(i=0;i<r;++i) - { - const btScalar t=i/(btScalar)(r-1); - x[i]=lerp(from,to,t); - m[i]=1; - } - btSoftBody* psb= new btSoftBody(&worldInfo,r,x,m); - if(fixeds&1) psb->setMass(0,0); - if(fixeds&2) psb->setMass(r-1,0); - delete[] x; - delete[] m; - /* Create links */ - for(i=1;i<r;++i) - { - psb->appendLink(i-1,i); - } - /* Finished */ - return(psb); -} - -// -btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags) -{ -#define IDX(_x_,_y_) ((_y_)*rx+(_x_)) - /* Create nodes */ - if((resx<2)||(resy<2)) return(0); - const int rx=resx; - const int ry=resy; - const int tot=rx*ry; - btVector3* x=new btVector3[tot]; - btScalar* m=new btScalar[tot]; - int iy; - - for(iy=0;iy<ry;++iy) - { - const btScalar ty=iy/(btScalar)(ry-1); - const btVector3 py0=lerp(corner00,corner01,ty); - const btVector3 py1=lerp(corner10,corner11,ty); - for(int ix=0;ix<rx;++ix) - { - const btScalar tx=ix/(btScalar)(rx-1); - x[IDX(ix,iy)]=lerp(py0,py1,tx); - m[IDX(ix,iy)]=1; - } - } - btSoftBody* psb=new btSoftBody(&worldInfo,tot,x,m); - if(fixeds&1) psb->setMass(IDX(0,0),0); - if(fixeds&2) psb->setMass(IDX(rx-1,0),0); - if(fixeds&4) psb->setMass(IDX(0,ry-1),0); - if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0); - delete[] x; - delete[] m; - /* Create links and faces */ - for(iy=0;iy<ry;++iy) - { - for(int ix=0;ix<rx;++ix) - { - const int idx=IDX(ix,iy); - const bool mdx=(ix+1)<rx; - const bool mdy=(iy+1)<ry; - if(mdx) psb->appendLink(idx,IDX(ix+1,iy)); - if(mdy) psb->appendLink(idx,IDX(ix,iy+1)); - if(mdx&&mdy) - { - if((ix+iy)&1) - { - psb->appendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1)); - psb->appendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1)); - if(gendiags) - { - psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1)); - } - } - else - { - psb->appendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy)); - psb->appendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1)); - if(gendiags) - { - psb->appendLink(IDX(ix+1,iy),IDX(ix,iy+1)); - } - } - } - } - } - /* Finished */ -#undef IDX - return(psb); -} - -// -btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo, - const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags, - float* tex_coords) -{ - - /* - * - * corners: - * - * [0][0] corner00 ------- corner01 [resx][0] - * | | - * | | - * [0][resy] corner10 -------- corner11 [resx][resy] - * - * - * - * - * - * - * "fixedgs" map: - * - * corner00 --> +1 - * corner01 --> +2 - * corner10 --> +4 - * corner11 --> +8 - * upper middle --> +16 - * left middle --> +32 - * right middle --> +64 - * lower middle --> +128 - * center --> +256 - * - * - * tex_coords size (resx-1)*(resy-1)*12 - * - * - * - * SINGLE QUAD INTERNALS - * - * 1) btSoftBody's nodes and links, - * diagonal link is optional ("gendiags") - * - * - * node00 ------ node01 - * | . - * | . - * | . - * | . - * | . - * node10 node11 - * - * - * - * 2) Faces: - * two triangles, - * UV Coordinates (hier example for single quad) - * - * (0,1) (0,1) (1,1) - * 1 |\ 3 \-----| 2 - * | \ \ | - * | \ \ | - * | \ \ | - * | \ \ | - * 2 |-----\ 3 \| 1 - * (0,0) (1,0) (1,0) - * - * - * - * - * - * - */ - -#define IDX(_x_,_y_) ((_y_)*rx+(_x_)) - /* Create nodes */ - if((resx<2)||(resy<2)) return(0); - const int rx=resx; - const int ry=resy; - const int tot=rx*ry; - btVector3* x=new btVector3[tot]; - btScalar* m=new btScalar[tot]; - - int iy; - - for(iy=0;iy<ry;++iy) - { - const btScalar ty=iy/(btScalar)(ry-1); - const btVector3 py0=lerp(corner00,corner01,ty); - const btVector3 py1=lerp(corner10,corner11,ty); - for(int ix=0;ix<rx;++ix) - { - const btScalar tx=ix/(btScalar)(rx-1); - x[IDX(ix,iy)]=lerp(py0,py1,tx); - m[IDX(ix,iy)]=1; - } - } - btSoftBody* psb=new btSoftBody(&worldInfo,tot,x,m); - if(fixeds&1) psb->setMass(IDX(0,0),0); - if(fixeds&2) psb->setMass(IDX(rx-1,0),0); - if(fixeds&4) psb->setMass(IDX(0,ry-1),0); - if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0); - if(fixeds&16) psb->setMass(IDX((rx-1)/2,0),0); - if(fixeds&32) psb->setMass(IDX(0,(ry-1)/2),0); - if(fixeds&64) psb->setMass(IDX(rx-1,(ry-1)/2),0); - if(fixeds&128) psb->setMass(IDX((rx-1)/2,ry-1),0); - if(fixeds&256) psb->setMass(IDX((rx-1)/2,(ry-1)/2),0); - delete[] x; - delete[] m; - - - int z = 0; - /* Create links and faces */ - for(iy=0;iy<ry;++iy) - { - for(int ix=0;ix<rx;++ix) - { - const bool mdx=(ix+1)<rx; - const bool mdy=(iy+1)<ry; - - int node00=IDX(ix,iy); - int node01=IDX(ix+1,iy); - int node10=IDX(ix,iy+1); - int node11=IDX(ix+1,iy+1); - - if(mdx) psb->appendLink(node00,node01); - if(mdy) psb->appendLink(node00,node10); - if(mdx&&mdy) - { - psb->appendFace(node00,node10,node11); - if (tex_coords) { - tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0); - tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1); - tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0); - tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2); - tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3); - tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2); - } - psb->appendFace(node11,node01,node00); - if (tex_coords) { - tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3); - tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2); - tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3); - tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1); - tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0); - tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1); - } - if (gendiags) psb->appendLink(node00,node11); - z += 12; - } - } - } - /* Finished */ -#undef IDX - return(psb); -} - -float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id) -{ - - /* - * - * - * node00 --- node01 - * | | - * node10 --- node11 - * - * - * ID map: - * - * node00 s --> 0 - * node00 t --> 1 - * - * node01 s --> 3 - * node01 t --> 1 - * - * node10 s --> 0 - * node10 t --> 2 - * - * node11 s --> 3 - * node11 t --> 2 - * - * - */ - - float tc=0.0f; - if (id == 0) { - tc = (1.0f/((resx-1))*ix); - } - else if (id==1) { - tc = (1.0f/((resy-1))*(resy-1-iy)); - } - else if (id==2) { - tc = (1.0f/((resy-1))*(resy-1-iy-1)); - } - else if (id==3) { - tc = (1.0f/((resx-1))*(ix+1)); - } - return tc; -} -// -btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center, - const btVector3& radius, - int res) -{ - struct Hammersley - { - static void Generate(btVector3* x,int n) - { - for(int i=0;i<n;i++) - { - btScalar p=0.5,t=0; - for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p; - btScalar w=2*t-1; - btScalar a=(SIMD_PI+2*i*SIMD_PI)/n; - btScalar s=btSqrt(1-w*w); - *x++=btVector3(s*btCos(a),s*btSin(a),w); - } - } - }; - btAlignedObjectArray<btVector3> vtx; - vtx.resize(3+res); - Hammersley::Generate(&vtx[0],vtx.size()); - for(int i=0;i<vtx.size();++i) - { - vtx[i]=vtx[i]*radius+center; - } - return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size())); -} - - - -// -btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar* vertices, - const int* triangles, - int ntriangles, bool randomizeConstraints) -{ - int maxidx=0; - int i,j,ni; - - for(i=0,ni=ntriangles*3;i<ni;++i) - { - maxidx=btMax(triangles[i],maxidx); - } - ++maxidx; - btAlignedObjectArray<bool> chks; - btAlignedObjectArray<btVector3> vtx; - chks.resize(maxidx*maxidx,false); - vtx.resize(maxidx); - for(i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3) - { - vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]); - } - btSoftBody* psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0); - for( i=0,ni=ntriangles*3;i<ni;i+=3) - { - const int idx[]={triangles[i],triangles[i+1],triangles[i+2]}; -#define IDX(_x_,_y_) ((_y_)*maxidx+(_x_)) - for(int j=2,k=0;k<3;j=k++) - { - if(!chks[IDX(idx[j],idx[k])]) - { - chks[IDX(idx[j],idx[k])]=true; - chks[IDX(idx[k],idx[j])]=true; - psb->appendLink(idx[j],idx[k]); - } - } -#undef IDX - psb->appendFace(idx[0],idx[1],idx[2]); - } - - if (randomizeConstraints) - { - psb->randomizeConstraints(); - } - - return(psb); -} - -// -btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, - int nvertices, bool randomizeConstraints) -{ - HullDesc hdsc(QF_TRIANGLES,nvertices,vertices); - HullResult hres; - HullLibrary hlib;/*??*/ - hdsc.mMaxVertices=nvertices; - hlib.CreateConvexHull(hdsc,hres); - btSoftBody* psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices, - &hres.m_OutputVertices[0],0); - for(int i=0;i<(int)hres.mNumFaces;++i) - { - const int idx[]={ static_cast<int>(hres.m_Indices[i*3+0]), - static_cast<int>(hres.m_Indices[i*3+1]), - static_cast<int>(hres.m_Indices[i*3+2])}; - if(idx[0]<idx[1]) psb->appendLink( idx[0],idx[1]); - if(idx[1]<idx[2]) psb->appendLink( idx[1],idx[2]); - if(idx[2]<idx[0]) psb->appendLink( idx[2],idx[0]); - psb->appendFace(idx[0],idx[1],idx[2]); - } - hlib.ReleaseResult(hres); - if (randomizeConstraints) - { - psb->randomizeConstraints(); - } - return(psb); -} - - - - -static int nextLine(const char* buffer) -{ - int numBytesRead=0; - - while (*buffer != '\n') - { - buffer++; - numBytesRead++; - } - - - if (buffer[0]==0x0a) - { - buffer++; - numBytesRead++; - } - return numBytesRead; -} - -/* Create from TetGen .ele, .face, .node data */ -btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo, - const char* ele, - const char* face, - const char* node, - bool bfacelinks, - bool btetralinks, - bool bfacesfromtetras) -{ -btAlignedObjectArray<btVector3> pos; -int nnode=0; -int ndims=0; -int nattrb=0; -int hasbounds=0; -int result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds); -result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds); -node += nextLine(node); - -pos.resize(nnode); -for(int i=0;i<pos.size();++i) - { - int index=0; - //int bound=0; - float x,y,z; - sscanf(node,"%d %f %f %f",&index,&x,&y,&z); - -// sn>>index; -// sn>>x;sn>>y;sn>>z; - node += nextLine(node); - - //for(int j=0;j<nattrb;++j) - // sn>>a; - - //if(hasbounds) - // sn>>bound; - - pos[index].setX(btScalar(x)); - pos[index].setY(btScalar(y)); - pos[index].setZ(btScalar(z)); - } -btSoftBody* psb=new btSoftBody(&worldInfo,nnode,&pos[0],0); -#if 0 -if(face&&face[0]) - { - int nface=0; - sf>>nface;sf>>hasbounds; - for(int i=0;i<nface;++i) - { - int index=0; - int bound=0; - int ni[3]; - sf>>index; - sf>>ni[0];sf>>ni[1];sf>>ni[2]; - sf>>bound; - psb->appendFace(ni[0],ni[1],ni[2]); - if(btetralinks) - { - psb->appendLink(ni[0],ni[1],0,true); - psb->appendLink(ni[1],ni[2],0,true); - psb->appendLink(ni[2],ni[0],0,true); - } - } - } -#endif - -if(ele&&ele[0]) - { - int ntetra=0; - int ncorner=0; - int neattrb=0; - sscanf(ele,"%d %d %d",&ntetra,&ncorner,&neattrb); - ele += nextLine(ele); - - //se>>ntetra;se>>ncorner;se>>neattrb; - for(int i=0;i<ntetra;++i) - { - int index=0; - int ni[4]; - - //se>>index; - //se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3]; - sscanf(ele,"%d %d %d %d %d",&index,&ni[0],&ni[1],&ni[2],&ni[3]); - ele+=nextLine(ele); - //for(int j=0;j<neattrb;++j) - // se>>a; - psb->appendTetra(ni[0],ni[1],ni[2],ni[3]); - if(btetralinks) - { - psb->appendLink(ni[0],ni[1],0,true); - psb->appendLink(ni[1],ni[2],0,true); - psb->appendLink(ni[2],ni[0],0,true); - psb->appendLink(ni[0],ni[3],0,true); - psb->appendLink(ni[1],ni[3],0,true); - psb->appendLink(ni[2],ni[3],0,true); - } - } - } -printf("Nodes: %u\r\n",psb->m_nodes.size()); -printf("Links: %u\r\n",psb->m_links.size()); -printf("Faces: %u\r\n",psb->m_faces.size()); -printf("Tetras: %u\r\n",psb->m_tetras.size()); -return(psb); -} - diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftBodyHelpers.h b/thirdparty/bullet/src/BulletSoftBody/btSoftBodyHelpers.h deleted file mode 100644 index 7271530109..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftBodyHelpers.h +++ /dev/null @@ -1,148 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFT_BODY_HELPERS_H -#define BT_SOFT_BODY_HELPERS_H - -#include "btSoftBody.h" - -// -// Helpers -// - -/* fDrawFlags */ -struct fDrawFlags { enum _ { - Nodes = 0x0001, - Links = 0x0002, - Faces = 0x0004, - Tetras = 0x0008, - Normals = 0x0010, - Contacts = 0x0020, - Anchors = 0x0040, - Notes = 0x0080, - Clusters = 0x0100, - NodeTree = 0x0200, - FaceTree = 0x0400, - ClusterTree = 0x0800, - Joints = 0x1000, - /* presets */ - Std = Links+Faces+Tetras+Anchors+Notes+Joints, - StdTetra = Std-Faces+Tetras -};}; - -struct btSoftBodyHelpers -{ - /* Draw body */ - static void Draw( btSoftBody* psb, - btIDebugDraw* idraw, - int drawflags=fDrawFlags::Std); - /* Draw body infos */ - static void DrawInfos( btSoftBody* psb, - btIDebugDraw* idraw, - bool masses, - bool areas, - bool stress); - /* Draw node tree */ - static void DrawNodeTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth=0, - int maxdepth=-1); - /* Draw face tree */ - static void DrawFaceTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth=0, - int maxdepth=-1); - /* Draw cluster tree */ - static void DrawClusterTree(btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth=0, - int maxdepth=-1); - /* Draw rigid frame */ - static void DrawFrame( btSoftBody* psb, - btIDebugDraw* idraw); - /* Create a rope */ - static btSoftBody* CreateRope( btSoftBodyWorldInfo& worldInfo, - const btVector3& from, - const btVector3& to, - int res, - int fixeds); - /* Create a patch */ - static btSoftBody* CreatePatch(btSoftBodyWorldInfo& worldInfo, - const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags); - /* Create a patch with UV Texture Coordinates */ - static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo, - const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags, - float* tex_coords=0); - static float CalculateUV(int resx,int resy,int ix,int iy,int id); - /* Create an ellipsoid */ - static btSoftBody* CreateEllipsoid(btSoftBodyWorldInfo& worldInfo, - const btVector3& center, - const btVector3& radius, - int res); - /* Create from trimesh */ - static btSoftBody* CreateFromTriMesh( btSoftBodyWorldInfo& worldInfo, - const btScalar* vertices, - const int* triangles, - int ntriangles, - bool randomizeConstraints = true); - /* Create from convex-hull */ - static btSoftBody* CreateFromConvexHull( btSoftBodyWorldInfo& worldInfo, - const btVector3* vertices, - int nvertices, - bool randomizeConstraints = true); - - - /* Export TetGen compatible .smesh file */ -// static void ExportAsSMeshFile( btSoftBody* psb, -// const char* filename); - /* Create from TetGen .ele, .face, .node files */ -// static btSoftBody* CreateFromTetGenFile( btSoftBodyWorldInfo& worldInfo, -// const char* ele, -// const char* face, -// const char* node, -// bool bfacelinks, -// bool btetralinks, -// bool bfacesfromtetras); - /* Create from TetGen .ele, .face, .node data */ - static btSoftBody* CreateFromTetGenData( btSoftBodyWorldInfo& worldInfo, - const char* ele, - const char* face, - const char* node, - bool bfacelinks, - bool btetralinks, - bool bfacesfromtetras); - - /// Sort the list of links to move link calculations that are dependent upon earlier - /// ones as far as possible away from the calculation of those values - /// This tends to make adjacent loop iterations not dependent upon one another, - /// so out-of-order processors can execute instructions from multiple iterations at once - static void ReoptimizeLinkOrder(btSoftBody *psb ); -}; - -#endif //BT_SOFT_BODY_HELPERS_H diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftBodyInternals.h b/thirdparty/bullet/src/BulletSoftBody/btSoftBodyInternals.h deleted file mode 100644 index 1ad82616ea..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftBodyInternals.h +++ /dev/null @@ -1,911 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btSoftBody implementation by Nathanael Presson - -#ifndef _BT_SOFT_BODY_INTERNALS_H -#define _BT_SOFT_BODY_INTERNALS_H - -#include "btSoftBody.h" - - -#include "LinearMath/btQuickprof.h" -#include "LinearMath/btPolarDecomposition.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" -#include <string.h> //for memset -// -// btSymMatrix -// -template <typename T> -struct btSymMatrix -{ - btSymMatrix() : dim(0) {} - btSymMatrix(int n,const T& init=T()) { resize(n,init); } - void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); } - int index(int c,int r) const { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); } - T& operator()(int c,int r) { return(store[index(c,r)]); } - const T& operator()(int c,int r) const { return(store[index(c,r)]); } - btAlignedObjectArray<T> store; - int dim; -}; - -// -// btSoftBodyCollisionShape -// -class btSoftBodyCollisionShape : public btConcaveShape -{ -public: - btSoftBody* m_body; - - btSoftBodyCollisionShape(btSoftBody* backptr) - { - m_shapeType = SOFTBODY_SHAPE_PROXYTYPE; - m_body=backptr; - } - - virtual ~btSoftBodyCollisionShape() - { - - } - - void processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const - { - //not yet - btAssert(0); - } - - ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t. - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - { - /* t is usually identity, except when colliding against btCompoundShape. See Issue 512 */ - const btVector3 mins=m_body->m_bounds[0]; - const btVector3 maxs=m_body->m_bounds[1]; - const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()), - t*btVector3(maxs.x(),mins.y(),mins.z()), - t*btVector3(maxs.x(),maxs.y(),mins.z()), - t*btVector3(mins.x(),maxs.y(),mins.z()), - t*btVector3(mins.x(),mins.y(),maxs.z()), - t*btVector3(maxs.x(),mins.y(),maxs.z()), - t*btVector3(maxs.x(),maxs.y(),maxs.z()), - t*btVector3(mins.x(),maxs.y(),maxs.z())}; - aabbMin=aabbMax=crns[0]; - for(int i=1;i<8;++i) - { - aabbMin.setMin(crns[i]); - aabbMax.setMax(crns[i]); - } - } - - - virtual void setLocalScaling(const btVector3& /*scaling*/) - { - ///na - } - virtual const btVector3& getLocalScaling() const - { - static const btVector3 dummy(1,1,1); - return dummy; - } - virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const - { - ///not yet - btAssert(0); - } - virtual const char* getName()const - { - return "SoftBody"; - } - -}; - -// -// btSoftClusterCollisionShape -// -class btSoftClusterCollisionShape : public btConvexInternalShape -{ -public: - const btSoftBody::Cluster* m_cluster; - - btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); } - - - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const - { - btSoftBody::Node* const * n=&m_cluster->m_nodes[0]; - btScalar d=btDot(vec,n[0]->m_x); - int j=0; - for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i) - { - const btScalar k=btDot(vec,n[i]->m_x); - if(k>d) { d=k;j=i; } - } - return(n[j]->m_x); - } - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const - { - return(localGetSupportingVertex(vec)); - } - //notice that the vectors should be unit length - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const - {} - - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const - {} - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - {} - - virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; } - - //debugging - virtual const char* getName()const {return "SOFTCLUSTER";} - - virtual void setMargin(btScalar margin) - { - btConvexInternalShape::setMargin(margin); - } - virtual btScalar getMargin() const - { - return btConvexInternalShape::getMargin(); - } -}; - -// -// Inline's -// - -// -template <typename T> -static inline void ZeroInitialize(T& value) -{ - memset(&value,0,sizeof(T)); -} -// -template <typename T> -static inline bool CompLess(const T& a,const T& b) -{ return(a<b); } -// -template <typename T> -static inline bool CompGreater(const T& a,const T& b) -{ return(a>b); } -// -template <typename T> -static inline T Lerp(const T& a,const T& b,btScalar t) -{ return(a+(b-a)*t); } -// -template <typename T> -static inline T InvLerp(const T& a,const T& b,btScalar t) -{ return((b+a*t-b*t)/(a*b)); } -// -static inline btMatrix3x3 Lerp( const btMatrix3x3& a, - const btMatrix3x3& b, - btScalar t) -{ - btMatrix3x3 r; - r[0]=Lerp(a[0],b[0],t); - r[1]=Lerp(a[1],b[1],t); - r[2]=Lerp(a[2],b[2],t); - return(r); -} -// -static inline btVector3 Clamp(const btVector3& v,btScalar maxlength) -{ - const btScalar sql=v.length2(); - if(sql>(maxlength*maxlength)) - return((v*maxlength)/btSqrt(sql)); - else - return(v); -} -// -template <typename T> -static inline T Clamp(const T& x,const T& l,const T& h) -{ return(x<l?l:x>h?h:x); } -// -template <typename T> -static inline T Sq(const T& x) -{ return(x*x); } -// -template <typename T> -static inline T Cube(const T& x) -{ return(x*x*x); } -// -template <typename T> -static inline T Sign(const T& x) -{ return((T)(x<0?-1:+1)); } -// -template <typename T> -static inline bool SameSign(const T& x,const T& y) -{ return((x*y)>0); } -// -static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y) -{ - const btVector3 d=x-y; - return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2])); -} -// -static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s) -{ - const btScalar xx=a.x()*a.x(); - const btScalar yy=a.y()*a.y(); - const btScalar zz=a.z()*a.z(); - const btScalar xy=a.x()*a.y(); - const btScalar yz=a.y()*a.z(); - const btScalar zx=a.z()*a.x(); - btMatrix3x3 m; - m[0]=btVector3(1-xx+xx*s,xy*s-xy,zx*s-zx); - m[1]=btVector3(xy*s-xy,1-yy+yy*s,yz*s-yz); - m[2]=btVector3(zx*s-zx,yz*s-yz,1-zz+zz*s); - return(m); -} -// -static inline btMatrix3x3 Cross(const btVector3& v) -{ - btMatrix3x3 m; - m[0]=btVector3(0,-v.z(),+v.y()); - m[1]=btVector3(+v.z(),0,-v.x()); - m[2]=btVector3(-v.y(),+v.x(),0); - return(m); -} -// -static inline btMatrix3x3 Diagonal(btScalar x) -{ - btMatrix3x3 m; - m[0]=btVector3(x,0,0); - m[1]=btVector3(0,x,0); - m[2]=btVector3(0,0,x); - return(m); -} -// -static inline btMatrix3x3 Add(const btMatrix3x3& a, - const btMatrix3x3& b) -{ - btMatrix3x3 r; - for(int i=0;i<3;++i) r[i]=a[i]+b[i]; - return(r); -} -// -static inline btMatrix3x3 Sub(const btMatrix3x3& a, - const btMatrix3x3& b) -{ - btMatrix3x3 r; - for(int i=0;i<3;++i) r[i]=a[i]-b[i]; - return(r); -} -// -static inline btMatrix3x3 Mul(const btMatrix3x3& a, - btScalar b) -{ - btMatrix3x3 r; - for(int i=0;i<3;++i) r[i]=a[i]*b; - return(r); -} -// -static inline void Orthogonalize(btMatrix3x3& m) -{ - m[2]=btCross(m[0],m[1]).normalized(); - m[1]=btCross(m[2],m[0]).normalized(); - m[0]=btCross(m[1],m[2]).normalized(); -} -// -static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r) -{ - const btMatrix3x3 cr=Cross(r); - return(Sub(Diagonal(im),cr*iwi*cr)); -} - -// -static inline btMatrix3x3 ImpulseMatrix( btScalar dt, - btScalar ima, - btScalar imb, - const btMatrix3x3& iwi, - const btVector3& r) -{ - return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse()); -} - -// -static inline btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra, - btScalar imb,const btMatrix3x3& iib,const btVector3& rb) -{ - return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse()); -} - -// -static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia, - const btMatrix3x3& iib) -{ - return(Add(iia,iib).inverse()); -} - -// -static inline btVector3 ProjectOnAxis( const btVector3& v, - const btVector3& a) -{ - return(a*btDot(v,a)); -} -// -static inline btVector3 ProjectOnPlane( const btVector3& v, - const btVector3& a) -{ - return(v-ProjectOnAxis(v,a)); -} - -// -static inline void ProjectOrigin( const btVector3& a, - const btVector3& b, - btVector3& prj, - btScalar& sqd) -{ - const btVector3 d=b-a; - const btScalar m2=d.length2(); - if(m2>SIMD_EPSILON) - { - const btScalar t=Clamp<btScalar>(-btDot(a,d)/m2,0,1); - const btVector3 p=a+d*t; - const btScalar l2=p.length2(); - if(l2<sqd) - { - prj=p; - sqd=l2; - } - } -} -// -static inline void ProjectOrigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - btVector3& prj, - btScalar& sqd) -{ - const btVector3& q=btCross(b-a,c-a); - const btScalar m2=q.length2(); - if(m2>SIMD_EPSILON) - { - const btVector3 n=q/btSqrt(m2); - const btScalar k=btDot(a,n); - const btScalar k2=k*k; - if(k2<sqd) - { - const btVector3 p=n*k; - if( (btDot(btCross(a-p,b-p),q)>0)&& - (btDot(btCross(b-p,c-p),q)>0)&& - (btDot(btCross(c-p,a-p),q)>0)) - { - prj=p; - sqd=k2; - } - else - { - ProjectOrigin(a,b,prj,sqd); - ProjectOrigin(b,c,prj,sqd); - ProjectOrigin(c,a,prj,sqd); - } - } - } -} - -// -template <typename T> -static inline T BaryEval( const T& a, - const T& b, - const T& c, - const btVector3& coord) -{ - return(a*coord.x()+b*coord.y()+c*coord.z()); -} -// -static inline btVector3 BaryCoord( const btVector3& a, - const btVector3& b, - const btVector3& c, - const btVector3& p) -{ - const btScalar w[]={ btCross(a-p,b-p).length(), - btCross(b-p,c-p).length(), - btCross(c-p,a-p).length()}; - const btScalar isum=1/(w[0]+w[1]+w[2]); - return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum)); -} - -// -inline static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn, - const btVector3& a, - const btVector3& b, - const btScalar accuracy, - const int maxiterations=256) -{ - btScalar span[2]={0,1}; - btScalar values[2]={fn->Eval(a),fn->Eval(b)}; - if(values[0]>values[1]) - { - btSwap(span[0],span[1]); - btSwap(values[0],values[1]); - } - if(values[0]>-accuracy) return(-1); - if(values[1]<+accuracy) return(-1); - for(int i=0;i<maxiterations;++i) - { - const btScalar t=Lerp(span[0],span[1],values[0]/(values[0]-values[1])); - const btScalar v=fn->Eval(Lerp(a,b,t)); - if((t<=0)||(t>=1)) break; - if(btFabs(v)<accuracy) return(t); - if(v<0) - { span[0]=t;values[0]=v; } - else - { span[1]=t;values[1]=v; } - } - return(-1); -} - -inline static void EvaluateMedium( const btSoftBodyWorldInfo* wfi, - const btVector3& x, - btSoftBody::sMedium& medium) -{ - medium.m_velocity = btVector3(0,0,0); - medium.m_pressure = 0; - medium.m_density = wfi->air_density; - if(wfi->water_density>0) - { - const btScalar depth=-(btDot(x,wfi->water_normal)+wfi->water_offset); - if(depth>0) - { - medium.m_density = wfi->water_density; - medium.m_pressure = depth*wfi->water_density*wfi->m_gravity.length(); - } - } -} - - -// -static inline btVector3 NormalizeAny(const btVector3& v) -{ - const btScalar l=v.length(); - if(l>SIMD_EPSILON) - return(v/l); - else - return(btVector3(0,0,0)); -} - -// -static inline btDbvtVolume VolumeOf( const btSoftBody::Face& f, - btScalar margin) -{ - const btVector3* pts[]={ &f.m_n[0]->m_x, - &f.m_n[1]->m_x, - &f.m_n[2]->m_x}; - btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3); - vol.Expand(btVector3(margin,margin,margin)); - return(vol); -} - -// -static inline btVector3 CenterOf( const btSoftBody::Face& f) -{ - return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3); -} - -// -static inline btScalar AreaOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2) -{ - const btVector3 a=x1-x0; - const btVector3 b=x2-x0; - const btVector3 cr=btCross(a,b); - const btScalar area=cr.length(); - return(area); -} - -// -static inline btScalar VolumeOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2, - const btVector3& x3) -{ - const btVector3 a=x1-x0; - const btVector3 b=x2-x0; - const btVector3 c=x3-x0; - return(btDot(a,btCross(b,c))); -} - -// - - -// -static inline void ApplyClampedForce( btSoftBody::Node& n, - const btVector3& f, - btScalar dt) -{ - const btScalar dtim=dt*n.m_im; - if((f*dtim).length2()>n.m_v.length2()) - {/* Clamp */ - n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim; - } - else - {/* Apply */ - n.m_f+=f; - } -} - -// -static inline int MatchEdge( const btSoftBody::Node* a, - const btSoftBody::Node* b, - const btSoftBody::Node* ma, - const btSoftBody::Node* mb) -{ - if((a==ma)&&(b==mb)) return(0); - if((a==mb)&&(b==ma)) return(1); - return(-1); -} - -// -// btEigen : Extract eigen system, -// straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html -// outputs are NOT sorted. -// -struct btEigen -{ - static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0) - { - static const int maxiterations=16; - static const btScalar accuracy=(btScalar)0.0001; - btMatrix3x3& v=*vectors; - int iterations=0; - vectors->setIdentity(); - do { - int p=0,q=1; - if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; } - if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; } - if(btFabs(a[p][q])>accuracy) - { - const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]); - const btScalar z=btFabs(w); - const btScalar t=w/(z*(btSqrt(1+w*w)+z)); - if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */ - { - const btScalar c=1/btSqrt(t*t+1); - const btScalar s=c*t; - mulPQ(a,c,s,p,q); - mulTPQ(a,c,s,p,q); - mulPQ(v,c,s,p,q); - } else break; - } else break; - } while((++iterations)<maxiterations); - if(values) - { - *values=btVector3(a[0][0],a[1][1],a[2][2]); - } - return(iterations); - } -private: - static inline void mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q) - { - const btScalar m[2][3]={ {a[p][0],a[p][1],a[p][2]}, - {a[q][0],a[q][1],a[q][2]}}; - int i; - - for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i]; - for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i]; - } - static inline void mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q) - { - const btScalar m[2][3]={ {a[0][p],a[1][p],a[2][p]}, - {a[0][q],a[1][q],a[2][q]}}; - int i; - - for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i]; - for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i]; - } -}; - -// -// Polar decomposition, -// "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986. -// -static inline int PolarDecompose( const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s) -{ - static const btPolarDecomposition polar; - return polar.decompose(m, q, s); -} - -// -// btSoftColliders -// -struct btSoftColliders -{ - // - // ClusterBase - // - struct ClusterBase : btDbvt::ICollide - { - btScalar erp; - btScalar idt; - btScalar m_margin; - btScalar friction; - btScalar threshold; - ClusterBase() - { - erp =(btScalar)1; - idt =0; - m_margin =0; - friction =0; - threshold =(btScalar)0; - } - bool SolveContact( const btGjkEpaSolver2::sResults& res, - btSoftBody::Body ba,const btSoftBody::Body bb, - btSoftBody::CJoint& joint) - { - if(res.distance<m_margin) - { - btVector3 norm = res.normal; - norm.normalize();//is it necessary? - - const btVector3 ra=res.witnesses[0]-ba.xform().getOrigin(); - const btVector3 rb=res.witnesses[1]-bb.xform().getOrigin(); - const btVector3 va=ba.velocity(ra); - const btVector3 vb=bb.velocity(rb); - const btVector3 vrel=va-vb; - const btScalar rvac=btDot(vrel,norm); - btScalar depth=res.distance-m_margin; - -// printf("depth=%f\n",depth); - const btVector3 iv=norm*rvac; - const btVector3 fv=vrel-iv; - joint.m_bodies[0] = ba; - joint.m_bodies[1] = bb; - joint.m_refs[0] = ra*ba.xform().getBasis(); - joint.m_refs[1] = rb*bb.xform().getBasis(); - joint.m_rpos[0] = ra; - joint.m_rpos[1] = rb; - joint.m_cfm = 1; - joint.m_erp = 1; - joint.m_life = 0; - joint.m_maxlife = 0; - joint.m_split = 1; - - joint.m_drift = depth*norm; - - joint.m_normal = norm; -// printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ()); - joint.m_delete = false; - joint.m_friction = fv.length2()<(rvac*friction*rvac*friction)?1:friction; - joint.m_massmatrix = ImpulseMatrix( ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0], - bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]); - - return(true); - } - return(false); - } - }; - // - // CollideCL_RS - // - struct CollideCL_RS : ClusterBase - { - btSoftBody* psb; - const btCollisionObjectWrapper* m_colObjWrap; - - void Process(const btDbvtNode* leaf) - { - btSoftBody::Cluster* cluster=(btSoftBody::Cluster*)leaf->data; - btSoftClusterCollisionShape cshape(cluster); - - const btConvexShape* rshape=(const btConvexShape*)m_colObjWrap->getCollisionShape(); - - ///don't collide an anchored cluster with a static/kinematic object - if(m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject() && cluster->m_containsAnchor) - return; - - btGjkEpaSolver2::sResults res; - if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(), - rshape,m_colObjWrap->getWorldTransform(), - btVector3(1,0,0),res)) - { - btSoftBody::CJoint joint; - if(SolveContact(res,cluster,m_colObjWrap->getCollisionObject(),joint))//prb,joint)) - { - btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); - *pj=joint;psb->m_joints.push_back(pj); - if(m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject()) - { - pj->m_erp *= psb->m_cfg.kSKHR_CL; - pj->m_split *= psb->m_cfg.kSK_SPLT_CL; - } - else - { - pj->m_erp *= psb->m_cfg.kSRHR_CL; - pj->m_split *= psb->m_cfg.kSR_SPLT_CL; - } - } - } - } - void ProcessColObj(btSoftBody* ps,const btCollisionObjectWrapper* colObWrap) - { - psb = ps; - m_colObjWrap = colObWrap; - idt = ps->m_sst.isdt; - m_margin = m_colObjWrap->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin(); - ///Bullet rigid body uses multiply instead of minimum to determine combined friction. Some customization would be useful. - friction = btMin(psb->m_cfg.kDF,m_colObjWrap->getCollisionObject()->getFriction()); - btVector3 mins; - btVector3 maxs; - - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; - colObWrap->getCollisionShape()->getAabb(colObWrap->getWorldTransform(),mins,maxs); - volume=btDbvtVolume::FromMM(mins,maxs); - volume.Expand(btVector3(1,1,1)*m_margin); - ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root,volume,*this); - } - }; - // - // CollideCL_SS - // - struct CollideCL_SS : ClusterBase - { - btSoftBody* bodies[2]; - void Process(const btDbvtNode* la,const btDbvtNode* lb) - { - btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data; - btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data; - - - bool connected=false; - if ((bodies[0]==bodies[1])&&(bodies[0]->m_clusterConnectivity.size())) - { - connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex+bodies[0]->m_clusters.size()*clb->m_clusterIndex]; - } - - if (!connected) - { - btSoftClusterCollisionShape csa(cla); - btSoftClusterCollisionShape csb(clb); - btGjkEpaSolver2::sResults res; - if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(), - &csb,btTransform::getIdentity(), - cla->m_com-clb->m_com,res)) - { - btSoftBody::CJoint joint; - if(SolveContact(res,cla,clb,joint)) - { - btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); - *pj=joint;bodies[0]->m_joints.push_back(pj); - pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL); - pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2; - } - } - } else - { - static int count=0; - count++; - //printf("count=%d\n",count); - - } - } - void ProcessSoftSoft(btSoftBody* psa,btSoftBody* psb) - { - idt = psa->m_sst.isdt; - //m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2; - m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin()); - friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF); - bodies[0] = psa; - bodies[1] = psb; - psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this); - } - }; - // - // CollideSDF_RS - // - struct CollideSDF_RS : btDbvt::ICollide - { - void Process(const btDbvtNode* leaf) - { - btSoftBody::Node* node=(btSoftBody::Node*)leaf->data; - DoNode(*node); - } - void DoNode(btSoftBody::Node& n) const - { - const btScalar m=n.m_im>0?dynmargin:stamargin; - btSoftBody::RContact c; - - if( (!n.m_battach)&& - psb->checkContact(m_colObj1Wrap,n.m_x,m,c.m_cti)) - { - const btScalar ima=n.m_im; - const btScalar imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f; - const btScalar ms=ima+imb; - if(ms>0) - { - const btTransform& wtr=m_rigidBody?m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); - static const btMatrix3x3 iwiStatic(0,0,0,0,0,0,0,0,0); - const btMatrix3x3& iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra=n.m_x-wtr.getOrigin(); - const btVector3 va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt : btVector3(0,0,0); - const btVector3 vb=n.m_x-n.m_q; - const btVector3 vr=vb-va; - const btScalar dn=btDot(vr,c.m_cti.m_normal); - const btVector3 fv=vr-c.m_cti.m_normal*dn; - const btScalar fc=psb->m_cfg.kDF*m_colObj1Wrap->getCollisionObject()->getFriction(); - c.m_node = &n; - c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra); - c.m_c1 = ra; - c.m_c2 = ima*psb->m_sst.sdt; - c.m_c3 = fv.length2()<(dn*fc*dn*fc)?0:1-fc; - c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR; - psb->m_rcontacts.push_back(c); - if (m_rigidBody) - m_rigidBody->activate(); - } - } - } - btSoftBody* psb; - const btCollisionObjectWrapper* m_colObj1Wrap; - btRigidBody* m_rigidBody; - btScalar dynmargin; - btScalar stamargin; - }; - // - // CollideVF_SS - // - struct CollideVF_SS : btDbvt::ICollide - { - void Process(const btDbvtNode* lnode, - const btDbvtNode* lface) - { - btSoftBody::Node* node=(btSoftBody::Node*)lnode->data; - btSoftBody::Face* face=(btSoftBody::Face*)lface->data; - btVector3 o=node->m_x; - btVector3 p; - btScalar d=SIMD_INFINITY; - ProjectOrigin( face->m_n[0]->m_x-o, - face->m_n[1]->m_x-o, - face->m_n[2]->m_x-o, - p,d); - const btScalar m=mrg+(o-node->m_q).length()*2; - if(d<(m*m)) - { - const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]}; - const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o); - const btScalar ma=node->m_im; - btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w); - if( (n[0]->m_im<=0)|| - (n[1]->m_im<=0)|| - (n[2]->m_im<=0)) - { - mb=0; - } - const btScalar ms=ma+mb; - if(ms>0) - { - btSoftBody::SContact c; - c.m_normal = p/-btSqrt(d); - c.m_margin = m; - c.m_node = node; - c.m_face = face; - c.m_weights = w; - c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF); - c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR; - c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR; - psb[0]->m_scontacts.push_back(c); - } - } - } - btSoftBody* psb[2]; - btScalar mrg; - }; -}; - -#endif //_BT_SOFT_BODY_INTERNALS_H diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp b/thirdparty/bullet/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp deleted file mode 100644 index f5a67f6d89..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp +++ /dev/null @@ -1,134 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btSoftBodyRigidBodyCollisionConfiguration.h" -#include "btSoftRigidCollisionAlgorithm.h" -#include "btSoftBodyConcaveCollisionAlgorithm.h" -#include "btSoftSoftCollisionAlgorithm.h" - -#include "LinearMath/btPoolAllocator.h" - -#define ENABLE_SOFTBODY_CONCAVE_COLLISIONS 1 - -btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo) -:btDefaultCollisionConfiguration(constructionInfo) -{ - void* mem; - - mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc),16); - m_softSoftCreateFunc = new(mem) btSoftSoftCollisionAlgorithm::CreateFunc; - - mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); - m_softRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; - - mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); - m_swappedSoftRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; - m_swappedSoftRigidConvexCreateFunc->m_swapped=true; - -#ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS - mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16); - m_softRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc; - - mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16); - m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc; - m_swappedSoftRigidConcaveCreateFunc->m_swapped=true; -#endif - - //replace pool by a new one, with potential larger size - - if (m_ownsCollisionAlgorithmPool && m_collisionAlgorithmPool) - { - int curElemSize = m_collisionAlgorithmPool->getElementSize(); - ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool - - - int maxSize0 = sizeof(btSoftSoftCollisionAlgorithm); - int maxSize1 = sizeof(btSoftRigidCollisionAlgorithm); - int maxSize2 = sizeof(btSoftBodyConcaveCollisionAlgorithm); - - int collisionAlgorithmMaxElementSize = btMax(maxSize0,maxSize1); - collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); - - if (collisionAlgorithmMaxElementSize > curElemSize) - { - m_collisionAlgorithmPool->~btPoolAllocator(); - btAlignedFree(m_collisionAlgorithmPool); - void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); - m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); - } - } - -} - -btSoftBodyRigidBodyCollisionConfiguration::~btSoftBodyRigidBodyCollisionConfiguration() -{ - m_softSoftCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_softSoftCreateFunc); - - m_softRigidConvexCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_softRigidConvexCreateFunc); - - m_swappedSoftRigidConvexCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_swappedSoftRigidConvexCreateFunc); - -#ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS - m_softRigidConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_softRigidConcaveCreateFunc); - - m_swappedSoftRigidConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_swappedSoftRigidConcaveCreateFunc); -#endif -} - -///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation -btCollisionAlgorithmCreateFunc* btSoftBodyRigidBodyCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) -{ - - ///try to handle the softbody interactions first - - if ((proxyType0 == SOFTBODY_SHAPE_PROXYTYPE ) && (proxyType1==SOFTBODY_SHAPE_PROXYTYPE)) - { - return m_softSoftCreateFunc; - } - - ///softbody versus convex - if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConvex(proxyType1)) - { - return m_softRigidConvexCreateFunc; - } - - ///convex versus soft body - if (btBroadphaseProxy::isConvex(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE ) - { - return m_swappedSoftRigidConvexCreateFunc; - } - -#ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS - ///softbody versus convex - if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConcave(proxyType1)) - { - return m_softRigidConcaveCreateFunc; - } - - ///convex versus soft body - if (btBroadphaseProxy::isConcave(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE ) - { - return m_swappedSoftRigidConcaveCreateFunc; - } -#endif - - ///fallback to the regular rigid collision shape - return btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(proxyType0,proxyType1); -} diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h b/thirdparty/bullet/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h deleted file mode 100644 index 21addcfe2e..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h +++ /dev/null @@ -1,48 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION -#define BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION - -#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h" - -class btVoronoiSimplexSolver; -class btGjkEpaPenetrationDepthSolver; - - -///btSoftBodyRigidBodyCollisionConfiguration add softbody interaction on top of btDefaultCollisionConfiguration -class btSoftBodyRigidBodyCollisionConfiguration : public btDefaultCollisionConfiguration -{ - - //default CreationFunctions, filling the m_doubleDispatch table - btCollisionAlgorithmCreateFunc* m_softSoftCreateFunc; - btCollisionAlgorithmCreateFunc* m_softRigidConvexCreateFunc; - btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConvexCreateFunc; - btCollisionAlgorithmCreateFunc* m_softRigidConcaveCreateFunc; - btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConcaveCreateFunc; - -public: - - btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo()); - - virtual ~btSoftBodyRigidBodyCollisionConfiguration(); - - ///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation - virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); - -}; - -#endif //BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION - diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h b/thirdparty/bullet/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h deleted file mode 100644 index c4733d6400..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h +++ /dev/null @@ -1,165 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H -#define BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H - - -class btVertexBufferDescriptor -{ -public: - enum BufferTypes - { - CPU_BUFFER, - DX11_BUFFER, - OPENGL_BUFFER - }; - -protected: - - bool m_hasVertexPositions; - bool m_hasNormals; - - int m_vertexOffset; - int m_vertexStride; - - int m_normalOffset; - int m_normalStride; - -public: - btVertexBufferDescriptor() - { - m_hasVertexPositions = false; - m_hasNormals = false; - m_vertexOffset = 0; - m_vertexStride = 0; - m_normalOffset = 0; - m_normalStride = 0; - } - - virtual ~btVertexBufferDescriptor() - { - - } - - virtual bool hasVertexPositions() const - { - return m_hasVertexPositions; - } - - virtual bool hasNormals() const - { - return m_hasNormals; - } - - /** - * Return the type of the vertex buffer descriptor. - */ - virtual BufferTypes getBufferType() const = 0; - - /** - * Return the vertex offset in floats from the base pointer. - */ - virtual int getVertexOffset() const - { - return m_vertexOffset; - } - - /** - * Return the vertex stride in number of floats between vertices. - */ - virtual int getVertexStride() const - { - return m_vertexStride; - } - - /** - * Return the vertex offset in floats from the base pointer. - */ - virtual int getNormalOffset() const - { - return m_normalOffset; - } - - /** - * Return the vertex stride in number of floats between vertices. - */ - virtual int getNormalStride() const - { - return m_normalStride; - } -}; - - -class btCPUVertexBufferDescriptor : public btVertexBufferDescriptor -{ -protected: - float *m_basePointer; - -public: - /** - * vertexBasePointer is pointer to beginning of the buffer. - * vertexOffset is the offset in floats to the first vertex. - * vertexStride is the stride in floats between vertices. - */ - btCPUVertexBufferDescriptor( float *basePointer, int vertexOffset, int vertexStride ) - { - m_basePointer = basePointer; - m_vertexOffset = vertexOffset; - m_vertexStride = vertexStride; - m_hasVertexPositions = true; - } - - /** - * vertexBasePointer is pointer to beginning of the buffer. - * vertexOffset is the offset in floats to the first vertex. - * vertexStride is the stride in floats between vertices. - */ - btCPUVertexBufferDescriptor( float *basePointer, int vertexOffset, int vertexStride, int normalOffset, int normalStride ) - { - m_basePointer = basePointer; - - m_vertexOffset = vertexOffset; - m_vertexStride = vertexStride; - m_hasVertexPositions = true; - - m_normalOffset = normalOffset; - m_normalStride = normalStride; - m_hasNormals = true; - } - - virtual ~btCPUVertexBufferDescriptor() - { - - } - - /** - * Return the type of the vertex buffer descriptor. - */ - virtual BufferTypes getBufferType() const - { - return CPU_BUFFER; - } - - /** - * Return the base pointer in memory to the first vertex. - */ - virtual float *getBasePointer() const - { - return m_basePointer; - } -}; - -#endif // #ifndef BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftBodySolvers.h b/thirdparty/bullet/src/BulletSoftBody/btSoftBodySolvers.h deleted file mode 100644 index 6947bc27d2..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftBodySolvers.h +++ /dev/null @@ -1,154 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFT_BODY_SOLVERS_H -#define BT_SOFT_BODY_SOLVERS_H - -#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" - - -class btSoftBodyTriangleData; -class btSoftBodyLinkData; -class btSoftBodyVertexData; -class btVertexBufferDescriptor; -class btCollisionObject; -class btSoftBody; - - -class btSoftBodySolver -{ -public: - enum SolverTypes - { - DEFAULT_SOLVER, - CPU_SOLVER, - CL_SOLVER, - CL_SIMD_SOLVER, - DX_SOLVER, - DX_SIMD_SOLVER - }; - - -protected: - int m_numberOfPositionIterations; - int m_numberOfVelocityIterations; - // Simulation timescale - float m_timeScale; - -public: - btSoftBodySolver() : - m_numberOfPositionIterations( 10 ), - m_timeScale( 1 ) - { - m_numberOfVelocityIterations = 0; - m_numberOfPositionIterations = 5; - } - - virtual ~btSoftBodySolver() - { - } - - /** - * Return the type of the solver. - */ - virtual SolverTypes getSolverType() const = 0; - - - /** Ensure that this solver is initialized. */ - virtual bool checkInitialized() = 0; - - /** Optimize soft bodies in this solver. */ - virtual void optimize( btAlignedObjectArray< btSoftBody * > &softBodies , bool forceUpdate=false) = 0; - - /** Copy necessary data back to the original soft body source objects. */ - virtual void copyBackToSoftBodies(bool bMove = true) = 0; - - /** Predict motion of soft bodies into next timestep */ - virtual void predictMotion( float solverdt ) = 0; - - /** Solve constraints for a set of soft bodies */ - virtual void solveConstraints( float solverdt ) = 0; - - /** Perform necessary per-step updates of soft bodies such as recomputing normals and bounding boxes */ - virtual void updateSoftBodies() = 0; - - /** Process a collision between one of the world's soft bodies and another collision object */ - virtual void processCollision( btSoftBody *, const struct btCollisionObjectWrapper* ) = 0; - - /** Process a collision between two soft bodies */ - virtual void processCollision( btSoftBody*, btSoftBody* ) = 0; - - /** Set the number of velocity constraint solver iterations this solver uses. */ - virtual void setNumberOfPositionIterations( int iterations ) - { - m_numberOfPositionIterations = iterations; - } - - /** Get the number of velocity constraint solver iterations this solver uses. */ - virtual int getNumberOfPositionIterations() - { - return m_numberOfPositionIterations; - } - - /** Set the number of velocity constraint solver iterations this solver uses. */ - virtual void setNumberOfVelocityIterations( int iterations ) - { - m_numberOfVelocityIterations = iterations; - } - - /** Get the number of velocity constraint solver iterations this solver uses. */ - virtual int getNumberOfVelocityIterations() - { - return m_numberOfVelocityIterations; - } - - /** Return the timescale that the simulation is using */ - float getTimeScale() - { - return m_timeScale; - } - -#if 0 - /** - * Add a collision object to be used by the indicated softbody. - */ - virtual void addCollisionObjectForSoftBody( int clothIdentifier, btCollisionObject *collisionObject ) = 0; -#endif -}; - -/** - * Class to manage movement of data from a solver to a given target. - * This version is abstract. Subclasses will have custom pairings for different combinations. - */ -class btSoftBodySolverOutput -{ -protected: - -public: - btSoftBodySolverOutput() - { - } - - virtual ~btSoftBodySolverOutput() - { - } - - - /** Output current computed vertex data to the vertex buffers for all cloths in the solver. */ - virtual void copySoftBodyToVertexBuffer( const btSoftBody * const softBody, btVertexBufferDescriptor *vertexBuffer ) = 0; -}; - - -#endif // #ifndef BT_SOFT_BODY_SOLVERS_H diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp b/thirdparty/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp deleted file mode 100644 index 4e76dca9db..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp +++ /dev/null @@ -1,367 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btSoftMultiBodyDynamicsWorld.h" -#include "LinearMath/btQuickprof.h" - -//softbody & helpers -#include "BulletSoftBody/btSoftBody.h" -#include "BulletSoftBody/btSoftBodyHelpers.h" -#include "BulletSoftBody/btSoftBodySolvers.h" -#include "BulletSoftBody/btDefaultSoftBodySolver.h" -#include "LinearMath/btSerializer.h" - - -btSoftMultiBodyDynamicsWorld::btSoftMultiBodyDynamicsWorld( - btDispatcher* dispatcher, - btBroadphaseInterface* pairCache, - btMultiBodyConstraintSolver* constraintSolver, - btCollisionConfiguration* collisionConfiguration, - btSoftBodySolver *softBodySolver ) : - btMultiBodyDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), - m_softBodySolver( softBodySolver ), - m_ownsSolver(false) -{ - if( !m_softBodySolver ) - { - void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver),16); - m_softBodySolver = new(ptr) btDefaultSoftBodySolver(); - m_ownsSolver = true; - } - - m_drawFlags = fDrawFlags::Std; - m_drawNodeTree = true; - m_drawFaceTree = false; - m_drawClusterTree = false; - m_sbi.m_broadphase = pairCache; - m_sbi.m_dispatcher = dispatcher; - m_sbi.m_sparsesdf.Initialize(); - m_sbi.m_sparsesdf.Reset(); - - m_sbi.air_density = (btScalar)1.2; - m_sbi.water_density = 0; - m_sbi.water_offset = 0; - m_sbi.water_normal = btVector3(0,0,0); - m_sbi.m_gravity.setValue(0,-10,0); - - m_sbi.m_sparsesdf.Initialize(); - - -} - -btSoftMultiBodyDynamicsWorld::~btSoftMultiBodyDynamicsWorld() -{ - if (m_ownsSolver) - { - m_softBodySolver->~btSoftBodySolver(); - btAlignedFree(m_softBodySolver); - } -} - -void btSoftMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) -{ - btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep ); - { - BT_PROFILE("predictUnconstraintMotionSoftBody"); - m_softBodySolver->predictMotion( float(timeStep) ); - } -} - -void btSoftMultiBodyDynamicsWorld::internalSingleStepSimulation( btScalar timeStep ) -{ - - // Let the solver grab the soft bodies and if necessary optimize for it - m_softBodySolver->optimize( getSoftBodyArray() ); - - if( !m_softBodySolver->checkInitialized() ) - { - btAssert( "Solver initialization failed\n" ); - } - - btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep ); - - ///solve soft bodies constraints - solveSoftBodiesConstraints( timeStep ); - - //self collisions - for ( int i=0;i<m_softBodies.size();i++) - { - btSoftBody* psb=(btSoftBody*)m_softBodies[i]; - psb->defaultCollisionHandler(psb); - } - - ///update soft bodies - m_softBodySolver->updateSoftBodies( ); - - // End solver-wise simulation step - // /////////////////////////////// - -} - -void btSoftMultiBodyDynamicsWorld::solveSoftBodiesConstraints( btScalar timeStep ) -{ - BT_PROFILE("solveSoftConstraints"); - - if(m_softBodies.size()) - { - btSoftBody::solveClusters(m_softBodies); - } - - // Solve constraints solver-wise - m_softBodySolver->solveConstraints( timeStep * m_softBodySolver->getTimeScale() ); - -} - -void btSoftMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask) -{ - m_softBodies.push_back(body); - - // Set the soft body solver that will deal with this body - // to be the world's solver - body->setSoftBodySolver( m_softBodySolver ); - - btCollisionWorld::addCollisionObject(body, - collisionFilterGroup, - collisionFilterMask); - -} - -void btSoftMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body) -{ - m_softBodies.remove(body); - - btCollisionWorld::removeCollisionObject(body); -} - -void btSoftMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) -{ - btSoftBody* body = btSoftBody::upcast(collisionObject); - if (body) - removeSoftBody(body); - else - btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); -} - -void btSoftMultiBodyDynamicsWorld::debugDrawWorld() -{ - btDiscreteDynamicsWorld::debugDrawWorld(); - - if (getDebugDrawer()) - { - int i; - for ( i=0;i<this->m_softBodies.size();i++) - { - btSoftBody* psb=(btSoftBody*)this->m_softBodies[i]; - if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) - { - btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer); - btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags); - } - - if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) - { - if(m_drawNodeTree) btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer); - if(m_drawFaceTree) btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer); - if(m_drawClusterTree) btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer); - } - } - } -} - - - - -struct btSoftSingleRayCallback : public btBroadphaseRayCallback -{ - btVector3 m_rayFromWorld; - btVector3 m_rayToWorld; - btTransform m_rayFromTrans; - btTransform m_rayToTrans; - btVector3 m_hitNormal; - - const btSoftMultiBodyDynamicsWorld* m_world; - btCollisionWorld::RayResultCallback& m_resultCallback; - - btSoftSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btSoftMultiBodyDynamicsWorld* world,btCollisionWorld::RayResultCallback& resultCallback) - :m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld), - m_world(world), - m_resultCallback(resultCallback) - { - m_rayFromTrans.setIdentity(); - m_rayFromTrans.setOrigin(m_rayFromWorld); - m_rayToTrans.setIdentity(); - m_rayToTrans.setOrigin(m_rayToWorld); - - btVector3 rayDir = (rayToWorld-rayFromWorld); - - rayDir.normalize (); - ///what about division by zero? --> just set rayDirection[i] to INF/1e30 - m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; - m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; - m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; - m_signs[0] = m_rayDirectionInverse[0] < 0.0; - m_signs[1] = m_rayDirectionInverse[1] < 0.0; - m_signs[2] = m_rayDirectionInverse[2] < 0.0; - - m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld); - - } - - - - virtual bool process(const btBroadphaseProxy* proxy) - { - ///terminate further ray tests, once the closestHitFraction reached zero - if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) - return false; - - btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; - - //only perform raycast if filterMask matches - if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) - { - //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; -#if 0 -#ifdef RECALCULATE_AABB - btVector3 collisionObjectAabbMin,collisionObjectAabbMax; - collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); -#else - //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax); - const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin; - const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax; -#endif -#endif - //btScalar hitLambda = m_resultCallback.m_closestHitFraction; - //culling already done by broadphase - //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) - { - m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - m_resultCallback); - } - } - return true; - } -}; - -void btSoftMultiBodyDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const -{ - BT_PROFILE("rayTest"); - /// use the broadphase to accelerate the search for objects, based on their aabb - /// and for each object with ray-aabb overlap, perform an exact ray test - btSoftSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback); - -#ifndef USE_BRUTEFORCE_RAYBROADPHASE - m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB); -#else - for (int i=0;i<this->getNumCollisionObjects();i++) - { - rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); - } -#endif //USE_BRUTEFORCE_RAYBROADPHASE - -} - - -void btSoftMultiBodyDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback) -{ - if (collisionShape->isSoftBody()) { - btSoftBody* softBody = btSoftBody::upcast(collisionObject); - if (softBody) { - btSoftBody::sRayCast softResult; - if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) - { - - if (softResult.fraction<= resultCallback.m_closestHitFraction) - { - - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = 0; - shapeInfo.m_triangleIndex = softResult.index; - // get the normal - btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); - btVector3 normal=-rayDir; - normal.normalize(); - - if (softResult.feature == btSoftBody::eFeature::Face) - { - normal = softBody->m_faces[softResult.index].m_normal; - if (normal.dot(rayDir) > 0) { - // normal always point toward origin of the ray - normal = -normal; - } - } - - btCollisionWorld::LocalRayResult rayResult - (collisionObject, - &shapeInfo, - normal, - softResult.fraction); - bool normalInWorldSpace = true; - resultCallback.addSingleResult(rayResult,normalInWorldSpace); - } - } - } - } - else { - btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback); - } -} - - -void btSoftMultiBodyDynamicsWorld::serializeSoftBodies(btSerializer* serializer) -{ - int i; - //serialize all collision objects - for (i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - if (colObj->getInternalType() & btCollisionObject::CO_SOFT_BODY) - { - int len = colObj->calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len,1); - const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_SOFTBODY_CODE,colObj); - } - } - -} - -void btSoftMultiBodyDynamicsWorld::serialize(btSerializer* serializer) -{ - - serializer->startSerialization(); - - serializeDynamicsWorldInfo( serializer); - - serializeSoftBodies(serializer); - - serializeRigidBodies(serializer); - - serializeCollisionObjects(serializer); - - serializer->finishSerialization(); -} - - diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h b/thirdparty/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h deleted file mode 100644 index 6d46a21db5..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h +++ /dev/null @@ -1,110 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H -#define BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H - -#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" -#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" -#include "BulletSoftBody/btSoftBody.h" - -#ifndef BT_SOFT_RIGID_DYNAMICS_WORLD_H -typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray; -#endif - -class btSoftBodySolver; - -class btSoftMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld -{ - - btSoftBodyArray m_softBodies; - int m_drawFlags; - bool m_drawNodeTree; - bool m_drawFaceTree; - bool m_drawClusterTree; - btSoftBodyWorldInfo m_sbi; - ///Solver classes that encapsulate multiple soft bodies for solving - btSoftBodySolver *m_softBodySolver; - bool m_ownsSolver; - -protected: - - virtual void predictUnconstraintMotion(btScalar timeStep); - - virtual void internalSingleStepSimulation( btScalar timeStep); - - void solveSoftBodiesConstraints( btScalar timeStep ); - - void serializeSoftBodies(btSerializer* serializer); - -public: - - btSoftMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver *softBodySolver = 0 ); - - virtual ~btSoftMultiBodyDynamicsWorld(); - - virtual void debugDrawWorld(); - - void addSoftBody(btSoftBody* body, int collisionFilterGroup=btBroadphaseProxy::DefaultFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter); - - void removeSoftBody(btSoftBody* body); - - ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btDiscreteDynamicsWorld::removeCollisionObject - virtual void removeCollisionObject(btCollisionObject* collisionObject); - - int getDrawFlags() const { return(m_drawFlags); } - void setDrawFlags(int f) { m_drawFlags=f; } - - btSoftBodyWorldInfo& getWorldInfo() - { - return m_sbi; - } - const btSoftBodyWorldInfo& getWorldInfo() const - { - return m_sbi; - } - - virtual btDynamicsWorldType getWorldType() const - { - return BT_SOFT_MULTIBODY_DYNAMICS_WORLD; - } - - btSoftBodyArray& getSoftBodyArray() - { - return m_softBodies; - } - - const btSoftBodyArray& getSoftBodyArray() const - { - return m_softBodies; - } - - - virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; - - /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. - /// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape. - /// This allows more customization. - static void rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback); - - virtual void serialize(btSerializer* serializer); - -}; - -#endif //BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp b/thirdparty/bullet/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp deleted file mode 100644 index 01c148a2ca..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btSoftRigidCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "btSoftBody.h" -#include "BulletSoftBody/btSoftBodySolvers.h" -#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - -///TODO: include all the shapes that the softbody can collide with -///alternatively, implement special case collision algorithms (just like for rigid collision shapes) - -//#include <stdio.h> - -btSoftRigidCollisionAlgorithm::btSoftRigidCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* ,const btCollisionObjectWrapper* , bool isSwapped) -: btCollisionAlgorithm(ci), -//m_ownManifold(false), -//m_manifoldPtr(mf), -m_isSwapped(isSwapped) -{ -} - - -btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm() -{ - - //m_softBody->m_overlappingRigidBodies.remove(m_rigidCollisionObject); - - /*if (m_ownManifold) - { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); - } - */ - -} - - -#include <stdio.h> - -void btSoftRigidCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)dispatchInfo; - (void)resultOut; - //printf("btSoftRigidCollisionAlgorithm\n"); -// const btCollisionObjectWrapper* softWrap = m_isSwapped?body1Wrap:body0Wrap; -// const btCollisionObjectWrapper* rigidWrap = m_isSwapped?body0Wrap:body1Wrap; - btSoftBody* softBody = m_isSwapped? (btSoftBody*)body1Wrap->getCollisionObject() : (btSoftBody*)body0Wrap->getCollisionObject(); - const btCollisionObjectWrapper* rigidCollisionObjectWrap = m_isSwapped? body0Wrap : body1Wrap; - - if (softBody->m_collisionDisabledObjects.findLinearSearch(rigidCollisionObjectWrap->getCollisionObject())==softBody->m_collisionDisabledObjects.size()) - { - softBody->getSoftBodySolver()->processCollision(softBody, rigidCollisionObjectWrap); - } - - -} - -btScalar btSoftRigidCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)resultOut; - (void)dispatchInfo; - (void)col0; - (void)col1; - - //not yet - return btScalar(1.); -} - - - diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h b/thirdparty/bullet/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h deleted file mode 100644 index 93fcc6065b..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h +++ /dev/null @@ -1,75 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFT_RIGID_COLLISION_ALGORITHM_H -#define BT_SOFT_RIGID_COLLISION_ALGORITHM_H - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -class btPersistentManifold; -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" - -#include "LinearMath/btVector3.h" -class btSoftBody; - -/// btSoftRigidCollisionAlgorithm provides collision detection between btSoftBody and btRigidBody -class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm -{ - // bool m_ownManifold; - // btPersistentManifold* m_manifoldPtr; - - //btSoftBody* m_softBody; - //btCollisionObject* m_rigidCollisionObject; - - ///for rigid versus soft (instead of soft versus rigid), we use this swapped boolean - bool m_isSwapped; - -public: - - btSoftRigidCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0,const btCollisionObjectWrapper* col1Wrap, bool isSwapped); - - virtual ~btSoftRigidCollisionAlgorithm(); - - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - //we don't add any manifolds - } - - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) - { - void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftRigidCollisionAlgorithm)); - if (!m_swapped) - { - return new(mem) btSoftRigidCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,false); - } else - { - return new(mem) btSoftRigidCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,true); - } - } - }; - -}; - -#endif //BT_SOFT_RIGID_COLLISION_ALGORITHM_H - - diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/thirdparty/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp deleted file mode 100644 index 204b4f576d..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp +++ /dev/null @@ -1,367 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btSoftRigidDynamicsWorld.h" -#include "LinearMath/btQuickprof.h" - -//softbody & helpers -#include "btSoftBody.h" -#include "btSoftBodyHelpers.h" -#include "btSoftBodySolvers.h" -#include "btDefaultSoftBodySolver.h" -#include "LinearMath/btSerializer.h" - - -btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld( - btDispatcher* dispatcher, - btBroadphaseInterface* pairCache, - btConstraintSolver* constraintSolver, - btCollisionConfiguration* collisionConfiguration, - btSoftBodySolver *softBodySolver ) : - btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), - m_softBodySolver( softBodySolver ), - m_ownsSolver(false) -{ - if( !m_softBodySolver ) - { - void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver),16); - m_softBodySolver = new(ptr) btDefaultSoftBodySolver(); - m_ownsSolver = true; - } - - m_drawFlags = fDrawFlags::Std; - m_drawNodeTree = true; - m_drawFaceTree = false; - m_drawClusterTree = false; - m_sbi.m_broadphase = pairCache; - m_sbi.m_dispatcher = dispatcher; - m_sbi.m_sparsesdf.Initialize(); - m_sbi.m_sparsesdf.Reset(); - - m_sbi.air_density = (btScalar)1.2; - m_sbi.water_density = 0; - m_sbi.water_offset = 0; - m_sbi.water_normal = btVector3(0,0,0); - m_sbi.m_gravity.setValue(0,-10,0); - - m_sbi.m_sparsesdf.Initialize(); - - -} - -btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld() -{ - if (m_ownsSolver) - { - m_softBodySolver->~btSoftBodySolver(); - btAlignedFree(m_softBodySolver); - } -} - -void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) -{ - btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep ); - { - BT_PROFILE("predictUnconstraintMotionSoftBody"); - m_softBodySolver->predictMotion( float(timeStep) ); - } -} - -void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep ) -{ - - // Let the solver grab the soft bodies and if necessary optimize for it - m_softBodySolver->optimize( getSoftBodyArray() ); - - if( !m_softBodySolver->checkInitialized() ) - { - btAssert( "Solver initialization failed\n" ); - } - - btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep ); - - ///solve soft bodies constraints - solveSoftBodiesConstraints( timeStep ); - - //self collisions - for ( int i=0;i<m_softBodies.size();i++) - { - btSoftBody* psb=(btSoftBody*)m_softBodies[i]; - psb->defaultCollisionHandler(psb); - } - - ///update soft bodies - m_softBodySolver->updateSoftBodies( ); - - // End solver-wise simulation step - // /////////////////////////////// - -} - -void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints( btScalar timeStep ) -{ - BT_PROFILE("solveSoftConstraints"); - - if(m_softBodies.size()) - { - btSoftBody::solveClusters(m_softBodies); - } - - // Solve constraints solver-wise - m_softBodySolver->solveConstraints( timeStep * m_softBodySolver->getTimeScale() ); - -} - -void btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask) -{ - m_softBodies.push_back(body); - - // Set the soft body solver that will deal with this body - // to be the world's solver - body->setSoftBodySolver( m_softBodySolver ); - - btCollisionWorld::addCollisionObject(body, - collisionFilterGroup, - collisionFilterMask); - -} - -void btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body) -{ - m_softBodies.remove(body); - - btCollisionWorld::removeCollisionObject(body); -} - -void btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) -{ - btSoftBody* body = btSoftBody::upcast(collisionObject); - if (body) - removeSoftBody(body); - else - btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); -} - -void btSoftRigidDynamicsWorld::debugDrawWorld() -{ - btDiscreteDynamicsWorld::debugDrawWorld(); - - if (getDebugDrawer()) - { - int i; - for ( i=0;i<this->m_softBodies.size();i++) - { - btSoftBody* psb=(btSoftBody*)this->m_softBodies[i]; - if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) - { - btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer); - btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags); - } - - if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) - { - if(m_drawNodeTree) btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer); - if(m_drawFaceTree) btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer); - if(m_drawClusterTree) btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer); - } - } - } -} - - - - -struct btSoftSingleRayCallback : public btBroadphaseRayCallback -{ - btVector3 m_rayFromWorld; - btVector3 m_rayToWorld; - btTransform m_rayFromTrans; - btTransform m_rayToTrans; - btVector3 m_hitNormal; - - const btSoftRigidDynamicsWorld* m_world; - btCollisionWorld::RayResultCallback& m_resultCallback; - - btSoftSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btSoftRigidDynamicsWorld* world,btCollisionWorld::RayResultCallback& resultCallback) - :m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld), - m_world(world), - m_resultCallback(resultCallback) - { - m_rayFromTrans.setIdentity(); - m_rayFromTrans.setOrigin(m_rayFromWorld); - m_rayToTrans.setIdentity(); - m_rayToTrans.setOrigin(m_rayToWorld); - - btVector3 rayDir = (rayToWorld-rayFromWorld); - - rayDir.normalize (); - ///what about division by zero? --> just set rayDirection[i] to INF/1e30 - m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; - m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; - m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; - m_signs[0] = m_rayDirectionInverse[0] < 0.0; - m_signs[1] = m_rayDirectionInverse[1] < 0.0; - m_signs[2] = m_rayDirectionInverse[2] < 0.0; - - m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld); - - } - - - - virtual bool process(const btBroadphaseProxy* proxy) - { - ///terminate further ray tests, once the closestHitFraction reached zero - if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) - return false; - - btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; - - //only perform raycast if filterMask matches - if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) - { - //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; -#if 0 -#ifdef RECALCULATE_AABB - btVector3 collisionObjectAabbMin,collisionObjectAabbMax; - collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); -#else - //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax); - const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin; - const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax; -#endif -#endif - //btScalar hitLambda = m_resultCallback.m_closestHitFraction; - //culling already done by broadphase - //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) - { - m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - m_resultCallback); - } - } - return true; - } -}; - -void btSoftRigidDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const -{ - BT_PROFILE("rayTest"); - /// use the broadphase to accelerate the search for objects, based on their aabb - /// and for each object with ray-aabb overlap, perform an exact ray test - btSoftSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback); - -#ifndef USE_BRUTEFORCE_RAYBROADPHASE - m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB); -#else - for (int i=0;i<this->getNumCollisionObjects();i++) - { - rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); - } -#endif //USE_BRUTEFORCE_RAYBROADPHASE - -} - - -void btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback) -{ - if (collisionShape->isSoftBody()) { - btSoftBody* softBody = btSoftBody::upcast(collisionObject); - if (softBody) { - btSoftBody::sRayCast softResult; - if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) - { - - if (softResult.fraction<= resultCallback.m_closestHitFraction) - { - - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = 0; - shapeInfo.m_triangleIndex = softResult.index; - // get the normal - btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); - btVector3 normal=-rayDir; - normal.normalize(); - - if (softResult.feature == btSoftBody::eFeature::Face) - { - normal = softBody->m_faces[softResult.index].m_normal; - if (normal.dot(rayDir) > 0) { - // normal always point toward origin of the ray - normal = -normal; - } - } - - btCollisionWorld::LocalRayResult rayResult - (collisionObject, - &shapeInfo, - normal, - softResult.fraction); - bool normalInWorldSpace = true; - resultCallback.addSingleResult(rayResult,normalInWorldSpace); - } - } - } - } - else { - btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback); - } -} - - -void btSoftRigidDynamicsWorld::serializeSoftBodies(btSerializer* serializer) -{ - int i; - //serialize all collision objects - for (i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - if (colObj->getInternalType() & btCollisionObject::CO_SOFT_BODY) - { - int len = colObj->calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len,1); - const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_SOFTBODY_CODE,colObj); - } - } - -} - -void btSoftRigidDynamicsWorld::serialize(btSerializer* serializer) -{ - - serializer->startSerialization(); - - serializeDynamicsWorldInfo( serializer); - - serializeSoftBodies(serializer); - - serializeRigidBodies(serializer); - - serializeCollisionObjects(serializer); - - serializer->finishSerialization(); -} - - diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.h b/thirdparty/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.h deleted file mode 100644 index d921a6488d..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.h +++ /dev/null @@ -1,107 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFT_RIGID_DYNAMICS_WORLD_H -#define BT_SOFT_RIGID_DYNAMICS_WORLD_H - -#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" -#include "btSoftBody.h" - -typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray; - -class btSoftBodySolver; - -class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld -{ - - btSoftBodyArray m_softBodies; - int m_drawFlags; - bool m_drawNodeTree; - bool m_drawFaceTree; - bool m_drawClusterTree; - btSoftBodyWorldInfo m_sbi; - ///Solver classes that encapsulate multiple soft bodies for solving - btSoftBodySolver *m_softBodySolver; - bool m_ownsSolver; - -protected: - - virtual void predictUnconstraintMotion(btScalar timeStep); - - virtual void internalSingleStepSimulation( btScalar timeStep); - - void solveSoftBodiesConstraints( btScalar timeStep ); - - void serializeSoftBodies(btSerializer* serializer); - -public: - - btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver *softBodySolver = 0 ); - - virtual ~btSoftRigidDynamicsWorld(); - - virtual void debugDrawWorld(); - - void addSoftBody(btSoftBody* body, int collisionFilterGroup=btBroadphaseProxy::DefaultFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter); - - void removeSoftBody(btSoftBody* body); - - ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btDiscreteDynamicsWorld::removeCollisionObject - virtual void removeCollisionObject(btCollisionObject* collisionObject); - - int getDrawFlags() const { return(m_drawFlags); } - void setDrawFlags(int f) { m_drawFlags=f; } - - btSoftBodyWorldInfo& getWorldInfo() - { - return m_sbi; - } - const btSoftBodyWorldInfo& getWorldInfo() const - { - return m_sbi; - } - - virtual btDynamicsWorldType getWorldType() const - { - return BT_SOFT_RIGID_DYNAMICS_WORLD; - } - - btSoftBodyArray& getSoftBodyArray() - { - return m_softBodies; - } - - const btSoftBodyArray& getSoftBodyArray() const - { - return m_softBodies; - } - - - virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; - - /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. - /// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape. - /// This allows more customization. - static void rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback); - - virtual void serialize(btSerializer* serializer); - -}; - -#endif //BT_SOFT_RIGID_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp b/thirdparty/bullet/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp deleted file mode 100644 index 72043e69e2..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp +++ /dev/null @@ -1,48 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btSoftSoftCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletSoftBody/btSoftBodySolvers.h" -#include "btSoftBody.h" -#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - -#define USE_PERSISTENT_CONTACTS 1 - -btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* /*obj0*/,const btCollisionObjectWrapper* /*obj1*/) -: btCollisionAlgorithm(ci) -//m_ownManifold(false), -//m_manifoldPtr(mf) -{ -} - -btSoftSoftCollisionAlgorithm::~btSoftSoftCollisionAlgorithm() -{ -} - -void btSoftSoftCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) -{ - btSoftBody* soft0 = (btSoftBody*)body0Wrap->getCollisionObject(); - btSoftBody* soft1 = (btSoftBody*)body1Wrap->getCollisionObject(); - soft0->getSoftBodySolver()->processCollision(soft0, soft1); -} - -btScalar btSoftSoftCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) -{ - //not yet - return 1.f; -} diff --git a/thirdparty/bullet/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h b/thirdparty/bullet/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h deleted file mode 100644 index 4eab7aea2f..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h +++ /dev/null @@ -1,69 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFT_SOFT_COLLISION_ALGORITHM_H -#define BT_SOFT_SOFT_COLLISION_ALGORITHM_H - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" - -class btPersistentManifold; -class btSoftBody; - -///collision detection between two btSoftBody shapes -class btSoftSoftCollisionAlgorithm : public btCollisionAlgorithm -{ - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - -// btSoftBody* m_softBody0; -// btSoftBody* m_softBody1; - - -public: - btSoftSoftCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) - : btCollisionAlgorithm(ci) {} - - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - if (m_manifoldPtr && m_ownManifold) - manifoldArray.push_back(m_manifoldPtr); - } - - btSoftSoftCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); - - virtual ~btSoftSoftCollisionAlgorithm(); - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) - { - int bbsize = sizeof(btSoftSoftCollisionAlgorithm); - void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); - return new(ptr) btSoftSoftCollisionAlgorithm(0,ci,body0Wrap,body1Wrap); - } - }; - -}; - -#endif //BT_SOFT_SOFT_COLLISION_ALGORITHM_H - - diff --git a/thirdparty/bullet/src/BulletSoftBody/btSparseSDF.h b/thirdparty/bullet/src/BulletSoftBody/btSparseSDF.h deleted file mode 100644 index ba437c28ef..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/btSparseSDF.h +++ /dev/null @@ -1,319 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btSparseSdf implementation by Nathanael Presson - -#ifndef BT_SPARSE_SDF_H -#define BT_SPARSE_SDF_H - -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" - -// Modified Paul Hsieh hash -template <const int DWORDLEN> -unsigned int HsiehHash(const void* pdata) -{ - const unsigned short* data=(const unsigned short*)pdata; - unsigned hash=DWORDLEN<<2,tmp; - for(int i=0;i<DWORDLEN;++i) - { - hash += data[0]; - tmp = (data[1]<<11)^hash; - hash = (hash<<16)^tmp; - data += 2; - hash += hash>>11; - } - hash^=hash<<3;hash+=hash>>5; - hash^=hash<<4;hash+=hash>>17; - hash^=hash<<25;hash+=hash>>6; - return(hash); -} - -template <const int CELLSIZE> -struct btSparseSdf -{ - // - // Inner types - // - struct IntFrac - { - int b; - int i; - btScalar f; - }; - struct Cell - { - btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1]; - int c[3]; - int puid; - unsigned hash; - const btCollisionShape* pclient; - Cell* next; - }; - // - // Fields - // - - btAlignedObjectArray<Cell*> cells; - btScalar voxelsz; - int puid; - int ncells; - int m_clampCells; - int nprobes; - int nqueries; - - // - // Methods - // - - // - void Initialize(int hashsize=2383, int clampCells = 256*1024) - { - //avoid a crash due to running out of memory, so clamp the maximum number of cells allocated - //if this limit is reached, the SDF is reset (at the cost of some performance during the reset) - m_clampCells = clampCells; - cells.resize(hashsize,0); - Reset(); - } - // - void Reset() - { - for(int i=0,ni=cells.size();i<ni;++i) - { - Cell* pc=cells[i]; - cells[i]=0; - while(pc) - { - Cell* pn=pc->next; - delete pc; - pc=pn; - } - } - voxelsz =0.25; - puid =0; - ncells =0; - nprobes =1; - nqueries =1; - } - // - void GarbageCollect(int lifetime=256) - { - const int life=puid-lifetime; - for(int i=0;i<cells.size();++i) - { - Cell*& root=cells[i]; - Cell* pp=0; - Cell* pc=root; - while(pc) - { - Cell* pn=pc->next; - if(pc->puid<life) - { - if(pp) pp->next=pn; else root=pn; - delete pc;pc=pp;--ncells; - } - pp=pc;pc=pn; - } - } - //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries); - nqueries=1; - nprobes=1; - ++puid; ///@todo: Reset puid's when int range limit is reached */ - /* else setup a priority list... */ - } - // - int RemoveReferences(btCollisionShape* pcs) - { - int refcount=0; - for(int i=0;i<cells.size();++i) - { - Cell*& root=cells[i]; - Cell* pp=0; - Cell* pc=root; - while(pc) - { - Cell* pn=pc->next; - if(pc->pclient==pcs) - { - if(pp) pp->next=pn; else root=pn; - delete pc;pc=pp;++refcount; - } - pp=pc;pc=pn; - } - } - return(refcount); - } - // - btScalar Evaluate( const btVector3& x, - const btCollisionShape* shape, - btVector3& normal, - btScalar margin) - { - /* Lookup cell */ - const btVector3 scx=x/voxelsz; - const IntFrac ix=Decompose(scx.x()); - const IntFrac iy=Decompose(scx.y()); - const IntFrac iz=Decompose(scx.z()); - const unsigned h=Hash(ix.b,iy.b,iz.b,shape); - Cell*& root=cells[static_cast<int>(h%cells.size())]; - Cell* c=root; - ++nqueries; - while(c) - { - ++nprobes; - if( (c->hash==h) && - (c->c[0]==ix.b) && - (c->c[1]==iy.b) && - (c->c[2]==iz.b) && - (c->pclient==shape)) - { break; } - else - { c=c->next; } - } - if(!c) - { - ++nprobes; - ++ncells; - //int sz = sizeof(Cell); - if (ncells>m_clampCells) - { - static int numResets=0; - numResets++; -// printf("numResets=%d\n",numResets); - Reset(); - } - - c=new Cell(); - c->next=root;root=c; - c->pclient=shape; - c->hash=h; - c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b; - BuildCell(*c); - } - c->puid=puid; - /* Extract infos */ - const int o[]={ ix.i,iy.i,iz.i}; - const btScalar d[]={ c->d[o[0]+0][o[1]+0][o[2]+0], - c->d[o[0]+1][o[1]+0][o[2]+0], - c->d[o[0]+1][o[1]+1][o[2]+0], - c->d[o[0]+0][o[1]+1][o[2]+0], - c->d[o[0]+0][o[1]+0][o[2]+1], - c->d[o[0]+1][o[1]+0][o[2]+1], - c->d[o[0]+1][o[1]+1][o[2]+1], - c->d[o[0]+0][o[1]+1][o[2]+1]}; - /* Normal */ -#if 1 - const btScalar gx[]={ d[1]-d[0],d[2]-d[3], - d[5]-d[4],d[6]-d[7]}; - const btScalar gy[]={ d[3]-d[0],d[2]-d[1], - d[7]-d[4],d[6]-d[5]}; - const btScalar gz[]={ d[4]-d[0],d[5]-d[1], - d[7]-d[3],d[6]-d[2]}; - normal.setX(Lerp( Lerp(gx[0],gx[1],iy.f), - Lerp(gx[2],gx[3],iy.f),iz.f)); - normal.setY(Lerp( Lerp(gy[0],gy[1],ix.f), - Lerp(gy[2],gy[3],ix.f),iz.f)); - normal.setZ(Lerp( Lerp(gz[0],gz[1],ix.f), - Lerp(gz[2],gz[3],ix.f),iy.f)); - normal = normal.normalized(); -#else - normal = btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized(); -#endif - /* Distance */ - const btScalar d0=Lerp(Lerp(d[0],d[1],ix.f), - Lerp(d[3],d[2],ix.f),iy.f); - const btScalar d1=Lerp(Lerp(d[4],d[5],ix.f), - Lerp(d[7],d[6],ix.f),iy.f); - return(Lerp(d0,d1,iz.f)-margin); - } - // - void BuildCell(Cell& c) - { - const btVector3 org=btVector3( (btScalar)c.c[0], - (btScalar)c.c[1], - (btScalar)c.c[2]) * - CELLSIZE*voxelsz; - for(int k=0;k<=CELLSIZE;++k) - { - const btScalar z=voxelsz*k+org.z(); - for(int j=0;j<=CELLSIZE;++j) - { - const btScalar y=voxelsz*j+org.y(); - for(int i=0;i<=CELLSIZE;++i) - { - const btScalar x=voxelsz*i+org.x(); - c.d[i][j][k]=DistanceToShape( btVector3(x,y,z), - c.pclient); - } - } - } - } - // - static inline btScalar DistanceToShape(const btVector3& x, - const btCollisionShape* shape) - { - btTransform unit; - unit.setIdentity(); - if(shape->isConvex()) - { - btGjkEpaSolver2::sResults res; - const btConvexShape* csh=static_cast<const btConvexShape*>(shape); - return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res)); - } - return(0); - } - // - static inline IntFrac Decompose(btScalar x) - { - /* That one need a lot of improvements... */ - /* Remove test, faster floor... */ - IntFrac r; - x/=CELLSIZE; - const int o=x<0?(int)(-x+1):0; - x+=o;r.b=(int)x; - const btScalar k=(x-r.b)*CELLSIZE; - r.i=(int)k;r.f=k-r.i;r.b-=o; - return(r); - } - // - static inline btScalar Lerp(btScalar a,btScalar b,btScalar t) - { - return(a+(b-a)*t); - } - - - - // - static inline unsigned int Hash(int x,int y,int z,const btCollisionShape* shape) - { - struct btS - { - int x,y,z; - void* p; - }; - - btS myset; - - myset.x=x;myset.y=y;myset.z=z;myset.p=(void*)shape; - const void* ptr = &myset; - - unsigned int result = HsiehHash<sizeof(btS)/4> (ptr); - - - return result; - } -}; - - -#endif //BT_SPARSE_SDF_H diff --git a/thirdparty/bullet/src/BulletSoftBody/premake4.lua b/thirdparty/bullet/src/BulletSoftBody/premake4.lua deleted file mode 100644 index ce384de2c8..0000000000 --- a/thirdparty/bullet/src/BulletSoftBody/premake4.lua +++ /dev/null @@ -1,11 +0,0 @@ - project "BulletSoftBody" - - kind "StaticLib" - - includedirs { - "..", - } - files { - "**.cpp", - "**.h" - }
\ No newline at end of file |