martin
/
rbdlsim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Added Collision Test Sphere vs Sphere, Collision returns for each body a separate contact point.

remotes/origingl/master
Martin Felis 2020-10-06 20:22:27 +02:00
parent c02255d64c
commit a881ad5ae2
3 changed files with 142 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
include/rbdlsim.h
View File

@ -18,11 +18,7 @@ struct CollisionInfo;
const double cCollisionEps = 1.0e-4;
struct SimShape {
enum ShapeType {
Box = 0,
Sphere = 1,
Plane = 2
};
enum ShapeType { Box = 0, Sphere = 1, Plane = 2 };
ShapeType mType;
Vector3d pos;
Quaternion orientation;
@ -49,7 +45,8 @@ struct CollisionInfo {
const SimShape* mShapeB;
int mBodyAIndex;
int mBodyBIndex;
Vector3d pos;
Vector3d posA;
Vector3d posB;
Vector3d dir;
double depth;
};
@ -71,6 +68,11 @@ bool CheckPenetration(
const SimShape& shape_b,
CollisionInfo& cinfo);
bool CheckPenetrationSphereVsSphere(
const SimShape& shape_a,
const SimShape& shape_b,
CollisionInfo& cinfo);
bool CheckPenetrationSphereVsPlane(
const SimShape& shape_a,
const SimShape& shape_b,

57
src/rbdlsim.cc
View File

@ -118,7 +118,8 @@ bool CheckPenetration(
ccdGJKPenetration(&shape_a, &shape_b, &ccd, &depth, &dir, &pos);
if (intersect == 0) {
cinfo.pos.set(pos.v[0], pos.v[1], pos.v[2]);
cinfo.posA.set(pos.v[0], pos.v[1], pos.v[2]);
cinfo.posB.set(pos.v[0], pos.v[1], pos.v[2]);
cinfo.dir.set(dir.v[0], dir.v[1], dir.v[2]);
cinfo.depth = depth;
}
@ -126,6 +127,35 @@ bool CheckPenetration(
return !intersect;
}
bool CheckPenetrationSphereVsSphere(
const SimShape& shape_a,
const SimShape& shape_b,
CollisionInfo& cinfo) {
assert(shape_a.mType == SimShape::Sphere);
assert(
shape_a.scale[0] == shape_a.scale[1]
&& shape_a.scale[1] == shape_a.scale[2]);
assert(shape_b.mType == SimShape::Sphere);
assert(
shape_b.scale[0] == shape_b.scale[1]
&& shape_b.scale[1] == shape_b.scale[2]);
Vector3d diff_pos = shape_b.pos - shape_a.pos;
double diff_pos_norm = diff_pos.norm();
double distance = diff_pos_norm - (shape_a.scale[0] + shape_b.scale[0]) * 0.5;
if (distance < cCollisionEps) {
cinfo.dir = diff_pos / diff_pos_norm;
cinfo.posA = shape_a.pos + cinfo.dir * shape_a.scale[0] * 0.5;
cinfo.posB = shape_b.pos - cinfo.dir * shape_b.scale[0] * 0.5;
cinfo.depth = fabs(distance);
return true;
}
return false;
}
bool CheckPenetrationSphereVsPlane(
const SimShape& shape_a,
const SimShape& shape_b,
@ -134,17 +164,24 @@ bool CheckPenetrationSphereVsPlane(
assert(shape_b.mType == SimShape::Plane);
// For now only support aligned spheres
assert((shape_a.orientation - Quaternion(0., 0., 0., 1.)).squaredNorm() < cCollisionEps);
assert(
(shape_a.orientation - Quaternion(0., 0., 0., 1.)).squaredNorm()
< cCollisionEps);
Vector3d plane_normal = shape_b.orientation.toMatrix().block(0, 1, 3, 1);
Vector3d plane_point = shape_b.pos;
Vector3d sphere_point_to_plane = shape_a.pos - plane_normal * shape_a.scale[0];
Vector3d sphere_point_to_plane =
shape_a.pos - plane_normal * shape_a.scale[0] * 0.5;
double sphere_center_height = plane_normal.transpose() * (sphere_point_to_plane - plane_point);
double sphere_center_height =
plane_normal.transpose() * (sphere_point_to_plane - plane_point);
if (sphere_center_height < cCollisionEps) {
cinfo.pos = sphere_point_to_plane;
cinfo.dir = plane_normal;
cinfo.depth = sphere_center_height;
cinfo.posA = sphere_point_to_plane;
cinfo.posB = sphere_point_to_plane - sphere_center_height * plane_normal;
return 1;
}
@ -247,7 +284,7 @@ void SimBody::resolveCollisions(
mModel,
q,
collisions[i].mBodyAIndex,
collisions[i].pos,
collisions[i].posA,
false));
}
@ -352,7 +389,9 @@ void World::resolveCollisions(double dt) {
rev_cinfo.mShapeB = cinfo.mShapeA;
rev_cinfo.mBodyBIndex = cinfo.mBodyAIndex;
rev_cinfo.pos = cinfo.pos;
Vector3d temp = cinfo.posA;
rev_cinfo.posA = cinfo.posB;
rev_cinfo.posA = temp;
rev_cinfo.dir = -cinfo.dir;
rev_cinfo.depth = cinfo.depth;
@ -362,7 +401,7 @@ void World::resolveCollisions(double dt) {
if (body_collisions.size() > 0) {
cout << "Collision at t = " << mSimTime
<< ", pos = " << body_collisions[0].pos.transpose()
<< ", pos = " << body_collisions[0].posA.transpose()
<< ", depth = " << body_collisions[0].depth << endl
<< " qdotpre = " << mBodies[0].qdot.transpose() << endl;
}
@ -475,6 +514,4 @@ SimBody CreateBoxBody(
return result;
}
} // namespace RBDLSim

70
tests/CollisionTests.cc
View File

@ -69,16 +69,80 @@ TEST_CASE ("CheckCollisionSphereVsPlane", "[Collision]") {
}
SECTION("Sphere touching") {
sphere.pos = Vector3d (0., 1.5, 0.);
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 == false);
REQUIRE(cresult == true);
}
SECTION("Sphere penetration") {
sphere.pos = Vector3d (0., -1., 0.);
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));
}
}