diff options
Diffstat (limited to 'extern/bullet2/src/BulletSoftBody')
50 files changed, 15658 insertions, 4729 deletions
diff --git a/extern/bullet2/src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h b/extern/bullet2/src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h new file mode 100644 index 00000000000..01c7e93a1b6 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h @@ -0,0 +1,45 @@ +// +// DeformableBodyInplaceSolverIslandCallback.h +// BulletSoftBody +// +// Created by Xuchen Han on 12/16/19. +// + +#ifndef DeformableBodyInplaceSolverIslandCallback_h +#define DeformableBodyInplaceSolverIslandCallback_h + +struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolverIslandCallback +{ + btDeformableMultiBodyConstraintSolver* m_deformableSolver; + + DeformableBodyInplaceSolverIslandCallback(btDeformableMultiBodyConstraintSolver* solver, + btDispatcher* dispatcher) + : MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver) + { + } + + virtual void processConstraints(int islandId = -1) + { + btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0; + btCollisionObject** softBodies = m_softBodies.size() ? &m_softBodies[0] : 0; + btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0; + btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0; + btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; + + //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); + + m_deformableSolver->solveDeformableBodyGroup(bodies, m_bodies.size(), softBodies, m_softBodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher); + if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics & 1)) + { + m_deformableSolver->m_analyticsData.m_islandId = islandId; + m_islandAnalyticsData.push_back(m_solver->m_analyticsData); + } + m_bodies.resize(0); + m_softBodies.resize(0); + m_manifolds.resize(0); + m_constraints.resize(0); + m_multiBodyConstraints.resize(0); + } +}; + +#endif /* DeformableBodyInplaceSolverIslandCallback_h */ diff --git a/extern/bullet2/src/BulletSoftBody/btCGProjection.h b/extern/bullet2/src/BulletSoftBody/btCGProjection.h new file mode 100644 index 00000000000..e05970664c2 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btCGProjection.h @@ -0,0 +1,104 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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_CG_PROJECTION_H +#define BT_CG_PROJECTION_H + +#include "btSoftBody.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" + +struct DeformableContactConstraint +{ + const btSoftBody::Node* m_node; + btAlignedObjectArray<const btSoftBody::RContact*> m_contact; + btAlignedObjectArray<btVector3> m_total_normal_dv; + btAlignedObjectArray<btVector3> m_total_tangent_dv; + btAlignedObjectArray<bool> m_static; + btAlignedObjectArray<bool> m_can_be_dynamic; + + DeformableContactConstraint(const btSoftBody::RContact& rcontact) : m_node(rcontact.m_node) + { + append(rcontact); + } + + DeformableContactConstraint() : m_node(NULL) + { + m_contact.push_back(NULL); + } + + void append(const btSoftBody::RContact& rcontact) + { + m_contact.push_back(&rcontact); + m_total_normal_dv.push_back(btVector3(0, 0, 0)); + m_total_tangent_dv.push_back(btVector3(0, 0, 0)); + m_static.push_back(false); + m_can_be_dynamic.push_back(true); + } + + void replace(const btSoftBody::RContact& rcontact) + { + m_contact.clear(); + m_total_normal_dv.clear(); + m_total_tangent_dv.clear(); + m_static.clear(); + m_can_be_dynamic.clear(); + append(rcontact); + } + + ~DeformableContactConstraint() + { + } +}; + +class btCGProjection +{ +public: + typedef btAlignedObjectArray<btVector3> TVStack; + typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack; + typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack; + btAlignedObjectArray<btSoftBody*>& m_softBodies; + const btScalar& m_dt; + // map from node indices to node pointers + const btAlignedObjectArray<btSoftBody::Node*>* m_nodes; + + btCGProjection(btAlignedObjectArray<btSoftBody*>& softBodies, const btScalar& dt) + : m_softBodies(softBodies), m_dt(dt) + { + } + + virtual ~btCGProjection() + { + } + + // apply the constraints + virtual void project(TVStack& x) = 0; + + virtual void setConstraints() = 0; + + // update the constraints + virtual btScalar update() = 0; + + virtual void reinitialize(bool nodeUpdated) + { + } + + virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes) + { + m_nodes = nodes; + } +}; + +#endif /* btCGProjection_h */ diff --git a/extern/bullet2/src/BulletSoftBody/btConjugateGradient.h b/extern/bullet2/src/BulletSoftBody/btConjugateGradient.h new file mode 100644 index 00000000000..bcd5e6b519d --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btConjugateGradient.h @@ -0,0 +1,117 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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_CONJUGATE_GRADIENT_H +#define BT_CONJUGATE_GRADIENT_H +#include "btKrylovSolver.h" +template <class MatrixX> +class btConjugateGradient : public btKrylovSolver<MatrixX> +{ + typedef btAlignedObjectArray<btVector3> TVStack; + typedef btKrylovSolver<MatrixX> Base; + TVStack r, p, z, temp; + +public: + btConjugateGradient(const int max_it_in) + : btKrylovSolver<MatrixX>(max_it_in, SIMD_EPSILON) + { + } + + virtual ~btConjugateGradient() {} + + // return the number of iterations taken + int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) + { + BT_PROFILE("CGSolve"); + btAssert(x.size() == b.size()); + reinitialize(b); + temp = b; + A.project(temp); + p = temp; + A.precondition(p, z); + btScalar d0 = this->dot(z, temp); + d0 = btMin(btScalar(1), d0); + // r = b - A * x --with assigned dof zeroed out + A.multiply(x, temp); + r = this->sub(b, temp); + A.project(r); + // z = M^(-1) * r + A.precondition(r, z); + A.project(z); + btScalar r_dot_z = this->dot(z, r); + if (r_dot_z <= Base::m_tolerance * d0) + { + if (verbose) + { + std::cout << "Iteration = 0" << std::endl; + std::cout << "Two norm of the residual = " << r_dot_z << std::endl; + } + return 0; + } + p = z; + btScalar r_dot_z_new = r_dot_z; + for (int k = 1; k <= Base::m_maxIterations; k++) + { + // temp = A*p + A.multiply(p, temp); + A.project(temp); + if (this->dot(p, temp) < 0) + { + if (verbose) + std::cout << "Encountered negative direction in CG!" << std::endl; + if (k == 1) + { + x = b; + } + return k; + } + // alpha = r^T * z / (p^T * A * p) + btScalar alpha = r_dot_z_new / this->dot(p, temp); + // x += alpha * p; + this->multAndAddTo(alpha, p, x); + // r -= alpha * temp; + this->multAndAddTo(-alpha, temp, r); + // z = M^(-1) * r + A.precondition(r, z); + r_dot_z = r_dot_z_new; + r_dot_z_new = this->dot(r, z); + if (r_dot_z_new < Base::m_tolerance * d0) + { + if (verbose) + { + std::cout << "ConjugateGradient iterations " << k << " residual = " << r_dot_z_new << std::endl; + } + return k; + } + + btScalar beta = r_dot_z_new / r_dot_z; + p = this->multAndAdd(beta, p, z); + } + if (verbose) + { + std::cout << "ConjugateGradient max iterations reached " << Base::m_maxIterations << " error = " << r_dot_z_new << std::endl; + } + return Base::m_maxIterations; + } + + void reinitialize(const TVStack& b) + { + r.resize(b.size()); + p.resize(b.size()); + z.resize(b.size()); + temp.resize(b.size()); + } +}; +#endif /* btConjugateGradient_h */ diff --git a/extern/bullet2/src/BulletSoftBody/btConjugateResidual.h b/extern/bullet2/src/BulletSoftBody/btConjugateResidual.h new file mode 100644 index 00000000000..61461203653 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btConjugateResidual.h @@ -0,0 +1,112 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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_CONJUGATE_RESIDUAL_H +#define BT_CONJUGATE_RESIDUAL_H +#include "btKrylovSolver.h" + +template <class MatrixX> +class btConjugateResidual : public btKrylovSolver<MatrixX> +{ + typedef btAlignedObjectArray<btVector3> TVStack; + typedef btKrylovSolver<MatrixX> Base; + TVStack r, p, z, temp_p, temp_r, best_x; + // temp_r = A*r + // temp_p = A*p + // z = M^(-1) * temp_p = M^(-1) * A * p + btScalar best_r; + +public: + btConjugateResidual(const int max_it_in) + : Base(max_it_in, 1e-8) + { + } + + virtual ~btConjugateResidual() {} + + // return the number of iterations taken + int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) + { + BT_PROFILE("CRSolve"); + btAssert(x.size() == b.size()); + reinitialize(b); + // r = b - A * x --with assigned dof zeroed out + A.multiply(x, temp_r); // borrow temp_r here to store A*x + r = this->sub(b, temp_r); + // z = M^(-1) * r + A.precondition(r, z); // borrow z to store preconditioned r + r = z; + btScalar residual_norm = this->norm(r); + if (residual_norm <= Base::m_tolerance) + { + return 0; + } + p = r; + btScalar r_dot_Ar, r_dot_Ar_new; + // temp_p = A*p + A.multiply(p, temp_p); + // temp_r = A*r + temp_r = temp_p; + r_dot_Ar = this->dot(r, temp_r); + for (int k = 1; k <= Base::m_maxIterations; k++) + { + // z = M^(-1) * Ap + A.precondition(temp_p, z); + // alpha = r^T * A * r / (Ap)^T * M^-1 * Ap) + btScalar alpha = r_dot_Ar / this->dot(temp_p, z); + // x += alpha * p; + this->multAndAddTo(alpha, p, x); + // r -= alpha * z; + this->multAndAddTo(-alpha, z, r); + btScalar norm_r = this->norm(r); + if (norm_r < best_r) + { + best_x = x; + best_r = norm_r; + if (norm_r < Base::m_tolerance) + { + return k; + } + } + // temp_r = A * r; + A.multiply(r, temp_r); + r_dot_Ar_new = this->dot(r, temp_r); + btScalar beta = r_dot_Ar_new / r_dot_Ar; + r_dot_Ar = r_dot_Ar_new; + // p = beta*p + r; + p = this->multAndAdd(beta, p, r); + // temp_p = beta*temp_p + temp_r; + temp_p = this->multAndAdd(beta, temp_p, temp_r); + } + if (verbose) + { + std::cout << "ConjugateResidual max iterations reached, residual = " << best_r << std::endl; + } + x = best_x; + return Base::m_maxIterations; + } + + void reinitialize(const TVStack& b) + { + r.resize(b.size()); + p.resize(b.size()); + z.resize(b.size()); + temp_p.resize(b.size()); + temp_r.resize(b.size()); + best_x.resize(b.size()); + best_r = SIMD_INFINITY; + } +}; +#endif /* btConjugateResidual_h */ diff --git a/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.cpp b/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.cpp index e90d24e6edf..5a79ef86e28 100644 --- a/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.cpp +++ b/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.cpp @@ -21,11 +21,10 @@ subject to the following restrictions: #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 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; } @@ -37,115 +36,111 @@ 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) +void btDefaultSoftBodySolver::optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate) { - m_softBodySet.copyFromArray( softBodies ); + m_softBodySet.copyFromArray(softBodies); } -void btDefaultSoftBodySolver::updateSoftBodies( ) +void btDefaultSoftBodySolver::updateSoftBodies() { - for ( int i=0; i < m_softBodySet.size(); i++) + for (int i = 0; i < m_softBodySet.size(); i++) { - btSoftBody* psb=(btSoftBody*)m_softBodySet[i]; + btSoftBody *psb = (btSoftBody *)m_softBodySet[i]; if (psb->isActive()) { - psb->integrateMotion(); + psb->integrateMotion(); } } -} // updateSoftBodies +} // updateSoftBodies bool btDefaultSoftBodySolver::checkInitialized() { return true; } -void btDefaultSoftBodySolver::solveConstraints( float solverdt ) +void btDefaultSoftBodySolver::solveConstraints(btScalar solverdt) { // Solve constraints for non-solver softbodies - for(int i=0; i < m_softBodySet.size(); ++i) + for (int i = 0; i < m_softBodySet.size(); ++i) { - btSoftBody* psb = static_cast<btSoftBody*>(m_softBodySet[i]); + btSoftBody *psb = static_cast<btSoftBody *>(m_softBodySet[i]); if (psb->isActive()) { psb->solveConstraints(); } - } -} // btDefaultSoftBodySolver::solveConstraints - + } +} // btDefaultSoftBodySolver::solveConstraints -void btDefaultSoftBodySolver::copySoftBodyToVertexBuffer( const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer ) +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 ) + if (vertexBuffer->getBufferType() == btVertexBufferDescriptor::CPU_BUFFER) { - const btAlignedObjectArray<btSoftBody::Node> &clothVertices( softBody->m_nodes ); + const btAlignedObjectArray<btSoftBody::Node> &clothVertices(softBody->m_nodes); int numVertices = clothVertices.size(); - const btCPUVertexBufferDescriptor *cpuVertexBuffer = static_cast< btCPUVertexBufferDescriptor* >(vertexBuffer); - float *basePointer = cpuVertexBuffer->getBasePointer(); + const btCPUVertexBufferDescriptor *cpuVertexBuffer = static_cast<btCPUVertexBufferDescriptor *>(vertexBuffer); + float *basePointer = cpuVertexBuffer->getBasePointer(); - if( vertexBuffer->hasVertexPositions() ) + if (vertexBuffer->hasVertexPositions()) { const int vertexOffset = cpuVertexBuffer->getVertexOffset(); const int vertexStride = cpuVertexBuffer->getVertexStride(); float *vertexPointer = basePointer + vertexOffset; - for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex ) + for (int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex) { btVector3 position = clothVertices[vertexIndex].m_x; - *(vertexPointer + 0) = position.getX(); - *(vertexPointer + 1) = position.getY(); - *(vertexPointer + 2) = position.getZ(); + *(vertexPointer + 0) = (float)position.getX(); + *(vertexPointer + 1) = (float)position.getY(); + *(vertexPointer + 2) = (float)position.getZ(); vertexPointer += vertexStride; } } - if( vertexBuffer->hasNormals() ) + if (vertexBuffer->hasNormals()) { const int normalOffset = cpuVertexBuffer->getNormalOffset(); const int normalStride = cpuVertexBuffer->getNormalStride(); float *normalPointer = basePointer + normalOffset; - for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex ) + for (int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex) { btVector3 normal = clothVertices[vertexIndex].m_n; - *(normalPointer + 0) = normal.getX(); - *(normalPointer + 1) = normal.getY(); - *(normalPointer + 2) = normal.getZ(); + *(normalPointer + 0) = (float)normal.getX(); + *(normalPointer + 1) = (float)normal.getY(); + *(normalPointer + 2) = (float)normal.getZ(); normalPointer += normalStride; } } } -} // btDefaultSoftBodySolver::copySoftBodyToVertexBuffer +} // btDefaultSoftBodySolver::copySoftBodyToVertexBuffer -void btDefaultSoftBodySolver::processCollision( btSoftBody* softBody, btSoftBody* otherSoftBody) +void btDefaultSoftBodySolver::processCollision(btSoftBody *softBody, btSoftBody *otherSoftBody) { - softBody->defaultCollisionHandler( 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 ) +void btDefaultSoftBodySolver::processCollision(btSoftBody *softBody, const btCollisionObjectWrapper *collisionObjectWrap) { - softBody->defaultCollisionHandler( collisionObjectWrap ); -} // btDefaultSoftBodySolver::processCollision - + softBody->defaultCollisionHandler(collisionObjectWrap); +} // btDefaultSoftBodySolver::processCollision -void btDefaultSoftBodySolver::predictMotion( float timeStep ) +void btDefaultSoftBodySolver::predictMotion(btScalar timeStep) { - for ( int i=0; i < m_softBodySet.size(); ++i) + for (int i = 0; i < m_softBodySet.size(); ++i) { - btSoftBody* psb = m_softBodySet[i]; + btSoftBody *psb = m_softBodySet[i]; if (psb->isActive()) { - psb->predictMotion(timeStep); + psb->predictMotion(timeStep); } } } - diff --git a/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.h b/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.h index 1c17ffcbb2b..3965b07c58e 100644 --- a/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.h +++ b/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.h @@ -16,25 +16,23 @@ subject to the following restrictions: #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: +protected: /** Variable to define whether we need to update solver constants on the next iteration */ bool m_updateSolverConstants; - btAlignedObjectArray< btSoftBody * > m_softBodySet; - + btAlignedObjectArray<btSoftBody *> m_softBodySet; public: btDefaultSoftBodySolver(); - + virtual ~btDefaultSoftBodySolver(); - + virtual SolverTypes getSolverType() const { return DEFAULT_SOLVER; @@ -42,22 +40,21 @@ public: virtual bool checkInitialized(); - virtual void updateSoftBodies( ); + virtual void updateSoftBodies(); - virtual void optimize( btAlignedObjectArray< btSoftBody * > &softBodies,bool forceUpdate=false ); + 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 solveConstraints(btScalar solverdt); - virtual void copySoftBodyToVertexBuffer( const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer ); + virtual void predictMotion(btScalar solverdt); - virtual void processCollision( btSoftBody *, const btCollisionObjectWrapper* ); + virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer); - virtual void processCollision( btSoftBody*, btSoftBody* ); + virtual void processCollision(btSoftBody *, const btCollisionObjectWrapper *); + virtual void processCollision(btSoftBody *, btSoftBody *); }; -#endif // #ifndef BT_ACCELERATED_SOFT_BODY_CPU_SOLVER_H +#endif // #ifndef BT_ACCELERATED_SOFT_BODY_CPU_SOLVER_H diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/extern/bullet2/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp new file mode 100644 index 00000000000..0c3e0b5eb82 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -0,0 +1,296 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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 "btDeformableBackwardEulerObjective.h" +#include "btPreconditioner.h" +#include "LinearMath/btQuickprof.h" + +btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v) + : m_softBodies(softBodies), m_projection(softBodies), m_backupVelocity(backup_v), m_implicit(false) +{ + m_massPreconditioner = new MassPreconditioner(m_softBodies); + m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit); + m_preconditioner = m_KKTPreconditioner; +} + +btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective() +{ + delete m_KKTPreconditioner; + delete m_massPreconditioner; +} + +void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar dt) +{ + BT_PROFILE("reinitialize"); + if (dt > 0) + { + setDt(dt); + } + if (nodeUpdated) + { + updateId(); + } + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->reinitialize(nodeUpdated); + } + btMatrix3x3 I; + I.setIdentity(); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im > 0) + psb->m_nodes[j].m_effectiveMass = I * (1.0 / psb->m_nodes[j].m_im); + } + } + m_projection.reinitialize(nodeUpdated); + // m_preconditioner->reinitialize(nodeUpdated); +} + +void btDeformableBackwardEulerObjective::setDt(btScalar dt) +{ + m_dt = dt; +} + +void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const +{ + BT_PROFILE("multiply"); + // add in the mass term + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + b[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : x[counter] / node.m_im; + ++counter; + } + } + + for (int i = 0; i < m_lf.size(); ++i) + { + // add damping matrix + m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b); + // Always integrate picking force implicitly for stability. + if (m_implicit || m_lf[i]->getForceType() == BT_MOUSE_PICKING_FORCE) + { + m_lf[i]->addScaledElasticForceDifferential(-m_dt * m_dt, x, b); + } + } + int offset = m_nodes.size(); + for (int i = offset; i < b.size(); ++i) + { + b[i].setZero(); + } + // add in the lagrange multiplier terms + + for (int c = 0; c < m_projection.m_lagrangeMultipliers.size(); ++c) + { + // C^T * lambda + const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[c]; + for (int i = 0; i < lm.m_num_nodes; ++i) + { + for (int j = 0; j < lm.m_num_constraints; ++j) + { + b[lm.m_indices[i]] += x[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j]; + } + } + // C * x + for (int d = 0; d < lm.m_num_constraints; ++d) + { + for (int i = 0; i < lm.m_num_nodes; ++i) + { + b[offset + c][d] += lm.m_weights[i] * x[lm.m_indices[i]].dot(lm.m_dirs[d]); + } + } + } +} + +void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv) +{ + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + node.m_v = m_backupVelocity[node.index] + dv[node.index]; + } + } +} + +void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero) +{ + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + counter += psb->m_nodes.size(); + continue; + } + if (m_implicit) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im != 0) + { + psb->m_nodes[j].m_v += psb->m_nodes[j].m_effectiveMass_inv * force[counter++]; + } + } + } + else + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im; + psb->m_nodes[j].m_v += one_over_mass * force[counter++]; + } + } + } + if (setZero) + { + for (int i = 0; i < force.size(); ++i) + force[i].setZero(); + } +} + +void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack& residual) +{ + BT_PROFILE("computeResidual"); + // add implicit force + for (int i = 0; i < m_lf.size(); ++i) + { + // Always integrate picking force implicitly for stability. + if (m_implicit || m_lf[i]->getForceType() == BT_MOUSE_PICKING_FORCE) + { + m_lf[i]->addScaledForces(dt, residual); + } + else + { + m_lf[i]->addScaledDampingForce(dt, residual); + } + } + // m_projection.project(residual); +} + +btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const +{ + btScalar mag = 0; + for (int i = 0; i < residual.size(); ++i) + { + mag += residual[i].length2(); + } + return std::sqrt(mag); +} + +btScalar btDeformableBackwardEulerObjective::totalEnergy(btScalar dt) +{ + btScalar e = 0; + for (int i = 0; i < m_lf.size(); ++i) + { + e += m_lf[i]->totalEnergy(dt); + } + return e; +} + +void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force) +{ + for (int i = 0; i < m_softBodies.size(); ++i) + { + m_softBodies[i]->advanceDeformation(); + } + if (m_implicit) + { + // apply forces except gravity force + btVector3 gravity; + for (int i = 0; i < m_lf.size(); ++i) + { + if (m_lf[i]->getForceType() == BT_GRAVITY_FORCE) + { + gravity = static_cast<btDeformableGravityForce*>(m_lf[i])->m_gravity; + } + else + { + m_lf[i]->addScaledForces(m_dt, force); + } + } + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->addScaledHessian(m_dt); + } + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + // add gravity explicitly + psb->m_nodes[j].m_v += m_dt * psb->m_gravityFactor * gravity; + } + } + } + } + else + { + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->addScaledExplicitForce(m_dt, force); + } + } + // calculate inverse mass matrix for all nodes + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_effectiveMass_inv = psb->m_nodes[j].m_effectiveMass.inverse(); + } + } + } + applyForce(force, true); +} + +void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual) +{ + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + dv[counter] = psb->m_nodes[j].m_im * residual[counter]; + ++counter; + } + } +} + +//set constraints as projections +void btDeformableBackwardEulerObjective::setConstraints(const btContactSolverInfo& infoGlobal) +{ + m_projection.setConstraints(infoGlobal); +} + +void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r) +{ + m_projection.applyDynamicFriction(r); +} diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/extern/bullet2/src/BulletSoftBody/btDeformableBackwardEulerObjective.h new file mode 100644 index 00000000000..eb05b9f010c --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -0,0 +1,198 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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_BACKWARD_EULER_OBJECTIVE_H +#define BT_BACKWARD_EULER_OBJECTIVE_H +//#include "btConjugateGradient.h" +#include "btDeformableLagrangianForce.h" +#include "btDeformableMassSpringForce.h" +#include "btDeformableGravityForce.h" +#include "btDeformableCorotatedForce.h" +#include "btDeformableMousePickingForce.h" +#include "btDeformableLinearElasticityForce.h" +#include "btDeformableNeoHookeanForce.h" +#include "btDeformableContactProjection.h" +#include "btPreconditioner.h" +#include "btDeformableMultiBodyDynamicsWorld.h" +#include "LinearMath/btQuickprof.h" + +class btDeformableBackwardEulerObjective +{ +public: + typedef btAlignedObjectArray<btVector3> TVStack; + btScalar m_dt; + btAlignedObjectArray<btDeformableLagrangianForce*> m_lf; + btAlignedObjectArray<btSoftBody*>& m_softBodies; + Preconditioner* m_preconditioner; + btDeformableContactProjection m_projection; + const TVStack& m_backupVelocity; + btAlignedObjectArray<btSoftBody::Node*> m_nodes; + bool m_implicit; + MassPreconditioner* m_massPreconditioner; + KKTPreconditioner* m_KKTPreconditioner; + + btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v); + + virtual ~btDeformableBackwardEulerObjective(); + + void initialize() {} + + // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual + void computeResidual(btScalar dt, TVStack& residual); + + // add explicit force to the velocity + void applyExplicitForce(TVStack& force); + + // apply force to velocity and optionally reset the force to zero + void applyForce(TVStack& force, bool setZero); + + // compute the norm of the residual + btScalar computeNorm(const TVStack& residual) const; + + // compute one step of the solve (there is only one solve if the system is linear) + void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt); + + // perform A*x = b + void multiply(const TVStack& x, TVStack& b) const; + + // set initial guess for CG solve + void initialGuess(TVStack& dv, const TVStack& residual); + + // reset data structure and reset dt + void reinitialize(bool nodeUpdated, btScalar dt); + + void setDt(btScalar dt); + + // add friction force to residual + void applyDynamicFriction(TVStack& r); + + // add dv to velocity + void updateVelocity(const TVStack& dv); + + //set constraints as projections + void setConstraints(const btContactSolverInfo& infoGlobal); + + // update the projections and project the residual + void project(TVStack& r) + { + BT_PROFILE("project"); + m_projection.project(r); + } + + // perform precondition M^(-1) x = b + void precondition(const TVStack& x, TVStack& b) + { + m_preconditioner->operator()(x, b); + } + + // reindex all the vertices + virtual void updateId() + { + size_t node_id = 0; + size_t face_id = 0; + m_nodes.clear(); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].index = node_id; + m_nodes.push_back(&psb->m_nodes[j]); + ++node_id; + } + for (int j = 0; j < psb->m_faces.size(); ++j) + { + psb->m_faces[j].m_index = face_id; + ++face_id; + } + } + } + + const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const + { + return &m_nodes; + } + + void setImplicit(bool implicit) + { + m_implicit = implicit; + } + + // Calculate the total potential energy in the system + btScalar totalEnergy(btScalar dt); + + void addLagrangeMultiplier(const TVStack& vec, TVStack& extended_vec) + { + extended_vec.resize(vec.size() + m_projection.m_lagrangeMultipliers.size()); + for (int i = 0; i < vec.size(); ++i) + { + extended_vec[i] = vec[i]; + } + int offset = vec.size(); + for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i) + { + extended_vec[offset + i].setZero(); + } + } + + void addLagrangeMultiplierRHS(const TVStack& residual, const TVStack& m_dv, TVStack& extended_residual) + { + extended_residual.resize(residual.size() + m_projection.m_lagrangeMultipliers.size()); + for (int i = 0; i < residual.size(); ++i) + { + extended_residual[i] = residual[i]; + } + int offset = residual.size(); + for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i) + { + const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[i]; + extended_residual[offset + i].setZero(); + for (int d = 0; d < lm.m_num_constraints; ++d) + { + for (int n = 0; n < lm.m_num_nodes; ++n) + { + extended_residual[offset + i][d] += lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]); + } + } + } + } + + void calculateContactForce(const TVStack& dv, const TVStack& rhs, TVStack& f) + { + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + f[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : dv[counter] / node.m_im; + ++counter; + } + } + for (int i = 0; i < m_lf.size(); ++i) + { + // add damping matrix + m_lf[i]->addScaledDampingForceDifferential(-m_dt, dv, f); + } + counter = 0; + for (; counter < f.size(); ++counter) + { + f[counter] = rhs[counter] - f[counter]; + } + } +}; + +#endif /* btBackwardEulerObjective_h */ diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableBodySolver.cpp b/extern/bullet2/src/BulletSoftBody/btDeformableBodySolver.cpp new file mode 100644 index 00000000000..4b11fccecba --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -0,0 +1,506 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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 <stdio.h> +#include <limits> +#include "btDeformableBodySolver.h" +#include "btSoftBodyInternals.h" +#include "LinearMath/btQuickprof.h" +static const int kMaxConjugateGradientIterations = 300; +btDeformableBodySolver::btDeformableBodySolver() + : m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(1), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false) +{ + m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity); +} + +btDeformableBodySolver::~btDeformableBodySolver() +{ + delete m_objective; +} + +void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) +{ + BT_PROFILE("solveDeformableConstraints"); + if (!m_implicit) + { + m_objective->computeResidual(solverdt, m_residual); + m_objective->applyDynamicFriction(m_residual); + if (m_useProjection) + { + computeStep(m_dv, m_residual); + } + else + { + TVStack rhs, x; + m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs); + m_objective->addLagrangeMultiplier(m_dv, x); + m_objective->m_preconditioner->reinitialize(true); + computeStep(x, rhs); + for (int i = 0; i < m_dv.size(); ++i) + { + m_dv[i] = x[i]; + } + } + updateVelocity(); + } + else + { + for (int i = 0; i < m_maxNewtonIterations; ++i) + { + updateState(); + // add the inertia term in the residual + int counter = 0; + for (int k = 0; k < m_softBodies.size(); ++k) + { + btSoftBody* psb = m_softBodies[k]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im > 0) + { + m_residual[counter] = (-1. / psb->m_nodes[j].m_im) * m_dv[counter]; + } + ++counter; + } + } + + m_objective->computeResidual(solverdt, m_residual); + if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0) + { + break; + } + // todo xuchenhan@: this really only needs to be calculated once + m_objective->applyDynamicFriction(m_residual); + if (m_lineSearch) + { + btScalar inner_product = computeDescentStep(m_ddv, m_residual); + btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8 + btScalar scale = 2; + btScalar f0 = m_objective->totalEnergy(solverdt) + kineticEnergy(), f1, f2; + backupDv(); + do + { + scale *= beta; + if (scale < 1e-8) + { + return; + } + updateEnergy(scale); + f1 = m_objective->totalEnergy(solverdt) + kineticEnergy(); + f2 = f0 - alpha * scale * inner_product; + } while (!(f1 < f2 + SIMD_EPSILON)); // if anything here is nan then the search continues + revertDv(); + updateDv(scale); + } + else + { + computeStep(m_ddv, m_residual); + updateDv(); + } + for (int j = 0; j < m_numNodes; ++j) + { + m_ddv[j].setZero(); + m_residual[j].setZero(); + } + } + updateVelocity(); + } +} + +btScalar btDeformableBodySolver::kineticEnergy() +{ + btScalar ke = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + if (node.m_im > 0) + { + ke += m_dv[node.index].length2() * 0.5 / node.m_im; + } + } + } + return ke; +} + +void btDeformableBodySolver::backupDv() +{ + m_backup_dv.resize(m_dv.size()); + for (int i = 0; i < m_backup_dv.size(); ++i) + { + m_backup_dv[i] = m_dv[i]; + } +} + +void btDeformableBodySolver::revertDv() +{ + for (int i = 0; i < m_backup_dv.size(); ++i) + { + m_dv[i] = m_backup_dv[i]; + } +} + +void btDeformableBodySolver::updateEnergy(btScalar scale) +{ + for (int i = 0; i < m_dv.size(); ++i) + { + m_dv[i] = m_backup_dv[i] + scale * m_ddv[i]; + } + updateState(); +} + +btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose) +{ + m_cg.solve(*m_objective, ddv, residual, false); + btScalar inner_product = m_cg.dot(residual, m_ddv); + btScalar res_norm = m_objective->computeNorm(residual); + btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv); + if (inner_product < -tol) + { + if (verbose) + { + std::cout << "Looking backwards!" << std::endl; + } + for (int i = 0; i < m_ddv.size(); ++i) + { + m_ddv[i] = -m_ddv[i]; + } + inner_product = -inner_product; + } + else if (std::abs(inner_product) < tol) + { + if (verbose) + { + std::cout << "Gradient Descent!" << std::endl; + } + btScalar scale = m_objective->computeNorm(m_ddv) / res_norm; + for (int i = 0; i < m_ddv.size(); ++i) + { + m_ddv[i] = scale * residual[i]; + } + inner_product = scale * res_norm * res_norm; + } + return inner_product; +} + +void btDeformableBodySolver::updateState() +{ + updateVelocity(); + updateTempPosition(); +} + +void btDeformableBodySolver::updateDv(btScalar scale) +{ + for (int i = 0; i < m_numNodes; ++i) + { + m_dv[i] += scale * m_ddv[i]; + } +} + +void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual) +{ + if (m_useProjection) + m_cg.solve(*m_objective, ddv, residual, false); + else + m_cr.solve(*m_objective, ddv, residual, false); +} + +void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt) +{ + m_softBodies.copyFromArray(softBodies); + bool nodeUpdated = updateNodes(); + + if (nodeUpdated) + { + m_dv.resize(m_numNodes, btVector3(0, 0, 0)); + m_ddv.resize(m_numNodes, btVector3(0, 0, 0)); + m_residual.resize(m_numNodes, btVector3(0, 0, 0)); + m_backupVelocity.resize(m_numNodes, btVector3(0, 0, 0)); + } + + // need to setZero here as resize only set value for newly allocated items + for (int i = 0; i < m_numNodes; ++i) + { + m_dv[i].setZero(); + m_ddv[i].setZero(); + m_residual[i].setZero(); + } + + if (dt > 0) + { + m_dt = dt; + } + m_objective->reinitialize(nodeUpdated, dt); + updateSoftBodies(); +} + +void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("setConstraint"); + m_objective->setConstraints(infoGlobal); +} + +btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("solveContactConstraints"); + btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies, numDeformableBodies, infoGlobal); + return maxSquaredResidual; +} + +void btDeformableBodySolver::updateVelocity() +{ + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + psb->m_maxSpeedSquared = 0; + if (!psb->isActive()) + { + counter += psb->m_nodes.size(); + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + // set NaN to zero; + if (m_dv[counter] != m_dv[counter]) + { + m_dv[counter].setZero(); + } + if (m_implicit) + { + psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter]; + } + else + { + psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv; + } + psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2()); + ++counter; + } + } +} + +void btDeformableBodySolver::updateTempPosition() +{ + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + counter += psb->m_nodes.size(); + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * (psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv); + ++counter; + } + psb->updateDeformation(); + } +} + +void btDeformableBodySolver::backupVelocity() +{ + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + m_backupVelocity[counter++] = psb->m_nodes[j].m_v; + } + } +} + +void btDeformableBodySolver::setupDeformableSolve(bool implicit) +{ + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + counter += psb->m_nodes.size(); + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (implicit) + { + // setting the initial guess for newton, need m_dv = v_{n+1} - v_n for dofs that are in constraint. + if (psb->m_nodes[j].m_v == m_backupVelocity[counter]) + m_dv[counter].setZero(); + else + m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn; + m_backupVelocity[counter] = psb->m_nodes[j].m_vn; + } + else + { + m_dv[counter] = psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv - m_backupVelocity[counter]; + } + psb->m_nodes[j].m_v = m_backupVelocity[counter]; + ++counter; + } + } +} + +void btDeformableBodySolver::revertVelocity() +{ + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_v = m_backupVelocity[counter++]; + } + } +} + +bool btDeformableBodySolver::updateNodes() +{ + int numNodes = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + numNodes += m_softBodies[i]->m_nodes.size(); + if (numNodes != m_numNodes) + { + m_numNodes = numNodes; + return true; + } + return false; +} + +void btDeformableBodySolver::predictMotion(btScalar solverdt) +{ + // apply explicit forces to velocity + if (m_implicit) + { + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + psb->m_nodes[j].m_v * solverdt; + } + } + } + } + m_objective->applyExplicitForce(m_residual); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + + if (psb->isActive()) + { + // predict motion for collision detection + predictDeformableMotion(psb, solverdt); + } + } +} + +void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt) +{ + BT_PROFILE("btDeformableBodySolver::predictDeformableMotion"); + int i, ni; + + /* Update */ + if (psb->m_bUpdateRtCst) + { + psb->m_bUpdateRtCst = false; + psb->updateConstants(); + psb->m_fdbvt.clear(); + if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD) + { + psb->initializeFaceTree(); + } + } + + /* Prepare */ + psb->m_sst.sdt = dt * psb->m_cfg.timescale; + psb->m_sst.isdt = 1 / psb->m_sst.sdt; + psb->m_sst.velmrg = psb->m_sst.sdt * 3; + psb->m_sst.radmrg = psb->getCollisionShape()->getMargin(); + psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25; + /* Bounds */ + psb->updateBounds(); + + /* Integrate */ + // do not allow particles to move more than the bounding box size + btScalar max_v = (psb->m_bounds[1] - psb->m_bounds[0]).norm() / dt; + for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) + { + btSoftBody::Node& n = psb->m_nodes[i]; + // apply drag + n.m_v *= (1 - psb->m_cfg.drag); + // scale velocity back + if (m_implicit) + { + n.m_q = n.m_x; + } + else + { + if (n.m_v.norm() > max_v) + { + n.m_v.safeNormalize(); + n.m_v *= max_v; + } + n.m_q = n.m_x + n.m_v * dt; + } + n.m_splitv.setZero(); + n.m_constrained = false; + } + + /* Nodes */ + psb->updateNodeTree(true, true); + if (!psb->m_fdbvt.empty()) + { + psb->updateFaceTree(true, true); + } + /* Clear contacts */ + psb->m_nodeRigidContacts.resize(0); + psb->m_faceRigidContacts.resize(0); + psb->m_faceNodeContacts.resize(0); + /* Optimize dbvt's */ + // psb->m_ndbvt.optimizeIncremental(1); + // psb->m_fdbvt.optimizeIncremental(1); +} + +void btDeformableBodySolver::updateSoftBodies() +{ + BT_PROFILE("updateSoftBodies"); + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = (btSoftBody*)m_softBodies[i]; + if (psb->isActive()) + { + psb->updateNormals(); + } + } +} + +void btDeformableBodySolver::setImplicit(bool implicit) +{ + m_implicit = implicit; + m_objective->setImplicit(implicit); +} + +void btDeformableBodySolver::setLineSearch(bool lineSearch) +{ + m_lineSearch = lineSearch; +} diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableBodySolver.h b/extern/bullet2/src/BulletSoftBody/btDeformableBodySolver.h new file mode 100644 index 00000000000..ae674d6e892 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDeformableBodySolver.h @@ -0,0 +1,160 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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_DEFORMABLE_BODY_SOLVERS_H +#define BT_DEFORMABLE_BODY_SOLVERS_H + +#include "btSoftBodySolvers.h" +#include "btDeformableBackwardEulerObjective.h" +#include "btDeformableMultiBodyDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" +#include "btConjugateResidual.h" +#include "btConjugateGradient.h" +struct btCollisionObjectWrapper; +class btDeformableBackwardEulerObjective; +class btDeformableMultiBodyDynamicsWorld; + +class btDeformableBodySolver : public btSoftBodySolver +{ + typedef btAlignedObjectArray<btVector3> TVStack; + +protected: + int m_numNodes; // total number of deformable body nodes + TVStack m_dv; // v_{n+1} - v_n + TVStack m_backup_dv; // backed up dv + TVStack m_ddv; // incremental dv + TVStack m_residual; // rhs of the linear solve + btAlignedObjectArray<btSoftBody*> m_softBodies; // all deformable bodies + TVStack m_backupVelocity; // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit + btScalar m_dt; // dt + btConjugateGradient<btDeformableBackwardEulerObjective> m_cg; // CG solver + btConjugateResidual<btDeformableBackwardEulerObjective> m_cr; // CR solver + bool m_implicit; // use implicit scheme if true, explicit scheme if false + int m_maxNewtonIterations; // max number of newton iterations + btScalar m_newtonTolerance; // stop newton iterations if f(x) < m_newtonTolerance + bool m_lineSearch; // If true, use newton's method with line search under implicit scheme +public: + // handles data related to objective function + btDeformableBackwardEulerObjective* m_objective; + bool m_useProjection; + + btDeformableBodySolver(); + + virtual ~btDeformableBodySolver(); + + virtual SolverTypes getSolverType() const + { + return DEFORMABLE_SOLVER; + } + + // update soft body normals + virtual void updateSoftBodies(); + + virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal); + + // solve the momentum equation + virtual void solveDeformableConstraints(btScalar solverdt); + + // resize/clear data structures + void reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt); + + // set up contact constraints + void setConstraints(const btContactSolverInfo& infoGlobal); + + // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion + virtual void predictMotion(btScalar solverdt); + + // move to temporary position x_{n+1}^* = x_n + dt * v_{n+1}^* + // x_{n+1}^* is stored in m_q + void predictDeformableMotion(btSoftBody* psb, btScalar dt); + + // save the current velocity to m_backupVelocity + void backupVelocity(); + + // set m_dv and m_backupVelocity to desired value to prepare for momentum solve + void setupDeformableSolve(bool implicit); + + // set the current velocity to that backed up in m_backupVelocity + void revertVelocity(); + + // set velocity to m_dv + m_backupVelocity + void updateVelocity(); + + // update the node count + bool updateNodes(); + + // calculate the change in dv resulting from the momentum solve + void computeStep(TVStack& ddv, const TVStack& residual); + + // calculate the change in dv resulting from the momentum solve when line search is turned on + btScalar computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose = false); + + virtual void copySoftBodyToVertexBuffer(const btSoftBody* const softBody, btVertexBufferDescriptor* vertexBuffer) {} + + // process collision between deformable and rigid + virtual void processCollision(btSoftBody* softBody, const btCollisionObjectWrapper* collisionObjectWrap) + { + softBody->defaultCollisionHandler(collisionObjectWrap); + } + + // process collision between deformable and deformable + virtual void processCollision(btSoftBody* softBody, btSoftBody* otherSoftBody) + { + softBody->defaultCollisionHandler(otherSoftBody); + } + + // If true, implicit time stepping scheme is used. + // Otherwise, explicit time stepping scheme is used + void setImplicit(bool implicit); + + // If true, newton's method with line search is used when implicit time stepping scheme is turned on + void setLineSearch(bool lineSearch); + + // set temporary position x^* = x_n + dt * v + // update the deformation gradient at position x^* + void updateState(); + + // set dv = dv + scale * ddv + void updateDv(btScalar scale = 1); + + // set temporary position x^* = x_n + dt * v^* + void updateTempPosition(); + + // save the current dv to m_backup_dv; + void backupDv(); + + // set dv to the backed-up value + void revertDv(); + + // set dv = dv + scale * ddv + // set v^* = v_n + dv + // set temporary position x^* = x_n + dt * v^* + // update the deformation gradient at position x^* + void updateEnergy(btScalar scale); + + // calculates the appropriately scaled kinetic energy in the system, which is + // 1/2 * dv^T * M * dv + // used in line search + btScalar kineticEnergy(); + + // unused functions + virtual void optimize(btAlignedObjectArray<btSoftBody*>& softBodies, bool forceUpdate = false) {} + virtual void solveConstraints(btScalar dt) {} + virtual bool checkInitialized() { return true; } + virtual void copyBackToSoftBodies(bool bMove = true) {} +}; + +#endif /* btDeformableBodySolver_h */ diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableContactConstraint.cpp b/extern/bullet2/src/BulletSoftBody/btDeformableContactConstraint.cpp new file mode 100644 index 00000000000..09398d79a5c --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -0,0 +1,720 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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 "btDeformableContactConstraint.h" +/* ================ Deformable Node Anchor =================== */ +btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a, const btContactSolverInfo& infoGlobal) + : m_anchor(&a), btDeformableContactConstraint(a.m_cti.m_normal, infoGlobal) +{ +} + +btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other) + : m_anchor(other.m_anchor), btDeformableContactConstraint(other) +{ +} + +btVector3 btDeformableNodeAnchorConstraint::getVa() const +{ + const btSoftBody::sCti& cti = m_anchor->m_cti; + btVector3 va(0, 0, 0); + if (cti.m_colObj->hasContactResponse()) + { + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_anchor->m_c1)) : 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; + const btScalar* J_n = &m_anchor->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0]; + const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); + const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_n[k]; + } + va = cti.m_normal * vel; + // add in the tangential components of the va + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_t1[k]; + } + va += m_anchor->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_t2[k]; + } + va += m_anchor->t2 * vel; + } + } + } + return va; +} + +btScalar btDeformableNodeAnchorConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) +{ + const btSoftBody::sCti& cti = m_anchor->m_cti; + btVector3 va = getVa(); + btVector3 vb = getVb(); + btVector3 vr = (vb - va); + // + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0 + const btScalar dn = btDot(vr, vr); + // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt + btScalar residualSquare = dn * dn; + btVector3 impulse = m_anchor->m_c0 * vr; + // apply impulse to deformable nodes involved and change their velocities + applyImpulse(impulse); + + // apply impulse to the rigid/multibodies involved and change their velocities + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = 0; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (rigidCol) + { + rigidCol->applyImpulse(impulse, m_anchor->m_c1); + } + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = 0; + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const btScalar* deltaV_normal = &m_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // apply normal component of the impulse + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); + // apply tangential component of the impulse + const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1)); + const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2)); + } + } + return residualSquare; +} + +btVector3 btDeformableNodeAnchorConstraint::getVb() const +{ + return m_anchor->m_node->m_v; +} + +void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse) +{ + btVector3 dv = impulse * m_anchor->m_c2; + m_anchor->m_node->m_v -= dv; +} + +/* ================ Deformable vs. Rigid =================== */ +btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal) + : m_contact(&c), btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal) +{ + m_total_normal_dv.setZero(); + m_total_tangent_dv.setZero(); + // The magnitude of penetration is the depth of penetration. + m_penetration = c.m_cti.m_offset; + m_total_split_impulse = 0; + m_binding = false; +} + +btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other) + : m_contact(other.m_contact), btDeformableContactConstraint(other), m_penetration(other.m_penetration), m_total_split_impulse(other.m_total_split_impulse), m_binding(other.m_binding) +{ + m_total_normal_dv = other.m_total_normal_dv; + m_total_tangent_dv = other.m_total_tangent_dv; +} + +btVector3 btDeformableRigidContactConstraint::getVa() const +{ + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 va(0, 0, 0); + if (cti.m_colObj->hasContactResponse()) + { + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_contact->m_c1)) : 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; + const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0]; + const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); + const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_n[k]; + } + va = cti.m_normal * vel; + // add in the tangential components of the va + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_t1[k]; + } + va += m_contact->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_t2[k]; + } + va += m_contact->t2 * vel; + } + } + } + return va; +} + +btVector3 btDeformableRigidContactConstraint::getSplitVa() const +{ + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 va(0, 0, 0); + if (cti.m_colObj->hasContactResponse()) + { + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getPushVelocityInLocalPoint(m_contact->m_c1)) : 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; + const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0]; + const btScalar* local_split_v = multibodyLinkCol->m_multiBody->getSplitVelocityVector(); + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_split_v[k] * J_n[k]; + } + va = cti.m_normal * vel; + // add in the tangential components of the va + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_split_v[k] * J_t1[k]; + } + va += m_contact->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_split_v[k] * J_t2[k]; + } + va += m_contact->t2 * vel; + } + } + } + return va; +} + +btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) +{ + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 va = getVa(); + btVector3 vb = getVb(); + btVector3 vr = vb - va; + btScalar dn = btDot(vr, cti.m_normal) + m_total_normal_dv.dot(cti.m_normal) * infoGlobal.m_deformable_cfm; + if (m_penetration > 0) + { + dn += m_penetration / infoGlobal.m_timeStep; + } + if (!infoGlobal.m_splitImpulse) + { + dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; + } + // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt + btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0, 0, 0))); + if (!infoGlobal.m_splitImpulse) + { + impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal); + } + btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn); + btVector3 impulse_tangent = impulse - impulse_normal; + if (dn > 0) + { + return 0; + } + m_binding = true; + btScalar residualSquare = dn * dn; + btVector3 old_total_tangent_dv = m_total_tangent_dv; + // m_c5 is the inverse mass of the deformable node/face + m_total_normal_dv -= m_contact->m_c5 * impulse_normal; + m_total_tangent_dv -= m_contact->m_c5 * impulse_tangent; + + if (m_total_normal_dv.dot(cti.m_normal) < 0) + { + // separating in the normal direction + m_binding = false; + m_static = false; + impulse_tangent.setZero(); + } + else + { + if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm()) + { + // dynamic friction + // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. + m_static = false; + if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON) + { + m_total_tangent_dv = btVector3(0, 0, 0); + } + else + { + m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3; + } + // impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv); + impulse_tangent = m_contact->m_c5.inverse() * (old_total_tangent_dv - m_total_tangent_dv); + } + else + { + // static friction + m_static = true; + } + } + impulse = impulse_normal + impulse_tangent; + // apply impulse to deformable nodes involved and change their velocities + applyImpulse(impulse); + // apply impulse to the rigid/multibodies involved and change their velocities + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = 0; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (rigidCol) + { + rigidCol->applyImpulse(impulse, m_contact->m_c1); + } + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = 0; + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // apply normal component of the impulse + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); + if (impulse_tangent.norm() > SIMD_EPSILON) + { + // apply tangential component of the impulse + const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1)); + const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2)); + } + } + } + return residualSquare; +} + +btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal) +{ + btScalar MAX_PENETRATION_CORRECTION = infoGlobal.m_deformable_maxErrorReduction; + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 vb = getSplitVb(); + btVector3 va = getSplitVa(); + btScalar p = m_penetration; + if (p > 0) + { + return 0; + } + btVector3 vr = vb - va; + btScalar dn = btDot(vr, cti.m_normal) + p * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; + if (dn > 0) + { + return 0; + } + if (m_total_split_impulse + dn > MAX_PENETRATION_CORRECTION) + { + dn = MAX_PENETRATION_CORRECTION - m_total_split_impulse; + } + if (m_total_split_impulse + dn < -MAX_PENETRATION_CORRECTION) + { + dn = -MAX_PENETRATION_CORRECTION - m_total_split_impulse; + } + m_total_split_impulse += dn; + + btScalar residualSquare = dn * dn; + const btVector3 impulse = m_contact->m_c0 * (cti.m_normal * dn); + applySplitImpulse(impulse); + + // apply split impulse to the rigid/multibodies involved and change their velocities + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = 0; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (rigidCol) + { + rigidCol->applyPushImpulse(impulse, m_contact->m_c1); + } + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = 0; + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // apply normal component of the impulse + multibodyLinkCol->m_multiBody->applyDeltaSplitVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal)); + } + } + return residualSquare; +} +/* ================ Node vs. Rigid =================== */ +btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal) + : m_node(contact.m_node), btDeformableRigidContactConstraint(contact, infoGlobal) +{ +} + +btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other) + : m_node(other.m_node), btDeformableRigidContactConstraint(other) +{ +} + +btVector3 btDeformableNodeRigidContactConstraint::getVb() const +{ + return m_node->m_v; +} + +btVector3 btDeformableNodeRigidContactConstraint::getSplitVb() const +{ + return m_node->m_splitv; +} + +btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const +{ + return m_total_normal_dv + m_total_tangent_dv; +} + +void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableNodeRigidContact* contact = getContact(); + btVector3 dv = contact->m_c5 * impulse; + contact->m_node->m_v -= dv; +} + +void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableNodeRigidContact* contact = getContact(); + btVector3 dv = contact->m_c5 * impulse; + contact->m_node->m_splitv -= dv; +} + +/* ================ Face vs. Rigid =================== */ +btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting) + : m_face(contact.m_face), m_useStrainLimiting(useStrainLimiting), btDeformableRigidContactConstraint(contact, infoGlobal) +{ +} + +btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other) + : m_face(other.m_face), m_useStrainLimiting(other.m_useStrainLimiting), btDeformableRigidContactConstraint(other) +{ +} + +btVector3 btDeformableFaceRigidContactConstraint::getVb() const +{ + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2]; + return vb; +} + +btVector3 btDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const +{ + btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv; + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + if (m_face->m_n[0] == node) + { + return face_dv * contact->m_weights[0]; + } + if (m_face->m_n[1] == node) + { + return face_dv * contact->m_weights[1]; + } + btAssert(node == m_face->m_n[2]); + return face_dv * contact->m_weights[2]; +} + +void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 dv = impulse * contact->m_c2; + btSoftBody::Face* face = contact->m_face; + + btVector3& v0 = face->m_n[0]->m_v; + btVector3& v1 = face->m_n[1]->m_v; + btVector3& v2 = face->m_n[2]->m_v; + const btScalar& im0 = face->m_n[0]->m_im; + const btScalar& im1 = face->m_n[1]->m_im; + const btScalar& im2 = face->m_n[2]->m_im; + if (im0 > 0) + v0 -= dv * contact->m_weights[0]; + if (im1 > 0) + v1 -= dv * contact->m_weights[1]; + if (im2 > 0) + v2 -= dv * contact->m_weights[2]; + if (m_useStrainLimiting) + { + btScalar relaxation = 1. / btScalar(m_infoGlobal->m_numIterations); + btScalar m01 = (relaxation / (im0 + im1)); + btScalar m02 = (relaxation / (im0 + im2)); + btScalar m12 = (relaxation / (im1 + im2)); +#ifdef USE_STRAIN_RATE_LIMITING + // apply strain limiting to prevent the new velocity to change the current length of the edge by more than 1%. + btScalar p = 0.01; + btVector3& x0 = face->m_n[0]->m_x; + btVector3& x1 = face->m_n[1]->m_x; + btVector3& x2 = face->m_n[2]->m_x; + const btVector3 x_diff[3] = {x1 - x0, x2 - x0, x2 - x1}; + const btVector3 v_diff[3] = {v1 - v0, v2 - v0, v2 - v1}; + btVector3 u[3]; + btScalar x_diff_dot_u, dn[3]; + btScalar dt = m_infoGlobal->m_timeStep; + for (int i = 0; i < 3; ++i) + { + btScalar x_diff_norm = x_diff[i].safeNorm(); + btScalar x_diff_norm_new = (x_diff[i] + v_diff[i] * dt).safeNorm(); + btScalar strainRate = x_diff_norm_new / x_diff_norm; + u[i] = v_diff[i]; + u[i].safeNormalize(); + if (x_diff_norm == 0 || (1 - p <= strainRate && strainRate <= 1 + p)) + { + dn[i] = 0; + continue; + } + x_diff_dot_u = btDot(x_diff[i], u[i]); + btScalar s; + if (1 - p > strainRate) + { + s = 1 / dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u * x_diff_dot_u + (p * p - 2 * p) * x_diff_norm * x_diff_norm)); + } + else + { + s = 1 / dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u * x_diff_dot_u + (p * p + 2 * p) * x_diff_norm * x_diff_norm)); + } + // x_diff_norm_new = (x_diff[i] + s * u[i] * dt).safeNorm(); + // strainRate = x_diff_norm_new/x_diff_norm; + dn[i] = s - v_diff[i].safeNorm(); + } + btVector3 dv0 = im0 * (m01 * u[0] * (-dn[0]) + m02 * u[1] * -(dn[1])); + btVector3 dv1 = im1 * (m01 * u[0] * (dn[0]) + m12 * u[2] * (-dn[2])); + btVector3 dv2 = im2 * (m12 * u[2] * (dn[2]) + m02 * u[1] * (dn[1])); +#else + // apply strain limiting to prevent undamped modes + btVector3 dv0 = im0 * (m01 * (v1 - v0) + m02 * (v2 - v0)); + btVector3 dv1 = im1 * (m01 * (v0 - v1) + m12 * (v2 - v1)); + btVector3 dv2 = im2 * (m12 * (v1 - v2) + m02 * (v0 - v2)); +#endif + v0 += dv0; + v1 += dv1; + v2 += dv2; + } +} + +btVector3 btDeformableFaceRigidContactConstraint::getSplitVb() const +{ + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 vb = (m_face->m_n[0]->m_splitv) * contact->m_bary[0] + (m_face->m_n[1]->m_splitv) * contact->m_bary[1] + (m_face->m_n[2]->m_splitv) * contact->m_bary[2]; + return vb; +} + +void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 dv = impulse * contact->m_c2; + btSoftBody::Face* face = contact->m_face; + btVector3& v0 = face->m_n[0]->m_splitv; + btVector3& v1 = face->m_n[1]->m_splitv; + btVector3& v2 = face->m_n[2]->m_splitv; + const btScalar& im0 = face->m_n[0]->m_im; + const btScalar& im1 = face->m_n[1]->m_im; + const btScalar& im2 = face->m_n[2]->m_im; + if (im0 > 0) + { + v0 -= dv * contact->m_weights[0]; + } + if (im1 > 0) + { + v1 -= dv * contact->m_weights[1]; + } + if (im2 > 0) + { + v2 -= dv * contact->m_weights[2]; + } +} + +/* ================ Face vs. Node =================== */ +btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal) + : m_node(contact.m_node), m_face(contact.m_face), m_contact(&contact), btDeformableContactConstraint(contact.m_normal, infoGlobal) +{ + m_total_normal_dv.setZero(); + m_total_tangent_dv.setZero(); +} + +btVector3 btDeformableFaceNodeContactConstraint::getVa() const +{ + return m_node->m_v; +} + +btVector3 btDeformableFaceNodeContactConstraint::getVb() const +{ + const btSoftBody::DeformableFaceNodeContact* contact = getContact(); + btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2]; + return vb; +} + +btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n) const +{ + btVector3 dv = m_total_normal_dv + m_total_tangent_dv; + if (n == m_node) + return dv; + const btSoftBody::DeformableFaceNodeContact* contact = getContact(); + if (m_face->m_n[0] == n) + { + return dv * contact->m_weights[0]; + } + if (m_face->m_n[1] == n) + { + return dv * contact->m_weights[1]; + } + btAssert(n == m_face->m_n[2]); + return dv * contact->m_weights[2]; +} + +btScalar btDeformableFaceNodeContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) +{ + btVector3 va = getVa(); + btVector3 vb = getVb(); + btVector3 vr = vb - va; + const btScalar dn = btDot(vr, m_contact->m_normal); + // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt + btScalar residualSquare = dn * dn; + btVector3 impulse = m_contact->m_c0 * vr; + const btVector3 impulse_normal = m_contact->m_c0 * (m_contact->m_normal * dn); + btVector3 impulse_tangent = impulse - impulse_normal; + + btVector3 old_total_tangent_dv = m_total_tangent_dv; + // m_c2 is the inverse mass of the deformable node/face + if (m_node->m_im > 0) + { + m_total_normal_dv -= impulse_normal * m_node->m_im; + m_total_tangent_dv -= impulse_tangent * m_node->m_im; + } + else + { + m_total_normal_dv -= impulse_normal * m_contact->m_imf; + m_total_tangent_dv -= impulse_tangent * m_contact->m_imf; + } + + if (m_total_normal_dv.dot(m_contact->m_normal) > 0) + { + // separating in the normal direction + m_static = false; + m_total_tangent_dv = btVector3(0, 0, 0); + impulse_tangent.setZero(); + } + else + { + if (m_total_normal_dv.norm() * m_contact->m_friction < m_total_tangent_dv.norm()) + { + // dynamic friction + // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. + m_static = false; + if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON) + { + m_total_tangent_dv = btVector3(0, 0, 0); + } + else + { + m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_friction; + } + impulse_tangent = -btScalar(1) / m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv); + } + else + { + // static friction + m_static = true; + } + } + impulse = impulse_normal + impulse_tangent; + // apply impulse to deformable nodes involved and change their velocities + applyImpulse(impulse); + return residualSquare; +} + +void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableFaceNodeContact* contact = getContact(); + btVector3 dva = impulse * contact->m_node->m_im; + btVector3 dvb = impulse * contact->m_imf; + if (contact->m_node->m_im > 0) + { + contact->m_node->m_v += dva; + } + + btSoftBody::Face* face = contact->m_face; + btVector3& v0 = face->m_n[0]->m_v; + btVector3& v1 = face->m_n[1]->m_v; + btVector3& v2 = face->m_n[2]->m_v; + const btScalar& im0 = face->m_n[0]->m_im; + const btScalar& im1 = face->m_n[1]->m_im; + const btScalar& im2 = face->m_n[2]->m_im; + if (im0 > 0) + { + v0 -= dvb * contact->m_weights[0]; + } + if (im1 > 0) + { + v1 -= dvb * contact->m_weights[1]; + } + if (im2 > 0) + { + v2 -= dvb * contact->m_weights[2]; + } +} diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableContactConstraint.h b/extern/bullet2/src/BulletSoftBody/btDeformableContactConstraint.h new file mode 100644 index 00000000000..1e2c9f5bce4 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDeformableContactConstraint.h @@ -0,0 +1,284 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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_DEFORMABLE_CONTACT_CONSTRAINT_H +#define BT_DEFORMABLE_CONTACT_CONSTRAINT_H +#include "btSoftBody.h" + +// btDeformableContactConstraint is an abstract class specifying the method that each type of contact constraint needs to implement +class btDeformableContactConstraint +{ +public: + // True if the friction is static + // False if the friction is dynamic + bool m_static; + const btContactSolverInfo* m_infoGlobal; + + // normal of the contact + btVector3 m_normal; + + btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal) : m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal) + { + } + + btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal) : m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal) + { + } + + btDeformableContactConstraint() {} + + btDeformableContactConstraint(const btDeformableContactConstraint& other) + : m_static(other.m_static), m_normal(other.m_normal), m_infoGlobal(other.m_infoGlobal) + { + } + + virtual ~btDeformableContactConstraint() {} + + // solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence + // the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0; + + // get the velocity of the object A in the contact + virtual btVector3 getVa() const = 0; + + // get the velocity of the object B in the contact + virtual btVector3 getVb() const = 0; + + // get the velocity change of the soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const = 0; + + // apply impulse to the soft body node and/or face involved + virtual void applyImpulse(const btVector3& impulse) = 0; + + // scale the penetration depth by erp + virtual void setPenetrationScale(btScalar scale) = 0; +}; + +// +// Constraint that a certain node in the deformable objects cannot move +class btDeformableStaticConstraint : public btDeformableContactConstraint +{ +public: + btSoftBody::Node* m_node; + + btDeformableStaticConstraint(btSoftBody::Node* node, const btContactSolverInfo& infoGlobal) : m_node(node), btDeformableContactConstraint(false, btVector3(0, 0, 0), infoGlobal) + { + } + btDeformableStaticConstraint() {} + btDeformableStaticConstraint(const btDeformableStaticConstraint& other) + : m_node(other.m_node), btDeformableContactConstraint(other) + { + } + + virtual ~btDeformableStaticConstraint() {} + + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) + { + return 0; + } + + virtual btVector3 getVa() const + { + return btVector3(0, 0, 0); + } + + virtual btVector3 getVb() const + { + return btVector3(0, 0, 0); + } + + virtual btVector3 getDv(const btSoftBody::Node* n) const + { + return btVector3(0, 0, 0); + } + + virtual void applyImpulse(const btVector3& impulse) {} + virtual void setPenetrationScale(btScalar scale) {} +}; + +// +// Anchor Constraint between rigid and deformable node +class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint +{ +public: + const btSoftBody::DeformableNodeRigidAnchor* m_anchor; + + btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal); + btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other); + btDeformableNodeAnchorConstraint() {} + virtual ~btDeformableNodeAnchorConstraint() + { + } + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); + + // object A is the rigid/multi body, and object B is the deformable node/face + virtual btVector3 getVa() const; + // get the velocity of the deformable node in contact + virtual btVector3 getVb() const; + virtual btVector3 getDv(const btSoftBody::Node* n) const + { + return btVector3(0, 0, 0); + } + virtual void applyImpulse(const btVector3& impulse); + + virtual void setPenetrationScale(btScalar scale) {} +}; + +// +// Constraint between rigid/multi body and deformable objects +class btDeformableRigidContactConstraint : public btDeformableContactConstraint +{ +public: + btVector3 m_total_normal_dv; + btVector3 m_total_tangent_dv; + btScalar m_penetration; + btScalar m_total_split_impulse; + bool m_binding; + const btSoftBody::DeformableRigidContact* m_contact; + + btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal); + btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other); + btDeformableRigidContactConstraint() {} + virtual ~btDeformableRigidContactConstraint() + { + } + + // object A is the rigid/multi body, and object B is the deformable node/face + virtual btVector3 getVa() const; + + // get the split impulse velocity of the deformable face at the contact point + virtual btVector3 getSplitVb() const = 0; + + // get the split impulse velocity of the rigid/multibdoy at the contaft + virtual btVector3 getSplitVa() const; + + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); + + virtual void setPenetrationScale(btScalar scale) + { + m_penetration *= scale; + } + + btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); + + virtual void applySplitImpulse(const btVector3& impulse) = 0; +}; + +// +// Constraint between rigid/multi body and deformable objects nodes +class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactConstraint +{ +public: + // the deformable node in contact + btSoftBody::Node* m_node; + + btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal); + btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other); + btDeformableNodeRigidContactConstraint() {} + virtual ~btDeformableNodeRigidContactConstraint() + { + } + + // get the velocity of the deformable node in contact + virtual btVector3 getVb() const; + + // get the split impulse velocity of the deformable face at the contact point + virtual btVector3 getSplitVb() const; + + // get the velocity change of the input soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const; + + // cast the contact to the desired type + const btSoftBody::DeformableNodeRigidContact* getContact() const + { + return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact); + } + + virtual void applyImpulse(const btVector3& impulse); + + virtual void applySplitImpulse(const btVector3& impulse); +}; + +// +// Constraint between rigid/multi body and deformable objects faces +class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint +{ +public: + btSoftBody::Face* m_face; + bool m_useStrainLimiting; + btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting); + btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other); + btDeformableFaceRigidContactConstraint() : m_useStrainLimiting(false) {} + virtual ~btDeformableFaceRigidContactConstraint() + { + } + + // get the velocity of the deformable face at the contact point + virtual btVector3 getVb() const; + + // get the split impulse velocity of the deformable face at the contact point + virtual btVector3 getSplitVb() const; + + // get the velocity change of the input soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const; + + // cast the contact to the desired type + const btSoftBody::DeformableFaceRigidContact* getContact() const + { + return static_cast<const btSoftBody::DeformableFaceRigidContact*>(m_contact); + } + + virtual void applyImpulse(const btVector3& impulse); + + virtual void applySplitImpulse(const btVector3& impulse); +}; + +// +// Constraint between deformable objects faces and deformable objects nodes +class btDeformableFaceNodeContactConstraint : public btDeformableContactConstraint +{ +public: + btSoftBody::Node* m_node; + btSoftBody::Face* m_face; + const btSoftBody::DeformableFaceNodeContact* m_contact; + btVector3 m_total_normal_dv; + btVector3 m_total_tangent_dv; + + btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal); + btDeformableFaceNodeContactConstraint() {} + virtual ~btDeformableFaceNodeContactConstraint() {} + + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); + + // get the velocity of the object A in the contact + virtual btVector3 getVa() const; + + // get the velocity of the object B in the contact + virtual btVector3 getVb() const; + + // get the velocity change of the input soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const; + + // cast the contact to the desired type + const btSoftBody::DeformableFaceNodeContact* getContact() const + { + return static_cast<const btSoftBody::DeformableFaceNodeContact*>(m_contact); + } + + virtual void applyImpulse(const btVector3& impulse); + + virtual void setPenetrationScale(btScalar scale) {} +}; +#endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */ diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableContactProjection.cpp b/extern/bullet2/src/BulletSoftBody/btDeformableContactProjection.cpp new file mode 100644 index 00000000000..7f67260ce6c --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -0,0 +1,639 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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 "btDeformableContactProjection.h" +#include "btDeformableMultiBodyDynamicsWorld.h" +#include <algorithm> +#include <cmath> +btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) +{ + btScalar residualSquare = 0; + for (int i = 0; i < numDeformableBodies; ++i) + { + for (int j = 0; j < m_softBodies.size(); ++j) + { + btCollisionObject* psb = m_softBodies[j]; + if (psb != deformableBodies[i]) + { + continue; + } + for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k) + { + btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k]; + btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k) + { + btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k]; + btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k) + { + btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k]; + btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + for (int k = 0; k < m_deformableConstraints[j].size(); ++k) + { + btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k]; + btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + } + } + return residualSquare; +} + +btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) +{ + btScalar residualSquare = 0; + for (int i = 0; i < numDeformableBodies; ++i) + { + for (int j = 0; j < m_softBodies.size(); ++j) + { + btCollisionObject* psb = m_softBodies[j]; + if (psb != deformableBodies[i]) + { + continue; + } + for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k) + { + btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k]; + btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k) + { + btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k]; + btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + } + } + return residualSquare; +} + +void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("setConstraints"); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + + // set Dirichlet constraint + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im == 0) + { + btDeformableStaticConstraint static_constraint(&psb->m_nodes[j], infoGlobal); + m_staticConstraints[i].push_back(static_constraint); + } + } + + // set up deformable anchors + for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) + { + btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j]; + // skip fixed points + if (anchor.m_node->m_im == 0) + { + continue; + } + anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local; + btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal); + m_nodeAnchorConstraints[i].push_back(constraint); + } + + // set Deformable Node vs. Rigid constraint + for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j) + { + const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j]; + // skip fixed points + if (contact.m_node->m_im == 0) + { + continue; + } + btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal); + m_nodeRigidConstraints[i].push_back(constraint); + } + + // set Deformable Face vs. Rigid constraint + for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j) + { + const btSoftBody::DeformableFaceRigidContact& contact = psb->m_faceRigidContacts[j]; + // skip fixed faces + if (contact.m_c2 == 0) + { + continue; + } + btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting); + m_faceRigidConstraints[i].push_back(constraint); + } + } +} + +void btDeformableContactProjection::project(TVStack& x) +{ +#ifndef USE_MGS + const int dim = 3; + for (int index = 0; index < m_projectionsDict.size(); ++index) + { + btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index); + size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1(); + if (projectionDirs.size() >= dim) + { + // static node + x[i].setZero(); + continue; + } + else if (projectionDirs.size() == 2) + { + btVector3 dir0 = projectionDirs[0]; + btVector3 dir1 = projectionDirs[1]; + btVector3 free_dir = btCross(dir0, dir1); + if (free_dir.safeNorm() < SIMD_EPSILON) + { + x[i] -= x[i].dot(dir0) * dir0; + } + else + { + free_dir.normalize(); + x[i] = x[i].dot(free_dir) * free_dir; + } + } + else + { + btAssert(projectionDirs.size() == 1); + btVector3 dir0 = projectionDirs[0]; + x[i] -= x[i].dot(dir0) * dir0; + } + } +#else + btReducedVector p(x.size()); + for (int i = 0; i < m_projections.size(); ++i) + { + p += (m_projections[i].dot(x) * m_projections[i]); + } + for (int i = 0; i < p.m_indices.size(); ++i) + { + x[p.m_indices[i]] -= p.m_vecs[i]; + } +#endif +} + +void btDeformableContactProjection::setProjection() +{ +#ifndef USE_MGS + BT_PROFILE("btDeformableContactProjection::setProjection"); + btAlignedObjectArray<btVector3> units; + units.push_back(btVector3(1, 0, 0)); + units.push_back(btVector3(0, 1, 0)); + units.push_back(btVector3(0, 0, 1)); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < m_staticConstraints[i].size(); ++j) + { + int index = m_staticConstraints[i][j].m_node->index; + m_staticConstraints[i][j].m_node->m_constrained = true; + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) + { + int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; + m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true; + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + int index = m_nodeRigidConstraints[i][j].m_node->index; + m_nodeRigidConstraints[i][j].m_node->m_constrained = true; + if (m_nodeRigidConstraints[i][j].m_binding) + { + if (m_nodeRigidConstraints[i][j].m_static) + { + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + else + { + if (m_projectionsDict.find(index) == NULL) + { + btAlignedObjectArray<btVector3> projections; + projections.push_back(m_nodeRigidConstraints[i][j].m_normal); + m_projectionsDict.insert(index, projections); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + projections.push_back(m_nodeRigidConstraints[i][j].m_normal); + } + } + } + } + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; + if (m_faceRigidConstraints[i][j].m_binding) + { + for (int k = 0; k < 3; ++k) + { + face->m_n[k]->m_constrained = true; + } + } + for (int k = 0; k < 3; ++k) + { + btSoftBody::Node* node = face->m_n[k]; + int index = node->index; + if (m_faceRigidConstraints[i][j].m_static) + { + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + for (int l = 0; l < 3; ++l) + { + projections.push_back(units[l]); + } + } + } + else + { + if (m_projectionsDict.find(index) == NULL) + { + btAlignedObjectArray<btVector3> projections; + projections.push_back(m_faceRigidConstraints[i][j].m_normal); + m_projectionsDict.insert(index, projections); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + projections.push_back(m_faceRigidConstraints[i][j].m_normal); + } + } + } + } + } +#else + int dof = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + dof += m_softBodies[i]->m_nodes.size(); + } + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < m_staticConstraints[i].size(); ++j) + { + int index = m_staticConstraints[i][j].m_node->index; + m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY; + btAlignedObjectArray<int> indices; + btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3; + indices.push_back(index); + vecs1.push_back(btVector3(1, 0, 0)); + vecs2.push_back(btVector3(0, 1, 0)); + vecs3.push_back(btVector3(0, 0, 1)); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + m_projections.push_back(btReducedVector(dof, indices, vecs2)); + m_projections.push_back(btReducedVector(dof, indices, vecs3)); + } + + for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) + { + int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; + m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY; + btAlignedObjectArray<int> indices; + btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3; + indices.push_back(index); + vecs1.push_back(btVector3(1, 0, 0)); + vecs2.push_back(btVector3(0, 1, 0)); + vecs3.push_back(btVector3(0, 0, 1)); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + m_projections.push_back(btReducedVector(dof, indices, vecs2)); + m_projections.push_back(btReducedVector(dof, indices, vecs3)); + } + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + int index = m_nodeRigidConstraints[i][j].m_node->index; + m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset; + btAlignedObjectArray<int> indices; + indices.push_back(index); + btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3; + if (m_nodeRigidConstraints[i][j].m_static) + { + vecs1.push_back(btVector3(1, 0, 0)); + vecs2.push_back(btVector3(0, 1, 0)); + vecs3.push_back(btVector3(0, 0, 1)); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + m_projections.push_back(btReducedVector(dof, indices, vecs2)); + m_projections.push_back(btReducedVector(dof, indices, vecs3)); + } + else + { + vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + } + } + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; + btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; + btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset; + for (int k = 0; k < 3; ++k) + { + face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); + } + if (m_faceRigidConstraints[i][j].m_static) + { + for (int l = 0; l < 3; ++l) + { + btReducedVector rv(dof); + for (int k = 0; k < 3; ++k) + { + rv.m_indices.push_back(face->m_n[k]->index); + btVector3 v(0, 0, 0); + v[l] = bary[k]; + rv.m_vecs.push_back(v); + rv.sort(); + } + m_projections.push_back(rv); + } + } + else + { + btReducedVector rv(dof); + for (int k = 0; k < 3; ++k) + { + rv.m_indices.push_back(face->m_n[k]->index); + rv.m_vecs.push_back(bary[k] * m_faceRigidConstraints[i][j].m_normal); + rv.sort(); + } + m_projections.push_back(rv); + } + } + } + btModifiedGramSchmidt<btReducedVector> mgs(m_projections); + mgs.solve(); + m_projections = mgs.m_out; +#endif +} + +void btDeformableContactProjection::checkConstraints(const TVStack& x) +{ + for (int i = 0; i < m_lagrangeMultipliers.size(); ++i) + { + btVector3 d(0, 0, 0); + const LagrangeMultiplier& lm = m_lagrangeMultipliers[i]; + for (int j = 0; j < lm.m_num_constraints; ++j) + { + for (int k = 0; k < lm.m_num_nodes; ++k) + { + d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]); + } + } + // printf("d = %f, %f, %f\n", d[0], d[1], d[2]); + // printf("val = %f, %f, %f\n", lm.m_vals[0], lm.m_vals[1], lm.m_vals[2]); + } +} + +void btDeformableContactProjection::setLagrangeMultiplier() +{ + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < m_staticConstraints[i].size(); ++j) + { + int index = m_staticConstraints[i][j].m_node->index; + m_staticConstraints[i][j].m_node->m_constrained = true; + LagrangeMultiplier lm; + lm.m_num_nodes = 1; + lm.m_indices[0] = index; + lm.m_weights[0] = 1.0; + lm.m_num_constraints = 3; + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); + m_lagrangeMultipliers.push_back(lm); + } + for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) + { + int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; + m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true; + LagrangeMultiplier lm; + lm.m_num_nodes = 1; + lm.m_indices[0] = index; + lm.m_weights[0] = 1.0; + lm.m_num_constraints = 3; + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); + m_lagrangeMultipliers.push_back(lm); + } + + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + if (!m_nodeRigidConstraints[i][j].m_binding) + { + continue; + } + int index = m_nodeRigidConstraints[i][j].m_node->index; + m_nodeRigidConstraints[i][j].m_node->m_constrained = true; + LagrangeMultiplier lm; + lm.m_num_nodes = 1; + lm.m_indices[0] = index; + lm.m_weights[0] = 1.0; + if (m_nodeRigidConstraints[i][j].m_static) + { + lm.m_num_constraints = 3; + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); + } + else + { + lm.m_num_constraints = 1; + lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal; + } + m_lagrangeMultipliers.push_back(lm); + } + + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + if (!m_faceRigidConstraints[i][j].m_binding) + { + continue; + } + btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; + + btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; + LagrangeMultiplier lm; + lm.m_num_nodes = 3; + + for (int k = 0; k < 3; ++k) + { + face->m_n[k]->m_constrained = true; + lm.m_indices[k] = face->m_n[k]->index; + lm.m_weights[k] = bary[k]; + } + if (m_faceRigidConstraints[i][j].m_static) + { + face->m_pcontact[3] = 1; + lm.m_num_constraints = 3; + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); + } + else + { + face->m_pcontact[3] = 0; + lm.m_num_constraints = 1; + lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal; + } + m_lagrangeMultipliers.push_back(lm); + } + } +} + +// +void btDeformableContactProjection::applyDynamicFriction(TVStack& f) +{ + for (int i = 0; i < m_softBodies.size(); ++i) + { + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + const btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j]; + const btSoftBody::Node* node = constraint.m_node; + if (node->m_im != 0) + { + int index = node->index; + f[index] += constraint.getDv(node) * (1. / node->m_im); + } + } + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + const btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j]; + const btSoftBody::Face* face = constraint.getContact()->m_face; + for (int k = 0; k < 3; ++k) + { + const btSoftBody::Node* node = face->m_n[k]; + if (node->m_im != 0) + { + int index = node->index; + f[index] += constraint.getDv(node) * (1. / node->m_im); + } + } + } + for (int j = 0; j < m_deformableConstraints[i].size(); ++j) + { + const btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[i][j]; + const btSoftBody::Face* face = constraint.getContact()->m_face; + const btSoftBody::Node* node = constraint.getContact()->m_node; + if (node->m_im != 0) + { + int index = node->index; + f[index] += constraint.getDv(node) * (1. / node->m_im); + } + for (int k = 0; k < 3; ++k) + { + const btSoftBody::Node* node = face->m_n[k]; + if (node->m_im != 0) + { + int index = node->index; + f[index] += constraint.getDv(node) * (1. / node->m_im); + } + } + } + } +} + +void btDeformableContactProjection::reinitialize(bool nodeUpdated) +{ + int N = m_softBodies.size(); + if (nodeUpdated) + { + m_staticConstraints.resize(N); + m_nodeAnchorConstraints.resize(N); + m_nodeRigidConstraints.resize(N); + m_faceRigidConstraints.resize(N); + m_deformableConstraints.resize(N); + } + for (int i = 0; i < N; ++i) + { + m_staticConstraints[i].clear(); + m_nodeAnchorConstraints[i].clear(); + m_nodeRigidConstraints[i].clear(); + m_faceRigidConstraints[i].clear(); + m_deformableConstraints[i].clear(); + } +#ifndef USE_MGS + m_projectionsDict.clear(); +#else + m_projections.clear(); +#endif + m_lagrangeMultipliers.clear(); +} diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableContactProjection.h b/extern/bullet2/src/BulletSoftBody/btDeformableContactProjection.h new file mode 100644 index 00000000000..4964eaf990a --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDeformableContactProjection.h @@ -0,0 +1,99 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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_CONTACT_PROJECTION_H +#define BT_CONTACT_PROJECTION_H +#include "btCGProjection.h" +#include "btSoftBody.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" +#include "btDeformableContactConstraint.h" +#include "LinearMath/btHashMap.h" +#include "LinearMath/btReducedVector.h" +#include "LinearMath/btModifiedGramSchmidt.h" +#include <vector> + +struct LagrangeMultiplier +{ + int m_num_constraints; // Number of constraints + int m_num_nodes; // Number of nodes in these constraints + btScalar m_weights[3]; // weights of the nodes involved, same size as m_num_nodes + btVector3 m_dirs[3]; // Constraint directions, same size of m_num_constraints; + int m_indices[3]; // indices of the nodes involved, same size as m_num_nodes; +}; + +class btDeformableContactProjection +{ +public: + typedef btAlignedObjectArray<btVector3> TVStack; + btAlignedObjectArray<btSoftBody*>& m_softBodies; + + // all constraints involving face + btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints; +#ifndef USE_MGS + // map from node index to projection directions + btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict; +#else + btAlignedObjectArray<btReducedVector> m_projections; +#endif + + btAlignedObjectArray<LagrangeMultiplier> m_lagrangeMultipliers; + + // map from node index to static constraint + btAlignedObjectArray<btAlignedObjectArray<btDeformableStaticConstraint> > m_staticConstraints; + // map from node index to node rigid constraint + btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints; + // map from node index to face rigid constraint + btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceRigidContactConstraint> > m_faceRigidConstraints; + // map from node index to deformable constraint + btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceNodeContactConstraint> > m_deformableConstraints; + // map from node index to node anchor constraint + btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeAnchorConstraint> > m_nodeAnchorConstraints; + + bool m_useStrainLimiting; + + btDeformableContactProjection(btAlignedObjectArray<btSoftBody*>& softBodies) + : m_softBodies(softBodies) + { + } + + virtual ~btDeformableContactProjection() + { + } + + // apply the constraints to the rhs of the linear solve + virtual void project(TVStack& x); + + // add friction force to the rhs of the linear solve + virtual void applyDynamicFriction(TVStack& f); + + // update and solve the constraints + virtual btScalar update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal); + + // Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict. + virtual void setConstraints(const btContactSolverInfo& infoGlobal); + + // Set up projections for each vertex by adding the projection direction to + virtual void setProjection(); + + virtual void reinitialize(bool nodeUpdated); + + btScalar solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal); + + virtual void setLagrangeMultiplier(); + + void checkConstraints(const TVStack& x); +}; +#endif /* btDeformableContactProjection_h */ diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableCorotatedForce.h b/extern/bullet2/src/BulletSoftBody/btDeformableCorotatedForce.h new file mode 100644 index 00000000000..dfd85523bc2 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDeformableCorotatedForce.h @@ -0,0 +1,124 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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_COROTATED_H +#define BT_COROTATED_H + +#include "btDeformableLagrangianForce.h" +#include "LinearMath/btPolarDecomposition.h" + +static inline int PolarDecomposition(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s) +{ + static const btPolarDecomposition polar; + return polar.decompose(m, q, s); +} + +class btDeformableCorotatedForce : public btDeformableLagrangianForce +{ +public: + typedef btAlignedObjectArray<btVector3> TVStack; + btScalar m_mu, m_lambda; + btDeformableCorotatedForce() : m_mu(1), m_lambda(1) + { + } + + btDeformableCorotatedForce(btScalar mu, btScalar lambda) : m_mu(mu), m_lambda(lambda) + { + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btMatrix3x3 P; + firstPiola(tetra.m_F, P); + btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose() * grad_N_hat_1st_col); + btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); + + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + + // elastic force + // explicit elastic force + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * force_on_node0; + force[id1] -= scale1 * force_on_node123.getColumn(0); + force[id2] -= scale1 * force_on_node123.getColumn(1); + force[id3] -= scale1 * force_on_node123.getColumn(2); + } + } + } + + void firstPiola(const btMatrix3x3& F, btMatrix3x3& P) + { + // btMatrix3x3 JFinvT = F.adjoint(); + btScalar J = F.determinant(); + P = F.adjoint().transpose() * (m_lambda * (J - 1)); + if (m_mu > SIMD_EPSILON) + { + btMatrix3x3 R, S; + if (J < 1024 * SIMD_EPSILON) + R.setIdentity(); + else + PolarDecomposition(F, R, S); // this QR is not robust, consider using implicit shift svd + /*https://fuchuyuan.github.io/research/svd/paper.pdf*/ + P += (F - R) * 2 * m_mu; + } + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_COROTATED_FORCE; + } +}; + +#endif /* btCorotated_h */ diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableGravityForce.h b/extern/bullet2/src/BulletSoftBody/btDeformableGravityForce.h new file mode 100644 index 00000000000..d91867f4578 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDeformableGravityForce.h @@ -0,0 +1,105 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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_DEFORMABLE_GRAVITY_FORCE_H +#define BT_DEFORMABLE_GRAVITY_FORCE_H + +#include "btDeformableLagrangianForce.h" + +class btDeformableGravityForce : public btDeformableLagrangianForce +{ +public: + typedef btAlignedObjectArray<btVector3> TVStack; + btVector3 m_gravity; + + btDeformableGravityForce(const btVector3& g) : m_gravity(g) + { + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledGravityForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledGravityForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + virtual void addScaledGravityForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& n = psb->m_nodes[j]; + size_t id = n.index; + btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im; + btVector3 scaled_force = scale * m_gravity * mass * m_softBodies[i]->m_gravityFactor; + force[id] += scaled_force; + } + } + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_GRAVITY_FORCE; + } + + // the gravitational potential energy + virtual double totalEnergy(btScalar dt) + { + double e = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + if (node.m_im > 0) + { + e -= m_gravity.dot(node.m_q) / node.m_im; + } + } + } + return e; + } +}; +#endif /* BT_DEFORMABLE_GRAVITY_FORCE_H */ diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableLagrangianForce.h b/extern/bullet2/src/BulletSoftBody/btDeformableLagrangianForce.h new file mode 100644 index 00000000000..d58d825d1ce --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -0,0 +1,372 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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_DEFORMABLE_LAGRANGIAN_FORCE_H +#define BT_DEFORMABLE_LAGRANGIAN_FORCE_H + +#include "btSoftBody.h" +#include <LinearMath/btHashMap.h> +#include <iostream> + +enum btDeformableLagrangianForceType +{ + BT_GRAVITY_FORCE = 1, + BT_MASSSPRING_FORCE = 2, + BT_COROTATED_FORCE = 3, + BT_NEOHOOKEAN_FORCE = 4, + BT_LINEAR_ELASTICITY_FORCE = 5, + BT_MOUSE_PICKING_FORCE = 6 +}; + +static inline double randomDouble(double low, double high) +{ + return low + static_cast<double>(rand()) / RAND_MAX * (high - low); +} + +class btDeformableLagrangianForce +{ +public: + typedef btAlignedObjectArray<btVector3> TVStack; + btAlignedObjectArray<btSoftBody*> m_softBodies; + const btAlignedObjectArray<btSoftBody::Node*>* m_nodes; + + btDeformableLagrangianForce() + { + } + + virtual ~btDeformableLagrangianForce() {} + + // add all forces + virtual void addScaledForces(btScalar scale, TVStack& force) = 0; + + // add damping df + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0; + + // build diagonal of A matrix + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) = 0; + + // add elastic df + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0; + + // add all forces that are explicit in explicit solve + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0; + + // add all damping forces + virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0; + + virtual void addScaledHessian(btScalar scale) {} + + virtual btDeformableLagrangianForceType getForceType() = 0; + + virtual void reinitialize(bool nodeUpdated) + { + } + + // get number of nodes that have the force + virtual int getNumNodes() + { + int numNodes = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + numNodes += m_softBodies[i]->m_nodes.size(); + } + return numNodes; + } + + // add a soft body to be affected by the particular lagrangian force + virtual void addSoftBody(btSoftBody* psb) + { + m_softBodies.push_back(psb); + } + + virtual void removeSoftBody(btSoftBody* psb) + { + m_softBodies.remove(psb); + } + + virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes) + { + m_nodes = nodes; + } + + // Calculate the incremental deformable generated from the input dx + virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx) + { + btVector3 c1 = dx[id1] - dx[id0]; + btVector3 c2 = dx[id2] - dx[id0]; + btVector3 c3 = dx[id3] - dx[id0]; + return btMatrix3x3(c1, c2, c3).transpose(); + } + + // Calculate the incremental deformable generated from the current velocity + virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3) + { + btVector3 c1 = n1->m_v - n0->m_v; + btVector3 c2 = n2->m_v - n0->m_v; + btVector3 c3 = n3->m_v - n0->m_v; + return btMatrix3x3(c1, c2, c3).transpose(); + } + + // test for addScaledElasticForce function + virtual void testDerivative() + { + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1)); + } + psb->updateDeformation(); + } + + TVStack dx; + dx.resize(getNumNodes()); + TVStack dphi_dx; + dphi_dx.resize(dx.size()); + for (int i = 0; i < dphi_dx.size(); ++i) + { + dphi_dx[i].setZero(); + } + addScaledForces(-1, dphi_dx); + + // write down the current position + TVStack x; + x.resize(dx.size()); + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + x[counter] = psb->m_nodes[j].m_q; + counter++; + } + } + counter = 0; + + // populate dx with random vectors + for (int i = 0; i < dx.size(); ++i) + { + dx[i].setX(randomDouble(-1, 1)); + dx[i].setY(randomDouble(-1, 1)); + dx[i].setZ(randomDouble(-1, 1)); + } + + btAlignedObjectArray<double> errors; + for (int it = 0; it < 10; ++it) + { + for (int i = 0; i < dx.size(); ++i) + { + dx[i] *= 0.5; + } + + // get dphi/dx * dx + double dphi = 0; + for (int i = 0; i < dx.size(); ++i) + { + dphi += dphi_dx[i].dot(dx[i]); + } + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] + dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + double f1 = totalElasticEnergy(0); + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] - dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + + double f2 = totalElasticEnergy(0); + + //restore m_q + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + double error = f1 - f2 - 2 * dphi; + errors.push_back(error); + std::cout << "Iteration = " << it << ", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl; + } + for (int i = 1; i < errors.size(); ++i) + { + std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl; + } + } + + // test for addScaledElasticForce function + virtual void testHessian() + { + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1)); + } + psb->updateDeformation(); + } + + TVStack dx; + dx.resize(getNumNodes()); + TVStack df; + df.resize(dx.size()); + TVStack f1; + f1.resize(dx.size()); + TVStack f2; + f2.resize(dx.size()); + + // write down the current position + TVStack x; + x.resize(dx.size()); + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + x[counter] = psb->m_nodes[j].m_q; + counter++; + } + } + counter = 0; + + // populate dx with random vectors + for (int i = 0; i < dx.size(); ++i) + { + dx[i].setX(randomDouble(-1, 1)); + dx[i].setY(randomDouble(-1, 1)); + dx[i].setZ(randomDouble(-1, 1)); + } + + btAlignedObjectArray<double> errors; + for (int it = 0; it < 10; ++it) + { + for (int i = 0; i < dx.size(); ++i) + { + dx[i] *= 0.5; + } + + // get df + for (int i = 0; i < df.size(); ++i) + { + df[i].setZero(); + f1[i].setZero(); + f2[i].setZero(); + } + + //set df + addScaledElasticForceDifferential(-1, dx, df); + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] + dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + + //set f1 + addScaledForces(-1, f1); + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] - dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + + //set f2 + addScaledForces(-1, f2); + + //restore m_q + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + double error = 0; + for (int i = 0; i < df.size(); ++i) + { + btVector3 error_vector = f1[i] - f2[i] - 2 * df[i]; + error += error_vector.length2(); + } + error = btSqrt(error); + errors.push_back(error); + std::cout << "Iteration = " << it << ", error = " << error << std::endl; + } + for (int i = 1; i < errors.size(); ++i) + { + std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl; + } + } + + // + virtual double totalElasticEnergy(btScalar dt) + { + return 0; + } + + // + virtual double totalDampingEnergy(btScalar dt) + { + return 0; + } + + // total Energy takes dt as input because certain energies depend on dt + virtual double totalEnergy(btScalar dt) + { + return totalElasticEnergy(dt) + totalDampingEnergy(dt); + } +}; +#endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */ diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableLinearElasticityForce.h b/extern/bullet2/src/BulletSoftBody/btDeformableLinearElasticityForce.h new file mode 100644 index 00000000000..971192050b4 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDeformableLinearElasticityForce.h @@ -0,0 +1,462 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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_LINEAR_ELASTICITY_H +#define BT_LINEAR_ELASTICITY_H + +#include "btDeformableLagrangianForce.h" +#include "LinearMath/btQuickprof.h" +#include "btSoftBodyInternals.h" +#define TETRA_FLAT_THRESHOLD 0.01 +class btDeformableLinearElasticityForce : public btDeformableLagrangianForce +{ +public: + typedef btAlignedObjectArray<btVector3> TVStack; + btScalar m_mu, m_lambda; + btScalar m_E, m_nu; // Young's modulus and Poisson ratio + btScalar m_damping_alpha, m_damping_beta; + btDeformableLinearElasticityForce() : m_mu(1), m_lambda(1), m_damping_alpha(0.01), m_damping_beta(0.01) + { + updateYoungsModulusAndPoissonRatio(); + } + + btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping_alpha = 0.01, btScalar damping_beta = 0.01) : m_mu(mu), m_lambda(lambda), m_damping_alpha(damping_alpha), m_damping_beta(damping_beta) + { + updateYoungsModulusAndPoissonRatio(); + } + + void updateYoungsModulusAndPoissonRatio() + { + // conversion from Lame Parameters to Young's modulus and Poisson ratio + // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters + m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu); + m_nu = m_lambda * 0.5 / (m_mu + m_lambda); + } + + void updateLameParameters() + { + // conversion from Young's modulus and Poisson ratio to Lame Parameters + // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters + m_mu = m_E * 0.5 / (1 + m_nu); + m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu)); + } + + void setYoungsModulus(btScalar E) + { + m_E = E; + updateLameParameters(); + } + + void setPoissonRatio(btScalar nu) + { + m_nu = nu; + updateLameParameters(); + } + + void setDamping(btScalar damping_alpha, btScalar damping_beta) + { + m_damping_alpha = damping_alpha; + m_damping_beta = damping_beta; + } + + void setLameParameters(btScalar mu, btScalar lambda) + { + m_mu = mu; + m_lambda = lambda; + updateYoungsModulusAndPoissonRatio(); + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + if (m_damping_alpha == 0 && m_damping_beta == 0) + return; + btScalar mu_damp = m_damping_beta * m_mu; + btScalar lambda_damp = m_damping_beta * m_lambda; + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + bool close_to_flat = (psb->m_tetraScratches[j].m_J < TETRA_FLAT_THRESHOLD); + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; + if (!close_to_flat) + { + dF = psb->m_tetraScratches[j].m_corotation.transpose() * dF; + } + btMatrix3x3 I; + I.setIdentity(); + btMatrix3x3 dP = (dF + dF.transpose()) * mu_damp + I * ((dF[0][0] + dF[1][1] + dF[2][2]) * lambda_damp); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + if (!close_to_flat) + { + df_on_node123 = psb->m_tetraScratches[j].m_corotation * df_on_node123; + } + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * df_on_node0; + force[id1] -= scale1 * df_on_node123.getColumn(0); + force[id2] -= scale1 * df_on_node123.getColumn(1); + force[id3] -= scale1 * df_on_node123.getColumn(2); + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + size_t id = node.index; + if (node.m_im > 0) + { + force[id] -= scale * node.m_v / node.m_im * m_damping_alpha; + } + } + } + } + + virtual double totalElasticEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetraScratches.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::TetraScratch& s = psb->m_tetraScratches[j]; + energy += tetra.m_element_measure * elasticEnergyDensity(s); + } + } + return energy; + } + + // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + int sz = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + sz = btMax(sz, psb->m_nodes[j].index); + } + } + TVStack dampingForce; + dampingForce.resize(sz + 1); + for (int i = 0; i < dampingForce.size(); ++i) + dampingForce[i].setZero(); + addScaledDampingForce(0.5, dampingForce); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + energy -= dampingForce[node.index].dot(node.m_v) / dt; + } + } + return energy; + } + + double elasticEnergyDensity(const btSoftBody::TetraScratch& s) + { + double density = 0; + btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); + btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2]; + density += m_mu * (epsilon[0].length2() + epsilon[1].length2() + epsilon[2].length2()); + density += m_lambda * trace * trace * 0.5; + return density; + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + btScalar max_p = psb->m_cfg.m_maxStress; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btMatrix3x3 P; + firstPiola(psb->m_tetraScratches[j], P); +#if USE_SVD + if (max_p > 0) + { + // since we want to clamp the principal stress to max_p, we only need to + // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p + btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2()); + if (trPTP > max_p * max_p) + { + btMatrix3x3 U, V; + btVector3 sigma; + singularValueDecomposition(P, U, sigma, V); + sigma[0] = btMin(sigma[0], max_p); + sigma[1] = btMin(sigma[1], max_p); + sigma[2] = btMin(sigma[2], max_p); + sigma[0] = btMax(sigma[0], -max_p); + sigma[1] = btMax(sigma[1], -max_p); + sigma[2] = btMax(sigma[2], -max_p); + btMatrix3x3 Sigma; + Sigma.setIdentity(); + Sigma[0][0] = sigma[0]; + Sigma[1][1] = sigma[1]; + Sigma[2][2] = sigma[2]; + P = U * Sigma * V.transpose(); + } + } +#endif + // btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 force_on_node123 = psb->m_tetraScratches[j].m_corotation * P * tetra.m_Dm_inverse.transpose(); + btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; + + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + + // elastic force + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * force_on_node0; + force[id1] -= scale1 * force_on_node123.getColumn(0); + force[id2] -= scale1 * force_on_node123.getColumn(1); + force[id3] -= scale1 * force_on_node123.getColumn(2); + } + } + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + if (m_damping_alpha == 0 && m_damping_beta == 0) + return; + btScalar mu_damp = m_damping_beta * m_mu; + btScalar lambda_damp = m_damping_beta * m_lambda; + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + bool close_to_flat = (psb->m_tetraScratches[j].m_J < TETRA_FLAT_THRESHOLD); + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; + if (!close_to_flat) + { + dF = psb->m_tetraScratches[j].m_corotation.transpose() * dF; + } + btMatrix3x3 I; + I.setIdentity(); + btMatrix3x3 dP = (dF + dF.transpose()) * mu_damp + I * ((dF[0][0] + dF[1][1] + dF[2][2]) * lambda_damp); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + if (!close_to_flat) + { + df_on_node123 = psb->m_tetraScratches[j].m_corotation * df_on_node123; + } + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + size_t id = node.index; + if (node.m_im > 0) + { + df[id] -= scale * dv[id] / node.m_im * m_damping_alpha; + } + } + } + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = psb->m_tetraScratches[j].m_corotation.transpose() * Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; + btMatrix3x3 dP; + firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); + // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = psb->m_tetraScratches[j].m_corotation * dP * tetra.m_Dm_inverse.transpose(); + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + + // elastic force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + + void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) + { + btMatrix3x3 corotated_F = s.m_corotation.transpose() * s.m_F; + + btMatrix3x3 epsilon = (corotated_F + corotated_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); + btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2]; + P = epsilon * btScalar(2) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace; + } + + // Let P be the first piola stress. + // This function calculates the dP = dP/dF * dF + void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) + { + btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]); + dP = (dF + dF.transpose()) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace; + } + + // Let Q be the damping stress. + // This function calculates the dP = dQ/dF * dF + void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) + { + btScalar mu_damp = m_damping_beta * m_mu; + btScalar lambda_damp = m_damping_beta * m_lambda; + btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]); + dP = (dF + dF.transpose()) * mu_damp + btMatrix3x3::getIdentity() * lambda_damp * trace; + } + + virtual void addScaledHessian(btScalar scale) + { + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btMatrix3x3 P; + firstPiola(psb->m_tetraScratches[j], P); // make sure scratch is evaluated at x_n + dt * vn + btMatrix3x3 force_on_node123 = psb->m_tetraScratches[j].m_corotation * P * tetra.m_Dm_inverse.transpose(); + btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + btScalar scale1 = scale * (scale + m_damping_beta) * tetra.m_element_measure; // stiff and stiffness-damping terms; + node0->m_effectiveMass += OuterProduct(force_on_node0, force_on_node0) * scale1; + node1->m_effectiveMass += OuterProduct(force_on_node123.getColumn(0), force_on_node123.getColumn(0)) * scale1; + node2->m_effectiveMass += OuterProduct(force_on_node123.getColumn(1), force_on_node123.getColumn(1)) * scale1; + node3->m_effectiveMass += OuterProduct(force_on_node123.getColumn(2), force_on_node123.getColumn(2)) * scale1; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + if (node.m_im > 0) + { + btMatrix3x3 I; + I.setIdentity(); + node.m_effectiveMass += I * (scale * (1.0 / node.m_im) * m_damping_alpha); + } + } + } + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_LINEAR_ELASTICITY_FORCE; + } +}; +#endif /* BT_LINEAR_ELASTICITY_H */ diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableMassSpringForce.h b/extern/bullet2/src/BulletSoftBody/btDeformableMassSpringForce.h new file mode 100644 index 00000000000..8c97bd1ba8b --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -0,0 +1,301 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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_MASS_SPRING_H +#define BT_MASS_SPRING_H + +#include "btDeformableLagrangianForce.h" + +class btDeformableMassSpringForce : public btDeformableLagrangianForce +{ + // If true, the damping force will be in the direction of the spring + // If false, the damping force will be in the direction of the velocity + bool m_momentum_conserving; + btScalar m_elasticStiffness, m_dampingStiffness, m_bendingStiffness; + +public: + typedef btAlignedObjectArray<btVector3> TVStack; + btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05) + { + } + btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true, double bending_k = -1) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d), m_bendingStiffness(bending_k) + { + if (m_bendingStiffness < btScalar(0)) + { + m_bendingStiffness = m_elasticStiffness; + } + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + + // damping force + btVector3 v_diff = (node2->m_v - node1->m_v); + btVector3 scaled_force = scale * m_dampingStiffness * v_diff; + if (m_momentum_conserving) + { + if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) + { + btVector3 dir = (node2->m_x - node1->m_x).normalized(); + scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; + } + } + force[id1] += scaled_force; + force[id2] -= scaled_force; + } + } + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + btScalar r = link.m_rl; + size_t id1 = node1->index; + size_t id2 = node2->index; + + // elastic force + btVector3 dir = (node2->m_q - node1->m_q); + btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0); + btScalar scaled_stiffness = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness); + btVector3 scaled_force = scaled_stiffness * (dir - dir_normalized * r); + force[id1] += scaled_force; + force[id2] -= scaled_force; + } + } + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + // implicit damping force differential + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + btScalar scaled_k_damp = m_dampingStiffness * scale; + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + + btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]); + if (m_momentum_conserving) + { + if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) + { + btVector3 dir = (node2->m_x - node1->m_x).normalized(); + local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir; + } + } + df[id1] += local_scaled_df; + df[id2] -= local_scaled_df; + } + } + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) + { + // implicit damping force differential + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + btScalar scaled_k_damp = m_dampingStiffness * scale; + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + if (m_momentum_conserving) + { + if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) + { + btVector3 dir = (node2->m_x - node1->m_x).normalized(); + for (int d = 0; d < 3; ++d) + { + if (node1->m_im > 0) + diagA[id1][d] -= scaled_k_damp * dir[d] * dir[d]; + if (node2->m_im > 0) + diagA[id2][d] -= scaled_k_damp * dir[d] * dir[d]; + } + } + } + else + { + for (int d = 0; d < 3; ++d) + { + if (node1->m_im > 0) + diagA[id1][d] -= scaled_k_damp; + if (node2->m_im > 0) + diagA[id2][d] -= scaled_k_damp; + } + } + } + } + } + + virtual double totalElasticEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + btScalar r = link.m_rl; + + // elastic force + btVector3 dir = (node2->m_q - node1->m_q); + energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() - r); + } + } + return energy; + } + + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + int sz = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + sz = btMax(sz, psb->m_nodes[j].index); + } + } + TVStack dampingForce; + dampingForce.resize(sz + 1); + for (int i = 0; i < dampingForce.size(); ++i) + dampingForce[i].setZero(); + addScaledDampingForce(0.5, dampingForce); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + energy -= dampingForce[node.index].dot(node.m_v) / dt; + } + } + return energy; + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + // implicit damping force differential + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + btScalar r = link.m_rl; + + btVector3 dir = (node1->m_q - node2->m_q); + btScalar dir_norm = dir.norm(); + btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0); + btVector3 dx_diff = dx[id1] - dx[id2]; + btVector3 scaled_df = btVector3(0, 0, 0); + btScalar scaled_k = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness); + if (dir_norm > SIMD_EPSILON) + { + scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized; + scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized; + scaled_df -= scaled_k * ((dir_norm - r) / dir_norm) * dx_diff; + } + + df[id1] += scaled_df; + df[id2] -= scaled_df; + } + } + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_MASSSPRING_FORCE; + } +}; + +#endif /* btMassSpring_h */ diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableMousePickingForce.h b/extern/bullet2/src/BulletSoftBody/btDeformableMousePickingForce.h new file mode 100644 index 00000000000..d218d962149 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDeformableMousePickingForce.h @@ -0,0 +1,162 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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_MOUSE_PICKING_FORCE_H +#define BT_MOUSE_PICKING_FORCE_H + +#include "btDeformableLagrangianForce.h" + +class btDeformableMousePickingForce : public btDeformableLagrangianForce +{ + // If true, the damping force will be in the direction of the spring + // If false, the damping force will be in the direction of the velocity + btScalar m_elasticStiffness, m_dampingStiffness; + const btSoftBody::Face& m_face; + btVector3 m_mouse_pos; + btScalar m_maxForce; + +public: + typedef btAlignedObjectArray<btVector3> TVStack; + btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, btVector3 mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce) + { + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + for (int i = 0; i < 3; ++i) + { + btVector3 v_diff = m_face.m_n[i]->m_v; + btVector3 scaled_force = scale * m_dampingStiffness * v_diff; + if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) + { + btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); + scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; + } + force[m_face.m_n[i]->index] -= scaled_force; + } + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + btScalar scaled_stiffness = scale * m_elasticStiffness; + for (int i = 0; i < 3; ++i) + { + btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); + btVector3 scaled_force = scaled_stiffness * dir; + if (scaled_force.safeNorm() > m_maxForce) + { + scaled_force.safeNormalize(); + scaled_force *= m_maxForce; + } + force[m_face.m_n[i]->index] -= scaled_force; + } + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + btScalar scaled_k_damp = m_dampingStiffness * scale; + for (int i = 0; i < 3; ++i) + { + btVector3 local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index]; + if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) + { + btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); + local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index].dot(dir) * dir; + } + df[m_face.m_n[i]->index] -= local_scaled_df; + } + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + virtual double totalElasticEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < 3; ++i) + { + btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); + btVector3 scaled_force = m_elasticStiffness * dir; + if (scaled_force.safeNorm() > m_maxForce) + { + scaled_force.safeNormalize(); + scaled_force *= m_maxForce; + } + energy += 0.5 * scaled_force.dot(dir); + } + return energy; + } + + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < 3; ++i) + { + btVector3 v_diff = m_face.m_n[i]->m_v; + btVector3 scaled_force = m_dampingStiffness * v_diff; + if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) + { + btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); + scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir; + } + energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt; + } + return energy; + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + btScalar scaled_stiffness = scale * m_elasticStiffness; + for (int i = 0; i < 3; ++i) + { + btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); + btScalar dir_norm = dir.norm(); + btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0); + int id = m_face.m_n[i]->index; + btVector3 dx_diff = dx[id]; + btScalar r = 0; // rest length is 0 for picking spring + btVector3 scaled_df = btVector3(0, 0, 0); + if (dir_norm > SIMD_EPSILON) + { + scaled_df -= scaled_stiffness * dir_normalized.dot(dx_diff) * dir_normalized; + scaled_df += scaled_stiffness * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized; + scaled_df -= scaled_stiffness * ((dir_norm - r) / dir_norm) * dx_diff; + } + df[id] += scaled_df; + } + } + + void setMousePos(const btVector3& p) + { + m_mouse_pos = p; + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_MOUSE_PICKING_FORCE; + } +}; + +#endif /* btMassSpring_h */ diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp new file mode 100644 index 00000000000..631fd5fbed5 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -0,0 +1,144 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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 "btDeformableMultiBodyConstraintSolver.h" +#include <iostream> +// override the iterations method to include deformable/multibody contact +btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) +{ + { + ///this is a special step to resolve penetrations (just for contacts) + solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + + int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; + for (int iteration = 0; iteration < maxIterations; iteration++) + { + // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body + + // solve rigid/rigid in solver body + m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + // solver body velocity -> rigid body velocity + solverBodyWriteBack(infoGlobal); + btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies, numDeformableBodies, infoGlobal); + // update rigid body velocity in rigid/deformable contact + m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual); + // solver body velocity <- rigid body velocity + writeToSolverBody(bodies, numBodies, infoGlobal); + + if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) + { +#ifdef VERBOSE_RESIDUAL_PRINTF + if (iteration >= (maxIterations - 1)) + printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration); +#endif + m_analyticsData.m_numSolverCalls++; + m_analyticsData.m_numIterationsUsed = iteration + 1; + m_analyticsData.m_islandId = -2; + if (numBodies > 0) + m_analyticsData.m_islandId = bodies[0]->getCompanionId(); + m_analyticsData.m_numBodies = numBodies; + m_analyticsData.m_numContactManifolds = numManifolds; + m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual; + break; + } + } + } + return 0.f; +} + +void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) +{ + m_tmpMultiBodyConstraints = multiBodyConstraints; + m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; + + // inherited from MultiBodyConstraintSolver + solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); + + // overriden + solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); + + // inherited from MultiBodyConstraintSolver + solveGroupCacheFriendlyFinish(bodies, numBodies, info); + + m_tmpMultiBodyConstraints = 0; + m_tmpNumMultiBodyConstraints = 0; +} + +void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +{ + for (int i = 0; i < numBodies; i++) + { + int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); + + btRigidBody* body = btRigidBody::upcast(bodies[i]); + if (body && body->getInvMass()) + { + btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; + solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity; + solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity; + } + } +} + +void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal) +{ + for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) + { + btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; + if (body) + { + m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity); + m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity + m_tmpSolverBodyPool[i].m_deltaAngularVelocity); + } + } +} + +void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) +{ + BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations"); + int iteration; + if (infoGlobal.m_splitImpulse) + { + { + for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) + { + btScalar leastSquaresResidual = 0.f; + { + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int j; + for (j = 0; j < numPoolConstraints; j++) + { + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + + btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); + } + // solve the position correction between deformable and rigid/multibody + // btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal); + btScalar residual = m_deformableSolver->m_objective->m_projection.solveSplitImpulse(deformableBodies, numDeformableBodies, infoGlobal); + leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); + } + if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1)) + { +#ifdef VERBOSE_RESIDUAL_PRINTF + if (iteration >= (infoGlobal.m_numIterations - 1)) + printf("split impulse residual = %f at iteration #%d\n", leastSquaresResidual, iteration); +#endif + break; + } + } + } + } +} diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h b/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h new file mode 100644 index 00000000000..94aabce838e --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h @@ -0,0 +1,61 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H +#define BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H + +#include "btDeformableBodySolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" + +class btDeformableBodySolver; + +// btDeformableMultiBodyConstraintSolver extendsn btMultiBodyConstraintSolver to solve for the contact among rigid/multibody and deformable bodies. Notice that the following constraints +// 1. rigid/multibody against rigid/multibody +// 2. rigid/multibody against deforamble +// 3. deformable against deformable +// 4. deformable self collision +// 5. joint constraints +// are all coupled in this solve. +ATTRIBUTE_ALIGNED16(class) +btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver +{ + btDeformableBodySolver* m_deformableSolver; + +protected: + // override the iterations method to include deformable/multibody contact + // virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + + // write the velocity of the the solver body to the underlying rigid body + void solverBodyWriteBack(const btContactSolverInfo& infoGlobal); + + // write the velocity of the underlying rigid body to the the the solver body + void writeToSolverBody(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal); + + virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + + virtual btScalar solveDeformableGroupIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + void setDeformableSolver(btDeformableBodySolver * deformableSolver) + { + m_deformableSolver = deformableSolver; + } + + virtual void solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); +}; + +#endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */ diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp new file mode 100644 index 00000000000..983e622b5f6 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -0,0 +1,814 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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. + */ + +/* ====== Overview of the Deformable Algorithm ====== */ + +/* +A single step of the deformable body simulation contains the following main components: +Call internalStepSimulation multiple times, to achieve 240Hz (4 steps of 60Hz). +1. Deformable maintaintenance of rest lengths and volume preservation. Forces only depend on position: Update velocity to a temporary state v_{n+1}^* = v_n + explicit_force * dt / mass, where explicit forces include gravity and elastic forces. +2. Detect discrete collisions between rigid and deformable bodies at position x_{n+1}^* = x_n + dt * v_{n+1}^*. + +3a. Solve all constraints, including LCP. Contact, position correction due to numerical drift, friction, and anchors for deformable. + +3b. 5 Newton steps (multiple step). Conjugent Gradient solves linear system. Deformable Damping: Then velocities of deformable bodies v_{n+1} are solved in + M(v_{n+1} - v_{n+1}^*) = damping_force * dt / mass, + by a conjugate gradient solver, where the damping force is implicit and depends on v_{n+1}. + Make sure contact constraints are not violated in step b by performing velocity projections as in the paper by Baraff and Witkin https://www.cs.cmu.edu/~baraff/papers/sig98.pdf. Dynamic frictions are treated as a force and added to the rhs of the CG solve, whereas static frictions are treated as constraints similar to contact. +4. Position is updated via x_{n+1} = x_n + dt * v_{n+1}. + + +The algorithm also closely resembles the one in http://physbam.stanford.edu/~fedkiw/papers/stanford2008-03.pdf + */ + +#include <stdio.h> +#include "btDeformableMultiBodyDynamicsWorld.h" +#include "DeformableBodyInplaceSolverIslandCallback.h" +#include "btDeformableBodySolver.h" +#include "LinearMath/btQuickprof.h" +#include "btSoftBodyInternals.h" +btDeformableMultiBodyDynamicsWorld::btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver) + : btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration), + m_deformableBodySolver(deformableBodySolver), + m_solverCallback(0) +{ + 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.setDefaultVoxelsz(0.005); + 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, -9.8, 0); + m_internalTime = 0.0; + m_implicit = false; + m_lineSearch = false; + m_useProjection = false; + m_ccdIterations = 5; + m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher); +} + +btDeformableMultiBodyDynamicsWorld::~btDeformableMultiBodyDynamicsWorld() +{ + delete m_solverDeformableBodyIslandCallback; +} + +void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) +{ + BT_PROFILE("internalSingleStepSimulation"); + if (0 != m_internalPreTickCallback) + { + (*m_internalPreTickCallback)(this, timeStep); + } + reinitialize(timeStep); + + // add gravity to velocity of rigid and multi bodys + applyRigidBodyGravity(timeStep); + + ///apply gravity and explicit force to velocity, predict motion + predictUnconstraintMotion(timeStep); + + ///perform collision detection that involves rigid/multi bodies + btMultiBodyDynamicsWorld::performDiscreteCollisionDetection(); + + btMultiBodyDynamicsWorld::calculateSimulationIslands(); + + beforeSolverCallbacks(timeStep); + + ///solve contact constraints and then deformable bodies momemtum equation + solveConstraints(timeStep); + + afterSolverCallbacks(timeStep); + + performDeformableCollisionDetection(); + + applyRepulsionForce(timeStep); + + performGeometricCollisions(timeStep); + + integrateTransforms(timeStep); + + ///update vehicle simulation + btMultiBodyDynamicsWorld::updateActions(timeStep); + + updateActivationState(timeStep); + // End solver-wise simulation step + // /////////////////////////////// +} + +void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection() +{ + for (int i = 0; i < m_softBodies.size(); ++i) + { + m_softBodies[i]->m_softSoftCollision = true; + } + + for (int i = 0; i < m_softBodies.size(); ++i) + { + for (int j = i; j < m_softBodies.size(); ++j) + { + m_softBodies[i]->defaultCollisionHandler(m_softBodies[j]); + } + } + + for (int i = 0; i < m_softBodies.size(); ++i) + { + m_softBodies[i]->m_softSoftCollision = false; + } +} + +void btDeformableMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) +{ + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = m_softBodies[i]; + psb->updateDeactivation(timeStep); + if (psb->wantsSleeping()) + { + if (psb->getActivationState() == ACTIVE_TAG) + psb->setActivationState(WANTS_DEACTIVATION); + if (psb->getActivationState() == ISLAND_SLEEPING) + { + psb->setZeroVelocity(); + } + } + else + { + if (psb->getActivationState() != DISABLE_DEACTIVATION) + psb->setActivationState(ACTIVE_TAG); + } + } + btMultiBodyDynamicsWorld::updateActivationState(timeStep); +} + +void btDeformableMultiBodyDynamicsWorld::applyRepulsionForce(btScalar timeStep) +{ + BT_PROFILE("btDeformableMultiBodyDynamicsWorld::applyRepulsionForce"); + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + psb->applyRepulsionForce(timeStep, true); + } + } +} + +void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar timeStep) +{ + BT_PROFILE("btDeformableMultiBodyDynamicsWorld::performGeometricCollisions"); + // refit the BVH tree for CCD + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + m_softBodies[i]->updateFaceTree(true, false); + m_softBodies[i]->updateNodeTree(true, false); + for (int j = 0; j < m_softBodies[i]->m_faces.size(); ++j) + { + btSoftBody::Face& f = m_softBodies[i]->m_faces[j]; + f.m_n0 = (f.m_n[1]->m_x - f.m_n[0]->m_x).cross(f.m_n[2]->m_x - f.m_n[0]->m_x); + } + } + } + + // clear contact points & update DBVT + for (int r = 0; r < m_ccdIterations; ++r) + { + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + // clear contact points in the previous iteration + psb->m_faceNodeContacts.clear(); + + // update m_q and normals for CCD calculation + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + timeStep * psb->m_nodes[j].m_v; + } + for (int j = 0; j < psb->m_faces.size(); ++j) + { + btSoftBody::Face& f = psb->m_faces[j]; + f.m_n1 = (f.m_n[1]->m_q - f.m_n[0]->m_q).cross(f.m_n[2]->m_q - f.m_n[0]->m_q); + f.m_vn = (f.m_n[1]->m_v - f.m_n[0]->m_v).cross(f.m_n[2]->m_v - f.m_n[0]->m_v) * timeStep * timeStep; + } + } + } + + // apply CCD to register new contact points + for (int i = 0; i < m_softBodies.size(); ++i) + { + for (int j = i; j < m_softBodies.size(); ++j) + { + btSoftBody* psb1 = m_softBodies[i]; + btSoftBody* psb2 = m_softBodies[j]; + if (psb1->isActive() && psb2->isActive()) + { + m_softBodies[i]->geometricCollisionHandler(m_softBodies[j]); + } + } + } + + int penetration_count = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + penetration_count += psb->m_faceNodeContacts.size(); + } + } + if (penetration_count == 0) + { + break; + } + + // apply inelastic impulse + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + psb->applyRepulsionForce(timeStep, false); + } + } + } +} + +void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision() +{ + BT_PROFILE("btDeformableMultiBodyDynamicsWorld::softBodySelfCollision"); + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + psb->defaultCollisionHandler(psb); + } + } +} + +void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep) +{ + // correct the position of rigid bodies with temporary velocity generated from split impulse + btContactSolverInfo infoGlobal; + btVector3 zero(0, 0, 0); + for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) + { + btRigidBody* rb = m_nonStaticRigidBodies[i]; + //correct the position/orientation based on push/turn recovery + btTransform newTransform; + btVector3 pushVelocity = rb->getPushVelocity(); + btVector3 turnVelocity = rb->getTurnVelocity(); + if (pushVelocity[0] != 0.f || pushVelocity[1] != 0 || pushVelocity[2] != 0 || turnVelocity[0] != 0.f || turnVelocity[1] != 0 || turnVelocity[2] != 0) + { + btTransformUtil::integrateTransform(rb->getWorldTransform(), pushVelocity, turnVelocity * infoGlobal.m_splitImpulseTurnErp, timeStep, newTransform); + rb->setWorldTransform(newTransform); + rb->setPushVelocity(zero); + rb->setTurnVelocity(zero); + } + } +} + +void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) +{ + BT_PROFILE("integrateTransforms"); + positionCorrection(timeStep); + btMultiBodyDynamicsWorld::integrateTransforms(timeStep); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement; + btScalar clampDeltaV = maxDisplacement / timeStep; + for (int c = 0; c < 3; c++) + { + if (node.m_v[c] > clampDeltaV) + { + node.m_v[c] = clampDeltaV; + } + if (node.m_v[c] < -clampDeltaV) + { + node.m_v[c] = -clampDeltaV; + } + } + node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv); + node.m_q = node.m_x; + node.m_vn = node.m_v; + } + // enforce anchor constraints + for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) + { + btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j]; + btSoftBody::Node* n = a.m_node; + n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local; + + // update multibody anchor info + if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 nrm; + const btCollisionShape* shp = multibodyLinkCol->getCollisionShape(); + const btTransform& wtr = multibodyLinkCol->getWorldTransform(); + psb->m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(n->m_x), + shp, + nrm, + 0); + a.m_cti.m_normal = wtr.getBasis() * nrm; + btVector3 normal = a.m_cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + a.m_c0 = rot.transpose() * local_impulse_matrix * rot; + a.jacobianData_normal = jacobianData_normal; + a.jacobianData_t1 = jacobianData_t1; + a.jacobianData_t2 = jacobianData_t2; + a.t1 = t1; + a.t2 = t2; + } + } + } + psb->interpolateRenderMesh(); + } +} + +void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) +{ + BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints"); + // save v_{n+1}^* velocity after explicit forces + m_deformableBodySolver->backupVelocity(); + + // set up constraints among multibodies and between multibodies and deformable bodies + setupConstraints(); + + // solve contact constraints + solveContactConstraints(); + + // set up the directions in which the velocity does not change in the momentum solve + if (m_useProjection) + m_deformableBodySolver->m_objective->m_projection.setProjection(); + else + m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier(); + + // for explicit scheme, m_backupVelocity = v_{n+1}^* + // for implicit scheme, m_backupVelocity = v_n + // Here, set dv = v_{n+1} - v_n for nodes in contact + m_deformableBodySolver->setupDeformableSolve(m_implicit); + + // At this point, dv should be golden for nodes in contact + // proceed to solve deformable momentum equation + m_deformableBodySolver->solveDeformableConstraints(timeStep); +} + +void btDeformableMultiBodyDynamicsWorld::setupConstraints() +{ + // set up constraints between multibody and deformable bodies + m_deformableBodySolver->setConstraints(m_solverInfo); + + // set up constraints among multibodies + { + sortConstraints(); + // setup the solver callback + btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; + btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; + m_solverDeformableBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); + + // build islands + m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld()); + } +} + +void btDeformableMultiBodyDynamicsWorld::sortConstraints() +{ + m_sortedConstraints.resize(m_constraints.size()); + int i; + for (i = 0; i < getNumConstraints(); i++) + { + m_sortedConstraints[i] = m_constraints[i]; + } + m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); + + m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); + for (i = 0; i < m_multiBodyConstraints.size(); i++) + { + m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; + } + m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); +} + +void btDeformableMultiBodyDynamicsWorld::solveContactConstraints() +{ + // process constraints on each island + m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverDeformableBodyIslandCallback); + + // process deferred + m_solverDeformableBodyIslandCallback->processConstraints(); + m_constraintSolver->allSolved(m_solverInfo, m_debugDrawer); + + // write joint feedback + { + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) + m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) + m_scratch_v.resize(bod->getNumLinks() + 1); + m_scratch_m.resize(bod->getNumLinks() + 1); + + if (bod->internalNeedsJointFeedback()) + { + if (!bod->isUsingRK4Integration()) + { + if (bod->internalNeedsJointFeedback()) + { + bool isConstraintPass = true; + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } + } + } + } + } + } + + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + bod->processDeltaVeeMultiDof2(); + } +} + +void btDeformableMultiBodyDynamicsWorld::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_deformableBodySolver); + + btCollisionWorld::addCollisionObject(body, + collisionFilterGroup, + collisionFilterMask); +} + +void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +{ + BT_PROFILE("predictUnconstraintMotion"); + btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep); + m_deformableBodySolver->predictMotion(timeStep); +} + +void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) +{ + m_internalTime += timeStep; + m_deformableBodySolver->setImplicit(m_implicit); + m_deformableBodySolver->setLineSearch(m_lineSearch); + m_deformableBodySolver->reinitialize(m_softBodies, timeStep); + btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo(); + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer(); + btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep; + if (m_useProjection) + { + m_deformableBodySolver->m_useProjection = true; + m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true; + m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner; + } + else + { + m_deformableBodySolver->m_useProjection = false; + m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false; + m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner; + } +} + +void btDeformableMultiBodyDynamicsWorld::debugDrawWorld() +{ + btMultiBodyDynamicsWorld::debugDrawWorld(); + + for (int i = 0; i < getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)getSoftBodyArray()[i]; + { + btSoftBodyHelpers::DrawFrame(psb, getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, getDebugDrawer(), getDrawFlags()); + } + } +} + +void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep) +{ + // Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again + // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep + // when there are multiple substeps + btMultiBodyDynamicsWorld::applyGravity(); + // integrate rigid body gravity + for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) + { + btRigidBody* rb = m_nonStaticRigidBodies[i]; + rb->integrateVelocities(timeStep); + } + + // integrate multibody gravity + { + forwardKinematics(); + clearMultiBodyConstraintForces(); + { + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + m_scratch_r.resize(bod->getNumLinks() + 1); + m_scratch_v.resize(bod->getNumLinks() + 1); + m_scratch_m.resize(bod->getNumLinks() + 1); + bool isConstraintPass = false; + { + if (!bod->isUsingRK4Integration()) + { + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, + m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } + else + { + btAssert(" RK4Integration is not supported"); + } + } + } + } + } + } + clearGravity(); +} + +void btDeformableMultiBodyDynamicsWorld::clearGravity() +{ + BT_PROFILE("btMultiBody clearGravity"); + // clear rigid body gravity + for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) + { + btRigidBody* body = m_nonStaticRigidBodies[i]; + if (body->isActive()) + { + body->clearGravity(); + } + } + // clear multibody gravity + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + bod->addBaseForce(-m_gravity * bod->getBaseMass()); + + for (int j = 0; j < bod->getNumLinks(); ++j) + { + bod->addLinkForce(j, -m_gravity * bod->getLinkMass(j)); + } + } + } +} + +void btDeformableMultiBodyDynamicsWorld::beforeSolverCallbacks(btScalar timeStep) +{ + if (0 != m_internalTickCallback) + { + (*m_internalTickCallback)(this, timeStep); + } + + if (0 != m_solverCallback) + { + (*m_solverCallback)(m_internalTime, this); + } +} + +void btDeformableMultiBodyDynamicsWorld::afterSolverCallbacks(btScalar timeStep) +{ + if (0 != m_solverCallback) + { + (*m_solverCallback)(m_internalTime, this); + } +} + +void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force) +{ + btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; + bool added = false; + for (int i = 0; i < forces.size(); ++i) + { + if (forces[i]->getForceType() == force->getForceType()) + { + forces[i]->addSoftBody(psb); + added = true; + break; + } + } + if (!added) + { + force->addSoftBody(psb); + force->setIndices(m_deformableBodySolver->m_objective->getIndices()); + forces.push_back(force); + } +} + +void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force) +{ + btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; + int removed_index = -1; + for (int i = 0; i < forces.size(); ++i) + { + if (forces[i]->getForceType() == force->getForceType()) + { + forces[i]->removeSoftBody(psb); + if (forces[i]->m_softBodies.size() == 0) + removed_index = i; + break; + } + } + if (removed_index >= 0) + forces.removeAtIndex(removed_index); +} + +void btDeformableMultiBodyDynamicsWorld::removeSoftBodyForce(btSoftBody* psb) +{ + btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; + for (int i = 0; i < forces.size(); ++i) + { + forces[i]->removeSoftBody(psb); + } +} + +void btDeformableMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body) +{ + removeSoftBodyForce(body); + m_softBodies.remove(body); + btCollisionWorld::removeCollisionObject(body); + // force a reinitialize so that node indices get updated. + m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1)); +} + +void btDeformableMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) +{ + btSoftBody* body = btSoftBody::upcast(collisionObject); + if (body) + removeSoftBody(body); + else + btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); +} + +int btDeformableMultiBodyDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) +{ + startProfiling(timeStep); + + int numSimulationSubSteps = 0; + + if (maxSubSteps) + { + //fixed timestep with interpolation + m_fixedTimeStep = fixedTimeStep; + m_localTime += timeStep; + if (m_localTime >= fixedTimeStep) + { + numSimulationSubSteps = int(m_localTime / fixedTimeStep); + m_localTime -= numSimulationSubSteps * fixedTimeStep; + } + } + else + { + //variable timestep + fixedTimeStep = timeStep; + m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep; + m_fixedTimeStep = 0; + if (btFuzzyZero(timeStep)) + { + numSimulationSubSteps = 0; + maxSubSteps = 0; + } + else + { + numSimulationSubSteps = 1; + maxSubSteps = 1; + } + } + + //process some debugging flags + if (getDebugDrawer()) + { + btIDebugDraw* debugDrawer = getDebugDrawer(); + gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; + } + if (numSimulationSubSteps) + { + //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt + int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; + + saveKinematicState(fixedTimeStep * clampedSimulationSteps); + + for (int i = 0; i < clampedSimulationSteps; i++) + { + internalSingleStepSimulation(fixedTimeStep); + synchronizeMotionStates(); + } + } + else + { + synchronizeMotionStates(); + } + + clearForces(); + +#ifndef BT_NO_PROFILE + CProfileManager::Increment_Frame_Counter(); +#endif //BT_NO_PROFILE + + return numSimulationSubSteps; +} diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h new file mode 100644 index 00000000000..4b7069aac7c --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -0,0 +1,316 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H +#define BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H + +#include "btSoftMultiBodyDynamicsWorld.h" +#include "btDeformableLagrangianForce.h" +#include "btDeformableMassSpringForce.h" +#include "btDeformableBodySolver.h" +#include "btDeformableMultiBodyConstraintSolver.h" +#include "btSoftBodyHelpers.h" +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" +#include <functional> +typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray; + +class btDeformableBodySolver; +class btDeformableLagrangianForce; +struct MultiBodyInplaceSolverIslandCallback; +struct DeformableBodyInplaceSolverIslandCallback; +class btDeformableMultiBodyConstraintSolver; + +typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray; + +class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld +{ + typedef btAlignedObjectArray<btVector3> TVStack; + ///Solver classes that encapsulate multiple deformable bodies for solving + btDeformableBodySolver* m_deformableBodySolver; + btSoftBodyArray m_softBodies; + int m_drawFlags; + bool m_drawNodeTree; + bool m_drawFaceTree; + bool m_drawClusterTree; + btSoftBodyWorldInfo m_sbi; + btScalar m_internalTime; + int m_ccdIterations; + bool m_implicit; + bool m_lineSearch; + bool m_useProjection; + DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback; + + typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world); + btSolverCallback m_solverCallback; + +protected: + virtual void internalSingleStepSimulation(btScalar timeStep); + + virtual void integrateTransforms(btScalar timeStep); + + void positionCorrection(btScalar timeStep); + + void solveConstraints(btScalar timeStep); + + void updateActivationState(btScalar timeStep); + + void clearGravity(); + +public: + btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0); + + virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)); + + virtual void debugDrawWorld(); + + void setSolverCallback(btSolverCallback cb) + { + m_solverCallback = cb; + } + + virtual ~btDeformableMultiBodyDynamicsWorld(); + + virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() + { + return (btMultiBodyDynamicsWorld*)(this); + } + + virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const + { + return (const btMultiBodyDynamicsWorld*)(this); + } + + virtual btDynamicsWorldType getWorldType() const + { + return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD; + } + + virtual void predictUnconstraintMotion(btScalar timeStep); + + virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); + + btSoftBodyArray& getSoftBodyArray() + { + return m_softBodies; + } + + const btSoftBodyArray& getSoftBodyArray() const + { + return m_softBodies; + } + + btSoftBodyWorldInfo& getWorldInfo() + { + return m_sbi; + } + + const btSoftBodyWorldInfo& getWorldInfo() const + { + return m_sbi; + } + + void reinitialize(btScalar timeStep); + + void applyRigidBodyGravity(btScalar timeStep); + + void beforeSolverCallbacks(btScalar timeStep); + + void afterSolverCallbacks(btScalar timeStep); + + void addForce(btSoftBody* psb, btDeformableLagrangianForce* force); + + void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force); + + void removeSoftBodyForce(btSoftBody* psb); + + void removeSoftBody(btSoftBody* body); + + void removeCollisionObject(btCollisionObject* collisionObject); + + int getDrawFlags() const { return (m_drawFlags); } + void setDrawFlags(int f) { m_drawFlags = f; } + + void setupConstraints(); + + void performDeformableCollisionDetection(); + + void solveMultiBodyConstraints(); + + void solveContactConstraints(); + + void sortConstraints(); + + void softBodySelfCollision(); + + void setImplicit(bool implicit) + { + m_implicit = implicit; + } + + void setLineSearch(bool lineSearch) + { + m_lineSearch = lineSearch; + } + + void setUseProjection(bool useProjection) + { + m_useProjection = useProjection; + } + + void applyRepulsionForce(btScalar timeStep); + + void performGeometricCollisions(btScalar timeStep); + + struct btDeformableSingleRayCallback : public btBroadphaseRayCallback + { + btVector3 m_rayFromWorld; + btVector3 m_rayToWorld; + btTransform m_rayFromTrans; + btTransform m_rayToTrans; + btVector3 m_hitNormal; + + const btDeformableMultiBodyDynamicsWorld* m_world; + btCollisionWorld::RayResultCallback& m_resultCallback; + + btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* 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 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 + btDeformableSingleRayCallback 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 rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback) const + { + if (collisionShape->isSoftBody()) + { + btSoftBody* softBody = btSoftBody::upcast(collisionObject); + if (softBody) + { + btSoftBody::sRayCast softResult; + if (softBody->rayFaceTest(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(); + { + 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); + } + } +}; + +#endif //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/extern/bullet2/src/BulletSoftBody/btDeformableNeoHookeanForce.h new file mode 100644 index 00000000000..60798c5bcd3 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -0,0 +1,420 @@ +/* +Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2019 Google Inc. http://bulletphysics.org +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_NEOHOOKEAN_H +#define BT_NEOHOOKEAN_H + +#include "btDeformableLagrangianForce.h" +#include "LinearMath/btQuickprof.h" +#include "LinearMath/btImplicitQRSVD.h" +// This energy is as described in https://graphics.pixar.com/library/StableElasticity/paper.pdf +class btDeformableNeoHookeanForce : public btDeformableLagrangianForce +{ +public: + typedef btAlignedObjectArray<btVector3> TVStack; + btScalar m_mu, m_lambda; // Lame Parameters + btScalar m_E, m_nu; // Young's modulus and Poisson ratio + btScalar m_mu_damp, m_lambda_damp; + btDeformableNeoHookeanForce() : m_mu(1), m_lambda(1) + { + btScalar damping = 0.05; + m_mu_damp = damping * m_mu; + m_lambda_damp = damping * m_lambda; + updateYoungsModulusAndPoissonRatio(); + } + + btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05) : m_mu(mu), m_lambda(lambda) + { + m_mu_damp = damping * m_mu; + m_lambda_damp = damping * m_lambda; + updateYoungsModulusAndPoissonRatio(); + } + + void updateYoungsModulusAndPoissonRatio() + { + // conversion from Lame Parameters to Young's modulus and Poisson ratio + // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters + m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu); + m_nu = m_lambda * 0.5 / (m_mu + m_lambda); + } + + void updateLameParameters() + { + // conversion from Young's modulus and Poisson ratio to Lame Parameters + // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters + m_mu = m_E * 0.5 / (1 + m_nu); + m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu)); + } + + void setYoungsModulus(btScalar E) + { + m_E = E; + updateLameParameters(); + } + + void setPoissonRatio(btScalar nu) + { + m_nu = nu; + updateLameParameters(); + } + + void setDamping(btScalar damping) + { + m_mu_damp = damping * m_mu; + m_lambda_damp = damping * m_lambda; + } + + void setLameParameters(btScalar mu, btScalar lambda) + { + m_mu = mu; + m_lambda = lambda; + updateYoungsModulusAndPoissonRatio(); + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + if (m_mu_damp == 0 && m_lambda_damp == 0) + return; + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; + btMatrix3x3 I; + I.setIdentity(); + btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0] + dF[1][1] + dF[2][2]) * m_lambda_damp; + // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); + btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose() * grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * df_on_node0; + force[id1] -= scale1 * df_on_node123.getColumn(0); + force[id2] -= scale1 * df_on_node123.getColumn(1); + force[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + + virtual double totalElasticEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetraScratches.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::TetraScratch& s = psb->m_tetraScratches[j]; + energy += tetra.m_element_measure * elasticEnergyDensity(s); + } + } + return energy; + } + + // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + int sz = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + sz = btMax(sz, psb->m_nodes[j].index); + } + } + TVStack dampingForce; + dampingForce.resize(sz + 1); + for (int i = 0; i < dampingForce.size(); ++i) + dampingForce[i].setZero(); + addScaledDampingForce(0.5, dampingForce); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + energy -= dampingForce[node.index].dot(node.m_v) / dt; + } + } + return energy; + } + + double elasticEnergyDensity(const btSoftBody::TetraScratch& s) + { + double density = 0; + density += m_mu * 0.5 * (s.m_trace - 3.); + density += m_lambda * 0.5 * (s.m_J - 1. - 0.75 * m_mu / m_lambda) * (s.m_J - 1. - 0.75 * m_mu / m_lambda); + density -= m_mu * 0.5 * log(s.m_trace + 1); + return density; + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + btScalar max_p = psb->m_cfg.m_maxStress; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btMatrix3x3 P; + firstPiola(psb->m_tetraScratches[j], P); +#ifdef USE_SVD + if (max_p > 0) + { + // since we want to clamp the principal stress to max_p, we only need to + // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p + btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2()); + if (trPTP > max_p * max_p) + { + btMatrix3x3 U, V; + btVector3 sigma; + singularValueDecomposition(P, U, sigma, V); + sigma[0] = btMin(sigma[0], max_p); + sigma[1] = btMin(sigma[1], max_p); + sigma[2] = btMin(sigma[2], max_p); + sigma[0] = btMax(sigma[0], -max_p); + sigma[1] = btMax(sigma[1], -max_p); + sigma[2] = btMax(sigma[2], -max_p); + btMatrix3x3 Sigma; + Sigma.setIdentity(); + Sigma[0][0] = sigma[0]; + Sigma[1][1] = sigma[1]; + Sigma[2][2] = sigma[2]; + P = U * Sigma * V.transpose(); + } + } +#endif + // btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); + btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; + + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + + // elastic force + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * force_on_node0; + force[id1] -= scale1 * force_on_node123.getColumn(0); + force[id2] -= scale1 * force_on_node123.getColumn(1); + force[id3] -= scale1 * force_on_node123.getColumn(2); + } + } + } + + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + if (m_mu_damp == 0 && m_lambda_damp == 0) + return; + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; + btMatrix3x3 I; + I.setIdentity(); + btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0] + dF[1][1] + dF[2][2]) * m_lambda_damp; + // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); + // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; + btMatrix3x3 dP; + firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); + // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + + // elastic force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + + void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) + { + btScalar c1 = (m_mu * (1. - 1. / (s.m_trace + 1.))); + btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu); + P = s.m_F * c1 + s.m_cofF * c2; + } + + // Let P be the first piola stress. + // This function calculates the dP = dP/dF * dF + void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) + { + btScalar c1 = m_mu * (1. - 1. / (s.m_trace + 1.)); + btScalar c2 = (2. * m_mu) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace))); + btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF)); + dP = dF * c1 + s.m_F * c2; + addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda * (s.m_J - 1.) - 0.75 * m_mu, dP); + dP += s.m_cofF * c3; + } + + // Let Q be the damping stress. + // This function calculates the dP = dQ/dF * dF + void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) + { + btScalar c1 = (m_mu_damp * (1. - 1. / (s.m_trace + 1.))); + btScalar c2 = ((2. * m_mu_damp) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace)))); + btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF)); + dP = dF * c1 + s.m_F * c2; + addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp * (s.m_J - 1.) - 0.75 * m_mu_damp, dP); + dP += s.m_cofF * c3; + } + + btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B) + { + btScalar ans = 0; + for (int i = 0; i < 3; ++i) + { + ans += A[i].dot(B[i]); + } + return ans; + } + + // Let C(A) be the cofactor of the matrix A + // Let H = the derivative of C(A) with respect to A evaluated at F = A + // This function calculates H*dF + void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M) + { + M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]); + M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]); + M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]); + M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]); + M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]); + M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]); + M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]); + M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]); + M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]); + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_NEOHOOKEAN_FORCE; + } +}; +#endif /* BT_NEOHOOKEAN_H */ diff --git a/extern/bullet2/src/BulletSoftBody/btKrylovSolver.h b/extern/bullet2/src/BulletSoftBody/btKrylovSolver.h new file mode 100644 index 00000000000..59126b47ae2 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btKrylovSolver.h @@ -0,0 +1,107 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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_KRYLOV_SOLVER_H +#define BT_KRYLOV_SOLVER_H +#include <iostream> +#include <cmath> +#include <limits> +#include <LinearMath/btAlignedObjectArray.h> +#include <LinearMath/btVector3.h> +#include <LinearMath/btScalar.h> +#include "LinearMath/btQuickprof.h" + +template <class MatrixX> +class btKrylovSolver +{ + typedef btAlignedObjectArray<btVector3> TVStack; + +public: + int m_maxIterations; + btScalar m_tolerance; + btKrylovSolver(int maxIterations, btScalar tolerance) + : m_maxIterations(maxIterations), m_tolerance(tolerance) + { + } + + virtual ~btKrylovSolver() {} + + virtual int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) = 0; + + virtual void reinitialize(const TVStack& b) = 0; + + virtual SIMD_FORCE_INLINE TVStack sub(const TVStack& a, const TVStack& b) + { + // c = a-b + btAssert(a.size() == b.size()); + TVStack c; + c.resize(a.size()); + for (int i = 0; i < a.size(); ++i) + { + c[i] = a[i] - b[i]; + } + return c; + } + + virtual SIMD_FORCE_INLINE btScalar squaredNorm(const TVStack& a) + { + return dot(a, a); + } + + virtual SIMD_FORCE_INLINE btScalar norm(const TVStack& a) + { + btScalar ret = 0; + for (int i = 0; i < a.size(); ++i) + { + for (int d = 0; d < 3; ++d) + { + ret = btMax(ret, btFabs(a[i][d])); + } + } + return ret; + } + + virtual SIMD_FORCE_INLINE btScalar dot(const TVStack& a, const TVStack& b) + { + btScalar ans(0); + for (int i = 0; i < a.size(); ++i) + ans += a[i].dot(b[i]); + return ans; + } + + virtual SIMD_FORCE_INLINE void multAndAddTo(btScalar s, const TVStack& a, TVStack& result) + { + // result += s*a + btAssert(a.size() == result.size()); + for (int i = 0; i < a.size(); ++i) + result[i] += s * a[i]; + } + + virtual SIMD_FORCE_INLINE TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b) + { + // result = a*s + b + TVStack result; + result.resize(a.size()); + for (int i = 0; i < a.size(); ++i) + result[i] = s * a[i] + b[i]; + return result; + } + + virtual SIMD_FORCE_INLINE void setTolerance(btScalar tolerance) + { + m_tolerance = tolerance; + } +}; +#endif /* BT_KRYLOV_SOLVER_H */ diff --git a/extern/bullet2/src/BulletSoftBody/btPreconditioner.h b/extern/bullet2/src/BulletSoftBody/btPreconditioner.h new file mode 100644 index 00000000000..21c1106a426 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btPreconditioner.h @@ -0,0 +1,285 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + 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_PRECONDITIONER_H +#define BT_PRECONDITIONER_H + +class Preconditioner +{ +public: + typedef btAlignedObjectArray<btVector3> TVStack; + virtual void operator()(const TVStack& x, TVStack& b) = 0; + virtual void reinitialize(bool nodeUpdated) = 0; + virtual ~Preconditioner() {} +}; + +class DefaultPreconditioner : public Preconditioner +{ +public: + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + for (int i = 0; i < b.size(); ++i) + b[i] = x[i]; + } + virtual void reinitialize(bool nodeUpdated) + { + } + + virtual ~DefaultPreconditioner() {} +}; + +class MassPreconditioner : public Preconditioner +{ + btAlignedObjectArray<btScalar> m_inv_mass; + const btAlignedObjectArray<btSoftBody*>& m_softBodies; + +public: + MassPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies) + : m_softBodies(softBodies) + { + } + + virtual void reinitialize(bool nodeUpdated) + { + if (nodeUpdated) + { + m_inv_mass.clear(); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + m_inv_mass.push_back(psb->m_nodes[j].m_im); + } + } + } + + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + btAssert(m_inv_mass.size() <= x.size()); + for (int i = 0; i < m_inv_mass.size(); ++i) + { + b[i] = x[i] * m_inv_mass[i]; + } + for (int i = m_inv_mass.size(); i < b.size(); ++i) + { + b[i] = x[i]; + } + } +}; + +class KKTPreconditioner : public Preconditioner +{ + const btAlignedObjectArray<btSoftBody*>& m_softBodies; + const btDeformableContactProjection& m_projections; + const btAlignedObjectArray<btDeformableLagrangianForce*>& m_lf; + TVStack m_inv_A, m_inv_S; + const btScalar& m_dt; + const bool& m_implicit; + +public: + KKTPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies, const btDeformableContactProjection& projections, const btAlignedObjectArray<btDeformableLagrangianForce*>& lf, const btScalar& dt, const bool& implicit) + : m_softBodies(softBodies), m_projections(projections), m_lf(lf), m_dt(dt), m_implicit(implicit) + { + } + + virtual void reinitialize(bool nodeUpdated) + { + if (nodeUpdated) + { + int num_nodes = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + num_nodes += psb->m_nodes.size(); + } + m_inv_A.resize(num_nodes); + } + buildDiagonalA(m_inv_A); + for (int i = 0; i < m_inv_A.size(); ++i) + { + // printf("A[%d] = %f, %f, %f \n", i, m_inv_A[i][0], m_inv_A[i][1], m_inv_A[i][2]); + for (int d = 0; d < 3; ++d) + { + m_inv_A[i][d] = (m_inv_A[i][d] == 0) ? 0.0 : 1.0 / m_inv_A[i][d]; + } + } + m_inv_S.resize(m_projections.m_lagrangeMultipliers.size()); + // printf("S.size() = %d \n", m_inv_S.size()); + buildDiagonalS(m_inv_A, m_inv_S); + for (int i = 0; i < m_inv_S.size(); ++i) + { + // printf("S[%d] = %f, %f, %f \n", i, m_inv_S[i][0], m_inv_S[i][1], m_inv_S[i][2]); + for (int d = 0; d < 3; ++d) + { + m_inv_S[i][d] = (m_inv_S[i][d] == 0) ? 0.0 : 1.0 / m_inv_S[i][d]; + } + } + } + + void buildDiagonalA(TVStack& diagA) const + { + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + diagA[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : btVector3(1.0 / node.m_im, 1.0 / node.m_im, 1.0 / node.m_im); + ++counter; + } + } + if (m_implicit) + { + printf("implicit not implemented\n"); + btAssert(false); + } + for (int i = 0; i < m_lf.size(); ++i) + { + // add damping matrix + m_lf[i]->buildDampingForceDifferentialDiagonal(-m_dt, diagA); + } + } + + void buildDiagonalS(const TVStack& inv_A, TVStack& diagS) + { + for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) + { + // S[k,k] = e_k^T * C A_d^-1 C^T * e_k + const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; + btVector3& t = diagS[c]; + t.setZero(); + for (int j = 0; j < lm.m_num_constraints; ++j) + { + for (int i = 0; i < lm.m_num_nodes; ++i) + { + for (int d = 0; d < 3; ++d) + { + t[j] += inv_A[lm.m_indices[i]][d] * lm.m_dirs[j][d] * lm.m_dirs[j][d] * lm.m_weights[i] * lm.m_weights[i]; + } + } + } + } + } +//#define USE_FULL_PRECONDITIONER +#ifndef USE_FULL_PRECONDITIONER + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i] = x[i] * m_inv_A[i]; + } + int offset = m_inv_A.size(); + for (int i = 0; i < m_inv_S.size(); ++i) + { + b[i + offset] = x[i + offset] * m_inv_S[i]; + } + } +#else + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + int offset = m_inv_A.size(); + + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i] = x[i] * m_inv_A[i]; + } + + for (int i = 0; i < m_inv_S.size(); ++i) + { + b[i + offset].setZero(); + } + + for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) + { + const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; + // C * x + for (int d = 0; d < lm.m_num_constraints; ++d) + { + for (int i = 0; i < lm.m_num_nodes; ++i) + { + b[offset + c][d] += lm.m_weights[i] * b[lm.m_indices[i]].dot(lm.m_dirs[d]); + } + } + } + + for (int i = 0; i < m_inv_S.size(); ++i) + { + b[i + offset] = b[i + offset] * m_inv_S[i]; + } + + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i].setZero(); + } + + for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) + { + // C^T * lambda + const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; + for (int i = 0; i < lm.m_num_nodes; ++i) + { + for (int j = 0; j < lm.m_num_constraints; ++j) + { + b[lm.m_indices[i]] += b[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j]; + } + } + } + + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i] = (x[i] - b[i]) * m_inv_A[i]; + } + + TVStack t; + t.resize(b.size()); + for (int i = 0; i < m_inv_S.size(); ++i) + { + t[i + offset] = x[i + offset] * m_inv_S[i]; + } + for (int i = 0; i < m_inv_A.size(); ++i) + { + t[i].setZero(); + } + for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) + { + // C^T * lambda + const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; + for (int i = 0; i < lm.m_num_nodes; ++i) + { + for (int j = 0; j < lm.m_num_constraints; ++j) + { + t[lm.m_indices[i]] += t[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j]; + } + } + } + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i] += t[i] * m_inv_A[i]; + } + + for (int i = 0; i < m_inv_S.size(); ++i) + { + b[i + offset] -= x[i + offset] * m_inv_S[i]; + } + } +#endif +}; + +#endif /* BT_PRECONDITIONER_H */ diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp index 51f4b33d034..0597a8016cd 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp @@ -18,442 +18,690 @@ subject to the following restrictions: #include "BulletSoftBody/btSoftBodySolvers.h" #include "btSoftBodyData.h" #include "LinearMath/btSerializer.h" - +#include "LinearMath/btImplicitQRSVD.h" +#include "LinearMath/btAlignedAllocator.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include <iostream> +// +static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& leafNodes, btAlignedObjectArray<btAlignedObjectArray<int> >& adj) +{ + int N = leafNodes.size(); + if (N == 0) + { + return NULL; + } + while (N > 1) + { + btAlignedObjectArray<bool> marked; + btAlignedObjectArray<btDbvtNode*> newLeafNodes; + btAlignedObjectArray<std::pair<int, int> > childIds; + btAlignedObjectArray<btAlignedObjectArray<int> > newAdj; + marked.resize(N); + for (int i = 0; i < N; ++i) + marked[i] = false; + + // pair adjacent nodes into new(parent) node + for (int i = 0; i < N; ++i) + { + if (marked[i]) + continue; + bool merged = false; + for (int j = 0; j < adj[i].size(); ++j) + { + int n = adj[i][j]; + if (!marked[adj[i][j]]) + { + btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode(); + node->parent = NULL; + node->childs[0] = leafNodes[i]; + node->childs[1] = leafNodes[n]; + leafNodes[i]->parent = node; + leafNodes[n]->parent = node; + newLeafNodes.push_back(node); + childIds.push_back(std::make_pair(i, n)); + merged = true; + marked[n] = true; + break; + } + } + if (!merged) + { + newLeafNodes.push_back(leafNodes[i]); + childIds.push_back(std::make_pair(i, -1)); + } + marked[i] = true; + } + // update adjacency matrix + newAdj.resize(newLeafNodes.size()); + for (int i = 0; i < newLeafNodes.size(); ++i) + { + for (int j = i + 1; j < newLeafNodes.size(); ++j) + { + bool neighbor = false; + const btAlignedObjectArray<int>& leftChildNeighbors = adj[childIds[i].first]; + for (int k = 0; k < leftChildNeighbors.size(); ++k) + { + if (leftChildNeighbors[k] == childIds[j].first || leftChildNeighbors[k] == childIds[j].second) + { + neighbor = true; + break; + } + } + if (!neighbor && childIds[i].second != -1) + { + const btAlignedObjectArray<int>& rightChildNeighbors = adj[childIds[i].second]; + for (int k = 0; k < rightChildNeighbors.size(); ++k) + { + if (rightChildNeighbors[k] == childIds[j].first || rightChildNeighbors[k] == childIds[j].second) + { + neighbor = true; + break; + } + } + } + if (neighbor) + { + newAdj[i].push_back(j); + newAdj[j].push_back(i); + } + } + } + leafNodes = newLeafNodes; + //this assignment leaks memory, the assignment doesn't do a deep copy, for now a manual copy + //adj = newAdj; + adj.clear(); + adj.resize(newAdj.size()); + for (int i = 0; i < newAdj.size(); i++) + { + for (int j = 0; j < newAdj[i].size(); j++) + { + adj[i].push_back(newAdj[i][j]); + } + } + N = leafNodes.size(); + } + return leafNodes[0]; +} // -btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m) -:m_softBodySolver(0),m_worldInfo(worldInfo) -{ - /* Init */ +btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m) + : m_softBodySolver(0), m_worldInfo(worldInfo) +{ + /* Init */ initDefaults(); - /* Default material */ - Material* pm=appendMaterial(); - pm->m_kLST = 1; - pm->m_kAST = 1; - pm->m_kVST = 1; - pm->m_flags = fMaterial::Default; + /* Default material */ + Material* pm = appendMaterial(); + pm->m_kLST = 1; + pm->m_kAST = 1; + pm->m_kVST = 1; + pm->m_flags = fMaterial::Default; - /* Nodes */ - const btScalar margin=getCollisionShape()->getMargin(); + /* Nodes */ + const btScalar margin = getCollisionShape()->getMargin(); m_nodes.resize(node_count); - for(int i=0,ni=node_count;i<ni;++i) - { - Node& n=m_nodes[i]; + m_X.resize(node_count); + for (int i = 0, ni = node_count; i < ni; ++i) + { + Node& n = m_nodes[i]; ZeroInitialize(n); - n.m_x = x?*x++:btVector3(0,0,0); - n.m_q = n.m_x; - n.m_im = m?*m++:1; - n.m_im = n.m_im>0?1/n.m_im:0; - n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n); - n.m_material= pm; + n.m_x = x ? *x++ : btVector3(0, 0, 0); + n.m_q = n.m_x; + n.m_im = m ? *m++ : 1; + n.m_im = n.m_im > 0 ? 1 / n.m_im : 0; + n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n); + n.m_material = pm; + m_X[i] = n.m_x; } - updateBounds(); - + updateBounds(); + setCollisionQuadrature(3); + m_fdbvnt = 0; } -btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo) -:m_worldInfo(worldInfo) +btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo) + : m_worldInfo(worldInfo) { initDefaults(); } - -void btSoftBody::initDefaults() -{ - m_internalType = CO_SOFT_BODY; - m_cfg.aeromodel = eAeroModel::V_Point; - m_cfg.kVCF = 1; - m_cfg.kDG = 0; - m_cfg.kLF = 0; - m_cfg.kDP = 0; - m_cfg.kPR = 0; - m_cfg.kVC = 0; - m_cfg.kDF = (btScalar)0.2; - m_cfg.kMT = 0; - m_cfg.kCHR = (btScalar)1.0; - m_cfg.kKHR = (btScalar)0.1; - m_cfg.kSHR = (btScalar)1.0; - m_cfg.kAHR = (btScalar)0.7; - m_cfg.kSRHR_CL = (btScalar)0.1; - m_cfg.kSKHR_CL = (btScalar)1; - m_cfg.kSSHR_CL = (btScalar)0.5; - m_cfg.kSR_SPLT_CL = (btScalar)0.5; - m_cfg.kSK_SPLT_CL = (btScalar)0.5; - m_cfg.kSS_SPLT_CL = (btScalar)0.5; - m_cfg.maxvolume = (btScalar)1; - m_cfg.timescale = 1; - m_cfg.viterations = 0; - m_cfg.piterations = 1; - m_cfg.diterations = 0; - m_cfg.citerations = 4; - m_cfg.collisions = fCollision::Default; - m_pose.m_bvolume = false; - m_pose.m_bframe = false; - m_pose.m_volume = 0; - m_pose.m_com = btVector3(0,0,0); +void btSoftBody::initDefaults() +{ + m_internalType = CO_SOFT_BODY; + m_cfg.aeromodel = eAeroModel::V_Point; + m_cfg.kVCF = 1; + m_cfg.kDG = 0; + m_cfg.kLF = 0; + m_cfg.kDP = 0; + m_cfg.kPR = 0; + m_cfg.kVC = 0; + m_cfg.kDF = (btScalar)0.2; + m_cfg.kMT = 0; + m_cfg.kCHR = (btScalar)1.0; + m_cfg.kKHR = (btScalar)0.1; + m_cfg.kSHR = (btScalar)1.0; + m_cfg.kAHR = (btScalar)0.7; + m_cfg.kSRHR_CL = (btScalar)0.1; + m_cfg.kSKHR_CL = (btScalar)1; + m_cfg.kSSHR_CL = (btScalar)0.5; + m_cfg.kSR_SPLT_CL = (btScalar)0.5; + m_cfg.kSK_SPLT_CL = (btScalar)0.5; + m_cfg.kSS_SPLT_CL = (btScalar)0.5; + m_cfg.maxvolume = (btScalar)1; + m_cfg.timescale = 1; + m_cfg.viterations = 0; + m_cfg.piterations = 1; + m_cfg.diterations = 0; + m_cfg.citerations = 4; + m_cfg.drag = 0; + m_cfg.m_maxStress = 0; + m_cfg.collisions = fCollision::Default; + m_pose.m_bvolume = false; + m_pose.m_bframe = false; + m_pose.m_volume = 0; + m_pose.m_com = btVector3(0, 0, 0); m_pose.m_rot.setIdentity(); m_pose.m_scl.setIdentity(); - m_tag = 0; - m_timeacc = 0; - m_bUpdateRtCst = true; - m_bounds[0] = btVector3(0,0,0); - m_bounds[1] = btVector3(0,0,0); + m_tag = 0; + m_timeacc = 0; + m_bUpdateRtCst = true; + m_bounds[0] = btVector3(0, 0, 0); + m_bounds[1] = btVector3(0, 0, 0); m_worldTransform.setIdentity(); setSolver(eSolverPresets::Positions); - - /* Collision shape */ + + /* Collision shape */ ///for now, create a collision shape internally m_collisionShape = new btSoftBodyCollisionShape(this); m_collisionShape->setMargin(0.25f); - - m_initialWorldTransform.setIdentity(); - m_windVelocity = btVector3(0,0,0); + m_worldTransform.setIdentity(); + + m_windVelocity = btVector3(0, 0, 0); m_restLengthScale = btScalar(1.0); + m_dampingCoefficient = 1.0; + m_sleepingThreshold = .04; + m_useSelfCollision = false; + m_collisionFlags = 0; + m_softSoftCollision = false; + m_maxSpeedSquared = 0; + m_repulsionStiffness = 0.5; + m_gravityFactor = 1; + m_fdbvnt = 0; } // btSoftBody::~btSoftBody() { //for now, delete the internal shape - delete m_collisionShape; + delete m_collisionShape; int i; releaseClusters(); - for(i=0;i<m_materials.size();++i) + for (i = 0; i < m_materials.size(); ++i) btAlignedFree(m_materials[i]); - for(i=0;i<m_joints.size();++i) + for (i = 0; i < m_joints.size(); ++i) btAlignedFree(m_joints[i]); + if (m_fdbvnt) + delete m_fdbvnt; } // -bool btSoftBody::checkLink(int node0,int node1) const +bool btSoftBody::checkLink(int node0, int node1) const { - return(checkLink(&m_nodes[node0],&m_nodes[node1])); + return (checkLink(&m_nodes[node0], &m_nodes[node1])); } // -bool btSoftBody::checkLink(const Node* node0,const Node* node1) const +bool btSoftBody::checkLink(const Node* node0, const Node* node1) const { - const Node* n[]={node0,node1}; - for(int i=0,ni=m_links.size();i<ni;++i) + const Node* n[] = {node0, node1}; + for (int i = 0, ni = m_links.size(); i < ni; ++i) { - const Link& l=m_links[i]; - if( (l.m_n[0]==n[0]&&l.m_n[1]==n[1])|| - (l.m_n[0]==n[1]&&l.m_n[1]==n[0])) + const Link& l = m_links[i]; + if ((l.m_n[0] == n[0] && l.m_n[1] == n[1]) || + (l.m_n[0] == n[1] && l.m_n[1] == n[0])) { - return(true); + return (true); } } - return(false); + return (false); } // -bool btSoftBody::checkFace(int node0,int node1,int node2) const +bool btSoftBody::checkFace(int node0, int node1, int node2) const { - const Node* n[]={ &m_nodes[node0], - &m_nodes[node1], - &m_nodes[node2]}; - for(int i=0,ni=m_faces.size();i<ni;++i) + const Node* n[] = {&m_nodes[node0], + &m_nodes[node1], + &m_nodes[node2]}; + for (int i = 0, ni = m_faces.size(); i < ni; ++i) { - const Face& f=m_faces[i]; - int c=0; - for(int j=0;j<3;++j) + const Face& f = m_faces[i]; + int c = 0; + for (int j = 0; j < 3; ++j) { - if( (f.m_n[j]==n[0])|| - (f.m_n[j]==n[1])|| - (f.m_n[j]==n[2])) c|=1<<j; else break; + if ((f.m_n[j] == n[0]) || + (f.m_n[j] == n[1]) || + (f.m_n[j] == n[2])) + c |= 1 << j; + else + break; } - if(c==7) return(true); + if (c == 7) return (true); } - return(false); + return (false); } // -btSoftBody::Material* btSoftBody::appendMaterial() +btSoftBody::Material* btSoftBody::appendMaterial() { - Material* pm=new(btAlignedAlloc(sizeof(Material),16)) Material(); - if(m_materials.size()>0) - *pm=*m_materials[0]; + Material* pm = new (btAlignedAlloc(sizeof(Material), 16)) Material(); + if (m_materials.size() > 0) + *pm = *m_materials[0]; else ZeroInitialize(*pm); m_materials.push_back(pm); - return(pm); + return (pm); } // -void btSoftBody::appendNote( const char* text, - const btVector3& o, - const btVector4& c, - Node* n0, - Node* n1, - Node* n2, - Node* n3) +void btSoftBody::appendNote(const char* text, + const btVector3& o, + const btVector4& c, + Node* n0, + Node* n1, + Node* n2, + Node* n3) { - Note n; + Note n; ZeroInitialize(n); - n.m_rank = 0; - n.m_text = text; - n.m_offset = o; - n.m_coords[0] = c.x(); - n.m_coords[1] = c.y(); - n.m_coords[2] = c.z(); - n.m_coords[3] = c.w(); - n.m_nodes[0] = n0;n.m_rank+=n0?1:0; - n.m_nodes[1] = n1;n.m_rank+=n1?1:0; - n.m_nodes[2] = n2;n.m_rank+=n2?1:0; - n.m_nodes[3] = n3;n.m_rank+=n3?1:0; + n.m_rank = 0; + n.m_text = text; + n.m_offset = o; + n.m_coords[0] = c.x(); + n.m_coords[1] = c.y(); + n.m_coords[2] = c.z(); + n.m_coords[3] = c.w(); + n.m_nodes[0] = n0; + n.m_rank += n0 ? 1 : 0; + n.m_nodes[1] = n1; + n.m_rank += n1 ? 1 : 0; + n.m_nodes[2] = n2; + n.m_rank += n2 ? 1 : 0; + n.m_nodes[3] = n3; + n.m_rank += n3 ? 1 : 0; m_notes.push_back(n); } // -void btSoftBody::appendNote( const char* text, - const btVector3& o, - Node* feature) +void btSoftBody::appendNote(const char* text, + const btVector3& o, + Node* feature) { - appendNote(text,o,btVector4(1,0,0,0),feature); + appendNote(text, o, btVector4(1, 0, 0, 0), feature); } // -void btSoftBody::appendNote( const char* text, - const btVector3& o, - Link* feature) +void btSoftBody::appendNote(const char* text, + const btVector3& o, + Link* feature) { - static const btScalar w=1/(btScalar)2; - appendNote(text,o,btVector4(w,w,0,0), feature->m_n[0], - feature->m_n[1]); + static const btScalar w = 1 / (btScalar)2; + appendNote(text, o, btVector4(w, w, 0, 0), feature->m_n[0], + feature->m_n[1]); } // -void btSoftBody::appendNote( const char* text, - const btVector3& o, - Face* feature) +void btSoftBody::appendNote(const char* text, + const btVector3& o, + Face* feature) { - static const btScalar w=1/(btScalar)3; - appendNote(text,o,btVector4(w,w,w,0), feature->m_n[0], - feature->m_n[1], - feature->m_n[2]); + static const btScalar w = 1 / (btScalar)3; + appendNote(text, o, btVector4(w, w, w, 0), feature->m_n[0], + feature->m_n[1], + feature->m_n[2]); } // -void btSoftBody::appendNode( const btVector3& x,btScalar m) +void btSoftBody::appendNode(const btVector3& x, btScalar m) { - if(m_nodes.capacity()==m_nodes.size()) + if (m_nodes.capacity() == m_nodes.size()) { pointersToIndices(); - m_nodes.reserve(m_nodes.size()*2+1); + m_nodes.reserve(m_nodes.size() * 2 + 1); indicesToPointers(); } - const btScalar margin=getCollisionShape()->getMargin(); + const btScalar margin = getCollisionShape()->getMargin(); m_nodes.push_back(Node()); - Node& n=m_nodes[m_nodes.size()-1]; + Node& n = m_nodes[m_nodes.size() - 1]; ZeroInitialize(n); - n.m_x = x; - n.m_q = n.m_x; - n.m_im = m>0?1/m:0; - n.m_material = m_materials[0]; - n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n); + n.m_x = x; + n.m_q = n.m_x; + n.m_im = m > 0 ? 1 / m : 0; + n.m_material = m_materials[0]; + n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n); } // -void btSoftBody::appendLink(int model,Material* mat) +void btSoftBody::appendLink(int model, Material* mat) { - Link l; - if(model>=0) - l=m_links[model]; + Link l; + if (model >= 0) + l = m_links[model]; else - { ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; } + { + ZeroInitialize(l); + l.m_material = mat ? mat : m_materials[0]; + } m_links.push_back(l); } // -void btSoftBody::appendLink( int node0, - int node1, - Material* mat, - bool bcheckexist) +void btSoftBody::appendLink(int node0, + int node1, + Material* mat, + bool bcheckexist) { - appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist); + appendLink(&m_nodes[node0], &m_nodes[node1], mat, bcheckexist); } // -void btSoftBody::appendLink( Node* node0, - Node* node1, - Material* mat, - bool bcheckexist) +void btSoftBody::appendLink(Node* node0, + Node* node1, + Material* mat, + bool bcheckexist) { - if((!bcheckexist)||(!checkLink(node0,node1))) + if ((!bcheckexist) || (!checkLink(node0, node1))) { - appendLink(-1,mat); - Link& l=m_links[m_links.size()-1]; - l.m_n[0] = node0; - l.m_n[1] = node1; - l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length(); - m_bUpdateRtCst=true; + appendLink(-1, mat); + Link& l = m_links[m_links.size() - 1]; + l.m_n[0] = node0; + l.m_n[1] = node1; + l.m_rl = (l.m_n[0]->m_x - l.m_n[1]->m_x).length(); + m_bUpdateRtCst = true; } } // -void btSoftBody::appendFace(int model,Material* mat) +void btSoftBody::appendFace(int model, Material* mat) { - Face f; - if(model>=0) - { f=m_faces[model]; } + Face f; + if (model >= 0) + { + f = m_faces[model]; + } else - { ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; } + { + ZeroInitialize(f); + f.m_material = mat ? mat : m_materials[0]; + } m_faces.push_back(f); } // -void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat) +void btSoftBody::appendFace(int node0, int node1, int node2, Material* mat) { - if (node0==node1) + if (node0 == node1) return; - if (node1==node2) + if (node1 == node2) return; - if (node2==node0) + if (node2 == node0) return; - appendFace(-1,mat); - Face& f=m_faces[m_faces.size()-1]; - btAssert(node0!=node1); - btAssert(node1!=node2); - btAssert(node2!=node0); - f.m_n[0] = &m_nodes[node0]; - f.m_n[1] = &m_nodes[node1]; - f.m_n[2] = &m_nodes[node2]; - f.m_ra = AreaOf( f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x); - m_bUpdateRtCst=true; + appendFace(-1, mat); + Face& f = m_faces[m_faces.size() - 1]; + btAssert(node0 != node1); + btAssert(node1 != node2); + btAssert(node2 != node0); + f.m_n[0] = &m_nodes[node0]; + f.m_n[1] = &m_nodes[node1]; + f.m_n[2] = &m_nodes[node2]; + f.m_ra = AreaOf(f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x); + m_bUpdateRtCst = true; } // -void btSoftBody::appendTetra(int model,Material* mat) +void btSoftBody::appendTetra(int model, Material* mat) { -Tetra t; -if(model>=0) - t=m_tetras[model]; + Tetra t; + if (model >= 0) + t = m_tetras[model]; else - { ZeroInitialize(t);t.m_material=mat?mat:m_materials[0]; } -m_tetras.push_back(t); + { + ZeroInitialize(t); + t.m_material = mat ? mat : m_materials[0]; + } + m_tetras.push_back(t); } // -void btSoftBody::appendTetra(int node0, - int node1, - int node2, - int node3, - Material* mat) +void btSoftBody::appendTetra(int node0, + int node1, + int node2, + int node3, + Material* mat) { - appendTetra(-1,mat); - Tetra& t=m_tetras[m_tetras.size()-1]; - t.m_n[0] = &m_nodes[node0]; - t.m_n[1] = &m_nodes[node1]; - t.m_n[2] = &m_nodes[node2]; - t.m_n[3] = &m_nodes[node3]; - t.m_rv = VolumeOf(t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x); - m_bUpdateRtCst=true; + appendTetra(-1, mat); + Tetra& t = m_tetras[m_tetras.size() - 1]; + t.m_n[0] = &m_nodes[node0]; + t.m_n[1] = &m_nodes[node1]; + t.m_n[2] = &m_nodes[node2]; + t.m_n[3] = &m_nodes[node3]; + t.m_rv = VolumeOf(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x); + m_bUpdateRtCst = true; } // -void btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisionBetweenLinkedBodies,btScalar influence) +void btSoftBody::appendAnchor(int node, btRigidBody* body, bool disableCollisionBetweenLinkedBodies, btScalar influence) { - btVector3 local = body->getWorldTransform().inverse()*m_nodes[node].m_x; - appendAnchor(node,body,local,disableCollisionBetweenLinkedBodies,influence); + btVector3 local = body->getWorldTransform().inverse() * m_nodes[node].m_x; + appendAnchor(node, body, local, disableCollisionBetweenLinkedBodies, influence); } // -void btSoftBody::appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies,btScalar influence) +void btSoftBody::appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies, btScalar influence) { if (disableCollisionBetweenLinkedBodies) { - if (m_collisionDisabledObjects.findLinearSearch(body)==m_collisionDisabledObjects.size()) + if (m_collisionDisabledObjects.findLinearSearch(body) == m_collisionDisabledObjects.size()) { m_collisionDisabledObjects.push_back(body); } } - Anchor a; - a.m_node = &m_nodes[node]; - a.m_body = body; - a.m_local = localPivot; - a.m_node->m_battach = 1; + Anchor a; + a.m_node = &m_nodes[node]; + a.m_body = body; + a.m_local = localPivot; + a.m_node->m_battach = 1; a.m_influence = influence; m_anchors.push_back(a); } // -void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1) +void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body) +{ + DeformableNodeRigidAnchor c; + btSoftBody::Node& n = m_nodes[node]; + const btScalar ima = n.m_im; + const btScalar imb = body->getInvMass(); + btVector3 nrm; + const btCollisionShape* shp = body->getCollisionShape(); + const btTransform& wtr = body->getWorldTransform(); + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(m_nodes[node].m_x), + shp, + nrm, + 0); + + c.m_cti.m_colObj = body; + c.m_cti.m_normal = wtr.getBasis() * nrm; + c.m_cti.m_offset = dst; + c.m_node = &m_nodes[node]; + const btScalar fc = m_cfg.kDF * body->getFriction(); + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR; + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = body->getInvInertiaTensorWorld(); + const btVector3 ra = n.m_x - wtr.getOrigin(); + + c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + c.m_c1 = ra; + c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x; + c.m_node->m_battach = 1; + m_deformableAnchors.push_back(c); +} + +void btSoftBody::removeAnchor(int node) +{ + const btSoftBody::Node& n = m_nodes[node]; + for (int i = 0; i < m_deformableAnchors.size();) + { + const DeformableNodeRigidAnchor& c = m_deformableAnchors[i]; + if (c.m_node == &n) + { + m_deformableAnchors.removeAtIndex(i); + } + else + { + i++; + } + } +} + +// +void btSoftBody::appendDeformableAnchor(int node, btMultiBodyLinkCollider* link) +{ + DeformableNodeRigidAnchor c; + btSoftBody::Node& n = m_nodes[node]; + const btScalar ima = n.m_im; + btVector3 nrm; + const btCollisionShape* shp = link->getCollisionShape(); + const btTransform& wtr = link->getWorldTransform(); + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(m_nodes[node].m_x), + shp, + nrm, + 0); + c.m_cti.m_colObj = link; + c.m_cti.m_normal = wtr.getBasis() * nrm; + c.m_cti.m_offset = dst; + c.m_node = &m_nodes[node]; + const btScalar fc = m_cfg.kDF * link->getFriction(); + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = link->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR; + btVector3 normal = c.m_cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(link, jacobianData_normal, c.m_node->m_x, normal); + findJacobian(link, jacobianData_t1, c.m_node->m_x, t1); + findJacobian(link, jacobianData_t2, c.m_node->m_x, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = link->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + c.m_c0 = rot.transpose() * local_impulse_matrix * rot; + c.jacobianData_normal = jacobianData_normal; + c.jacobianData_t1 = jacobianData_t1; + c.jacobianData_t2 = jacobianData_t2; + c.t1 = t1; + c.t2 = t2; + const btVector3 ra = n.m_x - wtr.getOrigin(); + c.m_c1 = ra; + c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x; + c.m_node->m_battach = 1; + m_deformableAnchors.push_back(c); +} +// +void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1) { - LJoint* pj = new(btAlignedAlloc(sizeof(LJoint),16)) LJoint(); - pj->m_bodies[0] = body0; - pj->m_bodies[1] = body1; - pj->m_refs[0] = pj->m_bodies[0].xform().inverse()*specs.position; - pj->m_refs[1] = pj->m_bodies[1].xform().inverse()*specs.position; - pj->m_cfm = specs.cfm; - pj->m_erp = specs.erp; - pj->m_split = specs.split; + LJoint* pj = new (btAlignedAlloc(sizeof(LJoint), 16)) LJoint(); + pj->m_bodies[0] = body0; + pj->m_bodies[1] = body1; + pj->m_refs[0] = pj->m_bodies[0].xform().inverse() * specs.position; + pj->m_refs[1] = pj->m_bodies[1].xform().inverse() * specs.position; + pj->m_cfm = specs.cfm; + pj->m_erp = specs.erp; + pj->m_split = specs.split; m_joints.push_back(pj); } // -void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body) +void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Body body) { - appendLinearJoint(specs,m_clusters[0],body); + appendLinearJoint(specs, m_clusters[0], body); } // -void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body) +void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, btSoftBody* body) { - appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]); + appendLinearJoint(specs, m_clusters[0], body->m_clusters[0]); } // -void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1) +void btSoftBody::appendAngularJoint(const AJoint::Specs& specs, Cluster* body0, Body body1) { - AJoint* pj = new(btAlignedAlloc(sizeof(AJoint),16)) AJoint(); - pj->m_bodies[0] = body0; - pj->m_bodies[1] = body1; - pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis()*specs.axis; - pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis()*specs.axis; - pj->m_cfm = specs.cfm; - pj->m_erp = specs.erp; - pj->m_split = specs.split; - pj->m_icontrol = specs.icontrol; + AJoint* pj = new (btAlignedAlloc(sizeof(AJoint), 16)) AJoint(); + pj->m_bodies[0] = body0; + pj->m_bodies[1] = body1; + pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis() * specs.axis; + pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis() * specs.axis; + pj->m_cfm = specs.cfm; + pj->m_erp = specs.erp; + pj->m_split = specs.split; + pj->m_icontrol = specs.icontrol; m_joints.push_back(pj); } // -void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body) +void btSoftBody::appendAngularJoint(const AJoint::Specs& specs, Body body) { - appendAngularJoint(specs,m_clusters[0],body); + appendAngularJoint(specs, m_clusters[0], body); } // -void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body) +void btSoftBody::appendAngularJoint(const AJoint::Specs& specs, btSoftBody* body) { - appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]); + appendAngularJoint(specs, m_clusters[0], body->m_clusters[0]); } // -void btSoftBody::addForce(const btVector3& force) +void btSoftBody::addForce(const btVector3& force) { - for(int i=0,ni=m_nodes.size();i<ni;++i) addForce(force,i); + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) addForce(force, i); } // -void btSoftBody::addForce(const btVector3& force,int node) +void btSoftBody::addForce(const btVector3& force, int node) { - Node& n=m_nodes[node]; - if(n.m_im>0) + Node& n = m_nodes[node]; + if (n.m_im > 0) { - n.m_f += force; + n.m_f += force; } } -void btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeIndex) +void btSoftBody::addAeroForceToNode(const btVector3& windVelocity, int nodeIndex) { btAssert(nodeIndex >= 0 && nodeIndex < m_nodes.size()); @@ -462,51 +710,51 @@ void btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeInde const btScalar kDG = m_cfg.kDG; //const btScalar kPR = m_cfg.kPR; //const btScalar kVC = m_cfg.kVC; - const bool as_lift = kLF>0; - const bool as_drag = kDG>0; + const bool as_lift = kLF > 0; + const bool as_drag = kDG > 0; const bool as_aero = as_lift || as_drag; const bool as_vaero = as_aero && (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided); Node& n = m_nodes[nodeIndex]; - if( n.m_im>0 ) + if (n.m_im > 0) { - btSoftBody::sMedium medium; + btSoftBody::sMedium medium; EvaluateMedium(m_worldInfo, n.m_x, medium); medium.m_velocity = windVelocity; medium.m_density = m_worldInfo->air_density; - /* Aerodynamics */ - if(as_vaero) - { - const btVector3 rel_v = n.m_v - medium.m_velocity; + /* Aerodynamics */ + if (as_vaero) + { + const btVector3 rel_v = n.m_v - medium.m_velocity; const btScalar rel_v_len = rel_v.length(); - const btScalar rel_v2 = rel_v.length2(); + const btScalar rel_v2 = rel_v.length2(); - if(rel_v2>SIMD_EPSILON) + if (rel_v2 > SIMD_EPSILON) { const btVector3 rel_v_nrm = rel_v.normalized(); - btVector3 nrm = n.m_n; + btVector3 nrm = n.m_n; if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSidedLiftDrag) { - nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); + nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1); btVector3 fDrag(0, 0, 0); btVector3 fLift(0, 0, 0); btScalar n_dot_v = nrm.dot(rel_v_nrm); btScalar tri_area = 0.5f * n.m_area; - + fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm); - + // Check angle of attack // cos(10º) = 0.98480 - if ( 0 < n_dot_v && n_dot_v < 0.98480f) - fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f-n_dot_v*n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm)); + if (0 < n_dot_v && n_dot_v < 0.98480f) + fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm)); // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node. - btVector3 del_v_by_fDrag = fDrag*n.m_im*m_sst.sdt; + btVector3 del_v_by_fDrag = fDrag * n.m_im * m_sst.sdt; btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2(); btScalar v_len2 = n.m_v.length2(); @@ -514,7 +762,7 @@ void btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeInde { btScalar del_v_by_fDrag_len = del_v_by_fDrag.length(); btScalar v_len = n.m_v.length(); - fDrag *= btScalar(0.8)*(v_len / del_v_by_fDrag_len); + fDrag *= btScalar(0.8) * (v_len / del_v_by_fDrag_len); } n.m_f += fDrag; @@ -522,84 +770,84 @@ void btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeInde } else if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_Point || m_cfg.aeromodel == btSoftBody::eAeroModel::V_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided) { - if (btSoftBody::eAeroModel::V_TwoSided) - nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); + if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided) + nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1); - const btScalar dvn = btDot(rel_v,nrm); - /* Compute forces */ - if(dvn>0) + const btScalar dvn = btDot(rel_v, nrm); + /* Compute forces */ + if (dvn > 0) { - btVector3 force(0,0,0); - const btScalar c0 = n.m_area * dvn * rel_v2/2; - const btScalar c1 = c0 * medium.m_density; - force += nrm*(-c1*kLF); - force += rel_v.normalized() * (-c1 * kDG); + btVector3 force(0, 0, 0); + const btScalar c0 = n.m_area * dvn * rel_v2 / 2; + const btScalar c1 = c0 * medium.m_density; + force += nrm * (-c1 * kLF); + force += rel_v.normalized() * (-c1 * kDG); ApplyClampedForce(n, force, dt); } - } + } } } } } -void btSoftBody::addAeroForceToFace(const btVector3& windVelocity,int faceIndex) +void btSoftBody::addAeroForceToFace(const btVector3& windVelocity, int faceIndex) { const btScalar dt = m_sst.sdt; const btScalar kLF = m_cfg.kLF; const btScalar kDG = m_cfg.kDG; -// const btScalar kPR = m_cfg.kPR; -// const btScalar kVC = m_cfg.kVC; - const bool as_lift = kLF>0; - const bool as_drag = kDG>0; + // const btScalar kPR = m_cfg.kPR; + // const btScalar kVC = m_cfg.kVC; + const bool as_lift = kLF > 0; + const bool as_drag = kDG > 0; const bool as_aero = as_lift || as_drag; const bool as_faero = as_aero && (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided); - if(as_faero) + if (as_faero) { - btSoftBody::Face& f=m_faces[faceIndex]; + btSoftBody::Face& f = m_faces[faceIndex]; - btSoftBody::sMedium medium; - - const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3; - const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3; - EvaluateMedium(m_worldInfo,x,medium); + btSoftBody::sMedium medium; + + const btVector3 v = (f.m_n[0]->m_v + f.m_n[1]->m_v + f.m_n[2]->m_v) / 3; + const btVector3 x = (f.m_n[0]->m_x + f.m_n[1]->m_x + f.m_n[2]->m_x) / 3; + EvaluateMedium(m_worldInfo, x, medium); medium.m_velocity = windVelocity; medium.m_density = m_worldInfo->air_density; - const btVector3 rel_v=v-medium.m_velocity; + const btVector3 rel_v = v - medium.m_velocity; const btScalar rel_v_len = rel_v.length(); - const btScalar rel_v2=rel_v.length2(); + const btScalar rel_v2 = rel_v.length2(); - if(rel_v2>SIMD_EPSILON) + if (rel_v2 > SIMD_EPSILON) { const btVector3 rel_v_nrm = rel_v.normalized(); - btVector3 nrm = f.m_normal; + btVector3 nrm = f.m_normal; if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSidedLiftDrag) { - nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); + nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1); btVector3 fDrag(0, 0, 0); btVector3 fLift(0, 0, 0); btScalar n_dot_v = nrm.dot(rel_v_nrm); btScalar tri_area = 0.5f * f.m_ra; - + fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm); // Check angle of attack // cos(10º) = 0.98480 - if ( 0 < n_dot_v && n_dot_v < 0.98480f) - fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f-n_dot_v*n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm)); + if (0 < n_dot_v && n_dot_v < 0.98480f) + fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm)); fDrag /= 3; fLift /= 3; - for(int j=0;j<3;++j) + for (int j = 0; j < 3; ++j) { - if (f.m_n[j]->m_im>0) + if (f.m_n[j]->m_im > 0) { // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node. - btVector3 del_v_by_fDrag = fDrag*f.m_n[j]->m_im*m_sst.sdt; + btVector3 del_v_by_fDrag = fDrag * f.m_n[j]->m_im * m_sst.sdt; btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2(); btScalar v_len2 = f.m_n[j]->m_v.length2(); @@ -607,237 +855,305 @@ void btSoftBody::addAeroForceToFace(const btVector3& windVelocity,int faceInde { btScalar del_v_by_fDrag_len = del_v_by_fDrag.length(); btScalar v_len = f.m_n[j]->m_v.length(); - fDrag *= btScalar(0.8)*(v_len / del_v_by_fDrag_len); + fDrag *= btScalar(0.8) * (v_len / del_v_by_fDrag_len); } - f.m_n[j]->m_f += fDrag; + f.m_n[j]->m_f += fDrag; f.m_n[j]->m_f += fLift; } } } else if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided) { - if (btSoftBody::eAeroModel::F_TwoSided) - nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); + if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided) + nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1); - const btScalar dvn=btDot(rel_v,nrm); - /* Compute forces */ - if(dvn>0) + const btScalar dvn = btDot(rel_v, nrm); + /* Compute forces */ + if (dvn > 0) { - btVector3 force(0,0,0); - const btScalar c0 = f.m_ra*dvn*rel_v2; - const btScalar c1 = c0*medium.m_density; - force += nrm*(-c1*kLF); - force += rel_v.normalized()*(-c1*kDG); - force /= 3; - for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt); + btVector3 force(0, 0, 0); + const btScalar c0 = f.m_ra * dvn * rel_v2; + const btScalar c1 = c0 * medium.m_density; + force += nrm * (-c1 * kLF); + force += rel_v.normalized() * (-c1 * kDG); + force /= 3; + for (int j = 0; j < 3; ++j) ApplyClampedForce(*f.m_n[j], force, dt); } } } } - } // -void btSoftBody::addVelocity(const btVector3& velocity) +void btSoftBody::addVelocity(const btVector3& velocity) { - for(int i=0,ni=m_nodes.size();i<ni;++i) addVelocity(velocity,i); + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) addVelocity(velocity, i); } -/* Set velocity for the entire body */ -void btSoftBody::setVelocity( const btVector3& velocity) +/* Set velocity for the entire body */ +void btSoftBody::setVelocity(const btVector3& velocity) { - for(int i=0,ni=m_nodes.size();i<ni;++i) + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - if(n.m_im>0) + Node& n = m_nodes[i]; + if (n.m_im > 0) { - n.m_v = velocity; + n.m_v = velocity; + n.m_vn = velocity; } } } - // -void btSoftBody::addVelocity(const btVector3& velocity,int node) +void btSoftBody::addVelocity(const btVector3& velocity, int node) { - Node& n=m_nodes[node]; - if(n.m_im>0) + Node& n = m_nodes[node]; + if (n.m_im > 0) { - n.m_v += velocity; + n.m_v += velocity; } } // -void btSoftBody::setMass(int node,btScalar mass) +void btSoftBody::setMass(int node, btScalar mass) { - m_nodes[node].m_im=mass>0?1/mass:0; - m_bUpdateRtCst=true; + m_nodes[node].m_im = mass > 0 ? 1 / mass : 0; + m_bUpdateRtCst = true; } // -btScalar btSoftBody::getMass(int node) const +btScalar btSoftBody::getMass(int node) const { - return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0); + return (m_nodes[node].m_im > 0 ? 1 / m_nodes[node].m_im : 0); } // -btScalar btSoftBody::getTotalMass() const +btScalar btSoftBody::getTotalMass() const { - btScalar mass=0; - for(int i=0;i<m_nodes.size();++i) + btScalar mass = 0; + for (int i = 0; i < m_nodes.size(); ++i) { - mass+=getMass(i); + mass += getMass(i); } - return(mass); + return (mass); } // -void btSoftBody::setTotalMass(btScalar mass,bool fromfaces) +void btSoftBody::setTotalMass(btScalar mass, bool fromfaces) { int i; - if(fromfaces) + if (fromfaces) { - - for(i=0;i<m_nodes.size();++i) + for (i = 0; i < m_nodes.size(); ++i) { - m_nodes[i].m_im=0; + m_nodes[i].m_im = 0; } - for(i=0;i<m_faces.size();++i) + for (i = 0; i < m_faces.size(); ++i) { - const Face& f=m_faces[i]; - const btScalar twicearea=AreaOf( f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x); - for(int j=0;j<3;++j) + const Face& f = m_faces[i]; + const btScalar twicearea = AreaOf(f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x); + for (int j = 0; j < 3; ++j) { - f.m_n[j]->m_im+=twicearea; + f.m_n[j]->m_im += twicearea; } } - for( i=0;i<m_nodes.size();++i) + for (i = 0; i < m_nodes.size(); ++i) { - m_nodes[i].m_im=1/m_nodes[i].m_im; + m_nodes[i].m_im = 1 / m_nodes[i].m_im; } } - const btScalar tm=getTotalMass(); - const btScalar itm=1/tm; - for( i=0;i<m_nodes.size();++i) + const btScalar tm = getTotalMass(); + const btScalar itm = 1 / tm; + for (i = 0; i < m_nodes.size(); ++i) { - m_nodes[i].m_im/=itm*mass; + m_nodes[i].m_im /= itm * mass; } - m_bUpdateRtCst=true; + m_bUpdateRtCst = true; } // -void btSoftBody::setTotalDensity(btScalar density) +void btSoftBody::setTotalDensity(btScalar density) { - setTotalMass(getVolume()*density,true); + setTotalMass(getVolume() * density, true); } // -void btSoftBody::setVolumeMass(btScalar mass) +void btSoftBody::setVolumeMass(btScalar mass) { -btAlignedObjectArray<btScalar> ranks; -ranks.resize(m_nodes.size(),0); -int i; + btAlignedObjectArray<btScalar> ranks; + ranks.resize(m_nodes.size(), 0); + int i; -for(i=0;i<m_nodes.size();++i) + for (i = 0; i < m_nodes.size(); ++i) { - m_nodes[i].m_im=0; + m_nodes[i].m_im = 0; } -for(i=0;i<m_tetras.size();++i) + for (i = 0; i < m_tetras.size(); ++i) { - const Tetra& t=m_tetras[i]; - for(int j=0;j<4;++j) + const Tetra& t = m_tetras[i]; + for (int j = 0; j < 4; ++j) { - t.m_n[j]->m_im+=btFabs(t.m_rv); - ranks[int(t.m_n[j]-&m_nodes[0])]+=1; + t.m_n[j]->m_im += btFabs(t.m_rv); + ranks[int(t.m_n[j] - &m_nodes[0])] += 1; } } -for( i=0;i<m_nodes.size();++i) + for (i = 0; i < m_nodes.size(); ++i) { - if(m_nodes[i].m_im>0) + if (m_nodes[i].m_im > 0) { - m_nodes[i].m_im=ranks[i]/m_nodes[i].m_im; + m_nodes[i].m_im = ranks[i] / m_nodes[i].m_im; } } -setTotalMass(mass,false); + setTotalMass(mass, false); } // -void btSoftBody::setVolumeDensity(btScalar density) +void btSoftBody::setVolumeDensity(btScalar density) { -btScalar volume=0; -for(int i=0;i<m_tetras.size();++i) + btScalar volume = 0; + for (int i = 0; i < m_tetras.size(); ++i) { - const Tetra& t=m_tetras[i]; - for(int j=0;j<4;++j) + const Tetra& t = m_tetras[i]; + for (int j = 0; j < 4; ++j) { - volume+=btFabs(t.m_rv); + volume += btFabs(t.m_rv); } } -setVolumeMass(volume*density/6); + setVolumeMass(volume * density / 6); +} + +// +btVector3 btSoftBody::getLinearVelocity() +{ + btVector3 total_momentum = btVector3(0, 0, 0); + for (int i = 0; i < m_nodes.size(); ++i) + { + btScalar mass = m_nodes[i].m_im == 0 ? 0 : 1.0 / m_nodes[i].m_im; + total_momentum += mass * m_nodes[i].m_v; + } + btScalar total_mass = getTotalMass(); + return total_mass == 0 ? total_momentum : total_momentum / total_mass; +} + +// +void btSoftBody::setLinearVelocity(const btVector3& linVel) +{ + btVector3 old_vel = getLinearVelocity(); + btVector3 diff = linVel - old_vel; + for (int i = 0; i < m_nodes.size(); ++i) + m_nodes[i].m_v += diff; +} + +// +void btSoftBody::setAngularVelocity(const btVector3& angVel) +{ + btVector3 old_vel = getLinearVelocity(); + btVector3 com = getCenterOfMass(); + for (int i = 0; i < m_nodes.size(); ++i) + { + m_nodes[i].m_v = angVel.cross(m_nodes[i].m_x - com) + old_vel; + } +} + +// +btTransform btSoftBody::getRigidTransform() +{ + btVector3 t = getCenterOfMass(); + btMatrix3x3 S; + S.setZero(); + // Get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i || + // It's important to make sure that S has the correct signs. + // SVD is only unique up to the ordering of singular values. + // SVD will manipulate U and V to ensure the ordering of singular values. If all three singular + // vaues are negative, SVD will permute colums of U to make two of them positive. + for (int i = 0; i < m_nodes.size(); ++i) + { + S -= OuterProduct(m_X[i], t - m_nodes[i].m_x); + } + btVector3 sigma; + btMatrix3x3 U, V; + singularValueDecomposition(S, U, sigma, V); + btMatrix3x3 R = V * U.transpose(); + btTransform trs; + trs.setIdentity(); + trs.setOrigin(t); + trs.setBasis(R); + return trs; +} + +// +void btSoftBody::transformTo(const btTransform& trs) +{ + // get the current best rigid fit + btTransform current_transform = getRigidTransform(); + // apply transform in material space + btTransform new_transform = trs * current_transform.inverse(); + transform(new_transform); } // -void btSoftBody::transform(const btTransform& trs) +void btSoftBody::transform(const btTransform& trs) { - const btScalar margin=getCollisionShape()->getMargin(); - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; - - for(int i=0,ni=m_nodes.size();i<ni;++i) + const btScalar margin = getCollisionShape()->getMargin(); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - n.m_x=trs*n.m_x; - n.m_q=trs*n.m_q; - n.m_n=trs.getBasis()*n.m_n; - vol = btDbvtVolume::FromCR(n.m_x,margin); - - m_ndbvt.update(n.m_leaf,vol); + Node& n = m_nodes[i]; + n.m_x = trs * n.m_x; + n.m_q = trs * n.m_q; + n.m_n = trs.getBasis() * n.m_n; + vol = btDbvtVolume::FromCR(n.m_x, margin); + + m_ndbvt.update(n.m_leaf, vol); } updateNormals(); updateBounds(); updateConstants(); - m_initialWorldTransform = trs; } // -void btSoftBody::translate(const btVector3& trs) +void btSoftBody::translate(const btVector3& trs) { - btTransform t; + btTransform t; t.setIdentity(); t.setOrigin(trs); transform(t); } // -void btSoftBody::rotate( const btQuaternion& rot) +void btSoftBody::rotate(const btQuaternion& rot) { - btTransform t; + btTransform t; t.setIdentity(); t.setRotation(rot); transform(t); } // -void btSoftBody::scale(const btVector3& scl) +void btSoftBody::scale(const btVector3& scl) { + const btScalar margin = getCollisionShape()->getMargin(); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; - const btScalar margin=getCollisionShape()->getMargin(); - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; - - for(int i=0,ni=m_nodes.size();i<ni;++i) + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - n.m_x*=scl; - n.m_q*=scl; - vol = btDbvtVolume::FromCR(n.m_x,margin); - m_ndbvt.update(n.m_leaf,vol); + Node& n = m_nodes[i]; + n.m_x *= scl; + n.m_q *= scl; + vol = btDbvtVolume::FromCR(n.m_x, margin); + m_ndbvt.update(n.m_leaf, vol); } updateNormals(); updateBounds(); updateConstants(); + initializeDmInverse(); } // @@ -849,437 +1165,432 @@ btScalar btSoftBody::getRestLengthScale() // void btSoftBody::setRestLengthScale(btScalar restLengthScale) { - for(int i=0, ni=m_links.size(); i<ni; ++i) + for (int i = 0, ni = m_links.size(); i < ni; ++i) { - Link& l=m_links[i]; - l.m_rl = l.m_rl / m_restLengthScale * restLengthScale; - l.m_c1 = l.m_rl*l.m_rl; + Link& l = m_links[i]; + l.m_rl = l.m_rl / m_restLengthScale * restLengthScale; + l.m_c1 = l.m_rl * l.m_rl; } m_restLengthScale = restLengthScale; - + if (getActivationState() == ISLAND_SLEEPING) activate(); } // -void btSoftBody::setPose(bool bvolume,bool bframe) +void btSoftBody::setPose(bool bvolume, bool bframe) { - m_pose.m_bvolume = bvolume; - m_pose.m_bframe = bframe; - int i,ni; - - /* Weights */ - const btScalar omass=getTotalMass(); - const btScalar kmass=omass*m_nodes.size()*1000; - btScalar tmass=omass; + m_pose.m_bvolume = bvolume; + m_pose.m_bframe = bframe; + int i, ni; + + /* Weights */ + const btScalar omass = getTotalMass(); + const btScalar kmass = omass * m_nodes.size() * 1000; + btScalar tmass = omass; m_pose.m_wgh.resize(m_nodes.size()); - for(i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - if(m_nodes[i].m_im<=0) tmass+=kmass; + if (m_nodes[i].m_im <= 0) tmass += kmass; } - for( i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - m_pose.m_wgh[i]= n.m_im>0 ? - 1/(m_nodes[i].m_im*tmass) : - kmass/tmass; + Node& n = m_nodes[i]; + m_pose.m_wgh[i] = n.m_im > 0 ? 1 / (m_nodes[i].m_im * tmass) : kmass / tmass; } - /* Pos */ - const btVector3 com=evaluateCom(); + /* Pos */ + const btVector3 com = evaluateCom(); m_pose.m_pos.resize(m_nodes.size()); - for( i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - m_pose.m_pos[i]=m_nodes[i].m_x-com; + m_pose.m_pos[i] = m_nodes[i].m_x - com; } - m_pose.m_volume = bvolume?getVolume():0; - m_pose.m_com = com; + m_pose.m_volume = bvolume ? getVolume() : 0; + m_pose.m_com = com; m_pose.m_rot.setIdentity(); m_pose.m_scl.setIdentity(); - /* Aqq */ - m_pose.m_aqq[0] = - m_pose.m_aqq[1] = - m_pose.m_aqq[2] = btVector3(0,0,0); - for( i=0,ni=m_nodes.size();i<ni;++i) - { - const btVector3& q=m_pose.m_pos[i]; - const btVector3 mq=m_pose.m_wgh[i]*q; - m_pose.m_aqq[0]+=mq.x()*q; - m_pose.m_aqq[1]+=mq.y()*q; - m_pose.m_aqq[2]+=mq.z()*q; - } - m_pose.m_aqq=m_pose.m_aqq.inverse(); - + /* Aqq */ + m_pose.m_aqq[0] = + m_pose.m_aqq[1] = + m_pose.m_aqq[2] = btVector3(0, 0, 0); + for (i = 0, ni = m_nodes.size(); i < ni; ++i) + { + const btVector3& q = m_pose.m_pos[i]; + const btVector3 mq = m_pose.m_wgh[i] * q; + m_pose.m_aqq[0] += mq.x() * q; + m_pose.m_aqq[1] += mq.y() * q; + m_pose.m_aqq[2] += mq.z() * q; + } + m_pose.m_aqq = m_pose.m_aqq.inverse(); + updateConstants(); } -void btSoftBody::resetLinkRestLengths() +void btSoftBody::resetLinkRestLengths() { - for(int i=0, ni=m_links.size();i<ni;++i) + for (int i = 0, ni = m_links.size(); i < ni; ++i) { - Link& l = m_links[i]; - l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length(); - l.m_c1 = l.m_rl*l.m_rl; + Link& l = m_links[i]; + l.m_rl = (l.m_n[0]->m_x - l.m_n[1]->m_x).length(); + l.m_c1 = l.m_rl * l.m_rl; } } // -btScalar btSoftBody::getVolume() const +btScalar btSoftBody::getVolume() const { - btScalar vol=0; - if(m_nodes.size()>0) + btScalar vol = 0; + if (m_nodes.size() > 0) { - int i,ni; + int i, ni; - const btVector3 org=m_nodes[0].m_x; - for(i=0,ni=m_faces.size();i<ni;++i) + const btVector3 org = m_nodes[0].m_x; + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - const Face& f=m_faces[i]; - vol+=btDot(f.m_n[0]->m_x-org,btCross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org)); + const Face& f = m_faces[i]; + vol += btDot(f.m_n[0]->m_x - org, btCross(f.m_n[1]->m_x - org, f.m_n[2]->m_x - org)); } - vol/=(btScalar)6; + vol /= (btScalar)6; } - return(vol); + return (vol); } // -int btSoftBody::clusterCount() const +int btSoftBody::clusterCount() const { - return(m_clusters.size()); + return (m_clusters.size()); } // -btVector3 btSoftBody::clusterCom(const Cluster* cluster) +btVector3 btSoftBody::clusterCom(const Cluster* cluster) { - btVector3 com(0,0,0); - for(int i=0,ni=cluster->m_nodes.size();i<ni;++i) + btVector3 com(0, 0, 0); + for (int i = 0, ni = cluster->m_nodes.size(); i < ni; ++i) { - com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i]; + com += cluster->m_nodes[i]->m_x * cluster->m_masses[i]; } - return(com*cluster->m_imass); + return (com * cluster->m_imass); } // -btVector3 btSoftBody::clusterCom(int cluster) const +btVector3 btSoftBody::clusterCom(int cluster) const { - return(clusterCom(m_clusters[cluster])); + return (clusterCom(m_clusters[cluster])); } // -btVector3 btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos) +btVector3 btSoftBody::clusterVelocity(const Cluster* cluster, const btVector3& rpos) { - return(cluster->m_lv+btCross(cluster->m_av,rpos)); + return (cluster->m_lv + btCross(cluster->m_av, rpos)); } // -void btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse) +void btSoftBody::clusterVImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse) { - const btVector3 li=cluster->m_imass*impulse; - const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse); - cluster->m_vimpulses[0]+=li;cluster->m_lv+=li; - cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; + const btVector3 li = cluster->m_imass * impulse; + const btVector3 ai = cluster->m_invwi * btCross(rpos, impulse); + cluster->m_vimpulses[0] += li; + cluster->m_lv += li; + cluster->m_vimpulses[1] += ai; + cluster->m_av += ai; cluster->m_nvimpulses++; } // -void btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse) +void btSoftBody::clusterDImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse) { - const btVector3 li=cluster->m_imass*impulse; - const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse); - cluster->m_dimpulses[0]+=li; - cluster->m_dimpulses[1]+=ai; + const btVector3 li = cluster->m_imass * impulse; + const btVector3 ai = cluster->m_invwi * btCross(rpos, impulse); + cluster->m_dimpulses[0] += li; + cluster->m_dimpulses[1] += ai; cluster->m_ndimpulses++; } // -void btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse) +void btSoftBody::clusterImpulse(Cluster* cluster, const btVector3& rpos, const Impulse& impulse) { - if(impulse.m_asVelocity) clusterVImpulse(cluster,rpos,impulse.m_velocity); - if(impulse.m_asDrift) clusterDImpulse(cluster,rpos,impulse.m_drift); + if (impulse.m_asVelocity) clusterVImpulse(cluster, rpos, impulse.m_velocity); + if (impulse.m_asDrift) clusterDImpulse(cluster, rpos, impulse.m_drift); } // -void btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse) +void btSoftBody::clusterVAImpulse(Cluster* cluster, const btVector3& impulse) { - const btVector3 ai=cluster->m_invwi*impulse; - cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; + const btVector3 ai = cluster->m_invwi * impulse; + cluster->m_vimpulses[1] += ai; + cluster->m_av += ai; cluster->m_nvimpulses++; } // -void btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse) +void btSoftBody::clusterDAImpulse(Cluster* cluster, const btVector3& impulse) { - const btVector3 ai=cluster->m_invwi*impulse; - cluster->m_dimpulses[1]+=ai; + const btVector3 ai = cluster->m_invwi * impulse; + cluster->m_dimpulses[1] += ai; cluster->m_ndimpulses++; } // -void btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse) +void btSoftBody::clusterAImpulse(Cluster* cluster, const Impulse& impulse) { - if(impulse.m_asVelocity) clusterVAImpulse(cluster,impulse.m_velocity); - if(impulse.m_asDrift) clusterDAImpulse(cluster,impulse.m_drift); + if (impulse.m_asVelocity) clusterVAImpulse(cluster, impulse.m_velocity); + if (impulse.m_asDrift) clusterDAImpulse(cluster, impulse.m_drift); } // -void btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse) +void btSoftBody::clusterDCImpulse(Cluster* cluster, const btVector3& impulse) { - cluster->m_dimpulses[0]+=impulse*cluster->m_imass; + cluster->m_dimpulses[0] += impulse * cluster->m_imass; cluster->m_ndimpulses++; } struct NodeLinks { - btAlignedObjectArray<int> m_links; + btAlignedObjectArray<int> m_links; }; - - // -int btSoftBody::generateBendingConstraints(int distance,Material* mat) +int btSoftBody::generateBendingConstraints(int distance, Material* mat) { - int i,j; + int i, j; - if(distance>1) + if (distance > 1) { - /* Build graph */ - const int n=m_nodes.size(); - const unsigned inf=(~(unsigned)0)>>1; - unsigned* adj=new unsigned[n*n]; - + /* Build graph */ + const int n = m_nodes.size(); + const unsigned inf = (~(unsigned)0) >> 1; + unsigned* adj = new unsigned[n * n]; -#define IDX(_x_,_y_) ((_y_)*n+(_x_)) - for(j=0;j<n;++j) +#define IDX(_x_, _y_) ((_y_)*n + (_x_)) + for (j = 0; j < n; ++j) { - for(i=0;i<n;++i) + for (i = 0; i < n; ++i) { - if(i!=j) + if (i != j) { - adj[IDX(i,j)]=adj[IDX(j,i)]=inf; + adj[IDX(i, j)] = adj[IDX(j, i)] = inf; } else { - adj[IDX(i,j)]=adj[IDX(j,i)]=0; + adj[IDX(i, j)] = adj[IDX(j, i)] = 0; } } } - for( i=0;i<m_links.size();++i) + for (i = 0; i < m_links.size(); ++i) { - const int ia=(int)(m_links[i].m_n[0]-&m_nodes[0]); - const int ib=(int)(m_links[i].m_n[1]-&m_nodes[0]); - adj[IDX(ia,ib)]=1; - adj[IDX(ib,ia)]=1; + const int ia = (int)(m_links[i].m_n[0] - &m_nodes[0]); + const int ib = (int)(m_links[i].m_n[1] - &m_nodes[0]); + adj[IDX(ia, ib)] = 1; + adj[IDX(ib, ia)] = 1; } - //special optimized case for distance == 2 if (distance == 2) { - btAlignedObjectArray<NodeLinks> nodeLinks; - /* Build node links */ nodeLinks.resize(m_nodes.size()); - for( i=0;i<m_links.size();++i) + for (i = 0; i < m_links.size(); ++i) { - const int ia=(int)(m_links[i].m_n[0]-&m_nodes[0]); - const int ib=(int)(m_links[i].m_n[1]-&m_nodes[0]); - if (nodeLinks[ia].m_links.findLinearSearch(ib)==nodeLinks[ia].m_links.size()) + const int ia = (int)(m_links[i].m_n[0] - &m_nodes[0]); + const int ib = (int)(m_links[i].m_n[1] - &m_nodes[0]); + if (nodeLinks[ia].m_links.findLinearSearch(ib) == nodeLinks[ia].m_links.size()) nodeLinks[ia].m_links.push_back(ib); - if (nodeLinks[ib].m_links.findLinearSearch(ia)==nodeLinks[ib].m_links.size()) + if (nodeLinks[ib].m_links.findLinearSearch(ia) == nodeLinks[ib].m_links.size()) nodeLinks[ib].m_links.push_back(ia); } - for (int ii=0;ii<nodeLinks.size();ii++) + for (int ii = 0; ii < nodeLinks.size(); ii++) { - int i=ii; + int i = ii; - for (int jj=0;jj<nodeLinks[ii].m_links.size();jj++) + for (int jj = 0; jj < nodeLinks[ii].m_links.size(); jj++) { int k = nodeLinks[ii].m_links[jj]; - for (int kk=0;kk<nodeLinks[k].m_links.size();kk++) + for (int kk = 0; kk < nodeLinks[k].m_links.size(); kk++) { int j = nodeLinks[k].m_links[kk]; - if (i!=j) + if (i != j) { - const unsigned sum=adj[IDX(i,k)]+adj[IDX(k,j)]; - btAssert(sum==2); - if(adj[IDX(i,j)]>sum) + const unsigned sum = adj[IDX(i, k)] + adj[IDX(k, j)]; + btAssert(sum == 2); + if (adj[IDX(i, j)] > sum) { - adj[IDX(i,j)]=adj[IDX(j,i)]=sum; + adj[IDX(i, j)] = adj[IDX(j, i)] = sum; } } - } } } - } else + } + else { ///generic Floyd's algorithm - for(int k=0;k<n;++k) + for (int k = 0; k < n; ++k) { - for(j=0;j<n;++j) + for (j = 0; j < n; ++j) { - for(i=j+1;i<n;++i) + for (i = j + 1; i < n; ++i) { - const unsigned sum=adj[IDX(i,k)]+adj[IDX(k,j)]; - if(adj[IDX(i,j)]>sum) + const unsigned sum = adj[IDX(i, k)] + adj[IDX(k, j)]; + if (adj[IDX(i, j)] > sum) { - adj[IDX(i,j)]=adj[IDX(j,i)]=sum; + adj[IDX(i, j)] = adj[IDX(j, i)] = sum; } } } } } - - /* Build links */ - int nlinks=0; - for(j=0;j<n;++j) + /* Build links */ + int nlinks = 0; + for (j = 0; j < n; ++j) { - for(i=j+1;i<n;++i) + for (i = j + 1; i < n; ++i) { - if(adj[IDX(i,j)]==(unsigned)distance) + if (adj[IDX(i, j)] == (unsigned)distance) { - appendLink(i,j,mat); - m_links[m_links.size()-1].m_bbending=1; + appendLink(i, j, mat); + m_links[m_links.size() - 1].m_bbending = 1; ++nlinks; } } } - delete[] adj; - return(nlinks); + delete[] adj; + return (nlinks); } - return(0); + return (0); } // -void btSoftBody::randomizeConstraints() +void btSoftBody::randomizeConstraints() { - unsigned long seed=243703; -#define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff) - int i,ni; + unsigned long seed = 243703; +#define NEXTRAND (seed = (1664525L * seed + 1013904223L) & 0xffffffff) + int i, ni; - for(i=0,ni=m_links.size();i<ni;++i) + for (i = 0, ni = m_links.size(); i < ni; ++i) { - btSwap(m_links[i],m_links[NEXTRAND%ni]); + btSwap(m_links[i], m_links[NEXTRAND % ni]); } - for(i=0,ni=m_faces.size();i<ni;++i) + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - btSwap(m_faces[i],m_faces[NEXTRAND%ni]); + btSwap(m_faces[i], m_faces[NEXTRAND % ni]); } #undef NEXTRAND } // -void btSoftBody::releaseCluster(int index) +void btSoftBody::releaseCluster(int index) { - Cluster* c=m_clusters[index]; - if(c->m_leaf) m_cdbvt.remove(c->m_leaf); + Cluster* c = m_clusters[index]; + if (c->m_leaf) m_cdbvt.remove(c->m_leaf); c->~Cluster(); btAlignedFree(c); m_clusters.remove(c); } // -void btSoftBody::releaseClusters() +void btSoftBody::releaseClusters() { - while(m_clusters.size()>0) releaseCluster(0); + while (m_clusters.size() > 0) releaseCluster(0); } // -int btSoftBody::generateClusters(int k,int maxiterations) +int btSoftBody::generateClusters(int k, int maxiterations) { int i; releaseClusters(); - m_clusters.resize(btMin(k,m_nodes.size())); - for(i=0;i<m_clusters.size();++i) - { - m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); - m_clusters[i]->m_collide= true; - } - k=m_clusters.size(); - if(k>0) - { - /* Initialize */ - btAlignedObjectArray<btVector3> centers; - btVector3 cog(0,0,0); - int i; - for(i=0;i<m_nodes.size();++i) - { - cog+=m_nodes[i].m_x; - m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]); - } - cog/=(btScalar)m_nodes.size(); - centers.resize(k,cog); - /* Iterate */ - const btScalar slope=16; - bool changed; - int iterations=0; - do { - const btScalar w=2-btMin<btScalar>(1,iterations/slope); - changed=false; - iterations++; + m_clusters.resize(btMin(k, m_nodes.size())); + for (i = 0; i < m_clusters.size(); ++i) + { + m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster(); + m_clusters[i]->m_collide = true; + } + k = m_clusters.size(); + if (k > 0) + { + /* Initialize */ + btAlignedObjectArray<btVector3> centers; + btVector3 cog(0, 0, 0); + int i; + for (i = 0; i < m_nodes.size(); ++i) + { + cog += m_nodes[i].m_x; + m_clusters[(i * 29873) % m_clusters.size()]->m_nodes.push_back(&m_nodes[i]); + } + cog /= (btScalar)m_nodes.size(); + centers.resize(k, cog); + /* Iterate */ + const btScalar slope = 16; + bool changed; + int iterations = 0; + do + { + const btScalar w = 2 - btMin<btScalar>(1, iterations / slope); + changed = false; + iterations++; int i; - for(i=0;i<k;++i) + for (i = 0; i < k; ++i) { - btVector3 c(0,0,0); - for(int j=0;j<m_clusters[i]->m_nodes.size();++j) + btVector3 c(0, 0, 0); + for (int j = 0; j < m_clusters[i]->m_nodes.size(); ++j) { - c+=m_clusters[i]->m_nodes[j]->m_x; + c += m_clusters[i]->m_nodes[j]->m_x; } - if(m_clusters[i]->m_nodes.size()) + if (m_clusters[i]->m_nodes.size()) { - c /= (btScalar)m_clusters[i]->m_nodes.size(); - c = centers[i]+(c-centers[i])*w; - changed |= ((c-centers[i]).length2()>SIMD_EPSILON); - centers[i] = c; + c /= (btScalar)m_clusters[i]->m_nodes.size(); + c = centers[i] + (c - centers[i]) * w; + changed |= ((c - centers[i]).length2() > SIMD_EPSILON); + centers[i] = c; m_clusters[i]->m_nodes.resize(0); - } + } } - for(i=0;i<m_nodes.size();++i) + for (i = 0; i < m_nodes.size(); ++i) { - const btVector3 nx=m_nodes[i].m_x; - int kbest=0; - btScalar kdist=ClusterMetric(centers[0],nx); - for(int j=1;j<k;++j) + const btVector3 nx = m_nodes[i].m_x; + int kbest = 0; + btScalar kdist = ClusterMetric(centers[0], nx); + for (int j = 1; j < k; ++j) { - const btScalar d=ClusterMetric(centers[j],nx); - if(d<kdist) + const btScalar d = ClusterMetric(centers[j], nx); + if (d < kdist) { - kbest=j; - kdist=d; + kbest = j; + kdist = d; } } m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]); - } - } while(changed&&(iterations<maxiterations)); - /* Merge */ - btAlignedObjectArray<int> cids; - cids.resize(m_nodes.size(),-1); - for(i=0;i<m_clusters.size();++i) + } + } while (changed && (iterations < maxiterations)); + /* Merge */ + btAlignedObjectArray<int> cids; + cids.resize(m_nodes.size(), -1); + for (i = 0; i < m_clusters.size(); ++i) { - for(int j=0;j<m_clusters[i]->m_nodes.size();++j) + for (int j = 0; j < m_clusters[i]->m_nodes.size(); ++j) { - cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i; + cids[int(m_clusters[i]->m_nodes[j] - &m_nodes[0])] = i; } } - for(i=0;i<m_faces.size();++i) + for (i = 0; i < m_faces.size(); ++i) { - const int idx[]={ int(m_faces[i].m_n[0]-&m_nodes[0]), - int(m_faces[i].m_n[1]-&m_nodes[0]), - int(m_faces[i].m_n[2]-&m_nodes[0])}; - for(int j=0;j<3;++j) + const int idx[] = {int(m_faces[i].m_n[0] - &m_nodes[0]), + int(m_faces[i].m_n[1] - &m_nodes[0]), + int(m_faces[i].m_n[2] - &m_nodes[0])}; + for (int j = 0; j < 3; ++j) { - const int cid=cids[idx[j]]; - for(int q=1;q<3;++q) + const int cid = cids[idx[j]]; + for (int q = 1; q < 3; ++q) { - const int kid=idx[(j+q)%3]; - if(cids[kid]!=cid) + const int kid = idx[(j + q) % 3]; + if (cids[kid] != cid) { - if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size()) + if (m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid]) == m_clusters[cid]->m_nodes.size()) { m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]); } @@ -1287,55 +1598,56 @@ int btSoftBody::generateClusters(int k,int maxiterations) } } } - /* Master */ - if(m_clusters.size()>1) + /* Master */ + if (m_clusters.size() > 1) { - Cluster* pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); - pmaster->m_collide = false; + Cluster* pmaster = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster(); + pmaster->m_collide = false; pmaster->m_nodes.reserve(m_nodes.size()); - for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]); + for (int i = 0; i < m_nodes.size(); ++i) pmaster->m_nodes.push_back(&m_nodes[i]); m_clusters.push_back(pmaster); - btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]); + btSwap(m_clusters[0], m_clusters[m_clusters.size() - 1]); } - /* Terminate */ - for(i=0;i<m_clusters.size();++i) + /* Terminate */ + for (i = 0; i < m_clusters.size(); ++i) { - if(m_clusters[i]->m_nodes.size()==0) + if (m_clusters[i]->m_nodes.size() == 0) { releaseCluster(i--); } } - } else + } + else { //create a cluster for each tetrahedron (if tetrahedra exist) or each face if (m_tetras.size()) { m_clusters.resize(m_tetras.size()); - for(i=0;i<m_clusters.size();++i) + for (i = 0; i < m_clusters.size(); ++i) { - m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); - m_clusters[i]->m_collide= true; + m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster(); + m_clusters[i]->m_collide = true; } - for (i=0;i<m_tetras.size();i++) + for (i = 0; i < m_tetras.size(); i++) { - for (int j=0;j<4;j++) + for (int j = 0; j < 4; j++) { m_clusters[i]->m_nodes.push_back(m_tetras[i].m_n[j]); } } - - } else + } + else { m_clusters.resize(m_faces.size()); - for(i=0;i<m_clusters.size();++i) + for (i = 0; i < m_clusters.size(); ++i) { - m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); - m_clusters[i]->m_collide= true; + m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster(); + m_clusters[i]->m_collide = true; } - for(i=0;i<m_faces.size();++i) + for (i = 0; i < m_faces.size(); ++i) { - for(int j=0;j<3;++j) + for (int j = 0; j < 3; ++j) { m_clusters[i]->m_nodes.push_back(m_faces[i].m_n[j]); } @@ -1348,261 +1660,272 @@ int btSoftBody::generateClusters(int k,int maxiterations) initializeClusters(); updateClusters(); - //for self-collision - m_clusterConnectivity.resize(m_clusters.size()*m_clusters.size()); + m_clusterConnectivity.resize(m_clusters.size() * m_clusters.size()); { - for (int c0=0;c0<m_clusters.size();c0++) + for (int c0 = 0; c0 < m_clusters.size(); c0++) { - m_clusters[c0]->m_clusterIndex=c0; - for (int c1=0;c1<m_clusters.size();c1++) + m_clusters[c0]->m_clusterIndex = c0; + for (int c1 = 0; c1 < m_clusters.size(); c1++) { - - bool connected=false; + bool connected = false; Cluster* cla = m_clusters[c0]; Cluster* clb = m_clusters[c1]; - for (int i=0;!connected&&i<cla->m_nodes.size();i++) + for (int i = 0; !connected && i < cla->m_nodes.size(); i++) { - for (int j=0;j<clb->m_nodes.size();j++) + for (int j = 0; j < clb->m_nodes.size(); j++) { if (cla->m_nodes[i] == clb->m_nodes[j]) { - connected=true; + connected = true; break; } } } - m_clusterConnectivity[c0+c1*m_clusters.size()]=connected; + m_clusterConnectivity[c0 + c1 * m_clusters.size()] = connected; } } } } - return(m_clusters.size()); + return (m_clusters.size()); } // -void btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut) +void btSoftBody::refine(ImplicitFn* ifn, btScalar accurary, bool cut) { - const Node* nbase = &m_nodes[0]; - int ncount = m_nodes.size(); - btSymMatrix<int> edges(ncount,-2); - int newnodes=0; - int i,j,k,ni; - - /* Filter out */ - for(i=0;i<m_links.size();++i) + const Node* nbase = &m_nodes[0]; + int ncount = m_nodes.size(); + btSymMatrix<int> edges(ncount, -2); + int newnodes = 0; + int i, j, k, ni; + + /* Filter out */ + for (i = 0; i < m_links.size(); ++i) { - Link& l=m_links[i]; - if(l.m_bbending) + Link& l = m_links[i]; + if (l.m_bbending) { - if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x))) + if (!SameSign(ifn->Eval(l.m_n[0]->m_x), ifn->Eval(l.m_n[1]->m_x))) { - btSwap(m_links[i],m_links[m_links.size()-1]); - m_links.pop_back();--i; + btSwap(m_links[i], m_links[m_links.size() - 1]); + m_links.pop_back(); + --i; } - } + } } - /* Fill edges */ - for(i=0;i<m_links.size();++i) + /* Fill edges */ + for (i = 0; i < m_links.size(); ++i) { - Link& l=m_links[i]; - edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1; + Link& l = m_links[i]; + edges(int(l.m_n[0] - nbase), int(l.m_n[1] - nbase)) = -1; } - for(i=0;i<m_faces.size();++i) - { - Face& f=m_faces[i]; - edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1; - edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1; - edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1; + for (i = 0; i < m_faces.size(); ++i) + { + Face& f = m_faces[i]; + edges(int(f.m_n[0] - nbase), int(f.m_n[1] - nbase)) = -1; + edges(int(f.m_n[1] - nbase), int(f.m_n[2] - nbase)) = -1; + edges(int(f.m_n[2] - nbase), int(f.m_n[0] - nbase)) = -1; } - /* Intersect */ - for(i=0;i<ncount;++i) + /* Intersect */ + for (i = 0; i < ncount; ++i) { - for(j=i+1;j<ncount;++j) + for (j = i + 1; j < ncount; ++j) { - if(edges(i,j)==-1) + if (edges(i, j) == -1) { - Node& a=m_nodes[i]; - Node& b=m_nodes[j]; - const btScalar t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary); - if(t>0) + Node& a = m_nodes[i]; + Node& b = m_nodes[j]; + const btScalar t = ImplicitSolve(ifn, a.m_x, b.m_x, accurary); + if (t > 0) { - const btVector3 x=Lerp(a.m_x,b.m_x,t); - const btVector3 v=Lerp(a.m_v,b.m_v,t); - btScalar m=0; - if(a.m_im>0) + const btVector3 x = Lerp(a.m_x, b.m_x, t); + const btVector3 v = Lerp(a.m_v, b.m_v, t); + btScalar m = 0; + if (a.m_im > 0) { - if(b.m_im>0) + if (b.m_im > 0) { - const btScalar ma=1/a.m_im; - const btScalar mb=1/b.m_im; - const btScalar mc=Lerp(ma,mb,t); - const btScalar f=(ma+mb)/(ma+mb+mc); - a.m_im=1/(ma*f); - b.m_im=1/(mb*f); - m=mc*f; + const btScalar ma = 1 / a.m_im; + const btScalar mb = 1 / b.m_im; + const btScalar mc = Lerp(ma, mb, t); + const btScalar f = (ma + mb) / (ma + mb + mc); + a.m_im = 1 / (ma * f); + b.m_im = 1 / (mb * f); + m = mc * f; } else - { a.m_im/=0.5f;m=1/a.m_im; } + { + a.m_im /= 0.5f; + m = 1 / a.m_im; + } } else { - if(b.m_im>0) - { b.m_im/=0.5f;m=1/b.m_im; } + if (b.m_im > 0) + { + b.m_im /= 0.5f; + m = 1 / b.m_im; + } else - m=0; + m = 0; } - appendNode(x,m); - edges(i,j)=m_nodes.size()-1; - m_nodes[edges(i,j)].m_v=v; + appendNode(x, m); + edges(i, j) = m_nodes.size() - 1; + m_nodes[edges(i, j)].m_v = v; ++newnodes; } } } } - nbase=&m_nodes[0]; - /* Refine links */ - for(i=0,ni=m_links.size();i<ni;++i) + nbase = &m_nodes[0]; + /* Refine links */ + for (i = 0, ni = m_links.size(); i < ni; ++i) { - Link& feat=m_links[i]; - const int idx[]={ int(feat.m_n[0]-nbase), - int(feat.m_n[1]-nbase)}; - if((idx[0]<ncount)&&(idx[1]<ncount)) + Link& feat = m_links[i]; + const int idx[] = {int(feat.m_n[0] - nbase), + int(feat.m_n[1] - nbase)}; + if ((idx[0] < ncount) && (idx[1] < ncount)) { - const int ni=edges(idx[0],idx[1]); - if(ni>0) + const int ni = edges(idx[0], idx[1]); + if (ni > 0) { appendLink(i); - Link* pft[]={ &m_links[i], - &m_links[m_links.size()-1]}; - pft[0]->m_n[0]=&m_nodes[idx[0]]; - pft[0]->m_n[1]=&m_nodes[ni]; - pft[1]->m_n[0]=&m_nodes[ni]; - pft[1]->m_n[1]=&m_nodes[idx[1]]; + Link* pft[] = {&m_links[i], + &m_links[m_links.size() - 1]}; + pft[0]->m_n[0] = &m_nodes[idx[0]]; + pft[0]->m_n[1] = &m_nodes[ni]; + pft[1]->m_n[0] = &m_nodes[ni]; + pft[1]->m_n[1] = &m_nodes[idx[1]]; } } } - /* Refine faces */ - for(i=0;i<m_faces.size();++i) + /* Refine faces */ + for (i = 0; i < m_faces.size(); ++i) { - const Face& feat=m_faces[i]; - const int idx[]={ int(feat.m_n[0]-nbase), - int(feat.m_n[1]-nbase), - int(feat.m_n[2]-nbase)}; - for(j=2,k=0;k<3;j=k++) + const Face& feat = m_faces[i]; + const int idx[] = {int(feat.m_n[0] - nbase), + int(feat.m_n[1] - nbase), + int(feat.m_n[2] - nbase)}; + for (j = 2, k = 0; k < 3; j = k++) { - if((idx[j]<ncount)&&(idx[k]<ncount)) + if ((idx[j] < ncount) && (idx[k] < ncount)) { - const int ni=edges(idx[j],idx[k]); - if(ni>0) + const int ni = edges(idx[j], idx[k]); + if (ni > 0) { appendFace(i); - const int l=(k+1)%3; - Face* pft[]={ &m_faces[i], - &m_faces[m_faces.size()-1]}; - pft[0]->m_n[0]=&m_nodes[idx[l]]; - pft[0]->m_n[1]=&m_nodes[idx[j]]; - pft[0]->m_n[2]=&m_nodes[ni]; - pft[1]->m_n[0]=&m_nodes[ni]; - pft[1]->m_n[1]=&m_nodes[idx[k]]; - pft[1]->m_n[2]=&m_nodes[idx[l]]; - appendLink(ni,idx[l],pft[0]->m_material); - --i;break; + const int l = (k + 1) % 3; + Face* pft[] = {&m_faces[i], + &m_faces[m_faces.size() - 1]}; + pft[0]->m_n[0] = &m_nodes[idx[l]]; + pft[0]->m_n[1] = &m_nodes[idx[j]]; + pft[0]->m_n[2] = &m_nodes[ni]; + pft[1]->m_n[0] = &m_nodes[ni]; + pft[1]->m_n[1] = &m_nodes[idx[k]]; + pft[1]->m_n[2] = &m_nodes[idx[l]]; + appendLink(ni, idx[l], pft[0]->m_material); + --i; + break; } } } } - /* Cut */ - if(cut) - { - btAlignedObjectArray<int> cnodes; - const int pcount=ncount; - int i; - ncount=m_nodes.size(); - cnodes.resize(ncount,0); - /* Nodes */ - for(i=0;i<ncount;++i) + /* Cut */ + if (cut) + { + btAlignedObjectArray<int> cnodes; + const int pcount = ncount; + int i; + ncount = m_nodes.size(); + cnodes.resize(ncount, 0); + /* Nodes */ + for (i = 0; i < ncount; ++i) { - const btVector3 x=m_nodes[i].m_x; - if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary)) + const btVector3 x = m_nodes[i].m_x; + if ((i >= pcount) || (btFabs(ifn->Eval(x)) < accurary)) { - const btVector3 v=m_nodes[i].m_v; - btScalar m=getMass(i); - if(m>0) { m*=0.5f;m_nodes[i].m_im/=0.5f; } - appendNode(x,m); - cnodes[i]=m_nodes.size()-1; - m_nodes[cnodes[i]].m_v=v; + const btVector3 v = m_nodes[i].m_v; + btScalar m = getMass(i); + if (m > 0) + { + m *= 0.5f; + m_nodes[i].m_im /= 0.5f; + } + appendNode(x, m); + cnodes[i] = m_nodes.size() - 1; + m_nodes[cnodes[i]].m_v = v; } } - nbase=&m_nodes[0]; - /* Links */ - for(i=0,ni=m_links.size();i<ni;++i) + nbase = &m_nodes[0]; + /* Links */ + for (i = 0, ni = m_links.size(); i < ni; ++i) { - const int id[]={ int(m_links[i].m_n[0]-nbase), - int(m_links[i].m_n[1]-nbase)}; - int todetach=0; - if(cnodes[id[0]]&&cnodes[id[1]]) + const int id[] = {int(m_links[i].m_n[0] - nbase), + int(m_links[i].m_n[1] - nbase)}; + int todetach = 0; + if (cnodes[id[0]] && cnodes[id[1]]) { appendLink(i); - todetach=m_links.size()-1; + todetach = m_links.size() - 1; } else { - if(( (ifn->Eval(m_nodes[id[0]].m_x)<accurary)&& - (ifn->Eval(m_nodes[id[1]].m_x)<accurary))) - todetach=i; + if (((ifn->Eval(m_nodes[id[0]].m_x) < accurary) && + (ifn->Eval(m_nodes[id[1]].m_x) < accurary))) + todetach = i; } - if(todetach) + if (todetach) { - Link& l=m_links[todetach]; - for(int j=0;j<2;++j) + Link& l = m_links[todetach]; + for (int j = 0; j < 2; ++j) { - int cn=cnodes[int(l.m_n[j]-nbase)]; - if(cn) l.m_n[j]=&m_nodes[cn]; - } + int cn = cnodes[int(l.m_n[j] - nbase)]; + if (cn) l.m_n[j] = &m_nodes[cn]; + } } } - /* Faces */ - for(i=0,ni=m_faces.size();i<ni;++i) + /* Faces */ + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - Node** n= m_faces[i].m_n; - if( (ifn->Eval(n[0]->m_x)<accurary)&& - (ifn->Eval(n[1]->m_x)<accurary)&& - (ifn->Eval(n[2]->m_x)<accurary)) + Node** n = m_faces[i].m_n; + if ((ifn->Eval(n[0]->m_x) < accurary) && + (ifn->Eval(n[1]->m_x) < accurary) && + (ifn->Eval(n[2]->m_x) < accurary)) { - for(int j=0;j<3;++j) + for (int j = 0; j < 3; ++j) { - int cn=cnodes[int(n[j]-nbase)]; - if(cn) n[j]=&m_nodes[cn]; + int cn = cnodes[int(n[j] - nbase)]; + if (cn) n[j] = &m_nodes[cn]; } } } - /* Clean orphans */ - int nnodes=m_nodes.size(); - btAlignedObjectArray<int> ranks; - btAlignedObjectArray<int> todelete; - ranks.resize(nnodes,0); - for(i=0,ni=m_links.size();i<ni;++i) + /* Clean orphans */ + int nnodes = m_nodes.size(); + btAlignedObjectArray<int> ranks; + btAlignedObjectArray<int> todelete; + ranks.resize(nnodes, 0); + for (i = 0, ni = m_links.size(); i < ni; ++i) { - for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++; + for (int j = 0; j < 2; ++j) ranks[int(m_links[i].m_n[j] - nbase)]++; } - for(i=0,ni=m_faces.size();i<ni;++i) + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++; + for (int j = 0; j < 3; ++j) ranks[int(m_faces[i].m_n[j] - nbase)]++; } - for(i=0;i<m_links.size();++i) + for (i = 0; i < m_links.size(); ++i) { - const int id[]={ int(m_links[i].m_n[0]-nbase), - int(m_links[i].m_n[1]-nbase)}; - const bool sg[]={ ranks[id[0]]==1, - ranks[id[1]]==1}; - if(sg[0]||sg[1]) + const int id[] = {int(m_links[i].m_n[0] - nbase), + int(m_links[i].m_n[1] - nbase)}; + const bool sg[] = {ranks[id[0]] == 1, + ranks[id[1]] == 1}; + if (sg[0] || sg[1]) { --ranks[id[0]]; --ranks[id[1]]; - btSwap(m_links[i],m_links[m_links.size()-1]); - m_links.pop_back();--i; + btSwap(m_links[i], m_links[m_links.size() - 1]); + m_links.pop_back(); + --i; } } #if 0 @@ -1629,679 +1952,999 @@ void btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut) } #endif } - m_bUpdateRtCst=true; + m_bUpdateRtCst = true; } // -bool btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position) +bool btSoftBody::cutLink(const Node* node0, const Node* node1, btScalar position) { - return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position)); + return (cutLink(int(node0 - &m_nodes[0]), int(node1 - &m_nodes[0]), position)); } // -bool btSoftBody::cutLink(int node0,int node1,btScalar position) +bool btSoftBody::cutLink(int node0, int node1, btScalar position) { - bool done=false; - int i,ni; -// const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x; - const btVector3 x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position); - const btVector3 v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position); - const btScalar m=1; - appendNode(x,m); - appendNode(x,m); - Node* pa=&m_nodes[node0]; - Node* pb=&m_nodes[node1]; - Node* pn[2]={ &m_nodes[m_nodes.size()-2], - &m_nodes[m_nodes.size()-1]}; - pn[0]->m_v=v; - pn[1]->m_v=v; - for(i=0,ni=m_links.size();i<ni;++i) + bool done = false; + int i, ni; + // const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x; + const btVector3 x = Lerp(m_nodes[node0].m_x, m_nodes[node1].m_x, position); + const btVector3 v = Lerp(m_nodes[node0].m_v, m_nodes[node1].m_v, position); + const btScalar m = 1; + appendNode(x, m); + appendNode(x, m); + Node* pa = &m_nodes[node0]; + Node* pb = &m_nodes[node1]; + Node* pn[2] = {&m_nodes[m_nodes.size() - 2], + &m_nodes[m_nodes.size() - 1]}; + pn[0]->m_v = v; + pn[1]->m_v = v; + for (i = 0, ni = m_links.size(); i < ni; ++i) { - const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb); - if(mtch!=-1) + const int mtch = MatchEdge(m_links[i].m_n[0], m_links[i].m_n[1], pa, pb); + if (mtch != -1) { appendLink(i); - Link* pft[]={&m_links[i],&m_links[m_links.size()-1]}; - pft[0]->m_n[1]=pn[mtch]; - pft[1]->m_n[0]=pn[1-mtch]; - done=true; + Link* pft[] = {&m_links[i], &m_links[m_links.size() - 1]}; + pft[0]->m_n[1] = pn[mtch]; + pft[1]->m_n[0] = pn[1 - mtch]; + done = true; } } - for(i=0,ni=m_faces.size();i<ni;++i) + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - for(int k=2,l=0;l<3;k=l++) + for (int k = 2, l = 0; l < 3; k = l++) { - const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb); - if(mtch!=-1) + const int mtch = MatchEdge(m_faces[i].m_n[k], m_faces[i].m_n[l], pa, pb); + if (mtch != -1) { appendFace(i); - Face* pft[]={&m_faces[i],&m_faces[m_faces.size()-1]}; - pft[0]->m_n[l]=pn[mtch]; - pft[1]->m_n[k]=pn[1-mtch]; - appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); - appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); + Face* pft[] = {&m_faces[i], &m_faces[m_faces.size() - 1]}; + pft[0]->m_n[l] = pn[mtch]; + pft[1]->m_n[k] = pn[1 - mtch]; + appendLink(pn[0], pft[0]->m_n[(l + 1) % 3], pft[0]->m_material, true); + appendLink(pn[1], pft[0]->m_n[(l + 1) % 3], pft[0]->m_material, true); } } } - if(!done) + if (!done) { m_ndbvt.remove(pn[0]->m_leaf); m_ndbvt.remove(pn[1]->m_leaf); m_nodes.pop_back(); m_nodes.pop_back(); } - return(done); + return (done); } // -bool btSoftBody::rayTest(const btVector3& rayFrom, - const btVector3& rayTo, - sRayCast& results) +bool btSoftBody::rayTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results) { - if(m_faces.size()&&m_fdbvt.empty()) + if (m_faces.size() && m_fdbvt.empty()) initializeFaceTree(); - results.body = this; + results.body = this; results.fraction = 1.f; - results.feature = eFeature::None; - results.index = -1; + results.feature = eFeature::None; + results.index = -1; - return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0); + return (rayTest(rayFrom, rayTo, results.fraction, results.feature, results.index, false) != 0); +} + +bool btSoftBody::rayFaceTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results) +{ + if (m_faces.size() == 0) + return false; + else + { + if (m_fdbvt.empty()) + initializeFaceTree(); + } + + results.body = this; + results.fraction = 1.f; + results.index = -1; + + return (rayFaceTest(rayFrom, rayTo, results.fraction, results.index) != 0); } // -void btSoftBody::setSolver(eSolverPresets::_ preset) +void btSoftBody::setSolver(eSolverPresets::_ preset) { m_cfg.m_vsequence.clear(); m_cfg.m_psequence.clear(); m_cfg.m_dsequence.clear(); - switch(preset) - { - case eSolverPresets::Positions: - m_cfg.m_psequence.push_back(ePSolver::Anchors); - m_cfg.m_psequence.push_back(ePSolver::RContacts); - m_cfg.m_psequence.push_back(ePSolver::SContacts); - m_cfg.m_psequence.push_back(ePSolver::Linear); - break; - case eSolverPresets::Velocities: - m_cfg.m_vsequence.push_back(eVSolver::Linear); - - m_cfg.m_psequence.push_back(ePSolver::Anchors); - m_cfg.m_psequence.push_back(ePSolver::RContacts); - m_cfg.m_psequence.push_back(ePSolver::SContacts); - - m_cfg.m_dsequence.push_back(ePSolver::Linear); - break; + switch (preset) + { + case eSolverPresets::Positions: + m_cfg.m_psequence.push_back(ePSolver::Anchors); + m_cfg.m_psequence.push_back(ePSolver::RContacts); + m_cfg.m_psequence.push_back(ePSolver::SContacts); + m_cfg.m_psequence.push_back(ePSolver::Linear); + break; + case eSolverPresets::Velocities: + m_cfg.m_vsequence.push_back(eVSolver::Linear); + + m_cfg.m_psequence.push_back(ePSolver::Anchors); + m_cfg.m_psequence.push_back(ePSolver::RContacts); + m_cfg.m_psequence.push_back(ePSolver::SContacts); + + m_cfg.m_dsequence.push_back(ePSolver::Linear); + break; } } -// -void btSoftBody::predictMotion(btScalar dt) +void btSoftBody::predictMotion(btScalar dt) { + int i, ni; - int i,ni; - - /* Update */ - if(m_bUpdateRtCst) + /* Update */ + if (m_bUpdateRtCst) { - m_bUpdateRtCst=false; + m_bUpdateRtCst = false; updateConstants(); m_fdbvt.clear(); - if(m_cfg.collisions&fCollision::VF_SS) + if (m_cfg.collisions & fCollision::VF_SS) { - initializeFaceTree(); + initializeFaceTree(); } } - /* Prepare */ - m_sst.sdt = dt*m_cfg.timescale; - m_sst.isdt = 1/m_sst.sdt; - m_sst.velmrg = m_sst.sdt*3; - m_sst.radmrg = getCollisionShape()->getMargin(); - m_sst.updmrg = m_sst.radmrg*(btScalar)0.25; - /* Forces */ - addVelocity(m_worldInfo->m_gravity*m_sst.sdt); + /* Prepare */ + m_sst.sdt = dt * m_cfg.timescale; + m_sst.isdt = 1 / m_sst.sdt; + m_sst.velmrg = m_sst.sdt * 3; + m_sst.radmrg = getCollisionShape()->getMargin(); + m_sst.updmrg = m_sst.radmrg * (btScalar)0.25; + /* Forces */ + addVelocity(m_worldInfo->m_gravity * m_sst.sdt); applyForces(); - /* Integrate */ - for(i=0,ni=m_nodes.size();i<ni;++i) + /* Integrate */ + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - n.m_q = n.m_x; - btVector3 deltaV = n.m_f*n.m_im*m_sst.sdt; + Node& n = m_nodes[i]; + n.m_q = n.m_x; + btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt; { btScalar maxDisplacement = m_worldInfo->m_maxDisplacement; - btScalar clampDeltaV = maxDisplacement/m_sst.sdt; - for (int c=0;c<3;c++) + btScalar clampDeltaV = maxDisplacement / m_sst.sdt; + for (int c = 0; c < 3; c++) { - if (deltaV[c]>clampDeltaV) + if (deltaV[c] > clampDeltaV) { deltaV[c] = clampDeltaV; } - if (deltaV[c]<-clampDeltaV) + if (deltaV[c] < -clampDeltaV) { - deltaV[c]=-clampDeltaV; + deltaV[c] = -clampDeltaV; } } } - n.m_v += deltaV; - n.m_x += n.m_v*m_sst.sdt; - n.m_f = btVector3(0,0,0); + n.m_v += deltaV; + n.m_x += n.m_v * m_sst.sdt; + n.m_f = btVector3(0, 0, 0); } - /* Clusters */ + /* Clusters */ updateClusters(); - /* Bounds */ - updateBounds(); - /* Nodes */ - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; - for(i=0,ni=m_nodes.size();i<ni;++i) - { - Node& n=m_nodes[i]; - vol = btDbvtVolume::FromCR(n.m_x,m_sst.radmrg); - m_ndbvt.update( n.m_leaf, - vol, - n.m_v*m_sst.velmrg, - m_sst.updmrg); - } - /* Faces */ - if(!m_fdbvt.empty()) - { - for(int i=0;i<m_faces.size();++i) - { - Face& f=m_faces[i]; - const btVector3 v=( f.m_n[0]->m_v+ - f.m_n[1]->m_v+ - f.m_n[2]->m_v)/3; - vol = VolumeOf(f,m_sst.radmrg); - m_fdbvt.update( f.m_leaf, - vol, - v*m_sst.velmrg, - m_sst.updmrg); - } - } - /* Pose */ + /* Bounds */ + updateBounds(); + /* Nodes */ + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + for (i = 0, ni = m_nodes.size(); i < ni; ++i) + { + Node& n = m_nodes[i]; + vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg); + m_ndbvt.update(n.m_leaf, + vol, + n.m_v * m_sst.velmrg, + m_sst.updmrg); + } + /* Faces */ + if (!m_fdbvt.empty()) + { + for (int i = 0; i < m_faces.size(); ++i) + { + Face& f = m_faces[i]; + const btVector3 v = (f.m_n[0]->m_v + + f.m_n[1]->m_v + + f.m_n[2]->m_v) / + 3; + vol = VolumeOf(f, m_sst.radmrg); + m_fdbvt.update(f.m_leaf, + vol, + v * m_sst.velmrg, + m_sst.updmrg); + } + } + /* Pose */ updatePose(); - /* Match */ - if(m_pose.m_bframe&&(m_cfg.kMT>0)) + /* Match */ + if (m_pose.m_bframe && (m_cfg.kMT > 0)) { - const btMatrix3x3 posetrs=m_pose.m_rot; - for(int i=0,ni=m_nodes.size();i<ni;++i) + const btMatrix3x3 posetrs = m_pose.m_rot; + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - if(n.m_im>0) + Node& n = m_nodes[i]; + if (n.m_im > 0) { - const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com; - n.m_x=Lerp(n.m_x,x,m_cfg.kMT); + const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com; + n.m_x = Lerp(n.m_x, x, m_cfg.kMT); } } } - /* Clear contacts */ + /* Clear contacts */ m_rcontacts.resize(0); m_scontacts.resize(0); - /* Optimize dbvt's */ + /* Optimize dbvt's */ m_ndbvt.optimizeIncremental(1); m_fdbvt.optimizeIncremental(1); m_cdbvt.optimizeIncremental(1); } // -void btSoftBody::solveConstraints() +void btSoftBody::solveConstraints() { - - /* Apply clusters */ + /* Apply clusters */ applyClusters(false); - /* Prepare links */ + /* Prepare links */ - int i,ni; + int i, ni; - for(i=0,ni=m_links.size();i<ni;++i) + for (i = 0, ni = m_links.size(); i < ni; ++i) { - Link& l=m_links[i]; - l.m_c3 = l.m_n[1]->m_q-l.m_n[0]->m_q; - l.m_c2 = 1/(l.m_c3.length2()*l.m_c0); + Link& l = m_links[i]; + l.m_c3 = l.m_n[1]->m_q - l.m_n[0]->m_q; + l.m_c2 = 1 / (l.m_c3.length2() * l.m_c0); } - /* Prepare anchors */ - for(i=0,ni=m_anchors.size();i<ni;++i) + /* Prepare anchors */ + for (i = 0, ni = m_anchors.size(); i < ni; ++i) { - Anchor& a=m_anchors[i]; - const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local; - a.m_c0 = ImpulseMatrix( m_sst.sdt, - a.m_node->m_im, - a.m_body->getInvMass(), - a.m_body->getInvInertiaTensorWorld(), - ra); - a.m_c1 = ra; - a.m_c2 = m_sst.sdt*a.m_node->m_im; + Anchor& a = m_anchors[i]; + const btVector3 ra = a.m_body->getWorldTransform().getBasis() * a.m_local; + a.m_c0 = ImpulseMatrix(m_sst.sdt, + a.m_node->m_im, + a.m_body->getInvMass(), + a.m_body->getInvInertiaTensorWorld(), + ra); + a.m_c1 = ra; + a.m_c2 = m_sst.sdt * a.m_node->m_im; a.m_body->activate(); } - /* Solve velocities */ - if(m_cfg.viterations>0) + /* Solve velocities */ + if (m_cfg.viterations > 0) { - /* Solve */ - for(int isolve=0;isolve<m_cfg.viterations;++isolve) + /* Solve */ + for (int isolve = 0; isolve < m_cfg.viterations; ++isolve) { - for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq) + for (int iseq = 0; iseq < m_cfg.m_vsequence.size(); ++iseq) { - getSolver(m_cfg.m_vsequence[iseq])(this,1); + getSolver(m_cfg.m_vsequence[iseq])(this, 1); } } - /* Update */ - for(i=0,ni=m_nodes.size();i<ni;++i) + /* Update */ + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - n.m_x = n.m_q+n.m_v*m_sst.sdt; + Node& n = m_nodes[i]; + n.m_x = n.m_q + n.m_v * m_sst.sdt; } } - /* Solve positions */ - if(m_cfg.piterations>0) + /* Solve positions */ + if (m_cfg.piterations > 0) { - for(int isolve=0;isolve<m_cfg.piterations;++isolve) + for (int isolve = 0; isolve < m_cfg.piterations; ++isolve) { - const btScalar ti=isolve/(btScalar)m_cfg.piterations; - for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq) + const btScalar ti = isolve / (btScalar)m_cfg.piterations; + for (int iseq = 0; iseq < m_cfg.m_psequence.size(); ++iseq) { - getSolver(m_cfg.m_psequence[iseq])(this,1,ti); + getSolver(m_cfg.m_psequence[iseq])(this, 1, ti); } } - const btScalar vc=m_sst.isdt*(1-m_cfg.kDP); - for(i=0,ni=m_nodes.size();i<ni;++i) + const btScalar vc = m_sst.isdt * (1 - m_cfg.kDP); + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - n.m_v = (n.m_x-n.m_q)*vc; - n.m_f = btVector3(0,0,0); + Node& n = m_nodes[i]; + n.m_v = (n.m_x - n.m_q) * vc; + n.m_f = btVector3(0, 0, 0); } } - /* Solve drift */ - if(m_cfg.diterations>0) + /* Solve drift */ + if (m_cfg.diterations > 0) { - const btScalar vcf=m_cfg.kVCF*m_sst.isdt; - for(i=0,ni=m_nodes.size();i<ni;++i) + const btScalar vcf = m_cfg.kVCF * m_sst.isdt; + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - n.m_q = n.m_x; + Node& n = m_nodes[i]; + n.m_q = n.m_x; } - for(int idrift=0;idrift<m_cfg.diterations;++idrift) + for (int idrift = 0; idrift < m_cfg.diterations; ++idrift) { - for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq) + for (int iseq = 0; iseq < m_cfg.m_dsequence.size(); ++iseq) { - getSolver(m_cfg.m_dsequence[iseq])(this,1,0); + getSolver(m_cfg.m_dsequence[iseq])(this, 1, 0); } } - for(int i=0,ni=m_nodes.size();i<ni;++i) + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - n.m_v += (n.m_x-n.m_q)*vcf; + Node& n = m_nodes[i]; + n.m_v += (n.m_x - n.m_q) * vcf; } } - /* Apply clusters */ + /* Apply clusters */ dampClusters(); applyClusters(true); } // -void btSoftBody::staticSolve(int iterations) +void btSoftBody::staticSolve(int iterations) { - for(int isolve=0;isolve<iterations;++isolve) + for (int isolve = 0; isolve < iterations; ++isolve) { - for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq) + for (int iseq = 0; iseq < m_cfg.m_psequence.size(); ++iseq) { - getSolver(m_cfg.m_psequence[iseq])(this,1,0); + getSolver(m_cfg.m_psequence[iseq])(this, 1, 0); } } } // -void btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/) +void btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/, int /*count*/, int /*iterations*/) { /// placeholder } // -void btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies) +void btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies) { - const int nb=bodies.size(); - int iterations=0; + const int nb = bodies.size(); + int iterations = 0; int i; - for(i=0;i<nb;++i) + for (i = 0; i < nb; ++i) { - iterations=btMax(iterations,bodies[i]->m_cfg.citerations); + iterations = btMax(iterations, bodies[i]->m_cfg.citerations); } - for(i=0;i<nb;++i) + for (i = 0; i < nb; ++i) { bodies[i]->prepareClusters(iterations); } - for(i=0;i<iterations;++i) + for (i = 0; i < iterations; ++i) { - const btScalar sor=1; - for(int j=0;j<nb;++j) + const btScalar sor = 1; + for (int j = 0; j < nb; ++j) { bodies[j]->solveClusters(sor); } } - for(i=0;i<nb;++i) + for (i = 0; i < nb; ++i) { bodies[i]->cleanupClusters(); } } // -void btSoftBody::integrateMotion() +void btSoftBody::integrateMotion() { - /* Update */ + /* Update */ updateNormals(); } // -btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt) +btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom, const btVector3& rayTo, btScalar mxt) { m_rayFrom = rayFrom; - m_rayNormalizedDirection = (rayTo-rayFrom); + m_rayNormalizedDirection = (rayTo - rayFrom); m_rayTo = rayTo; - m_mint = mxt; - m_face = 0; - m_tests = 0; + m_mint = mxt; + m_face = 0; + m_tests = 0; } // -void btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf) +void btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf) { - btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data; - const btScalar t=rayFromToTriangle( m_rayFrom,m_rayTo,m_rayNormalizedDirection, - f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - m_mint); - if((t>0)&&(t<m_mint)) - { - m_mint=t;m_face=&f; + btSoftBody::Face& f = *(btSoftBody::Face*)leaf->data; + const btScalar t = rayFromToTriangle(m_rayFrom, m_rayTo, m_rayNormalizedDirection, + f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + m_mint); + if ((t > 0) && (t < m_mint)) + { + m_mint = t; + m_face = &f; } ++m_tests; } // -btScalar btSoftBody::RayFromToCaster::rayFromToTriangle( const btVector3& rayFrom, - const btVector3& rayTo, - const btVector3& rayNormalizedDirection, - const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar maxt) +btScalar btSoftBody::RayFromToCaster::rayFromToTriangle(const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& rayNormalizedDirection, + const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar maxt) { - static const btScalar ceps=-SIMD_EPSILON*10; - static const btScalar teps=SIMD_EPSILON*10; + static const btScalar ceps = -SIMD_EPSILON * 10; + static const btScalar teps = SIMD_EPSILON * 10; - const btVector3 n=btCross(b-a,c-a); - const btScalar d=btDot(a,n); - const btScalar den=btDot(rayNormalizedDirection,n); - if(!btFuzzyZero(den)) + const btVector3 n = btCross(b - a, c - a); + const btScalar d = btDot(a, n); + const btScalar den = btDot(rayNormalizedDirection, n); + if (!btFuzzyZero(den)) { - const btScalar num=btDot(rayFrom,n)-d; - const btScalar t=-num/den; - if((t>teps)&&(t<maxt)) + const btScalar num = btDot(rayFrom, n) - d; + const btScalar t = -num / den; + if ((t > teps) && (t < maxt)) { - const btVector3 hit=rayFrom+rayNormalizedDirection*t; - if( (btDot(n,btCross(a-hit,b-hit))>ceps) && - (btDot(n,btCross(b-hit,c-hit))>ceps) && - (btDot(n,btCross(c-hit,a-hit))>ceps)) + const btVector3 hit = rayFrom + rayNormalizedDirection * t; + if ((btDot(n, btCross(a - hit, b - hit)) > ceps) && + (btDot(n, btCross(b - hit, c - hit)) > ceps) && + (btDot(n, btCross(c - hit, a - hit)) > ceps)) { - return(t); + return (t); } } } - return(-1); + return (-1); } // -void btSoftBody::pointersToIndices() +void btSoftBody::pointersToIndices() { -#define PTR2IDX(_p_,_b_) reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_)) - btSoftBody::Node* base=m_nodes.size() ? &m_nodes[0] : 0; - int i,ni; +#define PTR2IDX(_p_, _b_) reinterpret_cast<btSoftBody::Node*>((_p_) - (_b_)) + btSoftBody::Node* base = m_nodes.size() ? &m_nodes[0] : 0; + int i, ni; - for(i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - if(m_nodes[i].m_leaf) + if (m_nodes[i].m_leaf) { - m_nodes[i].m_leaf->data=*(void**)&i; + m_nodes[i].m_leaf->data = *(void**)&i; } } - for(i=0,ni=m_links.size();i<ni;++i) + for (i = 0, ni = m_links.size(); i < ni; ++i) { - m_links[i].m_n[0]=PTR2IDX(m_links[i].m_n[0],base); - m_links[i].m_n[1]=PTR2IDX(m_links[i].m_n[1],base); + m_links[i].m_n[0] = PTR2IDX(m_links[i].m_n[0], base); + m_links[i].m_n[1] = PTR2IDX(m_links[i].m_n[1], base); } - for(i=0,ni=m_faces.size();i<ni;++i) + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - m_faces[i].m_n[0]=PTR2IDX(m_faces[i].m_n[0],base); - m_faces[i].m_n[1]=PTR2IDX(m_faces[i].m_n[1],base); - m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base); - if(m_faces[i].m_leaf) + m_faces[i].m_n[0] = PTR2IDX(m_faces[i].m_n[0], base); + m_faces[i].m_n[1] = PTR2IDX(m_faces[i].m_n[1], base); + m_faces[i].m_n[2] = PTR2IDX(m_faces[i].m_n[2], base); + if (m_faces[i].m_leaf) { - m_faces[i].m_leaf->data=*(void**)&i; + m_faces[i].m_leaf->data = *(void**)&i; } } - for(i=0,ni=m_anchors.size();i<ni;++i) + for (i = 0, ni = m_anchors.size(); i < ni; ++i) { - m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base); + m_anchors[i].m_node = PTR2IDX(m_anchors[i].m_node, base); } - for(i=0,ni=m_notes.size();i<ni;++i) + for (i = 0, ni = m_notes.size(); i < ni; ++i) { - for(int j=0;j<m_notes[i].m_rank;++j) + for (int j = 0; j < m_notes[i].m_rank; ++j) { - m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base); + m_notes[i].m_nodes[j] = PTR2IDX(m_notes[i].m_nodes[j], base); } } -#undef PTR2IDX +#undef PTR2IDX } // -void btSoftBody::indicesToPointers(const int* map) +void btSoftBody::indicesToPointers(const int* map) { -#define IDX2PTR(_p_,_b_) map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]): \ - (&(_b_)[(((char*)_p_)-(char*)0)]) - btSoftBody::Node* base=m_nodes.size() ? &m_nodes[0]:0; - int i,ni; +#define IDX2PTR(_p_, _b_) map ? (&(_b_)[map[(((char*)_p_) - (char*)0)]]) : (&(_b_)[(((char*)_p_) - (char*)0)]) + btSoftBody::Node* base = m_nodes.size() ? &m_nodes[0] : 0; + int i, ni; - for(i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - if(m_nodes[i].m_leaf) + if (m_nodes[i].m_leaf) { - m_nodes[i].m_leaf->data=&m_nodes[i]; + m_nodes[i].m_leaf->data = &m_nodes[i]; } } - for(i=0,ni=m_links.size();i<ni;++i) + for (i = 0, ni = m_links.size(); i < ni; ++i) { - m_links[i].m_n[0]=IDX2PTR(m_links[i].m_n[0],base); - m_links[i].m_n[1]=IDX2PTR(m_links[i].m_n[1],base); + m_links[i].m_n[0] = IDX2PTR(m_links[i].m_n[0], base); + m_links[i].m_n[1] = IDX2PTR(m_links[i].m_n[1], base); } - for(i=0,ni=m_faces.size();i<ni;++i) + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - m_faces[i].m_n[0]=IDX2PTR(m_faces[i].m_n[0],base); - m_faces[i].m_n[1]=IDX2PTR(m_faces[i].m_n[1],base); - m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base); - if(m_faces[i].m_leaf) + m_faces[i].m_n[0] = IDX2PTR(m_faces[i].m_n[0], base); + m_faces[i].m_n[1] = IDX2PTR(m_faces[i].m_n[1], base); + m_faces[i].m_n[2] = IDX2PTR(m_faces[i].m_n[2], base); + if (m_faces[i].m_leaf) { - m_faces[i].m_leaf->data=&m_faces[i]; + m_faces[i].m_leaf->data = &m_faces[i]; } } - for(i=0,ni=m_anchors.size();i<ni;++i) + for (i = 0, ni = m_anchors.size(); i < ni; ++i) { - m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base); + m_anchors[i].m_node = IDX2PTR(m_anchors[i].m_node, base); } - for(i=0,ni=m_notes.size();i<ni;++i) + for (i = 0, ni = m_notes.size(); i < ni; ++i) { - for(int j=0;j<m_notes[i].m_rank;++j) + for (int j = 0; j < m_notes[i].m_rank; ++j) { - m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base); + m_notes[i].m_nodes[j] = IDX2PTR(m_notes[i].m_nodes[j], base); } } -#undef IDX2PTR +#undef IDX2PTR } // -int btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo, - btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const +int btSoftBody::rayTest(const btVector3& rayFrom, const btVector3& rayTo, + btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const { - int cnt=0; - btVector3 dir = rayTo-rayFrom; - + int cnt = 0; + btVector3 dir = rayTo - rayFrom; - if(bcountonly||m_fdbvt.empty()) - {/* Full search */ - - for(int i=0,ni=m_faces.size();i<ni;++i) - { - const btSoftBody::Face& f=m_faces[i]; + if (bcountonly || m_fdbvt.empty()) + { /* Full search */ - const btScalar t=RayFromToCaster::rayFromToTriangle( rayFrom,rayTo,dir, - f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - mint); - if(t>0) + for (int i = 0, ni = m_faces.size(); i < ni; ++i) + { + const btSoftBody::Face& f = m_faces[i]; + + const btScalar t = RayFromToCaster::rayFromToTriangle(rayFrom, rayTo, dir, + f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + mint); + if (t > 0) { ++cnt; - if(!bcountonly) + if (!bcountonly) { - feature=btSoftBody::eFeature::Face; - index=i; - mint=t; + feature = btSoftBody::eFeature::Face; + index = i; + mint = t; } } } } else - {/* Use dbvt */ - RayFromToCaster collider(rayFrom,rayTo,mint); + { /* Use dbvt */ + RayFromToCaster collider(rayFrom, rayTo, mint); - btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider); - if(collider.m_face) + btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider); + if (collider.m_face) { - mint=collider.m_mint; - feature=btSoftBody::eFeature::Face; - index=(int)(collider.m_face-&m_faces[0]); - cnt=1; + mint = collider.m_mint; + feature = btSoftBody::eFeature::Face; + index = (int)(collider.m_face - &m_faces[0]); + cnt = 1; } } - for (int i=0;i<m_tetras.size();i++) + for (int i = 0; i < m_tetras.size(); i++) { const btSoftBody::Tetra& tet = m_tetras[i]; - int tetfaces[4][3] = {{0,1,2},{0,1,3},{1,2,3},{0,2,3}}; - for (int f=0;f<4;f++) + int tetfaces[4][3] = {{0, 1, 2}, {0, 1, 3}, {1, 2, 3}, {0, 2, 3}}; + for (int f = 0; f < 4; f++) { + int index0 = tetfaces[f][0]; + int index1 = tetfaces[f][1]; + int index2 = tetfaces[f][2]; + btVector3 v0 = tet.m_n[index0]->m_x; + btVector3 v1 = tet.m_n[index1]->m_x; + btVector3 v2 = tet.m_n[index2]->m_x; + + const btScalar t = RayFromToCaster::rayFromToTriangle(rayFrom, rayTo, dir, + v0, v1, v2, + mint); + if (t > 0) + { + ++cnt; + if (!bcountonly) + { + feature = btSoftBody::eFeature::Tetra; + index = i; + mint = t; + } + } + } + } + return (cnt); +} - int index0=tetfaces[f][0]; - int index1=tetfaces[f][1]; - int index2=tetfaces[f][2]; - btVector3 v0=tet.m_n[index0]->m_x; - btVector3 v1=tet.m_n[index1]->m_x; - btVector3 v2=tet.m_n[index2]->m_x; +int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo, + btScalar& mint, int& index) const +{ + int cnt = 0; + { /* Use dbvt */ + RayFromToCaster collider(rayFrom, rayTo, mint); + btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider); + if (collider.m_face) + { + mint = collider.m_mint; + index = (int)(collider.m_face - &m_faces[0]); + cnt = 1; + } + } + return (cnt); +} + +// +static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n) +{ + if (n == 0) + return 0; + btDbvntNode* root = new btDbvntNode(n); + if (n->isinternal()) + { + btDbvntNode* c0 = copyToDbvnt(n->childs[0]); + root->childs[0] = c0; + btDbvntNode* c1 = copyToDbvnt(n->childs[1]); + root->childs[1] = c1; + } + return root; +} - const btScalar t=RayFromToCaster::rayFromToTriangle( rayFrom,rayTo,dir, - v0,v1,v2, - mint); - if(t>0) +static inline void calculateNormalCone(btDbvntNode* root) +{ + if (!root) + return; + if (root->isleaf()) + { + const btSoftBody::Face* face = (btSoftBody::Face*)root->data; + root->normal = face->m_normal; + root->angle = 0; + } + else + { + btVector3 n0(0, 0, 0), n1(0, 0, 0); + btScalar a0 = 0, a1 = 0; + if (root->childs[0]) + { + calculateNormalCone(root->childs[0]); + n0 = root->childs[0]->normal; + a0 = root->childs[0]->angle; + } + if (root->childs[1]) + { + calculateNormalCone(root->childs[1]); + n1 = root->childs[1]->normal; + a1 = root->childs[1]->angle; + } + root->normal = (n0 + n1).safeNormalize(); + root->angle = btMax(a0, a1) + btAngle(n0, n1) * 0.5; + } +} + +void btSoftBody::initializeFaceTree() +{ + BT_PROFILE("btSoftBody::initializeFaceTree"); + m_fdbvt.clear(); + // create leaf nodes; + btAlignedObjectArray<btDbvtNode*> leafNodes; + leafNodes.resize(m_faces.size()); + for (int i = 0; i < m_faces.size(); ++i) + { + Face& f = m_faces[i]; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol = VolumeOf(f, 0); + btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode(); + node->parent = NULL; + node->data = &f; + node->childs[1] = 0; + node->volume = vol; + leafNodes[i] = node; + f.m_leaf = node; + } + btAlignedObjectArray<btAlignedObjectArray<int> > adj; + adj.resize(m_faces.size()); + // construct the adjacency list for triangles + for (int i = 0; i < adj.size(); ++i) + { + for (int j = i + 1; j < adj.size(); ++j) + { + int dup = 0; + for (int k = 0; k < 3; ++k) { - ++cnt; - if(!bcountonly) + for (int l = 0; l < 3; ++l) { - feature=btSoftBody::eFeature::Tetra; - index=i; - mint=t; + if (m_faces[i].m_n[k] == m_faces[j].m_n[l]) + { + ++dup; + break; + } + } + if (dup == 2) + { + adj[i].push_back(j); + adj[j].push_back(i); } } } } - return(cnt); + m_fdbvt.m_root = buildTreeBottomUp(leafNodes, adj); + if (m_fdbvnt) + delete m_fdbvnt; + m_fdbvnt = copyToDbvnt(m_fdbvt.m_root); + updateFaceTree(false, false); + rebuildNodeTree(); } // -void btSoftBody::initializeFaceTree() +void btSoftBody::rebuildNodeTree() { - m_fdbvt.clear(); - for(int i=0;i<m_faces.size();++i) + m_ndbvt.clear(); + btAlignedObjectArray<btDbvtNode*> leafNodes; + leafNodes.resize(m_nodes.size()); + for (int i = 0; i < m_nodes.size(); ++i) + { + Node& n = m_nodes[i]; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol = btDbvtVolume::FromCR(n.m_x, 0); + btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode(); + node->parent = NULL; + node->data = &n; + node->childs[1] = 0; + node->volume = vol; + leafNodes[i] = node; + n.m_leaf = node; + } + btAlignedObjectArray<btAlignedObjectArray<int> > adj; + adj.resize(m_nodes.size()); + btAlignedObjectArray<int> old_id; + old_id.resize(m_nodes.size()); + for (int i = 0; i < m_nodes.size(); ++i) + old_id[i] = m_nodes[i].index; + for (int i = 0; i < m_nodes.size(); ++i) + m_nodes[i].index = i; + for (int i = 0; i < m_links.size(); ++i) { - Face& f=m_faces[i]; - f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f); + Link& l = m_links[i]; + adj[l.m_n[0]->index].push_back(l.m_n[1]->index); + adj[l.m_n[1]->index].push_back(l.m_n[0]->index); } + m_ndbvt.m_root = buildTreeBottomUp(leafNodes, adj); + for (int i = 0; i < m_nodes.size(); ++i) + m_nodes[i].index = old_id[i]; } // -btVector3 btSoftBody::evaluateCom() const +btVector3 btSoftBody::evaluateCom() const { - btVector3 com(0,0,0); - if(m_pose.m_bframe) + btVector3 com(0, 0, 0); + if (m_pose.m_bframe) { - for(int i=0,ni=m_nodes.size();i<ni;++i) + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) { - com+=m_nodes[i].m_x*m_pose.m_wgh[i]; + com += m_nodes[i].m_x * m_pose.m_wgh[i]; } } - return(com); + return (com); } -// -bool btSoftBody::checkContact( const btCollisionObjectWrapper* colObjWrap, - const btVector3& x, - btScalar margin, - btSoftBody::sCti& cti) const +bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, + const btVector3& x, + btScalar margin, + btSoftBody::sCti& cti) const { btVector3 nrm; - const btCollisionShape *shp = colObjWrap->getCollisionShape(); -// const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); + const btCollisionShape* shp = colObjWrap->getCollisionShape(); + // const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform(); - const btTransform &wtr = colObjWrap->getWorldTransform(); + const btTransform& wtr = colObjWrap->getWorldTransform(); //todo: check which transform is needed here - btScalar dst = - m_worldInfo->m_sparsesdf.Evaluate( + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), shp, nrm, margin); - if(dst<0) + if (dst < 0) { cti.m_colObj = colObjWrap->getCollisionObject(); - cti.m_normal = wtr.getBasis()*nrm; - cti.m_offset = -btDot( cti.m_normal, x - cti.m_normal * dst ); - return(true); + cti.m_normal = wtr.getBasis() * nrm; + cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst); + return (true); } - return(false); + return (false); } // -void btSoftBody::updateNormals() +bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, + const btVector3& x, + btScalar margin, + btSoftBody::sCti& cti, bool predict) const { + btVector3 nrm; + const btCollisionShape* shp = colObjWrap->getCollisionShape(); + const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); + // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect + // but resolve contact at x_n + btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) + : colObjWrap->getWorldTransform(); + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(x), + shp, + nrm, + margin); + + if (!predict) + { + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = wtr.getBasis() * nrm; + cti.m_offset = dst; + } + if (dst < 0) + return true; + return (false); +} - const btVector3 zv(0,0,0); - int i,ni; +// +// Compute barycentric coordinates (u, v, w) for +// point p with respect to triangle (a, b, c) +static void getBarycentric(const btVector3& p, btVector3& a, btVector3& b, btVector3& c, btVector3& bary) +{ + btVector3 v0 = b - a, v1 = c - a, v2 = p - a; + btScalar d00 = v0.dot(v0); + btScalar d01 = v0.dot(v1); + btScalar d11 = v1.dot(v1); + btScalar d20 = v2.dot(v0); + btScalar d21 = v2.dot(v1); + btScalar denom = d00 * d11 - d01 * d01; + bary.setY((d11 * d20 - d01 * d21) / denom); + bary.setZ((d00 * d21 - d01 * d20) / denom); + bary.setX(btScalar(1) - bary.getY() - bary.getZ()); +} - for(i=0,ni=m_nodes.size();i<ni;++i) +// +bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, + Face& f, + btVector3& contact_point, + btVector3& bary, + btScalar margin, + btSoftBody::sCti& cti, bool predict) const +{ + btVector3 nrm; + const btCollisionShape* shp = colObjWrap->getCollisionShape(); + const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); + // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect + // but resolve contact at x_n + btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) + : colObjWrap->getWorldTransform(); + btScalar dst; + btGjkEpaSolver2::sResults results; + +// #define USE_QUADRATURE 1 +//#define CACHE_PREV_COLLISION + + // use collision quadrature point +#ifdef USE_QUADRATURE + { + dst = SIMD_INFINITY; + btVector3 local_nrm; + for (int q = 0; q < m_quads.size(); ++q) + { + btVector3 p; + if (predict) + p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]); + else + p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]); + btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(p), + shp, + local_nrm, + margin); + if (local_dst < dst) + { + if (local_dst < 0 && predict) + return true; + dst = local_dst; + contact_point = p; + bary = m_quads[q]; + nrm = local_nrm; + } + if (!predict) + { + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = wtr.getBasis() * nrm; + cti.m_offset = dst; + } + } + return (dst < 0); + } +#endif + + // collision detection using x* + btTransform triangle_transform; + triangle_transform.setIdentity(); + triangle_transform.setOrigin(f.m_n[0]->m_q); + btTriangleShape triangle(btVector3(0, 0, 0), f.m_n[1]->m_q - f.m_n[0]->m_q, f.m_n[2]->m_q - f.m_n[0]->m_q); + btVector3 guess(0, 0, 0); + const btConvexShape* csh = static_cast<const btConvexShape*>(shp); + btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); + dst = results.distance - 2.0 * csh->getMargin() - margin; // margin padding so that the distance = the actual distance between face and rigid - margin of rigid - margin of deformable + if (dst >= 0) + return false; + +// Use consistent barycenter to recalculate distance. +#ifdef CACHE_PREV_COLLISION + if (f.m_pcontact[3] != 0) + { + for (int i = 0; i < 3; ++i) + bary[i] = f.m_pcontact[i]; + contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); + const btConvexShape* csh = static_cast<const btConvexShape*>(shp); + btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr, results); + cti.m_colObj = colObjWrap->getCollisionObject(); + dst = results.distance; + cti.m_normal = results.normal; + cti.m_offset = dst; + + //point-convex CD + wtr = colObjWrap->getWorldTransform(); + btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); + triangle_transform.setOrigin(f.m_n[0]->m_x); + btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results); + + dst = results.distance - csh->getMargin() - margin; + return true; + } +#endif + + // Use triangle-convex CD. + wtr = colObjWrap->getWorldTransform(); + btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); + triangle_transform.setOrigin(f.m_n[0]->m_x); + btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results); + contact_point = results.witnesses[0]; + getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); + + for (int i = 0; i < 3; ++i) + f.m_pcontact[i] = bary[i]; + + dst = results.distance - csh->getMargin() - margin; + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = results.normal; + cti.m_offset = dst; + return true; +} + +void btSoftBody::updateNormals() +{ + const btVector3 zv(0, 0, 0); + int i, ni; + + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - m_nodes[i].m_n=zv; + m_nodes[i].m_n = zv; } - for(i=0,ni=m_faces.size();i<ni;++i) + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - btSoftBody::Face& f=m_faces[i]; - const btVector3 n=btCross(f.m_n[1]->m_x-f.m_n[0]->m_x, - f.m_n[2]->m_x-f.m_n[0]->m_x); - f.m_normal=n.normalized(); - f.m_n[0]->m_n+=n; - f.m_n[1]->m_n+=n; - f.m_n[2]->m_n+=n; + btSoftBody::Face& f = m_faces[i]; + const btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, + f.m_n[2]->m_x - f.m_n[0]->m_x); + f.m_normal = n; + f.m_normal.safeNormalize(); + f.m_n[0]->m_n += n; + f.m_n[1]->m_n += n; + f.m_n[2]->m_n += n; } - for(i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { btScalar len = m_nodes[i].m_n.length(); - if (len>SIMD_EPSILON) + if (len > SIMD_EPSILON) m_nodes[i].m_n /= len; } } // -void btSoftBody::updateBounds() +void btSoftBody::updateBounds() { /*if( m_acceleratedSoftBody ) { @@ -2315,258 +2958,291 @@ void btSoftBody::updateBounds() m_bounds[1] = btVector3(1000, 1000, 1000); } else {*/ - if(m_ndbvt.m_root) - { - const btVector3& mins=m_ndbvt.m_root->volume.Mins(); - const btVector3& maxs=m_ndbvt.m_root->volume.Maxs(); - const btScalar csm=getCollisionShape()->getMargin(); - const btVector3 mrg=btVector3( csm, - csm, - csm)*1; // ??? to investigate... - m_bounds[0]=mins-mrg; - m_bounds[1]=maxs+mrg; - if(0!=getBroadphaseHandle()) - { - m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(), - m_bounds[0], - m_bounds[1], - m_worldInfo->m_dispatcher); + // if (m_ndbvt.m_root) + // { + // const btVector3& mins = m_ndbvt.m_root->volume.Mins(); + // const btVector3& maxs = m_ndbvt.m_root->volume.Maxs(); + // const btScalar csm = getCollisionShape()->getMargin(); + // const btVector3 mrg = btVector3(csm, + // csm, + // csm) * + // 1; // ??? to investigate... + // m_bounds[0] = mins - mrg; + // m_bounds[1] = maxs + mrg; + // if (0 != getBroadphaseHandle()) + // { + // m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), + // m_bounds[0], + // m_bounds[1], + // m_worldInfo->m_dispatcher); + // } + // } + // else + // { + // m_bounds[0] = + // m_bounds[1] = btVector3(0, 0, 0); + // } + if (m_nodes.size()) + { + btVector3 mins = m_nodes[0].m_x; + btVector3 maxs = m_nodes[0].m_x; + for (int i = 1; i < m_nodes.size(); ++i) + { + for (int d = 0; d < 3; ++d) + { + if (m_nodes[i].m_x[d] > maxs[d]) + maxs[d] = m_nodes[i].m_x[d]; + if (m_nodes[i].m_x[d] < mins[d]) + mins[d] = m_nodes[i].m_x[d]; } } - else + const btScalar csm = getCollisionShape()->getMargin(); + const btVector3 mrg = btVector3(csm, + csm, + csm); + m_bounds[0] = mins - mrg; + m_bounds[1] = maxs + mrg; + if (0 != getBroadphaseHandle()) { - m_bounds[0]= - m_bounds[1]=btVector3(0,0,0); - } - //} + m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), + m_bounds[0], + m_bounds[1], + m_worldInfo->m_dispatcher); + } + } + else + { + m_bounds[0] = + m_bounds[1] = btVector3(0, 0, 0); + } } - // -void btSoftBody::updatePose() +void btSoftBody::updatePose() { - if(m_pose.m_bframe) + if (m_pose.m_bframe) { - btSoftBody::Pose& pose=m_pose; - const btVector3 com=evaluateCom(); - /* Com */ - pose.m_com = com; - /* Rotation */ - btMatrix3x3 Apq; - const btScalar eps=SIMD_EPSILON; - Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0); - Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3); - for(int i=0,ni=m_nodes.size();i<ni;++i) + btSoftBody::Pose& pose = m_pose; + const btVector3 com = evaluateCom(); + /* Com */ + pose.m_com = com; + /* Rotation */ + btMatrix3x3 Apq; + const btScalar eps = SIMD_EPSILON; + Apq[0] = Apq[1] = Apq[2] = btVector3(0, 0, 0); + Apq[0].setX(eps); + Apq[1].setY(eps * 2); + Apq[2].setZ(eps * 3); + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) { - const btVector3 a=pose.m_wgh[i]*(m_nodes[i].m_x-com); - const btVector3& b=pose.m_pos[i]; - Apq[0]+=a.x()*b; - Apq[1]+=a.y()*b; - Apq[2]+=a.z()*b; + const btVector3 a = pose.m_wgh[i] * (m_nodes[i].m_x - com); + const btVector3& b = pose.m_pos[i]; + Apq[0] += a.x() * b; + Apq[1] += a.y() * b; + Apq[2] += a.z() * b; } - btMatrix3x3 r,s; - PolarDecompose(Apq,r,s); - pose.m_rot=r; - pose.m_scl=pose.m_aqq*r.transpose()*Apq; - if(m_cfg.maxvolume>1) + btMatrix3x3 r, s; + PolarDecompose(Apq, r, s); + pose.m_rot = r; + pose.m_scl = pose.m_aqq * r.transpose() * Apq; + if (m_cfg.maxvolume > 1) { - const btScalar idet=Clamp<btScalar>( 1/pose.m_scl.determinant(), - 1,m_cfg.maxvolume); - pose.m_scl=Mul(pose.m_scl,idet); + const btScalar idet = Clamp<btScalar>(1 / pose.m_scl.determinant(), + 1, m_cfg.maxvolume); + pose.m_scl = Mul(pose.m_scl, idet); } - } } // -void btSoftBody::updateArea(bool averageArea) +void btSoftBody::updateArea(bool averageArea) { - int i,ni; + int i, ni; - /* Face area */ - for(i=0,ni=m_faces.size();i<ni;++i) + /* Face area */ + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - Face& f=m_faces[i]; - f.m_ra = AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x); + Face& f = m_faces[i]; + f.m_ra = AreaOf(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x); } - - /* Node area */ + + /* Node area */ if (averageArea) { - btAlignedObjectArray<int> counts; - counts.resize(m_nodes.size(),0); - for(i=0,ni=m_nodes.size();i<ni;++i) + btAlignedObjectArray<int> counts; + counts.resize(m_nodes.size(), 0); + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - m_nodes[i].m_area = 0; + m_nodes[i].m_area = 0; } - for(i=0,ni=m_faces.size();i<ni;++i) + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - btSoftBody::Face& f=m_faces[i]; - for(int j=0;j<3;++j) + btSoftBody::Face& f = m_faces[i]; + for (int j = 0; j < 3; ++j) { - const int index=(int)(f.m_n[j]-&m_nodes[0]); + const int index = (int)(f.m_n[j] - &m_nodes[0]); counts[index]++; - f.m_n[j]->m_area+=btFabs(f.m_ra); + f.m_n[j]->m_area += btFabs(f.m_ra); } } - for(i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - if(counts[i]>0) - m_nodes[i].m_area/=(btScalar)counts[i]; + if (counts[i] > 0) + m_nodes[i].m_area /= (btScalar)counts[i]; else - m_nodes[i].m_area=0; + m_nodes[i].m_area = 0; } } else { // initialize node area as zero - for(i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - m_nodes[i].m_area=0; + m_nodes[i].m_area = 0; } - for(i=0,ni=m_faces.size();i<ni;++i) + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - btSoftBody::Face& f=m_faces[i]; + btSoftBody::Face& f = m_faces[i]; - for(int j=0;j<3;++j) + for (int j = 0; j < 3; ++j) { f.m_n[j]->m_area += f.m_ra; } } - for(i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { m_nodes[i].m_area *= 0.3333333f; } } } +void btSoftBody::updateLinkConstants() +{ + int i, ni; -void btSoftBody::updateLinkConstants() -{ - int i,ni; - - /* Links */ - for(i=0,ni=m_links.size();i<ni;++i) + /* Links */ + for (i = 0, ni = m_links.size(); i < ni; ++i) { - Link& l=m_links[i]; - Material& m=*l.m_material; - l.m_c0 = (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST; + Link& l = m_links[i]; + Material& m = *l.m_material; + l.m_c0 = (l.m_n[0]->m_im + l.m_n[1]->m_im) / m.m_kLST; } } -void btSoftBody::updateConstants() +void btSoftBody::updateConstants() { resetLinkRestLengths(); updateLinkConstants(); updateArea(); } - - // -void btSoftBody::initializeClusters() +void btSoftBody::initializeClusters() { int i; - for( i=0;i<m_clusters.size();++i) + for (i = 0; i < m_clusters.size(); ++i) { - Cluster& c=*m_clusters[i]; - c.m_imass=0; + Cluster& c = *m_clusters[i]; + c.m_imass = 0; c.m_masses.resize(c.m_nodes.size()); - for(int j=0;j<c.m_nodes.size();++j) + for (int j = 0; j < c.m_nodes.size(); ++j) { - if (c.m_nodes[j]->m_im==0) + if (c.m_nodes[j]->m_im == 0) { c.m_containsAnchor = true; - c.m_masses[j] = BT_LARGE_FLOAT; - } else + c.m_masses[j] = BT_LARGE_FLOAT; + } + else { - c.m_masses[j] = btScalar(1.)/c.m_nodes[j]->m_im; + c.m_masses[j] = btScalar(1.) / c.m_nodes[j]->m_im; } - c.m_imass += c.m_masses[j]; + c.m_imass += c.m_masses[j]; } - c.m_imass = btScalar(1.)/c.m_imass; - c.m_com = btSoftBody::clusterCom(&c); - c.m_lv = btVector3(0,0,0); - c.m_av = btVector3(0,0,0); - c.m_leaf = 0; - /* Inertia */ - btMatrix3x3& ii=c.m_locii; - ii[0]=ii[1]=ii[2]=btVector3(0,0,0); + c.m_imass = btScalar(1.) / c.m_imass; + c.m_com = btSoftBody::clusterCom(&c); + c.m_lv = btVector3(0, 0, 0); + c.m_av = btVector3(0, 0, 0); + c.m_leaf = 0; + /* Inertia */ + btMatrix3x3& ii = c.m_locii; + ii[0] = ii[1] = ii[2] = btVector3(0, 0, 0); { - int i,ni; + int i, ni; - for(i=0,ni=c.m_nodes.size();i<ni;++i) + for (i = 0, ni = c.m_nodes.size(); i < ni; ++i) { - const btVector3 k=c.m_nodes[i]->m_x-c.m_com; - const btVector3 q=k*k; - const btScalar m=c.m_masses[i]; - ii[0][0] += m*(q[1]+q[2]); - ii[1][1] += m*(q[0]+q[2]); - ii[2][2] += m*(q[0]+q[1]); - ii[0][1] -= m*k[0]*k[1]; - ii[0][2] -= m*k[0]*k[2]; - ii[1][2] -= m*k[1]*k[2]; + const btVector3 k = c.m_nodes[i]->m_x - c.m_com; + const btVector3 q = k * k; + const btScalar m = c.m_masses[i]; + ii[0][0] += m * (q[1] + q[2]); + ii[1][1] += m * (q[0] + q[2]); + ii[2][2] += m * (q[0] + q[1]); + ii[0][1] -= m * k[0] * k[1]; + ii[0][2] -= m * k[0] * k[2]; + ii[1][2] -= m * k[1] * k[2]; } } - ii[1][0]=ii[0][1]; - ii[2][0]=ii[0][2]; - ii[2][1]=ii[1][2]; - + ii[1][0] = ii[0][1]; + ii[2][0] = ii[0][2]; + ii[2][1] = ii[1][2]; + ii = ii.inverse(); - /* Frame */ + /* Frame */ c.m_framexform.setIdentity(); c.m_framexform.setOrigin(c.m_com); c.m_framerefs.resize(c.m_nodes.size()); { int i; - for(i=0;i<c.m_framerefs.size();++i) + for (i = 0; i < c.m_framerefs.size(); ++i) { - c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com; + c.m_framerefs[i] = c.m_nodes[i]->m_x - c.m_com; } } } } // -void btSoftBody::updateClusters() +void btSoftBody::updateClusters() { BT_PROFILE("UpdateClusters"); int i; - for(i=0;i<m_clusters.size();++i) + for (i = 0; i < m_clusters.size(); ++i) { - btSoftBody::Cluster& c=*m_clusters[i]; - const int n=c.m_nodes.size(); + btSoftBody::Cluster& c = *m_clusters[i]; + const int n = c.m_nodes.size(); //const btScalar invn=1/(btScalar)n; - if(n) - { - /* Frame */ - const btScalar eps=btScalar(0.0001); - btMatrix3x3 m,r,s; - m[0]=m[1]=m[2]=btVector3(0,0,0); - m[0][0]=eps*1; - m[1][1]=eps*2; - m[2][2]=eps*3; - c.m_com=clusterCom(&c); - for(int i=0;i<c.m_nodes.size();++i) - { - const btVector3 a=c.m_nodes[i]->m_x-c.m_com; - const btVector3& b=c.m_framerefs[i]; - m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b; - } - PolarDecompose(m,r,s); + if (n) + { + /* Frame */ + const btScalar eps = btScalar(0.0001); + btMatrix3x3 m, r, s; + m[0] = m[1] = m[2] = btVector3(0, 0, 0); + m[0][0] = eps * 1; + m[1][1] = eps * 2; + m[2][2] = eps * 3; + c.m_com = clusterCom(&c); + for (int i = 0; i < c.m_nodes.size(); ++i) + { + const btVector3 a = c.m_nodes[i]->m_x - c.m_com; + const btVector3& b = c.m_framerefs[i]; + m[0] += a[0] * b; + m[1] += a[1] * b; + m[2] += a[2] * b; + } + PolarDecompose(m, r, s); c.m_framexform.setOrigin(c.m_com); - c.m_framexform.setBasis(r); - /* Inertia */ -#if 1/* Constant */ - c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose(); + c.m_framexform.setBasis(r); + /* Inertia */ +#if 1 /* Constant */ + c.m_invwi = c.m_framexform.getBasis() * c.m_locii * c.m_framexform.getBasis().transpose(); #else -#if 0/* Sphere */ +#if 0 /* Sphere */ const btScalar rk=(2*c.m_extents.length2())/(5*c.m_imass); const btVector3 inertia(rk,rk,rk); const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0, @@ -2574,847 +3250,1203 @@ void btSoftBody::updateClusters() btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0); c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose(); -#else/* Actual */ - c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0); - for(int i=0;i<n;++i) - { - const btVector3 k=c.m_nodes[i]->m_x-c.m_com; - const btVector3 q=k*k; - const btScalar m=1/c.m_nodes[i]->m_im; - c.m_invwi[0][0] += m*(q[1]+q[2]); - c.m_invwi[1][1] += m*(q[0]+q[2]); - c.m_invwi[2][2] += m*(q[0]+q[1]); - c.m_invwi[0][1] -= m*k[0]*k[1]; - c.m_invwi[0][2] -= m*k[0]*k[2]; - c.m_invwi[1][2] -= m*k[1]*k[2]; - } - c.m_invwi[1][0]=c.m_invwi[0][1]; - c.m_invwi[2][0]=c.m_invwi[0][2]; - c.m_invwi[2][1]=c.m_invwi[1][2]; - c.m_invwi=c.m_invwi.inverse(); +#else /* Actual */ + c.m_invwi[0] = c.m_invwi[1] = c.m_invwi[2] = btVector3(0, 0, 0); + for (int i = 0; i < n; ++i) + { + const btVector3 k = c.m_nodes[i]->m_x - c.m_com; + const btVector3 q = k * k; + const btScalar m = 1 / c.m_nodes[i]->m_im; + c.m_invwi[0][0] += m * (q[1] + q[2]); + c.m_invwi[1][1] += m * (q[0] + q[2]); + c.m_invwi[2][2] += m * (q[0] + q[1]); + c.m_invwi[0][1] -= m * k[0] * k[1]; + c.m_invwi[0][2] -= m * k[0] * k[2]; + c.m_invwi[1][2] -= m * k[1] * k[2]; + } + c.m_invwi[1][0] = c.m_invwi[0][1]; + c.m_invwi[2][0] = c.m_invwi[0][2]; + c.m_invwi[2][1] = c.m_invwi[1][2]; + c.m_invwi = c.m_invwi.inverse(); #endif #endif - /* Velocities */ - c.m_lv=btVector3(0,0,0); - c.m_av=btVector3(0,0,0); + /* Velocities */ + c.m_lv = btVector3(0, 0, 0); + c.m_av = btVector3(0, 0, 0); { int i; - for(i=0;i<n;++i) + for (i = 0; i < n; ++i) { - const btVector3 v=c.m_nodes[i]->m_v*c.m_masses[i]; - c.m_lv += v; - c.m_av += btCross(c.m_nodes[i]->m_x-c.m_com,v); + const btVector3 v = c.m_nodes[i]->m_v * c.m_masses[i]; + c.m_lv += v; + c.m_av += btCross(c.m_nodes[i]->m_x - c.m_com, v); } } - c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping); - c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping); - c.m_vimpulses[0] = - c.m_vimpulses[1] = btVector3(0,0,0); - c.m_dimpulses[0] = - c.m_dimpulses[1] = btVector3(0,0,0); - c.m_nvimpulses = 0; - c.m_ndimpulses = 0; - /* Matching */ - if(c.m_matching>0) - { - for(int j=0;j<c.m_nodes.size();++j) + c.m_lv = c.m_imass * c.m_lv * (1 - c.m_ldamping); + c.m_av = c.m_invwi * c.m_av * (1 - c.m_adamping); + c.m_vimpulses[0] = + c.m_vimpulses[1] = btVector3(0, 0, 0); + c.m_dimpulses[0] = + c.m_dimpulses[1] = btVector3(0, 0, 0); + c.m_nvimpulses = 0; + c.m_ndimpulses = 0; + /* Matching */ + if (c.m_matching > 0) + { + for (int j = 0; j < c.m_nodes.size(); ++j) { - Node& n=*c.m_nodes[j]; - const btVector3 x=c.m_framexform*c.m_framerefs[j]; - n.m_x=Lerp(n.m_x,x,c.m_matching); + Node& n = *c.m_nodes[j]; + const btVector3 x = c.m_framexform * c.m_framerefs[j]; + n.m_x = Lerp(n.m_x, x, c.m_matching); } - } - /* Dbvt */ - if(c.m_collide) + } + /* Dbvt */ + if (c.m_collide) { - btVector3 mi=c.m_nodes[0]->m_x; - btVector3 mx=mi; - for(int j=1;j<n;++j) + btVector3 mi = c.m_nodes[0]->m_x; + btVector3 mx = mi; + for (int j = 1; j < n; ++j) { mi.setMin(c.m_nodes[j]->m_x); mx.setMax(c.m_nodes[j]->m_x); - } - ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx); - if(c.m_leaf) - m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg); + } + ATTRIBUTE_ALIGNED16(btDbvtVolume) + bounds = btDbvtVolume::FromMM(mi, mx); + if (c.m_leaf) + m_cdbvt.update(c.m_leaf, bounds, c.m_lv * m_sst.sdt * 3, m_sst.radmrg); else - c.m_leaf=m_cdbvt.insert(bounds,&c); + c.m_leaf = m_cdbvt.insert(bounds, &c); } } } - - } - - - // -void btSoftBody::cleanupClusters() +void btSoftBody::cleanupClusters() { - for(int i=0;i<m_joints.size();++i) + for (int i = 0; i < m_joints.size(); ++i) { m_joints[i]->Terminate(m_sst.sdt); - if(m_joints[i]->m_delete) + if (m_joints[i]->m_delete) { btAlignedFree(m_joints[i]); m_joints.remove(m_joints[i--]); - } + } } } // -void btSoftBody::prepareClusters(int iterations) +void btSoftBody::prepareClusters(int iterations) { - for(int i=0;i<m_joints.size();++i) + for (int i = 0; i < m_joints.size(); ++i) { - m_joints[i]->Prepare(m_sst.sdt,iterations); + m_joints[i]->Prepare(m_sst.sdt, iterations); } } - // -void btSoftBody::solveClusters(btScalar sor) +void btSoftBody::solveClusters(btScalar sor) { - for(int i=0,ni=m_joints.size();i<ni;++i) + for (int i = 0, ni = m_joints.size(); i < ni; ++i) { - m_joints[i]->Solve(m_sst.sdt,sor); + m_joints[i]->Solve(m_sst.sdt, sor); } } // -void btSoftBody::applyClusters(bool drift) +void btSoftBody::applyClusters(bool drift) { BT_PROFILE("ApplyClusters"); -// const btScalar f0=m_sst.sdt; + // const btScalar f0=m_sst.sdt; //const btScalar f1=f0/2; btAlignedObjectArray<btVector3> deltas; btAlignedObjectArray<btScalar> weights; - deltas.resize(m_nodes.size(),btVector3(0,0,0)); - weights.resize(m_nodes.size(),0); + deltas.resize(m_nodes.size(), btVector3(0, 0, 0)); + weights.resize(m_nodes.size(), 0); int i; - if(drift) + if (drift) { - for(i=0;i<m_clusters.size();++i) + for (i = 0; i < m_clusters.size(); ++i) { - Cluster& c=*m_clusters[i]; - if(c.m_ndimpulses) + Cluster& c = *m_clusters[i]; + if (c.m_ndimpulses) { - c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses; - c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses; + c.m_dimpulses[0] /= (btScalar)c.m_ndimpulses; + c.m_dimpulses[1] /= (btScalar)c.m_ndimpulses; } } } - - for(i=0;i<m_clusters.size();++i) + + for (i = 0; i < m_clusters.size(); ++i) { - Cluster& c=*m_clusters[i]; - if(0<(drift?c.m_ndimpulses:c.m_nvimpulses)) + Cluster& c = *m_clusters[i]; + if (0 < (drift ? c.m_ndimpulses : c.m_nvimpulses)) { - const btVector3 v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt; - const btVector3 w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt; - for(int j=0;j<c.m_nodes.size();++j) + const btVector3 v = (drift ? c.m_dimpulses[0] : c.m_vimpulses[0]) * m_sst.sdt; + const btVector3 w = (drift ? c.m_dimpulses[1] : c.m_vimpulses[1]) * m_sst.sdt; + for (int j = 0; j < c.m_nodes.size(); ++j) { - const int idx=int(c.m_nodes[j]-&m_nodes[0]); - const btVector3& x=c.m_nodes[j]->m_x; - const btScalar q=c.m_masses[j]; - deltas[idx] += (v+btCross(w,x-c.m_com))*q; - weights[idx] += q; + const int idx = int(c.m_nodes[j] - &m_nodes[0]); + const btVector3& x = c.m_nodes[j]->m_x; + const btScalar q = c.m_masses[j]; + deltas[idx] += (v + btCross(w, x - c.m_com)) * q; + weights[idx] += q; } } } - for(i=0;i<deltas.size();++i) + for (i = 0; i < deltas.size(); ++i) { - if(weights[i]>0) + if (weights[i] > 0) { - m_nodes[i].m_x+=deltas[i]/weights[i]; + m_nodes[i].m_x += deltas[i] / weights[i]; } } } // -void btSoftBody::dampClusters() +void btSoftBody::dampClusters() { int i; - for(i=0;i<m_clusters.size();++i) + for (i = 0; i < m_clusters.size(); ++i) { - Cluster& c=*m_clusters[i]; - if(c.m_ndamping>0) + Cluster& c = *m_clusters[i]; + if (c.m_ndamping > 0) { - for(int j=0;j<c.m_nodes.size();++j) + for (int j = 0; j < c.m_nodes.size(); ++j) { - Node& n=*c.m_nodes[j]; - if(n.m_im>0) + Node& n = *c.m_nodes[j]; + if (n.m_im > 0) { - const btVector3 vx=c.m_lv+btCross(c.m_av,c.m_nodes[j]->m_q-c.m_com); - if(vx.length2()<=n.m_v.length2()) - { - n.m_v += c.m_ndamping*(vx-n.m_v); - } + const btVector3 vx = c.m_lv + btCross(c.m_av, c.m_nodes[j]->m_q - c.m_com); + if (vx.length2() <= n.m_v.length2()) + { + n.m_v += c.m_ndamping * (vx - n.m_v); + } } } } } } +void btSoftBody::setSpringStiffness(btScalar k) +{ + for (int i = 0; i < m_links.size(); ++i) + { + m_links[i].Feature::m_material->m_kLST = k; + } + m_repulsionStiffness = k; +} + +void btSoftBody::setGravityFactor(btScalar gravFactor) +{ + m_gravityFactor = gravFactor; +} + +void btSoftBody::initializeDmInverse() +{ + btScalar unit_simplex_measure = 1. / 6.; + + for (int i = 0; i < m_tetras.size(); ++i) + { + Tetra& t = m_tetras[i]; + btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x; + btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x; + btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x; + btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(), + c1.getY(), c2.getY(), c3.getY(), + c1.getZ(), c2.getZ(), c3.getZ()); + t.m_element_measure = Dm.determinant() * unit_simplex_measure; + t.m_Dm_inverse = Dm.inverse(); + + // calculate the first three columns of P^{-1} + btVector3 a = t.m_n[0]->m_x; + btVector3 b = t.m_n[1]->m_x; + btVector3 c = t.m_n[2]->m_x; + btVector3 d = t.m_n[3]->m_x; + + btScalar det = 1 / (a[0] * b[1] * c[2] - a[0] * b[1] * d[2] - a[0] * b[2] * c[1] + a[0] * b[2] * d[1] + a[0] * c[1] * d[2] - a[0] * c[2] * d[1] + a[1] * (-b[0] * c[2] + b[0] * d[2] + b[2] * c[0] - b[2] * d[0] - c[0] * d[2] + c[2] * d[0]) + a[2] * (b[0] * c[1] - b[0] * d[1] + b[1] * (d[0] - c[0]) + c[0] * d[1] - c[1] * d[0]) - b[0] * c[1] * d[2] + b[0] * c[2] * d[1] + b[1] * c[0] * d[2] - b[1] * c[2] * d[0] - b[2] * c[0] * d[1] + b[2] * c[1] * d[0]); + + btScalar P11 = -b[2] * c[1] + d[2] * c[1] + b[1] * c[2] + b[2] * d[1] - c[2] * d[1] - b[1] * d[2]; + btScalar P12 = b[2] * c[0] - d[2] * c[0] - b[0] * c[2] - b[2] * d[0] + c[2] * d[0] + b[0] * d[2]; + btScalar P13 = -b[1] * c[0] + d[1] * c[0] + b[0] * c[1] + b[1] * d[0] - c[1] * d[0] - b[0] * d[1]; + btScalar P21 = a[2] * c[1] - d[2] * c[1] - a[1] * c[2] - a[2] * d[1] + c[2] * d[1] + a[1] * d[2]; + btScalar P22 = -a[2] * c[0] + d[2] * c[0] + a[0] * c[2] + a[2] * d[0] - c[2] * d[0] - a[0] * d[2]; + btScalar P23 = a[1] * c[0] - d[1] * c[0] - a[0] * c[1] - a[1] * d[0] + c[1] * d[0] + a[0] * d[1]; + btScalar P31 = -a[2] * b[1] + d[2] * b[1] + a[1] * b[2] + a[2] * d[1] - b[2] * d[1] - a[1] * d[2]; + btScalar P32 = a[2] * b[0] - d[2] * b[0] - a[0] * b[2] - a[2] * d[0] + b[2] * d[0] + a[0] * d[2]; + btScalar P33 = -a[1] * b[0] + d[1] * b[0] + a[0] * b[1] + a[1] * d[0] - b[1] * d[0] - a[0] * d[1]; + btScalar P41 = a[2] * b[1] - c[2] * b[1] - a[1] * b[2] - a[2] * c[1] + b[2] * c[1] + a[1] * c[2]; + btScalar P42 = -a[2] * b[0] + c[2] * b[0] + a[0] * b[2] + a[2] * c[0] - b[2] * c[0] - a[0] * c[2]; + btScalar P43 = a[1] * b[0] - c[1] * b[0] - a[0] * b[1] - a[1] * c[0] + b[1] * c[0] + a[0] * c[1]; + + btVector4 p1(P11 * det, P21 * det, P31 * det, P41 * det); + btVector4 p2(P12 * det, P22 * det, P32 * det, P42 * det); + btVector4 p3(P13 * det, P23 * det, P33 * det, P43 * det); + + t.m_P_inv[0] = p1; + t.m_P_inv[1] = p2; + t.m_P_inv[2] = p3; + } +} + +static btScalar Dot4(const btVector4& a, const btVector4& b) +{ + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]; +} + +void btSoftBody::updateDeformation() +{ + btQuaternion q; + for (int i = 0; i < m_tetras.size(); ++i) + { + btSoftBody::Tetra& t = m_tetras[i]; + btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q; + btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q; + btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q; + btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(), + c1.getY(), c2.getY(), c3.getY(), + c1.getZ(), c2.getZ(), c3.getZ()); + t.m_F = Ds * t.m_Dm_inverse; + + btSoftBody::TetraScratch& s = m_tetraScratches[i]; + s.m_F = t.m_F; + s.m_J = t.m_F.determinant(); + btMatrix3x3 C = t.m_F.transpose() * t.m_F; + s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ(); + s.m_cofF = t.m_F.adjoint().transpose(); + + btVector3 a = t.m_n[0]->m_q; + btVector3 b = t.m_n[1]->m_q; + btVector3 c = t.m_n[2]->m_q; + btVector3 d = t.m_n[3]->m_q; + btVector4 q1(a[0], b[0], c[0], d[0]); + btVector4 q2(a[1], b[1], c[1], d[1]); + btVector4 q3(a[2], b[2], c[2], d[2]); + btMatrix3x3 B(Dot4(q1, t.m_P_inv[0]), Dot4(q1, t.m_P_inv[1]), Dot4(q1, t.m_P_inv[2]), + Dot4(q2, t.m_P_inv[0]), Dot4(q2, t.m_P_inv[1]), Dot4(q2, t.m_P_inv[2]), + Dot4(q3, t.m_P_inv[0]), Dot4(q3, t.m_P_inv[1]), Dot4(q3, t.m_P_inv[2])); + q.setRotation(btVector3(0, 0, 1), 0); + B.extractRotation(q, 0.01); // precision of the rotation is not very important for visual correctness. + btMatrix3x3 Q(q); + s.m_corotation = Q; + } +} + +void btSoftBody::advanceDeformation() +{ + updateDeformation(); + for (int i = 0; i < m_tetras.size(); ++i) + { + m_tetraScratchesTn[i] = m_tetraScratches[i]; + } +} // -void btSoftBody::Joint::Prepare(btScalar dt,int) +void btSoftBody::Joint::Prepare(btScalar dt, int) { m_bodies[0].activate(); m_bodies[1].activate(); } // -void btSoftBody::LJoint::Prepare(btScalar dt,int iterations) +void btSoftBody::LJoint::Prepare(btScalar dt, int iterations) { - static const btScalar maxdrift=4; - Joint::Prepare(dt,iterations); - m_rpos[0] = m_bodies[0].xform()*m_refs[0]; - m_rpos[1] = m_bodies[1].xform()*m_refs[1]; - m_drift = Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt; - m_rpos[0] -= m_bodies[0].xform().getOrigin(); - m_rpos[1] -= m_bodies[1].xform().getOrigin(); - m_massmatrix = ImpulseMatrix( m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0], - m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]); - if(m_split>0) + static const btScalar maxdrift = 4; + Joint::Prepare(dt, iterations); + m_rpos[0] = m_bodies[0].xform() * m_refs[0]; + m_rpos[1] = m_bodies[1].xform() * m_refs[1]; + m_drift = Clamp(m_rpos[0] - m_rpos[1], maxdrift) * m_erp / dt; + m_rpos[0] -= m_bodies[0].xform().getOrigin(); + m_rpos[1] -= m_bodies[1].xform().getOrigin(); + m_massmatrix = ImpulseMatrix(m_bodies[0].invMass(), m_bodies[0].invWorldInertia(), m_rpos[0], + m_bodies[1].invMass(), m_bodies[1].invWorldInertia(), m_rpos[1]); + if (m_split > 0) { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; + m_sdrift = m_massmatrix * (m_drift * m_split); + m_drift *= 1 - m_split; } - m_drift /=(btScalar)iterations; + m_drift /= (btScalar)iterations; } // -void btSoftBody::LJoint::Solve(btScalar dt,btScalar sor) +void btSoftBody::LJoint::Solve(btScalar dt, btScalar sor) { - const btVector3 va=m_bodies[0].velocity(m_rpos[0]); - const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); - const btVector3 vr=va-vb; - btSoftBody::Impulse impulse; - impulse.m_asVelocity = 1; - impulse.m_velocity = m_massmatrix*(m_drift+vr*m_cfm)*sor; - m_bodies[0].applyImpulse(-impulse,m_rpos[0]); - m_bodies[1].applyImpulse( impulse,m_rpos[1]); + const btVector3 va = m_bodies[0].velocity(m_rpos[0]); + const btVector3 vb = m_bodies[1].velocity(m_rpos[1]); + const btVector3 vr = va - vb; + btSoftBody::Impulse impulse; + impulse.m_asVelocity = 1; + impulse.m_velocity = m_massmatrix * (m_drift + vr * m_cfm) * sor; + m_bodies[0].applyImpulse(-impulse, m_rpos[0]); + m_bodies[1].applyImpulse(impulse, m_rpos[1]); } // -void btSoftBody::LJoint::Terminate(btScalar dt) +void btSoftBody::LJoint::Terminate(btScalar dt) { - if(m_split>0) + if (m_split > 0) { - m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); - m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); + m_bodies[0].applyDImpulse(-m_sdrift, m_rpos[0]); + m_bodies[1].applyDImpulse(m_sdrift, m_rpos[1]); } } // -void btSoftBody::AJoint::Prepare(btScalar dt,int iterations) +void btSoftBody::AJoint::Prepare(btScalar dt, int iterations) { - static const btScalar maxdrift=SIMD_PI/16; + static const btScalar maxdrift = SIMD_PI / 16; m_icontrol->Prepare(this); - Joint::Prepare(dt,iterations); - m_axis[0] = m_bodies[0].xform().getBasis()*m_refs[0]; - m_axis[1] = m_bodies[1].xform().getBasis()*m_refs[1]; - m_drift = NormalizeAny(btCross(m_axis[1],m_axis[0])); - m_drift *= btMin(maxdrift,btAcos(Clamp<btScalar>(btDot(m_axis[0],m_axis[1]),-1,+1))); - m_drift *= m_erp/dt; - m_massmatrix= AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia()); - if(m_split>0) + Joint::Prepare(dt, iterations); + m_axis[0] = m_bodies[0].xform().getBasis() * m_refs[0]; + m_axis[1] = m_bodies[1].xform().getBasis() * m_refs[1]; + m_drift = NormalizeAny(btCross(m_axis[1], m_axis[0])); + m_drift *= btMin(maxdrift, btAcos(Clamp<btScalar>(btDot(m_axis[0], m_axis[1]), -1, +1))); + m_drift *= m_erp / dt; + m_massmatrix = AngularImpulseMatrix(m_bodies[0].invWorldInertia(), m_bodies[1].invWorldInertia()); + if (m_split > 0) { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; + m_sdrift = m_massmatrix * (m_drift * m_split); + m_drift *= 1 - m_split; } - m_drift /=(btScalar)iterations; + m_drift /= (btScalar)iterations; } // -void btSoftBody::AJoint::Solve(btScalar dt,btScalar sor) +void btSoftBody::AJoint::Solve(btScalar dt, btScalar sor) { - const btVector3 va=m_bodies[0].angularVelocity(); - const btVector3 vb=m_bodies[1].angularVelocity(); - const btVector3 vr=va-vb; - const btScalar sp=btDot(vr,m_axis[0]); - const btVector3 vc=vr-m_axis[0]*m_icontrol->Speed(this,sp); - btSoftBody::Impulse impulse; - impulse.m_asVelocity = 1; - impulse.m_velocity = m_massmatrix*(m_drift+vc*m_cfm)*sor; + const btVector3 va = m_bodies[0].angularVelocity(); + const btVector3 vb = m_bodies[1].angularVelocity(); + const btVector3 vr = va - vb; + const btScalar sp = btDot(vr, m_axis[0]); + const btVector3 vc = vr - m_axis[0] * m_icontrol->Speed(this, sp); + btSoftBody::Impulse impulse; + impulse.m_asVelocity = 1; + impulse.m_velocity = m_massmatrix * (m_drift + vc * m_cfm) * sor; m_bodies[0].applyAImpulse(-impulse); - m_bodies[1].applyAImpulse( impulse); + m_bodies[1].applyAImpulse(impulse); } // -void btSoftBody::AJoint::Terminate(btScalar dt) +void btSoftBody::AJoint::Terminate(btScalar dt) { - if(m_split>0) + if (m_split > 0) { m_bodies[0].applyDAImpulse(-m_sdrift); - m_bodies[1].applyDAImpulse( m_sdrift); + m_bodies[1].applyDAImpulse(m_sdrift); } } // -void btSoftBody::CJoint::Prepare(btScalar dt,int iterations) +void btSoftBody::CJoint::Prepare(btScalar dt, int iterations) { - Joint::Prepare(dt,iterations); - const bool dodrift=(m_life==0); - m_delete=(++m_life)>m_maxlife; - if(dodrift) + Joint::Prepare(dt, iterations); + const bool dodrift = (m_life == 0); + m_delete = (++m_life) > m_maxlife; + if (dodrift) { - m_drift=m_drift*m_erp/dt; - if(m_split>0) + m_drift = m_drift * m_erp / dt; + if (m_split > 0) { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; + m_sdrift = m_massmatrix * (m_drift * m_split); + m_drift *= 1 - m_split; } - m_drift/=(btScalar)iterations; + m_drift /= (btScalar)iterations; } else { - m_drift=m_sdrift=btVector3(0,0,0); + m_drift = m_sdrift = btVector3(0, 0, 0); } } // -void btSoftBody::CJoint::Solve(btScalar dt,btScalar sor) +void btSoftBody::CJoint::Solve(btScalar dt, btScalar sor) { - const btVector3 va=m_bodies[0].velocity(m_rpos[0]); - const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); - const btVector3 vrel=va-vb; - const btScalar rvac=btDot(vrel,m_normal); - btSoftBody::Impulse impulse; - impulse.m_asVelocity = 1; - impulse.m_velocity = m_drift; - if(rvac<0) + const btVector3 va = m_bodies[0].velocity(m_rpos[0]); + const btVector3 vb = m_bodies[1].velocity(m_rpos[1]); + const btVector3 vrel = va - vb; + const btScalar rvac = btDot(vrel, m_normal); + btSoftBody::Impulse impulse; + impulse.m_asVelocity = 1; + impulse.m_velocity = m_drift; + if (rvac < 0) { - const btVector3 iv=m_normal*rvac; - const btVector3 fv=vrel-iv; - impulse.m_velocity += iv+fv*m_friction; + const btVector3 iv = m_normal * rvac; + const btVector3 fv = vrel - iv; + impulse.m_velocity += iv + fv * m_friction; } - impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor; - - if (m_bodies[0].m_soft==m_bodies[1].m_soft) + impulse.m_velocity = m_massmatrix * impulse.m_velocity * sor; + + if (m_bodies[0].m_soft == m_bodies[1].m_soft) { - if ((impulse.m_velocity.getX() ==impulse.m_velocity.getX())&&(impulse.m_velocity.getY() ==impulse.m_velocity.getY())&& - (impulse.m_velocity.getZ() ==impulse.m_velocity.getZ())) + if ((impulse.m_velocity.getX() == impulse.m_velocity.getX()) && (impulse.m_velocity.getY() == impulse.m_velocity.getY()) && + (impulse.m_velocity.getZ() == impulse.m_velocity.getZ())) { if (impulse.m_asVelocity) { - if (impulse.m_velocity.length() <m_bodies[0].m_soft->m_maxSelfCollisionImpulse) + if (impulse.m_velocity.length() < m_bodies[0].m_soft->m_maxSelfCollisionImpulse) { - - } else + } + else { - m_bodies[0].applyImpulse(-impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[0]); - m_bodies[1].applyImpulse( impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[1]); + m_bodies[0].applyImpulse(-impulse * m_bodies[0].m_soft->m_selfCollisionImpulseFactor, m_rpos[0]); + m_bodies[1].applyImpulse(impulse * m_bodies[0].m_soft->m_selfCollisionImpulseFactor, m_rpos[1]); } } } - } else + } + else { - m_bodies[0].applyImpulse(-impulse,m_rpos[0]); - m_bodies[1].applyImpulse( impulse,m_rpos[1]); + m_bodies[0].applyImpulse(-impulse, m_rpos[0]); + m_bodies[1].applyImpulse(impulse, m_rpos[1]); } } // -void btSoftBody::CJoint::Terminate(btScalar dt) +void btSoftBody::CJoint::Terminate(btScalar dt) { - if(m_split>0) + if (m_split > 0) { - m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); - m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); + m_bodies[0].applyDImpulse(-m_sdrift, m_rpos[0]); + m_bodies[1].applyDImpulse(m_sdrift, m_rpos[1]); } } // -void btSoftBody::applyForces() +void btSoftBody::applyForces() { - BT_PROFILE("SoftBody applyForces"); -// const btScalar dt = m_sst.sdt; - const btScalar kLF = m_cfg.kLF; - const btScalar kDG = m_cfg.kDG; - const btScalar kPR = m_cfg.kPR; - const btScalar kVC = m_cfg.kVC; - const bool as_lift = kLF>0; - const bool as_drag = kDG>0; - const bool as_pressure = kPR!=0; - const bool as_volume = kVC>0; - const bool as_aero = as_lift || - as_drag ; + // const btScalar dt = m_sst.sdt; + const btScalar kLF = m_cfg.kLF; + const btScalar kDG = m_cfg.kDG; + const btScalar kPR = m_cfg.kPR; + const btScalar kVC = m_cfg.kVC; + const bool as_lift = kLF > 0; + const bool as_drag = kDG > 0; + const bool as_pressure = kPR != 0; + const bool as_volume = kVC > 0; + const bool as_aero = as_lift || + as_drag; //const bool as_vaero = as_aero && // (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided); //const bool as_faero = as_aero && // (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided); - const bool use_medium = as_aero; - const bool use_volume = as_pressure || - as_volume ; - btScalar volume = 0; - btScalar ivolumetp = 0; - btScalar dvolumetv = 0; - btSoftBody::sMedium medium; - if(use_volume) + const bool use_medium = as_aero; + const bool use_volume = as_pressure || + as_volume; + btScalar volume = 0; + btScalar ivolumetp = 0; + btScalar dvolumetv = 0; + btSoftBody::sMedium medium; + if (use_volume) { - volume = getVolume(); - ivolumetp = 1/btFabs(volume)*kPR; - dvolumetv = (m_pose.m_volume-volume)*kVC; + volume = getVolume(); + ivolumetp = 1 / btFabs(volume) * kPR; + dvolumetv = (m_pose.m_volume - volume) * kVC; } - /* Per vertex forces */ - int i,ni; + /* Per vertex forces */ + int i, ni; - for(i=0,ni=m_nodes.size();i<ni;++i) + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - btSoftBody::Node& n=m_nodes[i]; - if(n.m_im>0) + btSoftBody::Node& n = m_nodes[i]; + if (n.m_im > 0) { - if(use_medium) + if (use_medium) { - /* Aerodynamics */ + /* Aerodynamics */ addAeroForceToNode(m_windVelocity, i); } - /* Pressure */ - if(as_pressure) + /* Pressure */ + if (as_pressure) { - n.m_f += n.m_n*(n.m_area*ivolumetp); + n.m_f += n.m_n * (n.m_area * ivolumetp); } - /* Volume */ - if(as_volume) + /* Volume */ + if (as_volume) { - n.m_f += n.m_n*(n.m_area*dvolumetv); + n.m_f += n.m_n * (n.m_area * dvolumetv); } } } - /* Per face forces */ - for(i=0,ni=m_faces.size();i<ni;++i) + /* Per face forces */ + for (i = 0, ni = m_faces.size(); i < ni; ++i) { - // btSoftBody::Face& f=m_faces[i]; + // btSoftBody::Face& f=m_faces[i]; + + /* Aerodynamics */ + addAeroForceToFace(m_windVelocity, i); + } +} + +// +void btSoftBody::setMaxStress(btScalar maxStress) +{ + m_cfg.m_maxStress = maxStress; +} + +// +void btSoftBody::interpolateRenderMesh() +{ + if (m_z.size() > 0) + { + for (int i = 0; i < m_renderNodes.size(); ++i) + { + const Node* p0 = m_renderNodesParents[i][0]; + const Node* p1 = m_renderNodesParents[i][1]; + const Node* p2 = m_renderNodesParents[i][2]; + btVector3 normal = btCross(p1->m_x - p0->m_x, p2->m_x - p0->m_x); + btVector3 unit_normal = normal.normalized(); + Node& n = m_renderNodes[i]; + n.m_x.setZero(); + for (int j = 0; j < 3; ++j) + { + n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j]; + } + n.m_x += m_z[i] * unit_normal; + } + } + else + { + for (int i = 0; i < m_renderNodes.size(); ++i) + { + Node& n = m_renderNodes[i]; + n.m_x.setZero(); + for (int j = 0; j < 4; ++j) + { + if (m_renderNodesParents[i].size()) + { + n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j]; + } + } + } + } +} - /* Aerodynamics */ - addAeroForceToFace(m_windVelocity, i); +void btSoftBody::setCollisionQuadrature(int N) +{ + for (int i = 0; i <= N; ++i) + { + for (int j = 0; i + j <= N; ++j) + { + m_quads.push_back(btVector3(btScalar(i) / btScalar(N), btScalar(j) / btScalar(N), btScalar(N - i - j) / btScalar(N))); + } } } // -void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti) +void btSoftBody::PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti) { - const btScalar kAHR=psb->m_cfg.kAHR*kst; - const btScalar dt=psb->m_sst.sdt; - for(int i=0,ni=psb->m_anchors.size();i<ni;++i) + BT_PROFILE("PSolve_Anchors"); + const btScalar kAHR = psb->m_cfg.kAHR * kst; + const btScalar dt = psb->m_sst.sdt; + for (int i = 0, ni = psb->m_anchors.size(); i < ni; ++i) { - const Anchor& a=psb->m_anchors[i]; - const btTransform& t=a.m_body->getWorldTransform(); - Node& n=*a.m_node; - const btVector3 wa=t*a.m_local; - const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt; - const btVector3 vb=n.m_x-n.m_q; - const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR; - const btVector3 impulse=a.m_c0*vr*a.m_influence; - n.m_x+=impulse*a.m_c2; - a.m_body->applyImpulse(-impulse,a.m_c1); + const Anchor& a = psb->m_anchors[i]; + const btTransform& t = a.m_body->getWorldTransform(); + Node& n = *a.m_node; + const btVector3 wa = t * a.m_local; + const btVector3 va = a.m_body->getVelocityInLocalPoint(a.m_c1) * dt; + const btVector3 vb = n.m_x - n.m_q; + const btVector3 vr = (va - vb) + (wa - n.m_x) * kAHR; + const btVector3 impulse = a.m_c0 * vr * a.m_influence; + n.m_x += impulse * a.m_c2; + a.m_body->applyImpulse(-impulse, a.m_c1); } } // void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti) { - const btScalar dt = psb->m_sst.sdt; - const btScalar mrg = psb->getCollisionShape()->getMargin(); - for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i) + BT_PROFILE("PSolve_RContacts"); + const btScalar dt = psb->m_sst.sdt; + const btScalar mrg = psb->getCollisionShape()->getMargin(); + btMultiBodyJacobianData jacobianData; + for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i) { - const RContact& c = psb->m_rcontacts[i]; - const sCti& cti = c.m_cti; - if (cti.m_colObj->hasContactResponse()) + const RContact& c = psb->m_rcontacts[i]; + const sCti& cti = c.m_cti; + if (cti.m_colObj->hasContactResponse()) { - btRigidBody* tmpRigid = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - const btVector3 va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); - const btVector3 vb = c.m_node->m_x-c.m_node->m_q; - const btVector3 vr = vb-va; - const btScalar dn = btDot(vr, cti.m_normal); - if(dn<=SIMD_EPSILON) + btVector3 va(0, 0, 0); + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + btScalar* deltaV; + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? rigidCol->getVelocityInLocalPoint(c.m_c1) * dt : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + jacobianData.m_jacobians.resize(ndof); + jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof); + btScalar* jac = &jacobianData.m_jacobians[0]; + + multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, c.m_node->m_x, cti.m_normal, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m); + deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v); + + btScalar vel = 0.0; + for (int j = 0; j < ndof; ++j) + { + vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j]; + } + va = cti.m_normal * vel * dt; + } + } + + const btVector3 vb = c.m_node->m_x - c.m_node->m_q; + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, cti.m_normal); + if (dn <= SIMD_EPSILON) { - const btScalar dp = btMin( (btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg ); - const btVector3 fv = vr - (cti.m_normal * dn); + const btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg); + const btVector3 fv = vr - (cti.m_normal * dn); // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient - const btVector3 impulse = c.m_c0 * ( (vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst ); + const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst); c.m_node->m_x -= impulse * c.m_c2; - if (tmpRigid) - tmpRigid->applyImpulse(impulse,c.m_c1); + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + if (rigidCol) + rigidCol->applyImpulse(impulse, c.m_c1); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + if (multibodyLinkCol) + { + double multiplier = 0.5; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier); + } + } } } } } // -void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti) +void btSoftBody::PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti) { - for(int i=0,ni=psb->m_scontacts.size();i<ni;++i) + BT_PROFILE("PSolve_SContacts"); + + for (int i = 0, ni = psb->m_scontacts.size(); i < ni; ++i) { - const SContact& c=psb->m_scontacts[i]; - const btVector3& nr=c.m_normal; - Node& n=*c.m_node; - Face& f=*c.m_face; - const btVector3 p=BaryEval( f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - c.m_weights); - const btVector3 q=BaryEval( f.m_n[0]->m_q, - f.m_n[1]->m_q, - f.m_n[2]->m_q, - c.m_weights); - const btVector3 vr=(n.m_x-n.m_q)-(p-q); - btVector3 corr(0,0,0); - btScalar dot = btDot(vr,nr); - if(dot<0) + const SContact& c = psb->m_scontacts[i]; + const btVector3& nr = c.m_normal; + Node& n = *c.m_node; + Face& f = *c.m_face; + const btVector3 p = BaryEval(f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + c.m_weights); + const btVector3 q = BaryEval(f.m_n[0]->m_q, + f.m_n[1]->m_q, + f.m_n[2]->m_q, + c.m_weights); + const btVector3 vr = (n.m_x - n.m_q) - (p - q); + btVector3 corr(0, 0, 0); + btScalar dot = btDot(vr, nr); + if (dot < 0) { - const btScalar j=c.m_margin-(btDot(nr,n.m_x)-btDot(nr,p)); - corr+=c.m_normal*j; + const btScalar j = c.m_margin - (btDot(nr, n.m_x) - btDot(nr, p)); + corr += c.m_normal * j; } - corr -= ProjectOnPlane(vr,nr)*c.m_friction; - n.m_x += corr*c.m_cfm[0]; - f.m_n[0]->m_x -= corr*(c.m_cfm[1]*c.m_weights.x()); - f.m_n[1]->m_x -= corr*(c.m_cfm[1]*c.m_weights.y()); - f.m_n[2]->m_x -= corr*(c.m_cfm[1]*c.m_weights.z()); + corr -= ProjectOnPlane(vr, nr) * c.m_friction; + n.m_x += corr * c.m_cfm[0]; + f.m_n[0]->m_x -= corr * (c.m_cfm[1] * c.m_weights.x()); + f.m_n[1]->m_x -= corr * (c.m_cfm[1] * c.m_weights.y()); + f.m_n[2]->m_x -= corr * (c.m_cfm[1] * c.m_weights.z()); } } // -void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti) +void btSoftBody::PSolve_Links(btSoftBody* psb, btScalar kst, btScalar ti) { - for(int i=0,ni=psb->m_links.size();i<ni;++i) - { - Link& l=psb->m_links[i]; - if(l.m_c0>0) + BT_PROFILE("PSolve_Links"); + for (int i = 0, ni = psb->m_links.size(); i < ni; ++i) + { + Link& l = psb->m_links[i]; + if (l.m_c0 > 0) { - Node& a=*l.m_n[0]; - Node& b=*l.m_n[1]; - const btVector3 del=b.m_x-a.m_x; - const btScalar len=del.length2(); - if (l.m_c1+len > SIMD_EPSILON) + Node& a = *l.m_n[0]; + Node& b = *l.m_n[1]; + const btVector3 del = b.m_x - a.m_x; + const btScalar len = del.length2(); + if (l.m_c1 + len > SIMD_EPSILON) { - const btScalar k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst; - a.m_x-=del*(k*a.m_im); - b.m_x+=del*(k*b.m_im); + const btScalar k = ((l.m_c1 - len) / (l.m_c0 * (l.m_c1 + len))) * kst; + a.m_x -= del * (k * a.m_im); + b.m_x += del * (k * b.m_im); } } } } // -void btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst) +void btSoftBody::VSolve_Links(btSoftBody* psb, btScalar kst) { - for(int i=0,ni=psb->m_links.size();i<ni;++i) - { - Link& l=psb->m_links[i]; - Node** n=l.m_n; - const btScalar j=-btDot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst; - n[0]->m_v+= l.m_c3*(j*n[0]->m_im); - n[1]->m_v-= l.m_c3*(j*n[1]->m_im); + BT_PROFILE("VSolve_Links"); + for (int i = 0, ni = psb->m_links.size(); i < ni; ++i) + { + Link& l = psb->m_links[i]; + Node** n = l.m_n; + const btScalar j = -btDot(l.m_c3, n[0]->m_v - n[1]->m_v) * l.m_c2 * kst; + n[0]->m_v += l.m_c3 * (j * n[0]->m_im); + n[1]->m_v -= l.m_c3 * (j * n[1]->m_im); } } // -btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver) +btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver) { - switch(solver) + switch (solver) { - case ePSolver::Anchors: - return(&btSoftBody::PSolve_Anchors); - case ePSolver::Linear: - return(&btSoftBody::PSolve_Links); - case ePSolver::RContacts: - return(&btSoftBody::PSolve_RContacts); - case ePSolver::SContacts: - return(&btSoftBody::PSolve_SContacts); + case ePSolver::Anchors: + return (&btSoftBody::PSolve_Anchors); + case ePSolver::Linear: + return (&btSoftBody::PSolve_Links); + case ePSolver::RContacts: + return (&btSoftBody::PSolve_RContacts); + case ePSolver::SContacts: + return (&btSoftBody::PSolve_SContacts); default: { } } - return(0); + return (0); } // -btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) +btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) { - switch(solver) + switch (solver) { - case eVSolver::Linear: return(&btSoftBody::VSolve_Links); + case eVSolver::Linear: + return (&btSoftBody::VSolve_Links); default: { } } - return(0); + return (0); } -// -void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap) +void btSoftBody::setSelfCollision(bool useSelfCollision) +{ + m_useSelfCollision = useSelfCollision; +} + +bool btSoftBody::useSelfCollision() { + return m_useSelfCollision; +} - switch(m_cfg.collisions&fCollision::RVSmask) +// +void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap) +{ + switch (m_cfg.collisions & fCollision::RVSmask) { - case fCollision::SDF_RS: + case fCollision::SDF_RS: { - btSoftColliders::CollideSDF_RS docollide; - btRigidBody* prb1=(btRigidBody*) btRigidBody::upcast(pcoWrap->getCollisionObject()); - btTransform wtr=pcoWrap->getWorldTransform(); - - const btTransform ctr=pcoWrap->getWorldTransform(); - const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length(); - const btScalar basemargin=getCollisionShape()->getMargin(); - btVector3 mins; - btVector3 maxs; - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; - pcoWrap->getCollisionShape()->getAabb( pcoWrap->getWorldTransform(), - mins, - maxs); - volume=btDbvtVolume::FromMM(mins,maxs); - volume.Expand(btVector3(basemargin,basemargin,basemargin)); - docollide.psb = this; + btSoftColliders::CollideSDF_RS docollide; + btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject()); + btTransform wtr = pcoWrap->getWorldTransform(); + + const btTransform ctr = pcoWrap->getWorldTransform(); + const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length(); + const btScalar basemargin = getCollisionShape()->getMargin(); + btVector3 mins; + btVector3 maxs; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume; + pcoWrap->getCollisionShape()->getAabb(pcoWrap->getWorldTransform(), + mins, + maxs); + volume = btDbvtVolume::FromMM(mins, maxs); + volume.Expand(btVector3(basemargin, basemargin, basemargin)); + docollide.psb = this; docollide.m_colObj1Wrap = pcoWrap; docollide.m_rigidBody = prb1; - docollide.dynmargin = basemargin+timemargin; - docollide.stamargin = basemargin; - m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide); + docollide.dynmargin = basemargin + timemargin; + docollide.stamargin = basemargin; + m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollide); } break; - case fCollision::CL_RS: + case fCollision::CL_RS: { - btSoftColliders::CollideCL_RS collider; - collider.ProcessColObj(this,pcoWrap); + btSoftColliders::CollideCL_RS collider; + collider.ProcessColObj(this, pcoWrap); + } + break; + case fCollision::SDF_RD: + { + btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject()); + if (pcoWrap->getCollisionObject()->isActive() || this->isActive()) + { + const btTransform wtr = pcoWrap->getWorldTransform(); + const btScalar timemargin = 0; + const btScalar basemargin = getCollisionShape()->getMargin(); + btVector3 mins; + btVector3 maxs; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume; + pcoWrap->getCollisionShape()->getAabb(wtr, + mins, + maxs); + volume = btDbvtVolume::FromMM(mins, maxs); + volume.Expand(btVector3(basemargin, basemargin, basemargin)); + if (m_cfg.collisions & fCollision::SDF_RDN) + { + btSoftColliders::CollideSDF_RD docollideNode; + docollideNode.psb = this; + docollideNode.m_colObj1Wrap = pcoWrap; + docollideNode.m_rigidBody = prb1; + docollideNode.dynmargin = basemargin + timemargin; + docollideNode.stamargin = basemargin; + m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode); + } + + if (((pcoWrap->getCollisionObject()->getInternalType() == CO_RIGID_BODY) && (m_cfg.collisions & fCollision::SDF_RDF)) || ((pcoWrap->getCollisionObject()->getInternalType() == CO_FEATHERSTONE_LINK) && (m_cfg.collisions & fCollision::SDF_MDF))) + { + btSoftColliders::CollideSDF_RDF docollideFace; + docollideFace.psb = this; + docollideFace.m_colObj1Wrap = pcoWrap; + docollideFace.m_rigidBody = prb1; + docollideFace.dynmargin = basemargin + timemargin; + docollideFace.stamargin = basemargin; + m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); + } + } } break; } } // -void btSoftBody::defaultCollisionHandler(btSoftBody* psb) +void btSoftBody::defaultCollisionHandler(btSoftBody* psb) { - const int cf=m_cfg.collisions&psb->m_cfg.collisions; - switch(cf&fCollision::SVSmask) + BT_PROFILE("Deformable Collision"); + const int cf = m_cfg.collisions & psb->m_cfg.collisions; + switch (cf & fCollision::SVSmask) { - case fCollision::CL_SS: + case fCollision::CL_SS: { - //support self-collision if CL_SELF flag set - if (this!=psb || psb->m_cfg.collisions&fCollision::CL_SELF) + if (this != psb || psb->m_cfg.collisions & fCollision::CL_SELF) { - btSoftColliders::CollideCL_SS docollide; - docollide.ProcessSoftSoft(this,psb); + btSoftColliders::CollideCL_SS docollide; + docollide.ProcessSoftSoft(this, psb); } - } break; - case fCollision::VF_SS: + case fCollision::VF_SS: { //only self-collision for Cluster, not Vertex-Face yet - if (this!=psb) - { - btSoftColliders::CollideVF_SS docollide; - /* common */ - docollide.mrg= getCollisionShape()->getMargin()+ - psb->getCollisionShape()->getMargin(); - /* psb0 nodes vs psb1 faces */ - docollide.psb[0]=this; - docollide.psb[1]=psb; - docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); - /* psb1 nodes vs psb0 faces */ - docollide.psb[0]=psb; - docollide.psb[1]=this; - docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); + if (this != psb) + { + btSoftColliders::CollideVF_SS docollide; + /* common */ + docollide.mrg = getCollisionShape()->getMargin() + + psb->getCollisionShape()->getMargin(); + /* psb0 nodes vs psb1 faces */ + docollide.psb[0] = this; + docollide.psb[1] = psb; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + /* psb1 nodes vs psb0 faces */ + docollide.psb[0] = psb; + docollide.psb[1] = this; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); } } break; - default: + case fCollision::VF_DD: + { + if (!psb->m_softSoftCollision) + return; + if (psb->isActive() || this->isActive()) + { + if (this != psb) + { + btSoftColliders::CollideVF_DD docollide; + /* common */ + docollide.mrg = getCollisionShape()->getMargin() + + psb->getCollisionShape()->getMargin(); + /* psb0 nodes vs psb1 faces */ + if (psb->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + docollide.psb[0] = this; + docollide.psb[1] = psb; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + + /* psb1 nodes vs psb0 faces */ + if (this->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + docollide.psb[0] = psb; + docollide.psb[1] = this; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + } + else + { + if (psb->useSelfCollision()) + { + btSoftColliders::CollideFF_DD docollide; + docollide.mrg = 2 * getCollisionShape()->getMargin(); + docollide.psb[0] = this; + docollide.psb[1] = psb; + if (this->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + /* psb0 faces vs psb0 faces */ + calculateNormalCone(this->m_fdbvnt); + this->m_fdbvt.selfCollideT(m_fdbvnt, docollide); + } + } + } + } + break; + default: { - } } } +void btSoftBody::geometricCollisionHandler(btSoftBody* psb) +{ + if (psb->isActive() || this->isActive()) + { + if (this != psb) + { + btSoftColliders::CollideCCD docollide; + /* common */ + docollide.mrg = SAFE_EPSILON; // for rounding error instead of actual margin + docollide.dt = psb->m_sst.sdt; + /* psb0 nodes vs psb1 faces */ + if (psb->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + docollide.psb[0] = this; + docollide.psb[1] = psb; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + /* psb1 nodes vs psb0 faces */ + if (this->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + docollide.psb[0] = psb; + docollide.psb[1] = this; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + } + else + { + if (psb->useSelfCollision()) + { + btSoftColliders::CollideCCD docollide; + docollide.mrg = SAFE_EPSILON; + docollide.psb[0] = this; + docollide.psb[1] = psb; + docollide.dt = psb->m_sst.sdt; + if (this->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + /* psb0 faces vs psb0 faces */ + calculateNormalCone(this->m_fdbvnt); // should compute this outside of this scope + this->m_fdbvt.selfCollideT(m_fdbvnt, docollide); + } + } + } +} - -void btSoftBody::setWindVelocity( const btVector3 &velocity ) +void btSoftBody::setWindVelocity(const btVector3& velocity) { m_windVelocity = velocity; } - const btVector3& btSoftBody::getWindVelocity() { return m_windVelocity; } - - -int btSoftBody::calculateSerializeBufferSize() const +int btSoftBody::calculateSerializeBufferSize() const { int sz = sizeof(btSoftBodyData); return sz; } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const +///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const { - btSoftBodyData* sbd = (btSoftBodyData*) dataBuffer; + btSoftBodyData* sbd = (btSoftBodyData*)dataBuffer; btCollisionObject::serialize(&sbd->m_collisionObjectData, serializer); - btHashMap<btHashPtr,int> m_nodeIndexMap; + btHashMap<btHashPtr, int> m_nodeIndexMap; sbd->m_numMaterials = m_materials.size(); - sbd->m_materials = sbd->m_numMaterials? (SoftBodyMaterialData**) serializer->getUniquePointer((void*)&m_materials): 0; + sbd->m_materials = sbd->m_numMaterials ? (SoftBodyMaterialData**)serializer->getUniquePointer((void*)&m_materials) : 0; if (sbd->m_materials) { int sz = sizeof(SoftBodyMaterialData*); int numElem = sbd->m_numMaterials; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); //SoftBodyMaterialData** memPtr = chunk->m_oldPtr; SoftBodyMaterialData** memPtr = (SoftBodyMaterialData**)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { btSoftBody::Material* mat = m_materials[i]; *memPtr = mat ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)mat) : 0; if (!serializer->findPointer(mat)) { //serialize it here - btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData),1); + btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData), 1); SoftBodyMaterialData* memPtr = (SoftBodyMaterialData*)chunk->m_oldPtr; memPtr->m_flags = mat->m_flags; memPtr->m_angularStiffness = mat->m_kAST; memPtr->m_linearStiffness = mat->m_kLST; memPtr->m_volumeStiffness = mat->m_kVST; - serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_SBMATERIAL_CODE,mat); + serializer->finalizeChunk(chunk, "SoftBodyMaterialData", BT_SBMATERIAL_CODE, mat); } } - serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_ARRAY_CODE,(void*) &m_materials); + serializer->finalizeChunk(chunk, "SoftBodyMaterialData", BT_ARRAY_CODE, (void*)&m_materials); } - - - sbd->m_numNodes = m_nodes.size(); - sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes): 0; + sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes) : 0; if (sbd->m_nodes) { int sz = sizeof(SoftBodyNodeData); int numElem = sbd->m_numNodes; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); SoftBodyNodeData* memPtr = (SoftBodyNodeData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { - m_nodes[i].m_f.serializeFloat( memPtr->m_accumulatedForce); + m_nodes[i].m_f.serializeFloat(memPtr->m_accumulatedForce); memPtr->m_area = m_nodes[i].m_area; memPtr->m_attach = m_nodes[i].m_battach; memPtr->m_inverseMass = m_nodes[i].m_im; - memPtr->m_material = m_nodes[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_nodes[i].m_material):0; + memPtr->m_material = m_nodes[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_nodes[i].m_material) : 0; m_nodes[i].m_n.serializeFloat(memPtr->m_normal); m_nodes[i].m_x.serializeFloat(memPtr->m_position); m_nodes[i].m_q.serializeFloat(memPtr->m_previousPosition); m_nodes[i].m_v.serializeFloat(memPtr->m_velocity); - m_nodeIndexMap.insert(&m_nodes[i],i); + m_nodeIndexMap.insert(&m_nodes[i], i); } - serializer->finalizeChunk(chunk,"SoftBodyNodeData",BT_SBNODE_CODE,(void*) &m_nodes); + serializer->finalizeChunk(chunk, "SoftBodyNodeData", BT_SBNODE_CODE, (void*)&m_nodes); } sbd->m_numLinks = m_links.size(); - sbd->m_links = sbd->m_numLinks? (SoftBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0; + sbd->m_links = sbd->m_numLinks ? (SoftBodyLinkData*)serializer->getUniquePointer((void*)&m_links[0]) : 0; if (sbd->m_links) { int sz = sizeof(SoftBodyLinkData); int numElem = sbd->m_numLinks; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); SoftBodyLinkData* memPtr = (SoftBodyLinkData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { memPtr->m_bbending = m_links[i].m_bbending; - memPtr->m_material = m_links[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_links[i].m_material):0; - memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0]: -1; - memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0]: -1; - btAssert(memPtr->m_nodeIndices[0]<m_nodes.size()); - btAssert(memPtr->m_nodeIndices[1]<m_nodes.size()); + memPtr->m_material = m_links[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_links[i].m_material) : 0; + memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0] : -1; + memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0] : -1; + btAssert(memPtr->m_nodeIndices[0] < m_nodes.size()); + btAssert(memPtr->m_nodeIndices[1] < m_nodes.size()); memPtr->m_restLength = m_links[i].m_rl; } - serializer->finalizeChunk(chunk,"SoftBodyLinkData",BT_ARRAY_CODE,(void*) &m_links[0]); - + serializer->finalizeChunk(chunk, "SoftBodyLinkData", BT_ARRAY_CODE, (void*)&m_links[0]); } - sbd->m_numFaces = m_faces.size(); - sbd->m_faces = sbd->m_numFaces? (SoftBodyFaceData*) serializer->getUniquePointer((void*)&m_faces[0]):0; + sbd->m_faces = sbd->m_numFaces ? (SoftBodyFaceData*)serializer->getUniquePointer((void*)&m_faces[0]) : 0; if (sbd->m_faces) { int sz = sizeof(SoftBodyFaceData); int numElem = sbd->m_numFaces; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); SoftBodyFaceData* memPtr = (SoftBodyFaceData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { - memPtr->m_material = m_faces[i].m_material ? (SoftBodyMaterialData*) serializer->getUniquePointer((void*)m_faces[i].m_material): 0; - m_faces[i].m_normal.serializeFloat( memPtr->m_normal); - for (int j=0;j<3;j++) + memPtr->m_material = m_faces[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_faces[i].m_material) : 0; + m_faces[i].m_normal.serializeFloat(memPtr->m_normal); + for (int j = 0; j < 3; j++) { - memPtr->m_nodeIndices[j] = m_faces[i].m_n[j]? m_faces[i].m_n[j] - &m_nodes[0]: -1; + memPtr->m_nodeIndices[j] = m_faces[i].m_n[j] ? m_faces[i].m_n[j] - &m_nodes[0] : -1; } memPtr->m_restArea = m_faces[i].m_ra; } - serializer->finalizeChunk(chunk,"SoftBodyFaceData",BT_ARRAY_CODE,(void*) &m_faces[0]); + serializer->finalizeChunk(chunk, "SoftBodyFaceData", BT_ARRAY_CODE, (void*)&m_faces[0]); } - sbd->m_numTetrahedra = m_tetras.size(); - sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*) serializer->getUniquePointer((void*)&m_tetras[0]):0; + sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*)serializer->getUniquePointer((void*)&m_tetras[0]) : 0; if (sbd->m_tetrahedra) { int sz = sizeof(SoftBodyTetraData); int numElem = sbd->m_numTetrahedra; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); SoftBodyTetraData* memPtr = (SoftBodyTetraData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { - for (int j=0;j<4;j++) + for (int j = 0; j < 4; j++) { - m_tetras[i].m_c0[j].serializeFloat( memPtr->m_c0[j] ); - memPtr->m_nodeIndices[j] = m_tetras[j].m_n[j]? m_tetras[j].m_n[j]-&m_nodes[0] : -1; + m_tetras[i].m_c0[j].serializeFloat(memPtr->m_c0[j]); + memPtr->m_nodeIndices[j] = m_tetras[i].m_n[j] ? m_tetras[i].m_n[j] - &m_nodes[0] : -1; } memPtr->m_c1 = m_tetras[i].m_c1; memPtr->m_c2 = m_tetras[i].m_c2; - memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_tetras[i].m_material): 0; + memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_tetras[i].m_material) : 0; memPtr->m_restVolume = m_tetras[i].m_rv; } - serializer->finalizeChunk(chunk,"SoftBodyTetraData",BT_ARRAY_CODE,(void*) &m_tetras[0]); + serializer->finalizeChunk(chunk, "SoftBodyTetraData", BT_ARRAY_CODE, (void*)&m_tetras[0]); } sbd->m_numAnchors = m_anchors.size(); - sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*) serializer->getUniquePointer((void*)&m_anchors[0]):0; + sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*)serializer->getUniquePointer((void*)&m_anchors[0]) : 0; if (sbd->m_anchors) { int sz = sizeof(SoftRigidAnchorData); int numElem = sbd->m_numAnchors; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); SoftRigidAnchorData* memPtr = (SoftRigidAnchorData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { m_anchors[i].m_c0.serializeFloat(memPtr->m_c0); m_anchors[i].m_c1.serializeFloat(memPtr->m_c1); memPtr->m_c2 = m_anchors[i].m_c2; m_anchors[i].m_local.serializeFloat(memPtr->m_localFrame); - memPtr->m_nodeIndex = m_anchors[i].m_node? m_anchors[i].m_node-&m_nodes[0]: -1; - - memPtr->m_rigidBody = m_anchors[i].m_body? (btRigidBodyData*) serializer->getUniquePointer((void*)m_anchors[i].m_body): 0; + memPtr->m_nodeIndex = m_anchors[i].m_node ? m_anchors[i].m_node - &m_nodes[0] : -1; + + memPtr->m_rigidBody = m_anchors[i].m_body ? (btRigidBodyData*)serializer->getUniquePointer((void*)m_anchors[i].m_body) : 0; btAssert(memPtr->m_nodeIndex < m_nodes.size()); } - serializer->finalizeChunk(chunk,"SoftRigidAnchorData",BT_ARRAY_CODE,(void*) &m_anchors[0]); + serializer->finalizeChunk(chunk, "SoftRigidAnchorData", BT_ARRAY_CODE, (void*)&m_anchors[0]); } - sbd->m_config.m_dynamicFriction = m_cfg.kDF; sbd->m_config.m_baumgarte = m_cfg.kVCF; @@ -3449,64 +4481,63 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ sbd->m_pose = (SoftBodyPoseData*)serializer->getUniquePointer((void*)&m_pose); int sz = sizeof(SoftBodyPoseData); - btChunk* chunk = serializer->allocate(sz,1); + btChunk* chunk = serializer->allocate(sz, 1); SoftBodyPoseData* memPtr = (SoftBodyPoseData*)chunk->m_oldPtr; - + m_pose.m_aqq.serializeFloat(memPtr->m_aqq); memPtr->m_bframe = m_pose.m_bframe; memPtr->m_bvolume = m_pose.m_bvolume; m_pose.m_com.serializeFloat(memPtr->m_com); - + memPtr->m_numPositions = m_pose.m_pos.size(); - memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]): 0; + memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]) : 0; if (memPtr->m_numPositions) { int numElem = memPtr->m_numPositions; int sz = sizeof(btVector3Data); - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { m_pose.m_pos[i].serializeFloat(*memPtr); } - serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_pose.m_pos[0]); + serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)&m_pose.m_pos[0]); } memPtr->m_restVolume = m_pose.m_volume; m_pose.m_rot.serializeFloat(memPtr->m_rot); m_pose.m_scl.serializeFloat(memPtr->m_scale); memPtr->m_numWeigts = m_pose.m_wgh.size(); - memPtr->m_weights = memPtr->m_numWeigts? (float*) serializer->getUniquePointer((void*) &m_pose.m_wgh[0]) : 0; + memPtr->m_weights = memPtr->m_numWeigts ? (float*)serializer->getUniquePointer((void*)&m_pose.m_wgh[0]) : 0; if (memPtr->m_numWeigts) { - int numElem = memPtr->m_numWeigts; int sz = sizeof(float); - btChunk* chunk = serializer->allocate(sz,numElem); - float* memPtr = (float*) chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + btChunk* chunk = serializer->allocate(sz, numElem); + float* memPtr = (float*)chunk->m_oldPtr; + for (int i = 0; i < numElem; i++, memPtr++) { *memPtr = m_pose.m_wgh[i]; } - serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_pose.m_wgh[0]); + serializer->finalizeChunk(chunk, "float", BT_ARRAY_CODE, (void*)&m_pose.m_wgh[0]); } - serializer->finalizeChunk(chunk,"SoftBodyPoseData",BT_ARRAY_CODE,(void*)&m_pose); + serializer->finalizeChunk(chunk, "SoftBodyPoseData", BT_ARRAY_CODE, (void*)&m_pose); } //clusters for convex-cluster collision detection sbd->m_numClusters = m_clusters.size(); - sbd->m_clusters = sbd->m_numClusters? (SoftBodyClusterData*) serializer->getUniquePointer((void*)m_clusters[0]) : 0; + sbd->m_clusters = sbd->m_numClusters ? (SoftBodyClusterData*)serializer->getUniquePointer((void*)m_clusters[0]) : 0; if (sbd->m_numClusters) { int numElem = sbd->m_numClusters; int sz = sizeof(SoftBodyClusterData); - btChunk* chunk = serializer->allocate(sz,numElem); - SoftBodyClusterData* memPtr = (SoftBodyClusterData*) chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + btChunk* chunk = serializer->allocate(sz, numElem); + SoftBodyClusterData* memPtr = (SoftBodyClusterData*)chunk->m_oldPtr; + for (int i = 0; i < numElem; i++, memPtr++) { - memPtr->m_adamping= m_clusters[i]->m_adamping; + memPtr->m_adamping = m_clusters[i]->m_adamping; m_clusters[i]->m_av.serializeFloat(memPtr->m_av); memPtr->m_clusterIndex = m_clusters[i]->m_clusterIndex; memPtr->m_collide = m_clusters[i]->m_collide; @@ -3537,69 +4568,64 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ m_clusters[i]->m_vimpulses[1].serializeFloat(memPtr->m_vimpulses[1]); memPtr->m_ndimpulses = m_clusters[i]->m_ndimpulses; - - - memPtr->m_framerefs = memPtr->m_numFrameRefs? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0; + memPtr->m_framerefs = memPtr->m_numFrameRefs ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0; if (memPtr->m_framerefs) { int numElem = memPtr->m_numFrameRefs; int sz = sizeof(btVector3FloatData); - btChunk* chunk = serializer->allocate(sz,numElem); - btVector3FloatData* memPtr = (btVector3FloatData*) chunk->m_oldPtr; - for (int j=0;j<numElem;j++,memPtr++) + btChunk* chunk = serializer->allocate(sz, numElem); + btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr; + for (int j = 0; j < numElem; j++, memPtr++) { m_clusters[i]->m_framerefs[j].serializeFloat(*memPtr); } - serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_framerefs[0]); + serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_framerefs[0]); } - - memPtr->m_masses = memPtr->m_numMasses ? (float*) serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]): 0; + + memPtr->m_masses = memPtr->m_numMasses ? (float*)serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]) : 0; if (memPtr->m_masses) { int numElem = memPtr->m_numMasses; int sz = sizeof(float); - btChunk* chunk = serializer->allocate(sz,numElem); - float* memPtr = (float*) chunk->m_oldPtr; - for (int j=0;j<numElem;j++,memPtr++) + btChunk* chunk = serializer->allocate(sz, numElem); + float* memPtr = (float*)chunk->m_oldPtr; + for (int j = 0; j < numElem; j++, memPtr++) { *memPtr = m_clusters[i]->m_masses[j]; } - serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_masses[0]); + serializer->finalizeChunk(chunk, "float", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_masses[0]); } - memPtr->m_nodeIndices = memPtr->m_numNodes ? (int*) serializer->getUniquePointer((void*) &m_clusters[i]->m_nodes) : 0; - if (memPtr->m_nodeIndices ) + memPtr->m_nodeIndices = memPtr->m_numNodes ? (int*)serializer->getUniquePointer((void*)&m_clusters[i]->m_nodes) : 0; + if (memPtr->m_nodeIndices) { int numElem = memPtr->m_numMasses; int sz = sizeof(int); - btChunk* chunk = serializer->allocate(sz,numElem); - int* memPtr = (int*) chunk->m_oldPtr; - for (int j=0;j<numElem;j++,memPtr++) + btChunk* chunk = serializer->allocate(sz, numElem); + int* memPtr = (int*)chunk->m_oldPtr; + for (int j = 0; j < numElem; j++, memPtr++) { int* indexPtr = m_nodeIndexMap.find(m_clusters[i]->m_nodes[j]); btAssert(indexPtr); *memPtr = *indexPtr; } - serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_nodes); + serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_nodes); } } - serializer->finalizeChunk(chunk,"SoftBodyClusterData",BT_ARRAY_CODE,(void*)m_clusters[0]); - + serializer->finalizeChunk(chunk, "SoftBodyClusterData", BT_ARRAY_CODE, (void*)m_clusters[0]); } - - sbd->m_numJoints = m_joints.size(); - sbd->m_joints = m_joints.size()? (btSoftBodyJointData*) serializer->getUniquePointer((void*)&m_joints[0]) : 0; + sbd->m_joints = m_joints.size() ? (btSoftBodyJointData*)serializer->getUniquePointer((void*)&m_joints[0]) : 0; if (sbd->m_joints) { int sz = sizeof(btSoftBodyJointData); int numElem = m_joints.size(); - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); btSoftBodyJointData* memPtr = (btSoftBodyJointData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { memPtr->m_jointType = (int)m_joints[i]->Type(); m_joints[i]->m_refs[0].serializeFloat(memPtr->m_refs[0]); @@ -3608,8 +4634,8 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ memPtr->m_erp = float(m_joints[i]->m_erp); memPtr->m_split = float(m_joints[i]->m_split); memPtr->m_delete = m_joints[i]->m_delete; - - for (int j=0;j<4;j++) + + for (int j = 0; j < 4; j++) { memPtr->m_relPosition[0].m_floats[j] = 0.f; memPtr->m_relPosition[1].m_floats[j] = 0.f; @@ -3648,10 +4674,51 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_rigid); } } - serializer->finalizeChunk(chunk,"btSoftBodyJointData",BT_ARRAY_CODE,(void*) &m_joints[0]); + serializer->finalizeChunk(chunk, "btSoftBodyJointData", BT_ARRAY_CODE, (void*)&m_joints[0]); } - return btSoftBodyDataName; } +void btSoftBody::updateDeactivation(btScalar timeStep) +{ + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) + return; + + if (m_maxSpeedSquared < m_sleepingThreshold * m_sleepingThreshold) + { + m_deactivationTime += timeStep; + } + else + { + m_deactivationTime = btScalar(0.); + setActivationState(0); + } +} + +void btSoftBody::setZeroVelocity() +{ + for (int i = 0; i < m_nodes.size(); ++i) + { + m_nodes[i].m_v.setZero(); + } +} + +bool btSoftBody::wantsSleeping() +{ + if (getActivationState() == DISABLE_DEACTIVATION) + return false; + + //disable deactivation + if (gDisableDeactivation || (gDeactivationTime == btScalar(0.))) + return false; + + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) + return true; + + if (m_deactivationTime > gDeactivationTime) + { + return true; + } + return false; +} diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.h b/extern/bullet2/src/BulletSoftBody/btSoftBody.h index bd5846bfb67..efe3f5f3cdc 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBody.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.h @@ -20,870 +20,1067 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btTransform.h" #include "LinearMath/btIDebugDraw.h" +#include "LinearMath/btVector3.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "BulletCollision/CollisionShapes/btConcaveShape.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" #include "btSparseSDF.h" #include "BulletCollision/BroadphaseCollision/btDbvt.h" - +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" //#ifdef BT_USE_DOUBLE_PRECISION //#define btRigidBodyData btRigidBodyDoubleData //#define btRigidBodyDataName "btRigidBodyDoubleData" //#else -#define btSoftBodyData btSoftBodyFloatData -#define btSoftBodyDataName "btSoftBodyFloatData" +#define btSoftBodyData btSoftBodyFloatData +#define btSoftBodyDataName "btSoftBodyFloatData" +static const btScalar OVERLAP_REDUCTION_FACTOR = 0.1; +static unsigned long seed = 243703; //#endif //BT_USE_DOUBLE_PRECISION class btBroadphaseInterface; class btDispatcher; class btSoftBodySolver; -/* btSoftBodyWorldInfo */ -struct btSoftBodyWorldInfo +/* 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; + 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) + : 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. +///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 +class btSoftBody : public btCollisionObject { public: btAlignedObjectArray<const class btCollisionObject*> m_collisionDisabledObjects; // The solver object that handles this soft body - btSoftBodySolver *m_softBodySolver; + 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 - };}; + ///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 - };}; + 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 - };}; + 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 - };}; + 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; + 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 - };}; + 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 + SDF_RD = 0x0004, ///rigid vs deformable + + SVSmask = 0x00f0, ///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 + VF_DD = 0x0080, ///Vertex vs face soft vs soft handling + + RVDFmask = 0x0f00, /// Rigid versus deformable face mask + SDF_RDF = 0x0100, /// GJK based Rigid vs. deformable face + SDF_MDF = 0x0200, /// GJK based Multibody vs. deformable face + SDF_RDN = 0x0400, /// SDF based Rigid vs. deformable node + /* presets */ + Default = SDF_RS, + END + }; + }; ///fMaterial - struct fMaterial { enum _ { - DebugDraw = 0x0001, /// Enable debug draw - /* presets */ - Default = DebugDraw, - END - };}; + struct fMaterial + { + enum _ + { + DebugDraw = 0x0001, /// Enable debug draw + /* presets */ + Default = DebugDraw, + END + }; + }; // // API Types // - /* sRayCast */ + /* 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) + 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 + /* ImplicitFn */ + struct ImplicitFn { virtual ~ImplicitFn() {} - virtual btScalar Eval(const btVector3& x)=0; + virtual btScalar Eval(const btVector3& x) = 0; }; // // Internal types // - typedef btAlignedObjectArray<btScalar> tScalarArray; - typedef btAlignedObjectArray<btVector3> tVector3Array; + typedef btAlignedObjectArray<btScalar> tScalarArray; + typedef btAlignedObjectArray<btVector3> tVector3Array; - /* sCti is Softbody contact info */ - struct sCti + /* sCti is Softbody contact info */ + struct sCti { - const btCollisionObject* m_colObj; /* Rigid body */ - btVector3 m_normal; /* Outward normal */ - btScalar m_offset; /* Offset from origin */ - }; + const btCollisionObject* m_colObj; /* Rigid body */ + btVector3 m_normal; /* Outward normal */ + btScalar m_offset; /* Offset from origin */ + btVector3 m_bary; /* Barycentric weights for faces */ + }; - /* sMedium */ - struct sMedium + /* sMedium */ + struct sMedium { - btVector3 m_velocity; /* Velocity */ - btScalar m_pressure; /* Pressure */ - btScalar m_density; /* Density */ + btVector3 m_velocity; /* Velocity */ + btScalar m_pressure; /* Pressure */ + btScalar m_density; /* Density */ }; - /* Base type */ - struct Element + /* Base type */ + struct Element { - void* m_tag; // User data + 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 */ - struct Link : Feature - { - 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 - btVector3 m_c3; // gradient - }; - /* 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); + /* 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/Test position + btVector3 m_v; // Velocity + btVector3 m_vn; // Previous step 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_constrained; // depth of penetration + int m_battach : 1; // Attached + int index; + btVector3 m_splitv; // velocity associated with split impulse + btMatrix3x3 m_effectiveMass; // effective mass in contact + btMatrix3x3 m_effectiveMass_inv; // inverse of effective mass + }; + /* 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 + btVector4 m_pcontact; // barycentric weights of the persistent contact + btVector3 m_n0, m_n1, m_vn; + int m_index; + }; + /* 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) + btMatrix3x3 m_Dm_inverse; // rest Dm^-1 + btMatrix3x3 m_F; + btScalar m_element_measure; + btVector4 m_P_inv[3]; // first three columns of P_inv matrix + }; + + /* TetraScratch */ + struct TetraScratch + { + btMatrix3x3 m_F; // deformation gradient F + btScalar m_trace; // trace of F^T * F + btScalar m_J; // det(F) + btMatrix3x3 m_cofF; // cofactor of F + btMatrix3x3 m_corotation; // corotatio of the tetra + }; + + /* 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 + + // jacobians and unit impulse responses for multibody + btMultiBodyJacobianData jacobianData_normal; + btMultiBodyJacobianData jacobianData_t1; + btMultiBodyJacobianData jacobianData_t2; + btVector3 t1; + btVector3 t2; + }; + + class DeformableRigidContact + { + public: + sCti m_cti; // Contact infos + btMatrix3x3 m_c0; // Impulse matrix + btVector3 m_c1; // Relative anchor + btScalar m_c2; // inverse mass of node/face + btScalar m_c3; // Friction + btScalar m_c4; // Hardness + btMatrix3x3 m_c5; // inverse effective mass + + // jacobians and unit impulse responses for multibody + btMultiBodyJacobianData jacobianData_normal; + btMultiBodyJacobianData jacobianData_t1; + btMultiBodyJacobianData jacobianData_t2; + btVector3 t1; + btVector3 t2; + }; + + class DeformableNodeRigidContact : public DeformableRigidContact + { + public: + Node* m_node; // Owner node + }; + + class DeformableNodeRigidAnchor : public DeformableNodeRigidContact + { + public: + btVector3 m_local; // Anchor position in body space + }; + + class DeformableFaceRigidContact : public DeformableRigidContact + { + public: + Face* m_face; // Owner face + btVector3 m_contactPoint; // Contact point + btVector3 m_bary; // Barycentric weights + btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; + }; + + struct DeformableFaceNodeContact + { + Node* m_node; // Node + Face* m_face; // Face + btVector3 m_bary; // Barycentric weights + btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; + btVector3 m_normal; // Normal + btScalar m_margin; // Margin + btScalar m_friction; // Friction + btScalar m_imf; // inverse mass of the face at contact point + btScalar m_c0; // scale of the impulse matrix; + }; + + /* 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 operator*(btScalar x) const { - Impulse i=*this; - i.m_velocity*=x; - i.m_drift*=x; - return(i); + Impulse i = *this; + i.m_velocity *= x; + i.m_drift *= x; + return (i); } }; - /* Body */ - struct Body + /* Body */ + struct Body { - Cluster* m_soft; - btRigidBody* m_rigid; - const btCollisionObject* m_collisionObject; + 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) + 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 + void activate() const { - if(m_rigid) + if (m_rigid) m_rigid->activate(); if (m_collisionObject) m_collisionObject->activate(); - } - const btMatrix3x3& invWorldInertia() const + 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); + 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 + btScalar invMass() const { - if(m_rigid) return(m_rigid->getInvMass()); - if(m_soft) return(m_soft->m_imass); - return(0); + if (m_rigid) return (m_rigid->getInvMass()); + if (m_soft) return (m_soft->m_imass); + return (0); } - const btTransform& xform() const + 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); + 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 + btVector3 linearVelocity() const { - if(m_rigid) return(m_rigid->getLinearVelocity()); - if(m_soft) return(m_soft->m_lv); - return(btVector3(0,0,0)); + 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 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 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 + btVector3 velocity(const btVector3& rpos) const { - return(linearVelocity()+angularVelocity(rpos)); + return (linearVelocity() + angularVelocity(rpos)); } - void applyVImpulse(const btVector3& impulse,const btVector3& rpos) const + 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); + 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 + 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 (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) + 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); + // 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) + 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); + // 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 + void applyVAImpulse(const btVector3& impulse) const { - if(m_rigid) m_rigid->applyTorqueImpulse(impulse); - if(m_soft) btSoftBody::clusterVAImpulse(m_soft,impulse); + if (m_rigid) m_rigid->applyTorqueImpulse(impulse); + if (m_soft) btSoftBody::clusterVAImpulse(m_soft, impulse); } - void applyDAImpulse(const btVector3& impulse) const + void applyDAImpulse(const btVector3& impulse) const { - if(m_rigid) m_rigid->applyTorqueImpulse(impulse); - if(m_soft) btSoftBody::clusterDAImpulse(m_soft,impulse); + if (m_rigid) m_rigid->applyTorqueImpulse(impulse); + if (m_soft) btSoftBody::clusterDAImpulse(m_soft, impulse); } - void applyAImpulse(const Impulse& impulse) const + void applyAImpulse(const Impulse& impulse) const { - if(impulse.m_asVelocity) applyVAImpulse(impulse.m_velocity); - if(impulse.m_asDrift) applyDAImpulse(impulse.m_drift); + if (impulse.m_asVelocity) applyVAImpulse(impulse.m_velocity); + if (impulse.m_asDrift) applyDAImpulse(impulse.m_drift); } - void applyDCImpulse(const btVector3& impulse) const + void applyDCImpulse(const btVector3& impulse) const { - if(m_rigid) m_rigid->applyCentralImpulse(impulse); - if(m_soft) btSoftBody::clusterDCImpulse(m_soft,impulse); + if (m_rigid) m_rigid->applyCentralImpulse(impulse); + if (m_soft) btSoftBody::clusterDCImpulse(m_soft, impulse); } }; - /* Joint */ - struct Joint + /* Joint */ + struct Joint { - struct eType { enum _ { - Linear=0, - Angular, - Contact - };}; + struct eType + { + enum _ + { + Linear = 0, + Angular, + Contact + }; + }; struct Specs { - Specs() : erp(1),cfm(1),split(1) {} - btScalar erp; - btScalar cfm; - btScalar split; + 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() {} + 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; + 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 + /* 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); } + 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 + /* 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); } + 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 - }; + 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 + btScalar drag; // deformable air drag + btScalar m_maxStress; // Maximum principle first Piola stress + }; + /* SolverState */ + struct SolverState + { + //if you add new variables, always initialize them! + SolverState() + : sdt(0), + isdt(0), + velmrg(0), + radmrg(0), + updmrg(0) + { + } + 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); + 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; + 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; - + 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 + tNodeArray m_renderNodes; // Nodes + tLinkArray m_links; // Links + tFaceArray m_faces; // Faces + tFaceArray m_renderFaces; // Faces + tTetraArray m_tetras; // Tetras + btAlignedObjectArray<TetraScratch> m_tetraScratches; + btAlignedObjectArray<TetraScratch> m_tetraScratchesTn; + tAnchorArray m_anchors; // Anchors + btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors; + tRContactArray m_rcontacts; // Rigid contacts + btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts; + btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts; + btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts; + 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 + btDbvntNode* m_fdbvnt; // Faces tree with normals + btDbvt m_cdbvt; // Clusters tree + tClusterArray m_clusters; // Clusters + btScalar m_dampingCoefficient; // Damping Coefficient + btScalar m_sleepingThreshold; + btScalar m_maxSpeedSquared; + btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection + btScalar m_repulsionStiffness; + btScalar m_gravityFactor; + btAlignedObjectArray<btVector3> m_X; // initial positions + + btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights; + btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents; + btAlignedObjectArray<btScalar> m_z; // vertical distance used in extrapolation + bool m_useSelfCollision; + bool m_softSoftCollision; + + btAlignedObjectArray<bool> m_clusterConnectivity; //cluster connectivity, for self-collision + + btVector3 m_windVelocity; + + btScalar m_restLengthScale; + // // Api // - /* ctor */ - btSoftBody( btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m); + /* ctor */ + btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m); - /* ctor */ - btSoftBody( btSoftBodyWorldInfo* worldInfo); + /* ctor */ + btSoftBody(btSoftBodyWorldInfo* worldInfo); - void initDefaults(); + void initDefaults(); - /* dtor */ + /* dtor */ virtual ~btSoftBody(); - /* Check for existing link */ + /* Check for existing link */ - btAlignedObjectArray<int> m_userIndexMapping; + btAlignedObjectArray<int> m_userIndexMapping; - btSoftBodyWorldInfo* getWorldInfo() + btSoftBodyWorldInfo* getWorldInfo() { return m_worldInfo; } + void setDampingCoefficient(btScalar damping_coeff) + { + m_dampingCoefficient = damping_coeff; + } + ///@todo: avoid internal softbody shape hack and move collision code to collision library - virtual void setCollisionShape(btCollisionShape* collisionShape) + 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); + 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); + void appendTetra(int node0, + int node1, + int node2, + int node3, + Material* mat = 0); + + /* Append anchor */ + void appendDeformableAnchor(int node, btRigidBody* body); + void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link); + 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); + void removeAnchor(int node); + /* 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); + 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); + 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); + 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); + void setVolumeDensity(btScalar density); + /* Get the linear velocity of the center of mass */ + btVector3 getLinearVelocity(); + /* Set the linear velocity of the center of mass */ + void setLinearVelocity(const btVector3& linVel); + /* Set the angular velocity of the center of mass */ + void setAngularVelocity(const btVector3& angVel); + /* Get best fit rigid transform */ + btTransform getRigidTransform(); + /* Transform to given pose */ + void transformTo(const btTransform& trs); + /* 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(); + 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) */ + 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 */ + btVector3 getCenterOfMass() const + { + btVector3 com(0, 0, 0); + for (int i = 0; i < m_nodes.size(); i++) + { + com += (m_nodes[i].m_x * this->getMass(i)); + } + com /= this->getTotalMass(); + return com; + } + 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); + 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); - - + bool rayTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results); + bool rayFaceTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results); + int rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo, + btScalar& mint, int& index) const; + /* 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); + void setSelfCollision(bool useSelfCollision); + bool useSelfCollision(); + void updateDeactivation(btScalar timeStep); + void setZeroVelocity(); + bool wantsSleeping(); // // Functionality to deal with new accelerated solvers. @@ -892,8 +1089,7 @@ public: /** * Set a wind velocity for interaction with the air. */ - void setWindVelocity( const btVector3 &velocity ); - + void setWindVelocity(const btVector3& velocity); /** * Return the wind velocity for interaction with the air. @@ -904,41 +1100,40 @@ public: // 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 ) + void setSoftBodySolver(btSoftBodySolver* softBodySolver) { m_softBodySolver = softBodySolver; } // // Return the solver that handles this soft body - // - btSoftBodySolver *getSoftBodySolver() + // + btSoftBodySolver* getSoftBodySolver() { return m_softBodySolver; } // // Return the solver that handles this soft body - // - btSoftBodySolver *getSoftBodySolver() const + // + btSoftBodySolver* getSoftBodySolver() const { return m_softBodySolver; } - // // Cast // - static const btSoftBody* upcast(const btCollisionObject* colObj) + static const btSoftBody* upcast(const btCollisionObject* colObj) { - if (colObj->getInternalType()==CO_SOFT_BODY) + if (colObj->getInternalType() == CO_SOFT_BODY) return (const btSoftBody*)colObj; return 0; } - static btSoftBody* upcast(btCollisionObject* colObj) + static btSoftBody* upcast(btCollisionObject* colObj) { - if (colObj->getInternalType()==CO_SOFT_BODY) + if (colObj->getInternalType() == CO_SOFT_BODY) return (btSoftBody*)colObj; return 0; } @@ -947,7 +1142,7 @@ public: // ::btCollisionObject // - virtual void getAabb(btVector3& aabbMin,btVector3& aabbMax) const + virtual void getAabb(btVector3& aabbMin, btVector3& aabbMax) const { aabbMin = m_bounds[0]; aabbMax = m_bounds[1]; @@ -955,48 +1150,230 @@ public: // // 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; + 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(); + void rebuildNodeTree(); + btVector3 evaluateCom() const; + bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; + bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& f, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) 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 setSpringStiffness(btScalar k); + void setGravityFactor(btScalar gravFactor); + void initializeDmInverse(); + void updateDeformation(); + void advanceDeformation(); + void applyForces(); + void setMaxStress(btScalar maxStress); + void interpolateRenderMesh(); + void setCollisionQuadrature(int N); + 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); + void geometricCollisionHandler(btSoftBody* psb); +#define SAFE_EPSILON SIMD_EPSILON * 100.0 + void updateNode(btDbvtNode* node, bool use_velocity, bool margin) + { + if (node->isleaf()) + { + btSoftBody::Node* n = (btSoftBody::Node*)(node->data); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision + if (use_velocity) + { + btVector3 points[2] = {n->m_x, n->m_x + m_sst.sdt * n->m_v}; + vol = btDbvtVolume::FromPoints(points, 2); + vol.Expand(btVector3(pad, pad, pad)); + } + else + { + vol = btDbvtVolume::FromCR(n->m_x, pad); + } + node->volume = vol; + return; + } + else + { + updateNode(node->childs[0], use_velocity, margin); + updateNode(node->childs[1], use_velocity, margin); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + Merge(node->childs[0]->volume, node->childs[1]->volume, vol); + node->volume = vol; + } + } - //virtual void serializeSingleObject(class btSerializer* serializer) const; + void updateNodeTree(bool use_velocity, bool margin) + { + if (m_ndbvt.m_root) + updateNode(m_ndbvt.m_root, use_velocity, margin); + } + template <class DBVTNODE> // btDbvtNode or btDbvntNode + void updateFace(DBVTNODE* node, bool use_velocity, bool margin) + { + if (node->isleaf()) + { + btSoftBody::Face* f = (btSoftBody::Face*)(node->data); + btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + if (use_velocity) + { + btVector3 points[6] = {f->m_n[0]->m_x, f->m_n[0]->m_x + m_sst.sdt * f->m_n[0]->m_v, + f->m_n[1]->m_x, f->m_n[1]->m_x + m_sst.sdt * f->m_n[1]->m_v, + f->m_n[2]->m_x, f->m_n[2]->m_x + m_sst.sdt * f->m_n[2]->m_v}; + vol = btDbvtVolume::FromPoints(points, 6); + } + else + { + btVector3 points[3] = {f->m_n[0]->m_x, + f->m_n[1]->m_x, + f->m_n[2]->m_x}; + vol = btDbvtVolume::FromPoints(points, 3); + } + vol.Expand(btVector3(pad, pad, pad)); + node->volume = vol; + return; + } + else + { + updateFace(node->childs[0], use_velocity, margin); + updateFace(node->childs[1], use_velocity, margin); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + Merge(node->childs[0]->volume, node->childs[1]->volume, vol); + node->volume = vol; + } + } + void updateFaceTree(bool use_velocity, bool margin) + { + if (m_fdbvt.m_root) + updateFace(m_fdbvt.m_root, use_velocity, margin); + if (m_fdbvnt) + updateFace(m_fdbvnt, use_velocity, margin); + } -}; + 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()); + } + void applyRepulsionForce(btScalar timeStep, bool applySpringForce) + { + btAlignedObjectArray<int> indices; + { + // randomize the order of repulsive force + indices.resize(m_faceNodeContacts.size()); + for (int i = 0; i < m_faceNodeContacts.size(); ++i) + indices[i] = i; +#define NEXTRAND (seed = (1664525L * seed + 1013904223L) & 0xffffffff) + int i, ni; + + for (i = 0, ni = indices.size(); i < ni; ++i) + { + btSwap(indices[i], indices[NEXTRAND % ni]); + } + } + for (int k = 0; k < m_faceNodeContacts.size(); ++k) + { + int i = indices[k]; + btSoftBody::DeformableFaceNodeContact& c = m_faceNodeContacts[i]; + btSoftBody::Node* node = c.m_node; + btSoftBody::Face* face = c.m_face; + const btVector3& w = c.m_bary; + const btVector3& n = c.m_normal; + btVector3 l = node->m_x - BaryEval(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, w); + btScalar d = c.m_margin - n.dot(l); + d = btMax(btScalar(0), d); + + const btVector3& va = node->m_v; + btVector3 vb = BaryEval(face->m_n[0]->m_v, face->m_n[1]->m_v, face->m_n[2]->m_v, w); + btVector3 vr = va - vb; + const btScalar vn = btDot(vr, n); // dn < 0 <==> opposing + if (vn > OVERLAP_REDUCTION_FACTOR * d / timeStep) + continue; + btVector3 vt = vr - vn * n; + btScalar I = 0; + btScalar mass = node->m_im == 0 ? 0 : btScalar(1) / node->m_im; + if (applySpringForce) + I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn)); + if (vn < 0) + I += 0.5 * mass * vn; + int face_penetration = 0, node_penetration = node->m_constrained; + for (int i = 0; i < 3; ++i) + face_penetration |= face->m_n[i]->m_constrained; + btScalar I_tilde = 2.0 * I / (1.0 + w.length2()); + + // double the impulse if node or face is constrained. + if (face_penetration > 0 || node_penetration > 0) + { + I_tilde *= 2.0; + } + if (face_penetration <= 0) + { + for (int j = 0; j < 3; ++j) + face->m_n[j]->m_v += w[j] * n * I_tilde * node->m_im; + } + if (node_penetration <= 0) + { + node->m_v -= I_tilde * node->m_im * n; + } + // apply frictional impulse + btScalar vt_norm = vt.safeNorm(); + if (vt_norm > SIMD_EPSILON) + { + btScalar delta_vn = -2 * I * node->m_im; + btScalar mu = c.m_friction; + btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0)) * vt_norm; + I = 0.5 * mass * (vt_norm - vt_new); + vt.safeNormalize(); + I_tilde = 2.0 * I / (1.0 + w.length2()); + // double the impulse if node or face is constrained. + if (face_penetration > 0 || node_penetration > 0) + I_tilde *= 2.0; + if (face_penetration <= 0) + { + for (int j = 0; j < 3; ++j) + face->m_n[j]->m_v += w[j] * vt * I_tilde * (face->m_n[j])->m_im; + } + if (node_penetration <= 0) + { + node->m_v -= I_tilde * node->m_im * vt; + } + } + } + } + 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; +}; -#endif //_BT_SOFT_BODY_H +#endif //_BT_SOFT_BODY_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp index 9f0d44526bf..750718f57f8 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 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" @@ -27,34 +26,28 @@ subject to the following restrictions: #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 +#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(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) +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(); + m_softBody = (isSwapped ? (btSoftBody*)body1Wrap->getCollisionObject() : (btSoftBody*)body0Wrap->getCollisionObject()); + m_triBody = isSwapped ? body0Wrap->getCollisionObject() : body1Wrap->getCollisionObject(); // // create the manifold from the dispatcher 'manifold pool' @@ -68,46 +61,42 @@ btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback() { clearCache(); // m_dispatcher->releaseManifold( m_manifoldPtr ); - } - -void btSoftBodyTriangleCallback::clearCache() +void btSoftBodyTriangleCallback::clearCache() { - for (int i=0;i<m_shapeCache.size();i++) + 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? + 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) +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)) + if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)) { - btVector3 color(1,1,0); + 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); + 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); + btTriIndex triIndex(partId, triangleIndex, 0); btHashKey<btTriIndex> triKey(triIndex.getUid()); - btTriIndex* shapeIndex = m_shapeCache[triKey]; if (shapeIndex) { @@ -117,81 +106,73 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, //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 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); + 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); - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr); - - colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut); + colAlgo->processCollision(&softBody, &triBody, *m_dispatchInfoPtr, m_resultOut); colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); - + return; } - //aabb filter is already applied! + //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]); + btVector3 normal = (triangle[1] - triangle[0]).cross(triangle[2] - triangle[0]); normal.normalize(); - normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION; + 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); + 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]); + //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());//?? + 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());//?? - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr); + 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->processCollision(&softBody, &triBody, *m_dispatchInfoPtr, m_resultOut); colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); triIndex.m_childShape = tm; - m_shapeCache.insert(triKey,triIndex); - + m_shapeCache.insert(triKey, triIndex); } - - - } - - -void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btCollisionObjectWrapper* triBodyWrap, const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +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_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); + 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(); @@ -199,56 +180,45 @@ void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMargin btTransform convexInTriangleSpace; convexInTriangleSpace = triBodyWrap->getWorldTransform().inverse() * softTransform; - btTransformAabb(halfExtents,m_collisionMarginTriangle,convexInTriangleSpace,m_aabbMin,m_aabbMax); + 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) +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()); + 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); + m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, triBody, dispatchInfo, resultOut); - - concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax()); + concaveShape->processAllTriangles(&m_btSoftBodyTriangleCallback, m_btSoftBodyTriangleCallback.getAabbMin(), m_btSoftBodyTriangleCallback.getAabbMax()); // resultOut->refreshContactPoints(); - } - } - } - -btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +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 @@ -267,25 +237,23 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO btTransform convexFromLocal = triInv * convexbody->getWorldTransform(); btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform(); - struct LocalTriangleSphereCastCallback : public btTriangleCallback + struct LocalTriangleSphereCastCallback : public btTriangleCallback { btTransform m_ccdSphereFromTrans; btTransform m_ccdSphereToTrans; - btTransform m_meshTransform; + btTransform m_meshTransform; - btScalar m_ccdSphereRadius; - btScalar m_hitFraction; + 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) - { + 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; @@ -295,29 +263,23 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO 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); + 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 (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(); @@ -325,33 +287,30 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO btVector3 rayAabbMax = convexFromLocal.getOrigin(); rayAabbMax.setMax(convexToLocal.getOrigin()); btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius(); - rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0); - rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0); + rayAabbMin -= btVector3(ccdRadius0, ccdRadius0, ccdRadius0); + rayAabbMax += btVector3(ccdRadius0, ccdRadius0, ccdRadius0); - btScalar curHitFraction = btScalar(1.); //is this available? - LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal, - convexbody->getCcdSweptSphereRadius(),curHitFraction); + 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(); + btConcaveShape* triangleMesh = (btConcaveShape*)concavebody->getCollisionShape(); if (triangleMesh) { - triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); + triangleMesh->processAllTriangles(&raycastCallback, rayAabbMin, rayAabbMax); } - - if (raycastCallback.m_hitFraction < convexbody->getHitFraction()) { - convexbody->setHitFraction( raycastCallback.m_hitFraction); + convexbody->setHitFraction(raycastCallback.m_hitFraction); return raycastCallback.m_hitFraction; } } return btScalar(1.); - } diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h index 11c7b88f98e..3adedbd8051 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h @@ -29,63 +29,62 @@ class btCollisionShape; #include "LinearMath/btHashMap.h" -#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h" //for definition of MAX_NUM_PARTS_IN_BITS +#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h" //for definition of MAX_NUM_PARTS_IN_BITS struct btTriIndex { int m_PartIdTriangleIndex; - class btCollisionShape* m_childShape; + class btCollisionShape* m_childShape; - btTriIndex(int partId,int triangleIndex,btCollisionShape* shape) + btTriIndex(int partId, int triangleIndex, btCollisionShape* shape) { - m_PartIdTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; + m_PartIdTriangleIndex = (partId << (31 - MAX_NUM_PARTS_IN_BITS)) | triangleIndex; m_childShape = shape; } - int getTriangleIndex() const + 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)); + unsigned int y = (~(x & 0)) << (31 - MAX_NUM_PARTS_IN_BITS); + return (m_PartIdTriangleIndex & ~(y)); } - int getPartId() const + int getPartId() const { // Get only the highest bits where the part index is stored - return (m_PartIdTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS)); + return (m_PartIdTriangleIndex >> (31 - MAX_NUM_PARTS_IN_BITS)); } - int getUid() const + 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 ; + btVector3 m_aabbMin; + btVector3 m_aabbMax; btManifoldResult* m_resultOut; - btDispatcher* m_dispatcher; + btDispatcher* m_dispatcher; const btDispatcherInfo* m_dispatchInfoPtr; btScalar m_collisionMarginTriangle; - btHashMap<btHashKey<btTriIndex>,btTriIndex> m_shapeCache; + btHashMap<btHashKey<btTriIndex>, btTriIndex> m_shapeCache; public: - int m_triangleCount; + int m_triangleCount; // btPersistentManifold* m_manifoldPtr; - btSoftBodyTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + btSoftBodyTriangleCallback(btDispatcher* dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped); - void setTimeStepAndCounters(btScalar collisionMarginTriangle,const btCollisionObjectWrapper* triObjWrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + void setTimeStepAndCounters(btScalar collisionMarginTriangle, const btCollisionObjectWrapper* triObjWrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); virtual ~btSoftBodyTriangleCallback(); @@ -101,55 +100,48 @@ public: { return m_aabbMax; } - }; - - - /// btSoftBodyConcaveCollisionAlgorithm supports collision between soft body shapes and (concave) trianges meshes. -class btSoftBodyConcaveCollisionAlgorithm : public btCollisionAlgorithm +class btSoftBodyConcaveCollisionAlgorithm : public btCollisionAlgorithm { - - bool m_isSwapped; + bool m_isSwapped; btSoftBodyTriangleCallback m_btSoftBodyTriangleCallback; public: - - btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + 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); + 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); + btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { //we don't add any manifolds } - void clearCache(); + void clearCache(); - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + 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); + return new (mem) btSoftBodyConcaveCollisionAlgorithm(ci, body0Wrap, body1Wrap, false); } }; - struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + 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); + return new (mem) btSoftBodyConcaveCollisionAlgorithm(ci, body0Wrap, body1Wrap, true); } }; - }; -#endif //BT_SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H +#endif //BT_SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyData.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyData.h index 87d8841cfa4..cec6f401ecc 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyData.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyData.h @@ -19,199 +19,194 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletDynamics/Dynamics/btRigidBody.h" - -struct SoftBodyMaterialData +struct SoftBodyMaterialData { - float m_linearStiffness; - float m_angularStiffness; - float m_volumeStiffness; - int m_flags; + float m_linearStiffness; + float m_angularStiffness; + float m_volumeStiffness; + int m_flags; }; -struct SoftBodyNodeData +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; + 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 +struct SoftBodyLinkData { - SoftBodyMaterialData *m_material; - int m_nodeIndices[2]; // Node pointers - float m_restLength; // Rest length - int m_bbending; // Bending link + SoftBodyMaterialData *m_material; + int m_nodeIndices[2]; // Node pointers + float m_restLength; // Rest length + int m_bbending; // Bending link }; -struct SoftBodyFaceData +struct SoftBodyFaceData { - btVector3FloatData m_normal; // Normal - SoftBodyMaterialData *m_material; - int m_nodeIndices[3]; // Node pointers - float m_restArea; // Rest area -}; + btVector3FloatData m_normal; // Normal + SoftBodyMaterialData *m_material; + int m_nodeIndices[3]; // Node pointers + float m_restArea; // Rest area +}; -struct SoftBodyTetraData +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; + 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 +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 + 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 +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 + 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 +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; + 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 +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; + 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 +enum btSoftJointBodyType { - BT_JOINT_SOFT_BODY_CLUSTER=1, + BT_JOINT_SOFT_BODY_CLUSTER = 1, BT_JOINT_RIGID_BODY, BT_JOINT_COLLISION_OBJECT }; -struct btSoftBodyJointData +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; + 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 +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; + 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 - +#endif //BT_SOFTBODY_FLOAT_DATA diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp index d96f85ec630..c30b65e895a 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -16,115 +16,126 @@ subject to the following restrictions: #include "btSoftBodyInternals.h" #include <stdio.h> +#include <string> +#include <iostream> +#include <sstream> #include <string.h> +#include <algorithm> #include "btSoftBodyHelpers.h" #include "LinearMath/btConvexHull.h" #include "LinearMath/btConvexHullComputer.h" +#include <map> +#include <vector> - -// -static void drawVertex( btIDebugDraw* idraw, - const btVector3& x,btScalar s,const btVector3& c) +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); + 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) +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); + 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) +static void drawTree(btIDebugDraw* idraw, + const btDbvtNode* node, + int depth, + const btVector3& ncolor, + const btVector3& lcolor, + int mindepth, + int maxdepth) { - if(node) + if (node) { - if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0))) + 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); + 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) + 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); + 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) +static inline T sum(const btAlignedObjectArray<T>& items) { - T v; - if(items.size()) + T v; + if (items.size()) { - v=items[0]; - for(int i=1,ni=items.size();i<ni;++i) + v = items[0]; + for (int i = 1, ni = items.size(); i < ni; ++i) { - v+=items[i]; + v += items[i]; } } - return(v); + return (v); } // -template <typename T,typename Q> -static inline void add(btAlignedObjectArray<T>& items,const Q& value) +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) + for (int i = 0, ni = items.size(); i < ni; ++i) { - items[i]+=value; + items[i] += value; } } // -template <typename T,typename Q> -static inline void mul(btAlignedObjectArray<T>& items,const Q& 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) + for (int i = 0, ni = items.size(); i < ni; ++i) { - items[i]*=value; + items[i] *= value; } } // template <typename T> -static inline T average(const btAlignedObjectArray<T>& items) +static inline T average(const btAlignedObjectArray<T>& items) { - const btScalar n=(btScalar)(items.size()>0?items.size():1); - return(sum(items)/n); + const btScalar n = (btScalar)(items.size() > 0 ? items.size() : 1); + return (sum(items) / n); } +#if 0 // -static inline btScalar tetravolume(const btVector3& x0, + inline static btScalar tetravolume(const btVector3& x0, const btVector3& x1, const btVector3& x2, const btVector3& x3) @@ -134,6 +145,7 @@ static inline btScalar tetravolume(const btVector3& x0, const btVector3 c=x3-x0; return(btDot(a,btCross(b,c))); } +#endif // #if 0 @@ -156,86 +168,84 @@ static btVector3 stresscolor(btScalar stress) #endif // -void btSoftBodyHelpers::Draw( btSoftBody* psb, - btIDebugDraw* idraw, - int drawflags) +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)) + 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) + for (i = 0; i < psb->m_clusters.size(); ++i) { - if(psb->m_clusters[i]->m_collide) + 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; + 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; + 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; + 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++) + 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(); + const btConvexHullComputer::Edge* firstEdge = &computer.edges[face]; + const btConvexHullComputer::Edge* edge = firstEdge->getNextEdgeOfFace(); int v0 = firstEdge->getSourceVertex(); int v1 = firstEdge->getTargetVertex(); - while (edge!=firstEdge) + while (edge != firstEdge) { int v2 = edge->getTargetVertex(); - idraw->drawTriangle(computer.vertices[v0],computer.vertices[v1],computer.vertices[v2],color,1); + idraw->drawTriangle(computer.vertices[v0], computer.vertices[v1], computer.vertices[v2], color, 1); edge = edge->getNextEdgeOfFace(); - v0=v1; - v1=v2; + 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) + 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]}; + 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); + hres.m_OutputVertices[idx[1]], + hres.m_OutputVertices[idx[2]], + color, 1); } hlib.ReleaseResult(hres); #endif - } - /* Velocities */ + /* Velocities */ #if 0 for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j) { @@ -245,273 +255,269 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, 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)); + /* 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)) + /* Nodes */ + if (0 != (drawflags & fDrawFlags::Nodes)) { - for(i=0;i<psb->m_nodes.size();++i) + 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)); + 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)) + /* Links */ + if (0 != (drawflags & fDrawFlags::Links)) { - for(i=0;i<psb->m_links.size();++i) + 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); + 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)) + /* Normals */ + if (0 != (drawflags & fDrawFlags::Normals)) { - for(i=0;i<psb->m_nodes.size();++i) + 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); + 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)) + /* 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)); + 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) + /* Faces */ + if (0 != (drawflags & fDrawFlags::Faces)) { - 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 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 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); - } - } + 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)) + /* Anchors */ + if (0 != (drawflags & fDrawFlags::Anchors)) { - for(i=0;i<psb->m_anchors.size();++i) + 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)); + 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) + 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) + 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)); + drawVertex(idraw, n.m_x, 0.25, btVector3(1, 0, 0)); } } } - - /* Notes */ - if(0!=(drawflags&fDrawFlags::Notes)) + /* Notes */ + if (0 != (drawflags & fDrawFlags::Notes)) { - for(i=0;i<psb->m_notes.size();++i) + 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) + 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]; + p += n.m_nodes[j]->m_x * n.m_coords[j]; } - idraw->draw3dText(p,n.m_text); + 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)) + /* 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) + for (i = 0; i < psb->m_joints.size(); ++i) { - const btSoftBody::Joint* pj=psb->m_joints[i]; - switch(pj->Type()) + const btSoftBody::Joint* pj = psb->m_joints[i]; + switch (pj->Type()) { - case btSoftBody::Joint::eType::Linear: + 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)); + 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: + 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)); + 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*/) +void btSoftBodyHelpers::DrawInfos(btSoftBody* psb, + btIDebugDraw* idraw, + bool masses, + bool areas, + bool /*stress*/) { - for(int i=0;i<psb->m_nodes.size();++i) + 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) + 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); + sprintf(buff, " M(%.2f)", 1 / n.m_im); + strcat(text, buff); } - if(areas) + if (areas) { - sprintf(buff," A(%.2f)",n.m_area); - strcat(text,buff); + sprintf(buff, " A(%.2f)", n.m_area); + strcat(text, buff); } - if(text[0]) idraw->draw3dText(n.m_x,text); + if (text[0]) idraw->draw3dText(n.m_x, text); } } // -void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) +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); + 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) +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); + 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) +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); + 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]. +// 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, +// - 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, +//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 +// 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 @@ -520,93 +526,103 @@ void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb, //=================================================================== // 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 +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; +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 - +#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 */) +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 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]); + 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 - + 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); + 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++) { + for (i = 0; i < nNodes + 1; i++) + { nodeWrittenAt[i] = REOP_NOT_DEPENDENT; } - for (i=0; i < nLinks; i++) { + 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++) { - + 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) { + 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 { + } + else + { linkDepA[i] = REOP_NOT_DEPENDENT; } - if (nodeWrittenAt[br] > REOP_NOT_DEPENDENT) { + if (nodeWrittenAt[br] > REOP_NOT_DEPENDENT) + { linkDepB[i] = nodeWrittenAt[br]; linkDep = &linkDepFreeList[linkDepFrees++]; - linkDep->value = -(i+1); + linkDep->value = -(i + 1); linkDep->next = linkDepListStarts[nodeWrittenAt[br]]; linkDepListStarts[nodeWrittenAt[br]] = linkDep; - } else { + } + 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)) { + 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 + 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) { + 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 @@ -614,180 +630,190 @@ void btSoftBodyHelpers::ReoptimizeLinkOrder(btSoftBody *psb /* This can be repla // Free up any link inputs that are dependent on this one linkDep = linkDepListStarts[linkNum]; - while (linkDep) { + while (linkDep) + { depLink = linkDep->value; - if (depLink >= 0) { + if (depLink >= 0) + { linkDepA[depLink] = REOP_NOT_DEPENDENT; - } else { + } + 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)) { + 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 + 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; + delete[] nodeWrittenAt; + delete[] linkDepA; + delete[] linkDepB; + delete[] readyList; + delete[] linkDepFreeList; + delete[] linkDepListStarts; + delete[] linkBuffer; } - // -void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, - btIDebugDraw* idraw) +void btSoftBodyHelpers::DrawFrame(btSoftBody* psb, + btIDebugDraw* idraw) { - if(psb->m_pose.m_bframe) + 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) + 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)); + 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) +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]; + /* 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) + for (i = 0; i < r; ++i) { - const btScalar t=i/(btScalar)(r-1); - x[i]=lerp(from,to,t); - m[i]=1; + 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); + 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) + /* Create links */ + for (i = 1; i < r; ++i) { - psb->appendLink(i-1,i); + psb->appendLink(i - 1, i); } - /* Finished */ - return(psb); + /* 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) +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, + btScalar perturbation) { -#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]; +#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) + 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 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; + const btScalar tx = ix / (btScalar)(rx - 1); + btScalar pert = perturbation * btScalar(rand()) / RAND_MAX; + btVector3 temp1 = py1; + temp1.setY(py1.getY() + pert); + btVector3 temp = py0; + pert = perturbation * btScalar(rand()) / RAND_MAX; + temp.setY(py0.getY() + pert); + x[IDX(ix, iy)] = lerp(temp, temp1, 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); + 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) + /* Create links and faces */ + for (iy = 0; iy < ry; ++iy) { - for(int ix=0;ix<rx;++ix) + 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) + 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) + 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->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)); + 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->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)); + psb->appendLink(IDX(ix + 1, iy), IDX(ix, iy + 1)); } } } } } - /* Finished */ + /* Finished */ #undef IDX - return(psb); + 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) +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: @@ -855,92 +881,92 @@ btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo, * */ -#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]; +#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) + 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 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; + 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); + 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) + /* Create links and faces */ + for (iy = 0; iy < ry; ++iy) { - for(int ix=0;ix<rx;++ix) + for (int ix = 0; ix < rx; ++ix) { - const bool mdx=(ix+1)<rx; - const bool mdy=(iy+1)<ry; + 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); + 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) + 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(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); + 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); + if (gendiags) psb->appendLink(node00, node11); z += 12; } } } - /* Finished */ + /* Finished */ #undef IDX - return(psb); + return (psb); } -float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id) +float btSoftBodyHelpers::CalculateUV(int resx, int resy, int ix, int iy, int id) { - /* * * @@ -966,90 +992,93 @@ float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id) * */ - float tc=0.0f; - if (id == 0) { - tc = (1.0f/((resx-1))*ix); + 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 == 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 == 2) + { + tc = (1.0f / ((resy - 1)) * (resy - 1 - iy - 1)); } - else if (id==3) { - tc = (1.0f/((resx-1))*(ix+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) +btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo, const btVector3& center, + const btVector3& radius, + int res) { - struct Hammersley + struct Hammersley { - static void Generate(btVector3* x,int n) + static void Generate(btVector3* x, int n) { - for(int i=0;i<n;i++) + 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); + 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) + 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; + vtx[i] = vtx[i] * radius + center; } - return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size())); + return (CreateFromConvexHull(worldInfo, &vtx[0], vtx.size())); } - - // -btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar* vertices, - const int* triangles, - int ntriangles, bool randomizeConstraints) +btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo, const btScalar* vertices, + const int* triangles, + int ntriangles, bool randomizeConstraints) { - int maxidx=0; - int i,j,ni; + int maxidx = 0; + int i, j, ni; - for(i=0,ni=ntriangles*3;i<ni;++i) + for (i = 0, ni = ntriangles * 3; i < ni; ++i) { - maxidx=btMax(triangles[i],maxidx); + maxidx = btMax(triangles[i], maxidx); } ++maxidx; - btAlignedObjectArray<bool> chks; - btAlignedObjectArray<btVector3> vtx; - chks.resize(maxidx*maxidx,false); + 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) + for (i = 0, j = 0, ni = maxidx * 3; i < ni; ++j, i += 3) { - vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]); + 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) + 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++) + 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])]) + 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]); + 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]); + psb->appendFace(idx[0], idx[1], idx[2]); } if (randomizeConstraints) @@ -1057,44 +1086,41 @@ btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo psb->randomizeConstraints(); } - return(psb); + return (psb); } // -btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, - int nvertices, bool randomizeConstraints) +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) + 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]); + 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); + return (psb); } - - - static int nextLine(const char* buffer) { - int numBytesRead=0; + int numBytesRead = 0; while (*buffer != '\n') { @@ -1102,8 +1128,7 @@ static int nextLine(const char* buffer) numBytesRead++; } - - if (buffer[0]==0x0a) + if (buffer[0] == 0x0a) { buffer++; numBytesRead++; @@ -1111,8 +1136,8 @@ static int nextLine(const char* buffer) return numBytesRead; } -/* Create from TetGen .ele, .face, .node data */ -btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo, +/* Create from TetGen .ele, .face, .node data */ +btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo, const char* ele, const char* face, const char* node, @@ -1120,39 +1145,38 @@ btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldIn 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); -(void)result; -node += nextLine(node); - -pos.resize(nnode); -for(int i=0;i<pos.size();++i) + 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); + 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); + // sn>>index; + // sn>>x;sn>>y;sn>>z; + node += nextLine(node); - //for(int j=0;j<nattrb;++j) - // sn>>a; + //for(int j=0;j<nattrb;++j) + // sn>>a; - //if(hasbounds) - // sn>>bound; + //if(hasbounds) + // sn>>bound; - pos[index].setX(btScalar(x)); - pos[index].setY(btScalar(y)); - pos[index].setZ(btScalar(z)); + pos[index].setX(btScalar(x)); + pos[index].setY(btScalar(y)); + pos[index].setZ(btScalar(z)); } -btSoftBody* psb=new btSoftBody(&worldInfo,nnode,&pos[0],0); + btSoftBody* psb = new btSoftBody(&worldInfo, nnode, &pos[0], 0); #if 0 if(face&&face[0]) { @@ -1177,42 +1201,461 @@ if(face&&face[0]) } #endif -if(ele&&ele[0]) + 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 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) + 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); + 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); + } + } + } + psb->initializeDmInverse(); + psb->m_tetraScratches.resize(psb->m_tetras.size()); + psb->m_tetraScratchesTn.resize(psb->m_tetras.size()); + 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); +} + +btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file) +{ + std::ifstream fs; + fs.open(vtk_file); + btAssert(fs); + + typedef btAlignedObjectArray<int> Index; + std::string line; + btAlignedObjectArray<btVector3> X; + btVector3 position; + btAlignedObjectArray<Index> indices; + bool reading_points = false; + bool reading_tets = false; + size_t n_points = 0; + size_t n_tets = 0; + size_t x_count = 0; + size_t indices_count = 0; + while (std::getline(fs, line)) + { + std::stringstream ss(line); + if (line.size() == (size_t)(0)) + { + } + else if (line.substr(0, 6) == "POINTS") + { + reading_points = true; + reading_tets = false; + ss.ignore(128, ' '); // ignore "POINTS" + ss >> n_points; + X.resize(n_points); + } + else if (line.substr(0, 5) == "CELLS") + { + reading_points = false; + reading_tets = true; + ss.ignore(128, ' '); // ignore "CELLS" + ss >> n_tets; + indices.resize(n_tets); + } + else if (line.substr(0, 10) == "CELL_TYPES") + { + reading_points = false; + reading_tets = false; + } + else if (reading_points) + { + btScalar p; + ss >> p; + position.setX(p); + ss >> p; + position.setY(p); + ss >> p; + position.setZ(p); + X[x_count++] = position; + } + else if (reading_tets) + { + int d; + ss >> d; + if (d != 4) + { + printf("Load deformable failed: Only Tetrahedra are supported in VTK file.\n"); + fs.close(); + return 0; } + ss.ignore(128, ' '); // ignore "4" + Index tet; + tet.resize(4); + for (size_t i = 0; i < 4; i++) + { + ss >> tet[i]; + printf("%d ", tet[i]); + } + printf("\n"); + indices[indices_count++] = tet; + } + } + btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0); + + for (int i = 0; i < n_tets; ++i) + { + const Index& ni = indices[i]; + psb->appendTetra(ni[0], ni[1], ni[2], ni[3]); + { + 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: %d\r\n",psb->m_nodes.size()); -printf("Links: %d\r\n",psb->m_links.size()); -printf("Faces: %d\r\n",psb->m_faces.size()); -printf("Tetras: %d\r\n",psb->m_tetras.size()); -return(psb); + + generateBoundaryFaces(psb); + psb->initializeDmInverse(); + psb->m_tetraScratches.resize(psb->m_tetras.size()); + psb->m_tetraScratchesTn.resize(psb->m_tetras.size()); + 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()); + + fs.close(); + return psb; } +void btSoftBodyHelpers::generateBoundaryFaces(btSoftBody* psb) +{ + int counter = 0; + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + psb->m_nodes[i].index = counter++; + } + typedef btAlignedObjectArray<int> Index; + btAlignedObjectArray<Index> indices; + indices.resize(psb->m_tetras.size()); + for (int i = 0; i < indices.size(); ++i) + { + Index index; + index.push_back(psb->m_tetras[i].m_n[0]->index); + index.push_back(psb->m_tetras[i].m_n[1]->index); + index.push_back(psb->m_tetras[i].m_n[2]->index); + index.push_back(psb->m_tetras[i].m_n[3]->index); + indices[i] = index; + } + + std::map<std::vector<int>, std::vector<int> > dict; + for (int i = 0; i < indices.size(); ++i) + { + for (int j = 0; j < 4; ++j) + { + std::vector<int> f; + if (j == 0) + { + f.push_back(indices[i][1]); + f.push_back(indices[i][0]); + f.push_back(indices[i][2]); + } + if (j == 1) + { + f.push_back(indices[i][3]); + f.push_back(indices[i][0]); + f.push_back(indices[i][1]); + } + if (j == 2) + { + f.push_back(indices[i][3]); + f.push_back(indices[i][1]); + f.push_back(indices[i][2]); + } + if (j == 3) + { + f.push_back(indices[i][2]); + f.push_back(indices[i][0]); + f.push_back(indices[i][3]); + } + std::vector<int> f_sorted = f; + std::sort(f_sorted.begin(), f_sorted.end()); + if (dict.find(f_sorted) != dict.end()) + { + dict.erase(f_sorted); + } + else + { + dict.insert(std::make_pair(f_sorted, f)); + } + } + } + + for (std::map<std::vector<int>, std::vector<int> >::iterator it = dict.begin(); it != dict.end(); ++it) + { + std::vector<int> f = it->second; + psb->appendFace(f[0], f[1], f[2]); + } +} + +//Write the surface mesh to an obj file. +void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb) +{ + std::ofstream fs; + fs.open(filename); + btAssert(fs); + + if (psb->m_tetras.size() > 0) + { + // For tetrahedron mesh, we need to re-index the surface mesh for it to be in obj file/ + std::map<int, int> dict; + for (int i = 0; i < psb->m_faces.size(); i++) + { + for (int d = 0; d < 3; d++) + { + int index = psb->m_faces[i].m_n[d]->index; + if (dict.find(index) == dict.end()) + { + int dict_size = dict.size(); + dict[index] = dict_size; + fs << "v"; + for (int k = 0; k < 3; k++) + { + fs << " " << psb->m_nodes[index].m_x[k]; + } + fs << "\n"; + } + } + } + // Write surface mesh. + for (int i = 0; i < psb->m_faces.size(); ++i) + { + fs << "f"; + for (int n = 0; n < 3; n++) + { + fs << " " << dict[psb->m_faces[i].m_n[n]->index] + 1; + } + fs << "\n"; + } + } + else + { + // For trimesh, directly write out all the nodes and faces.xs + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + fs << "v"; + for (int d = 0; d < 3; d++) + { + fs << " " << psb->m_nodes[i].m_x[d]; + } + fs << "\n"; + } + + for (int i = 0; i < psb->m_faces.size(); ++i) + { + fs << "f"; + for (int n = 0; n < 3; n++) + { + fs << " " << psb->m_faces[i].m_n[n]->index + 1; + } + fs << "\n"; + } + } + fs.close(); +} + +void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb) +{ + std::ifstream fs_read; + fs_read.open(filename); + std::string line; + btVector3 pos; + btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces; + while (std::getline(fs_read, line)) + { + std::stringstream ss(line); + if (line[0] == 'v') + { + } + else if (line[0] == 'f') + { + ss.ignore(); + int id0, id1, id2; + ss >> id0; + ss >> id1; + ss >> id2; + btAlignedObjectArray<int> new_face; + new_face.push_back(id1); + new_face.push_back(id0); + new_face.push_back(id2); + additional_faces.push_back(new_face); + } + } + fs_read.close(); + + std::ofstream fs_write; + fs_write.open(filename, std::ios_base::app); + for (int i = 0; i < additional_faces.size(); ++i) + { + fs_write << "f"; + for (int n = 0; n < 3; n++) + { + fs_write << " " << additional_faces[i][n]; + } + fs_write << "\n"; + } + fs_write.close(); +} + +// Given a simplex with vertices a,b,c,d, find the barycentric weights of p in this simplex +void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary) +{ + btVector3 vap = p - a; + btVector3 vbp = p - b; + + btVector3 vab = b - a; + btVector3 vac = c - a; + btVector3 vad = d - a; + + btVector3 vbc = c - b; + btVector3 vbd = d - b; + btScalar va6 = (vbp.cross(vbd)).dot(vbc); + btScalar vb6 = (vap.cross(vac)).dot(vad); + btScalar vc6 = (vap.cross(vad)).dot(vab); + btScalar vd6 = (vap.cross(vab)).dot(vac); + btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad)); + bary = btVector4(va6 * v6, vb6 * v6, vc6 * v6, vd6 * v6); +} + +// Given a simplex with vertices a,b,c, find the barycentric weights of p in this simplex. bary[3] = 0. +void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary) +{ + btVector3 v0 = b - a, v1 = c - a, v2 = p - a; + btScalar d00 = btDot(v0, v0); + btScalar d01 = btDot(v0, v1); + btScalar d11 = btDot(v1, v1); + btScalar d20 = btDot(v2, v0); + btScalar d21 = btDot(v2, v1); + btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01); + bary[1] = (d11 * d20 - d01 * d21) * invDenom; + bary[2] = (d00 * d21 - d01 * d20) * invDenom; + bary[0] = 1.0 - bary[1] - bary[2]; + bary[3] = 0; +} + +// Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights +// If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight +void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb) +{ + psb->m_z.resize(0); + psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); + psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); + for (int i = 0; i < psb->m_renderNodes.size(); ++i) + { + const btVector3& p = psb->m_renderNodes[i].m_x; + btVector4 bary; + btVector4 optimal_bary; + btScalar min_bary_weight = -1e3; + btAlignedObjectArray<const btSoftBody::Node*> optimal_parents; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + const btSoftBody::Tetra& t = psb->m_tetras[j]; + getBarycentricWeights(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x, p, bary); + btScalar new_min_bary_weight = bary[0]; + for (int k = 1; k < 4; ++k) + { + new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); + } + if (new_min_bary_weight > min_bary_weight) + { + btAlignedObjectArray<const btSoftBody::Node*> parents; + parents.push_back(t.m_n[0]); + parents.push_back(t.m_n[1]); + parents.push_back(t.m_n[2]); + parents.push_back(t.m_n[3]); + optimal_parents = parents; + optimal_bary = bary; + min_bary_weight = new_min_bary_weight; + // stop searching if p is inside the tetrahedron at hand + if (bary[0] >= 0. && bary[1] >= 0. && bary[2] >= 0. && bary[3] >= 0.) + { + break; + } + } + } + psb->m_renderNodesInterpolationWeights[i] = optimal_bary; + psb->m_renderNodesParents[i] = optimal_parents; + } +} + +// Iterate through all render nodes to find the simulation triangle that's closest to the node in the barycentric sense. +void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb) +{ + psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); + psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); + psb->m_z.resize(psb->m_renderNodes.size()); + for (int i = 0; i < psb->m_renderNodes.size(); ++i) + { + const btVector3& p = psb->m_renderNodes[i].m_x; + btVector4 bary; + btVector4 optimal_bary; + btScalar min_bary_weight = -SIMD_INFINITY; + btAlignedObjectArray<const btSoftBody::Node*> optimal_parents; + btScalar dist = 0, optimal_dist = 0; + for (int j = 0; j < psb->m_faces.size(); ++j) + { + const btSoftBody::Face& f = psb->m_faces[j]; + 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); + btVector3 unit_n = n.normalized(); + dist = (p - f.m_n[0]->m_x).dot(unit_n); + btVector3 proj_p = p - dist * unit_n; + getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary); + btScalar new_min_bary_weight = bary[0]; + for (int k = 1; k < 3; ++k) + { + new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); + } + + // p is out of the current best triangle, we found a traingle that's better + bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight < 0.); + // p is inside of the current best triangle, we found a triangle that's better + bool better_than_best_inside = (new_min_bary_weight >= 0 && min_bary_weight >= 0 && btFabs(dist) < btFabs(optimal_dist)); + + if (better_than_closest_outisde || better_than_best_inside) + { + btAlignedObjectArray<const btSoftBody::Node*> parents; + parents.push_back(f.m_n[0]); + parents.push_back(f.m_n[1]); + parents.push_back(f.m_n[2]); + optimal_parents = parents; + optimal_bary = bary; + optimal_dist = dist; + min_bary_weight = new_min_bary_weight; + } + } + psb->m_renderNodesInterpolationWeights[i] = optimal_bary; + psb->m_renderNodesParents[i] = optimal_parents; + psb->m_z[i] = optimal_dist; + } +} diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h index 7271530109a..237d29761d7 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h @@ -17,132 +17,151 @@ subject to the following restrictions: #define BT_SOFT_BODY_HELPERS_H #include "btSoftBody.h" - +#include <fstream> +#include <string> // // 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 -};}; +/* 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 +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); + /* 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, + btScalar perturbation = 0.); + /* 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); + static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file); + + static void writeObj(const char* file, const btSoftBody* psb); + + static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary); + + static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary); + + static void interpolateBarycentricWeights(btSoftBody* psb); + static void extrapolateBarycentricWeights(btSoftBody* psb); - /* 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); + static void generateBoundaryFaces(btSoftBody* psb); + static void duplicateFaces(const char* filename, const btSoftBody* psb); /// 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 ); + static void ReoptimizeLinkOrder(btSoftBody* psb); }; -#endif //BT_SOFT_BODY_HELPERS_H +#endif //BT_SOFT_BODY_HELPERS_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h index 1b9d02d79f9..c17bbb5cd4c 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h @@ -18,30 +18,643 @@ subject to the following restrictions: #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 +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" +#include <string.h> //for memset +#include <cmath> +#include "poly34.h" + +// Given a multibody link, a contact point and a contact direction, fill in the jacobian data needed to calculate the velocity change given an impulse in the contact direction +static SIMD_FORCE_INLINE void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, + btMultiBodyJacobianData& jacobianData, + const btVector3& contact_point, + const btVector3& dir) +{ + 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, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m); + multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v); +} +static SIMD_FORCE_INLINE btVector3 generateUnitOrthogonalVector(const btVector3& u) +{ + btScalar ux = u.getX(); + btScalar uy = u.getY(); + btScalar uz = u.getZ(); + btScalar ax = std::abs(ux); + btScalar ay = std::abs(uy); + btScalar az = std::abs(uz); + btVector3 v; + if (ax <= ay && ax <= az) + v = btVector3(0, -uz, uy); + else if (ay <= ax && ay <= az) + v = btVector3(-uz, 0, ux); + else + v = btVector3(-uy, ux, 0); + v.normalize(); + return v; +} + +static SIMD_FORCE_INLINE bool proximityTest(const btVector3& x1, const btVector3& x2, const btVector3& x3, const btVector3& x4, const btVector3& normal, const btScalar& mrg, btVector3& bary) +{ + btVector3 x43 = x4 - x3; + if (std::abs(x43.dot(normal)) > mrg) + return false; + btVector3 x13 = x1 - x3; + btVector3 x23 = x2 - x3; + btScalar a11 = x13.length2(); + btScalar a22 = x23.length2(); + btScalar a12 = x13.dot(x23); + btScalar b1 = x13.dot(x43); + btScalar b2 = x23.dot(x43); + btScalar det = a11 * a22 - a12 * a12; + if (det < SIMD_EPSILON) + return false; + btScalar w1 = (b1 * a22 - b2 * a12) / det; + btScalar w2 = (b2 * a11 - b1 * a12) / det; + btScalar w3 = 1 - w1 - w2; + btScalar delta = mrg / std::sqrt(0.5 * std::abs(x13.cross(x23).safeNorm())); + bary = btVector3(w1, w2, w3); + for (int i = 0; i < 3; ++i) + { + if (bary[i] < -delta || bary[i] > 1 + delta) + return false; + } + return true; +} +static const int KDOP_COUNT = 13; +static btVector3 dop[KDOP_COUNT] = {btVector3(1, 0, 0), + btVector3(0, 1, 0), + btVector3(0, 0, 1), + btVector3(1, 1, 0), + btVector3(1, 0, 1), + btVector3(0, 1, 1), + btVector3(1, -1, 0), + btVector3(1, 0, -1), + btVector3(0, 1, -1), + btVector3(1, 1, 1), + btVector3(1, -1, 1), + btVector3(1, 1, -1), + btVector3(1, -1, -1)}; + +static inline int getSign(const btVector3& n, const btVector3& x) +{ + btScalar d = n.dot(x); + if (d > SIMD_EPSILON) + return 1; + if (d < -SIMD_EPSILON) + return -1; + return 0; +} + +static SIMD_FORCE_INLINE bool hasSeparatingPlane(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) +{ + btVector3 hex[6] = {face->m_n[0]->m_x - node->m_x, + face->m_n[1]->m_x - node->m_x, + face->m_n[2]->m_x - node->m_x, + face->m_n[0]->m_x + dt * face->m_n[0]->m_v - node->m_x, + face->m_n[1]->m_x + dt * face->m_n[1]->m_v - node->m_x, + face->m_n[2]->m_x + dt * face->m_n[2]->m_v - node->m_x}; + btVector3 segment = dt * node->m_v; + for (int i = 0; i < KDOP_COUNT; ++i) + { + int s = getSign(dop[i], segment); + int j = 0; + for (; j < 6; ++j) + { + if (getSign(dop[i], hex[j]) == s) + break; + } + if (j == 6) + return true; + } + return false; +} + +static SIMD_FORCE_INLINE bool nearZero(const btScalar& a) +{ + return (a > -SAFE_EPSILON && a < SAFE_EPSILON); +} +static SIMD_FORCE_INLINE bool sameSign(const btScalar& a, const btScalar& b) +{ + return (nearZero(a) || nearZero(b) || (a > SAFE_EPSILON && b > SAFE_EPSILON) || (a < -SAFE_EPSILON && b < -SAFE_EPSILON)); +} +static SIMD_FORCE_INLINE bool diffSign(const btScalar& a, const btScalar& b) +{ + return !sameSign(a, b); +} +inline btScalar evaluateBezier2(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& t, const btScalar& s) +{ + btScalar s2 = s * s; + btScalar t2 = t * t; + + return p0 * s2 + p1 * btScalar(2.0) * s * t + p2 * t2; +} +inline btScalar evaluateBezier(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& p3, const btScalar& t, const btScalar& s) +{ + btScalar s2 = s * s; + btScalar s3 = s2 * s; + btScalar t2 = t * t; + btScalar t3 = t2 * t; + + return p0 * s3 + p1 * btScalar(3.0) * s2 * t + p2 * btScalar(3.0) * s * t2 + p3 * t3; +} +static SIMD_FORCE_INLINE bool getSigns(bool type_c, const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, const btScalar& t1, btScalar& lt0, btScalar& lt1) +{ + if (sameSign(t0, t1)) + { + lt0 = t0; + lt1 = t0; + return true; + } + + if (type_c || diffSign(k0, k3)) + { + btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1); + if (t0 < -0) + ft = -ft; + + if (sameSign(ft, k0)) + { + lt0 = t1; + lt1 = t1; + } + else + { + lt0 = t0; + lt1 = t0; + } + return true; + } + + if (!type_c) + { + btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1); + if (t0 < -0) + ft = -ft; + + if (diffSign(ft, k0)) + { + lt0 = t0; + lt1 = t1; + return true; + } + + btScalar fk = evaluateBezier2(k1 - k0, k2 - k1, k3 - k2, t0, -t1); + + if (sameSign(fk, k1 - k0)) + lt0 = lt1 = t1; + else + lt0 = lt1 = t0; + + return true; + } + return false; +} + +static SIMD_FORCE_INLINE void getBernsteinCoeff(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, btScalar& k0, btScalar& k1, btScalar& k2, btScalar& k3) +{ + const btVector3& n0 = face->m_n0; + const btVector3& n1 = face->m_n1; + btVector3 n_hat = n0 + n1 - face->m_vn; + btVector3 p0ma0 = node->m_x - face->m_n[0]->m_x; + btVector3 p1ma1 = node->m_q - face->m_n[0]->m_q; + k0 = (p0ma0).dot(n0) * 3.0; + k1 = (p0ma0).dot(n_hat) + (p1ma1).dot(n0); + k2 = (p1ma1).dot(n_hat) + (p0ma0).dot(n1); + k3 = (p1ma1).dot(n1) * 3.0; +} + +static SIMD_FORCE_INLINE void polyDecomposition(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, btScalar& u0, btScalar& u1, btScalar& v0, btScalar& v1) +{ + btScalar denom = 4.0 * (j1 - j2) * (j1 - j0) + (j2 - j0) * (j2 - j0); + u0 = (2.0 * (j1 - j2) * (3.0 * k1 - 2.0 * k0 - k3) - (j0 - j2) * (3.0 * k2 - 2.0 * k3 - k0)) / denom; + u1 = (2.0 * (j1 - j0) * (3.0 * k2 - 2.0 * k3 - k0) - (j2 - j0) * (3.0 * k1 - 2.0 * k0 - k3)) / denom; + v0 = k0 - u0 * j0; + v1 = k3 - u1 * j2; +} + +static SIMD_FORCE_INLINE bool rootFindingLemma(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3) +{ + btScalar u0, u1, v0, v1; + btScalar j0 = 3.0 * (k1 - k0); + btScalar j1 = 3.0 * (k2 - k1); + btScalar j2 = 3.0 * (k3 - k2); + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (sameSign(v0, v1)) + { + btScalar Ypa = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * v0 * (1.0 - v0) + j2 * v0 * v0; // Y'(v0) + if (sameSign(Ypa, j0)) + { + return (diffSign(k0, v1)); + } + } + return diffSign(k0, v0); +} + +static SIMD_FORCE_INLINE void getJs(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Node* a, const btSoftBody::Node* b, const btSoftBody::Node* c, const btSoftBody::Node* p, const btScalar& dt, btScalar& j0, btScalar& j1, btScalar& j2) +{ + const btVector3& a0 = a->m_x; + const btVector3& b0 = b->m_x; + const btVector3& c0 = c->m_x; + const btVector3& va = a->m_v; + const btVector3& vb = b->m_v; + const btVector3& vc = c->m_v; + const btVector3 a1 = a0 + dt * va; + const btVector3 b1 = b0 + dt * vb; + const btVector3 c1 = c0 + dt * vc; + btVector3 n0 = (b0 - a0).cross(c0 - a0); + btVector3 n1 = (b1 - a1).cross(c1 - a1); + btVector3 n_hat = n0 + n1 - dt * dt * (vb - va).cross(vc - va); + const btVector3& p0 = p->m_x; + const btVector3& vp = p->m_v; + btVector3 p1 = p0 + dt * vp; + btVector3 m0 = (b0 - p0).cross(c0 - p0); + btVector3 m1 = (b1 - p1).cross(c1 - p1); + btVector3 m_hat = m0 + m1 - dt * dt * (vb - vp).cross(vc - vp); + btScalar l0 = m0.dot(n0); + btScalar l1 = 0.25 * (m0.dot(n_hat) + m_hat.dot(n0)); + btScalar l2 = btScalar(1) / btScalar(6) * (m0.dot(n1) + m_hat.dot(n_hat) + m1.dot(n0)); + btScalar l3 = 0.25 * (m_hat.dot(n1) + m1.dot(n_hat)); + btScalar l4 = m1.dot(n1); + + btScalar k1p = 0.25 * k0 + 0.75 * k1; + btScalar k2p = 0.5 * k1 + 0.5 * k2; + btScalar k3p = 0.75 * k2 + 0.25 * k3; + + btScalar s0 = (l1 * k0 - l0 * k1p) * 4.0; + btScalar s1 = (l2 * k0 - l0 * k2p) * 2.0; + btScalar s2 = (l3 * k0 - l0 * k3p) * btScalar(4) / btScalar(3); + btScalar s3 = l4 * k0 - l0 * k3; + + j0 = (s1 * k0 - s0 * k1) * 3.0; + j1 = (s2 * k0 - s0 * k2) * 1.5; + j2 = (s3 * k0 - s0 * k3); +} + +static SIMD_FORCE_INLINE bool signDetermination1Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1) +{ + btScalar Yu0 = k0 * (1.0 - u0) * (1.0 - u0) * (1.0 - u0) + 3.0 * k1 * u0 * (1.0 - u0) * (1.0 - u0) + 3.0 * k2 * u0 * u0 * (1.0 - u0) + k3 * u0 * u0 * u0; // Y(u0) + btScalar Yv0 = k0 * (1.0 - v0) * (1.0 - v0) * (1.0 - v0) + 3.0 * k1 * v0 * (1.0 - v0) * (1.0 - v0) + 3.0 * k2 * v0 * v0 * (1.0 - v0) + k3 * v0 * v0 * v0; // Y(v0) + + btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0; + btScalar L = sameSign(sign_Ytp, k0) ? u1 : u0; + sign_Ytp = (v0 > v1) ? Yv0 : -Yv0; + btScalar K = (sameSign(sign_Ytp, k0)) ? v1 : v0; + return diffSign(L, K); +} + +static SIMD_FORCE_INLINE bool signDetermination2Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1) +{ + btScalar Yu0 = k0 * (1.0 - u0) * (1.0 - u0) * (1.0 - u0) + 3.0 * k1 * u0 * (1.0 - u0) * (1.0 - u0) + 3.0 * k2 * u0 * u0 * (1.0 - u0) + k3 * u0 * u0 * u0; // Y(u0) + btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0, L1, L2; + if (diffSign(sign_Ytp, k0)) + { + L1 = u0; + L2 = u1; + } + else + { + btScalar Yp_u0 = j0 * (1.0 - u0) * (1.0 - u0) + 2.0 * j1 * (1.0 - u0) * u0 + j2 * u0 * u0; + if (sameSign(Yp_u0, j0)) + { + L1 = u1; + L2 = u1; + } + else + { + L1 = u0; + L2 = u0; + } + } + btScalar Yv0 = k0 * (1.0 - v0) * (1.0 - v0) * (1.0 - v0) + 3.0 * k1 * v0 * (1.0 - v0) * (1.0 - v0) + 3.0 * k2 * v0 * v0 * (1.0 - v0) + k3 * v0 * v0 * v0; // Y(uv0) + sign_Ytp = (v0 > v1) ? Yv0 : -Yv0; + btScalar K1, K2; + if (diffSign(sign_Ytp, k0)) + { + K1 = v0; + K2 = v1; + } + else + { + btScalar Yp_v0 = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * (1.0 - v0) * v0 + j2 * v0 * v0; + if (sameSign(Yp_v0, j0)) + { + K1 = v1; + K2 = v1; + } + else + { + K1 = v0; + K2 = v0; + } + } + return (diffSign(K1, L1) || diffSign(L2, K2)); +} + +static SIMD_FORCE_INLINE bool signDetermination1(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) +{ + btScalar j0, j1, j2, u0, u1, v0, v1; + // p1 + getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1)) + return false; + } + // p2 + getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1)) + return false; + } + // p3 + getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1)) + return false; + } + return true; +} + +static SIMD_FORCE_INLINE bool signDetermination2(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) +{ + btScalar j0, j1, j2, u0, u1, v0, v1; + // p1 + getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + bool bt0 = true, bt1 = true; + getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + bt0 = false; + if (lt1 < -SAFE_EPSILON) + bt1 = false; + if (!bt0 && !bt1) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1)) + return false; + } + // p2 + getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + bool bt0 = true, bt1 = true; + getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + bt0 = false; + if (lt1 < -SAFE_EPSILON) + bt1 = false; + if (!bt0 && !bt1) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1)) + return false; + } + // p3 + getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + bool bt0 = true, bt1 = true; + getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + bt0 = false; + if (lt1 < -SAFE_EPSILON) + bt1 = false; + if (!bt0 && !bt1) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1)) + return false; + } + return true; +} + +static SIMD_FORCE_INLINE bool coplanarAndInsideTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) +{ + // Coplanar test + if (diffSign(k1 - k0, k3 - k2)) + { + // Case b: + if (sameSign(k0, k3) && !rootFindingLemma(k0, k1, k2, k3)) + return false; + // inside test + return signDetermination2(k0, k1, k2, k3, face, node, dt); + } + else + { + // Case c: + if (sameSign(k0, k3)) + return false; + // inside test + return signDetermination1(k0, k1, k2, k3, face, node, dt); + } + return false; +} +static SIMD_FORCE_INLINE bool conservativeCulling(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg) +{ + if (k0 > mrg && k1 > mrg && k2 > mrg && k3 > mrg) + return true; + if (k0 < -mrg && k1 < -mrg && k2 < -mrg && k3 < -mrg) + return true; + return false; +} + +static SIMD_FORCE_INLINE bool bernsteinVFTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) +{ + if (conservativeCulling(k0, k1, k2, k3, mrg)) + return false; + return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt); +} + +static SIMD_FORCE_INLINE void deCasteljau(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, btScalar& k10, btScalar& k20, btScalar& k30, btScalar& k21, btScalar& k12) +{ + k10 = k0 * (1.0 - t0) + k1 * t0; + btScalar k11 = k1 * (1.0 - t0) + k2 * t0; + k12 = k2 * (1.0 - t0) + k3 * t0; + k20 = k10 * (1.0 - t0) + k11 * t0; + k21 = k11 * (1.0 - t0) + k12 * t0; + k30 = k20 * (1.0 - t0) + k21 * t0; +} +static SIMD_FORCE_INLINE bool bernsteinVFTest(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg) +{ + btScalar k0, k1, k2, k3; + getBernsteinCoeff(face, node, dt, k0, k1, k2, k3); + if (conservativeCulling(k0, k1, k2, k3, mrg)) + return false; + return true; + if (diffSign(k2 - 2.0 * k1 + k0, k3 - 2.0 * k2 + k1)) + { + btScalar k10, k20, k30, k21, k12; + btScalar t0 = (k2 - 2.0 * k1 + k0) / (k0 - 3.0 * k1 + 3.0 * k2 - k3); + deCasteljau(k0, k1, k2, k3, t0, k10, k20, k30, k21, k12); + return bernsteinVFTest(k0, k10, k20, k30, mrg, face, node, dt) || bernsteinVFTest(k30, k21, k12, k3, mrg, face, node, dt); + } + return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt); +} + +static SIMD_FORCE_INLINE bool continuousCollisionDetection(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary) +{ + if (hasSeparatingPlane(face, node, dt)) + return false; + btVector3 x21 = face->m_n[1]->m_x - face->m_n[0]->m_x; + btVector3 x31 = face->m_n[2]->m_x - face->m_n[0]->m_x; + btVector3 x41 = node->m_x - face->m_n[0]->m_x; + btVector3 v21 = face->m_n[1]->m_v - face->m_n[0]->m_v; + btVector3 v31 = face->m_n[2]->m_v - face->m_n[0]->m_v; + btVector3 v41 = node->m_v - face->m_n[0]->m_v; + btVector3 a = x21.cross(x31); + btVector3 b = x21.cross(v31) + v21.cross(x31); + btVector3 c = v21.cross(v31); + btVector3 d = x41; + btVector3 e = v41; + btScalar a0 = a.dot(d); + btScalar a1 = a.dot(e) + b.dot(d); + btScalar a2 = c.dot(d) + b.dot(e); + btScalar a3 = c.dot(e); + btScalar eps = SAFE_EPSILON; + int num_roots = 0; + btScalar roots[3]; + if (std::abs(a3) < eps) + { + // cubic term is zero + if (std::abs(a2) < eps) + { + if (std::abs(a1) < eps) + { + if (std::abs(a0) < eps) + { + num_roots = 2; + roots[0] = 0; + roots[1] = dt; + } + } + else + { + num_roots = 1; + roots[0] = -a0 / a1; + } + } + else + { + num_roots = SolveP2(roots, a1 / a2, a0 / a2); + } + } + else + { + num_roots = SolveP3(roots, a2 / a3, a1 / a3, a0 / a3); + } + // std::sort(roots, roots+num_roots); + if (num_roots > 1) + { + if (roots[0] > roots[1]) + btSwap(roots[0], roots[1]); + } + if (num_roots > 2) + { + if (roots[0] > roots[2]) + btSwap(roots[0], roots[2]); + if (roots[1] > roots[2]) + btSwap(roots[1], roots[2]); + } + for (int r = 0; r < num_roots; ++r) + { + double root = roots[r]; + if (root <= 0) + continue; + if (root > dt + SIMD_EPSILON) + return false; + btVector3 x1 = face->m_n[0]->m_x + root * face->m_n[0]->m_v; + btVector3 x2 = face->m_n[1]->m_x + root * face->m_n[1]->m_v; + btVector3 x3 = face->m_n[2]->m_x + root * face->m_n[2]->m_v; + btVector3 x4 = node->m_x + root * node->m_v; + btVector3 normal = (x2 - x1).cross(x3 - x1); + normal.safeNormalize(); + if (proximityTest(x1, x2, x3, x4, normal, mrg, bary)) + return true; + } + return false; +} +static SIMD_FORCE_INLINE bool bernsteinCCD(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary) +{ + if (!bernsteinVFTest(face, node, dt, mrg)) + return false; + if (!continuousCollisionDetection(face, node, dt, 1e-6, bary)) + return false; + return true; +} + // // 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; -}; + 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 @@ -49,67 +662,64 @@ struct btSymMatrix class btSoftBodyCollisionShape : public btConcaveShape { public: - btSoftBody* m_body; + btSoftBody* m_body; btSoftBodyCollisionShape(btSoftBody* backptr) { m_shapeType = SOFTBODY_SHAPE_PROXYTYPE; - m_body=backptr; + m_body = backptr; } virtual ~btSoftBodyCollisionShape() { - } - void processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const + 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 + 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) + 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*/) - { + virtual void setLocalScaling(const btVector3& /*scaling*/) + { ///na } virtual const btVector3& getLocalScaling() const { - static const btVector3 dummy(1,1,1); + static const btVector3 dummy(1, 1, 1); return dummy; } - virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const + virtual void calculateLocalInertia(btScalar /*mass*/, btVector3& /*inertia*/) const { ///not yet btAssert(0); } - virtual const char* getName()const + virtual const char* getName() const { return "SoftBody"; } - }; // @@ -118,48 +728,53 @@ public: class btSoftClusterCollisionShape : public btConvexInternalShape { public: - const btSoftBody::Cluster* m_cluster; - - btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); } + const btSoftBody::Cluster* m_cluster; + btSoftClusterCollisionShape(const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); } - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const + 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) + 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; } + const btScalar k = btDot(vec, n[i]->m_x); + if (k > d) + { + d = k; + j = i; + } } - return(n[j]->m_x); + return (n[j]->m_x); } - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const { - return(localGetSupportingVertex(vec)); + return (localGetSupportingVertex(vec)); } //notice that the vectors should be unit length - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const - {} - + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const + { + } - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const - {} + virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const + { + } - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - {} + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const + { + } - virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; } + virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; } //debugging - virtual const char* getName()const {return "SOFTCLUSTER";} + virtual const char* getName() const { return "SOFTCLUSTER"; } - virtual void setMargin(btScalar margin) + virtual void setMargin(btScalar margin) { btConvexInternalShape::setMargin(margin); } - virtual btScalar getMargin() const + virtual btScalar getMargin() const { return btConvexInternalShape::getMargin(); } @@ -171,382 +786,548 @@ public: // template <typename T> -static inline void ZeroInitialize(T& value) +static inline void ZeroInitialize(T& value) { - memset(&value,0,sizeof(T)); + memset(&value, 0, sizeof(T)); } // template <typename T> -static inline bool CompLess(const T& a,const T& b) -{ return(a<b); } +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); } +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); } +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 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) +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); + 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) +static inline btVector3 Clamp(const btVector3& v, btScalar maxlength) { - const btScalar sql=v.length2(); - if(sql>(maxlength*maxlength)) - return((v*maxlength)/btSqrt(sql)); + const btScalar sql = v.length2(); + if (sql > (maxlength * maxlength)) + return ((v * maxlength) / btSqrt(sql)); else - return(v); + 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); } +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); } +static inline T Sq(const T& x) +{ + return (x * x); +} // template <typename T> -static inline T Cube(const T& x) -{ return(x*x*x); } +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)); } +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 bool SameSign(const T& x, const T& y) +{ + return ((x * y) > 0); +} // -static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y) +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])); + const btVector3 d = x - y; + return (btFabs(d[0]) + btFabs(d[1]) + btFabs(d[2])); } // -static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s) +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); + 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) +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 Diagonal(const btVector3& v) +{ + btMatrix3x3 m; + m[0] = btVector3(v.getX(), 0, 0); + m[1] = btVector3(0, v.getY(), 0); + m[2] = btVector3(0, 0, v.getZ()); + return (m); +} + +static inline btScalar Dot(const btScalar* a, const btScalar* b, int ndof) +{ + btScalar result = 0; + for (int i = 0; i < ndof; ++i) + result += a[i] * b[i]; + return result; +} + +static inline btMatrix3x3 OuterProduct(const btScalar* v1, const btScalar* v2, const btScalar* v3, + const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof) +{ + btMatrix3x3 m; + btScalar a11 = Dot(v1, u1, ndof); + btScalar a12 = Dot(v1, u2, ndof); + btScalar a13 = Dot(v1, u3, ndof); + + btScalar a21 = Dot(v2, u1, ndof); + btScalar a22 = Dot(v2, u2, ndof); + btScalar a23 = Dot(v2, u3, ndof); + + btScalar a31 = Dot(v3, u1, ndof); + btScalar a32 = Dot(v3, u2, ndof); + btScalar a33 = Dot(v3, u3, ndof); + m[0] = btVector3(a11, a12, a13); + m[1] = btVector3(a21, a22, a23); + m[2] = btVector3(a31, a32, a33); + return (m); +} + +static inline btMatrix3x3 OuterProduct(const btVector3& v1, const btVector3& v2) { - 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); + btMatrix3x3 m; + btScalar a11 = v1[0] * v2[0]; + btScalar a12 = v1[0] * v2[1]; + btScalar a13 = v1[0] * v2[2]; + + btScalar a21 = v1[1] * v2[0]; + btScalar a22 = v1[1] * v2[1]; + btScalar a23 = v1[1] * v2[2]; + + btScalar a31 = v1[2] * v2[0]; + btScalar a32 = v1[2] * v2[1]; + btScalar a33 = v1[2] * v2[2]; + m[0] = btVector3(a11, a12, a13); + m[1] = btVector3(a21, a22, a23); + m[2] = btVector3(a31, a32, a33); + return (m); } + // -static inline btMatrix3x3 Diagonal(btScalar x) +static inline btMatrix3x3 Add(const btMatrix3x3& a, + const btMatrix3x3& b) { - btMatrix3x3 m; - m[0]=btVector3(x,0,0); - m[1]=btVector3(0,x,0); - m[2]=btVector3(0,0,x); - return(m); + btMatrix3x3 r; + for (int i = 0; i < 3; ++i) r[i] = a[i] + b[i]; + return (r); } // -static inline btMatrix3x3 Add(const btMatrix3x3& a, - const btMatrix3x3& b) +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); + 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) +static inline btMatrix3x3 Mul(const btMatrix3x3& a, + btScalar b) { - btMatrix3x3 r; - for(int i=0;i<3;++i) r[i]=a[i]-b[i]; - return(r); + btMatrix3x3 r; + for (int i = 0; i < 3; ++i) r[i] = a[i] * b; + return (r); } // -static inline btMatrix3x3 Mul(const btMatrix3x3& a, - btScalar b) +static inline void Orthogonalize(btMatrix3x3& m) { - btMatrix3x3 r; - for(int i=0;i<3;++i) r[i]=a[i]*b; - return(r); + 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 void Orthogonalize(btMatrix3x3& m) +static inline btMatrix3x3 MassMatrix(btScalar im, const btMatrix3x3& iwi, const btVector3& r) { - m[2]=btCross(m[0],m[1]).normalized(); - m[1]=btCross(m[2],m[0]).normalized(); - m[0]=btCross(m[1],m[2]).normalized(); + const btMatrix3x3 cr = Cross(r); + return (Sub(Diagonal(im), cr * iwi * cr)); } + // -static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r) +static inline btMatrix3x3 ImpulseMatrix(btScalar dt, + btScalar ima, + btScalar imb, + const btMatrix3x3& iwi, + const btVector3& r) { - const btMatrix3x3 cr=Cross(r); - return(Sub(Diagonal(im),cr*iwi*cr)); + return (Diagonal(1 / dt) * Add(Diagonal(ima), MassMatrix(imb, iwi, r)).inverse()); } // -static inline btMatrix3x3 ImpulseMatrix( btScalar dt, - btScalar ima, - btScalar imb, - const btMatrix3x3& iwi, - const btVector3& r) +static inline btMatrix3x3 ImpulseMatrix(btScalar dt, + const btMatrix3x3& effective_mass_inv, + btScalar imb, + const btMatrix3x3& iwi, + const btVector3& r) { - return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse()); + return (Diagonal(1 / dt) * Add(effective_mass_inv, MassMatrix(imb, iwi, r)).inverse()); + // btMatrix3x3 iimb = MassMatrix(imb, iwi, r); + // if (iimb.determinant() == 0) + // return effective_mass_inv.inverse(); + // return effective_mass_inv.inverse() * Add(effective_mass_inv.inverse(), iimb.inverse()).inverse() * iimb.inverse(); } // -static inline btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra, - btScalar imb,const btMatrix3x3& iib,const btVector3& rb) +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()); + return (Add(MassMatrix(ima, iia, ra), MassMatrix(imb, iib, rb)).inverse()); } // -static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia, - const btMatrix3x3& iib) +static inline btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3& iia, + const btMatrix3x3& iib) { - return(Add(iia,iib).inverse()); + return (Add(iia, iib).inverse()); } // -static inline btVector3 ProjectOnAxis( const btVector3& v, - const btVector3& a) +static inline btVector3 ProjectOnAxis(const btVector3& v, + const btVector3& a) { - return(a*btDot(v,a)); + return (a * btDot(v, a)); } // -static inline btVector3 ProjectOnPlane( const btVector3& v, - const btVector3& a) +static inline btVector3 ProjectOnPlane(const btVector3& v, + const btVector3& a) { - return(v-ProjectOnAxis(v,a)); + return (v - ProjectOnAxis(v, a)); } // -static inline void ProjectOrigin( const btVector3& a, - const btVector3& b, - btVector3& prj, - btScalar& sqd) +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) + 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; + prj = p; + sqd = l2; } } } // -static inline void ProjectOrigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - btVector3& prj, - btScalar& sqd) +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& 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 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; + 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); + ProjectOrigin(a, b, prj, sqd); + ProjectOrigin(b, c, prj, sqd); + ProjectOrigin(c, a, prj, sqd); } } } } // +static inline bool rayIntersectsTriangle(const btVector3& origin, const btVector3& dir, const btVector3& v0, const btVector3& v1, const btVector3& v2, btScalar& t) +{ + btScalar a, f, u, v; + + btVector3 e1 = v1 - v0; + btVector3 e2 = v2 - v0; + btVector3 h = dir.cross(e2); + a = e1.dot(h); + + if (a > -0.00001 && a < 0.00001) + return (false); + + f = btScalar(1) / a; + btVector3 s = origin - v0; + u = f * s.dot(h); + + if (u < 0.0 || u > 1.0) + return (false); + + btVector3 q = s.cross(e1); + v = f * dir.dot(q); + if (v < 0.0 || u + v > 1.0) + return (false); + // at this stage we can compute t to find out where + // the intersection point is on the line + t = f * e2.dot(q); + if (t > 0) // ray intersection + return (true); + else // this means that there is a line intersection + // but not a ray intersection + return (false); +} + +static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVector3& rayEnd, const btVector3& p1, const btVector3& p2, const btVector3& p3, btVector3& sect, btVector3& normal) +{ + btVector3 dir = rayEnd - rayStart; + btScalar dir_norm = dir.norm(); + if (dir_norm < SIMD_EPSILON) + return false; + dir.normalize(); + btScalar t; + bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t); + + if (ret) + { + if (t <= dir_norm) + { + sect = rayStart + dir * t; + } + else + { + ret = false; + } + } + + if (ret) + { + btVector3 n = (p3 - p1).cross(p2 - p1); + n.safeNormalize(); + if (n.dot(dir) < 0) + normal = n; + else + normal = -n; + } + return ret; +} + +// template <typename T> -static inline T BaryEval( const T& a, - const T& b, - const T& c, - const btVector3& coord) +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()); + 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) +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)); + 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)); } // -static inline btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn, - const btVector3& a, - const btVector3& b, - const btScalar accuracy, - const int maxiterations=256) +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]) + 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]); + 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) + 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; } + 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; } + { + span[1] = t; + values[1] = v; + } } - return(-1); + return (-1); } -// -static inline btVector3 NormalizeAny(const btVector3& v) +inline static void EvaluateMedium(const btSoftBodyWorldInfo* wfi, + const btVector3& x, + btSoftBody::sMedium& medium) { - const btScalar l=v.length(); - if(l>SIMD_EPSILON) - return(v/l); - else - return(btVector3(0,0,0)); + 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 btDbvtVolume VolumeOf( const btSoftBody::Face& f, - btScalar margin) +static inline btVector3 NormalizeAny(const btVector3& v) { - 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); + const btScalar l = v.length(); + if (l > SIMD_EPSILON) + return (v / l); + else + return (btVector3(0, 0, 0)); } // -static inline btVector3 CenterOf( const btSoftBody::Face& f) +static inline btDbvtVolume VolumeOf(const btSoftBody::Face& f, + btScalar margin) { - return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3); + 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 btScalar AreaOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2) +static inline btVector3 CenterOf(const btSoftBody::Face& f) { - const btVector3 a=x1-x0; - const btVector3 b=x2-x0; - const btVector3 cr=btCross(a,b); - const btScalar area=cr.length(); - return(area); + return ((f.m_n[0]->m_x + f.m_n[1]->m_x + f.m_n[2]->m_x) / 3); } // -static inline btScalar VolumeOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2, - const btVector3& x3) +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 c=x3-x0; - return(btDot(a,btCross(b,c))); + 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 void EvaluateMedium( const btSoftBodyWorldInfo* wfi, - const btVector3& x, - btSoftBody::sMedium& medium) +static inline btScalar VolumeOf(const btVector3& x0, + const btVector3& x1, + const btVector3& x2, + const btVector3& x3) { - 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(); - } - } + 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) + +// +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; + 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; + { /* 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) +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); + if ((a == ma) && (b == mb)) return (0); + if ((a == mb) && (b == ma)) return (1); + return (-1); } // @@ -554,58 +1335,72 @@ static inline int MatchEdge( const btSoftBody::Node* a, // straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html // outputs are NOT sorted. // -struct btEigen +struct btEigen { - static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0) + 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; + 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) + 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])) { - 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... */ + 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) + 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]); + *values = btVector3(a[0][0], a[1][1], a[2][2]); } - return(iterations); + return (iterations); } + private: - static inline void mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q) + 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]}}; + 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]; + 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) + 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]}}; + 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]; + 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]; } }; @@ -613,9 +1408,9 @@ private: // Polar decomposition, // "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986. // -static inline int PolarDecompose( const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s) +static inline int PolarDecompose(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s) { - static const btPolarDecomposition polar; + static const btPolarDecomposition polar; return polar.decompose(m, q, s); } @@ -627,282 +1422,687 @@ struct btSoftColliders // // ClusterBase // - struct ClusterBase : btDbvt::ICollide + struct ClusterBase : btDbvt::ICollide { - btScalar erp; - btScalar idt; - btScalar m_margin; - btScalar friction; - btScalar threshold; + 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; + 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) + bool SolveContact(const btGjkEpaSolver2::sResults& res, + btSoftBody::Body ba, const btSoftBody::Body bb, + btSoftBody::CJoint& joint) { - if(res.distance<m_margin) + 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); + 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); + return (false); } }; // // CollideCL_RS // - struct CollideCL_RS : ClusterBase + struct CollideCL_RS : ClusterBase { - btSoftBody* psb; - const btCollisionObjectWrapper* m_colObjWrap; + btSoftBody* psb; + const btCollisionObjectWrapper* m_colObjWrap; - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { - btSoftBody::Cluster* cluster=(btSoftBody::Cluster*)leaf->data; - btSoftClusterCollisionShape cshape(cluster); - - const btConvexShape* rshape=(const btConvexShape*)m_colObjWrap->getCollisionShape(); + 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) + 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)) + 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 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()) + 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; + 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; + pj->m_erp *= psb->m_cfg.kSRHR_CL; + pj->m_split *= psb->m_cfg.kSR_SPLT_CL; } } } } - void ProcessColObj(btSoftBody* ps,const btCollisionObjectWrapper* colObWrap) + 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(); + 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); - } + 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 + struct CollideCL_SS : ClusterBase { - btSoftBody* bodies[2]; - void Process(const btDbvtNode* la,const btDbvtNode* lb) + 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; - + 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())) + 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]; + 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)) + 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 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; + 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 + } + else { - static int count=0; + static int count = 0; count++; //printf("count=%d\n",count); - } } - void ProcessSoftSoft(btSoftBody* psa,btSoftBody* psb) + void ProcessSoftSoft(btSoftBody* psa, btSoftBody* psb) { - idt = psa->m_sst.isdt; + 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); - } + 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 + struct CollideSDF_RS : btDbvt::ICollide { - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { - btSoftBody::Node* node=(btSoftBody::Node*)leaf->data; + btSoftBody::Node* node = (btSoftBody::Node*)leaf->data; DoNode(*node); } - void DoNode(btSoftBody::Node& n) const + void DoNode(btSoftBody::Node& n) const { - const btScalar m=n.m_im>0?dynmargin:stamargin; - btSoftBody::RContact c; + 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)) + 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 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; + 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; + btSoftBody* psb; + const btCollisionObjectWrapper* m_colObj1Wrap; + btRigidBody* m_rigidBody; + btScalar dynmargin; + btScalar stamargin; + }; + + // + // CollideSDF_RD + // + struct CollideSDF_RD : 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::DeformableNodeRigidContact c; + + if (!n.m_battach) + { + // check for collision at x_{n+1}^* + if (psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true)) + { + const btScalar ima = n.m_im; + // todo: collision between multibody and fixed deformable node will be missed. + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + const btScalar ms = ima + imb; + if (ms > 0) + { + // resolve contact at x_n + psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false); + btSoftBody::sCti& cti = c.m_cti; + c.m_node = &n; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + c.m_c5 = n.m_effectiveMass_inv; + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + 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(); + + c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra); + // c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + c.m_c1 = ra; + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 normal = cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_x, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_x, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + c.m_c0 = rot.transpose() * local_impulse_matrix * rot; + c.jacobianData_normal = jacobianData_normal; + c.jacobianData_t1 = jacobianData_t1; + c.jacobianData_t2 = jacobianData_t2; + c.t1 = t1; + c.t2 = t2; + } + } + psb->m_nodeRigidContacts.push_back(c); + } + } + } + } + btSoftBody* psb; + const btCollisionObjectWrapper* m_colObj1Wrap; + btRigidBody* m_rigidBody; + btScalar dynmargin; + btScalar stamargin; + }; + + // + // CollideSDF_RDF + // + struct CollideSDF_RDF : btDbvt::ICollide + { + void Process(const btDbvtNode* leaf) + { + btSoftBody::Face* face = (btSoftBody::Face*)leaf->data; + DoNode(*face); + } + void DoNode(btSoftBody::Face& f) const + { + btSoftBody::Node* n0 = f.m_n[0]; + btSoftBody::Node* n1 = f.m_n[1]; + btSoftBody::Node* n2 = f.m_n[2]; + const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0) ? dynmargin : stamargin; + btSoftBody::DeformableFaceRigidContact c; + btVector3 contact_point; + btVector3 bary; + if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true)) + { + btScalar ima = n0->m_im + n1->m_im + n2->m_im; + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + // todo: collision between multibody and fixed deformable face will be missed. + const btScalar ms = ima + imb; + if (ms > 0) + { + // resolve contact at x_n + // psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false); + btSoftBody::sCti& cti = c.m_cti; + c.m_contactPoint = contact_point; + c.m_bary = bary; + // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices + c.m_weights = btScalar(2) / (btScalar(1) + bary.length2()) * bary; + c.m_face = &f; + // friction is handled by the nodes to prevent sticking + // const btScalar fc = 0; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + + // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf + ima = bary.getX() * c.m_weights.getX() * n0->m_im + bary.getY() * c.m_weights.getY() * n1->m_im + bary.getZ() * c.m_weights.getZ() * n2->m_im; + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + c.m_c5 = Diagonal(ima); + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + 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 = contact_point - wtr.getOrigin(); + + // we do not scale the impulse matrix by dt + c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + c.m_c1 = ra; + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 normal = cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + c.m_c0 = rot.transpose() * local_impulse_matrix * rot; + c.jacobianData_normal = jacobianData_normal; + c.jacobianData_t1 = jacobianData_t1; + c.jacobianData_t2 = jacobianData_t2; + c.t1 = t1; + c.t2 = t2; + } + } + psb->m_faceRigidContacts.push_back(c); + } + } + // Set caching barycenters to be false after collision detection. + // Only turn on when contact is static. + f.m_pcontact[3] = 0; + } + 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)) + 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; + for (int i = 0; i < 3; ++i) { - 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)) + if (face->m_n[i] == node) + continue; + } + + 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; + mb = 0; } - const btScalar ms=ma+mb; - if(ms>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; + 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; + btSoftBody* psb[2]; + btScalar mrg; }; -}; -#endif //_BT_SOFT_BODY_INTERNALS_H + // + // CollideVF_DD + // + struct CollideVF_DD : 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 bary; + if (proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary)) + { + const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; + const btVector3 w = bary; + 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::DeformableFaceNodeContact c; + c.m_normal = face->m_normal; + if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) + c.m_normal = -face->m_normal; + c.m_margin = mrg; + c.m_node = node; + c.m_face = face; + c.m_bary = w; + c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; + psb[0]->m_faceNodeContacts.push_back(c); + } + } + } + btSoftBody* psb[2]; + btScalar mrg; + bool useFaceNormal; + }; + + // + // CollideFF_DD + // + struct CollideFF_DD : btDbvt::ICollide + { + void Process(const btDbvntNode* lface1, + const btDbvntNode* lface2) + { + btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data; + btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data; + if (f1 != f2) + { + Repel(f1, f2); + Repel(f2, f1); + } + } + void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2) + { + //#define REPEL_NEIGHBOR 1 +#ifndef REPEL_NEIGHBOR + for (int node_id = 0; node_id < 3; ++node_id) + { + btSoftBody::Node* node = f1->m_n[node_id]; + for (int i = 0; i < 3; ++i) + { + if (f2->m_n[i] == node) + return; + } + } +#endif + bool skip = false; + for (int node_id = 0; node_id < 3; ++node_id) + { + btSoftBody::Node* node = f1->m_n[node_id]; +#ifdef REPEL_NEIGHBOR + for (int i = 0; i < 3; ++i) + { + if (f2->m_n[i] == node) + { + skip = true; + break; + } + } + if (skip) + { + skip = false; + continue; + } +#endif + btSoftBody::Face* face = f2; + btVector3 bary; + if (!proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary)) + continue; + btSoftBody::DeformableFaceNodeContact c; + c.m_normal = face->m_normal; + if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) + c.m_normal = -face->m_normal; + c.m_margin = mrg; + c.m_node = node; + c.m_face = face; + c.m_bary = bary; + c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; + psb[0]->m_faceNodeContacts.push_back(c); + } + } + btSoftBody* psb[2]; + btScalar mrg; + bool useFaceNormal; + }; + + struct CollideCCD : 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 bary; + if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary)) + { + btSoftBody::DeformableFaceNodeContact c; + c.m_normal = face->m_normal; + if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) + c.m_normal = -face->m_normal; + c.m_node = node; + c.m_face = face; + c.m_bary = bary; + c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; + psb[0]->m_faceNodeContacts.push_back(c); + } + } + void Process(const btDbvntNode* lface1, + const btDbvntNode* lface2) + { + btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data; + btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data; + if (f1 != f2) + { + Repel(f1, f2); + Repel(f2, f1); + } + } + void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2) + { + //#define REPEL_NEIGHBOR 1 +#ifndef REPEL_NEIGHBOR + for (int node_id = 0; node_id < 3; ++node_id) + { + btSoftBody::Node* node = f1->m_n[node_id]; + for (int i = 0; i < 3; ++i) + { + if (f2->m_n[i] == node) + return; + } + } +#endif + bool skip = false; + for (int node_id = 0; node_id < 3; ++node_id) + { + btSoftBody::Node* node = f1->m_n[node_id]; +#ifdef REPEL_NEIGHBOR + for (int i = 0; i < 3; ++i) + { + if (f2->m_n[i] == node) + { + skip = true; + break; + } + } + if (skip) + { + skip = false; + continue; + } +#endif + btSoftBody::Face* face = f2; + btVector3 bary; + if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary)) + { + btSoftBody::DeformableFaceNodeContact c; + c.m_normal = face->m_normal; + if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) + c.m_normal = -face->m_normal; + c.m_node = node; + c.m_face = face; + c.m_bary = bary; + c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; + psb[0]->m_faceNodeContacts.push_back(c); + } + } + } + btSoftBody* psb[2]; + btScalar dt, mrg; + bool useFaceNormal; + }; +}; +#endif //_BT_SOFT_BODY_INTERNALS_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp index f5a67f6d895..3127369ccd5 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp @@ -23,27 +23,27 @@ subject to the following restrictions: #define ENABLE_SOFTBODY_CONCAVE_COLLISIONS 1 btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo) -:btDefaultCollisionConfiguration(constructionInfo) + : btDefaultCollisionConfiguration(constructionInfo) { void* mem; - mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc),16); - m_softSoftCreateFunc = new(mem) btSoftSoftCollisionAlgorithm::CreateFunc; + 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_softRigidConvexCreateFunc = new (mem) btSoftRigidCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); - m_swappedSoftRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; - m_swappedSoftRigidConvexCreateFunc->m_swapped=true; + 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_softRigidConcaveCreateFunc = new (mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16); - m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc; - m_swappedSoftRigidConcaveCreateFunc->m_swapped=true; + 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 @@ -53,82 +53,79 @@ btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfigura 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); - + 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); + void* mem = btAlignedAlloc(sizeof(btPoolAllocator), 16); + m_collisionAlgorithmPool = new (mem) btPoolAllocator(collisionAlgorithmMaxElementSize, constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); } } - } btSoftBodyRigidBodyCollisionConfiguration::~btSoftBodyRigidBodyCollisionConfiguration() { m_softSoftCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_softSoftCreateFunc); + btAlignedFree(m_softSoftCreateFunc); m_softRigidConvexCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_softRigidConvexCreateFunc); + btAlignedFree(m_softRigidConvexCreateFunc); m_swappedSoftRigidConvexCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_swappedSoftRigidConvexCreateFunc); + btAlignedFree(m_swappedSoftRigidConvexCreateFunc); #ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS m_softRigidConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_softRigidConcaveCreateFunc); + btAlignedFree(m_softRigidConcaveCreateFunc); m_swappedSoftRigidConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_swappedSoftRigidConcaveCreateFunc); + 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) +btCollisionAlgorithmCreateFunc* btSoftBodyRigidBodyCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1) { - ///try to handle the softbody interactions first - if ((proxyType0 == SOFTBODY_SHAPE_PROXYTYPE ) && (proxyType1==SOFTBODY_SHAPE_PROXYTYPE)) + if ((proxyType0 == SOFTBODY_SHAPE_PROXYTYPE) && (proxyType1 == SOFTBODY_SHAPE_PROXYTYPE)) { - return m_softSoftCreateFunc; + return m_softSoftCreateFunc; } ///softbody versus convex - if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConvex(proxyType1)) + if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConvex(proxyType1)) { - return m_softRigidConvexCreateFunc; + return m_softRigidConvexCreateFunc; } ///convex versus soft body - if (btBroadphaseProxy::isConvex(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE ) + if (btBroadphaseProxy::isConvex(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE) { - return m_swappedSoftRigidConvexCreateFunc; + return m_swappedSoftRigidConvexCreateFunc; } #ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS ///softbody versus convex - if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConcave(proxyType1)) + if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConcave(proxyType1)) { - return m_softRigidConcaveCreateFunc; + return m_softRigidConcaveCreateFunc; } ///convex versus soft body - if (btBroadphaseProxy::isConcave(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE ) + if (btBroadphaseProxy::isConcave(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE) { - return m_swappedSoftRigidConcaveCreateFunc; + return m_swappedSoftRigidConcaveCreateFunc; } #endif ///fallback to the regular rigid collision shape - return btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(proxyType0,proxyType1); + return btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(proxyType0, proxyType1); } diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h index 21addcfe2e1..0396a52dac7 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h @@ -21,28 +21,23 @@ subject to the following restrictions: class btVoronoiSimplexSolver; class btGjkEpaPenetrationDepthSolver; - ///btSoftBodyRigidBodyCollisionConfiguration add softbody interaction on top of btDefaultCollisionConfiguration -class btSoftBodyRigidBodyCollisionConfiguration : public 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; + 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); - + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1); }; -#endif //BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION - +#endif //BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h b/extern/bullet2/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h index c4733d64000..bc538db4a25 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h @@ -16,7 +16,6 @@ subject to the following restrictions: #ifndef BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H #define BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H - class btVertexBufferDescriptor { public: @@ -27,8 +26,7 @@ public: OPENGL_BUFFER }; -protected: - +protected: bool m_hasVertexPositions; bool m_hasNormals; @@ -51,7 +49,6 @@ public: virtual ~btVertexBufferDescriptor() { - } virtual bool hasVertexPositions() const @@ -102,7 +99,6 @@ public: } }; - class btCPUVertexBufferDescriptor : public btVertexBufferDescriptor { protected: @@ -114,7 +110,7 @@ public: * 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 ) + btCPUVertexBufferDescriptor(float *basePointer, int vertexOffset, int vertexStride) { m_basePointer = basePointer; m_vertexOffset = vertexOffset; @@ -127,7 +123,7 @@ public: * 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 ) + btCPUVertexBufferDescriptor(float *basePointer, int vertexOffset, int vertexStride, int normalOffset, int normalStride) { m_basePointer = basePointer; @@ -142,7 +138,6 @@ public: virtual ~btCPUVertexBufferDescriptor() { - } /** @@ -162,4 +157,4 @@ public: } }; -#endif // #ifndef BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H +#endif // #ifndef BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodySolvers.h b/extern/bullet2/src/BulletSoftBody/btSoftBodySolvers.h index 6947bc27d28..dbb2624eee0 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodySolvers.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodySolvers.h @@ -18,7 +18,6 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" - class btSoftBodyTriangleData; class btSoftBodyLinkData; class btSoftBodyVertexData; @@ -26,7 +25,6 @@ class btVertexBufferDescriptor; class btCollisionObject; class btSoftBody; - class btSoftBodySolver { public: @@ -37,20 +35,19 @@ public: CL_SOLVER, CL_SIMD_SOLVER, DX_SOLVER, - DX_SIMD_SOLVER + DX_SIMD_SOLVER, + DEFORMABLE_SOLVER }; - protected: int m_numberOfPositionIterations; int m_numberOfVelocityIterations; // Simulation timescale float m_timeScale; - + public: - btSoftBodySolver() : - m_numberOfPositionIterations( 10 ), - m_timeScale( 1 ) + btSoftBodySolver() : m_numberOfPositionIterations(10), + m_timeScale(1) { m_numberOfVelocityIterations = 0; m_numberOfPositionIterations = 5; @@ -59,39 +56,38 @@ public: 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; + 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; + virtual void predictMotion(btScalar solverdt) = 0; /** Solve constraints for a set of soft bodies */ - virtual void solveConstraints( float solverdt ) = 0; + virtual void solveConstraints(btScalar 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; + virtual void processCollision(btSoftBody *, const struct btCollisionObjectWrapper *) = 0; /** Process a collision between two soft bodies */ - virtual void processCollision( btSoftBody*, btSoftBody* ) = 0; + virtual void processCollision(btSoftBody *, btSoftBody *) = 0; /** Set the number of velocity constraint solver iterations this solver uses. */ - virtual void setNumberOfPositionIterations( int iterations ) + virtual void setNumberOfPositionIterations(int iterations) { m_numberOfPositionIterations = iterations; } @@ -103,7 +99,7 @@ public: } /** Set the number of velocity constraint solver iterations this solver uses. */ - virtual void setNumberOfVelocityIterations( int iterations ) + virtual void setNumberOfVelocityIterations(int iterations) { m_numberOfVelocityIterations = iterations; } @@ -135,7 +131,6 @@ public: class btSoftBodySolverOutput { protected: - public: btSoftBodySolverOutput() { @@ -145,10 +140,8 @@ public: { } - /** 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; + virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) = 0; }; - -#endif // #ifndef BT_SOFT_BODY_SOLVERS_H +#endif // #ifndef BT_SOFT_BODY_SOLVERS_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp b/extern/bullet2/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp new file mode 100644 index 00000000000..282dbf75f08 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp @@ -0,0 +1,350 @@ +/* +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() +{ + btMultiBodyDynamicsWorld::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); + + serializeMultiBodies(serializer); + + serializeRigidBodies(serializer); + + serializeCollisionObjects(serializer); + + serializeContactManifolds(serializer); + + serializer->finishSerialization(); +} diff --git a/extern/bullet2/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h b/extern/bullet2/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h new file mode 100644 index 00000000000..f295945a6db --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h @@ -0,0 +1,105 @@ +/* +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/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp index 01c148a2ca8..5b65216e4b5 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp @@ -27,18 +27,16 @@ subject to the following restrictions: //#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(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) @@ -47,31 +45,28 @@ btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm() m_dispatcher->releaseManifold(m_manifoldPtr); } */ - } - #include <stdio.h> - -void btSoftRigidCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +#include "LinearMath/btQuickprof.h" +void btSoftRigidCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { + BT_PROFILE("btSoftRigidCollisionAlgorithm::processCollision"); (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()) + // 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) +btScalar btSoftRigidCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; @@ -81,6 +76,3 @@ btScalar btSoftRigidCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* //not yet return btScalar(1.); } - - - diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h index a9b513e3639..9773af19a0c 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h @@ -31,45 +31,41 @@ class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm // bool m_ownManifold; // btPersistentManifold* m_manifoldPtr; - btSoftBody* m_softBody; - btCollisionObject* m_rigidCollisionObject; + //btSoftBody* m_softBody; + //btCollisionObject* m_rigidCollisionObject; ///for rigid versus soft (instead of soft versus rigid), we use this swapped boolean - bool m_isSwapped; + bool m_isSwapped; public: - - btSoftRigidCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0,const btCollisionObjectWrapper* col1Wrap, bool isSwapped); + 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 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 btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { //we don't add any manifolds } - - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + 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, false); + } + else { - return new(mem) btSoftRigidCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,true); + return new (mem) btSoftRigidCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, true); } } }; - }; -#endif //BT_SOFT_RIGID_COLLISION_ALGORITHM_H - - +#endif //BT_SOFT_RIGID_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp index 653d5a06b49..510b731fc15 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btSoftRigidDynamicsWorld.h" #include "LinearMath/btQuickprof.h" @@ -24,42 +23,38 @@ subject to the following restrictions: #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) + btSoftBodySolver* softBodySolver) : btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration), + m_softBodySolver(softBodySolver), + m_ownsSolver(false) { - if( !m_softBodySolver ) + if (!m_softBodySolver) { - void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver),16); - m_softBodySolver = new(ptr) btDefaultSoftBodySolver(); + 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_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.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() @@ -71,82 +66,78 @@ btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld() } } -void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { - btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep ); + btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); { BT_PROFILE("predictUnconstraintMotionSoftBody"); - m_softBodySolver->predictMotion( float(timeStep) ); + m_softBodySolver->predictMotion(float(timeStep)); } } -void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep ) +void btSoftRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { - // Let the solver grab the soft bodies and if necessary optimize for it - m_softBodySolver->optimize( getSoftBodyArray() ); + m_softBodySolver->optimize(getSoftBodyArray()); - if( !m_softBodySolver->checkInitialized() ) + if (!m_softBodySolver->checkInitialized()) { - btAssert( "Solver initialization failed\n" ); + btAssert("Solver initialization failed\n"); } - btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep ); + btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep); ///solve soft bodies constraints - solveSoftBodiesConstraints( timeStep ); + solveSoftBodiesConstraints(timeStep); //self collisions - for ( int i=0;i<m_softBodies.size();i++) + for (int i = 0; i < m_softBodies.size(); i++) { - btSoftBody* psb=(btSoftBody*)m_softBodies[i]; + btSoftBody* psb = (btSoftBody*)m_softBodies[i]; psb->defaultCollisionHandler(psb); } ///update soft bodies - m_softBodySolver->updateSoftBodies( ); - + m_softBodySolver->updateSoftBodies(); + // End solver-wise simulation step // /////////////////////////////// - } -void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints( btScalar timeStep ) +void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints(btScalar timeStep) { BT_PROFILE("solveSoftConstraints"); - if(m_softBodies.size()) + if (m_softBodies.size()) { btSoftBody::solveClusters(m_softBodies); } // Solve constraints solver-wise - m_softBodySolver->solveConstraints( timeStep * m_softBodySolver->getTimeScale() ); - + m_softBodySolver->solveConstraints(timeStep * m_softBodySolver->getTimeScale()); } -void btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body,short int collisionFilterGroup,short int collisionFilterMask) +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 ); + body->setSoftBodySolver(m_softBodySolver); btCollisionWorld::addCollisionObject(body, - collisionFilterGroup, - collisionFilterMask); - + collisionFilterGroup, + collisionFilterMask); } -void btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body) +void btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body) { m_softBodies.remove(body); btCollisionWorld::removeCollisionObject(body); } -void btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) +void btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) { btSoftBody* body = btSoftBody::upcast(collisionObject); if (body) @@ -155,60 +146,57 @@ void btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisio btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); } -void btSoftRigidDynamicsWorld::debugDrawWorld() +void btSoftRigidDynamicsWorld::debugDrawWorld() { btDiscreteDynamicsWorld::debugDrawWorld(); if (getDebugDrawer()) { int i; - for ( i=0;i<this->m_softBodies.size();i++) + for (i = 0; i < this->m_softBodies.size(); i++) { - btSoftBody* psb=(btSoftBody*)this->m_softBodies[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); + 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); + 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) + 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); + btVector3 rayDir = (rayToWorld - rayFromWorld); - rayDir.normalize (); + 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]; @@ -217,22 +205,19 @@ struct btSoftSingleRayCallback : public btBroadphaseRayCallback 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); - + m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld); } - - - virtual bool process(const btBroadphaseProxy* proxy) + 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; + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; //only perform raycast if filterMask matches - if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; @@ -250,110 +235,106 @@ struct btSoftSingleRayCallback : public btBroadphaseRayCallback //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); + 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 +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); + btSoftSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback); #ifndef USE_BRUTEFORCE_RAYBROADPHASE - m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB); + m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB); #else - for (int i=0;i<this->getNumCollisionObjects();i++) + for (int i = 0; i < this->getNumCollisionObjects(); i++) { rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); - } -#endif //USE_BRUTEFORCE_RAYBROADPHASE - + } +#endif //USE_BRUTEFORCE_RAYBROADPHASE } - -void btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback) +void btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback) { - if (collisionShape->isSoftBody()) { + if (collisionShape->isSoftBody()) + { btSoftBody* softBody = btSoftBody::upcast(collisionObject); - if (softBody) { + if (softBody) + { btSoftBody::sRayCast softResult; - if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) + if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) { - - if (softResult.fraction<= resultCallback.m_closestHitFraction) + 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; + btVector3 normal = -rayDir; normal.normalize(); if (softResult.feature == btSoftBody::eFeature::Face) { normal = softBody->m_faces[softResult.index].m_normal; - if (normal.dot(rayDir) > 0) { + 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); + + btCollisionWorld::LocalRayResult rayResult(collisionObject, + &shapeInfo, + normal, + softResult.fraction); + bool normalInWorldSpace = true; + resultCallback.addSingleResult(rayResult, normalInWorldSpace); } } } - } - else { - btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback); + } + else + { + btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback); } } - -void btSoftRigidDynamicsWorld::serializeSoftBodies(btSerializer* serializer) +void btSoftRigidDynamicsWorld::serializeSoftBodies(btSerializer* serializer) { int i; //serialize all collision objects - for (i=0;i<m_collisionObjects.size();i++) + 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); + btChunk* chunk = serializer->allocate(len, 1); const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_SOFTBODY_CODE,colObj); + serializer->finalizeChunk(chunk, structType, BT_SOFTBODY_CODE, colObj); } } - } -void btSoftRigidDynamicsWorld::serialize(btSerializer* serializer) +void btSoftRigidDynamicsWorld::serialize(btSerializer* serializer) { - serializer->startSerialization(); - serializeDynamicsWorldInfo( serializer); + serializeDynamicsWorldInfo(serializer); serializeSoftBodies(serializer); @@ -363,5 +344,3 @@ void btSoftRigidDynamicsWorld::serialize(btSerializer* serializer) serializer->finishSerialization(); } - - diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h index 3e0efafd6c7..be49c444d7d 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h @@ -19,63 +19,60 @@ subject to the following restrictions: #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" #include "btSoftBody.h" -typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray; +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; + 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; + btSoftBodySolver* m_softBodySolver; + bool m_ownsSolver; protected: + virtual void predictUnconstraintMotion(btScalar timeStep); - virtual void predictUnconstraintMotion(btScalar timeStep); - - virtual void internalSingleStepSimulation( btScalar timeStep); + virtual void internalSingleStepSimulation(btScalar timeStep); - void solveSoftBodiesConstraints( btScalar timeStep ); + void solveSoftBodiesConstraints(btScalar timeStep); - void serializeSoftBodies(btSerializer* serializer); + void serializeSoftBodies(btSerializer* serializer); public: - - btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver *softBodySolver = 0 ); + btSoftRigidDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver* softBodySolver = 0); virtual ~btSoftRigidDynamicsWorld(); - virtual void debugDrawWorld(); + virtual void debugDrawWorld(); - void addSoftBody(btSoftBody* body,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter); + void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); - void removeSoftBody(btSoftBody* body); + 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); + virtual void removeCollisionObject(btCollisionObject* collisionObject); - int getDrawFlags() const { return(m_drawFlags); } - void setDrawFlags(int f) { m_drawFlags=f; } + int getDrawFlags() const { return (m_drawFlags); } + void setDrawFlags(int f) { m_drawFlags = f; } - btSoftBodyWorldInfo& getWorldInfo() + btSoftBodyWorldInfo& getWorldInfo() { return m_sbi; } - const btSoftBodyWorldInfo& getWorldInfo() const + const btSoftBodyWorldInfo& getWorldInfo() const { return m_sbi; } - virtual btDynamicsWorldType getWorldType() const + virtual btDynamicsWorldType getWorldType() const { - return BT_SOFT_RIGID_DYNAMICS_WORLD; + return BT_SOFT_RIGID_DYNAMICS_WORLD; } btSoftBodyArray& getSoftBodyArray() @@ -88,20 +85,18 @@ public: return m_softBodies; } - - virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; + 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); + 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 +#endif //BT_SOFT_RIGID_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp b/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp index 72043e69e2f..9c3e904f645 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp @@ -23,8 +23,8 @@ subject to the following restrictions: #define USE_PERSISTENT_CONTACTS 1 -btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* /*obj0*/,const btCollisionObjectWrapper* /*obj1*/) -: btCollisionAlgorithm(ci) +btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold* /*mf*/, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* /*obj0*/, const btCollisionObjectWrapper* /*obj1*/) + : btCollisionAlgorithm(ci) //m_ownManifold(false), //m_manifoldPtr(mf) { @@ -34,14 +34,14 @@ btSoftSoftCollisionAlgorithm::~btSoftSoftCollisionAlgorithm() { } -void btSoftSoftCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) +void btSoftSoftCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& /*dispatchInfo*/, btManifoldResult* /*resultOut*/) { - btSoftBody* soft0 = (btSoftBody*)body0Wrap->getCollisionObject(); - btSoftBody* soft1 = (btSoftBody*)body1Wrap->getCollisionObject(); + 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*/) +btScalar btSoftSoftCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/, btCollisionObject* /*body1*/, const btDispatcherInfo& /*dispatchInfo*/, btManifoldResult* /*resultOut*/) { //not yet return 1.f; diff --git a/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h b/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h index 43b1439cc53..6f871f5b855 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h @@ -27,43 +27,39 @@ class btSoftBody; ///collision detection between two btSoftBody shapes class btSoftSoftCollisionAlgorithm : public btCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - - btSoftBody* m_softBody0; - btSoftBody* m_softBody1; + 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 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 btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + 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); + btSoftSoftCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); virtual ~btSoftSoftCollisionAlgorithm(); - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + 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); + return new (ptr) btSoftSoftCollisionAlgorithm(0, ci, body0Wrap, body1Wrap); } }; - }; -#endif //BT_SOFT_SOFT_COLLISION_ALGORITHM_H - - +#endif //BT_SOFT_SOFT_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h index 8992ddbb68d..d611726bcdc 100644 --- a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h +++ b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h @@ -20,299 +20,353 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" -// Modified Paul Hsieh hash -template <const int DWORDLEN> -unsigned int HsiehHash(const void* pdata) +// Fast Hash + +#if !defined(get16bits) +#define get16bits(d) ((((unsigned int)(((const unsigned char*)(d))[1])) << 8) + (unsigned int)(((const unsigned char*)(d))[0])) +#endif +// +// super hash function by Paul Hsieh +// +inline unsigned int HsiehHash(const char* data, int len) { - const unsigned short* data=(const unsigned short*)pdata; - unsigned hash=DWORDLEN<<2,tmp; - for(int i=0;i<DWORDLEN;++i) + unsigned int hash = len, tmp; + len >>= 2; + + /* Main loop */ + for (; len > 0; len--) { - hash += data[0]; - tmp = (data[1]<<11)^hash; - hash = (hash<<16)^tmp; - data += 2; - hash += hash>>11; + hash += get16bits(data); + tmp = (get16bits(data + 2) << 11) ^ hash; + hash = (hash << 16) ^ tmp; + data += 2 * sizeof(unsigned short); + hash += hash >> 11; } - hash^=hash<<3;hash+=hash>>5; - hash^=hash<<4;hash+=hash>>17; - hash^=hash<<25;hash+=hash>>6; - return(hash); + + /* Force "avalanching" of final 127 bits */ + 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 +struct btSparseSdf { // // Inner types // struct IntFrac { - int b; - int i; - btScalar f; + int b; + int i; + btScalar f; }; - struct Cell + struct Cell { - btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1]; - int c[3]; - int puid; - unsigned hash; - const btCollisionShape* pclient; - Cell* next; + 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; + btAlignedObjectArray<Cell*> cells; + btScalar voxelsz; + btScalar m_defaultVoxelsz; + int puid; + int ncells; + int m_clampCells; + int nprobes; + int nqueries; + ~btSparseSdf() + { + Reset(); + } // // Methods // // - void Initialize(int hashsize=2383, int clampCells = 256*1024) + 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); + cells.resize(hashsize, 0); + m_defaultVoxelsz = 0.25; Reset(); } // - void Reset() + + void setDefaultVoxelsz(btScalar sz) + { + m_defaultVoxelsz = sz; + } + + void Reset() { - for(int i=0,ni=cells.size();i<ni;++i) + for (int i = 0, ni = cells.size(); i < ni; ++i) { - Cell* pc=cells[i]; - cells[i]=0; - while(pc) + Cell* pc = cells[i]; + cells[i] = 0; + while (pc) { - Cell* pn=pc->next; + Cell* pn = pc->next; delete pc; - pc=pn; + pc = pn; } } - voxelsz =0.25; - puid =0; - ncells =0; - nprobes =1; - nqueries =1; + voxelsz = m_defaultVoxelsz; + puid = 0; + ncells = 0; + nprobes = 1; + nqueries = 1; } // - void GarbageCollect(int lifetime=256) + void GarbageCollect(int lifetime = 256) { - const int life=puid-lifetime; - for(int i=0;i<cells.size();++i) + 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*& root = cells[i]; + Cell* pp = 0; + Cell* pc = root; + while (pc) { - Cell* pn=pc->next; - if(pc->puid<life) + Cell* pn = pc->next; + if (pc->puid < life) { - if(pp) pp->next=pn; else root=pn; - delete pc;pc=pp;--ncells; + if (pp) + pp->next = pn; + else + root = pn; + delete pc; + pc = pp; + --ncells; } - pp=pc;pc=pn; + 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... */ + 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 RemoveReferences(btCollisionShape* pcs) { - int refcount=0; - for(int i=0;i<cells.size();++i) + int refcount = 0; + for (int i = 0; i < cells.size(); ++i) { - Cell*& root=cells[i]; - Cell* pp=0; - Cell* pc=root; - while(pc) + Cell*& root = cells[i]; + Cell* pp = 0; + Cell* pc = root; + while (pc) { - Cell* pn=pc->next; - if(pc->pclient==pcs) + Cell* pn = pc->next; + if (pc->pclient == pcs) { - if(pp) pp->next=pn; else root=pn; - delete pc;pc=pp;++refcount; + if (pp) + pp->next = pn; + else + root = pn; + delete pc; + pc = pp; + ++refcount; } - pp=pc;pc=pn; + pp = pc; + pc = pn; } } - return(refcount); + return (refcount); } // - btScalar Evaluate( const btVector3& x, - const btCollisionShape* shape, - btVector3& normal, - btScalar margin) + 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; + /* 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) + 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; } + 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; } + { + // printf("c->hash/c[0][1][2]=%d,%d,%d,%d\n", c->hash, c->c[0], c->c[1],c->c[2]); + //printf("h,ixb,iyb,izb=%d,%d,%d,%d\n", h,ix.b, iy.b, iz.b); + + c = c->next; + } } - if(!c) + if (!c) { - ++nprobes; + ++nprobes; ++ncells; - if (ncells>m_clampCells) + //int sz = sizeof(Cell); + if (ncells > m_clampCells) { - static int numResets=0; + static int numResets = 0; numResets++; -// printf("numResets=%d\n",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; + 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 */ + 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(); + 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.safeNormalize(); #else - normal = btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized(); + 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); + /* 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) + 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 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 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 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); + 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) + static inline btScalar DistanceToShape(const btVector3& x, + const btCollisionShape* shape) { - btTransform unit; + btTransform unit; unit.setIdentity(); - if(shape->isConvex()) + if (shape->isConvex()) { - btGjkEpaSolver2::sResults res; - const btConvexShape* csh=static_cast<const btConvexShape*>(shape); - return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res)); + btGjkEpaSolver2::sResults res; + const btConvexShape* csh = static_cast<const btConvexShape*>(shape); + return (btGjkEpaSolver2::SignedDistance(x, 0, csh, unit, res)); } - return(0); + return (0); } // - static inline IntFrac Decompose(btScalar x) + 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); + /* 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) + static inline btScalar Lerp(btScalar a, btScalar b, btScalar t) { - return(a+(b-a)*t); + return (a + (b - a) * t); } - - // - static inline unsigned int Hash(int x,int y,int z,const btCollisionShape* shape) + static inline unsigned int Hash(int x, int y, int z, const btCollisionShape* shape) { struct btS - { - int x,y,z; + { + int x, y, z, w; void* p; }; btS myset; + //memset may be needed in case of additional (uninitialized) padding! + //memset(&myset, 0, sizeof(btS)); - 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); + myset.x = x; + myset.y = y; + myset.z = z; + myset.w = 0; + myset.p = (void*)shape; + const char* ptr = (const char*)&myset; + unsigned int result = HsiehHash(ptr, sizeof(btS)); return result; } }; - -#endif //BT_SPARSE_SDF_H +#endif //BT_SPARSE_SDF_H diff --git a/extern/bullet2/src/BulletSoftBody/poly34.cpp b/extern/bullet2/src/BulletSoftBody/poly34.cpp new file mode 100644 index 00000000000..ec7549c8e8d --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/poly34.cpp @@ -0,0 +1,447 @@ +// poly34.cpp : solution of cubic and quartic equation +// (c) Khashin S.I. http://math.ivanovo.ac.ru/dalgebra/Khashin/index.html +// khash2 (at) gmail.com +// Thanks to Alexandr Rakhmanin <rakhmanin (at) gmail.com> +// public domain +// +#include <math.h> + +#include "poly34.h" // solution of cubic and quartic equation +#define TwoPi 6.28318530717958648 +const btScalar eps = SIMD_EPSILON; + +//============================================================================= +// _root3, root3 from http://prografix.narod.ru +//============================================================================= +static SIMD_FORCE_INLINE btScalar _root3(btScalar x) +{ + btScalar s = 1.; + while (x < 1.) + { + x *= 8.; + s *= 0.5; + } + while (x > 8.) + { + x *= 0.125; + s *= 2.; + } + btScalar r = 1.5; + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + return r * s; +} + +btScalar SIMD_FORCE_INLINE root3(btScalar x) +{ + if (x > 0) + return _root3(x); + else if (x < 0) + return -_root3(-x); + else + return 0.; +} + +// x - array of size 2 +// return 2: 2 real roots x[0], x[1] +// return 0: pair of complex roots: x[0]i*x[1] +int SolveP2(btScalar* x, btScalar a, btScalar b) +{ // solve equation x^2 + a*x + b = 0 + btScalar D = 0.25 * a * a - b; + if (D >= 0) + { + D = sqrt(D); + x[0] = -0.5 * a + D; + x[1] = -0.5 * a - D; + return 2; + } + x[0] = -0.5 * a; + x[1] = sqrt(-D); + return 0; +} +//--------------------------------------------------------------------------- +// x - array of size 3 +// In case 3 real roots: => x[0], x[1], x[2], return 3 +// 2 real roots: x[0], x[1], return 2 +// 1 real root : x[0], x[1] i*x[2], return 1 +int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c) +{ // solve cubic equation x^3 + a*x^2 + b*x + c = 0 + btScalar a2 = a * a; + btScalar q = (a2 - 3 * b) / 9; + if (q < 0) + q = eps; + btScalar r = (a * (2 * a2 - 9 * b) + 27 * c) / 54; + // equation x^3 + q*x + r = 0 + btScalar r2 = r * r; + btScalar q3 = q * q * q; + btScalar A, B; + if (r2 <= (q3 + eps)) + { //<<-- FIXED! + btScalar t = r / sqrt(q3); + if (t < -1) + t = -1; + if (t > 1) + t = 1; + t = acos(t); + a /= 3; + q = -2 * sqrt(q); + x[0] = q * cos(t / 3) - a; + x[1] = q * cos((t + TwoPi) / 3) - a; + x[2] = q * cos((t - TwoPi) / 3) - a; + return (3); + } + else + { + //A =-pow(fabs(r)+sqrt(r2-q3),1./3); + A = -root3(fabs(r) + sqrt(r2 - q3)); + if (r < 0) + A = -A; + B = (A == 0 ? 0 : q / A); + + a /= 3; + x[0] = (A + B) - a; + x[1] = -0.5 * (A + B) - a; + x[2] = 0.5 * sqrt(3.) * (A - B); + if (fabs(x[2]) < eps) + { + x[2] = x[1]; + return (2); + } + return (1); + } +} // SolveP3(btScalar *x,btScalar a,btScalar b,btScalar c) { +//--------------------------------------------------------------------------- +// a>=0! +void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b) // returns: a+i*s = sqrt(x+i*y) +{ + btScalar r = sqrt(x * x + y * y); + if (y == 0) + { + r = sqrt(r); + if (x >= 0) + { + a = r; + b = 0; + } + else + { + a = 0; + b = r; + } + } + else + { // y != 0 + a = sqrt(0.5 * (x + r)); + b = 0.5 * y / a; + } +} +//--------------------------------------------------------------------------- +int SolveP4Bi(btScalar* x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 + d = 0 +{ + btScalar D = b * b - 4 * d; + if (D >= 0) + { + btScalar sD = sqrt(D); + btScalar x1 = (-b + sD) / 2; + btScalar x2 = (-b - sD) / 2; // x2 <= x1 + if (x2 >= 0) // 0 <= x2 <= x1, 4 real roots + { + btScalar sx1 = sqrt(x1); + btScalar sx2 = sqrt(x2); + x[0] = -sx1; + x[1] = sx1; + x[2] = -sx2; + x[3] = sx2; + return 4; + } + if (x1 < 0) // x2 <= x1 < 0, two pair of imaginary roots + { + btScalar sx1 = sqrt(-x1); + btScalar sx2 = sqrt(-x2); + x[0] = 0; + x[1] = sx1; + x[2] = 0; + x[3] = sx2; + return 0; + } + // now x2 < 0 <= x1 , two real roots and one pair of imginary root + btScalar sx1 = sqrt(x1); + btScalar sx2 = sqrt(-x2); + x[0] = -sx1; + x[1] = sx1; + x[2] = 0; + x[3] = sx2; + return 2; + } + else + { // if( D < 0 ), two pair of compex roots + btScalar sD2 = 0.5 * sqrt(-D); + CSqrt(-0.5 * b, sD2, x[0], x[1]); + CSqrt(-0.5 * b, -sD2, x[2], x[3]); + return 0; + } // if( D>=0 ) +} // SolveP4Bi(btScalar *x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 d +//--------------------------------------------------------------------------- +#define SWAP(a, b) \ + { \ + t = b; \ + b = a; \ + a = t; \ + } +static void dblSort3(btScalar& a, btScalar& b, btScalar& c) // make: a <= b <= c +{ + btScalar t; + if (a > b) + SWAP(a, b); // now a<=b + if (c < b) + { + SWAP(b, c); // now a<=b, b<=c + if (a > b) + SWAP(a, b); // now a<=b + } +} +//--------------------------------------------------------------------------- +int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d +{ + //if( c==0 ) return SolveP4Bi(x,b,d); // After that, c!=0 + if (fabs(c) < 1e-14 * (fabs(b) + fabs(d))) + return SolveP4Bi(x, b, d); // After that, c!=0 + + int res3 = SolveP3(x, 2 * b, b * b - 4 * d, -c * c); // solve resolvent + // by Viet theorem: x1*x2*x3=-c*c not equals to 0, so x1!=0, x2!=0, x3!=0 + if (res3 > 1) // 3 real roots, + { + dblSort3(x[0], x[1], x[2]); // sort roots to x[0] <= x[1] <= x[2] + // Note: x[0]*x[1]*x[2]= c*c > 0 + if (x[0] > 0) // all roots are positive + { + btScalar sz1 = sqrt(x[0]); + btScalar sz2 = sqrt(x[1]); + btScalar sz3 = sqrt(x[2]); + // Note: sz1*sz2*sz3= -c (and not equal to 0) + if (c > 0) + { + x[0] = (-sz1 - sz2 - sz3) / 2; + x[1] = (-sz1 + sz2 + sz3) / 2; + x[2] = (+sz1 - sz2 + sz3) / 2; + x[3] = (+sz1 + sz2 - sz3) / 2; + return 4; + } + // now: c<0 + x[0] = (-sz1 - sz2 + sz3) / 2; + x[1] = (-sz1 + sz2 - sz3) / 2; + x[2] = (+sz1 - sz2 - sz3) / 2; + x[3] = (+sz1 + sz2 + sz3) / 2; + return 4; + } // if( x[0] > 0) // all roots are positive + // now x[0] <= x[1] < 0, x[2] > 0 + // two pair of comlex roots + btScalar sz1 = sqrt(-x[0]); + btScalar sz2 = sqrt(-x[1]); + btScalar sz3 = sqrt(x[2]); + + if (c > 0) // sign = -1 + { + x[0] = -sz3 / 2; + x[1] = (sz1 - sz2) / 2; // x[0]i*x[1] + x[2] = sz3 / 2; + x[3] = (-sz1 - sz2) / 2; // x[2]i*x[3] + return 0; + } + // now: c<0 , sign = +1 + x[0] = sz3 / 2; + x[1] = (-sz1 + sz2) / 2; + x[2] = -sz3 / 2; + x[3] = (sz1 + sz2) / 2; + return 0; + } // if( res3>1 ) // 3 real roots, + // now resoventa have 1 real and pair of compex roots + // x[0] - real root, and x[0]>0, + // x[1]i*x[2] - complex roots, + // x[0] must be >=0. But one times x[0]=~ 1e-17, so: + if (x[0] < 0) + x[0] = 0; + btScalar sz1 = sqrt(x[0]); + btScalar szr, szi; + CSqrt(x[1], x[2], szr, szi); // (szr+i*szi)^2 = x[1]+i*x[2] + if (c > 0) // sign = -1 + { + x[0] = -sz1 / 2 - szr; // 1st real root + x[1] = -sz1 / 2 + szr; // 2nd real root + x[2] = sz1 / 2; + x[3] = szi; + return 2; + } + // now: c<0 , sign = +1 + x[0] = sz1 / 2 - szr; // 1st real root + x[1] = sz1 / 2 + szr; // 2nd real root + x[2] = -sz1 / 2; + x[3] = szi; + return 2; +} // SolveP4De(btScalar *x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d +//----------------------------------------------------------------------------- +btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d) // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d +{ + btScalar fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c; // f'(x) + if (fxs == 0) + return x; //return 1e99; <<-- FIXED! + btScalar fx = (((x + a) * x + b) * x + c) * x + d; // f(x) + return x - fx / fxs; +} +//----------------------------------------------------------------------------- +// x - array of size 4 +// return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots +// return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3], +// return 0: two pair of complex roots: x[0]i*x[1], x[2]i*x[3], +int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d) +{ // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method + // move to a=0: + btScalar d1 = d + 0.25 * a * (0.25 * b * a - 3. / 64 * a * a * a - c); + btScalar c1 = c + 0.5 * a * (0.25 * a * a - b); + btScalar b1 = b - 0.375 * a * a; + int res = SolveP4De(x, b1, c1, d1); + if (res == 4) + { + x[0] -= a / 4; + x[1] -= a / 4; + x[2] -= a / 4; + x[3] -= a / 4; + } + else if (res == 2) + { + x[0] -= a / 4; + x[1] -= a / 4; + x[2] -= a / 4; + } + else + { + x[0] -= a / 4; + x[2] -= a / 4; + } + // one Newton step for each real root: + if (res > 0) + { + x[0] = N4Step(x[0], a, b, c, d); + x[1] = N4Step(x[1], a, b, c, d); + } + if (res > 2) + { + x[2] = N4Step(x[2], a, b, c, d); + x[3] = N4Step(x[3], a, b, c, d); + } + return res; +} +//----------------------------------------------------------------------------- +#define F5(t) (((((t + a) * t + b) * t + c) * t + d) * t + e) +//----------------------------------------------------------------------------- +btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +{ + int cnt; + if (fabs(e) < eps) + return 0; + + btScalar brd = fabs(a); // brd - border of real roots + if (fabs(b) > brd) + brd = fabs(b); + if (fabs(c) > brd) + brd = fabs(c); + if (fabs(d) > brd) + brd = fabs(d); + if (fabs(e) > brd) + brd = fabs(e); + brd++; // brd - border of real roots + + btScalar x0, f0; // less than root + btScalar x1, f1; // greater than root + btScalar x2, f2, f2s; // next values, f(x2), f'(x2) + btScalar dx = 0; + + if (e < 0) + { + x0 = 0; + x1 = brd; + f0 = e; + f1 = F5(x1); + x2 = 0.01 * brd; + } // positive root + else + { + x0 = -brd; + x1 = 0; + f0 = F5(x0); + f1 = e; + x2 = -0.01 * brd; + } // negative root + + if (fabs(f0) < eps) + return x0; + if (fabs(f1) < eps) + return x1; + + // now x0<x1, f(x0)<0, f(x1)>0 + // Firstly 10 bisections + for (cnt = 0; cnt < 10; cnt++) + { + x2 = (x0 + x1) / 2; // next point + //x2 = x0 - f0*(x1 - x0) / (f1 - f0); // next point + f2 = F5(x2); // f(x2) + if (fabs(f2) < eps) + return x2; + if (f2 > 0) + { + x1 = x2; + f1 = f2; + } + else + { + x0 = x2; + f0 = f2; + } + } + + // At each step: + // x0<x1, f(x0)<0, f(x1)>0. + // x2 - next value + // we hope that x0 < x2 < x1, but not necessarily + do + { + if (cnt++ > 50) + break; + if (x2 <= x0 || x2 >= x1) + x2 = (x0 + x1) / 2; // now x0 < x2 < x1 + f2 = F5(x2); // f(x2) + if (fabs(f2) < eps) + return x2; + if (f2 > 0) + { + x1 = x2; + f1 = f2; + } + else + { + x0 = x2; + f0 = f2; + } + f2s = (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d; // f'(x2) + if (fabs(f2s) < eps) + { + x2 = 1e99; + continue; + } + dx = f2 / f2s; + x2 -= dx; + } while (fabs(dx) > eps); + return x2; +} // SolveP5_1(btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +//----------------------------------------------------------------------------- +int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +{ + btScalar r = x[0] = SolveP5_1(a, b, c, d, e); + btScalar a1 = a + r, b1 = b + r * a1, c1 = c + r * b1, d1 = d + r * c1; + return 1 + SolveP4(x + 1, a1, b1, c1, d1); +} // SolveP5(btScalar *x,btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +//----------------------------------------------------------------------------- diff --git a/extern/bullet2/src/BulletSoftBody/poly34.h b/extern/bullet2/src/BulletSoftBody/poly34.h new file mode 100644 index 00000000000..35a52c5fecf --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/poly34.h @@ -0,0 +1,38 @@ +// poly34.h : solution of cubic and quartic equation +// (c) Khashin S.I. http://math.ivanovo.ac.ru/dalgebra/Khashin/index.html +// khash2 (at) gmail.com + +#ifndef POLY_34 +#define POLY_34 +#include "LinearMath/btScalar.h" +// x - array of size 2 +// return 2: 2 real roots x[0], x[1] +// return 0: pair of complex roots: x[0]i*x[1] +int SolveP2(btScalar* x, btScalar a, btScalar b); // solve equation x^2 + a*x + b = 0 + +// x - array of size 3 +// return 3: 3 real roots x[0], x[1], x[2] +// return 1: 1 real root x[0] and pair of complex roots: x[1]i*x[2] +int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c); // solve cubic equation x^3 + a*x^2 + b*x + c = 0 + +// x - array of size 4 +// return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots +// return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3], +// return 0: two pair of complex roots: x[0]i*x[1], x[2]i*x[3], +int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d); // solve equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method + +// x - array of size 5 +// return 5: 5 real roots x[0], x[1], x[2], x[3], x[4], possible multiple roots +// return 3: 3 real roots x[0], x[1], x[2] and complex x[3]i*x[4], +// return 1: 1 real root x[0] and two pair of complex roots: x[1]i*x[2], x[3]i*x[4], +int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 + +//----------------------------------------------------------------------------- +// And some additional functions for internal use. +// Your may remove this definitions from here +int SolveP4Bi(btScalar* x, btScalar b, btScalar d); // solve equation x^4 + b*x^2 + d = 0 +int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d); // solve equation x^4 + b*x^2 + c*x + d = 0 +void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b); // returns as a+i*s, sqrt(x+i*y) +btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d); // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d +btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +#endif |