support applying impulse in alternating order. change several example setups.

This commit is contained in:
jingyuc 2021-09-28 00:55:12 -04:00
parent 354c5a6e62
commit 1bee1ba4c4
6 changed files with 134 additions and 122 deletions

View File

@ -33,7 +33,7 @@ static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0;
static btScalar COLLIDING_VELOCITY = 0;
static int start_mode = 6;
static int num_modes = 20;
static int num_modes = 40;
class FreeFall : public CommonDeformableBodyBase
{
@ -56,29 +56,29 @@ public:
void resetCamera()
{
float dist = 6;
float pitch = -20;
float yaw = 90;
float targetPos[3] = {0, 2, 0};
// float dist = 20;
// float pitch = -30;
// float yaw = 125;
// float targetPos[3] = {-2, 0, 2};
// float dist = 6;
// float pitch = -20;
// float yaw = 90;
// float targetPos[3] = {0, 2, 0};
float dist = 20;
float pitch = -30;
float yaw = 125;
float targetPos[3] = {-2, 0, 2};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void Ctor_RbUpStack()
{
float mass = 10;
btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
// btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
// btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
btTransform startTransform;
startTransform.setIdentity();
// startTransform.setOrigin(btVector3(0, 12, 0));
// btRigidBody* rb0 = createRigidBody(mass, startTransform, shape);
// rb0->setLinearVelocity(btVector3(0, 0, 0));
startTransform.setOrigin(btVector3(0,10,0));
startTransform.setOrigin(btVector3(0,4,0));
// startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 4.0));
btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
rb1->setActivationState(DISABLE_DEACTIVATION);
@ -146,18 +146,18 @@ void FreeFall::initPhysics()
// create volumetric reduced deformable body
{
btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedCube(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes);
btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedBeam(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.01);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 5, 0));
init_transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 2.0));
init_transform.setOrigin(btVector3(0, 10, 0));
// init_transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 2.0));
rsb->transform(init_transform);
rsb->setStiffnessScale(25);
rsb->setStiffnessScale(20);
rsb->setDamping(damping_alpha, damping_beta);
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
@ -261,7 +261,7 @@ void FreeFall::initPhysics()
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.3;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;

View File

