rbdlsim/tests/CollisionTests.cc

84 lines
1.9 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., 1.5, 0.);
cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);
REQUIRE(cresult == false);
}
SECTION ("Sphere penetration") {
sphere.pos = Vector3d (0., -1., 0.);
cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);
REQUIRE(cresult == false);
}
}