AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 13:20:07 +00:00
Code Issues Releases Wiki Activity

WIP warm starting

Browse Source
This commit is contained in:
jingyuc 2021-08-23 01:17:37 -04:00
parent 368385d5ad
commit d9152b9cf5
9 changed files with 117 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/ReducedDeformableDemo/BasicTest.cpp
View File

@ -183,7 +183,7 @@ void BasicTest::initPhysics()
// rsb->scale(btVector3(1, 1, 1)); //TODO: add back scale
rsb->translate(btVector3(0, 4, 0));
// rsb->setTotalMass(0.5);
rsb->setStiffnessScale(20);
rsb->setStiffnessScale(100);
rsb->setDamping(damping_alpha, damping_beta);
// set fixed nodes
@ -213,7 +213,7 @@ void BasicTest::initPhysics()
getDeformableDynamicsWorld()->setUseProjection(true);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 0;
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
// add a few rigid bodies

16
examples/ReducedDeformableDemo/FreeFall.cpp
View File

@ -30,7 +30,7 @@
// static btScalar E = 50;
// static btScalar nu = 0.3;
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.01;
static btScalar damping_beta = 0.0;
static btScalar COLLIDING_VELOCITY = 0;
static int start_mode = 6;
static int num_modes = 1;
@ -161,9 +161,9 @@ void FreeFall::initPhysics()
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.1);
// rsb->scale(btVector3(1, 1, 1));
rsb->translate(btVector3(0, 10, 0)); //TODO: add back translate and scale
rsb->translate(btVector3(0, 5, 0)); //TODO: add back translate and scale
// rsb->setTotalMass(0.5);
rsb->setStiffnessScale(0.5);
rsb->setStiffnessScale(1);
rsb->setDamping(damping_alpha, damping_beta);
// rsb->setFriction(200);
@ -179,7 +179,7 @@ void FreeFall::initPhysics()
btSoftBodyHelpers::generateBoundaryFaces(rsb);
// rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
// rsb->setRigidVelocity(btVector3(0, 0, 0));
// rsb->setRigidVelocity(btVector3(0, 0, 1));
// rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
// btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
@ -188,14 +188,16 @@ void FreeFall::initPhysics()
}
// create a static rigid box as the ground
{
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(3), btScalar(3), btScalar(3)));
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(5), btScalar(5), btScalar(5)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -50, 0));
// groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
groundTransform.setOrigin(btVector3(0, -2, -2));
// groundTransform.setOrigin(btVector3(0, 0, 6));
// groundTransform.setOrigin(btVector3(0, -50, 0));
{
btScalar mass(0.);
createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));

69
src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp
View File

