mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 21:30:09 +00:00
support applying impulse in alternating order. change several example setups.
This commit is contained in:
parent
354c5a6e62
commit
1bee1ba4c4
@ -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;
|
||||
|
@ -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);
|
||||
|
@ -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;
|
||||
|
@ -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;
|
||||
|
@ -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;
|
||||
}
|
@ -10,6 +10,7 @@ class btReducedSoftBody;
|
||||
class btReducedSoftBodySolver : public btDeformableBodySolver
|
||||
{
|
||||
protected:
|
||||
bool m_ascendOrder;
|
||||
btScalar m_dampingAlpha;
|
||||
btScalar m_dampingBeta;
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user