Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-06-08 08:13:55 +00:00
Code Issues Packages Projects Releases Wiki Activity

re-write the fixed constraint. the drift is smaller, but still need some improvements

Browse Source
This commit is contained in:
jingyuc
2021-10-21 23:14:12 -04:00
parent d945e48af2
commit 3a3d8c7a3a
7 changed files with 83 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
examples/ReducedDeformableDemo/ConservationTest.cpp
View File

@@ -22,14 +22,15 @@
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include <stdio.h> //printf debugging
#include <random>
#include "../CommonInterfaces/CommonDeformableBodyBase.h"
#include "../Utils/b3ResourcePath.h"
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.001;
static btScalar damping_beta = 0.0;
static int start_mode = 6;
static int num_modes = 1;
static int num_modes = 20;
class ConservationTest : public CommonDeformableBodyBase
{
@@ -42,13 +43,21 @@ class ConservationTest : public CommonDeformableBodyBase
// for (int i = 0; i < rsb->m_nodes.size(); ++i)
// for (int k = 0; k < 3; ++k)
// rsb->m_nodes[i].m_x[k] += rsb->m_modes[mode_n][3 * i + k] * scale;
rsb->m_reducedDofs[mode_n] = scale;
rsb->m_reducedDofsBuffer[mode_n] = scale;
// srand(1);
// for (int r = 0; r < rsb->m_nReduced; r++)
// {
// rsb->m_reducedDofs[r] = btScalar(rand()) / btScalar(RAND_MAX) - 0.5;
// rsb->m_reducedDofsBuffer[r] = rsb->m_reducedDofs[r];
// }
rsb->mapToFullPosition(rsb->getRigidTransform());
std::cout << "-----------\n";
std::cout << rsb->m_nodes[0].m_x[0] << '\t' << rsb->m_nodes[0].m_x[1] << '\t' << rsb->m_nodes[0].m_x[2] << '\n';
std::cout << "-----------\n";
// std::cout << "-----------\n";
// std::cout << rsb->m_nodes[0].m_x[0] << '\t' << rsb->m_nodes[0].m_x[1] << '\t' << rsb->m_nodes[0].m_x[2] << '\n';
// std::cout << "-----------\n";
}
public:
@@ -114,6 +123,19 @@ public:
myfile << sim_time << "\t" << total_angular[0] << "\t" << total_angular[1] << "\t" << total_angular[2] << "\n";
myfile.close();
}
{
btVector3 angular_rigid(0, 0, 0);
std::ofstream myfile("angular_momentum_rigid.txt", std::ios_base::app);
btVector3 ri(0, 0, 0);
for (int i = 0; i < rsb->m_nFull; ++i)
{
ri = rsb->m_nodes[i].m_x - x_com;
btMatrix3x3 ri_star = Cross(ri);
angular_rigid += rsb->m_nodalMass[i] * (ri_star.transpose() * ri_star * rsb->getAngularVelocity());
}
myfile << sim_time << "\t" << angular_rigid[0] << "\t" << angular_rigid[1] << "\t" << angular_rigid[2] << "\n";
myfile.close();
}
{
std::ofstream myfile("reduced_velocity.txt", std::ios_base::app);
@@ -193,7 +215,7 @@ void ConservationTest::initPhysics()
// create volumetric reduced deformable body
{
btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedCube(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes);
btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedBeam(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.1);
@@ -217,7 +239,7 @@ void ConservationTest::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));
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);

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

