martin
/
rbdlsim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Working on unilateral contact constraints.

master
Martin Felis 2020-11-13 21:54:08 +01:00
parent ff4490887f
commit 4389dd19fc
2 changed files with 26 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/rbdlsim.cc
View File

@ -363,7 +363,8 @@ void CalcConstraintImpulse(
double denom = cinfo.GMInvGTA + cinfo.GMInvGTB;
double old_impulse = cinfo.accumImpulse;
cinfo.deltaImpulse = rhs / denom;
// TODO: is this really needed here??
cinfo.deltaImpulse = std::max(0., rhs / denom);
cinfo.accumImpulse = std::max(0., cinfo.accumImpulse + cinfo.deltaImpulse);
cinfo.deltaImpulse = cinfo.accumImpulse - old_impulse;
}

32
src/simulator.cc
View File

@ -25,7 +25,7 @@ typedef SimpleMath::Matrix<float, 3, 1> Vector3f;
typedef SimpleMath::Matrix<float, 4, 4> Matrix44f;
typedef SimpleMath::Matrix<float, 4, 1> Vector4f;
static bool sIsPaused = false;
static bool sIsPaused = true;
static bool nSteps = 0;
static double sSimTime = 0.;
static double sSimTimeAccumulator = 0.;
@ -45,13 +45,13 @@ void simulator_init() {
sWorld.mStaticShapes.push_back(sGroundShape);
double restitution = 0.97;
double restitution = 0.9;
sSphereBody = CreateSphereBody(
1.,
1.,
restitution,
Vector3d(2.00, 2.48, 0.),
Vector3d(0., 2.48, 0.),
Vector3d::Zero());
sWorld.mBodies.push_back(sSphereBody);
@ -59,18 +59,22 @@ void simulator_init() {
1.,
1.,
restitution,
Vector3d(2.05, 5.405, 0.),
Vector3d(0., 5.405, 0.),
Vector3d::Zero());
//sWorld.mBodies.push_back(sSphereBody2);
sWorld.mBodies.push_back(sSphereBody2);
for (int i = 0; i < sWorld.mBodies.size(); i++) {
SimBody& body = sWorld.mBodies[i];
sthstry_register(
sStateHistory,
sWorld.mBodies[0].q.data(),
sSphereBody.q.size() * sizeof(double));
body.q.data(),
body.q.size() * sizeof(double));
sthstry_register(
sStateHistory,
sWorld.mBodies[0].qdot.data(),
sSphereBody.qdot.size() * sizeof(double));
body.qdot.data(),
body.qdot.size() * sizeof(double));
}
// sthstry_register(sStateHistory, sWorld.mBodies[1].q.data(), sSphereBody2.q.size() * sizeof(double));
// sthstry_register(sStateHistory, sWorld.mBodies[1].qdot.data(), sSphereBody2.qdot.size() * sizeof(double));
@ -96,6 +100,10 @@ void simulator_reset() {
sWorld.mBodies[0].q[0] = 0.0;
sWorld.mBodies[0].q[1] = 1.50;
sWorld.mBodies[1].q[0] = 0.2;
sWorld.mBodies[1].q[1] = 3.50;
sWorld.mBodies[1].q[2] = 0.0;
sthstry_reset_storage(sStateHistory);
sthstry_store(sStateHistory);
@ -105,12 +113,12 @@ void simulator_reset() {
void simulator_update(double dt) {
ImGui::Begin("Simulator");
sStateHistoryCurrent = std::min (sStateHistoryCurrent, sthstry_get_num_states(sStateHistory) - 1);
if (ImGui::Button("Reset")) {
simulator_reset();
}
if (ImGui::Button("Step")) {
simulator_step(sSimTimeStep);
}
ImGui::Checkbox("Paused", &sIsPaused);
if (ImGui::Button("<")) {