AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-18 21:10:05 +00:00
Code Issues Releases Wiki Activity

need to update the rigid frame every time when the impulse is applied. reduced external force needs to be zeroed in every iteration.

Browse Source
This commit is contained in:
jingyuc 2021-08-09 11:23:52 -04:00
parent 4f66d8bb87
commit 0acf18f246
4 changed files with 66 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
examples/ReducedDeformableDemo/BasicTest.cpp
View File

@ -106,8 +106,8 @@ public:
// // rsb->mapToReducedDofs();
// }
float internalTimeStep = 1. / 60.f;
// float internalTimeStep = 1e-4;
// float internalTimeStep = 1. / 60.f;
float internalTimeStep = 1e-3;
m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
}
@ -185,7 +185,7 @@ void BasicTest::initPhysics()
// rsb->scale(btVector3(1, 1, 1));
rsb->translate(btVector3(0, 4, 0)); //TODO: add back translate and scale
// rsb->setTotalMass(0.5);
rsb->setStiffnessScale(1);
rsb->setStiffnessScale(100);
rsb->setFixedNodes();
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
rsb->m_cfg.kCHR = 1; // collision hardness with rigid body

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

@ -2,6 +2,9 @@
#include "btReducedSoftBodyHelpers.h"
#include "LinearMath/btTransformUtil.h"
#include <iostream>
#include <fstream>
static int counter = 0;
btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
: btSoftBody(worldInfo, node_count, x, m)
@ -38,7 +41,8 @@ void btReducedSoftBody::setReducedModes(int start_mode, int num_modes, int full_
m_nFull = full_size;
m_reducedDofs.resize(m_nReduced, 0);
m_reducedVelocity.resize(m_nReduced, 0);
m_reducedForce.resize(m_nReduced, 0);
m_reducedForceInternal.resize(m_nReduced, 0);
m_reducedForceExternal.resize(m_nReduced, 0);
m_nodalMass.resize(full_size, 0);
m_localMomentArm.resize(m_nFull);
}
@ -222,14 +226,15 @@ void btReducedSoftBody::endOfTimeStepZeroing()
{
for (int i = 0; i < m_nReduced; ++i)
{
m_reducedForce[i] = 0;
m_reducedForceInternal[i] = 0;
m_reducedForceExternal[i] = 0;
}
std::cout << "zeroed!\n";
}
void btReducedSoftBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform)
void btReducedSoftBody::predictIntegratedTransform(btScalar dt, btTransform& predictedTransform)
{
btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform);
btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, dt, predictedTransform);
}
void btReducedSoftBody::updateReducedDofs(btScalar solverdt)
@ -258,7 +263,7 @@ void btReducedSoftBody::updateReducedVelocity(btScalar solverdt)
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];
btScalar delta_v = solverdt * mass_inv * (m_reducedForceInternal[r] + m_reducedForceExternal[r]);
// std::cout << mass_inv << '\t' << delta_v << '\t' << m_reducedForce[r] << '\n';
m_reducedVelocity[r] += delta_v;
}
@ -287,27 +292,22 @@ void btReducedSoftBody::mapToFullVelocity(const btTransform& ref_trans)
ref_trans.getBasis() * v_from_reduced[i] +
m_linearVelocity;
}
std::cout << m_nodes[0].m_v[0] << '\t' << m_nodes[0].m_v[1] << '\t' << m_nodes[0].m_v[2] << '\n';
}
void btReducedSoftBody::proceedToTransform(const btTransform& newTrans)
void btReducedSoftBody::proceedToTransform(btScalar dt, bool end_of_time_step)
{
setCenterOfMassTransform(newTrans);
}
void btReducedSoftBody::setCenterOfMassTransform(const btTransform& xform)
{
if (isKinematicObject())
{
m_interpolationWorldTransform = m_rigidTransformWorld;
}
else
{
m_interpolationWorldTransform = xform;
}
m_interpolationLinearVelocity = getLinearVelocity(); // TODO: check where these are used?
m_interpolationAngularVelocity = getAngularVelocity();
m_rigidTransformWorld = xform;
updateInertiaTensor();
if (end_of_time_step)
{
m_rigidTransformWorld = m_interpolationWorldTransform;
updateInertiaTensor();
}
else
{
btTransformUtil::integrateTransform(m_interpolationWorldTransform, m_linearVelocity, m_angularVelocity, dt, m_interpolationWorldTransform);
}
// m_interpolationLinearVelocity = getLinearVelocity(); // TODO: check where these are used?
// m_interpolationAngularVelocity = getAngularVelocity();
}
void btReducedSoftBody::translate(const btVector3& trs)
@ -432,7 +432,23 @@ void btReducedSoftBody::applyVelocityConstraint(const btVector3& target_vel, int
btMatrix3x3 impulse_factor = K1 + K2; //TODO: add back
// btMatrix3x3 impulse_factor = K1;
btVector3 impulse = impulse_factor.inverse() * (target_vel - m_nodes[n_node].m_v);
btScalar impulse_magnitude = impulse.norm();
std::cout << "impulse: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n';
std::cout << "impulse magnitude: " << impulse.norm() << '\n';
std::cout << "vel: " << m_nodes[n_node].m_v[0] << '\t' << m_nodes[n_node].m_v[1] << '\t' << m_nodes[n_node].m_v[2] << '\n';
std::cout << "vel magnitude: " << m_nodes[n_node].m_v.norm() << '\n';
if (impulse_magnitude < 5e-7)
{
impulse.setZero();
impulse_magnitude = 0;
}
std::ofstream myfile("impulse_"+std::to_string(counter)+".txt", std::ios_base::app);
myfile << impulse_magnitude << '\n';
myfile.close();
// apply full space impulse
applyFullSpaceImpulse(impulse, ri, n_node, dt);
@ -472,7 +488,7 @@ void btReducedSoftBody::applyFullSpaceNodalForce(const btVector3& f_ext, int n_n
{
// f_local = R^-1 * f_ext
btVector3 f_local = m_rigidTransformWorld.getBasis().transpose() * f_ext;
std::cout << "f_ext: " << f_ext[0] << '\t' << f_ext[1] << '\t' << f_ext[2] << '\n';
// std::cout << "f_ext: " << f_ext[0] << '\t' << f_ext[1] << '\t' << f_ext[2] << '\n';
// f_scaled = localInvInertia * (r_k x f_local)
// btVector3 rk_cross_f_local = m_localMomentArm[n_node].cross(f_local);
@ -487,7 +503,8 @@ void btReducedSoftBody::applyFullSpaceNodalForce(const btVector3& f_ext, int n_n
f_ext_r.resize(m_nReduced, 0);
for (int r = 0; r < m_nReduced; ++r)
{
std::cout << "reduced_f before: " << m_reducedForce[r] << '\n';
m_reducedForceExternal[r] = 0;
// std::cout << "reduced_f before: " << m_reducedForce[r] << '\n';
for (int k = 0; k < 3; ++k)
{
f_ext_r[r] += (m_projPA[r][3 * n_node + k] + m_projCq[r][3 * n_node + k]) * f_local[k];
@ -496,12 +513,12 @@ void btReducedSoftBody::applyFullSpaceNodalForce(const btVector3& f_ext, int n_n
// std::cout << "projPA: " << m_projPA[r][0] << '\t' << m_projPA[r][1] << '\t' << m_projPA[r][2] << '\n';
// std::cout << "projCq: " << m_projCq[r][0] << '\t' << m_projCq[r][1] << '\t' << m_projCq[r][2] << '\n';
m_reducedForce[r] += f_ext_r[r];
std::cout << "f_ext_r: " << f_ext_r[r] << '\n';
std::cout << "reduced_f after: " << m_reducedForce[r] << '\n';
m_reducedForceExternal[r] += f_ext_r[r];
// std::cout << "f_ext_r: " << f_ext_r[r] << '\n';
// std::cout << "reduced_f after: " << m_reducedForce[r] << '\n';
}
// std::cout << "reduced_f: " << m_reducedForce[0] << '\n';
std::cout << "gravity: " << m_mass * 10 << '\n';
// std::cout << "gravity: " << m_mass * 10 << '\n';
}
void btReducedSoftBody::applyRigidGravity(const btVector3& gravity, btScalar dt)
@ -514,20 +531,24 @@ void btReducedSoftBody::applyReducedInternalForce(const btScalar damping_alpha,
{
for (int r = 0; r < m_nReduced; ++r)
{
m_reducedForce[r] = - m_ksScale * m_Kr[r] * (m_reducedDofs[r] + damping_beta * m_reducedVelocity[r]);
m_reducedForceInternal[r] = - m_ksScale * m_Kr[r] * (m_reducedDofs[r] + damping_beta * m_reducedVelocity[r]);
}
}
void btReducedSoftBody::applyFixedContraints(btScalar dt)
{
for (int iter = 0; iter < 50; ++iter)
for (int iter = 0; iter < 200; ++iter)
{
// std::cout << "iteration: " << iter << '\n';
// btVector3 vel_error(0, 0, 0);
for (int n = 0; n < m_fixedNodes.size(); ++n)
{
{
// apply impulse, velocity constraint
applyVelocityConstraint(btVector3(0, 0, 0), m_fixedNodes[n], dt);
// update predicted basis
proceedToTransform(dt, false); // TODO: maybe don't need?
// update full space velocity
mapToFullVelocity(getInterpolationWorldTransform());
@ -547,4 +568,8 @@ void btReducedSoftBody::applyFixedContraints(btScalar dt)
// break;
// }
}
// std::cin.get();
if (counter == 2)
exit(1);
counter++;
}

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

@ -63,7 +63,8 @@ class btReducedSoftBody : public btSoftBody
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_reducedVelocity; // Reduced velocity array
tDenseArray m_reducedForce; // reduced force
tDenseArray m_reducedForceExternal; // reduced external force
tDenseArray m_reducedForceInternal; // reduced internal force
tDenseArray m_eigenvalues; // eigenvalues of the reduce deformable model
tDenseArray m_Kr; // reduced stiffness matrix
tDenseArray m_Mr; // reduced mass matrix //TODO: do we need this?
@ -112,7 +113,7 @@ class btReducedSoftBody : public btSoftBody
void updateLocalMomentArm();
void predictIntegratedTransform(btScalar step, btTransform& predictedTransform);
void predictIntegratedTransform(btScalar dt, btTransform& predictedTransform);
// update the external force projection matrix
void updateExternalForceProjectMatrix(bool initialized);
@ -144,9 +145,7 @@ class btReducedSoftBody : public btSoftBody
void applyTorqueImpulse(const btVector3& torque);
void proceedToTransform(const btTransform& newTrans);
void setCenterOfMassTransform(const btTransform& xform);
void proceedToTransform(btScalar dt, bool end_of_time_step);
//
// force related

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

@ -98,7 +98,8 @@ void btReducedSoftBodySolver::applyTransforms(btScalar timeStep)
// rigid motion
// btTransform predictedTrans;
// rsb->predictIntegratedTransform(timeStep, predictedTrans);
rsb->proceedToTransform(rsb->getInterpolationWorldTransform());
// rsb->proceedToTransform(rsb->getInterpolationWorldTransform());
rsb->proceedToTransform(timeStep, true);
// update mesh nodal positions for the next time step
rsb->mapToFullDofs(rsb->getRigidTransform());