mirror of
https://github.com/bulletphysics/bullet3.git
synced 2026-11-18 13:46:05 +00:00
add support for damping. the fixed constraint is working again with the deltaImpulse form
This commit is contained in:
@@ -30,7 +30,7 @@
|
||||
// static btScalar E = 50;
|
||||
// static btScalar nu = 0.3;
|
||||
static btScalar damping_alpha = 0.0;
|
||||
static btScalar damping_beta = 0.0;
|
||||
static btScalar damping_beta = 0.01;
|
||||
static btScalar COLLIDING_VELOCITY = 0;
|
||||
static int start_mode = 6;
|
||||
static int num_modes = 1;
|
||||
@@ -134,16 +134,15 @@ public:
|
||||
void stepSimulation(float deltaTime)
|
||||
{
|
||||
// TODO: remove this. very hacky way of adding initial deformation
|
||||
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(static_cast<btDeformableMultiBodyDynamicsWorld*>(m_dynamicsWorld)->getSoftBodyArray()[0]);
|
||||
if (first_step /* && !rsb->m_bUpdateRtCst*/)
|
||||
{
|
||||
getDeformedShape(rsb, 0, 1);
|
||||
first_step = false;
|
||||
// rsb->mapToReducedDofs();
|
||||
}
|
||||
// btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(static_cast<btDeformableMultiBodyDynamicsWorld*>(m_dynamicsWorld)->getSoftBodyArray()[0]);
|
||||
// if (first_step /* && !rsb->m_bUpdateRtCst*/)
|
||||
// {
|
||||
// getDeformedShape(rsb, 0, 1);
|
||||
// first_step = false;
|
||||
// // rsb->mapToReducedDofs();
|
||||
// }
|
||||
|
||||
float internalTimeStep = 1. / 60.f;
|
||||
// float internalTimeStep = 1e-3;
|
||||
m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
|
||||
|
||||
// sim_time += internalTimeStep;
|
||||
@@ -198,7 +197,7 @@ void BasicTest::initPhysics()
|
||||
|
||||
m_broadphase = new btDbvtBroadphase();
|
||||
btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver();
|
||||
btVector3 gravity = btVector3(0, 0, 0);
|
||||
btVector3 gravity = btVector3(0, -10, 0);
|
||||
reducedSoftBodySolver->setGravity(gravity);
|
||||
|
||||
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
|
||||
@@ -224,18 +223,17 @@ void BasicTest::initPhysics()
|
||||
btTransform init_transform;
|
||||
init_transform.setIdentity();
|
||||
init_transform.setOrigin(btVector3(0, 4, 0));
|
||||
init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
|
||||
// init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
|
||||
rsb->transform(init_transform);
|
||||
|
||||
// rsb->setTotalMass(0.5);
|
||||
rsb->setStiffnessScale(100);
|
||||
rsb->setDamping(damping_alpha, damping_beta);
|
||||
|
||||
// set fixed nodes
|
||||
// rsb->setFixedNodes(0);
|
||||
// rsb->setFixedNodes(1);
|
||||
// rsb->setFixedNodes(2);
|
||||
// rsb->setFixedNodes(3);
|
||||
rsb->setFixedNodes(0);
|
||||
rsb->setFixedNodes(1);
|
||||
rsb->setFixedNodes(2);
|
||||
rsb->setFixedNodes(3);
|
||||
|
||||
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
|
||||
rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
|
||||
@@ -247,7 +245,7 @@ void BasicTest::initPhysics()
|
||||
|
||||
// rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
|
||||
// rsb->setRigidVelocity(btVector3(0, 1, 0));
|
||||
rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
|
||||
// rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
|
||||
|
||||
// btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
|
||||
// getDeformableDynamicsWorld()->addForce(rsb, gravity_force);
|
||||
@@ -255,11 +253,11 @@ void BasicTest::initPhysics()
|
||||
}
|
||||
getDeformableDynamicsWorld()->setImplicit(false);
|
||||
getDeformableDynamicsWorld()->setLineSearch(false);
|
||||
getDeformableDynamicsWorld()->setUseProjection(true);
|
||||
getDeformableDynamicsWorld()->setUseProjection(false);
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3;
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true;
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
|
||||
// add a few rigid bodies
|
||||
// Ctor_RbUpStack(); // TODO: no rigid body for now
|
||||
|
||||
@@ -30,7 +30,7 @@
|
||||
// static btScalar E = 50;
|
||||
// static btScalar nu = 0.3;
|
||||
static btScalar damping_alpha = 0.0;
|
||||
static btScalar damping_beta = 0.0;
|
||||
static btScalar damping_beta = 0.01;
|
||||
static btScalar COLLIDING_VELOCITY = 0;
|
||||
static int start_mode = 6;
|
||||
static int num_modes = 10;
|
||||
@@ -157,11 +157,11 @@ void FreeFall::initPhysics()
|
||||
|
||||
btTransform init_transform;
|
||||
init_transform.setIdentity();
|
||||
// init_transform.setOrigin(btVector3(0, 2.5, 0));
|
||||
init_transform.setOrigin(btVector3(0, 10, 0));
|
||||
// init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0));
|
||||
// init_transform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 6.0));
|
||||
// init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
|
||||
// init_transform.setRotation(btQuaternion(SIMD_PI / 2.0, 0, 0));
|
||||
// init_transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
|
||||
init_transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 2.0));
|
||||
rsb->transform(init_transform);
|
||||
|
||||
// rsb->setTotalMass(0.5);
|
||||
@@ -275,7 +275,7 @@ void FreeFall::initPhysics()
|
||||
getDeformableDynamicsWorld()->setLineSearch(false);
|
||||
getDeformableDynamicsWorld()->setUseProjection(false);
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0;
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.3;
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
|
||||
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
|
||||
|
||||
@@ -30,10 +30,10 @@
|
||||
// static btScalar E = 50;
|
||||
// static btScalar nu = 0.3;
|
||||
static btScalar damping_alpha = 0.0;
|
||||
static btScalar damping_beta = 0.0;
|
||||
static btScalar damping_beta = 0.01;
|
||||
static btScalar COLLIDING_VELOCITY = 4;
|
||||
static int start_mode = 6;
|
||||
static int num_modes = 1;
|
||||
static int num_modes = 10;
|
||||
|
||||
class ReducedGrasp : public CommonDeformableBodyBase
|
||||
{
|
||||
@@ -287,8 +287,8 @@ void ReducedGrasp::initPhysics()
|
||||
btTransform init_transform;
|
||||
init_transform.setIdentity();
|
||||
init_transform.setOrigin(btVector3(0, 4, 0));
|
||||
// init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0));
|
||||
init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
|
||||
init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0));
|
||||
// init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
|
||||
rsb->transform(init_transform);
|
||||
|
||||
rsb->setStiffnessScale(200);
|
||||
|
||||
@@ -11,33 +11,38 @@ btReducedDeformableStaticConstraint::btReducedDeformableStaticConstraint(
|
||||
: m_rsb(rsb), m_ri(ri), m_dt(dt), btDeformableStaticConstraint(node, infoGlobal)
|
||||
{
|
||||
// get impulse
|
||||
m_impulseFactor = rsb->getImpulseFactor(m_node->index);
|
||||
m_impulseFactorInv = rsb->getImpulseFactor(m_node->index).inverse();
|
||||
}
|
||||
|
||||
btScalar btReducedDeformableStaticConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
// target velocity of fixed constraint is 0
|
||||
btVector3 impulse = -(m_impulseFactor.inverse() * m_node->m_v);
|
||||
|
||||
// apply full space impulse
|
||||
std::cout << "node: " << m_node->index << " impulse: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n';
|
||||
// std::cout << "impulse norm: " << impulse.norm() << "\n";
|
||||
|
||||
m_rsb->applyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt);
|
||||
|
||||
// get residual //TODO: only calculate the velocity of the given node
|
||||
m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform());
|
||||
btVector3 deltaVa = getDeltaVa();
|
||||
btVector3 rel_vel = m_node->m_v + deltaVa;
|
||||
btVector3 deltaImpulse = -(m_impulseFactorInv * rel_vel);
|
||||
applyImpulse(deltaImpulse);
|
||||
|
||||
// calculate residual
|
||||
btScalar residualSquare = btDot(m_node->m_v, m_node->m_v);
|
||||
btScalar residualSquare = btDot(rel_vel, rel_vel);
|
||||
|
||||
return residualSquare;
|
||||
}
|
||||
|
||||
// this calls reduced deformable body's applyFullSpaceImpulse
|
||||
// this calls reduced deformable body's internalApplyFullSpaceImpulse
|
||||
void btReducedDeformableStaticConstraint::applyImpulse(const btVector3& impulse)
|
||||
{
|
||||
m_rsb->applyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt);
|
||||
// apply full space impulse
|
||||
std::cout << "node: " << m_node->index << " impulse: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n';
|
||||
m_rsb->internalApplyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt);
|
||||
|
||||
// get the new nodal velocity
|
||||
// m_node->m_v = m_rsb->computeNodeFullVelocity(m_rsb->getInterpolationWorldTransform(), m_node->index);
|
||||
// m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform());
|
||||
}
|
||||
|
||||
btVector3 btReducedDeformableStaticConstraint::getDeltaVa() const
|
||||
{
|
||||
return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_node->index);
|
||||
}
|
||||
|
||||
// ================= base contact constraints ===================
|
||||
|
||||
@@ -7,7 +7,7 @@ class btReducedDeformableStaticConstraint : public btDeformableStaticConstraint
|
||||
public:
|
||||
btReducedSoftBody* m_rsb;
|
||||
btScalar m_dt;
|
||||
btMatrix3x3 m_impulseFactor;
|
||||
btMatrix3x3 m_impulseFactorInv;
|
||||
btVector3 m_ri;
|
||||
|
||||
btReducedDeformableStaticConstraint(btReducedSoftBody* rsb,
|
||||
@@ -24,6 +24,8 @@ class btReducedDeformableStaticConstraint : public btDeformableStaticConstraint
|
||||
// this calls reduced deformable body's applyFullSpaceImpulse
|
||||
virtual void applyImpulse(const btVector3& impulse);
|
||||
|
||||
btVector3 getDeltaVa() const;
|
||||
|
||||
// virtual void applySplitImpulse(const btVector3& impulse) {}
|
||||
};
|
||||
|
||||
|
||||
@@ -272,7 +272,7 @@ void btReducedSoftBody::updateReducedVelocity(btScalar solverdt, bool explicit_f
|
||||
btScalar delta_v = 0;
|
||||
if (explicit_force)
|
||||
{
|
||||
delta_v = solverdt * mass_inv * m_reducedForceElastic[r];
|
||||
delta_v = solverdt * mass_inv * (m_reducedForceElastic[r] + m_reducedForceDamping[r]);
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -571,8 +571,8 @@ void btReducedSoftBody::internalApplyFullSpaceImpulse(const btVector3& impulse,
|
||||
// apply impulse force
|
||||
applyFullSpaceNodalForce(impulse / dt, n_node);
|
||||
|
||||
// update reduced internal force
|
||||
applyReducedDampingForce(m_reducedVelocity); //TODO: this needs to be the current velocity
|
||||
// update delta damping force
|
||||
applyReducedDampingForce(m_internalDeltaReducedVelocity);
|
||||
|
||||
// delta reduced velocity
|
||||
for (int r = 0; r < m_nReduced; ++r)
|
||||
|
||||
@@ -128,7 +128,6 @@ void btReducedSoftBodySolver::predictReduceDeformableMotion(btScalar solverdt)
|
||||
|
||||
void btReducedSoftBodySolver::applyExplicitForce(btScalar solverdt)
|
||||
{
|
||||
static bool applied = false;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
|
||||
@@ -143,8 +142,6 @@ void btReducedSoftBodySolver::applyExplicitForce(btScalar solverdt)
|
||||
rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer);
|
||||
|
||||
// get reduced velocity at time^*
|
||||
// get reduced velocity at time^*
|
||||
// get reduced velocity at time^*
|
||||
rsb->updateReducedVelocity(solverdt, true);
|
||||
}
|
||||
|
||||
@@ -273,27 +270,12 @@ btScalar btReducedSoftBodySolver::solveContactConstraints(btCollisionObject** de
|
||||
{
|
||||
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
|
||||
|
||||
btAlignedObjectArray<btScalar> residual;
|
||||
residual.resize(m_staticConstraints[i].size(), 0);
|
||||
|
||||
for (int k = 0; k < m_staticConstraints[i].size(); ++k)
|
||||
{
|
||||
btReducedDeformableStaticConstraint& constraint = m_staticConstraints[i][k];
|
||||
btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
|
||||
residualSquare = btMax(residualSquare, localResidualSquare);
|
||||
|
||||
btVector3 error;
|
||||
error.setZero();
|
||||
std::cout << "fixed_nodes: ";
|
||||
for (int p = 0; p < rsb->m_fixedNodes.size(); ++p)
|
||||
{
|
||||
std::cout << rsb->m_nodes[rsb->m_fixedNodes[p]].m_v.norm() << '\t';
|
||||
error += rsb->m_nodes[rsb->m_fixedNodes[p]].m_v;
|
||||
}
|
||||
std::cout << '\n';
|
||||
std::cout << "norm: " << error.norm() << "\n";
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// handle contact constraint
|
||||
|
||||
Reference in New Issue
Block a user