271 lines
8.2 KiB
C++
271 lines
8.2 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,
|
|
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);
|
|
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
|
|
double reference_impulseB = -sphere_a_mass * sphere_a_body.qdot[1];
|
|
REQUIRE(cinfo.accumImpulseA < 1.0e-12);
|
|
REQUIRE(cinfo.accumImpulseB - reference_impulseB < 1.0e-12);
|
|
|
|
ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
|
|
REQUIRE(sphere_a_body.qdot.norm() < 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);
|
|
CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0);
|
|
double reference_impulseA = -sphere_a_mass * sphere_a_body.qdot[1];
|
|
REQUIRE(cinfo.accumImpulseA - reference_impulseA < 1.0e-12);
|
|
REQUIRE(cinfo.accumImpulseB < 1.0e-12);
|
|
|
|
ApplyConstraintImpulse(&sphere_a_body, &ground_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,
|
|
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);
|
|
}
|
|
}
|