@ -48,7 +48,16 @@ btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstr
const btContactSolverInfo& infoGlobal,
btScalar dt)
: m_rsb(rsb), m_dt(dt), btDeformableRigidContactConstraint(c, infoGlobal)
{}
{
m_appliedNormalImpulse = 0;
m_appliedTangentImpulse = 0;
m_impulseFactorNormal = 0;
m_impulseFactorTangent = 0;
m_contactNormalA = c.m_cti.m_normal;
m_contactNormalB = -c.m_cti.m_normal;
m_impulseFactorInv = c.m_c0;
}
btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
{
@ -77,7 +86,7 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont
}
// if (!infoGlobal.m_splitImpulse)
// {
// dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
// v_rel_normal += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
// }
// if it's separating, no need to do anything
@ -87,33 +96,36 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont
}
btScalar residualSquare = v_rel_normal * v_rel_normal; // get residual
// compute the tangential relative vel
btVector3 v_rel_tangent = v_rel - v_rel_normal * cti.m_normal;
// // compute the tangential relative vel
// btVector3 v_rel_tangent = v_rel - v_rel_normal * cti.m_normal;
// friction correction
btScalar delta_v_rel_normal = v_rel_normal;
btScalar delta_v_rel_tangent = m_contact->m_c3 * v_rel_normal;
// btScalar delta_v_rel_tangent = 0.3 * v_rel_normal;
// // friction correction
// btScalar delta_v_rel_normal = v_rel_normal;
// btScalar delta_v_rel_tangent = m_contact->m_c3 * v_rel_normal;
// // btScalar delta_v_rel_tangent = 0.3 * v_rel_normal;
btVector3 impulse_tangent(0, 0, 0);
if (v_rel_tangent.norm() < delta_v_rel_tangent)
{
// the object should be static
impulse_tangent = m_contact->m_c0 * (-v_rel_tangent);
}
else
{
// apply friction
impulse_tangent = m_contact->m_c0 * (-v_rel_tangent.safeNormalize() * delta_v_rel_tangent);
std::cout << "friction called\n";
}
// btVector3 impulse_tangent(0, 0, 0);
// if (v_rel_tangent.norm() < delta_v_rel_tangent)
// {
// // the object should be static
// impulse_tangent = m_contact->m_c0 * (-v_rel_tangent);
// }
// else
// {
// // apply friction
// impulse_tangent = m_contact->m_c0 * (-v_rel_tangent.safeNormalize() * delta_v_rel_tangent);
// std::cout << "friction called\n";
// }
// get total impulse
btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * (-v_rel_normal));
btVector3 impulse_normal = m_contact->m_c0 * (-v_rel);
// btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * (-v_rel_normal));
// btVector3 impulse = impulse_normal + impulse_tangent;
btVector3 impulse = impulse_normal.dot(cti.m_normal) * cti.m_normal;
std::cout << "impulse direct: " << impulse[0] / cti.m_normal[0] << '\t' << impulse[1] / cti.m_normal[1]<< '\t' << impulse[2] / cti.m_normal[2] << '\n';
btVector3 impulse_dir = impulse;
impulse_dir.safeNormalize();
std::cout << "impulse direct: " << impulse_dir[0] << '\t' << impulse_dir[1] << '\t' << impulse_dir[2] << '\n';
applyImpulse(impulse);
// applyImpulse(impulse); // TODO: apply impulse?
@ -156,7 +168,16 @@ btReducedDeformableNodeRigidContactConstraint::btReducedDeformableNodeRigidConta
const btContactSolverInfo& infoGlobal,
btScalar dt)
: m_node(contact.m_node), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt)
{}
{
m_relPosA = contact.m_c1;
m_relPosB = m_node->m_x - m_rsb->getRigidTransform().getOrigin();
warmStarting();
}
void btReducedDeformableNodeRigidContactConstraint::warmStarting()
{
//
}
btVector3 btReducedDeformableNodeRigidContactConstraint::getVb() const
{
@ -176,7 +197,7 @@ btVector3 btReducedDeformableNodeRigidContactConstraint::getDv(const btSoftBody:
void btReducedDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse)
{
std::cout << "impulse applied: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n';
m_rsb->applyFullSpaceImpulse(impulse, m_contact->m_c1, m_node->index, m_dt);
m_rsb->applyFullSpaceImpulse(impulse, m_relPosB, m_node->index, m_dt);
m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform());
std::cout << "node: " << m_node->index << " vel: " << m_node->m_v[0] << '\t' << m_node->m_v[1] << '\t' << m_node->m_v[2] << '\n';
// std::cout << "node: " << m_node->index << " m_x: " << m_node->m_x[0] << '\t' << m_node->m_x[1] << '\t' << m_node->m_x[2] << '\n';

17
src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h
View File

@ -34,6 +34,17 @@ class btReducedDeformableRigidContactConstraint : public btDeformableRigidContac
btReducedSoftBody* m_rsb;
btScalar m_dt;
btScalar m_appliedNormalImpulse;
btScalar m_appliedTangentImpulse;
btScalar m_impulseFactorNormal;
btScalar m_impulseFactorTangent;
btVector3 m_contactNormalA; // for rigid body
btVector3 m_contactNormalB; // for reduced deformable body
btVector3 m_relPosA; // relative position of the contact point for A
btVector3 m_relPosB; // relative position of the contact point for B
btMatrix3x3 m_impulseFactorInv; // total inverse impulse matrix
btReducedDeformableRigidContactConstraint(btReducedSoftBody* rsb,
const btSoftBody::DeformableRigidContact& c,
const btContactSolverInfo& infoGlobal,
@ -42,8 +53,12 @@ class btReducedDeformableRigidContactConstraint : public btDeformableRigidContac
btReducedDeformableRigidContactConstraint() {}
virtual ~btReducedDeformableRigidContactConstraint() {}
virtual void warmStarting() {}
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
virtual void applyImpulse(const btVector3& impulse) {}
virtual void applySplitImpulse(const btVector3& impulse) {} // TODO: may need later
};
@ -61,6 +76,8 @@ class btReducedDeformableNodeRigidContactConstraint : public btReducedDeformable
btReducedDeformableNodeRigidContactConstraint() {}
virtual ~btReducedDeformableNodeRigidContactConstraint() {}
virtual void warmStarting();
// get the velocity of the deformable node in contact
virtual btVector3 getVb() const;

21
src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp
View File

@ -1,4 +1,5 @@
#include "btReducedSoftBody.h"
#include "../btSoftBodyInternals.h"
#include "btReducedSoftBodyHelpers.h"
#include "LinearMath/btTransformUtil.h"
#include <iostream>
@ -7,6 +8,8 @@
btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
: btSoftBody(worldInfo, node_count, x, m)
{
m_rigidOnly = true; //! only use rigid frame to debug
// reduced deformable
m_reducedModel = true;
m_startMode = 0;
@ -450,8 +453,7 @@ btMatrix3x3 btReducedSoftBody::getImpulseFactor(int n_node)
btMatrix3x3 K2 = RSARinv + ri_skew * m_interpolateInvInertiaTensorWorld * sum_multiply_A * rotation.transpose();
return K1; //TODO: change back
// return K1 + K2;
return m_rigidOnly ? K1 : K1 + K2;
}
void btReducedSoftBody::applyVelocityConstraint(const btVector3& target_vel, int n_node, btScalar dt)
@ -485,14 +487,17 @@ void btReducedSoftBody::applyPositionConstraint(const btVector3& target_pos, int
void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt)
{
// // apply impulse force
// applyFullSpaceNodalForce(impulse / dt, n_node);
if (!m_rigidOnly)
{
// apply impulse force
applyFullSpaceNodalForce(impulse / dt, n_node);
// // update reduced internal force
// applyReducedDampingForce(m_reducedVelocity);
// update reduced internal force
applyReducedDampingForce(m_reducedVelocity);
// // update reduced velocity
// updateReducedVelocity(dt); // TODO: add back
// update reduced velocity
updateReducedVelocity(dt); // TODO: add back
}
// // update reduced dofs
// updateReducedDofs(dt);

4
src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h
View File

@ -2,7 +2,6 @@
#define BT_REDUCED_SOFT_BODY_H
#include "../btSoftBody.h"
#include "../btSoftBodyInternals.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h"
@ -55,6 +54,8 @@ class btReducedSoftBody : public btSoftBody
public:
bool m_rigidOnly;
//
// Fields
//
@ -80,6 +81,7 @@ class btReducedSoftBody : public btSoftBody
tDenseArray m_nodalMass; // Mass on each node
btAlignedObjectArray<int> m_fixedNodes; // index of the fixed nodes
// contacts
btAlignedObjectArray<int> m_contactNodesList;
//

33
src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp
View File

@ -54,6 +54,12 @@ void btReducedSoftBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody
// m_faceRigidConstraints[i].clear();
}
for (int i = 0; i < m_softBodies.size(); ++i)
{
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
rsb->m_contactNodesList.clear();
}
btDeformableBodySolver::updateSoftBodies();
}
@ -129,12 +135,17 @@ void btReducedSoftBodySolver::applyExplicitForce(btScalar solverdt)
// apply gravity to the rigid frame, get m_linearVelocity at time^*
rsb->applyRigidGravity(m_gravity, solverdt);
// add internal force (elastic force & damping force)
// rsb->applyReducedElasticForce(rsb->m_reducedDofsBuffer);
// rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer);
if (!rsb->m_rigidOnly)
{
// add internal force (elastic force & damping force)
rsb->applyReducedElasticForce(rsb->m_reducedDofsBuffer);
rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer);
// get reduced velocity at time^*
// get reduced velocity at time^*
rsb->updateReducedVelocity(solverdt, true);
// get reduced velocity at time^*
rsb->updateReducedVelocity(solverdt, true);
}
// apply damping (no need at this point)
// rsb->applyDamping(solverdt);
@ -150,12 +161,15 @@ void btReducedSoftBodySolver::applyTransforms(btScalar timeStep)
// rigid motion
rsb->proceedToTransform(timeStep, true);
// update reduced dofs for time^n+1
// rsb->updateReducedDofs(timeStep);
if (!rsb->m_rigidOnly)
{
// update reduced dofs for time^n+1
rsb->updateReducedDofs(timeStep);
// update local moment arm for time^n+1
rsb->updateLocalMomentArm();
rsb->updateExternalForceProjectMatrix(true);
// update local moment arm for time^n+1
rsb->updateLocalMomentArm();
rsb->updateExternalForceProjectMatrix(true);
}
// update mesh nodal positions for time^n+1
rsb->mapToFullPosition(rsb->getRigidTransform());
@ -207,6 +221,7 @@ void btReducedSoftBodySolver::setConstraints(const btContactSolverInfo& infoGlob
m_nodeRigidConstraints[i].push_back(constraint);
rsb->m_contactNodesList.push_back(contact.m_node->index);
}
std::cout << "contact list size: " << rsb->m_contactNodesList.size() << "\n";
std::cout << "#contact nodes: " << m_nodeRigidConstraints[i].size() << "\n";
// set Deformable Face vs. Rigid constraint

3
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
View File

@ -37,7 +37,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
// solver body velocity <- rigid body velocity
writeToSolverBody(bodies, numBodies, infoGlobal);
std::cout << "iter: " << iteration << "\tres: " << m_leastSquaresResidual << '\n';
std::cout << "iter: " << iteration << "\tres: " << m_leastSquaresResidual << '\t';
std::cout << "------------------\n";
// std::cout << infoGlobal.m_leastSquaresResidualThreshold << '\n';
// std::cout << maxIterations << '\n';
@ -59,6 +59,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
break;
}
}
std::cout << "======next step=========\n";
}
return 0.f;
}

4
src/BulletSoftBody/btSoftBodyInternals.h
View File

@ -1696,7 +1696,7 @@ struct btSoftColliders
if (psb->m_reducedModel)
{
c.m_c0 = (psb->getImpulseFactor(n.index)).inverse();
c.m_c1 = n.m_x - psb->getRigidTransform().getOrigin();
// c.m_c1 = n.m_x - psb->getRigidTransform().getOrigin();
}
else
{
@ -1705,8 +1705,8 @@ struct btSoftColliders
c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra);
// c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
c.m_c1 = ra;
}
c.m_c1 = ra;
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{