mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 21:30:09 +00:00
re-work the simulation time loop in order to support fixed constraints. still wip
This commit is contained in:
parent
5d17353269
commit
a37d23fa24
@ -95,9 +95,9 @@ public:
|
|||||||
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(static_cast<btDeformableMultiBodyDynamicsWorld*>(m_dynamicsWorld)->getSoftBodyArray()[0]);
|
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(static_cast<btDeformableMultiBodyDynamicsWorld*>(m_dynamicsWorld)->getSoftBodyArray()[0]);
|
||||||
if (first_step /* && !rsb->m_bUpdateRtCst*/)
|
if (first_step /* && !rsb->m_bUpdateRtCst*/)
|
||||||
{
|
{
|
||||||
// getDeformedShape(rsb, 0, 0.5);
|
getDeformedShape(rsb, 0, 0.5);
|
||||||
first_step = false;
|
first_step = false;
|
||||||
rsb->updateReducedDofs();
|
rsb->mapToReducedDofs();
|
||||||
}
|
}
|
||||||
|
|
||||||
float internalTimeStep = 1. / 60.f;
|
float internalTimeStep = 1. / 60.f;
|
||||||
@ -138,7 +138,7 @@ void BasicTest::initPhysics()
|
|||||||
m_broadphase = new btDbvtBroadphase();
|
m_broadphase = new btDbvtBroadphase();
|
||||||
btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver();
|
btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver();
|
||||||
reducedSoftBodySolver->setDamping(damping_alpha, damping_beta);
|
reducedSoftBodySolver->setDamping(damping_alpha, damping_beta);
|
||||||
btVector3 gravity = btVector3(0, -9.8, 0);
|
btVector3 gravity = btVector3(0, 0, 0);
|
||||||
reducedSoftBodySolver->setGravity(gravity);
|
reducedSoftBodySolver->setGravity(gravity);
|
||||||
|
|
||||||
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
|
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
|
||||||
@ -164,7 +164,7 @@ void BasicTest::initPhysics()
|
|||||||
rsb->translate(btVector3(0, 4, 0)); //TODO: add back translate and scale
|
rsb->translate(btVector3(0, 4, 0)); //TODO: add back translate and scale
|
||||||
// rsb->setTotalMass(0.5);
|
// rsb->setTotalMass(0.5);
|
||||||
rsb->setStiffnessScale(1);
|
rsb->setStiffnessScale(1);
|
||||||
// rsb->setFixedNodes();
|
rsb->setFixedNodes();
|
||||||
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
|
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
|
||||||
rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
|
rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
|
||||||
rsb->m_cfg.kDF = 0;
|
rsb->m_cfg.kDF = 0;
|
||||||
@ -175,7 +175,7 @@ void BasicTest::initPhysics()
|
|||||||
|
|
||||||
// rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
|
// rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
|
||||||
// rsb->setRigidVelocity(btVector3(0, 1, 0));
|
// rsb->setRigidVelocity(btVector3(0, 1, 0));
|
||||||
rsb->setRigidAngularVelocity(btVector3(10, 0, 0));
|
// rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
|
||||||
|
|
||||||
// btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
|
// btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
|
||||||
// getDeformableDynamicsWorld()->addForce(rsb, gravity_force);
|
// getDeformableDynamicsWorld()->addForce(rsb, gravity_force);
|
||||||
|
@ -34,7 +34,9 @@ void btReducedSoftBody::setReducedModes(int start_mode, int num_modes, int full_
|
|||||||
m_nFull = full_size;
|
m_nFull = full_size;
|
||||||
m_reducedDofs.resize(m_nReduced, 0);
|
m_reducedDofs.resize(m_nReduced, 0);
|
||||||
m_reducedVelocity.resize(m_nReduced, 0);
|
m_reducedVelocity.resize(m_nReduced, 0);
|
||||||
|
m_reducedForce.resize(m_nReduced, 0);
|
||||||
m_nodalMass.resize(full_size, 0);
|
m_nodalMass.resize(full_size, 0);
|
||||||
|
endOfTimeStepZeroing();
|
||||||
}
|
}
|
||||||
|
|
||||||
void btReducedSoftBody::setMassProps(const tDenseArray& mass_array)
|
void btReducedSoftBody::setMassProps(const tDenseArray& mass_array)
|
||||||
@ -95,12 +97,20 @@ void btReducedSoftBody::setFixedNodes()
|
|||||||
m_fixedNodes.push_back(0);
|
m_fixedNodes.push_back(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void btReducedSoftBody::endOfTimeStepZeroing()
|
||||||
|
{
|
||||||
|
for (int i = 0; i < m_nReduced; ++i)
|
||||||
|
{
|
||||||
|
m_reducedForce[i] = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void btReducedSoftBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform)
|
void btReducedSoftBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform)
|
||||||
{
|
{
|
||||||
btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform);
|
btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform);
|
||||||
}
|
}
|
||||||
|
|
||||||
void btReducedSoftBody::updateReducedDofs()
|
void btReducedSoftBody::mapToReducedDofs()
|
||||||
{
|
{
|
||||||
btAssert(m_reducedDofs.size() == m_nReduced);
|
btAssert(m_reducedDofs.size() == m_nReduced);
|
||||||
for (int j = 0; j < m_nReduced; ++j)
|
for (int j = 0; j < m_nReduced; ++j)
|
||||||
@ -117,7 +127,23 @@ void btReducedSoftBody::updateReducedDofs()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void btReducedSoftBody::updateFullDofs()
|
void btReducedSoftBody::updateReducedDofs(btScalar solverdt)
|
||||||
|
{
|
||||||
|
for (int r = 0; r < m_nReduced; ++r)
|
||||||
|
{
|
||||||
|
m_reducedDofs[r] += solverdt * m_reducedVelocity[r];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void btReducedSoftBody::updateFullDofs(btScalar solverdt)
|
||||||
|
{
|
||||||
|
for (int i = 0; i < m_nFull; ++i)
|
||||||
|
{
|
||||||
|
m_nodes[i].m_x += solverdt * m_nodes[i].m_v;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void btReducedSoftBody::mapToFullDofs()
|
||||||
{
|
{
|
||||||
btAssert(m_nodes.size() == m_nFull);
|
btAssert(m_nodes.size() == m_nFull);
|
||||||
TVStack delta_x;
|
TVStack delta_x;
|
||||||
@ -141,6 +167,51 @@ void btReducedSoftBody::updateFullDofs()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void btReducedSoftBody::updateReducedVelocity(btScalar solverdt)
|
||||||
|
{
|
||||||
|
// update reduced velocity
|
||||||
|
for (int r = 0; r < m_nReduced; ++r)
|
||||||
|
{
|
||||||
|
btScalar mass_inv = (m_Mr[r] == 0) ? 0 : 1.0 / m_Mr[r]; // TODO: this might be redundant, because Mr is identity
|
||||||
|
btScalar delta_v = solverdt * mass_inv * m_reducedForce[r];
|
||||||
|
m_reducedVelocity[r] -= delta_v; //! check this
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void btReducedSoftBody::updateFullVelocity(btScalar solverdt)
|
||||||
|
{
|
||||||
|
TVStack v_from_reduced;
|
||||||
|
v_from_reduced.resize(m_nFull);
|
||||||
|
// btVector3 current_com = m_rigidTransformWorld.getOrigin() - m_initialOrigin;
|
||||||
|
for (int i = 0; i < m_nFull; ++i)
|
||||||
|
{
|
||||||
|
btVector3 r = m_nodes[i].m_x - m_rigidTransformWorld.getOrigin();
|
||||||
|
|
||||||
|
// compute velocity contributed by the reduced velocity
|
||||||
|
for (int k = 0; k < 3; ++k)
|
||||||
|
{
|
||||||
|
v_from_reduced[i][k] = 0;
|
||||||
|
for (int r = 0; r < m_nReduced; ++r)
|
||||||
|
{
|
||||||
|
v_from_reduced[i][k] += m_modes[r][3 * i + k] * m_reducedVelocity[r];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// get new velocity
|
||||||
|
m_nodes[i].m_v = m_angularVelocity.cross(r) +
|
||||||
|
m_rigidTransformWorld.getBasis() * v_from_reduced[i] +
|
||||||
|
m_linearVelocity;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void btReducedSoftBody::updateMeshNodePositions(btScalar solverdt)
|
||||||
|
{
|
||||||
|
for (int i = 0; i < m_nFull; ++i)
|
||||||
|
{
|
||||||
|
m_nodes[i].m_x = solverdt * m_nodes[i].m_v;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void btReducedSoftBody::proceedToTransform(const btTransform& newTrans)
|
void btReducedSoftBody::proceedToTransform(const btTransform& newTrans)
|
||||||
{
|
{
|
||||||
setCenterOfMassTransform(newTrans);
|
setCenterOfMassTransform(newTrans);
|
||||||
@ -215,19 +286,29 @@ void btReducedSoftBody::applyImpulse(const btVector3& impulse, const btVector3&
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt, tDenseArray& reduced_force)
|
void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt)
|
||||||
{
|
{
|
||||||
|
// zero the reduced force vector
|
||||||
|
endOfTimeStepZeroing();
|
||||||
|
|
||||||
// impulse leads to the deformation in the reduced space
|
// impulse leads to the deformation in the reduced space
|
||||||
btVector3 impulse = m_nodalMass[n_node] / dt * (target_vel - m_nodes[n_node].m_v);
|
btVector3 impulse = m_nodalMass[n_node] / dt * (target_vel - m_nodes[n_node].m_v);
|
||||||
for (int i = 0; i < m_nReduced; ++i)
|
for (int i = 0; i < m_nReduced; ++i)
|
||||||
{
|
{
|
||||||
for (int k = 0; k < 3; ++k)
|
for (int k = 0; k < 3; ++k)
|
||||||
{
|
{
|
||||||
reduced_force[i] += m_modes[i][3 * n_node + k] * impulse[k];
|
m_reducedForce[i] = m_modes[i][3 * n_node + k] * impulse[k];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// impulse causes rigid motion
|
// impulse causes rigid motion
|
||||||
applyImpulse(impulse, m_nodes[n_node].m_x);
|
applyImpulse(impulse, m_nodes[n_node].m_x);
|
||||||
|
|
||||||
|
// apply impulse to the reduced velocity
|
||||||
|
updateReducedVelocity(dt);
|
||||||
|
|
||||||
|
// apply impulse force, update velocity
|
||||||
|
updateFullVelocity(dt);
|
||||||
}
|
}
|
||||||
|
|
||||||
void btReducedSoftBody::applyRigidGravity(const btVector3& gravity, btScalar dt)
|
void btReducedSoftBody::applyRigidGravity(const btVector3& gravity, btScalar dt)
|
||||||
@ -235,27 +316,27 @@ void btReducedSoftBody::applyRigidGravity(const btVector3& gravity, btScalar dt)
|
|||||||
// update rigid frame velocity
|
// update rigid frame velocity
|
||||||
m_linearVelocity += dt * gravity;
|
m_linearVelocity += dt * gravity;
|
||||||
|
|
||||||
// update nodal velocity
|
// // update nodal velocity
|
||||||
for (int i = 0; i < m_nFull; ++i)
|
// for (int i = 0; i < m_nFull; ++i)
|
||||||
{
|
// {
|
||||||
m_nodes[i].m_v = m_nodes[i].m_v + dt * gravity;
|
// m_nodes[i].m_v += dt * gravity;
|
||||||
}
|
// }
|
||||||
}
|
}
|
||||||
|
|
||||||
void btReducedSoftBody::applyReducedInternalForce(tDenseArray& reduced_force, const btScalar damping_alpha, const btScalar damping_beta)
|
void btReducedSoftBody::applyReducedInternalForce(const btScalar damping_alpha, const btScalar damping_beta)
|
||||||
{
|
{
|
||||||
for (int r = 0; r < m_nReduced; ++r)
|
for (int r = 0; r < m_nReduced; ++r)
|
||||||
{
|
{
|
||||||
reduced_force[r] += m_ksScale * m_Kr[r] * (m_reducedDofs[r] + damping_beta * m_reducedVelocity[r]);
|
m_reducedForce[r] += m_ksScale * m_Kr[r] * (m_reducedDofs[r] + damping_beta * m_reducedVelocity[r]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void btReducedSoftBody::applyFixedContraints(btScalar dt, tDenseArray& reduced_force)
|
void btReducedSoftBody::applyFixedContraints(btScalar dt)
|
||||||
{
|
{
|
||||||
for (int n = 0; n < m_fixedNodes.size(); ++n)
|
for (int n = 0; n < m_fixedNodes.size(); ++n)
|
||||||
{
|
{
|
||||||
// std::cout << reduced_force[0] << "\t" << reduced_force[1] << "\n";
|
// std::cout << reduced_force[0] << "\t" << reduced_force[1] << "\n";
|
||||||
applyFullSpaceImpulse(btVector3(0, 0, 0), m_fixedNodes[n], dt, reduced_force);
|
applyFullSpaceImpulse(btVector3(0, 0, 0), m_fixedNodes[n], dt);
|
||||||
// std::cout << reduced_force[0] << "\t" << reduced_force[1] << "\n";
|
// std::cout << reduced_force[0] << "\t" << reduced_force[1] << "\n";
|
||||||
}
|
}
|
||||||
}
|
}
|
@ -49,6 +49,7 @@ class btReducedSoftBody : public btSoftBody
|
|||||||
tDenseMatrix m_modes; // modes of the reduced deformable model. Each inner array is a mode, outer array size = n_modes
|
tDenseMatrix m_modes; // modes of the reduced deformable model. Each inner array is a mode, outer array size = n_modes
|
||||||
tDenseArray m_reducedDofs; // Reduced degree of freedom
|
tDenseArray m_reducedDofs; // Reduced degree of freedom
|
||||||
tDenseArray m_reducedVelocity; // Reduced velocity array
|
tDenseArray m_reducedVelocity; // Reduced velocity array
|
||||||
|
tDenseArray m_reducedForce; // reduced force
|
||||||
tDenseArray m_eigenvalues; // eigenvalues of the reduce deformable model
|
tDenseArray m_eigenvalues; // eigenvalues of the reduce deformable model
|
||||||
tDenseArray m_Kr; // reduced stiffness matrix
|
tDenseArray m_Kr; // reduced stiffness matrix
|
||||||
tDenseArray m_Mr; // reduced mass matrix //TODO: do we need this?
|
tDenseArray m_Mr; // reduced mass matrix //TODO: do we need this?
|
||||||
@ -89,15 +90,32 @@ class btReducedSoftBody : public btSoftBody
|
|||||||
|
|
||||||
void predictIntegratedTransform(btScalar step, btTransform& predictedTransform);
|
void predictIntegratedTransform(btScalar step, btTransform& predictedTransform);
|
||||||
|
|
||||||
|
void endOfTimeStepZeroing();
|
||||||
|
|
||||||
//
|
//
|
||||||
// reduced dof related
|
// reduced dof related
|
||||||
//
|
//
|
||||||
|
|
||||||
// compute reduced degree of freedoms
|
// compute reduced degree of freedoms
|
||||||
void updateReducedDofs();
|
void updateReducedDofs(btScalar solverdt);
|
||||||
|
|
||||||
|
// map to reduced degree of freedoms
|
||||||
|
void mapToReducedDofs();
|
||||||
|
|
||||||
// compute full degree of freedoms
|
// compute full degree of freedoms
|
||||||
void updateFullDofs();
|
void updateFullDofs(btScalar solverdt);
|
||||||
|
|
||||||
|
// map to full degree of freedoms
|
||||||
|
void mapToFullDofs();
|
||||||
|
|
||||||
|
// compute reduced velocity update
|
||||||
|
void updateReducedVelocity(btScalar solverdt);
|
||||||
|
|
||||||
|
// compute full space velocity from the reduced velocity
|
||||||
|
void updateFullVelocity(btScalar solverdt);
|
||||||
|
|
||||||
|
// update the full space mesh positions
|
||||||
|
void updateMeshNodePositions(btScalar solverdt);
|
||||||
|
|
||||||
//
|
//
|
||||||
// rigid motion related
|
// rigid motion related
|
||||||
@ -110,16 +128,16 @@ class btReducedSoftBody : public btSoftBody
|
|||||||
void applyImpulse(const btVector3& impulse, const btVector3& rel_pos);
|
void applyImpulse(const btVector3& impulse, const btVector3& rel_pos);
|
||||||
|
|
||||||
// apply impulse to nodes in the full space
|
// apply impulse to nodes in the full space
|
||||||
void applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt, tDenseArray& reduced_force);
|
void applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt);
|
||||||
|
|
||||||
// apply fixed contraints to the nodes
|
// apply fixed contraints to the nodes
|
||||||
void applyFixedContraints(btScalar dt, tDenseArray& reduced_force);
|
void applyFixedContraints(btScalar dt);
|
||||||
|
|
||||||
// apply gravity to the rigid frame
|
// apply gravity to the rigid frame
|
||||||
void applyRigidGravity(const btVector3& gravity, btScalar dt);
|
void applyRigidGravity(const btVector3& gravity, btScalar dt);
|
||||||
|
|
||||||
// apply reduced force
|
// apply reduced force
|
||||||
void applyReducedInternalForce(tDenseArray& reduced_force, const btScalar damping_alpha, const btScalar damping_beta);
|
void applyReducedInternalForce(const btScalar damping_alpha, const btScalar damping_beta);
|
||||||
|
|
||||||
void proceedToTransform(const btTransform& newTrans);
|
void proceedToTransform(const btTransform& newTrans);
|
||||||
|
|
||||||
|
@ -21,93 +21,70 @@ void btReducedSoftBodySolver::setGravity(const btVector3& gravity)
|
|||||||
|
|
||||||
void btReducedSoftBodySolver::predictMotion(btScalar solverdt)
|
void btReducedSoftBodySolver::predictMotion(btScalar solverdt)
|
||||||
{
|
{
|
||||||
applyExplicitForce();
|
applyExplicitForce(solverdt);
|
||||||
|
|
||||||
|
// predict new mesh location
|
||||||
|
predictReduceDeformableMotion(solverdt);
|
||||||
}
|
}
|
||||||
|
|
||||||
void btReducedSoftBodySolver::applyForce()
|
void btReducedSoftBodySolver::predictReduceDeformableMotion(btScalar solverdt)
|
||||||
{
|
{
|
||||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||||
{
|
{
|
||||||
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
|
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
|
||||||
|
|
||||||
// get reduced force
|
rsb->updateReducedVelocity(solverdt);
|
||||||
btAlignedObjectArray<btScalar> reduced_force;
|
|
||||||
reduced_force.resize(rsb->m_reducedDofs.size(), 0);
|
// map reduced velocity to full space velocity
|
||||||
|
rsb->updateFullVelocity(solverdt);
|
||||||
|
|
||||||
|
// update mesh nodal position
|
||||||
|
// rsb->updateMeshNodePositions(solverdt);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void btReducedSoftBodySolver::applyExplicitForce(btScalar solverdt)
|
||||||
|
{
|
||||||
|
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||||
|
{
|
||||||
|
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
|
||||||
|
|
||||||
|
// apply gravity to the rigid frame
|
||||||
|
rsb->applyRigidGravity(m_gravity, solverdt);
|
||||||
|
|
||||||
// add internal force (elastic force & damping force)
|
// add internal force (elastic force & damping force)
|
||||||
rsb->applyReducedInternalForce(reduced_force, m_dampingAlpha, m_dampingBeta);
|
rsb->applyReducedInternalForce(m_dampingAlpha, m_dampingBeta);
|
||||||
|
|
||||||
// apply impulses to reduced deformable objects
|
|
||||||
static btScalar sim_time = 0;
|
|
||||||
static int apply_impulse = 0;
|
|
||||||
// if (rsb->m_reducedModel && apply_impulse < 4)
|
|
||||||
// {
|
|
||||||
// if (sim_time > 1 && apply_impulse == 0)
|
|
||||||
// {
|
|
||||||
// rsb->applyFullSpaceImpulse(btVector3(0, 1, 0), 0, 2.0 * m_dt, reduced_force);
|
|
||||||
// apply_impulse++;
|
|
||||||
// }
|
|
||||||
// if (sim_time > 2 && apply_impulse == 1)
|
|
||||||
// {
|
|
||||||
// rsb->applyFullSpaceImpulse(btVector3(0, -1.2, 0), 0, m_dt, reduced_force);
|
|
||||||
// apply_impulse++;
|
|
||||||
// }
|
|
||||||
// if (sim_time > 3 && apply_impulse == 2)
|
|
||||||
// {
|
|
||||||
// rsb->applyFullSpaceImpulse(btVector3(1.1, 0, 0), 0, m_dt, reduced_force);
|
|
||||||
// apply_impulse++;
|
|
||||||
// }
|
|
||||||
// if (sim_time > 4 && apply_impulse == 3)
|
|
||||||
// {
|
|
||||||
// rsb->applyFullSpaceImpulse(btVector3(-1, 0, 0), 0, 2.0 * m_dt, reduced_force);
|
|
||||||
// apply_impulse++;
|
|
||||||
// }
|
|
||||||
// }
|
|
||||||
|
|
||||||
// apply fixed contraints
|
|
||||||
// rsb->applyFixedContraints(m_dt, reduced_force); //TODO: solver iteratively with other constraints
|
|
||||||
|
|
||||||
// update reduced velocity
|
|
||||||
for (int r = 0; r < rsb->m_reducedDofs.size(); ++r)
|
|
||||||
{
|
|
||||||
btScalar mass_inv = (rsb->m_Mr[r] == 0) ? 0 : 1.0 / rsb->m_Mr[r];
|
|
||||||
btScalar delta_v = m_dt * mass_inv * reduced_force[r];
|
|
||||||
rsb->m_reducedVelocity[r] -= delta_v;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void btReducedSoftBodySolver::applyExplicitForce()
|
|
||||||
{
|
|
||||||
// apply gravity to the rigid frame
|
|
||||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
|
||||||
{
|
|
||||||
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
|
|
||||||
rsb->applyRigidGravity(m_gravity, m_dt);
|
|
||||||
}
|
|
||||||
|
|
||||||
// apply internal forces and impulses
|
|
||||||
applyForce();
|
|
||||||
}
|
|
||||||
|
|
||||||
void btReducedSoftBodySolver::applyTransforms(btScalar timeStep)
|
void btReducedSoftBodySolver::applyTransforms(btScalar timeStep)
|
||||||
{
|
{
|
||||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||||
{
|
{
|
||||||
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
|
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
|
||||||
|
|
||||||
for (int r = 0; r < rsb->m_reducedDofs.size(); ++r)
|
// update reduced dofs for the next time step
|
||||||
rsb->m_reducedDofs[r] += timeStep * rsb->m_reducedVelocity[r];
|
rsb->updateReducedDofs(timeStep);
|
||||||
|
|
||||||
// rigid motion
|
// rigid motion
|
||||||
btTransform predictedTrans;
|
btTransform predictedTrans;
|
||||||
rsb->predictIntegratedTransform(timeStep, predictedTrans);
|
rsb->predictIntegratedTransform(timeStep, predictedTrans);
|
||||||
// std::cout << predictedTrans.getOrigin()[0] << '\t' << predictedTrans.getOrigin()[1] << '\t' << predictedTrans.getOrigin()[2] << '\n';
|
|
||||||
rsb->proceedToTransform(predictedTrans);
|
rsb->proceedToTransform(predictedTrans);
|
||||||
// std::cout << rsb->getWorldTransform().getOrigin()[0] << '\t' << rsb->getWorldTransform().getOrigin()[1] << '\t' << rsb->getWorldTransform().getOrigin()[2] << '\n';
|
|
||||||
// std::cout << "----------\n";
|
|
||||||
|
|
||||||
// map reduced dof back to full space
|
// update mesh nodal positions for the next time step
|
||||||
rsb->updateFullDofs();
|
rsb->updateFullDofs(timeStep);
|
||||||
|
|
||||||
|
// end of time step clean up
|
||||||
|
rsb->endOfTimeStepZeroing();
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void btReducedSoftBodySolver::solveConstraints(btScalar timeStep)
|
||||||
|
{
|
||||||
|
// for (int i = 0; i < m_softBodies.size(); ++i)
|
||||||
|
// {
|
||||||
|
// btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
|
||||||
|
|
||||||
|
// rsb->applyFixedContraints(timeStep);
|
||||||
|
// }
|
||||||
}
|
}
|
@ -14,7 +14,9 @@ class btReducedSoftBodySolver : public btDeformableBodySolver
|
|||||||
|
|
||||||
btVector3 m_gravity;
|
btVector3 m_gravity;
|
||||||
|
|
||||||
void applyForce();
|
void predictReduceDeformableMotion(btScalar solverdt);
|
||||||
|
|
||||||
|
void applyExplicitForce(btScalar solverdt);
|
||||||
|
|
||||||
public:
|
public:
|
||||||
btReducedSoftBodySolver();
|
btReducedSoftBodySolver();
|
||||||
@ -31,10 +33,17 @@ class btReducedSoftBodySolver : public btDeformableBodySolver
|
|||||||
|
|
||||||
virtual void predictMotion(btScalar solverdt);
|
virtual void predictMotion(btScalar solverdt);
|
||||||
|
|
||||||
virtual void applyExplicitForce();
|
|
||||||
|
|
||||||
virtual void applyTransforms(btScalar timeStep);
|
virtual void applyTransforms(btScalar timeStep);
|
||||||
|
|
||||||
|
virtual void solveConstraints(btScalar timeStep);
|
||||||
|
|
||||||
|
// virtual void setProjection() {}
|
||||||
|
|
||||||
|
// virtual void setLagrangeMultiplier() {}
|
||||||
|
|
||||||
|
// virtual void setupDeformableSolve(bool implicit);
|
||||||
|
|
||||||
|
// virtual void solveDeformableConstraints(btScalar solverdt);
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif // BT_REDUCED_SOFT_BODY_DYNAMICS_WORLD_H
|
#endif // BT_REDUCED_SOFT_BODY_DYNAMICS_WORLD_H
|
@ -85,7 +85,7 @@ public:
|
|||||||
void backupVelocity();
|
void backupVelocity();
|
||||||
|
|
||||||
// set m_dv and m_backupVelocity to desired value to prepare for momentum solve
|
// set m_dv and m_backupVelocity to desired value to prepare for momentum solve
|
||||||
void setupDeformableSolve(bool implicit);
|
virtual void setupDeformableSolve(bool implicit);
|
||||||
|
|
||||||
// set the current velocity to that backed up in m_backupVelocity
|
// set the current velocity to that backed up in m_backupVelocity
|
||||||
void revertVelocity();
|
void revertVelocity();
|
||||||
@ -199,9 +199,11 @@ public:
|
|||||||
m_objective->m_projection.setLagrangeMultiplier();
|
m_objective->m_projection.setLagrangeMultiplier();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
virtual void solveConstraints(btScalar timeStep) {}
|
||||||
|
|
||||||
// unused functions
|
// unused functions
|
||||||
virtual void optimize(btAlignedObjectArray<btSoftBody*>& softBodies, bool forceUpdate = false) {}
|
virtual void optimize(btAlignedObjectArray<btSoftBody*>& softBodies, bool forceUpdate = false) {}
|
||||||
virtual void solveConstraints(btScalar dt) {}
|
// virtual void solveConstraints(btScalar dt) {}
|
||||||
virtual bool checkInitialized() { return true; }
|
virtual bool checkInitialized() { return true; }
|
||||||
virtual void copyBackToSoftBodies(bool bMove = true) {}
|
virtual void copyBackToSoftBodies(bool bMove = true) {}
|
||||||
};
|
};
|
||||||
|
@ -302,30 +302,33 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
|
|||||||
|
|
||||||
void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
|
void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
|
||||||
{
|
{
|
||||||
BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints");
|
// BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints");
|
||||||
// save v_{n+1}^* velocity after explicit forces
|
// // save v_{n+1}^* velocity after explicit forces
|
||||||
m_deformableBodySolver->backupVelocity();
|
// m_deformableBodySolver->backupVelocity();
|
||||||
|
|
||||||
// set up constraints among multibodies and between multibodies and deformable bodies
|
// // set up constraints among multibodies and between multibodies and deformable bodies
|
||||||
setupConstraints();
|
// setupConstraints();
|
||||||
|
|
||||||
// solve contact constraints
|
// // solve contact constraints
|
||||||
solveContactConstraints();
|
// solveContactConstraints();
|
||||||
|
|
||||||
// set up the directions in which the velocity does not change in the momentum solve
|
// // set up the directions in which the velocity does not change in the momentum solve
|
||||||
if (m_useProjection)
|
// if (m_useProjection)
|
||||||
m_deformableBodySolver->setProjection();
|
// m_deformableBodySolver->setProjection();
|
||||||
else
|
// else
|
||||||
m_deformableBodySolver->setLagrangeMultiplier();
|
// m_deformableBodySolver->setLagrangeMultiplier();
|
||||||
|
|
||||||
// for explicit scheme, m_backupVelocity = v_{n+1}^*
|
// // for explicit scheme, m_backupVelocity = v_{n+1}^*
|
||||||
// for implicit scheme, m_backupVelocity = v_n
|
// // for implicit scheme, m_backupVelocity = v_n
|
||||||
// Here, set dv = v_{n+1} - v_n for nodes in contact
|
// // Here, set dv = v_{n+1} - v_n for nodes in contact
|
||||||
m_deformableBodySolver->setupDeformableSolve(m_implicit);
|
// m_deformableBodySolver->setupDeformableSolve(m_implicit);
|
||||||
|
|
||||||
// At this point, dv should be golden for nodes in contact
|
// // At this point, dv should be golden for nodes in contact
|
||||||
// proceed to solve deformable momentum equation
|
// // proceed to solve deformable momentum equation
|
||||||
m_deformableBodySolver->solveDeformableConstraints(timeStep);
|
// m_deformableBodySolver->solveDeformableConstraints(timeStep);
|
||||||
|
|
||||||
|
// TODO: need better design
|
||||||
|
m_deformableBodySolver->solveConstraints(timeStep);
|
||||||
}
|
}
|
||||||
|
|
||||||
void btDeformableMultiBodyDynamicsWorld::setupConstraints()
|
void btDeformableMultiBodyDynamicsWorld::setupConstraints()
|
||||||
|
Loading…
Reference in New Issue
Block a user