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

add a function to compute the total angular momentum

Browse Source
This commit is contained in:
jingyuc
2021-10-28 14:12:19 -04:00
parent 3540400a9d
commit 2255e26e14
5 changed files with 95 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
examples/ReducedDeformableDemo/ConservationTest.cpp
View File

@@ -30,7 +30,7 @@
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0;
static int start_mode = 6;
static int num_modes = 20;
static int num_modes = 78;
class ConservationTest : public CommonDeformableBodyBase
{
@@ -44,15 +44,15 @@ class ConservationTest : public CommonDeformableBodyBase
// 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;
// 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];
// }
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";
@@ -114,12 +114,13 @@ public:
}
{
std::ofstream myfile("angular_momentum.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;
total_angular += rsb->m_nodalMass[i] * ri.cross(rsb->m_nodes[i].m_v);
}
// btVector3 ri(0, 0, 0);
// for (int i = 0; i < rsb->m_nFull; ++i)
// {
// ri = rsb->m_nodes[i].m_x - x_com;
// total_angular += rsb->m_nodalMass[i] * ri.cross(rsb->m_nodes[i].m_v);
// }
total_angular = rsb->computeTotalAngularMomentum();
myfile << sim_time << "\t" << total_angular[0] << "\t" << total_angular[1] << "\t" << total_angular[2] << "\n";
myfile.close();
}
@@ -154,8 +155,8 @@ public:
first_step = false;
}
float internalTimeStep = 1. / 60.f;
m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
sim_time += internalTimeStep;
checkMomentum(rsb);
@@ -239,7 +240,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);

12
examples/ReducedDeformableDemo/ModeVisualizer.cpp
View File

@@ -28,7 +28,7 @@
static int start_mode = 6;
static int num_modes = 40;
static int num_modes = 78;
static btScalar visualize_mode = 0;
static btScalar frequency_scale = 1;
@@ -92,10 +92,10 @@ public:
int n_mode = floor(visualize_mode);
btScalar scale = sin(sqrt(rsb->m_eigenvalues[n_mode]) * sim_time / frequency_scale);
getDeformedShape(rsb, n_mode, scale);
btVector3 mass_weighted_column_sum = computeMassWeightedColumnSum(rsb, visualize_mode);
std::cout << "mode=" << int(visualize_mode) << "\t" << mass_weighted_column_sum[0] << "\t"
<< mass_weighted_column_sum[1] << "\t"
<< mass_weighted_column_sum[2] << "\n";
// btVector3 mass_weighted_column_sum = computeMassWeightedColumnSum(rsb, visualize_mode);
// std::cout << "mode=" << int(visualize_mode) << "\t" << mass_weighted_column_sum[0] << "\t"
// << mass_weighted_column_sum[1] << "\t"
// << mass_weighted_column_sum[2] << "\n";
}
virtual void renderScene()
@@ -136,7 +136,7 @@ void ModeVisualizer::initPhysics()
// create volumetric soft 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);

34
examples/ReducedDeformableDemo/Springboard.cpp
View File

@@ -84,11 +84,11 @@ public:
sim_time += internalTimeStep;
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(getDeformableDynamicsWorld()->getSoftBodyArray()[0]);
std::ofstream myfile("fixed_node.txt", std::ios_base::app);
myfile << sim_time << "\t" << rsb->m_nodes[0].m_x[0] - rsb->m_x0[0][0] << "\t"
<< rsb->m_nodes[0].m_x[1] - rsb->m_x0[0][1] << "\t"
<< rsb->m_nodes[0].m_x[2] - rsb->m_x0[0][2] << "\n";
myfile.close();
// std::ofstream myfile("fixed_node.txt", std::ios_base::app);
// myfile << sim_time << "\t" << rsb->m_nodes[0].m_x[0] - rsb->m_x0[0][0] << "\t"
// << rsb->m_nodes[0].m_x[1] - rsb->m_x0[0][1] << "\t"
// << rsb->m_nodes[0].m_x[2] - rsb->m_x0[0][2] << "\n";
// myfile.close();
}
}
@@ -187,19 +187,19 @@ void Springboard::initPhysics()
Ctor_RbUpStack();
// create a static rigid box as the ground
// {
// // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10), btScalar(2), btScalar(10)));
// m_collisionShapes.push_back(groundShape);
{
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10), btScalar(2), btScalar(10)));
m_collisionShapes.push_back(groundShape);
// btTransform groundTransform;
// groundTransform.setIdentity();
// groundTransform.setOrigin(btVector3(0, 0, 0));
// {
// btScalar mass(0.);
// createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
// }
// }
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, 0, 0));
{
btScalar mass(0.);
createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
}
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}

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

@@ -282,13 +282,55 @@ void btReducedSoftBody::updateReducedVelocity(btScalar solverdt)
void btReducedSoftBody::mapToFullVelocity(const btTransform& ref_trans)
{
// compute the reduced contribution to the angular velocity
btVector3 sum_linear(0, 0, 0);
btVector3 sum_angular(0, 0, 0);
m_linearVelocityFromReduced.setZero();
m_angularVelocityFromReduced.setZero();
// btVector3 sum_linear(0, 0, 0);
// 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);
// 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;
// std::cout << "-----end here------\n";
// m_linearVelocity -= m_linearVelocityFromReduced;
// m_angularVelocity -= m_angularVelocityFromReduced;
for (int i = 0; i < m_nFull; ++i)
{
btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[i];
m_nodes[i].m_v = computeNodeFullVelocity(ref_trans, i);
}
}
const btVector3 btReducedSoftBody::computeTotalAngularMomentum() const
{
btVector3 L_rigid = m_invInertiaTensorWorld.inverse() * m_angularVelocity;
btVector3 L_reduced(0, 0, 0);
btMatrix3x3 omega_prime_star = Cross(m_angularVelocityFromReduced);
for (int i = 0; i < m_nFull; ++i)
{
btVector3 r_com = m_rigidTransformWorld.getBasis() * m_localMomentArm[i];
btMatrix3x3 r_star = Cross(r_com);
btVector3 v_from_reduced(0, 0, 0);
@@ -300,26 +342,10 @@ void btReducedSoftBody::mapToFullVelocity(const btTransform& ref_trans)
}
}
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);
L_reduced += m_nodalMass[i] * (r_star * (m_rigidTransformWorld.getBasis() * v_from_reduced - omega_prime_star * r_com));
// L_reduced += m_nodalMass[i] * (r_star * (m_rigidTransformWorld.getBasis() * v_from_reduced));
}
return L_rigid + L_reduced;
}
const btVector3 btReducedSoftBody::computeNodeFullVelocity(const btTransform& ref_trans, int n_node) const

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

@@ -184,6 +184,9 @@ class btReducedSoftBody : public btSoftBody
// compute full space velocity from the reduced velocity
void mapToFullVelocity(const btTransform& ref_trans);
// compute total angular momentum
const btVector3 computeTotalAngularMomentum() const;
// get a single node's full space velocity from the reduced velocity
const btVector3 computeNodeFullVelocity(const btTransform& ref_trans, int n_node) const;