rbdlsim/tests/CollisionTests.cc

279 lines
9.4 KiB
C++

#include <iostream>
#include "catch.hpp"
#include "rbdlsim.h"
using namespace std;
using namespace RBDLSim;
TEST_CASE("Simple Box vs Sphere Collision", "[Collision]") {
SimShape box;
box.mType = SimShape::Box;
box.pos.set(0.0, 0.5, 0.);
box.scale.set(1., 1., 1.);
box.orientation.set(0., 0., 0., 1.);
SimShape sphere;
sphere.mType = SimShape::Sphere;
sphere.scale.set(0.5, 0.5, 0.5);
sphere.orientation.set(0., 0., 0., 1.);
bool cresult = false;
CollisionInfo cinfo;
SECTION("Box and Sphere Touching") {
sphere.pos.set(0., 1.5, 0.);
cresult = CheckPenetration(box, sphere, cinfo);
REQUIRE(cresult == true);
}
SECTION("Box and Sphere Intersecting") {
sphere.pos.set(0., 1.4, 0.);
cresult = CheckPenetration(box, sphere, cinfo);
REQUIRE(cresult == true);
}
SECTION("Box and Sphere Separated") {
sphere.pos.set(0., 1.5001, 0.);
cresult = CheckPenetration(box, sphere, cinfo);
REQUIRE(cresult == false);
}
}
TEST_CASE("CheckCollisionSphereVsPlane", "[Collision]") {
SimShape plane;
plane.mType = SimShape::Plane;
plane.pos = Vector3d(0., 0., 0.);
plane.orientation = Quaternion(0., 1., 0., 1.);
plane.scale = Vector3d(1., 1., 1.);
SimShape sphere;
sphere.mType = SimShape::Sphere;
sphere.scale = Vector3d(1.5, 1.5, 1.5);
sphere.orientation = Quaternion(0., 0., 0., 1.);
CollisionInfo cinfo;
bool cresult = false;
SECTION("Sphere above plane") {
sphere.pos = Vector3d(0., 2.0, 0.);
cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);
REQUIRE(cresult == false);
}
SECTION("Sphere touching") {
sphere.pos = Vector3d(0., 0.75, 0.);
cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);
REQUIRE((cinfo.posA - Vector3d(0., 0.0, 0.)).norm() < 1.0e-12);
REQUIRE((cinfo.posB - Vector3d(0., 0.0, 0.)).norm() < 1.0e-12);
REQUIRE(cresult == true);
}
SECTION("Sphere penetration") {
sphere.pos = Vector3d(1., -1., 0.);
cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);
REQUIRE((cinfo.posA - Vector3d(1., -1.75, 0.)).norm() < 1.0e-12);
REQUIRE((cinfo.posB - Vector3d(1., 0.0, 0.)).norm() < 1.0e-12);
REQUIRE(cresult == true);
}
}
TEST_CASE("CheckCollisionSphereVsSphere", "[Collision]") {
SimShape sphere_a;
sphere_a.mType = SimShape::Sphere;
sphere_a.scale = Vector3d(1.4, 1.4, 1.4);
sphere_a.orientation = Quaternion(0., 0., 0., 1.);
SimShape sphere_b;
sphere_b.mType = SimShape::Sphere;
sphere_b.scale = Vector3d(1.6, 1.6, 1.6);
sphere_b.orientation = Quaternion(0., 0., 0., 1.);
CollisionInfo cinfo;
bool cresult = false;
SECTION("Spheres non-overlapping") {
sphere_a.pos = Vector3d(0., 4.0, 0.);
sphere_b.pos = Vector3d(0., 0.0, 0.);
cresult = CheckPenetrationSphereVsSphere(sphere_a, sphere_b, cinfo);
REQUIRE(cresult == false);
}
SECTION("Spheres touching") {
sphere_a.pos = Vector3d(0., 1.5, 0.);
sphere_b.pos = Vector3d(0., 0.0, 0.);
cresult = CheckPenetrationSphereVsSphere(sphere_a, sphere_b, cinfo);
REQUIRE(cresult == true);
}
SECTION("Spheres overlapping") {
sphere_a.pos = Vector3d(0., 1.0, 0.);
sphere_b.pos = Vector3d(0., 0.0, 0.);
cresult = CheckPenetrationSphereVsSphere(sphere_a, sphere_b, cinfo);
REQUIRE(cresult == true);
double err_pos_A = (cinfo.posA - Vector3d(0., 0.3, 0.)).norm();
REQUIRE_THAT(
(cinfo.dir - Vector3d(0., -1., 0.)).norm(),
Catch::WithinRel(0., 1.0e-12));
REQUIRE((cinfo.posA - Vector3d(0., 0.3, 0.)).norm() < 1.0e-12);
REQUIRE((cinfo.posB - Vector3d(0., 0.8, 0.)).norm() < 1.0e-12);
REQUIRE_THAT(cinfo.depth, Catch::WithinRel(0.5, 1.0e-12));
}
SECTION("Spheres overlapping reversed") {
sphere_a.pos = Vector3d(0., 1.0, 0.);
sphere_b.pos = Vector3d(0., 0.0, 0.);
cresult = CheckPenetrationSphereVsSphere(sphere_b, sphere_a, cinfo);
REQUIRE(cresult == true);
REQUIRE_THAT(
(cinfo.dir - Vector3d(0., 1., 0.)).norm(),
Catch::WithinRel(0., 1.0e-12));
REQUIRE((cinfo.posA - Vector3d(0., 0.8, 0.)).norm() < 1.0e-12);
REQUIRE((cinfo.posB - Vector3d(0., 0.3, 0.)).norm() < 1.0e-12);
REQUIRE_THAT(cinfo.depth, Catch::WithinRel(0.5, 1.0e-12));
}
}
TEST_CASE("CalcConstraintImpulse", "[Collision]") {
SimBody ground_body;
SimShape ground_shape;
ground_shape.mType = SimShape::Plane;
ground_shape.pos = Vector3d::Zero();
ground_shape.orientation = Quaternion(0., 0., 0., 1.);
ground_body.mCollisionShapes.push_back(SimBody::BodyCollisionInfo(-1, ground_shape));
ground_body.mIsStatic = true;
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_a_body.q[1] = 0.5;
sphere_a_body.qdot[1] = -1.23;
sphere_a_body.updateCollisionShapes();
std::vector<CollisionInfo> collisions;
CalcCollisions(ground_body, sphere_a_body, collisions);
REQUIRE(collisions.size() == 1);
cinfo = collisions[0];
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_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_a_mass * sphere_a_body.qdot[1], 0.)).norm() < 1.0e-12);
ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
REQUIRE(sphere_a_body.qdot.norm() < 1.0e-12);
}
SECTION ("SphereVsSphereCollision") {
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_a_body.q[1] = 0.5;
sphere_a_body.qdot[1] = -1.23;
sphere_a_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_a_body, sphere_b_body, collisions);
REQUIRE(collisions.size() == 1);
cinfo = collisions[0];
REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);
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_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;
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_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;
}
}
}