rbdlsim/tests/CollisionTests.cc

407 lines
13 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);
}
SECTION("Sphere touching shifted") {
sphere.pos = Vector3d(3., 0.75, 0.);
cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);
REQUIRE((cinfo.posA - Vector3d(3., 0.0, 0.)).norm() < 1.0e-12);
REQUIRE((cinfo.posB - Vector3d(3., 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_shape.restitution = 1.0;
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,
0.,
Vector3d(0., 0.5, 0.),
Vector3d(0., -1., 0.));
SimBody sphere_b_body = CreateSphereBody(
sphere_b_mass,
1.0,
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);
SECTION("EnsureImpulseDirection") {
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
REQUIRE(fabs(cinfo.deltaImpulse) > 1.0e-3);
REQUIRE(cinfo.deltaImpulse > -1.0e-12);
}
SECTION("CheckForceAndJacobianDirections") {
REQUIRE(cinfo.jacB * sphere_a_body.qdot == -1.23);
cinfo.deltaImpulse = 1.0;
VectorNd qdot_old = sphere_a_body.qdot;
ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
REQUIRE(sphere_a_body.qdot[1] > qdot_old[1]);
}
SECTION("CalculateImpulse") {
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
double reference_impulse = -sphere_a_mass * sphere_a_body.qdot[1];
REQUIRE(fabs(cinfo.accumImpulse - reference_impulse) < 1.0e-12);
ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
REQUIRE(sphere_a_body.qdot[1] > -1.0e-12);
}
SECTION("ImpulseMustNotPull") {
sphere_a_body.qdot[1] = 1.23;
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
REQUIRE(fabs(cinfo.accumImpulse) < 1.0e-12);
}
SECTION("CheckBounce") {
cinfo.effectiveRestitution = 1.0;
PrepareConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
VectorNd old_vel = sphere_a_body.qdot;
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
REQUIRE(fabs(sphere_a_body.qdot[1] + old_vel[1]) < 1.0e-12);
}
}
SECTION("SphereOnGroundButCollidingReverseBodyOrder") {
sphere_a_body.q[1] = 0.5;
sphere_a_body.qdot[1] = -1.23;
sphere_a_body.updateCollisionShapes();
std::vector<CollisionInfo> collisions;
CalcCollisions(sphere_a_body, ground_body, collisions);
REQUIRE(collisions.size() == 1);
cinfo = collisions[0];
bool cresult = CheckPenetration(
sphere_a_body.mCollisionShapes[0].second,
ground_shape,
cinfo);
REQUIRE(cresult == true);
REQUIRE((cinfo.dir - Vector3d(0., -1., 0.)).norm() < 1.0e-12);
PrepareConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
SECTION("EnsureImpulseDirection") {
CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0);
REQUIRE(fabs(cinfo.deltaImpulse) > 1.0e-3);
REQUIRE(cinfo.deltaImpulse > -1.0e-12);
}
SECTION("CheckForceAndJacobianDirections") {
REQUIRE(cinfo.jacA * sphere_a_body.qdot == 1.23);
cinfo.deltaImpulse = 1.0;
VectorNd qdot_old = sphere_a_body.qdot;
ApplyConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
REQUIRE(sphere_a_body.qdot[1] > qdot_old[1]);
}
SECTION("CalculateImpulse") {
CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0);
double reference_impulse = -sphere_a_mass * sphere_a_body.qdot[1];
REQUIRE(fabs(cinfo.accumImpulse - reference_impulse) < 1.0e-12);
ApplyConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
REQUIRE(sphere_a_body.qdot[1] > -1.0e-12);
}
}
SECTION("SphereVsSphereCollision") {
SimBody sphere_b_body = CreateSphereBody(
sphere_b_mass,
1.0,
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);
SECTION("CheckForceAndJacobianDirections") {
REQUIRE(cinfo.jacA * sphere_a_body.qdot == 1.23);
cinfo.deltaImpulse = -1.0;
VectorNd qdot_a_old = sphere_a_body.qdot;
VectorNd qdot_b_old = sphere_b_body.qdot;
ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
REQUIRE(sphere_a_body.qdot[1] < qdot_a_old[1]);
REQUIRE(sphere_b_body.qdot[1] > qdot_b_old[1]);
}
SECTION("CalculateImpulse") {
for (int i = 0; i < 1; i++) {
cout << "Iter " << i << endl;
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
// REQUIRE((cinfo.accumImpulse - Vector3d(0., 0., 0.)).norm() < 1.0e-12);
// REQUIRE((cinfo.accumImpulse - 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;
cout << "impulse: " << cinfo.deltaImpulse << 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[1] > -0.1);
REQUIRE(sphere_b_body.qdot[1] < 0.1);
}
SECTION("CheckBounce") {
cinfo.effectiveRestitution = 1.0;
PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
VectorNd old_vel_a = sphere_a_body.qdot;
VectorNd old_vel_b = sphere_b_body.qdot;
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
REQUIRE(fabs(sphere_a_body.qdot[1] + old_vel_a[1]) < 1.0e-12);
REQUIRE(fabs(sphere_b_body.qdot[1] + old_vel_b[1]) < 1.0e-12);
}
}
SECTION("SphereVsSphereCollisionReversed") {
SimBody sphere_b_body = CreateSphereBody(
sphere_b_mass,
1.0,
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);
SECTION("CheckForceAndJacobianDirections") {
REQUIRE(cinfo.jacA * sphere_a_body.qdot == 1.23);
cinfo.deltaImpulse = -1.0;
VectorNd qdot_a_old = sphere_a_body.qdot;
VectorNd qdot_b_old = sphere_b_body.qdot;
ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
REQUIRE(sphere_a_body.qdot[1] > qdot_a_old[1]);
REQUIRE(sphere_b_body.qdot[1] < qdot_b_old[1]);
}
SECTION("CalculateImpulse") {
for (int i = 0; i < 1; i++) {
cout << "Iter " << i << endl;
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
// REQUIRE((cinfo.accumImpulse - Vector3d(0., 0., 0.)).norm() < 1.0e-12);
// REQUIRE((cinfo.accumImpulse - 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;
cout << "impulse: " << cinfo.deltaImpulse << 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[1] < 0.1);
REQUIRE(sphere_b_body.qdot[1] > -0.1);
}
}
}