diff --git a/examples/ExampleBrowser/main.cpp b/examples/ExampleBrowser/main.cpp index f306c3f02..6138da267 100644 --- a/examples/ExampleBrowser/main.cpp +++ b/examples/ExampleBrowser/main.cpp @@ -96,7 +96,7 @@ int main(int argc, char* argv[]) else { clock.reset(); - exampleBrowser->update(deltaTimeInSeconds); + exampleBrowser->update(1.0); } } while (!exampleBrowser->requestedExit() && !interrupted); } diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index 6d4ffd775..3072cebd4 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -37,42 +37,90 @@ static btScalar COLLIDING_VELOCITY = 0; class BasicTest : public CommonDeformableBodyBase { + typedef btAlignedObjectArray tNodeArray; + // btDeformableLinearElasticityForce* m_linearElasticity; btDeformableMassSpringForce* m_massSpring; - btAlignedObjectArray m_eigenvalues, m_Kr, m_Mr; - btAlignedObjectArray > m_modes; // each inner array is a mode, outer array size = n_modes - unsigned int m_numModes; + unsigned int m_nReduced; + unsigned int m_nFull; bool first_step; + btScalar sim_time = 0; + + // compute reduced degree of freedoms + void mapToReducedDofs(const btAlignedObjectArray >& modes, + const btAlignedObjectArray& full_mass, + const tNodeArray& full_nodes, + btAlignedObjectArray& reduced_dof, + btAlignedObjectArray& reduced_vel) + { + 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]; + } + } + + // compute full degree of freedoms + void mapToFullDofs(const btAlignedObjectArray& m_x0, + const btAlignedObjectArray >& modes, + const btAlignedObjectArray& reduced_dof, + const btAlignedObjectArray& reduced_vel, + tNodeArray& full_nodes) + { + btAssert(full_nodes.size() == m_nFull); + 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; + 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]; + } + } + + // get deformed shape + void getDeformedShape(btSoftBody* psb, const int mode_n, const btScalar scale = 1) + { + for (int i = 0; i < psb->m_nodes.size(); ++i) + for (int k = 0; k < 3; ++k) + psb->m_nodes[i].m_x[k] += psb->m_modes[mode_n][3 * i + k] * scale; + } public: - BasicTest(struct GUIHelperInterface* helper) - : CommonDeformableBodyBase(helper), m_numModes(0) - { + BasicTest(struct GUIHelperInterface* helper) + : CommonDeformableBodyBase(helper) + { // m_linearElasticity = 0; - m_massSpring = 0; + m_massSpring = nullptr; + m_nReduced = 0; + m_nFull = 0; first_step = true; - } - virtual ~BasicTest() - { - } - void initPhysics(); + } + virtual ~BasicTest() + { + } + void initPhysics(); - void exitPhysics(); + void exitPhysics(); // TODO: disable pick force, non-interactive for now. bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) { return false; } - void resetCamera() - { + void resetCamera() + { float dist = 20; float pitch = 0; float yaw = 90; float targetPos[3] = {0, 3, 0}; - m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); - } + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } void Ctor_RbUpStack() { @@ -87,24 +135,27 @@ public: void stepSimulation(float deltaTime) { - // TODO: remove this. very hacky way of adding initial deformation - btSoftBody* psb = static_cast(m_dynamicsWorld)->getSoftBodyArray()[0]; - if (first_step && !psb->m_bUpdateRtCst) { - // save rest length - btAlignedObjectArray rest_length; + // TODO: remove this. very hacky way of adding initial deformation + btSoftBody* psb = static_cast(m_dynamicsWorld)->getSoftBodyArray()[0]; + if (first_step && !psb->m_bUpdateRtCst) { + getDeformedShape(psb, 0); - // assign initial displacement (perturb each nodes by mode 1) - for (int i = 0; i < psb->m_nodes.size(); ++i) - { - for (int k = 0; k < 3; ++k) - { - psb->m_nodes[i].m_x[k] += m_modes[0][3 * i + k]; - } - } - first_step = false; - } - float internalTimeStep = 1. / 60.f; - m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep); + first_step = false; + } + + // compute reduced dofs + psb->m_reducedNodes.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); + + float internalTimeStep = 1. / 60.f; + m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep); + + // map reduced dof back to full + mapToFullDofs(psb->m_x0, psb->m_modes, psb->m_reducedNodes, psb->m_reducedVelocity, psb->m_nodes); } virtual void renderScene() @@ -125,46 +176,91 @@ public: void BasicTest::initPhysics() { - m_guiHelper->setUpAxis(1); + m_guiHelper->setUpAxis(1); - ///collision configuration contains default setup for memory, collision setup + ///collision configuration contains default setup for memory, collision setup m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); - ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) - m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); - m_broadphase = new btDbvtBroadphase(); + m_broadphase = new btDbvtBroadphase(); btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); - btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); + btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); sol->setDeformableSolver(deformableBodySolver); - m_solver = sol; + m_solver = sol; - m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); btVector3 gravity = btVector3(0, 0, 0); m_dynamicsWorld->setGravity(gravity); - m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); // create volumetric soft body { std::string filename("../../../examples/SoftDemo/mesh.vtk"); btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), filename.c_str()); + 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(m_eigenvalues, buffer_n_modes, eigenvalues_file.c_str()); - m_numModes = buffer_n_modes; + btSoftBodyHelpers::readBinary(psb->m_eigenvalues, buffer_n_modes, eigenvalues_file.c_str()); + m_nReduced = buffer_n_modes; std::string Kr_file("../../../examples/SoftDemo/K_r_diag_mat.bin"); - btSoftBodyHelpers::readBinary(m_Kr, buffer_n_modes, Kr_file.c_str()); + btSoftBodyHelpers::readBinary(psb->m_Kr, buffer_n_modes, Kr_file.c_str()); std::string Mr_file("../../../examples/SoftDemo/M_r_diag_mat.bin"); - btSoftBodyHelpers::readBinary(m_Mr, buffer_n_modes, Mr_file.c_str()); + btSoftBodyHelpers::readBinary(psb->m_Mr, buffer_n_modes, Mr_file.c_str()); std::string modes_file("../../../examples/SoftDemo/modes.bin"); - btSoftBodyHelpers::readBinaryMat(m_modes, 3 * psb->m_nodes.size(), buffer_n_modes, modes_file.c_str()); // default to 3D + btSoftBodyHelpers::readBinaryMat(psb->m_modes, 3 * psb->m_nodes.size(), buffer_n_modes, 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()); + + // get rest position + psb->m_x0.resize(3 * psb->m_nodes.size()); + for (int i = 0; i < psb->m_nodes.size(); ++i) + for (int k = 0; k < 3; ++k) + psb->m_x0[3 * i + k] = psb->m_nodes[i].m_x[k]; + + // btAlignedObjectArray > M_red; + // M_red.resize(m_nReduced); + // for (int i = 0; i < m_nReduced; ++i) { + // M_red.resize(m_nReduced); + // } + + // for (int i = 0; i < m_nFull; ++i) { + // for (int j = 0; j < m_nReduced; ++j) { + // for (int k = 0; k < m_nFull; ++k) { + // for (int l = 0; l < m_nReduced; ++l) { + // btScalar tmp = psb->m_modes[i][j] * psb->m_M[i] * psb->m_modes[k][l]; + // std::cout << tmp << '\n'; + // } + // } + // } + // } + + // std::ofstream outfile1("before_map.txt"); + // for (int i = 0; i < psb->m_nodes.size(); ++i) { + // for (int k = 0; k < 3; ++k) + // outfile1 << psb->m_nodes[i].m_x[k] << '\n'; + // } + // outfile1.close(); + + // mapToFullDofs(psb->m_modes, psb->m_reducedNodes, psb->m_reducedVelocity, psb->m_nodes); + + // std::ofstream outfile2("after_map.txt"); + // for (int i = 0; i < psb->m_nodes.size(); ++i) { + // for (int k = 0; k < 3; ++k) + // outfile2 << psb->m_nodes[i].m_x[k] << '\n'; + // } + // outfile2.close(); getDeformableDynamicsWorld()->addSoftBody(psb); psb->scale(btVector3(2, 2, 2)); @@ -173,23 +269,23 @@ void BasicTest::initPhysics() psb->setTotalMass(0.5); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 0; + psb->m_cfg.kDF = 0; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN; - psb->m_sleepingThreshold = 0; + psb->m_sleepingThreshold = 0; btSoftBodyHelpers::generateBoundaryFaces(psb); psb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0)); // btDeformableLinearElasticityForce* linearElasticity = new btDeformableLinearElasticityForce(100,100,0.01); - // m_linearElasticity = linearElasticity; + // m_linearElasticity = linearElasticity; m_massSpring = new btDeformableMassSpringForce(10.0, 0.0); //TODO: this stiffness needs to match the modes getDeformableDynamicsWorld()->addForce(psb, m_massSpring); m_forces.push_back(m_massSpring); } - getDeformableDynamicsWorld()->setReducedModelFlag(true); + getDeformableDynamicsWorld()->setReducedModelFlag(false); getDeformableDynamicsWorld()->setImplicit(false); getDeformableDynamicsWorld()->setLineSearch(false); getDeformableDynamicsWorld()->setUseProjection(true); @@ -200,8 +296,8 @@ void BasicTest::initPhysics() getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100; // add a few rigid bodies // Ctor_RbUpStack(); // TODO: no rigid body for now - m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); - + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + // { // SliderParams slider("Young's Modulus", &E); // slider.m_minVal = 0; @@ -234,21 +330,21 @@ void BasicTest::initPhysics() void BasicTest::exitPhysics() { - //cleanup in the reverse order of creation/initialization + //cleanup in the reverse order of creation/initialization removePickingConstraint(); - //remove the rigidbodies from the dynamics world and delete them - int i; - for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) - { - btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; - btRigidBody* body = btRigidBody::upcast(obj); - if (body && body->getMotionState()) - { - delete body->getMotionState(); - } - m_dynamicsWorld->removeCollisionObject(obj); - delete obj; - } + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } // delete forces for (int j = 0; j < m_forces.size(); j++) { @@ -257,30 +353,30 @@ void BasicTest::exitPhysics() } m_forces.clear(); - //delete collision shapes - for (int j = 0; j < m_collisionShapes.size(); j++) - { - btCollisionShape* shape = m_collisionShapes[j]; - delete shape; - } - m_collisionShapes.clear(); + //delete collision shapes + for (int j = 0; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); - delete m_dynamicsWorld; + delete m_dynamicsWorld; - delete m_solver; + delete m_solver; - delete m_broadphase; + delete m_broadphase; - delete m_dispatcher; + delete m_dispatcher; - delete m_collisionConfiguration; + delete m_collisionConfiguration; } class CommonExampleInterface* ReducedBasicTestCreateFunc(struct CommonExampleOptions& options) { - return new BasicTest(options.m_guiHelper); + return new BasicTest(options.m_guiHelper); } diff --git a/examples/SoftDemo/M_diag_mat.bin b/examples/SoftDemo/M_diag_mat.bin new file mode 100644 index 000000000..816a62348 Binary files /dev/null and b/examples/SoftDemo/M_diag_mat.bin differ diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 2455ed213..9a634f710 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -23,6 +23,7 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAligned m_massPreconditioner = new MassPreconditioner(m_softBodies); m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit); m_preconditioner = m_KKTPreconditioner; + m_reducedModel = false; } btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective() @@ -147,20 +148,51 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero } if (m_implicit) { - for (int j = 0; j < psb->m_nodes.size(); ++j) + if (m_reducedModel) { - if (psb->m_nodes[j].m_im != 0) + // for (int j = 0; j < psb->m_reducedNodes.size(); ++j) // TODO: reduced soft body + // { + // if (psb->m_nodes[j].m_im != 0) + // { + // psb->m_nodes[j].m_v += psb->m_nodes[j].m_effectiveMass_inv * force[counter++]; + // } + // } + } + else + { + for (int j = 0; j < psb->m_nodes.size(); ++j) { - psb->m_nodes[j].m_v += psb->m_nodes[j].m_effectiveMass_inv * force[counter++]; + if (psb->m_nodes[j].m_im != 0) + { + psb->m_nodes[j].m_v += psb->m_nodes[j].m_effectiveMass_inv * force[counter++]; + } } } } else { - for (int j = 0; j < psb->m_nodes.size(); ++j) + if (m_reducedModel) { - btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im; - psb->m_nodes[j].m_v += one_over_mass * force[counter++]; + // // get reduced force + // btAlignedObjectArray reduced_force; + // reduced_force.resize(psb->m_reducedNodes.size()); + // for (int r = 0; r < psb->m_reducedNodes.size(); ++r) + // reduced_force[r] = psb->m_Kr[r] * psb->m_reducedNodes[r]; + + // // update reduced velocity + // 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]; + // } + } + else + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im; + psb->m_nodes[j].m_v += one_over_mass * force[counter++]; + } } } } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index eb05b9f01..ae4ed8690 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -40,6 +40,7 @@ public: const TVStack& m_backupVelocity; btAlignedObjectArray m_nodes; bool m_implicit; + bool m_reducedModel; MassPreconditioner* m_massPreconditioner; KKTPreconditioner* m_KKTPreconditioner; @@ -130,6 +131,16 @@ public: m_implicit = implicit; } + void setReducedModel(bool reduced_model) + { + m_reducedModel = reduced_model; + } + + bool getReducedModelFlag() const + { + return m_reducedModel; + } + // Calculate the total potential energy in the system btScalar totalEnergy(btScalar dt); diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 983e622b5..2cc44eb0d 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -72,6 +72,12 @@ btDeformableMultiBodyDynamicsWorld::~btDeformableMultiBodyDynamicsWorld() delete m_solverDeformableBodyIslandCallback; } +void btDeformableMultiBodyDynamicsWorld::setReducedModelFlag(bool reduced_model) +{ + m_reducedModel = reduced_model; + m_deformableBodySolver->m_objective->setReducedModel(reduced_model); +} + void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { BT_PROFILE("internalSingleStepSimulation"); @@ -300,25 +306,33 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) + if (m_reducedModel) { - btSoftBody::Node& node = psb->m_nodes[j]; - btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement; - btScalar clampDeltaV = maxDisplacement / timeStep; - for (int c = 0; c < 3; c++) + for (int r = 0; r < psb->m_reducedNodes.size(); ++r) + psb->m_reducedNodes[r] += timeStep * psb->m_reducedVelocity[r]; + } + else + { + for (int j = 0; j < psb->m_nodes.size(); ++j) { - if (node.m_v[c] > clampDeltaV) + btSoftBody::Node& node = psb->m_nodes[j]; + btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement; + btScalar clampDeltaV = maxDisplacement / timeStep; + for (int c = 0; c < 3; c++) { - node.m_v[c] = clampDeltaV; - } - if (node.m_v[c] < -clampDeltaV) - { - node.m_v[c] = -clampDeltaV; + if (node.m_v[c] > clampDeltaV) + { + node.m_v[c] = clampDeltaV; + } + if (node.m_v[c] < -clampDeltaV) + { + node.m_v[c] = -clampDeltaV; + } } + node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv); + node.m_q = node.m_x; + node.m_vn = node.m_v; } - node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv); - node.m_q = node.m_x; - node.m_vn = node.m_v; } // enforce anchor constraints for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 4b7069aac..405b335f9 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -48,6 +48,7 @@ class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld btScalar m_internalTime; int m_ccdIterations; bool m_implicit; + bool m_reducedModel; bool m_lineSearch; bool m_useProjection; DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback; @@ -154,6 +155,8 @@ public: void softBodySelfCollision(); + void setReducedModelFlag(bool reduced_model); + void setImplicit(bool implicit) { m_implicit = implicit; diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index f578487b8..ca904c83b 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -855,6 +855,15 @@ public: btScalar m_restLengthScale; + btAlignedObjectArray m_reducedNodes; // Reduced degree of freedom + btAlignedObjectArray m_reducedVelocity; // Reduced velocity array + btAlignedObjectArray m_x0; // Rest position + btAlignedObjectArray m_eigenvalues; // eigenvalues of the reduce deformable model + btAlignedObjectArray > m_modes; // modes of the reduced deformable model. Each inner array is a mode, outer array size = n_modes + btAlignedObjectArray m_Kr; // reduced stiffness matrix + btAlignedObjectArray m_Mr; // reduced mass matrix //TODO: do we need this? + btAlignedObjectArray m_M; // full mass matrix //TODO: maybe don't need this? + // // Api //