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

add initial impulse

Browse Source
This commit is contained in:
jingyuc 2021-07-18 00:52:13 -04:00
parent c24f2a72ef
commit 4119d8812e
3 changed files with 58 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
examples/ReducedDeformableDemo/BasicTest.cpp
View File

@ -40,10 +40,10 @@ class BasicTest : public CommonDeformableBodyBase
typedef btAlignedObjectArray<btSoftBody::Node> tNodeArray;
// btDeformableLinearElasticityForce* m_linearElasticity;
btDeformableMassSpringForce* m_massSpring;
// btDeformableMassSpringForce* m_massSpring;
static const unsigned int m_startMode = 6; // actual mode# should +1
static const unsigned int m_nReduced = 1;
static const unsigned int m_nReduced = 2;
static const unsigned int selected_mode = 0;
unsigned int m_nFull;
btScalar sim_time;
@ -55,13 +55,10 @@ class BasicTest : public CommonDeformableBodyBase
btAssert(psb->m_reducedDofs.size() == m_nReduced);
for (int j = 0; j < m_nReduced; ++j)
{
psb->m_reducedDofs[j] = 0;
for (int i = 0; i < m_nFull; ++i)
for (int k = 0; k < 3; ++k)
{
int idx_f = 3 * i + k;
psb->m_reducedDofs[j] = psb->m_modes[j][idx_f] * (psb->m_nodes[i].m_x[k] - psb->m_x0[idx_f]);
// psb->m_reducedVelocity[j] = psb->m_modes[j][idx_f] * psb->m_nodes[i].m_v[k];
}
psb->m_reducedDofs[j] += psb->m_modes[j][3 * i + k] * (psb->m_nodes[i].m_x[k] - psb->m_x0[3 * i + k]);
}
}
@ -69,16 +66,21 @@ class BasicTest : public CommonDeformableBodyBase
void mapToFullDofs(btSoftBody* psb)
{
btAssert(psb->m_nodes.size() == m_nFull);
for (int j = 0; j < m_nReduced; ++j)
btAlignedObjectArray<btVector3> delta_x;
delta_x.resize(m_nFull);
for (int i = 0; i < m_nFull; ++i)
{
for (int i = 0; i < m_nFull; ++i)
for (int k = 0; k < 3; ++k)
{
int idx_f = 3 * i + k;
psb->m_nodes[i].m_x[k] = psb->m_x0[idx_f] + psb->m_modes[j][idx_f] * psb->m_reducedDofs[j];
// psb->m_nodes[i].m_v[k] = psb->m_modes[j][idx_f] * psb->m_reducedVelocity[j];
}
for (int k = 0; k < 3; ++k)
{
// compute displacement
delta_x[i][k] = 0;
for (int j = 0; j < m_nReduced; ++j)
delta_x[i][k] += psb->m_modes[j][3 * i + k] * psb->m_reducedDofs[j];
// get new coordinates
psb->m_nodes[i].m_x[k] = psb->m_x0[3 * i + k] + delta_x[i][k];
}
}
}
// get deformed shape
@ -93,7 +95,6 @@ public:
BasicTest(struct GUIHelperInterface* helper)
: CommonDeformableBodyBase(helper)
{
m_massSpring = nullptr;
m_nFull = 0;
sim_time = 0;
first_step = true;
@ -137,7 +138,7 @@ public:
// TODO: remove this. very hacky way of adding initial deformation
if (first_step && !psb->m_bUpdateRtCst)
{
getDeformedShape(psb, 0, 100);
// getDeformedShape(psb, 0, 0.5);
first_step = false;
mapToReducedDofs(psb);
// std::cout << psb->m_reducedDofs[0] << "\n";
@ -192,7 +193,7 @@ void BasicTest::initPhysics()
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
btVector3 gravity = btVector3(0, 0, 0);
btVector3 gravity = btVector3(0, -10, 0);
m_dynamicsWorld->setGravity(gravity);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
@ -216,56 +217,18 @@ void BasicTest::initPhysics()
std::string modes_file("../../../examples/SoftDemo/modes.bin");
btSoftBodyHelpers::readBinaryModes(psb->m_modes, m_startMode, m_nReduced, 3 * m_nFull, modes_file.c_str()); // default to 3D
// std::string Kr_dense_file("../../../examples/SoftDemo/Kr_dense.bin");
// btSoftBodyHelpers::readBinaryMat(psb->m_KrDense, m_startMode, m_nReduced, 3 * m_nFull, Kr_dense_file.c_str());
// std::string Mr_dense_file("../../../examples/SoftDemo/Mr_dense.bin");
// btSoftBodyHelpers::readBinaryMat(psb->m_MrDense, m_startMode, m_nReduced, 3 * m_nFull, Mr_dense_file.c_str());
// for (int i = 0; i < 3*m_nFull; ++i)
// std::cout << psb->m_modes[0][i] << '\n';
// std::string M_file("../../../examples/SoftDemo/M_diag_mat.bin");
// btSoftBodyHelpers::readBinary(psb->m_M, buffer_n_modes, M_file.c_str());
// get rest position
psb->m_x0.resize(3 * psb->m_nodes.size());
for (int i = 0; i < psb->m_nodes.size(); ++i)
for (int k = 0; k < 3; ++k)
psb->m_x0[3 * i + k] = psb->m_nodes[i].m_x[k];
// btAlignedObjectArray<btAlignedObjectArray<btScalar> > M_red;
// M_red.resize(m_nReduced);
// for (int i = 0; i < m_nReduced; ++i) {
// M_red.resize(m_nReduced);
// }
// for (int i = 0; i < m_nFull; ++i) {
// for (int j = 0; j < m_nReduced; ++j) {
// for (int k = 0; k < m_nFull; ++k) {
// for (int l = 0; l < m_nReduced; ++l) {
// btScalar tmp = psb->m_modes[i][j] * psb->m_M[i] * psb->m_modes[k][l];
// std::cout << tmp << '\n';
// }
// }
// }
// }
// std::ofstream outfile1("before_map.txt");
// for (int i = 0; i < psb->m_nodes.size(); ++i) {
// for (int k = 0; k < 3; ++k)
// outfile1 << psb->m_nodes[i].m_x[k] << '\n';
// }
// outfile1.close();
// mapToFullDofs(psb->m_modes, psb->m_reducedNodes, psb->m_reducedVelocity, psb->m_nodes);
// std::ofstream outfile2("after_map.txt");
// for (int i = 0; i < psb->m_nodes.size(); ++i) {
// for (int k = 0; k < 3; ++k)
// outfile2 << psb->m_nodes[i].m_x[k] << '\n';
// }
// outfile2.close();
std::ofstream outfile1("before_map.txt");
for (int i = 0; i < psb->m_nodes.size(); ++i) {
for (int k = 0; k < 3; ++k)
outfile1 << psb->m_nodes[i].m_x[k] << '\n';
}
outfile1.close();
getDeformableDynamicsWorld()->addSoftBody(psb);
psb->scale(btVector3(2, 2, 2));
@ -281,14 +244,10 @@ void BasicTest::initPhysics()
btSoftBodyHelpers::generateBoundaryFaces(psb);
psb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
// btDeformableLinearElasticityForce* linearElasticity = new btDeformableLinearElasticityForce(100,100,0.01);
// m_linearElasticity = linearElasticity;
m_massSpring = new btDeformableMassSpringForce(10.0, 0.0); //TODO: this stiffness needs to match the modes
getDeformableDynamicsWorld()->addForce(psb, m_massSpring);
m_forces.push_back(m_massSpring);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);

45
src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
View File

@ -151,13 +151,7 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero
{
if (psb->m_reducedModel)
{
// for (int j = 0; j < psb->m_reducedNodes.size(); ++j) // TODO: reduced soft body
// {
// if (psb->m_nodes[j].m_im != 0)
// {
// psb->m_nodes[j].m_v += psb->m_nodes[j].m_effectiveMass_inv * force[counter++];
// }
// }
// TODO: reduced soft body
}
else
{
@ -174,14 +168,41 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero
{
if (psb->m_reducedModel)
{
btScalar scale = 100;
// get reduced force
btAlignedObjectArray<btScalar> reduced_force;
reduced_force.resize(psb->m_reducedDofs.size());
reduced_force.resize(psb->m_reducedDofs.size(), 0);
for (int r = 0; r < psb->m_reducedDofs.size(); ++r) {
reduced_force[r] = 100 * psb->m_Kr[r] * psb->m_reducedDofs[r]; //TODO: increase stiffness
// map all force to reduced
// for (int i = 0; i < force.size(); ++i)
// for (int k = 0; k < 3; ++k)
// reduced_force[r] += scale * psb->m_modes[r][3 * i + k] * force[i][k];
// std::cout << reduced_force[r] << '\t';
// add internal force
reduced_force[r] += scale * psb->m_Kr[r] * psb->m_reducedDofs[r]; //TODO: increase stiffness
// std::cout << reduced_force[r] << '\n';
// std::cout << psb->m_Kr[r] << "\t" << psb->m_reducedDofs[r] << "\n";
}
// apply impulses to reduced deformable objects
static btScalar time_remain = 0.1;
static btScalar target_vel = 1;
if (psb->m_reducedModel && time_remain > 0)
{
btScalar dt = (m_dt < time_remain) ? m_dt : time_remain;
time_remain -= m_dt;
btScalar f_imp = (target_vel - psb->m_nodes[0].m_v[1]) / dt;// TODO: need full mass?
for (int i = 0; i < psb->m_reducedDofs.size(); ++i)
{
reduced_force[i] += psb->m_modes[i][0 * 3 + 1] * f_imp;
}
}
// update reduced velocity
for (int r = 0; r < psb->m_reducedDofs.size(); ++r)
{
@ -189,13 +210,7 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero
btScalar delta_v = m_dt * mass_inv * reduced_force[r];
sim_time += m_dt;
// btScalar delta_v = 1;
// btScalar delta_v = 0.005 * cos(psb->m_eigenvalues[r] * sim_time);
// psb->m_reducedVelocity[r] = sin(psb->m_eigenvalues[r] * sim_time);
psb->m_reducedVelocity[r] -= delta_v;
// std::cout << sim_time << "\t" << psb->m_reducedDofs[r] << "\t" << psb->m_reducedVelocity[r] << "\t" << delta_v << "\t" << reduced_force[r] << "\n";
}
}
else

1
src/BulletSoftBody/btSoftBody.h
View File

@ -867,7 +867,6 @@ public:
btAlignedObjectArray<btScalar> m_Kr; // reduced stiffness matrix
btAlignedObjectArray<btScalar> m_Mr; // reduced mass matrix //TODO: do we need this?
btAlignedObjectArray<btScalar> m_M; // full mass matrix //TODO: maybe don't need this?
//
// Api
//