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

clean up kinematic test setup

Browse Source
This commit is contained in:
jingyuc 2021-07-14 20:01:50 -04:00
parent f1e86bf257
commit d390360f9e
10 changed files with 96 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
examples/ReducedDeformableDemo/BasicTest.cpp
View File

@ -42,45 +42,53 @@ class BasicTest : public CommonDeformableBodyBase
// btDeformableLinearElasticityForce* m_linearElasticity;
btDeformableMassSpringForce* m_massSpring;
unsigned int m_nReduced;
static const unsigned int m_startMode = 6; // actual mode# should +1
static const unsigned int m_nReduced = 1;
static const unsigned int selected_mode = 0;
unsigned int m_nFull;
btScalar sim_time;
bool first_step;
btScalar sim_time = 0;
// compute reduced degree of freedoms
void mapToReducedDofs(const btAlignedObjectArray<btAlignedObjectArray<btScalar> >& modes,
const btAlignedObjectArray<btScalar>& full_mass,
const tNodeArray& full_nodes,
btAlignedObjectArray<btScalar>& reduced_dof,
btAlignedObjectArray<btScalar>& reduced_vel)
void mapToReducedDofs(btSoftBody* psb)
{
btAssert(reduced_dof.size() == m_nReduced);
for (int j = 0; j < 1; ++j)
for (int i = 0; i < m_nFull; ++i)
for (int k = 0; k < 3; ++k)
{
int idx_f = 3 * i + k;
reduced_dof[j] = modes[j][idx_f] * full_mass[idx_f] * full_nodes[i].m_x[k];
reduced_vel[j] = modes[j][idx_f] * full_mass[idx_f] * full_nodes[i].m_v[k];
}
btAssert(psb->m_reducedDofs.size() == m_nReduced);
for (int j = 0; j < m_nReduced; ++j) {
if (j == selected_mode)
psb->m_reducedDofs[j] = sin(psb->m_eigenvalues[j] * sim_time);
else
psb->m_reducedDofs[j] = 0;
}
// for (int j = 0; j < m_nReduced; ++j)
// {
// // if (j != selected_mode)
// // continue;
// for (int i = 0; i < m_nFull; ++i)
// for (int k = 0; k < 3; ++k)
// {
// int idx_f = 3 * i + k;
// psb->m_reducedNodes[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];
// }
// }
}
// compute full degree of freedoms
void mapToFullDofs(const btAlignedObjectArray<btScalar>& m_x0,
const btAlignedObjectArray<btAlignedObjectArray<btScalar> >& modes,
const btAlignedObjectArray<btScalar>& reduced_dof,
const btAlignedObjectArray<btScalar>& reduced_vel,
tNodeArray& full_nodes)
void mapToFullDofs(btSoftBody* psb)
{
btAssert(full_nodes.size() == m_nFull);
for (int j = 0; j < 1; ++j)
btAssert(psb->m_nodes.size() == m_nFull);
for (int j = 0; j < m_nReduced; ++j)
{
// if (j != selected_mode)
// continue;
for (int i = 0; i < m_nFull; ++i)
for (int k = 0; k < 3; ++k)
{
int idx_f = 3 * i + k;
full_nodes[i].m_x[k] = m_x0[idx_f] + modes[j][idx_f] * reduced_dof[j];
full_nodes[i].m_v[k] = m_x0[idx_f] + modes[j][idx_f] * reduced_vel[j];
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];
}
}
}
// get deformed shape
@ -97,8 +105,8 @@ public:
{
// m_linearElasticity = 0;
m_massSpring = nullptr;
m_nReduced = 0;
m_nFull = 0;
sim_time = 0;
first_step = true;
}
virtual ~BasicTest()
@ -135,27 +143,31 @@ public:
void stepSimulation(float deltaTime)
{
// TODO: remove this. very hacky way of adding initial deformation
btSoftBody* psb = static_cast<btDeformableMultiBodyDynamicsWorld*>(m_dynamicsWorld)->getSoftBodyArray()[0];
if (first_step && !psb->m_bUpdateRtCst) {
getDeformedShape(psb, 0);
// TODO: remove this. very hacky way of adding initial deformation
if (first_step && !psb->m_bUpdateRtCst)
{
getDeformedShape(psb, 0);
first_step = false;
}
// compute reduced dofs
psb->m_reducedNodes.resize(m_nReduced);
psb->m_reducedDofs.resize(m_nReduced);
psb->m_reducedVelocity.resize(m_nReduced);
mapToReducedDofs(psb->m_modes, psb->m_M, psb->m_nodes, psb->m_reducedNodes, psb->m_reducedVelocity);
sim_time += deltaTime;
psb->m_reducedNodes[1] = sin(psb->m_eigenvalues[1] * sim_time);
// std::cout << psb->m_eigenvalues[0] << "\t" << sim_time << "\t" << deltaTime << "\t" << sin(psb->m_eigenvalues[0] * sim_time) << "\n";
mapToReducedDofs(psb);
float internalTimeStep = 1. / 60.f;
m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
// for (int i = 0; i < m_nReduced; ++i)
// std::cout << psb->m_reducedDofs[i] << "\t";
// std::cout << "\n";
// map reduced dof back to full
mapToFullDofs(psb->m_x0, psb->m_modes, psb->m_reducedNodes, psb->m_reducedVelocity, psb->m_nodes);
mapToFullDofs(psb);
}
virtual void renderScene()
@ -203,25 +215,23 @@ void BasicTest::initPhysics()
m_nFull = psb->m_nodes.size();
// read in eigenmodes, stiffness and mass matrices
unsigned int buffer_n_modes = 0;
// TODO: save all modes, but only use the selected ones here
std::string eigenvalues_file("../../../examples/SoftDemo/eigenvalues.bin");
btSoftBodyHelpers::readBinary(psb->m_eigenvalues, buffer_n_modes, eigenvalues_file.c_str());
m_nReduced = buffer_n_modes;
btSoftBodyHelpers::readBinary(psb->m_eigenvalues, m_startMode, m_nReduced, 3 * m_nFull, eigenvalues_file.c_str());
std::string Kr_file("../../../examples/SoftDemo/K_r_diag_mat.bin");
btSoftBodyHelpers::readBinary(psb->m_Kr, buffer_n_modes, Kr_file.c_str());
btSoftBodyHelpers::readBinary(psb->m_Kr, m_startMode, m_nReduced, 3 * m_nFull, Kr_file.c_str());
std::string Mr_file("../../../examples/SoftDemo/M_r_diag_mat.bin");
btSoftBodyHelpers::readBinary(psb->m_Mr, buffer_n_modes, Mr_file.c_str());
btSoftBodyHelpers::readBinary(psb->m_Mr, m_startMode, m_nReduced, 3 * m_nFull, Mr_file.c_str());
std::string modes_file("../../../examples/SoftDemo/modes.bin");
btSoftBodyHelpers::readBinaryMat(psb->m_modes, 3 * psb->m_nodes.size(), buffer_n_modes, modes_file.c_str()); // default to 3D
btSoftBodyHelpers::readBinaryMat(psb->m_modes, m_startMode, m_nReduced, 3 * m_nFull, modes_file.c_str()); // default to 3D
std::string M_file("../../../examples/SoftDemo/M_diag_mat.bin");
btSoftBodyHelpers::readBinary(psb->m_M, buffer_n_modes, M_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());

BIN
examples/SoftDemo/K_r_diag_mat.bin
View File

Binary file not shown.

BIN
examples/SoftDemo/M_r_diag_mat.bin
View File

Binary file not shown.

BIN
examples/SoftDemo/eigenvalues.bin
View File

Binary file not shown.

BIN
examples/SoftDemo/modes.bin
View File

Binary file not shown.

9
src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
View File

@ -173,7 +173,7 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero
{
if (m_reducedModel)
{
// // get reduced force
// get reduced force
// btAlignedObjectArray<btScalar> reduced_force;
// reduced_force.resize(psb->m_reducedNodes.size());
// for (int r = 0; r < psb->m_reducedNodes.size(); ++r)
@ -183,8 +183,13 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero
// for (int r = 0; r < psb->m_reducedNodes.size(); ++r) // TODO: reduced soft body
// {
// btScalar mass_inv = (psb->m_Mr[r] == 0) ? 0 : 1.0 / psb->m_Mr[r];
// psb->m_reducedVelocity[r] += m_dt * mass_inv * reduced_force[r];
// // btScalar delta_v = m_dt * mass_inv * reduced_force[r];
// btScalar delta_v = 1.0;
// psb->m_reducedVelocity[r] += delta_v;
// std::cout << psb->m_reducedVelocity[r] << "\t";
// }
// std::cout << "\n";
}
else
{

4
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@ -308,8 +308,8 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
btSoftBody* psb = m_softBodies[i];
if (m_reducedModel)
{
for (int r = 0; r < psb->m_reducedNodes.size(); ++r)
psb->m_reducedNodes[r] += timeStep * psb->m_reducedVelocity[r];
for (int r = 0; r < psb->m_reducedDofs.size(); ++r)
psb->m_reducedDofs[r] += timeStep * psb->m_reducedVelocity[r];
}
else
{

2
src/BulletSoftBody/btSoftBody.h
View File

@ -855,7 +855,7 @@ public:
btScalar m_restLengthScale;
btAlignedObjectArray<btScalar> m_reducedNodes; // Reduced degree of freedom
btAlignedObjectArray<btScalar> m_reducedDofs; // Reduced degree of freedom
btAlignedObjectArray<btScalar> m_reducedVelocity; // Reduced velocity array
btAlignedObjectArray<btScalar> m_x0; // Rest position
btAlignedObjectArray<btScalar> m_eigenvalues; // eigenvalues of the reduce deformable model

39
src/BulletSoftBody/btSoftBodyHelpers.cpp
View File

@ -1488,40 +1488,57 @@ void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
}
// read in binary files
void btSoftBodyHelpers::readBinary(btAlignedObjectArray<btScalar>& vec, unsigned int& size, const char* file)
void btSoftBodyHelpers::readBinary(btAlignedObjectArray<btScalar>& vec,
const unsigned int n_start, // starting index
const unsigned int n_modes, // #entries read
const unsigned int n_full, // array size
const char* file)
{
std::ifstream f_in(file, std::ios::in | std::ios::binary);
// first get size
unsigned int size;
f_in.read((char*)&size, sizeof(uint32_t));
btAssert(size == n_full);
// read data
vec.resize(size);
vec.resize(n_modes);
double temp;
for (int i = 0; i < size; i++) {
for (unsigned int i = 0; i < n_start + n_modes; ++i)
{
f_in.read((char*)&temp, sizeof(double));
vec[i] = btScalar(temp);
if (i >= n_start)
vec[i - n_start] = btScalar(temp);
}
f_in.close();
}
void btSoftBodyHelpers::readBinaryMat(btAlignedObjectArray<btAlignedObjectArray<btScalar> >& mat, const unsigned int n_row, const unsigned int n_col, const char* file)
void btSoftBodyHelpers::readBinaryMat(btAlignedObjectArray<btAlignedObjectArray<btScalar> >& mat,
const unsigned int n_start, // starting mode index
const unsigned int n_modes, // #modes, outer array size
const unsigned int n_full, // inner array size
const char* file)
{
std::ifstream f_in(file, std::ios::in | std::ios::binary);
// first get size
unsigned int v_size;
f_in.read((char*)&v_size, sizeof(uint32_t));
btAssert(v_size == n_row * n_col);
btAssert(v_size == n_full * n_full);
// read data
mat.resize(n_col);
for (int i = 0; i < n_col; ++i)
mat.resize(n_modes);
for (int i = 0; i < n_start + n_modes; ++i)
{
mat[i].resize(n_row);
for (int j = 0; j < n_row; ++j)
for (int j = 0; j < n_full; ++j)
{
double temp;
f_in.read((char*)&temp, sizeof(double));
mat[i][j] = btScalar(temp);
if (i >= n_start)
{
if (mat[i - n_start].size() != n_full)
mat[i - n_start].resize(n_full);
mat[i - n_start][j] = btScalar(temp);
}
}
}
f_in.close();

4
src/BulletSoftBody/btSoftBodyHelpers.h
View File

@ -145,9 +145,9 @@ struct btSoftBodyHelpers
static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file);
// read in a binary vector
static void readBinary(btAlignedObjectArray<btScalar>& vec, unsigned int& size, const char* file);
static void readBinary(btAlignedObjectArray<btScalar>& vec, const unsigned int n_start, const unsigned int n_modes, const unsigned int n_full, const char* file);
// read in a binary matrix (must provide matrix size)
static void readBinaryMat(btAlignedObjectArray<btAlignedObjectArray<btScalar> >& mat, const unsigned int n_row, const unsigned int n_col, const char* file);
static void readBinaryMat(btAlignedObjectArray<btAlignedObjectArray<btScalar> >& mat, const unsigned int n_start, const unsigned int n_modes, const unsigned int n_full, const char* file);
static void writeObj(const char* file, const btSoftBody* psb);