@ -61,22 +61,22 @@ public:
{
// float dist = 20;
// float pitch = -10;
// // float dist = 5;
// // float pitch = -5;
// float yaw = 90;
// float targetPos[3] = {0, 0, 0};
float dist = 10;
float pitch = -5;
float yaw = 90;
float targetPos[3] = {0, 0, 0};
float dist = 5;
float pitch = -35;
float yaw = 50;
float targetPos[3] = {-3, 2.8, -2.5};
// float dist = 5;
// float pitch = -35;
// float yaw = 50;
// float targetPos[3] = {-3, 2.8, -2.5};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void Ctor_RbUpStack()
{
// float mass = 55;
float mass = 10;
float mass = 8;
btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
btVector3 localInertia(0, 0, 0);
@ -85,7 +85,7 @@ public:
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,4,0));
startTransform.setOrigin(btVector3(0,-2,0));
// startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 3.0));
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
@ -95,7 +95,7 @@ public:
m_dynamicsWorld->addRigidBody(body, 1, 1+2);
body->setActivationState(DISABLE_DEACTIVATION);
body->setLinearVelocity(btVector3(0, 0, 0));
body->setLinearVelocity(btVector3(0, COLLIDING_VELOCITY, 0));
// body->setFriction(1);
}
@ -190,7 +190,7 @@ void ReducedCollide::initPhysics()
m_broadphase = new btDbvtBroadphase();
btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver();
btVector3 gravity = btVector3(0, -10, 0);
btVector3 gravity = btVector3(0, 0, 0);
reducedSoftBodySolver->setGravity(gravity);
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
@ -200,6 +200,7 @@ void ReducedCollide::initPhysics()
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
m_dynamicsWorld->setGravity(gravity);
m_dynamicsWorld->getSolverInfo().m_globalCfm = 1e-3;
m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_RANDMIZE_ORDER;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// create volumetric reduced deformable body
@ -211,7 +212,7 @@ void ReducedCollide::initPhysics()
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 10, 0));
init_transform.setOrigin(btVector3(0, 4, 0));
// init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0));
rsb->transform(init_transform);
@ -226,70 +227,70 @@ void ReducedCollide::initPhysics()
rsb->m_sleepingThreshold = 0;
btSoftBodyHelpers::generateBoundaryFaces(rsb);
rsb->setRigidVelocity(btVector3(0, 0, 0));
rsb->setRigidVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
// rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
}
// rigidBar();
// add a few rigid bodies
// Ctor_RbUpStack();
Ctor_RbUpStack();
// create ground
createGround();
// createGround();
// create multibody
{
bool damping = false;
bool gyro = true;
int numLinks = 0;
bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool multibodyOnly = true;
bool canSleep = false;
bool selfCollide = true;
bool multibodyConstraint = false;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(1, 1, 1);
// btVector3 baseHalfExtents(2.5, 0.5, 2.5);
// btVector3 baseHalfExtents(0.05, 0.37, 0.1);
bool g_floatingBase = true;
// btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0, 4, 0), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 4.f, 0.f), baseHalfExtents, linkHalfExtents, spherical, g_floatingBase);
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
//
if (!damping)
{
mbC->setLinearDamping(0.f);
mbC->setAngularDamping(0.f);
}
else
{
mbC->setLinearDamping(0.1f);
mbC->setAngularDamping(0.9f);
}
//
//////////////////////////////////////////////
// if (numLinks > 0)
// {
// btScalar q0 = 45.f * SIMD_PI / 180.f;
// if (!spherical)
// bool damping = false;
// bool gyro = true;
// int numLinks = 0;
// bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
// bool multibodyOnly = true;
// bool canSleep = false;
// bool selfCollide = true;
// bool multibodyConstraint = false;
// btVector3 linkHalfExtents(0.05, 0.37, 0.1);
// btVector3 baseHalfExtents(1, 1, 1);
// // btVector3 baseHalfExtents(2.5, 0.5, 2.5);
// // btVector3 baseHalfExtents(0.05, 0.37, 0.1);
// bool g_floatingBase = true;
// // btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0, 4, 0), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
// btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 4.f, 0.f), baseHalfExtents, linkHalfExtents, spherical, g_floatingBase);
// //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
// mbC->setCanSleep(canSleep);
// mbC->setHasSelfCollision(selfCollide);
// mbC->setUseGyroTerm(gyro);
// //
// if (!damping)
// {
// mbC->setJointPosMultiDof(0, &q0);
// mbC->setLinearDamping(0.f);
// mbC->setAngularDamping(0.f);
// }
// else
// {
// btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
// quat0.normalize();
// mbC->setJointPosMultiDof(0, quat0);
// mbC->setLinearDamping(0.1f);
// mbC->setAngularDamping(0.9f);
// }
// //
// //////////////////////////////////////////////
// // if (numLinks > 0)
// // {
// // btScalar q0 = 45.f * SIMD_PI / 180.f;
// // if (!spherical)
// // {
// // mbC->setJointPosMultiDof(0, &q0);
// // }
// // else
// // {
// // btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
// // quat0.normalize();
// // mbC->setJointPosMultiDof(0, quat0);
// // }
// // }
// ///
// addColliders_testMultiDof(mbC, m_dynamicsWorld, baseHalfExtents, linkHalfExtents);
// }
///
addColliders_testMultiDof(mbC, m_dynamicsWorld, baseHalfExtents, linkHalfExtents);
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);

View File

