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

Refactoring, added draft for double sphere on ground test.

master
Martin Felis 2020-10-09 21:53:56 +02:00
parent fff003ec24
commit 823072ad72
1 changed files with 91 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

125
tests/CollisionTests.cc
View File

@ -156,66 +156,123 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
ground_body.mCollisionShapes.push_back(SimBody::BodyCollisionInfo(-1, ground_shape));
ground_body.mIsStatic = true;
double sphere_mass = 1.5;
SimBody sphere_body = CreateSphereBody(sphere_mass, 1.0, Vector3d (0., 0.5, 0.), Vector3d (0., -1., 0.));
double sphere_a_mass = 1.5;
double sphere_b_mass = 1.5;
SimBody sphere_a_body = CreateSphereBody(
sphere_a_mass, 1.0, Vector3d (0., 0.5, 0.), Vector3d (0., -1., 0.));
SimBody sphere_b_body = CreateSphereBody(
sphere_b_mass, 1.0, Vector3d (0., 0.5, 0.), Vector3d (0., -1., 0.));
CollisionInfo cinfo;
SECTION ("SphereOnGroundButColliding") {
sphere_body.q[1] = 0.5;
sphere_body.qdot[1] = -1.23;
sphere_a_body.q[1] = 0.5;
sphere_a_body.qdot[1] = -1.23;
sphere_body.updateCollisionShapes();
sphere_a_body.updateCollisionShapes();
std::vector<CollisionInfo> collisions;
CalcCollisions(ground_body, sphere_body, collisions);
CalcCollisions(ground_body, sphere_a_body, collisions);
REQUIRE(collisions.size() == 1);
cinfo = collisions[0];
bool cresult = CheckPenetration(ground_shape, sphere_body.mCollisionShapes[0].second, cinfo);
bool cresult = CheckPenetration(ground_shape,
sphere_a_body.mCollisionShapes[0].second, cinfo);
REQUIRE(cresult == true);
REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);
PrepareConstraintImpulse(&ground_body, &sphere_body, cinfo);
CalcConstraintImpulse(&ground_body, &sphere_body, cinfo, 0);
PrepareConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
REQUIRE((cinfo.accumImpulseA - Vector3d(0., 0., 0.)).norm() < 1.0e-12);
REQUIRE((cinfo.accumImpulseB - Vector3d(0., -sphere_mass * sphere_body.qdot[1], 0.)).norm() < 1.0e-12);
REQUIRE((cinfo.accumImpulseB - Vector3d(0., -sphere_a_mass * sphere_a_body.qdot[1], 0.)).norm() < 1.0e-12);
ApplyConstraintImpulse(&ground_body, &sphere_body, cinfo);
REQUIRE(sphere_body.qdot.norm() < 1.0e-12);
ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
REQUIRE(sphere_a_body.qdot.norm() < 1.0e-12);
}
SECTION ("SphereVsSphereCollision") {
double sphere2_mass = 1.5;
SimBody sphere2_body = CreateSphereBody(sphere2_mass, 1.0, Vector3d (0., -0.5, 0.), Vector3d (0., 1., 0.));
double sphere_b_mass = 1.5;
SimBody sphere_b_body = CreateSphereBody(sphere_b_mass, 1.0, Vector3d (0., -0.5, 0.), Vector3d (0., 1., 0.));
sphere_body.q[1] = 0.5;
sphere_body.qdot[1] = -1.23;
sphere_body.updateCollisionShapes();
sphere_a_body.q[1] = 0.5;
sphere_a_body.qdot[1] = -1.23;
sphere_a_body.updateCollisionShapes();
sphere2_body.q[1] = -0.5;
sphere2_body.qdot[1] = 1.23;
sphere2_body.updateCollisionShapes();
sphere_b_body.q[1] = -0.5;
sphere_b_body.qdot[1] = 1.23;
sphere_b_body.updateCollisionShapes();
std::vector<CollisionInfo> collisions;
CalcCollisions(sphere_body, sphere2_body, collisions);
CalcCollisions(sphere_a_body, sphere_b_body, collisions);
REQUIRE(collisions.size() == 1);
cinfo = collisions[0];
bool cresult = CheckPenetration(sphere_body.mCollisionShapes[0].second, sphere2_body.mCollisionShapes[0].second, cinfo);
REQUIRE(cresult == true);
REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);
PrepareConstraintImpulse(&sphere_body, &sphere2_body, cinfo);
CalcConstraintImpulse(&sphere_body, &sphere2_body, cinfo, 0);
PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
// REQUIRE((cinfo.accumImpulseA - Vector3d(0., 0., 0.)).norm() < 1.0e-12);
// REQUIRE((cinfo.accumImpulseB - Vector3d(0., -sphere_mass * sphere_body.qdot[1], 0.)).norm() < 1.0e-12);
// REQUIRE((cinfo.accumImpulseB - Vector3d(0., -sphere_mass * sphere_a_body.qdot[1], 0.)).norm() < 1.0e-12);
cout << "pre impulse: " << sphere_body.qdot.transpose() << endl;
cout << "pre impulse2: " << sphere2_body.qdot.transpose() << endl;
ApplyConstraintImpulse(&sphere_body, &sphere2_body, cinfo);
cout << "pst impulse: " << sphere_body.qdot.transpose() << endl;
cout << "pst impulse2: " << sphere2_body.qdot.transpose() << endl;
cout << "pre impulse: " << sphere_a_body.qdot.transpose() << endl;
cout << "pre impulse2: " << sphere_b_body.qdot.transpose() << endl;
ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
cout << "pst impulse: " << sphere_a_body.qdot.transpose() << endl;
cout << "pst impulse2: " << sphere_b_body.qdot.transpose() << endl;
REQUIRE(sphere_body.qdot.norm() < 1.0e-12);
REQUIRE(sphere2_body.qdot.norm() < 1.0e-12);
REQUIRE(sphere_a_body.qdot.norm() < 1.0e-12);
REQUIRE(sphere_b_body.qdot.norm() < 1.0e-12);
}
}
SECTION ("DoubleSpheresOnGround") {
sphere_b_body = CreateSphereBody(
sphere_b_mass, 5.0, Vector3d (0., 1.5, 0.), Vector3d (0., -1., 0.));
sphere_a_body.q[1] = 0.5;
sphere_a_body.qdot[1] = -1.8;
sphere_a_body.updateCollisionShapes();
sphere_b_body.q[1] = 1.5;
sphere_b_body.qdot[1] = -1.23;
sphere_b_body.updateCollisionShapes();
std::vector<CollisionInfo> collisions;
CalcCollisions(sphere_a_body, sphere_b_body, collisions);
REQUIRE(collisions.size() == 1);
CollisionInfo sph_v_sph_cinfo = collisions[0];
// REQUIRE((sph_v_sph_cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);
CalcCollisions(ground_body, sphere_a_body, collisions);
REQUIRE(collisions.size() == 1);
CollisionInfo sph_v_ground_cinfo = collisions[0];
REQUIRE((sph_v_ground_cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);
PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, sph_v_sph_cinfo);
PrepareConstraintImpulse(&ground_body, &sphere_a_body, sph_v_ground_cinfo);
int num_iter = 20;
for (int i = 0; i < num_iter; i++) {
cout << "-- Iter " << i << endl;
// REQUIRE((cinfo.accumImpulseA - Vector3d(0., 0., 0.)).norm() < 1.0e-12);
// REQUIRE((cinfo.accumImpulseB - Vector3d(0., -sphere_a_mass * sphere_a_body.qdot[1], 0.)).norm() < 1.0e-12);
cout << "pre impulse: " << sphere_a_body.qdot.transpose() << endl;
cout << "pre impulse2: " << sphere_b_body.qdot.transpose() << endl;
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, sph_v_sph_cinfo, 0);
ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, sph_v_sph_cinfo);
cout << "Sph v Sph Impulse: " << sph_v_sph_cinfo.accumImpulseB.transpose() * sph_v_sph_cinfo.dir << endl;
cout << "pst sph_v_sph impulse: " << sphere_a_body.qdot.transpose()
<< endl;
cout << "pst sph_v_sph impulse2: " << sphere_b_body.qdot.transpose()
<< endl;
CalcConstraintImpulse(&ground_body, &sphere_a_body, sph_v_ground_cinfo, 0);
ApplyConstraintImpulse(&ground_body, &sphere_a_body, sph_v_ground_cinfo);
cout << "Gnd v Sph Impulse: " << sph_v_ground_cinfo.accumImpulseB.transpose() * sph_v_ground_cinfo.dir << endl;
cout << "pst gnd_v_sph impulse: " << sphere_a_body.qdot.transpose()
<< endl;
}
}
}