From 3bc701c487bf558ec68c48f308848a80df3d431f Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 6 Feb 2020 16:58:35 -0800 Subject: [PATCH 1/6] Improve intra-deformable collision and deformable self collision --- examples/DeformableDemo/ClothFriction.cpp | 8 +- examples/DeformableDemo/DeformableContact.cpp | 2 + .../DeformableSelfCollision.cpp | 103 ++- examples/DeformableDemo/PinchFriction.cpp | 9 +- src/BulletSoftBody/CMakeLists.txt | 4 +- src/BulletSoftBody/btDeformableBodySolver.cpp | 32 +- .../btDeformableContactConstraint.cpp | 11 - .../btDeformableContactConstraint.h | 6 +- .../btDeformableContactProjection.cpp | 12 +- .../btDeformableMultiBodyDynamicsWorld.cpp | 87 +- .../btDeformableMultiBodyDynamicsWorld.h | 7 +- src/BulletSoftBody/btSoftBody.cpp | 339 ++++++-- src/BulletSoftBody/btSoftBody.h | 180 +++++ src/BulletSoftBody/btSoftBodyInternals.h | 763 ++++++++++++++++-- .../btSoftRigidCollisionAlgorithm.cpp | 3 +- src/BulletSoftBody/poly34.cpp | 419 ++++++++++ src/BulletSoftBody/poly34.h | 38 + 17 files changed, 1788 insertions(+), 235 deletions(-) create mode 100644 src/BulletSoftBody/poly34.cpp create mode 100644 src/BulletSoftBody/poly34.h diff --git a/examples/DeformableDemo/ClothFriction.cpp b/examples/DeformableDemo/ClothFriction.cpp index 192769db4..5ea2d8701 100644 --- a/examples/DeformableDemo/ClothFriction.cpp +++ b/examples/DeformableDemo/ClothFriction.cpp @@ -152,9 +152,10 @@ void ClothFriction::initPhysics() 10,10, 0, true); - psb->getCollisionShape()->setMargin(0.05); + psb->getCollisionShape()->setMargin(0.005); psb->generateBendingConstraints(2); psb->setTotalMass(1); + psb->setSpringStiffness(10); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 3; @@ -171,7 +172,7 @@ void ClothFriction::initPhysics() m_forces.push_back(gravity_force); - h = 2; + h = .5; s = 2; btSoftBody* psb2 = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), btVector3(+s, h, -s), @@ -179,9 +180,10 @@ void ClothFriction::initPhysics() btVector3(+s, h, +s), 5,5, 0, true); - psb2->getCollisionShape()->setMargin(0.05); + psb2->getCollisionShape()->setMargin(0.005); psb2->generateBendingConstraints(2); psb2->setTotalMass(1); + psb2->setSpringStiffness(10); psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb2->m_cfg.kCHR = 1; // collision hardness with rigid body psb2->m_cfg.kDF = 20; diff --git a/examples/DeformableDemo/DeformableContact.cpp b/examples/DeformableDemo/DeformableContact.cpp index 8523e0378..287d150df 100644 --- a/examples/DeformableDemo/DeformableContact.cpp +++ b/examples/DeformableDemo/DeformableContact.cpp @@ -156,6 +156,7 @@ void DeformableContact::initPhysics() psb->getCollisionShape()->setMargin(0.1); psb->generateBendingConstraints(2); + psb->setSpringStiffness(10); psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body @@ -183,6 +184,7 @@ void DeformableContact::initPhysics() 0, true); psb2->getCollisionShape()->setMargin(0.1); psb2->generateBendingConstraints(2); + psb2->setSpringStiffness(10); psb2->setTotalMass(1); psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb2->m_cfg.kCHR = 1; // collision hardness with rigid body diff --git a/examples/DeformableDemo/DeformableSelfCollision.cpp b/examples/DeformableDemo/DeformableSelfCollision.cpp index 20caa45d5..44da9d542 100644 --- a/examples/DeformableDemo/DeformableSelfCollision.cpp +++ b/examples/DeformableDemo/DeformableSelfCollision.cpp @@ -42,19 +42,21 @@ public: void resetCamera() { - float dist = 10; + float dist = 1.0; float pitch = -8; float yaw = 100; - float targetPos[3] = {0, -10, 0}; + float targetPos[3] = {0, -1.0, 0}; m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } void stepSimulation(float deltaTime) { - float internalTimeStep = 1. / 240.f; - m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + float internalTimeStep = 1. / 480.f; + m_dynamicsWorld->stepSimulation(deltaTime, 8, internalTimeStep); } + void addCloth(btVector3 origin); + virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const { ///just make it a btSoftRigidDynamicsWorld please @@ -72,23 +74,12 @@ public: virtual void renderScene() { CommonRigidBodyBase::renderScene(); - btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); - - for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) - { - btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; - { - btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); - btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), fDrawFlags::Faces);// deformableWorld->getDrawFlags()); - } - } } }; void DeformableSelfCollision::initPhysics() { m_guiHelper->setUpAxis(1); - ///collision configuration contains default setup for memory, collision setup m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); @@ -106,22 +97,23 @@ void DeformableSelfCollision::initPhysics() m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); // deformableBodySolver->setWorld(getDeformableDynamicsWorld()); // m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality - btVector3 gravity = btVector3(0, -100, 0); + btVector3 gravity = btVector3(0, -9.8, 0); m_dynamicsWorld->setGravity(gravity); getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25); // getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); { ///create a ground - btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.))); + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(2.5), btScalar(150.))); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0, -35, 0)); + groundTransform.setOrigin(btVector3(0, -3.5, 0)); groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: btScalar mass(0.); @@ -142,44 +134,51 @@ void DeformableSelfCollision::initPhysics() //add the ground to the dynamics world m_dynamicsWorld->addRigidBody(body); } - - // create a piece of cloth - { - const btScalar s = 2; - const btScalar h = 0; - - btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -4*s), - btVector3(+s, h, -4*s), - btVector3(-s, h, +4*s), - btVector3(+s, h, +4*s), - 10,40, - 0, true, 0.01); - - - psb->getCollisionShape()->setMargin(0.2); - psb->generateBendingConstraints(2); - psb->setTotalMass(1); - psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects - psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 0.2; - psb->rotate(btQuaternion(0,SIMD_PI / 2, 0)); - psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; - psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; - getDeformableDynamicsWorld()->addSoftBody(psb); - psb->setSelfCollision(true); - - btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(10,0.2, true); - getDeformableDynamicsWorld()->addForce(psb, mass_spring); - m_forces.push_back(mass_spring); - - btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); - getDeformableDynamicsWorld()->addForce(psb, gravity_force); - m_forces.push_back(gravity_force); - } + addCloth(btVector3(0, -0.4, 0)); getDeformableDynamicsWorld()->setImplicit(false); getDeformableDynamicsWorld()->setLineSearch(false); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); } +void DeformableSelfCollision::addCloth(btVector3 origin) +// create a piece of cloth +{ + const btScalar s = 0.3; + const btScalar h = 0; + + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -2*s), + btVector3(+s, h, -2*s), + btVector3(-s, h, +2*s), + btVector3(+s, h, +2*s), + 20,40, +// 4,4, + 0, true, 0.0); + + + psb->getCollisionShape()->setMargin(0.0075); + psb->generateBendingConstraints(2); + psb->setTotalMass(.5); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = 0.1; + psb->rotate(btQuaternion(0, SIMD_PI / 2, 0)); + btTransform clothTransform; + clothTransform.setIdentity(); + clothTransform.setOrigin(btVector3(0,0.2,0)); + psb->transform(clothTransform); + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + getDeformableDynamicsWorld()->addSoftBody(psb); + psb->setSelfCollision(true); + + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(3,0.2, true); + psb->setSpringStiffness(3); + getDeformableDynamicsWorld()->addForce(psb, mass_spring); + m_forces.push_back(mass_spring); + btVector3 gravity = btVector3(0, -9.8, 0); + btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); + getDeformableDynamicsWorld()->addForce(psb, gravity_force); + m_forces.push_back(gravity_force); +} void DeformableSelfCollision::exitPhysics() { diff --git a/examples/DeformableDemo/PinchFriction.cpp b/examples/DeformableDemo/PinchFriction.cpp index 657c36e46..24db82111 100644 --- a/examples/DeformableDemo/PinchFriction.cpp +++ b/examples/DeformableDemo/PinchFriction.cpp @@ -266,6 +266,7 @@ void PinchFriction::initPhysics() psb->scale(btVector3(2, 2, 1)); psb->translate(btVector3(0, 2.1, 2.2)); psb->getCollisionShape()->setMargin(0.05); + psb->setSpringStiffness(10); psb->setTotalMass(.6); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body @@ -279,7 +280,7 @@ void PinchFriction::initPhysics() getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(4,8,.1); + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(6,6,.003); getDeformableDynamicsWorld()->addForce(psb, neohookean); m_forces.push_back(neohookean); } @@ -299,6 +300,7 @@ void PinchFriction::initPhysics() psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb2->m_cfg.kCHR = 1; // collision hardness with rigid body psb2->m_cfg.kDF = 2; + psb2->setSpringStiffness(10); psb2->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; psb2->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; btSoftBodyHelpers::generateBoundaryFaces(psb2); @@ -308,7 +310,7 @@ void PinchFriction::initPhysics() getDeformableDynamicsWorld()->addForce(psb2, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(4,8,.1); + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(6,6,.003); getDeformableDynamicsWorld()->addForce(psb2, neohookean); m_forces.push_back(neohookean); } @@ -325,6 +327,7 @@ void PinchFriction::initPhysics() psb3->translate(btVector3(0, 2.1, 0)); psb3->getCollisionShape()->setMargin(0.05); psb3->setTotalMass(.6); + psb3->setSpringStiffness(10); psb3->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb3->m_cfg.kCHR = 1; // collision hardness with rigid body psb3->m_cfg.kDF = 2; @@ -337,7 +340,7 @@ void PinchFriction::initPhysics() getDeformableDynamicsWorld()->addForce(psb3, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(4,8,.1); + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(6,6,.003); getDeformableDynamicsWorld()->addForce(psb3, neohookean); m_forces.push_back(neohookean); } diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt index f20fb4c0a..26f75e5da 100644 --- a/src/BulletSoftBody/CMakeLists.txt +++ b/src/BulletSoftBody/CMakeLists.txt @@ -23,9 +23,10 @@ SET(BulletSoftBody_SRCS btDeformableContactProjection.cpp btDeformableMultiBodyDynamicsWorld.cpp btDeformableContactConstraint.cpp - + poly34.cpp ) + SET(BulletSoftBody_HDRS btSoftBody.h btSoftBodyData.h @@ -57,6 +58,7 @@ SET(BulletSoftBody_HDRS btDeformableContactProjection.h btDeformableMultiBodyDynamicsWorld.h btDeformableContactConstraint.h + poly34.h btSoftBodySolverVertexBuffer.h ) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 6404ce6ee..a334dd443 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -226,6 +226,7 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArrayreinitialize(nodeUpdated, dt); + updateSoftBodies(); } void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal) @@ -385,6 +386,7 @@ void btDeformableBodySolver::predictMotion(btScalar solverdt) void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt) { + BT_PROFILE("btDeformableBodySolver::predictDeformableMotion"); int i, ni; /* Update */ @@ -423,40 +425,22 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d n.m_v *= max_v; } n.m_q = n.m_x + n.m_v * dt; + n.m_constrained = false; } /* Nodes */ - ATTRIBUTE_ALIGNED16(btDbvtVolume) - vol; - for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) - { - btSoftBody::Node& n = psb->m_nodes[i]; - btVector3 points[2] = {n.m_x, n.m_q}; - vol = btDbvtVolume::FromPoints(points, 2); - vol.Expand(btVector3(psb->m_sst.radmrg, psb->m_sst.radmrg, psb->m_sst.radmrg)); - psb->m_ndbvt.update(n.m_leaf, vol); - } - + psb->updateNodeTree(true, true); if (!psb->m_fdbvt.empty()) { - for (int i = 0; i < psb->m_faces.size(); ++i) - { - btSoftBody::Face& f = psb->m_faces[i]; - btVector3 points[6] = {f.m_n[0]->m_x, f.m_n[0]->m_q, - f.m_n[1]->m_x, f.m_n[1]->m_q, - f.m_n[2]->m_x, f.m_n[2]->m_q}; - vol = btDbvtVolume::FromPoints(points, 6); - vol.Expand(btVector3(psb->m_sst.radmrg, psb->m_sst.radmrg, psb->m_sst.radmrg)); - psb->m_fdbvt.update(f.m_leaf, vol); - } + psb->updateFaceTree(true, true); } - /* Clear contacts */ + /* 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); +// psb->m_ndbvt.optimizeIncremental(1); +// psb->m_fdbvt.optimizeIncremental(1); } diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index 3a5dfc88d..389411def 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -620,15 +620,4 @@ void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impuls { v2 -= dvb * contact->m_weights[2]; } - // todo: Face node constraints needs more work -// btScalar m01 = (btScalar(1)/(im0 + im1)); -// btScalar m02 = (btScalar(1)/(im0 + im2)); -// btScalar m12 = (btScalar(1)/(im1 + im2)); -// -// 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)); -// v0 += dv0; -// v1 += dv1; -// v2 += dv2; } diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h index 7acf9aa69..cc2d37da8 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.h +++ b/src/BulletSoftBody/btDeformableContactConstraint.h @@ -79,9 +79,9 @@ public: class btDeformableStaticConstraint : public btDeformableContactConstraint { public: - const btSoftBody::Node* m_node; + btSoftBody::Node* m_node; - btDeformableStaticConstraint(const btSoftBody::Node* node, const btContactSolverInfo& infoGlobal): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0), infoGlobal) + btDeformableStaticConstraint(btSoftBody::Node* node, const btContactSolverInfo& infoGlobal): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0), infoGlobal) { } btDeformableStaticConstraint(){} @@ -195,7 +195,7 @@ class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactCo { public: // the deformable node in contact - const btSoftBody::Node* m_node; + btSoftBody::Node* m_node; btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal); btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other); diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index a59177e16..350ad0c73 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -122,6 +122,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in // set Dirichlet constraint for (int j = 0; j < psb->m_nodes.size(); ++j) { + psb->m_nodes[i].m_constrained = false; // reset the m_constrained to false if (psb->m_nodes[j].m_im == 0) { btDeformableStaticConstraint static_constraint(&psb->m_nodes[j], infoGlobal); @@ -184,7 +185,8 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in m_faceRigidConstraints[i].push_back(constraint); } } - +// skip deformable constraints as they are done separately now +#if 0 // set Deformable Face vs. Deformable Node constraint for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j) { @@ -200,6 +202,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in m_deformableConstraints[i].push_back(constraint); } } +#endif } } @@ -243,6 +246,7 @@ void btDeformableContactProjection::project(TVStack& x) void btDeformableContactProjection::setProjection() { + BT_PROFILE("btDeformableContactProjection::setProjection"); btAlignedObjectArray units; units.push_back(btVector3(1,0,0)); units.push_back(btVector3(0,1,0)); @@ -257,6 +261,7 @@ void btDeformableContactProjection::setProjection() 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); @@ -273,6 +278,7 @@ void btDeformableContactProjection::setProjection() 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); @@ -289,6 +295,7 @@ void btDeformableContactProjection::setProjection() 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_static) { if (m_projectionsDict.find(index) == NULL) @@ -324,7 +331,8 @@ void btDeformableContactProjection::setProjection() const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; for (int k = 0; k < 3; ++k) { - const btSoftBody::Node* node = face->m_n[k]; + btSoftBody::Node* node = face->m_n[k]; + node->m_constrained = true; int index = node->index; if (m_faceRigidConstraints[i][j].m_static) { diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index f8de4b11e..aabe87f9f 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -58,11 +58,12 @@ m_deformableBodySolver(deformableBodySolver), m_solverCallback(0) 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_gravity.setValue(0, -9.8, 0); m_internalTime = 0.0; m_implicit = false; m_lineSearch = false; m_selfCollision = true; + m_ccdIterations = 3; m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher); } @@ -101,7 +102,11 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t solveConstraints(timeStep); afterSolverCallbacks(timeStep); - + + applyRepulsionForce(timeStep); + + performGeometricCollisions(timeStep); + integrateTransforms(timeStep); ///update vehicle simulation @@ -136,10 +141,85 @@ void btDeformableMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep 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) + { + 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) + { + // clear contact points in the previous iteration + m_softBodies[i]->m_faceNodeContacts.clear(); + + // update m_q and normals for CCD calculation + for (int j = 0; j < m_softBodies[i]->m_nodes.size(); ++j) + { + m_softBodies[i]->m_nodes[j].m_q = m_softBodies[i]->m_nodes[j].m_x + timeStep * m_softBodies[i]->m_nodes[j].m_v; + } + for (int j = 0; j < m_softBodies[i]->m_faces.size(); ++j) + { + btSoftBody::Face& f = m_softBodies[i]->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) + { + m_softBodies[i]->geometricCollisionHandler(m_softBodies[j]); + } + } + + int penetration_count = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + penetration_count += m_softBodies[i]->m_faceNodeContacts.size(); + } + if (penetration_count == 0) + { + return; + } + + // apply inelastic impulse + for (int i = 0; i < m_softBodies.size(); ++i) + { + m_softBodies[i]->applyRepulsionForce(timeStep, false); + } + } +} void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision() { - m_deformableBodySolver->updateSoftBodies(); + BT_PROFILE("btDeformableMultiBodyDynamicsWorld::softBodySelfCollision"); for (int i = 0; i < m_softBodies.size(); i++) { btSoftBody* psb = m_softBodies[i]; @@ -260,6 +340,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) { + BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints"); // save v_{n+1}^* velocity after explicit forces m_deformableBodySolver->backupVelocity(); diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index a1ed4d1d9..e55b02d01 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -46,7 +46,7 @@ class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld bool m_drawClusterTree; btSoftBodyWorldInfo m_sbi; btScalar m_internalTime; - int m_contact_iterations; + int m_ccdIterations; bool m_implicit; bool m_lineSearch; bool m_selfCollision; @@ -157,7 +157,10 @@ public: { m_lineSearch = lineSearch; } - + + void applyRepulsionForce(btScalar timeStep); + + void performGeometricCollisions(btScalar timeStep); }; #endif //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 634778d00..20dc489f0 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -24,6 +24,93 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" #include "BulletCollision/CollisionShapes/btTriangleShape.h" #include +// +static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray& leafNodes, btAlignedObjectArray >& adj) +{ + int N = leafNodes.size(); + while (N > 1) + { + btAlignedObjectArray marked; + btAlignedObjectArray newLeafNodes; + btAlignedObjectArray > childIds; + btAlignedObjectArray > 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& 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& 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; + adj = newAdj; + 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) @@ -54,6 +141,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV } updateBounds(); setCollisionQuadrature(3); + m_fdbvnt = 0; } btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo) @@ -135,6 +223,8 @@ btSoftBody::~btSoftBody() btAlignedFree(m_materials[i]); for (i = 0; i < m_joints.size(); ++i) btAlignedFree(m_joints[i]); + if (m_fdbvnt) + delete m_fdbvnt; } // @@ -2341,14 +2431,135 @@ int btSoftBody::rayTest(const btVector3& rayFrom, const btVector3& rayTo, } // +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; +} + +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 leafNodes; + leafNodes.resize(m_faces.size()); for (int i = 0; i < m_faces.size(); ++i) { Face& f = m_faces[i]; - f.m_leaf = m_fdbvt.insert(VolumeOf(f, 0), &f); + btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode(); + node->parent = NULL; + node->data = &f; + node->childs[1] = 0; + leafNodes[i] = node; + f.m_leaf = node; } + btAlignedObjectArray > 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) + { + for (int l = 0; l < 3; ++l) + { + 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); + } + } + } + } + m_fdbvt.m_root = buildTreeBottomUp(leafNodes, adj); + if (m_fdbvnt) + delete m_fdbvnt; + m_fdbvnt = copyToDbvnt(m_fdbvt.m_root); + rebuildNodeTree(); +} + +// +void btSoftBody::rebuildNodeTree() +{ + m_ndbvt.clear(); + btAlignedObjectArray leafNodes; + leafNodes.resize(m_nodes.size()); + for (int i = 0; i < m_nodes.size(); ++i) + { + Node& n = m_nodes[i]; + btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode(); + node->parent = NULL; + node->data = &n; + node->childs[1] = 0; + leafNodes[i] = node; + n.m_leaf = node; + } + btAlignedObjectArray > adj; + adj.resize(m_nodes.size()); + btAlignedObjectArray 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) + { + 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]; } // @@ -2459,7 +2670,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO : colObjWrap->getWorldTransform(); btScalar dst; -#define USE_QUADRATURE 1 +//#define USE_QUADRATURE 1 //#define CACHE_PREV_COLLISION // use the contact position of the previous collision @@ -3092,6 +3303,7 @@ void btSoftBody::setSpringStiffness(btScalar k) { m_links[i].Feature::m_material->m_kLST = k; } + repulsionStiffness = k; } void btSoftBody::initializeDmInverse() @@ -3708,51 +3920,6 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap } } -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; -} - -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::defaultCollisionHandler(btSoftBody* psb) { @@ -3814,6 +3981,7 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* 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; @@ -3829,20 +3997,17 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) { if (psb->useSelfCollision()) { - btSoftColliders::CollideFF_DD docollide; - docollide.mrg = getCollisionShape()->getMargin() + - psb->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 */ - btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root); - calculateNormalCone(root); - this->m_fdbvt.selfCollideT(root,docollide); - delete root; + btSoftColliders::CollideFF_DD docollide; + docollide.mrg = getCollisionShape()->getMargin()*2.0; + 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); } } } @@ -3854,6 +4019,58 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) } } +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 = 1e-6; + 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) { m_windVelocity = velocity; diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 2b048c111..6a1bfc473 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -35,6 +35,8 @@ subject to the following restrictions: //#else #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; @@ -264,6 +266,7 @@ public: btScalar m_im; // 1/mass btScalar m_area; // Area btDbvtNode* m_leaf; // Leaf data + bool m_constrained; // constrained node int m_battach : 1; // Attached int index; }; @@ -289,6 +292,7 @@ public: 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 */ @@ -796,6 +800,7 @@ public: 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 @@ -803,6 +808,7 @@ public: btScalar m_maxSpeedSquared; bool m_useFaceContact; btAlignedObjectArray m_quads; // quadrature points for collision detection + btScalar repulsionStiffness; btAlignedObjectArray m_renderNodesInterpolationWeights; btAlignedObjectArray > m_renderNodesParents; @@ -1120,6 +1126,7 @@ public: 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; @@ -1152,7 +1159,180 @@ public: 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*10.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; + } + } + + void updateNodeTree(bool use_velocity, bool margin) + { + if (m_ndbvt.m_root) + updateNode(m_ndbvt.m_root, use_velocity, margin); + } + template // 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 + 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 indices; + { + // randomize the order of repulsive force + indices.resize(m_faceNodeContacts.size()); + for (int i = 0; i < m_faceNodeContacts.size(); ++i) + indices[i] = i; +// static unsigned long seed = 243703; +#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; + if (applySpringForce) + I = -btMin(repulsionStiffness * timeStep * d, btScalar(1)/node->m_im * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn)); + if (vn < 0) + I += btScalar(0.5)/node->m_im * vn; + bool face_constrained = false, node_constrained = node->m_constrained; + for (int i = 0; i < 3; ++i) + face_constrained |= 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_constrained || node_constrained) + I_tilde *= 2.0; + if (!face_constrained) + { + for (int j = 0; j < 3; ++j) + face->m_n[j]->m_v += w[j]*n*I_tilde*node->m_im; + } + if (!node_constrained) + { + 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 = btScalar(0.5)/node->m_im * (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_constrained || node_constrained) + I_tilde *= 2.0; + if (!face_constrained) + { + for (int j = 0; j < 3; ++j) + face->m_n[j]->m_v += w[j]*vt*I_tilde*node->m_im; + } + if (!node_constrained) + { + 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) diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 6ef0ee367..7faa93dde 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -18,7 +18,6 @@ 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" @@ -29,9 +28,10 @@ subject to the following restrictions: #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" #include //for memset #include +#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 void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, +static SIMD_FORCE_INLINE void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, btMultiBodyJacobianData& jacobianData, const btVector3& contact_point, const btVector3& dir) @@ -44,7 +44,7 @@ static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, 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 btVector3 generateUnitOrthogonalVector(const btVector3& u) +static SIMD_FORCE_INLINE btVector3 generateUnitOrthogonalVector(const btVector3& u) { btScalar ux = u.getX(); btScalar uy = u.getY(); @@ -62,6 +62,559 @@ static btVector3 generateUnitOrthogonalVector(const btVector3& u) 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 && aSAFE_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 <0, btScalar <1) +{ + 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; + float 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); + 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 // @@ -1079,6 +1632,7 @@ struct btSoftColliders const btScalar ms = ima + imb; if (ms > 0) { + n.m_constrained = true; // resolve contact at x_n psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false); btSoftBody::sCti& cti = c.m_cti; @@ -1159,6 +1713,8 @@ struct btSoftColliders btSoftBody::Node* n0 = f.m_n[0]; btSoftBody::Node* n1 = f.m_n[1]; btSoftBody::Node* n2 = f.m_n[2]; + if (n0->m_constrained && n1->m_constrained && n2->m_constrained) + return; const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0 )? dynmargin : stamargin; btSoftBody::DeformableFaceRigidContact c; btVector3 contact_point; @@ -1317,19 +1873,11 @@ struct btSoftColliders { 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).safeNorm() * 2; - if (d < (m * m)) + 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 = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o); + 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) || @@ -1342,20 +1890,14 @@ struct btSoftColliders if (ms > 0) { btSoftBody::DeformableFaceNodeContact c; - if (useFaceNormal) - c.m_normal = face->m_normal; - else - c.m_normal = p / -btSqrt(d); + 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; - // 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) + w.length2()) * w; c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; - // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf - c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im; - c.m_c0 = btScalar(1)/(ma + c.m_imf); psb[0]->m_faceNodeContacts.push_back(c); } } @@ -1373,69 +1915,152 @@ struct btSoftColliders void Process(const btDbvntNode* lface1, const btDbvntNode* lface2) { - btSoftBody::Face* f = (btSoftBody::Face*)lface1->data; - btSoftBody::Face* face = (btSoftBody::Face*)lface2->data; + 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 = f->m_n[node_id]; - bool skip = false; + btSoftBody::Node* node = f1->m_n[node_id]; for (int i = 0; i < 3; ++i) { - if (face->m_n[i] == node) + 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) - 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).safeNorm() * 2; - if (d < (m * m)) { - const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; - const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o); - const btScalar ma = node->m_im; - btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w); - if ((n[0]->m_im <= 0) || - (n[1]->m_im <= 0) || - (n[2]->m_im <= 0)) - { - mb = 0; - } - const btScalar ms = ma + mb; - if (ms > 0) - { - btSoftBody::DeformableFaceNodeContact c; - if (useFaceNormal) - c.m_normal = face->m_normal; - else - c.m_normal = p / -btSqrt(d); - c.m_margin = mrg; - c.m_node = node; - c.m_face = face; - c.m_bary = w; - // 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) + w.length2()) * w; - c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; - // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf - c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im; - c.m_c0 = btScalar(1)/(ma + c.m_imf); - psb[0]->m_faceNodeContacts.push_back(c); - } + 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/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp b/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp index 56d8083f2..5b65216e4 100644 --- a/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp +++ b/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp @@ -48,9 +48,10 @@ btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm() } #include - +#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"); diff --git a/src/BulletSoftBody/poly34.cpp b/src/BulletSoftBody/poly34.cpp new file mode 100644 index 000000000..6566ee5f1 --- /dev/null +++ b/src/BulletSoftBody/poly34.cpp @@ -0,0 +1,419 @@ +// 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 +// public domain +// +#include + +#include "poly34.h" // solution of cubic and quartic equation +#define TwoPi 6.28318530717958648 +const float eps = 1e-7; + +//============================================================================= +// _root3, root3 from http://prografix.narod.ru +//============================================================================= +static SIMD_FORCE_INLINE float _root3(float x) +{ + float s = 1.; + while (x < 1.) { + x *= 8.; + s *= 0.5; + } + while (x > 8.) { + x *= 0.125; + s *= 2.; + } + float 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; +} + +float SIMD_FORCE_INLINE root3(float 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(float* x, float a, float b) +{ // solve equation x^2 + a*x + b = 0 + float 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(float* x, float a, float b, float c) +{ // solve cubic equation x^3 + a*x^2 + b*x + c = 0 + float a2 = a * a; + float q = (a2 - 3 * b) / 9; + if (q < 0) + q = eps; + float r = (a * (2 * a2 - 9 * b) + 27 * c) / 54; + // equation x^3 + q*x + r = 0 + float r2 = r * r; + float q3 = q * q * q; + float A, B; + if (r2 <= (q3 + eps)) { //<<-- FIXED! + float 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(float *x,float a,float b,float c) { +//--------------------------------------------------------------------------- +// a>=0! +void CSqrt(float x, float y, float& a, float& b) // returns: a+i*s = sqrt(x+i*y) +{ + float 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(float* x, float b, float d) // solve equation x^4 + b*x^2 + d = 0 +{ + float D = b * b - 4 * d; + if (D >= 0) { + float sD = sqrt(D); + float x1 = (-b + sD) / 2; + float x2 = (-b - sD) / 2; // x2 <= x1 + if (x2 >= 0) // 0 <= x2 <= x1, 4 real roots + { + float sx1 = sqrt(x1); + float 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 + { + float sx1 = sqrt(-x1); + float 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 + float sx1 = sqrt(x1); + float 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 + float 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(float *x, float b, float d) // solve equation x^4 + b*x^2 d +//--------------------------------------------------------------------------- +#define SWAP(a, b) \ +{ \ +t = b; \ +b = a; \ +a = t; \ +} +static void dblSort3(float& a, float& b, float& c) // make: a <= b <= c +{ + float 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(float* x, float b, float c, float 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 + { + float sz1 = sqrt(x[0]); + float sz2 = sqrt(x[1]); + float 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 + float sz1 = sqrt(-x[0]); + float sz2 = sqrt(-x[1]); + float 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; + float sz1 = sqrt(x[0]); + float 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(float *x, float b, float c, float d) // solve equation x^4 + b*x^2 + c*x + d +//----------------------------------------------------------------------------- +float N4Step(float x, float a, float b, float c, float d) // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d +{ + float fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c; // f'(x) + if (fxs == 0) + return x; //return 1e99; <<-- FIXED! + float 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(float* x, float a, float b, float c, float d) +{ // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method + // move to a=0: + float d1 = d + 0.25 * a * (0.25 * b * a - 3. / 64 * a * a * a - c); + float c1 = c + 0.5 * a * (0.25 * a * a - b); + float 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) +//----------------------------------------------------------------------------- +float SolveP5_1(float a, float b, float c, float d, float 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; + + float 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 + + float x0, f0; // less than root + float x1, f1; // greater than root + float x2, f2, f2s; // next values, f(x2), f'(x2) + float 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 x00 + // 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: + // x00. + // 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(float a,float b,float c,float d,float e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +//----------------------------------------------------------------------------- +int SolveP5(float* x, float a, float b, float c, float d, float e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +{ + float r = x[0] = SolveP5_1(a, b, c, d, e); + float 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(float *x,float a,float b,float c,float d,float e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +//----------------------------------------------------------------------------- diff --git a/src/BulletSoftBody/poly34.h b/src/BulletSoftBody/poly34.h new file mode 100644 index 000000000..d6ae8664e --- /dev/null +++ b/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 +#define SIMD_FORCE_INLINE inline __attribute__ ((always_inline)) +// 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(float* x, float a, float 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(float* x, float a, float b, float 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(float* x, float a, float b, float c, float 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(float* x, float a, float b, float c, float d, float 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(float* x, float b, float d); // solve equation x^4 + b*x^2 + d = 0 +int SolveP4De(float* x, float b, float c, float d); // solve equation x^4 + b*x^2 + c*x + d = 0 +void CSqrt(float x, float y, float& a, float& b); // returns as a+i*s, sqrt(x+i*y) +float N4Step(float x, float a, float b, float c, float d); // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d +float SolveP5_1(float a, float b, float c, float d, float e); // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +#endif From 2f73c5130eca8d3c1f8c00152cac7202612ad7b2 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 6 Feb 2020 22:57:42 -0800 Subject: [PATCH 2/6] fix roots sorting --- src/BulletSoftBody/btSoftBodyInternals.h | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 7faa93dde..192cc667c 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -536,6 +536,14 @@ static SIMD_FORCE_INLINE bool bernsteinVFTest(const btSoftBody::Face* face, cons return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt); } +template +static SIMD_FORCE_INLINE void swap(T& a, T& b) +{ + T temp = a; + a = b; + b = temp; +} + 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)) @@ -587,7 +595,19 @@ static SIMD_FORCE_INLINE bool continuousCollisionDetection(const btSoftBody::Fac { num_roots = SolveP3(roots, a2/a3, a1/a3, a0/a3); } - std::sort(roots, roots+num_roots); +// std::sort(roots, roots+num_roots); + if (num_roots > 1) + { + if (roots[0] > roots[1]) + swap(roots[0], roots[1]); + } + if (num_roots > 2) + { + if (roots[0] > roots[2]) + swap(roots[0], roots[2]); + if (roots[1] > roots[2]) + swap(roots[1], roots[2]); + } for (int r = 0; r < num_roots; ++r) { double root = roots[r]; From dafed0101352c439610fb46c50f1ebd18606d473 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 7 Feb 2020 13:22:15 -0800 Subject: [PATCH 3/6] float->btScalar; add optional post collision damping --- .../btDeformableContactProjection.cpp | 1 - .../btDeformableMultiBodyDynamicsWorld.cpp | 15 ++- src/BulletSoftBody/btSoftBody.cpp | 9 +- src/BulletSoftBody/btSoftBody.h | 21 +-- src/BulletSoftBody/poly34.cpp | 122 +++++++++--------- 5 files changed, 91 insertions(+), 77 deletions(-) diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 350ad0c73..509378c6d 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -122,7 +122,6 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in // set Dirichlet constraint for (int j = 0; j < psb->m_nodes.size(); ++j) { - psb->m_nodes[i].m_constrained = false; // reset the m_constrained to false if (psb->m_nodes[j].m_im == 0) { btDeformableStaticConstraint static_constraint(&psb->m_nodes[j], infoGlobal); diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index aabe87f9f..8f59c16bf 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -206,7 +206,7 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim } if (penetration_count == 0) { - return; + break; } // apply inelastic impulse @@ -215,6 +215,19 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim m_softBodies[i]->applyRepulsionForce(timeStep, false); } } + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->m_usePostCollisionDamping) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (!psb->m_nodes[j].m_constrained) + psb->m_nodes[j].m_v *= psb->m_dampingCoefficient; + } + } + } } void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision() diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 20dc489f0..f6812fd10 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -204,11 +204,12 @@ void btSoftBody::initDefaults() m_windVelocity = btVector3(0, 0, 0); m_restLengthScale = btScalar(1.0); - m_dampingCoefficient = 1; - m_sleepingThreshold = 0.1; - m_useFaceContact = true; + m_dampingCoefficient = 1.0; + m_sleepingThreshold = 0.1; + m_useFaceContact = true; m_useSelfCollision = false; - m_collisionFlags = 0; + m_usePostCollisionDamping = false; + m_collisionFlags = 0; } // diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 6a1bfc473..0c26a3481 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -803,16 +803,17 @@ public: 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; - bool m_useFaceContact; - btAlignedObjectArray m_quads; // quadrature points for collision detection - btScalar repulsionStiffness; - - btAlignedObjectArray m_renderNodesInterpolationWeights; - btAlignedObjectArray > m_renderNodesParents; - bool m_useSelfCollision; + btScalar m_dampingCoefficient; // Damping Coefficient + btScalar m_sleepingThreshold; + btScalar m_maxSpeedSquared; + bool m_useFaceContact; + btAlignedObjectArray m_quads; // quadrature points for collision detection + btScalar repulsionStiffness; + + btAlignedObjectArray m_renderNodesInterpolationWeights; + btAlignedObjectArray > m_renderNodesParents; + bool m_useSelfCollision; + bool m_usePostCollisionDamping; btAlignedObjectArray m_clusterConnectivity; //cluster connectivity, for self-collision diff --git a/src/BulletSoftBody/poly34.cpp b/src/BulletSoftBody/poly34.cpp index 6566ee5f1..819d0c79f 100644 --- a/src/BulletSoftBody/poly34.cpp +++ b/src/BulletSoftBody/poly34.cpp @@ -8,14 +8,14 @@ #include "poly34.h" // solution of cubic and quartic equation #define TwoPi 6.28318530717958648 -const float eps = 1e-7; +const btScalar eps = SIMD_EPSILON; //============================================================================= // _root3, root3 from http://prografix.narod.ru //============================================================================= -static SIMD_FORCE_INLINE float _root3(float x) +static SIMD_FORCE_INLINE btScalar _root3(btScalar x) { - float s = 1.; + btScalar s = 1.; while (x < 1.) { x *= 8.; s *= 0.5; @@ -24,7 +24,7 @@ static SIMD_FORCE_INLINE float _root3(float x) x *= 0.125; s *= 2.; } - float r = 1.5; + 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)); @@ -34,7 +34,7 @@ static SIMD_FORCE_INLINE float _root3(float x) return r * s; } -float SIMD_FORCE_INLINE root3(float x) +btScalar SIMD_FORCE_INLINE root3(btScalar x) { if (x > 0) return _root3(x); @@ -47,9 +47,9 @@ float SIMD_FORCE_INLINE root3(float x) // 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(float* x, float a, float b) +int SolveP2(btScalar* x, btScalar a, btScalar b) { // solve equation x^2 + a*x + b = 0 - float D = 0.25 * a * a - b; + btScalar D = 0.25 * a * a - b; if (D >= 0) { D = sqrt(D); x[0] = -0.5 * a + D; @@ -65,19 +65,19 @@ int SolveP2(float* x, float a, float b) // 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(float* x, float a, float b, float c) +int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c) { // solve cubic equation x^3 + a*x^2 + b*x + c = 0 - float a2 = a * a; - float q = (a2 - 3 * b) / 9; + btScalar a2 = a * a; + btScalar q = (a2 - 3 * b) / 9; if (q < 0) q = eps; - float r = (a * (2 * a2 - 9 * b) + 27 * c) / 54; + btScalar r = (a * (2 * a2 - 9 * b) + 27 * c) / 54; // equation x^3 + q*x + r = 0 - float r2 = r * r; - float q3 = q * q * q; - float A, B; + btScalar r2 = r * r; + btScalar q3 = q * q * q; + btScalar A, B; if (r2 <= (q3 + eps)) { //<<-- FIXED! - float t = r / sqrt(q3); + btScalar t = r / sqrt(q3); if (t < -1) t = -1; if (t > 1) @@ -107,12 +107,12 @@ int SolveP3(float* x, float a, float b, float c) } return (1); } -} // SolveP3(float *x,float a,float b,float c) { +} // SolveP3(btScalar *x,btScalar a,btScalar b,btScalar c) { //--------------------------------------------------------------------------- // a>=0! -void CSqrt(float x, float y, float& a, float& b) // returns: a+i*s = sqrt(x+i*y) +void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b) // returns: a+i*s = sqrt(x+i*y) { - float r = sqrt(x * x + y * y); + btScalar r = sqrt(x * x + y * y); if (y == 0) { r = sqrt(r); if (x >= 0) { @@ -130,17 +130,17 @@ void CSqrt(float x, float y, float& a, float& b) // returns: a+i*s = sqrt(x+i*y } } //--------------------------------------------------------------------------- -int SolveP4Bi(float* x, float b, float d) // solve equation x^4 + b*x^2 + d = 0 +int SolveP4Bi(btScalar* x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 + d = 0 { - float D = b * b - 4 * d; + btScalar D = b * b - 4 * d; if (D >= 0) { - float sD = sqrt(D); - float x1 = (-b + sD) / 2; - float x2 = (-b - sD) / 2; // x2 <= x1 + 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 { - float sx1 = sqrt(x1); - float sx2 = sqrt(x2); + btScalar sx1 = sqrt(x1); + btScalar sx2 = sqrt(x2); x[0] = -sx1; x[1] = sx1; x[2] = -sx2; @@ -149,8 +149,8 @@ int SolveP4Bi(float* x, float b, float d) // solve equation x^4 + b*x^2 + d = 0 } if (x1 < 0) // x2 <= x1 < 0, two pair of imaginary roots { - float sx1 = sqrt(-x1); - float sx2 = sqrt(-x2); + btScalar sx1 = sqrt(-x1); + btScalar sx2 = sqrt(-x2); x[0] = 0; x[1] = sx1; x[2] = 0; @@ -158,8 +158,8 @@ int SolveP4Bi(float* x, float b, float d) // solve equation x^4 + b*x^2 + d = 0 return 0; } // now x2 < 0 <= x1 , two real roots and one pair of imginary root - float sx1 = sqrt(x1); - float sx2 = sqrt(-x2); + btScalar sx1 = sqrt(x1); + btScalar sx2 = sqrt(-x2); x[0] = -sx1; x[1] = sx1; x[2] = 0; @@ -167,12 +167,12 @@ int SolveP4Bi(float* x, float b, float d) // solve equation x^4 + b*x^2 + d = 0 return 2; } else { // if( D < 0 ), two pair of compex roots - float sD2 = 0.5 * sqrt(-D); + 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(float *x, float b, float d) // solve equation x^4 + b*x^2 d +} // SolveP4Bi(btScalar *x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 d //--------------------------------------------------------------------------- #define SWAP(a, b) \ { \ @@ -180,9 +180,9 @@ t = b; \ b = a; \ a = t; \ } -static void dblSort3(float& a, float& b, float& c) // make: a <= b <= c +static void dblSort3(btScalar& a, btScalar& b, btScalar& c) // make: a <= b <= c { - float t; + btScalar t; if (a > b) SWAP(a, b); // now a<=b if (c < b) { @@ -192,7 +192,7 @@ static void dblSort3(float& a, float& b, float& c) // make: a <= b <= c } } //--------------------------------------------------------------------------- -int SolveP4De(float* x, float b, float c, float d) // solve equation x^4 + b*x^2 + c*x + d +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))) @@ -206,9 +206,9 @@ int SolveP4De(float* x, float b, float c, float d) // solve equation x^4 + b*x^2 // Note: x[0]*x[1]*x[2]= c*c > 0 if (x[0] > 0) // all roots are positive { - float sz1 = sqrt(x[0]); - float sz2 = sqrt(x[1]); - float sz3 = sqrt(x[2]); + 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; @@ -226,9 +226,9 @@ int SolveP4De(float* x, float b, float c, float d) // solve equation x^4 + b*x^2 } // if( x[0] > 0) // all roots are positive // now x[0] <= x[1] < 0, x[2] > 0 // two pair of comlex roots - float sz1 = sqrt(-x[0]); - float sz2 = sqrt(-x[1]); - float sz3 = sqrt(x[2]); + btScalar sz1 = sqrt(-x[0]); + btScalar sz2 = sqrt(-x[1]); + btScalar sz3 = sqrt(x[2]); if (c > 0) // sign = -1 { @@ -251,8 +251,8 @@ int SolveP4De(float* x, float b, float c, float d) // solve equation x^4 + b*x^2 // x[0] must be >=0. But one times x[0]=~ 1e-17, so: if (x[0] < 0) x[0] = 0; - float sz1 = sqrt(x[0]); - float szr, szi; + 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 { @@ -268,14 +268,14 @@ int SolveP4De(float* x, float b, float c, float d) // solve equation x^4 + b*x^2 x[2] = -sz1 / 2; x[3] = szi; return 2; -} // SolveP4De(float *x, float b, float c, float d) // solve equation x^4 + b*x^2 + c*x + d +} // SolveP4De(btScalar *x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d //----------------------------------------------------------------------------- -float N4Step(float x, float a, float b, float c, float d) // one Newton step for x^4 + a*x^3 + 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 { - float fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c; // f'(x) + btScalar fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c; // f'(x) if (fxs == 0) return x; //return 1e99; <<-- FIXED! - float fx = (((x + a) * x + b) * x + c) * x + d; // f(x) + btScalar fx = (((x + a) * x + b) * x + c) * x + d; // f(x) return x - fx / fxs; } //----------------------------------------------------------------------------- @@ -283,12 +283,12 @@ float N4Step(float x, float a, float b, float c, float d) // one Newton step for // 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(float* x, float a, float b, float c, float d) +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: - float d1 = d + 0.25 * a * (0.25 * b * a - 3. / 64 * a * a * a - c); - float c1 = c + 0.5 * a * (0.25 * a * a - b); - float b1 = b - 0.375 * a * a; + 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; @@ -319,13 +319,13 @@ int SolveP4(float* x, float a, float b, float c, float d) //----------------------------------------------------------------------------- #define F5(t) (((((t + a) * t + b) * t + c) * t + d) * t + e) //----------------------------------------------------------------------------- -float SolveP5_1(float a, float b, float c, float d, float e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +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; - float brd = fabs(a); // brd - border of real roots + btScalar brd = fabs(a); // brd - border of real roots if (fabs(b) > brd) brd = fabs(b); if (fabs(c) > brd) @@ -336,10 +336,10 @@ float SolveP5_1(float a, float b, float c, float d, float e) // return real root brd = fabs(e); brd++; // brd - border of real roots - float x0, f0; // less than root - float x1, f1; // greater than root - float x2, f2, f2s; // next values, f(x2), f'(x2) - float dx = 0; + 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; @@ -408,12 +408,12 @@ float SolveP5_1(float a, float b, float c, float d, float e) // return real root x2 -= dx; } while (fabs(dx) > eps); return x2; -} // SolveP5_1(float a,float b,float c,float d,float e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +} // 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(float* x, float a, float b, float c, float d, float e) // solve equation 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 { - float r = x[0] = SolveP5_1(a, b, c, d, e); - float a1 = a + r, b1 = b + r * a1, c1 = c + r * b1, d1 = d + r * c1; + 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(float *x,float a,float b,float c,float d,float e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +} // 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 //----------------------------------------------------------------------------- From b9c0456d85bda58a99c2e72583faafca1a149237 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 7 Feb 2020 13:22:41 -0800 Subject: [PATCH 4/6] fix split impulse demo --- examples/DeformableDemo/SplitImpulse.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/examples/DeformableDemo/SplitImpulse.cpp b/examples/DeformableDemo/SplitImpulse.cpp index c24a47c42..c7c8088c2 100644 --- a/examples/DeformableDemo/SplitImpulse.cpp +++ b/examples/DeformableDemo/SplitImpulse.cpp @@ -24,7 +24,6 @@ #include "../CommonInterfaces/CommonRigidBodyBase.h" #include "../Utils/b3ResourcePath.h" -#define USE_SPLIT_IMPULSE 1 ///The SplitImpulse shows the effect of split impulse in deformable rigid contact. class SplitImpulse : public CommonRigidBodyBase { @@ -161,12 +160,6 @@ void SplitImpulse::initPhysics() m_dynamicsWorld->addRigidBody(body); } -#ifdef USE_SPLIT_IMPULSE - getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.03; -#else - getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.0; -#endif - // create a piece of cloth { const btScalar s = 4; @@ -187,7 +180,7 @@ void SplitImpulse::initPhysics() psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 2; + psb->m_cfg.kDF = 0.1; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); From c4eb4ad66c2ca30d46edb457ef00ab3161fdb689 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 7 Feb 2020 13:23:17 -0800 Subject: [PATCH 5/6] float->btScalar --- src/BulletSoftBody/btSoftBodyInternals.h | 2 +- src/BulletSoftBody/poly34.h | 20 ++++++++++---------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 192cc667c..898703edb 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -565,7 +565,7 @@ static SIMD_FORCE_INLINE bool continuousCollisionDetection(const btSoftBody::Fac btScalar a3 = c.dot(e); btScalar eps = SAFE_EPSILON; int num_roots = 0; - float roots[3]; + btScalar roots[3]; if (std::abs(a3) < eps) { // cubic term is zero diff --git a/src/BulletSoftBody/poly34.h b/src/BulletSoftBody/poly34.h index d6ae8664e..32ad5d7da 100644 --- a/src/BulletSoftBody/poly34.h +++ b/src/BulletSoftBody/poly34.h @@ -4,35 +4,35 @@ #ifndef POLY_34 #define POLY_34 -#define SIMD_FORCE_INLINE inline __attribute__ ((always_inline)) +#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(float* x, float a, float b); // solve equation x^2 + a*x + b = 0 +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(float* x, float a, float b, float c); // solve cubic equation x^3 + a*x^2 + b*x + c = 0 +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(float* x, float a, float b, float c, float d); // solve equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method +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(float* x, float a, float b, float c, float d, float e); // solve equation 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 //----------------------------------------------------------------------------- // And some additional functions for internal use. // Your may remove this definitions from here -int SolveP4Bi(float* x, float b, float d); // solve equation x^4 + b*x^2 + d = 0 -int SolveP4De(float* x, float b, float c, float d); // solve equation x^4 + b*x^2 + c*x + d = 0 -void CSqrt(float x, float y, float& a, float& b); // returns as a+i*s, sqrt(x+i*y) -float N4Step(float x, float a, float b, float c, float d); // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d -float SolveP5_1(float a, float b, float c, float d, float e); // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +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 From 60f73963a843ec6590f7bceda3cda14a72f55e18 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sat, 8 Feb 2020 21:21:43 -0800 Subject: [PATCH 6/6] add poly34.cpp to setup.py --- setup.py | 1 + 1 file changed, 1 insertion(+) diff --git a/setup.py b/setup.py index a6caac784..dda2d06a2 100644 --- a/setup.py +++ b/setup.py @@ -253,6 +253,7 @@ sources = ["examples/pybullet/pybullet.c"]\ +["src/BulletSoftBody/btDeformableContactConstraint.cpp"]\ +["src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp"]\ +["src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp"]\ ++["src/BulletSoftBody/poly34.cpp"]\ +["src/BulletInverseDynamics/IDMath.cpp"]\ +["src/BulletInverseDynamics/MultiBodyTree.cpp"]\ +["src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp"]\