Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-09-12 13:28:24 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #2612 from xhan0619/master

Browse Source 
Improve intra-deformable collision and deformable self collision
This commit is contained in:
erwincoumans
2020-02-12 19:30:46 -08:00
committed by GitHub
parent 8504dc7816 60f73963a8
commit b06cdd5fc1
19 changed files with 1837 additions and 256 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
examples/DeformableDemo/ClothFriction.cpp
View File

@@ -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;

2
examples/DeformableDemo/DeformableContact.cpp
View File

@@ -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

103
examples/DeformableDemo/DeformableSelfCollision.cpp
View File

@@ -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()
{

9
examples/DeformableDemo/PinchFriction.cpp
View File

@@ -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);
}

9
examples/DeformableDemo/SplitImpulse.cpp
View File

@@ -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);

1
setup.py
View File

@@ -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"]\

4
src/BulletSoftBody/CMakeLists.txt
View File

@@ -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
)

32
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -226,6 +226,7 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody
m_dt = dt;
m_objective->reinitialize(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);
}

11
src/BulletSoftBody/btDeformableContactConstraint.cpp
View File

@@ -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;
}

6
src/BulletSoftBody/btDeformableContactConstraint.h
View File

@@ -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);

11
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@@ -184,7 +184,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 +201,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
m_deformableConstraints[i].push_back(constraint);
}
}
#endif
}
}
@@ -243,6 +245,7 @@ void btDeformableContactProjection::project(TVStack& x)
void btDeformableContactProjection::setProjection()
{
BT_PROFILE("btDeformableContactProjection::setProjection");
btAlignedObjectArray<btVector3> units;
units.push_back(btVector3(1,0,0));
units.push_back(btVector3(0,1,0));
@@ -257,6 +260,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 +277,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 +294,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 +330,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)
{

100
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@@ -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,98 @@ 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)
{
break;
}
// apply inelastic impulse
for (int i = 0; i < m_softBodies.size(); ++i)
{
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()
{
m_deformableBodySolver->updateSoftBodies();
BT_PROFILE("btDeformableMultiBodyDynamicsWorld::softBodySelfCollision");
for (int i = 0; i < m_softBodies.size(); i++)
{
btSoftBody* psb = m_softBodies[i];
@@ -260,6 +353,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();

7
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
View File

@@ -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

348
src/BulletSoftBody/btSoftBody.cpp
View File

@@ -24,6 +24,93 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include <iostream>
//
static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& leafNodes, btAlignedObjectArray<btAlignedObjectArray<int> >& adj)
{
int N = leafNodes.size();
while (N > 1)
{
btAlignedObjectArray<bool> marked;
btAlignedObjectArray<btDbvtNode*> newLeafNodes;
btAlignedObjectArray<std::pair<int,int> > childIds;
btAlignedObjectArray<btAlignedObjectArray<int> > newAdj;
marked.resize(N);
for (int i = 0; i < N; ++i)
marked[i] = false;
// pair adjacent nodes into new(parent) node
for (int i = 0; i < N; ++i)
{
if (marked[i])
continue;
bool merged = false;
for (int j = 0; j < adj[i].size(); ++j)
{
int n = adj[i][j];
if (!marked[adj[i][j]])
{
btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
node->parent = NULL;
node->childs[0] = leafNodes[i];
node->childs[1] = leafNodes[n];
leafNodes[i]->parent = node;
leafNodes[n]->parent = node;
newLeafNodes.push_back(node);
childIds.push_back(std::make_pair(i,n));
merged = true;
marked[n] = true;
break;
}
}
if (!merged)
{
newLeafNodes.push_back(leafNodes[i]);
childIds.push_back(std::make_pair(i,-1));
}
marked[i] = true;
}
// update adjacency matrix
newAdj.resize(newLeafNodes.size());
for (int i = 0; i < newLeafNodes.size(); ++i)
{
for (int j = i+1; j < newLeafNodes.size(); ++j)
{
bool neighbor = false;
const btAlignedObjectArray<int>& leftChildNeighbors = adj[childIds[i].first];
for (int k = 0; k < leftChildNeighbors.size(); ++k)
{
if (leftChildNeighbors[k] == childIds[j].first || leftChildNeighbors[k] == childIds[j].second)
{
neighbor = true;
break;
}
}
if (!neighbor && childIds[i].second != -1)
{
const btAlignedObjectArray<int>& rightChildNeighbors = adj[childIds[i].second];
for (int k = 0; k < rightChildNeighbors.size(); ++k)
{
if (rightChildNeighbors[k] == childIds[j].first || rightChildNeighbors[k] == childIds[j].second)
{
neighbor = true;
break;
}
}
}
if (neighbor)
{
newAdj[i].push_back(j);
newAdj[j].push_back(i);
}
}
}
leafNodes = newLeafNodes;
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)
@@ -116,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;
}
//
@@ -135,6 +224,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 +2432,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<btDbvtNode*> 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<btAlignedObjectArray<int> > adj;
adj.resize(m_faces.size());
// construct the adjacency list for triangles
for (int i = 0; i < adj.size(); ++i)
{
for (int j = i+1; j < adj.size(); ++j)
{
int dup = 0;
for (int k = 0; k < 3; ++k)
{
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<btDbvtNode*> 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<btAlignedObjectArray<int> > adj;
adj.resize(m_nodes.size());
btAlignedObjectArray<int> old_id;
old_id.resize(m_nodes.size());
for (int i = 0; i < m_nodes.size(); ++i)
old_id[i] = m_nodes[i].index;
for (int i = 0; i < m_nodes.size(); ++i)
m_nodes[i].index = i;
for (int i = 0; i < m_links.size(); ++i)
{
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 +2671,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 +3304,7 @@ void btSoftBody::setSpringStiffness(btScalar k)
{
m_links[i].Feature::m_material->m_kLST = k;
}
repulsionStiffness = k;
}
void btSoftBody::initializeDmInverse()
@@ -3708,51 +3921,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 +3982,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 +3998,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 +4020,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;

199
src/BulletSoftBody/btSoftBody.h
View File

@@ -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,17 +800,20 @@ 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
btScalar m_sleepingThreshold;
btScalar m_maxSpeedSquared;
bool m_useFaceContact;
btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection
btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents;
bool m_useSelfCollision;
btScalar m_dampingCoefficient; // Damping Coefficient
btScalar m_sleepingThreshold;
btScalar m_maxSpeedSquared;
bool m_useFaceContact;
btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection
btScalar repulsionStiffness;
btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents;
bool m_useSelfCollision;
bool m_usePostCollisionDamping;
btAlignedObjectArray<bool> m_clusterConnectivity; //cluster connectivity, for self-collision
@@ -1120,6 +1127,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 +1160,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 <class DBVTNODE> // btDbvtNode or btDbvntNode
void updateFace(DBVTNODE* node, bool use_velocity, bool margin)
{
if (node->isleaf())
{
btSoftBody::Face* f = (btSoftBody::Face*)(node->data);
btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
if (use_velocity)
{
btVector3 points[6] = {f->m_n[0]->m_x, f->m_n[0]->m_x + m_sst.sdt * f->m_n[0]->m_v,
f->m_n[1]->m_x, f->m_n[1]->m_x + m_sst.sdt * f->m_n[1]->m_v,
f->m_n[2]->m_x, f->m_n[2]->m_x + m_sst.sdt * f->m_n[2]->m_v};
vol = btDbvtVolume::FromPoints(points, 6);
}
else
{
btVector3 points[3] = {f->m_n[0]->m_x,
f->m_n[1]->m_x,
f->m_n[2]->m_x};
vol = btDbvtVolume::FromPoints(points, 3);
}
vol.Expand(btVector3(pad, pad, pad));
node->volume = vol;
return;
}
else
{
updateFace(node->childs[0], use_velocity, margin);
updateFace(node->childs[1], use_velocity, margin);
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
Merge(node->childs[0]->volume, node->childs[1]->volume, vol);
node->volume = vol;
}
}
void updateFaceTree(bool use_velocity, bool margin)
{
if (m_fdbvt.m_root)
updateFace(m_fdbvt.m_root, use_velocity, margin);
if (m_fdbvnt)
updateFace(m_fdbvnt, use_velocity, margin);
}
template <typename T>
static inline T BaryEval(const T& a,
const T& b,
const T& c,
const btVector3& coord)
{
return (a * coord.x() + b * coord.y() + c * coord.z());
}
void applyRepulsionForce(btScalar timeStep, bool applySpringForce)
{
btAlignedObjectArray<int> indices;
{
// randomize the order of repulsive force
indices.resize(m_faceNodeContacts.size());
for (int i = 0; i < m_faceNodeContacts.size(); ++i)
indices[i] = i;
// 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)

783
src/BulletSoftBody/btSoftBodyInternals.h
View File

@@ -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 <string.h> //for memset
#include <cmath>
#include "poly34.h"
// Given a multibody link, a contact point and a contact direction, fill in the jacobian data needed to calculate the velocity change given an impulse in the contact direction
static 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,579 @@ 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 && a<SAFE_EPSILON);
}
static SIMD_FORCE_INLINE bool sameSign(const btScalar& a, const btScalar& b)
{
return (nearZero(a) || nearZero(b) || (a>SAFE_EPSILON && b>SAFE_EPSILON) || (a<-SAFE_EPSILON && b<-SAFE_EPSILON));
}
static SIMD_FORCE_INLINE bool diffSign(const btScalar& a, const btScalar& b)
{
return !sameSign(a, b);
}
inline btScalar evaluateBezier2(const btScalar &p0, const btScalar &p1, const btScalar &p2, const btScalar &t, const btScalar &s)
{
btScalar s2 = s*s;
btScalar t2 = t*t;
return p0*s2+p1*btScalar(2.0)*s*t+p2*t2;
}
inline btScalar evaluateBezier(const btScalar &p0, const btScalar &p1, const btScalar &p2, const btScalar &p3, const btScalar &t, const btScalar &s)
{
btScalar s2 = s*s;
btScalar s3 = s2*s;
btScalar t2 = t*t;
btScalar t3 = t2*t;
return p0*s3+p1*btScalar(3.0)*s2*t+p2*btScalar(3.0)*s*t2+p3*t3;
}
static SIMD_FORCE_INLINE bool getSigns(bool type_c, const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, const btScalar& t1, btScalar &lt0, btScalar &lt1)
{
if (sameSign(t0, t1)) {
lt0 = t0;
lt1 = t0;
return true;
}
if (type_c || diffSign(k0, k3)) {
btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1);
if (t0<-0)
ft = -ft;
if (sameSign(ft, k0)) {
lt0 = t1;
lt1 = t1;
}
else {
lt0 = t0;
lt1 = t0;
}
return true;
}
if (!type_c) {
btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1);
if (t0<-0)
ft = -ft;
if (diffSign(ft, k0)) {
lt0 = t0;
lt1 = t1;
return true;
}
btScalar fk = evaluateBezier2(k1-k0, k2-k1, k3-k2, t0, -t1);
if (sameSign(fk, k1-k0))
lt0 = lt1 = t1;
else
lt0 = lt1 = t0;
return true;
}
return false;
}
static SIMD_FORCE_INLINE void getBernsteinCoeff(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, btScalar& k0, btScalar& k1, btScalar& k2, btScalar& k3)
{
const btVector3& n0 = face->m_n0;
const btVector3& n1 = face->m_n1;
btVector3 n_hat = n0 + n1 - face->m_vn;
btVector3 p0ma0 = node->m_x - face->m_n[0]->m_x;
btVector3 p1ma1 = node->m_q - face->m_n[0]->m_q;
k0 = (p0ma0).dot(n0) * 3.0;
k1 = (p0ma0).dot(n_hat) + (p1ma1).dot(n0);
k2 = (p1ma1).dot(n_hat) + (p0ma0).dot(n1);
k3 = (p1ma1).dot(n1) * 3.0;
}
static SIMD_FORCE_INLINE void polyDecomposition(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, btScalar& u0, btScalar& u1, btScalar& v0, btScalar& v1)
{
btScalar denom = 4.0 * (j1-j2) * (j1-j0) + (j2-j0) * (j2-j0);
u0 = (2.0*(j1-j2)*(3.0*k1-2.0*k0-k3) - (j0-j2)*(3.0*k2-2.0*k3-k0)) / denom;
u1 = (2.0*(j1-j0)*(3.0*k2-2.0*k3-k0) - (j2-j0)*(3.0*k1-2.0*k0-k3)) / denom;
v0 = k0-u0*j0;
v1 = k3-u1*j2;
}
static SIMD_FORCE_INLINE bool rootFindingLemma(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3)
{
btScalar u0, u1, v0, v1;
btScalar j0 = 3.0*(k1-k0);
btScalar j1 = 3.0*(k2-k1);
btScalar j2 = 3.0*(k3-k2);
polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1);
if (sameSign(v0, v1))
{
btScalar Ypa = j0*(1.0-v0)*(1.0-v0) + 2.0*j1*v0*(1.0-v0) + j2*v0*v0; // Y'(v0)
if (sameSign(Ypa, j0))
{
return (diffSign(k0,v1));
}
}
return diffSign(k0,v0);
}
static SIMD_FORCE_INLINE void getJs(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Node* a, const btSoftBody::Node* b, const btSoftBody::Node* c, const btSoftBody::Node* p, const btScalar& dt, btScalar& j0, btScalar& j1, btScalar& j2)
{
const btVector3& a0 = a->m_x;
const btVector3& b0 = b->m_x;
const btVector3& c0 = c->m_x;
const btVector3& va = a->m_v;
const btVector3& vb = b->m_v;
const btVector3& vc = c->m_v;
const btVector3 a1 = a0 + dt*va;
const btVector3 b1 = b0 + dt*vb;
const btVector3 c1 = c0 + dt*vc;
btVector3 n0 = (b0-a0).cross(c0-a0);
btVector3 n1 = (b1-a1).cross(c1-a1);
btVector3 n_hat = n0+n1 - dt*dt*(vb-va).cross(vc-va);
const btVector3& p0 = p->m_x;
const btVector3& vp = p->m_v;
btVector3 p1 = p0 + dt*vp;
btVector3 m0 = (b0-p0).cross(c0-p0);
btVector3 m1 = (b1-p1).cross(c1-p1);
btVector3 m_hat = m0+m1 - dt*dt*(vb-vp).cross(vc-vp);
btScalar l0 = m0.dot(n0);
btScalar l1 = 0.25 * (m0.dot(n_hat) + m_hat.dot(n0));
btScalar l2 = btScalar(1)/btScalar(6)*(m0.dot(n1) + m_hat.dot(n_hat) + m1.dot(n0));
btScalar l3 = 0.25 * (m_hat.dot(n1) + m1.dot(n_hat));
btScalar l4 = m1.dot(n1);
btScalar k1p = 0.25 * k0 + 0.75 * k1;
btScalar k2p = 0.5 * k1 + 0.5 * k2;
btScalar k3p = 0.75 * k2 + 0.25 * k3;
btScalar s0 = (l1 * k0 - l0 * k1p)*4.0;
btScalar s1 = (l2 * k0 - l0 * k2p)*2.0;
btScalar s2 = (l3 * k0 - l0 * k3p)*btScalar(4)/btScalar(3);
btScalar s3 = l4 * k0 - l0 * k3;
j0 = (s1*k0 - s0*k1) * 3.0;
j1 = (s2*k0 - s0*k2) * 1.5;
j2 = (s3*k0 - s0*k3);
}
static SIMD_FORCE_INLINE bool signDetermination1Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1)
{
btScalar Yu0 = k0*(1.0-u0)*(1.0-u0)*(1.0-u0) + 3.0*k1*u0*(1.0-u0)*(1.0-u0) + 3.0*k2*u0*u0*(1.0-u0) + k3*u0*u0*u0; // Y(u0)
btScalar Yv0 = k0*(1.0-v0)*(1.0-v0)*(1.0-v0) + 3.0*k1*v0*(1.0-v0)*(1.0-v0) + 3.0*k2*v0*v0*(1.0-v0) + k3*v0*v0*v0; // Y(v0)
btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0;
btScalar L = sameSign(sign_Ytp, k0) ? u1 : u0;
sign_Ytp = (v0 > v1) ? Yv0 : -Yv0;
btScalar K = (sameSign(sign_Ytp,k0)) ? v1 : v0;
return diffSign(L,K);
}
static SIMD_FORCE_INLINE bool signDetermination2Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1)
{
btScalar Yu0 = k0*(1.0-u0)*(1.0-u0)*(1.0-u0) + 3.0*k1*u0*(1.0-u0)*(1.0-u0) + 3.0*k2*u0*u0*(1.0-u0) + k3*u0*u0*u0; // Y(u0)
btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0, L1, L2;
if (diffSign(sign_Ytp,k0))
{
L1 = u0;
L2 = u1;
}
else
{
btScalar Yp_u0 = j0*(1.0-u0)*(1.0-u0) + 2.0*j1*(1.0-u0)*u0 + j2*u0*u0;
if (sameSign(Yp_u0,j0))
{
L1 = u1;
L2 = u1;
}
else
{
L1 = u0;
L2 = u0;
}
}
btScalar Yv0 = k0*(1.0-v0)*(1.0-v0)*(1.0-v0) + 3.0*k1*v0*(1.0-v0)*(1.0-v0) + 3.0*k2*v0*v0*(1.0-v0) + k3*v0*v0*v0; // Y(uv0)
sign_Ytp = (v0 > v1) ? Yv0 : -Yv0;
btScalar K1, K2;
if (diffSign(sign_Ytp,k0))
{
K1 = v0;
K2 = v1;
}
else
{
btScalar Yp_v0 = j0*(1.0-v0)*(1.0-v0) + 2.0*j1*(1.0-v0)*v0 + j2*v0*v0;
if (sameSign(Yp_v0,j0))
{
K1 = v1;
K2 = v1;
}
else
{
K1 = v0;
K2 = v0;
}
}
return (diffSign(K1, L1) || diffSign(L2, K2));
}
static SIMD_FORCE_INLINE bool signDetermination1(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
{
btScalar j0, j1, j2, u0, u1, v0, v1;
// p1
getJs(k0,k1,k2,k3,face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2);
if (nearZero(j0+j2-j1*2.0))
{
btScalar lt0, lt1;
getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
if (lt0 < -SAFE_EPSILON)
return false;
}
else
{
polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1);
if (!signDetermination1Internal(k0,k1,k2,k3,u0,u1,v0,v1))
return false;
}
// p2
getJs(k0,k1,k2,k3,face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2);
if (nearZero(j0+j2-j1*2.0))
{
btScalar lt0, lt1;
getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
if (lt0 < -SAFE_EPSILON)
return false;
}
else
{
polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1);
if (!signDetermination1Internal(k0,k1,k2,k3,u0,u1,v0,v1))
return false;
}
// p3
getJs(k0,k1,k2,k3,face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2);
if (nearZero(j0+j2-j1*2.0))
{
btScalar lt0, lt1;
getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
if (lt0 < -SAFE_EPSILON)
return false;
}
else
{
polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1);
if (!signDetermination1Internal(k0,k1,k2,k3,u0,u1,v0,v1))
return false;
}
return true;
}
static SIMD_FORCE_INLINE bool signDetermination2(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
{
btScalar j0, j1, j2, u0, u1, v0, v1;
// p1
getJs(k0,k1,k2,k3,face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2);
if (nearZero(j0+j2-j1*2.0))
{
btScalar lt0, lt1;
bool bt0 = true, bt1=true;
getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
if (lt0 < -SAFE_EPSILON)
bt0 = false;
if (lt1 < -SAFE_EPSILON)
bt1 = false;
if (!bt0 && !bt1)
return false;
}
else
{
polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1);
if (!signDetermination2Internal(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1))
return false;
}
// p2
getJs(k0,k1,k2,k3,face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2);
if (nearZero(j0+j2-j1*2.0))
{
btScalar lt0, lt1;
bool bt0=true, bt1=true;
getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
if (lt0 < -SAFE_EPSILON)
bt0 = false;
if (lt1 < -SAFE_EPSILON)
bt1 = false;
if (!bt0 && !bt1)
return false;
}
else
{
polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1);
if (!signDetermination2Internal(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1))
return false;
}
// p3
getJs(k0,k1,k2,k3,face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2);
if (nearZero(j0+j2-j1*2.0))
{
btScalar lt0, lt1;
bool bt0=true, bt1=true;
getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
if (lt0 < -SAFE_EPSILON)
bt0 = false;
if (lt1 < -SAFE_EPSILON)
bt1 = false;
if (!bt0 && !bt1)
return false;
}
else
{
polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1);
if (!signDetermination2Internal(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1))
return false;
}
return true;
}
static SIMD_FORCE_INLINE bool coplanarAndInsideTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
{
// Coplanar test
if (diffSign(k1-k0, k3-k2))
{
// Case b:
if (sameSign(k0, k3) && !rootFindingLemma(k0,k1,k2,k3))
return false;
// inside test
return signDetermination2(k0, k1, k2, k3, face, node, dt);
}
else
{
// Case c:
if (sameSign(k0, k3))
return false;
// inside test
return signDetermination1(k0, k1, k2, k3, face, node, dt);
}
return false;
}
static SIMD_FORCE_INLINE bool conservativeCulling(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg)
{
if (k0 > mrg && k1 > mrg && k2 > mrg && k3 > mrg)
return true;
if (k0 < -mrg && k1 < -mrg && k2 < -mrg && k3 < -mrg)
return true;
return false;
}
static SIMD_FORCE_INLINE bool bernsteinVFTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
{
if (conservativeCulling(k0, k1, k2, k3, mrg))
return false;
return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt);
}
static SIMD_FORCE_INLINE void deCasteljau(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, btScalar& k10, btScalar& k20, btScalar& k30, btScalar& k21, btScalar& k12)
{
k10 = k0*(1.0-t0) + k1*t0;
btScalar k11 = k1*(1.0-t0) + k2*t0;
k12 = k2*(1.0-t0) + k3*t0;
k20 = k10*(1.0-t0) + k11*t0;
k21 = k11*(1.0-t0) + k12*t0;
k30 = k20*(1.0-t0) + k21*t0;
}
static SIMD_FORCE_INLINE bool bernsteinVFTest(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg)
{
btScalar k0, k1, k2, k3;
getBernsteinCoeff(face, node, dt, k0, k1, k2, k3);
if (conservativeCulling(k0, k1, k2, k3, mrg))
return false;
return true;
if (diffSign(k2-2.0*k1+k0, k3-2.0*k2+k1))
{
btScalar k10, k20, k30, k21, k12;
btScalar t0 = (k2-2.0*k1+k0)/(k0-3.0*k1+3.0*k2-k3);
deCasteljau(k0, k1, k2, k3, t0, k10, k20, k30, k21, k12);
return bernsteinVFTest(k0, k10, k20, k30, mrg, face, node, dt) || bernsteinVFTest(k30, k21, k12, k3, mrg, face, node, dt);
}
return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt);
}
template <class T>
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))
return false;
btVector3 x21 = face->m_n[1]->m_x - face->m_n[0]->m_x;
btVector3 x31 = face->m_n[2]->m_x - face->m_n[0]->m_x;
btVector3 x41 = node->m_x - face->m_n[0]->m_x;
btVector3 v21 = face->m_n[1]->m_v - face->m_n[0]->m_v;
btVector3 v31 = face->m_n[2]->m_v - face->m_n[0]->m_v;
btVector3 v41 = node->m_v - face->m_n[0]->m_v;
btVector3 a = x21.cross(x31);
btVector3 b = x21.cross(v31) + v21.cross(x31);
btVector3 c = v21.cross(v31);
btVector3 d = x41;
btVector3 e = v41;
btScalar a0 = a.dot(d);
btScalar a1 = a.dot(e) + b.dot(d);
btScalar a2 = c.dot(d) + b.dot(e);
btScalar a3 = c.dot(e);
btScalar eps = SAFE_EPSILON;
int num_roots = 0;
btScalar roots[3];
if (std::abs(a3) < eps)
{
// cubic term is zero
if (std::abs(a2) < eps)
{
if (std::abs(a1) < eps)
{
if (std::abs(a0) < eps)
{
num_roots = 2;
roots[0] = 0;
roots[1] = dt;
}
}
else
{
num_roots = 1;
roots[0] = -a0/a1;
}
}
else
{
num_roots = SolveP2(roots, a1/a2, a0/a2);
}
}
else
{
num_roots = SolveP3(roots, a2/a3, a1/a3, a0/a3);
}
// std::sort(roots, roots+num_roots);
if (num_roots > 1)
{
if (roots[0] > roots[1])
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];
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 +1652,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 +1733,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 +1893,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 +1910,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 +1935,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