@@ -6,24 +6,37 @@ btReducedDeformableStaticConstraint::btReducedDeformableStaticConstraint(
btReducedSoftBody* rsb,
btSoftBody::Node* node,
const btVector3& ri,
const btVector3& x0,
const btContactSolverInfo& infoGlobal,
btScalar dt)
: m_rsb(rsb), m_ri(ri), m_dt(dt), btDeformableStaticConstraint(node, infoGlobal)
: m_rsb(rsb), m_ri(ri), m_x0(x0), m_dt(dt), btDeformableStaticConstraint(node, infoGlobal)
{
// get impulse
m_impulseFactorInv = rsb->getImpulseFactor(m_node->index).inverse();
btVector3 vel_error = -m_node->m_v;
btVector3 pos_error = m_x0 - m_node->m_x;
std::cout << "pos_errors: " << pos_error[0] << "\t" << pos_error[1] << "\t" << pos_error[2] << "\n";
m_appliedImpulse = btVector3(0, 0, 0);
m_rhs = m_impulseFactorInv * (vel_error + 0.2 * pos_error / m_dt);
}
btScalar btReducedDeformableStaticConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
{
// target velocity of fixed constraint is 0
btVector3 deltaVa = getDeltaVa();
btVector3 rel_vel = m_node->m_v + deltaVa;
btVector3 deltaImpulse = -(m_impulseFactorInv * rel_vel);
btVector3 rel_vel = deltaVa;
btVector3 deltaImpulse = m_rhs - m_impulseFactorInv * rel_vel;
m_appliedImpulse = m_appliedImpulse + deltaImpulse;
applyImpulse(deltaImpulse);
btVector3 deltaV = m_rsb->getImpulseFactor(m_node->index) * deltaImpulse;
// calculate residual
btScalar residualSquare = btDot(rel_vel, rel_vel);
btScalar residualSquare = btDot(deltaV, deltaV);
std::cout << "residualSquare: " << residualSquare << "\n";
return residualSquare;
}

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

@@ -9,10 +9,14 @@ class btReducedDeformableStaticConstraint : public btDeformableStaticConstraint
btScalar m_dt;
btMatrix3x3 m_impulseFactorInv;
btVector3 m_ri;
btVector3 m_x0;
btVector3 m_rhs;
btVector3 m_appliedImpulse;
btReducedDeformableStaticConstraint(btReducedSoftBody* rsb,
btSoftBody::Node* node,
const btVector3& ri,
const btVector3& x0,
const btContactSolverInfo& infoGlobal,
btScalar dt);
// btReducedDeformableStaticConstraint(const btReducedDeformableStaticConstraint& other);

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

