From 823072ad72321905a298d70533556799ab3c1655 Mon Sep 17 00:00:00 2001 From: Martin Felis Date: Fri, 9 Oct 2020 21:53:56 +0200 Subject: [PATCH] Refactoring, added draft for double sphere on ground test. --- tests/CollisionTests.cc | 125 +++++++++++++++++++++++++++++----------- 1 file changed, 91 insertions(+), 34 deletions(-) diff --git a/tests/CollisionTests.cc b/tests/CollisionTests.cc index 7981c9b..1e9e859 100644 --- a/tests/CollisionTests.cc +++ b/tests/CollisionTests.cc @@ -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 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 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); } -} \ No newline at end of file + + 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 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; + } + } +}