@ -346,12 +346,6 @@ void btReducedDeformableNodeRigidContactConstraint::warmStarting()
m_bufferVelocityA = va;
m_bufferVelocityB = vb;
// add the external impulse force (TODO: add external torque impulse)
// if (!m_collideStatic)
// {
// va += m_solverBody->m_originalBody->getTotalForce() * m_solverBody->m_originalBody->getInvMass() * m_dt;
// }
// we define the (+) direction of errors to be the outward surface normal of the rigid object
btVector3 v_rel = vb - va;
// get tangent direction of the relative velocity
@ -367,7 +361,7 @@ void btReducedDeformableNodeRigidContactConstraint::warmStarting()
{
if (!m_collideMultibody)
{
m_contactTangent = v_tangent.normalize();
m_contactTangent = v_tangent.normalized();
// tangent impulse factor
m_tangentImpulseFactor = (m_impulseFactor * m_contactTangent).dot(m_contactTangent);
m_tangentImpulseFactorInv = btScalar(1) / m_tangentImpulseFactor;

View File

@ -8,7 +8,7 @@
btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
: btSoftBody(worldInfo, node_count, x, m)
{
m_rigidOnly = false; //! only use rigid frame to debug
m_rigidOnly = true; //! only use rigid frame to debug
// reduced deformable
m_reducedModel = true;

View File

@ -3,6 +3,7 @@
btReducedSoftBodySolver::btReducedSoftBodySolver()
{
m_ascendOrder = true;
m_reducedSolver = true;
m_dampingAlpha = 0;
m_dampingBeta = 0;
@ -265,48 +266,62 @@ btScalar btReducedSoftBodySolver::solveContactConstraints(btCollisionObject** de
{
btScalar residualSquare = 0;
// handle fixed constraint
btAlignedObjectArray<int> m_orderNonContactConstraintPool;
btAlignedObjectArray<int> m_orderContactConstraintPool;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
// shuffle the order of applying constraint
// if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
{
m_orderNonContactConstraintPool.resize(m_staticConstraints[i].size());
m_orderContactConstraintPool.resize(m_nodeRigidConstraints[i].size());
// fixed constraint order
for (int j = 0; j < m_staticConstraints[i].size(); ++j)
{
m_orderNonContactConstraintPool[j] = m_ascendOrder ? j : m_staticConstraints[i].size() - 1 - j;
}
// contact constraint order
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
{
m_orderContactConstraintPool[j] = m_ascendOrder ? j : m_nodeRigidConstraints[i].size() - 1 - j;
std::cout << m_orderContactConstraintPool[j] << '\n';
}
m_ascendOrder = m_ascendOrder ? false : true;
}
// handle fixed constraint
for (int k = 0; k < m_staticConstraints[i].size(); ++k)
{
btReducedDeformableStaticConstraint& constraint = m_staticConstraints[i][k];
btReducedDeformableStaticConstraint& constraint = m_staticConstraints[i][m_orderNonContactConstraintPool[k]];
btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
}
// handle contact constraint
for (int i = 0; i < numDeformableBodies; ++i)
{
for (int j = 0; j < m_softBodies.size(); ++j)
{
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
if (rsb != deformableBodies[i])
{
continue;
}
// node vs rigid contact
std::cout << "!!#contact_nodes: " << m_nodeRigidConstraints[j].size() << '\n';
for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
std::cout << "!!#contact_nodes: " << m_nodeRigidConstraints[i].size() << '\n';
for (int k = 0; k < m_nodeRigidConstraints[i].size(); ++k)
{
btReducedDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
btReducedDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][m_orderContactConstraintPool[k]];
btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
// face vs rigid contact
// for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
// for (int k = 0; k < m_faceRigidConstraints[i].size(); ++k)
// {
// btReducedDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
// btReducedDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][k];
// btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
// residualSquare = btMax(residualSquare, localResidualSquare);
// }
}
}
return residualSquare;
}
@ -318,4 +333,5 @@ void btReducedSoftBodySolver::deformableBodyInternalWriteBack()
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
rsb->applyInternalVelocityChanges();
}
m_ascendOrder = true;
}

View File

@ -10,6 +10,7 @@ class btReducedSoftBody;
class btReducedSoftBodySolver : public btDeformableBodySolver
{
protected:
bool m_ascendOrder;
btScalar m_dampingAlpha;
btScalar m_dampingBeta;