@@ -286,33 +286,36 @@ void btReducedSoftBody::mapToFullVelocity(const btTransform& ref_trans)
btVector3 sum_angular(0, 0, 0);
m_linearVelocityFromReduced.setZero();
m_angularVelocityFromReduced.setZero();
// for (int i = 0; i < m_nFull; ++i)
// {
// btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[i];
// btMatrix3x3 r_star = Cross(r_com);
for (int i = 0; i < m_nFull; ++i)
{
btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[i];
btMatrix3x3 r_star = Cross(r_com);
// btVector3 v_from_reduced(0, 0, 0);
// for (int k = 0; k < 3; ++k)
// {
// for (int r = 0; r < m_nReduced; ++r)
// {
// v_from_reduced[k] += m_modes[r][3 * i + k] * m_reducedVelocity[r];
// }
// }
btVector3 v_from_reduced(0, 0, 0);
for (int k = 0; k < 3; ++k)
{
for (int r = 0; r < m_nReduced; ++r)
{
v_from_reduced[k] += m_modes[r][3 * i + k] * m_reducedVelocity[r];
}
}
// btVector3 delta_linear = m_nodalMass[i] * v_from_reduced;
// btVector3 delta_angular = m_nodalMass[i] * (r_star * ref_trans.getBasis() * v_from_reduced);
// sum_linear += delta_linear;
// sum_angular += delta_angular;
// std::cout << "delta_linear: " << delta_linear[0] << "\t" << delta_linear[1] << "\t" << delta_linear[2] << "\n";
// std::cout << "delta_angular: " << delta_angular[0] << "\t" << delta_angular[1] << "\t" << delta_angular[2] << "\n";
// std::cout << "sum_linear: " << sum_linear[0] << "\t" << sum_linear[1] << "\t" << sum_linear[2] << "\n";
// std::cout << "sum_angular: " << sum_angular[0] << "\t" << sum_angular[1] << "\t" << sum_angular[2] << "\n";
// }
// m_linearVelocityFromReduced = 1.0 / m_mass * (ref_trans.getBasis() * sum_linear);
// m_angularVelocityFromReduced = m_interpolateInvInertiaTensorWorld * sum_angular;
btVector3 delta_linear = m_nodalMass[i] * v_from_reduced;
btVector3 delta_angular = m_nodalMass[i] * (r_star * ref_trans.getBasis() * v_from_reduced);
sum_linear += delta_linear;
sum_angular += delta_angular;
// std::cout << "delta_linear: " << delta_linear[0] << "\t" << delta_linear[1] << "\t" << delta_linear[2] << "\n";
// std::cout << "delta_angular: " << delta_angular[0] << "\t" << delta_angular[1] << "\t" << delta_angular[2] << "\n";
// std::cout << "sum_linear: " << sum_linear[0] << "\t" << sum_linear[1] << "\t" << sum_linear[2] << "\n";
// std::cout << "sum_angular: " << sum_angular[0] << "\t" << sum_angular[1] << "\t" << sum_angular[2] << "\n";
}
m_linearVelocityFromReduced = 1.0 / m_mass * (ref_trans.getBasis() * sum_linear);
m_angularVelocityFromReduced = m_interpolateInvInertiaTensorWorld * sum_angular;
// std::cout << "-----end here------\n";
// m_linearVelocity -= m_linearVelocityFromReduced;
// m_angularVelocity -= m_angularVelocityFromReduced;
for (int i = 0; i < m_nFull; ++i)
{
m_nodes[i].m_v = computeNodeFullVelocity(ref_trans, i);
@@ -332,9 +335,9 @@ const btVector3 btReducedSoftBody::computeNodeFullVelocity(const btTransform& re
}
}
// get new velocity
btVector3 vel = (m_angularVelocity - m_angularVelocityFromReduced).cross(r_com) +
btVector3 vel = m_angularVelocity.cross(r_com) +
ref_trans.getBasis() * v_from_reduced +
m_linearVelocity - m_linearVelocityFromReduced;
m_linearVelocity;
return vel;
}

2
src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.h
View File

@@ -15,7 +15,7 @@ struct btReducedSoftBodyHelpers
// read in geometry info from Vtk file
static btReducedSoftBody* createFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file);
// read in all reduced files
static void readReducedDeformableInfoFromFiles(btReducedSoftBody* rsb, const char* file_path, const btVector3& half_extents);
static void readReducedDeformableInfoFromFiles(btReducedSoftBody* rsb, const char* file_path, const btVector3& half_extents = btVector3(0, 0, 0));
// read in a binary vector
static void readBinary(btReducedSoftBody::tDenseArray& vec, const unsigned int n_start, const unsigned int n_modes, const unsigned int n_full, const char* file);
// read in a binary matrix

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

@@ -201,7 +201,7 @@ void btReducedSoftBodySolver::setConstraints(const btContactSolverInfo& infoGlob
int i_node = rsb->m_fixedNodes[j];
if (rsb->m_nodes[i_node].m_im == 0)
{
btReducedDeformableStaticConstraint static_constraint(rsb, &rsb->m_nodes[i_node], rsb->getRelativePos(i_node), infoGlobal, m_dt);
btReducedDeformableStaticConstraint static_constraint(rsb, &rsb->m_nodes[i_node], rsb->getRelativePos(i_node), rsb->m_x0[i_node], infoGlobal, m_dt);
m_staticConstraints[i].push_back(static_constraint);
}
}

2
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
View File

@@ -29,6 +29,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
for (int iteration = 0; iteration < maxIterations; iteration++)
{
std::cout << "iter: " << iteration << " -------------\n";
// rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body
// solve rigid/rigid in solver body
@@ -60,6 +61,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
break;
}
}
std::cout << "===========new step============\n";
}
return 0.f;
}