3
src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp
View File

@@ -48,9 +48,10 @@ btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm()
}
#include <stdio.h>
#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");

419
src/BulletSoftBody/poly34.cpp Normal file
View File

@@ -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 <rakhmanin (at) gmail.com>
// public domain
//
#include <math.h>
#include "poly34.h" // solution of cubic and quartic equation
#define TwoPi 6.28318530717958648
const btScalar eps = SIMD_EPSILON;
//=============================================================================
// _root3, root3 from http://prografix.narod.ru
//=============================================================================
static SIMD_FORCE_INLINE btScalar _root3(btScalar x)
{
btScalar s = 1.;
while (x < 1.) {
x *= 8.;
s *= 0.5;
}
while (x > 8.) {
x *= 0.125;
s *= 2.;
}
btScalar r = 1.5;
r -= 1. / 3. * (r - x / (r * r));
r -= 1. / 3. * (r - x / (r * r));
r -= 1. / 3. * (r - x / (r * r));
r -= 1. / 3. * (r - x / (r * r));
r -= 1. / 3. * (r - x / (r * r));
r -= 1. / 3. * (r - x / (r * r));
return r * s;
}
btScalar SIMD_FORCE_INLINE root3(btScalar x)
{
if (x > 0)
return _root3(x);
else if (x < 0)
return -_root3(-x);
else
return 0.;
}
// x - array of size 2
// return 2: 2 real roots x[0], x[1]
// return 0: pair of complex roots: x[0]i*x[1]
int SolveP2(btScalar* x, btScalar a, btScalar b)
{ // solve equation x^2 + a*x + b = 0
btScalar D = 0.25 * a * a - b;
if (D >= 0) {
D = sqrt(D);
x[0] = -0.5 * a + D;
x[1] = -0.5 * a - D;
return 2;
}
x[0] = -0.5 * a;
x[1] = sqrt(-D);
return 0;
}
//---------------------------------------------------------------------------
// x - array of size 3
// In case 3 real roots: => x[0], x[1], x[2], return 3
// 2 real roots: x[0], x[1], return 2
// 1 real root : x[0], x[1] i*x[2], return 1
int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c)
{ // solve cubic equation x^3 + a*x^2 + b*x + c = 0
btScalar a2 = a * a;
btScalar q = (a2 - 3 * b) / 9;
if (q < 0)
q = eps;
btScalar r = (a * (2 * a2 - 9 * b) + 27 * c) / 54;
// equation x^3 + q*x + r = 0
btScalar r2 = r * r;
btScalar q3 = q * q * q;
btScalar A, B;
if (r2 <= (q3 + eps)) { //<<-- FIXED!
btScalar t = r / sqrt(q3);
if (t < -1)
t = -1;
if (t > 1)
t = 1;
t = acos(t);
a /= 3;
q = -2 * sqrt(q);
x[0] = q * cos(t / 3) - a;
x[1] = q * cos((t + TwoPi) / 3) - a;
x[2] = q * cos((t - TwoPi) / 3) - a;
return (3);
}
else {
//A =-pow(fabs(r)+sqrt(r2-q3),1./3);
A = -root3(fabs(r) + sqrt(r2 - q3));
if (r < 0)
A = -A;
B = (A == 0 ? 0 : q / A);
a /= 3;
x[0] = (A + B) - a;
x[1] = -0.5 * (A + B) - a;
x[2] = 0.5 * sqrt(3.) * (A - B);
if (fabs(x[2]) < eps) {
x[2] = x[1];
return (2);
}
return (1);
}
} // SolveP3(btScalar *x,btScalar a,btScalar b,btScalar c) {
//---------------------------------------------------------------------------
// a>=0!
void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b) // returns: a+i*s = sqrt(x+i*y)
{
btScalar r = sqrt(x * x + y * y);
if (y == 0) {
r = sqrt(r);
if (x >= 0) {
a = r;
b = 0;
}
else {
a = 0;
b = r;
}
}
else { // y != 0
a = sqrt(0.5 * (x + r));
b = 0.5 * y / a;
}
}
//---------------------------------------------------------------------------
int SolveP4Bi(btScalar* x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 + d = 0
{
btScalar D = b * b - 4 * d;
if (D >= 0) {
btScalar sD = sqrt(D);
btScalar x1 = (-b + sD) / 2;
btScalar x2 = (-b - sD) / 2; // x2 <= x1
if (x2 >= 0) // 0 <= x2 <= x1, 4 real roots
{
btScalar sx1 = sqrt(x1);
btScalar sx2 = sqrt(x2);
x[0] = -sx1;
x[1] = sx1;
x[2] = -sx2;
x[3] = sx2;
return 4;
}
if (x1 < 0) // x2 <= x1 < 0, two pair of imaginary roots
{
btScalar sx1 = sqrt(-x1);
btScalar sx2 = sqrt(-x2);
x[0] = 0;
x[1] = sx1;
x[2] = 0;
x[3] = sx2;
return 0;
}
// now x2 < 0 <= x1 , two real roots and one pair of imginary root
btScalar sx1 = sqrt(x1);
btScalar sx2 = sqrt(-x2);
x[0] = -sx1;
x[1] = sx1;
x[2] = 0;
x[3] = sx2;
return 2;
}
else { // if( D < 0 ), two pair of compex roots
btScalar sD2 = 0.5 * sqrt(-D);
CSqrt(-0.5 * b, sD2, x[0], x[1]);
CSqrt(-0.5 * b, -sD2, x[2], x[3]);
return 0;
} // if( D>=0 )
} // SolveP4Bi(btScalar *x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 d
//---------------------------------------------------------------------------
#define SWAP(a, b) \
{ \
t = b; \
b = a; \
a = t; \
}
static void dblSort3(btScalar& a, btScalar& b, btScalar& c) // make: a <= b <= c
{
btScalar t;
if (a > b)
SWAP(a, b); // now a<=b
if (c < b) {
SWAP(b, c); // now a<=b, b<=c
if (a > b)
SWAP(a, b); // now a<=b
}
}
//---------------------------------------------------------------------------
int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d
{
//if( c==0 ) return SolveP4Bi(x,b,d); // After that, c!=0
if (fabs(c) < 1e-14 * (fabs(b) + fabs(d)))
return SolveP4Bi(x, b, d); // After that, c!=0
int res3 = SolveP3(x, 2 * b, b * b - 4 * d, -c * c); // solve resolvent
// by Viet theorem: x1*x2*x3=-c*c not equals to 0, so x1!=0, x2!=0, x3!=0
if (res3 > 1) // 3 real roots,
{
dblSort3(x[0], x[1], x[2]); // sort roots to x[0] <= x[1] <= x[2]
// Note: x[0]*x[1]*x[2]= c*c > 0
if (x[0] > 0) // all roots are positive
{
btScalar sz1 = sqrt(x[0]);
btScalar sz2 = sqrt(x[1]);
btScalar sz3 = sqrt(x[2]);
// Note: sz1*sz2*sz3= -c (and not equal to 0)
if (c > 0) {
x[0] = (-sz1 - sz2 - sz3) / 2;
x[1] = (-sz1 + sz2 + sz3) / 2;
x[2] = (+sz1 - sz2 + sz3) / 2;
x[3] = (+sz1 + sz2 - sz3) / 2;
return 4;
}
// now: c<0
x[0] = (-sz1 - sz2 + sz3) / 2;
x[1] = (-sz1 + sz2 - sz3) / 2;
x[2] = (+sz1 - sz2 - sz3) / 2;
x[3] = (+sz1 + sz2 + sz3) / 2;
return 4;
} // if( x[0] > 0) // all roots are positive
// now x[0] <= x[1] < 0, x[2] > 0
// two pair of comlex roots
btScalar sz1 = sqrt(-x[0]);
btScalar sz2 = sqrt(-x[1]);
btScalar sz3 = sqrt(x[2]);
if (c > 0) // sign = -1
{
x[0] = -sz3 / 2;
x[1] = (sz1 - sz2) / 2; // x[0]i*x[1]
x[2] = sz3 / 2;
x[3] = (-sz1 - sz2) / 2; // x[2]i*x[3]
return 0;
}
// now: c<0 , sign = +1
x[0] = sz3 / 2;
x[1] = (-sz1 + sz2) / 2;
x[2] = -sz3 / 2;
x[3] = (sz1 + sz2) / 2;
return 0;
} // if( res3>1 ) // 3 real roots,
// now resoventa have 1 real and pair of compex roots
// x[0] - real root, and x[0]>0,
// x[1]i*x[2] - complex roots,
// x[0] must be >=0. But one times x[0]=~ 1e-17, so:
if (x[0] < 0)
x[0] = 0;
btScalar sz1 = sqrt(x[0]);
btScalar szr, szi;
CSqrt(x[1], x[2], szr, szi); // (szr+i*szi)^2 = x[1]+i*x[2]
if (c > 0) // sign = -1
{
x[0] = -sz1 / 2 - szr; // 1st real root
x[1] = -sz1 / 2 + szr; // 2nd real root
x[2] = sz1 / 2;
x[3] = szi;
return 2;
}
// now: c<0 , sign = +1
x[0] = sz1 / 2 - szr; // 1st real root
x[1] = sz1 / 2 + szr; // 2nd real root
x[2] = -sz1 / 2;
x[3] = szi;
return 2;
} // SolveP4De(btScalar *x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d
//-----------------------------------------------------------------------------
btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d) // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d
{
btScalar fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c; // f'(x)
if (fxs == 0)
return x; //return 1e99; <<-- FIXED!
btScalar fx = (((x + a) * x + b) * x + c) * x + d; // f(x)
return x - fx / fxs;
}
//-----------------------------------------------------------------------------
// x - array of size 4
// return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots
// return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3],
// return 0: two pair of complex roots: x[0]i*x[1], x[2]i*x[3],
int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d)
{ // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method
// move to a=0:
btScalar d1 = d + 0.25 * a * (0.25 * b * a - 3. / 64 * a * a * a - c);
btScalar c1 = c + 0.5 * a * (0.25 * a * a - b);
btScalar b1 = b - 0.375 * a * a;
int res = SolveP4De(x, b1, c1, d1);
if (res == 4) {
x[0] -= a / 4;
x[1] -= a / 4;
x[2] -= a / 4;
x[3] -= a / 4;
}
else if (res == 2) {
x[0] -= a / 4;
x[1] -= a / 4;
x[2] -= a / 4;
}
else {
x[0] -= a / 4;
x[2] -= a / 4;
}
// one Newton step for each real root:
if (res > 0) {
x[0] = N4Step(x[0], a, b, c, d);
x[1] = N4Step(x[1], a, b, c, d);
}
if (res > 2) {
x[2] = N4Step(x[2], a, b, c, d);
x[3] = N4Step(x[3], a, b, c, d);
}
return res;
}
//-----------------------------------------------------------------------------
#define F5(t) (((((t + a) * t + b) * t + c) * t + d) * t + e)
//-----------------------------------------------------------------------------
btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
{
int cnt;
if (fabs(e) < eps)
return 0;
btScalar brd = fabs(a); // brd - border of real roots
if (fabs(b) > brd)
brd = fabs(b);
if (fabs(c) > brd)
brd = fabs(c);
if (fabs(d) > brd)
brd = fabs(d);
if (fabs(e) > brd)
brd = fabs(e);
brd++; // brd - border of real roots
btScalar x0, f0; // less than root
btScalar x1, f1; // greater than root
btScalar x2, f2, f2s; // next values, f(x2), f'(x2)
btScalar dx = 0;
if (e < 0) {
x0 = 0;
x1 = brd;
f0 = e;
f1 = F5(x1);
x2 = 0.01 * brd;
} // positive root
else {
x0 = -brd;
x1 = 0;
f0 = F5(x0);
f1 = e;
x2 = -0.01 * brd;
} // negative root
if (fabs(f0) < eps)
return x0;
if (fabs(f1) < eps)
return x1;
// now x0<x1, f(x0)<0, f(x1)>0
// Firstly 10 bisections
for (cnt = 0; cnt < 10; cnt++) {
x2 = (x0 + x1) / 2; // next point
//x2 = x0 - f0*(x1 - x0) / (f1 - f0); // next point
f2 = F5(x2); // f(x2)
if (fabs(f2) < eps)
return x2;
if (f2 > 0) {
x1 = x2;
f1 = f2;
}
else {
x0 = x2;
f0 = f2;
}
}
// At each step:
// x0<x1, f(x0)<0, f(x1)>0.
// x2 - next value
// we hope that x0 < x2 < x1, but not necessarily
do {
if (cnt++ > 50)
break;
if (x2 <= x0 || x2 >= x1)
x2 = (x0 + x1) / 2; // now x0 < x2 < x1
f2 = F5(x2); // f(x2)
if (fabs(f2) < eps)
return x2;
if (f2 > 0) {
x1 = x2;
f1 = f2;
}
else {
x0 = x2;
f0 = f2;
}
f2s = (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d; // f'(x2)
if (fabs(f2s) < eps) {
x2 = 1e99;
continue;
}
dx = f2 / f2s;
x2 -= dx;
} while (fabs(dx) > eps);
return x2;
} // SolveP5_1(btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
//-----------------------------------------------------------------------------
int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
{
btScalar r = x[0] = SolveP5_1(a, b, c, d, e);
btScalar a1 = a + r, b1 = b + r * a1, c1 = c + r * b1, d1 = d + r * c1;
return 1 + SolveP4(x + 1, a1, b1, c1, d1);
} // SolveP5(btScalar *x,btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
//-----------------------------------------------------------------------------

38
src/BulletSoftBody/poly34.h Normal file
View File

@@ -0,0 +1,38 @@
// poly34.h : solution of cubic and quartic equation
// (c) Khashin S.I. http://math.ivanovo.ac.ru/dalgebra/Khashin/index.html
// khash2 (at) gmail.com
#ifndef POLY_34
#define POLY_34
#include "LinearMath/btScalar.h"
// x - array of size 2
// return 2: 2 real roots x[0], x[1]
// return 0: pair of complex roots: x[0]i*x[1]
int SolveP2(btScalar* x, btScalar a, btScalar b); // solve equation x^2 + a*x + b = 0
// x - array of size 3
// return 3: 3 real roots x[0], x[1], x[2]
// return 1: 1 real root x[0] and pair of complex roots: x[1]i*x[2]
int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c); // solve cubic equation x^3 + a*x^2 + b*x + c = 0
// x - array of size 4
// return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots
// return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3],
// return 0: two pair of complex roots: x[0]i*x[1], x[2]i*x[3],
int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d); // solve equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method
// x - array of size 5
// return 5: 5 real roots x[0], x[1], x[2], x[3], x[4], possible multiple roots
// return 3: 3 real roots x[0], x[1], x[2] and complex x[3]i*x[4],
// return 1: 1 real root x[0] and two pair of complex roots: x[1]i*x[2], x[3]i*x[4],
int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
//-----------------------------------------------------------------------------
// And some additional functions for internal use.
// Your may remove this definitions from here
int SolveP4Bi(btScalar* x, btScalar b, btScalar d); // solve equation x^4 + b*x^2 + d = 0
int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d); // solve equation x^4 + b*x^2 + c*x + d = 0
void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b); // returns as a+i*s, sqrt(x+i*y)
btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d); // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d
btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
#endif