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

Properly allowing for rotated box vs plane checking.

master
Martin Felis 2020-11-20 21:47:17 +01:00
parent cd57d8aa7e
commit 0624a7e1ac
2 changed files with 37 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
src/rbdlsim.cc
View File

@ -94,6 +94,13 @@ void SimShapeSupport(
ccdVec3Add(v, &pos);
}
static void sSwapCollisionInfoShapeOrder(CollisionInfo &cinfo) {
cinfo.dir *= -1.;
Vector3d temp_pos = cinfo.posA;
cinfo.posA = cinfo.posB;
cinfo.posB = temp_pos;
}
bool CheckPenetration(
const SimShape& shape_a,
const SimShape& shape_b,
@ -105,10 +112,7 @@ bool CheckPenetration(
}
if (shape_b.mType == SimShape::Sphere && shape_a.mType == SimShape::Plane) {
bool result = CheckPenetrationSphereVsPlane(shape_b, shape_a, cinfo);
cinfo.dir *= -1.;
Vector3d temp_pos = cinfo.posA;
cinfo.posA = cinfo.posB;
cinfo.posB = temp_pos;
sSwapCollisionInfoShapeOrder(cinfo);
return result;
}
if (shape_a.mType == SimShape::Sphere && shape_b.mType == SimShape::Sphere) {
@ -117,6 +121,11 @@ bool CheckPenetration(
if (shape_a.mType == SimShape::Box && shape_b.mType == SimShape::Plane) {
return CheckPenetrationBoxVsPlane(shape_a, shape_b, cinfo);
}
if (shape_a.mType == SimShape::Plane && shape_b.mType == SimShape::Box) {
bool result = CheckPenetrationBoxVsPlane(shape_a, shape_b, cinfo);
sSwapCollisionInfoShapeOrder(cinfo);
return result;
}
ccd_t ccd;
CCD_INIT(&ccd);
@ -181,7 +190,8 @@ bool CheckPenetrationSphereVsPlane(
(shape_a.orientation - Quaternion(0., 0., 0., 1.)).squaredNorm()
< cCollisionEps);
Vector3d plane_normal = shape_b.orientation.conjugate().rotate(Vector3d (0., 1., 0.));
Vector3d plane_normal =
shape_b.orientation.conjugate().rotate(Vector3d(0., 1., 0.));
Vector3d plane_point = shape_b.pos;
Vector3d sphere_point_to_plane =
shape_a.pos - plane_normal * shape_a.scale[0] * 0.5;
@ -216,7 +226,7 @@ void CalcIntersectionLineSegmentPlane(
result = vA + s * (vB - vA);
}
bool CheckPenetrationBoxVsPlane (
bool CheckPenetrationBoxVsPlane(
const SimShape& shape_a,
const SimShape& shape_b,
CollisionInfo& cinfo) {
@ -229,13 +239,19 @@ bool CheckPenetrationBoxVsPlane (
aabb.pos.setZero();
aabb.orientation = Quaternion(0., 0., 0., 1.);
plane.pos = shape_a.orientation.rotate(shape_b.pos - shape_a.pos);
Quaternion rot_rel_box = shape_a.orientation.conjugate() * shape_b.orientation;
const Matrix3d shape_a_rot = shape_a.orientation.toMatrix();
plane.pos = shape_a_rot * (shape_b.pos - shape_a.pos);
Quaternion rot_rel_box =
shape_a.orientation.conjugate() * shape_b.orientation;
plane.orientation = rot_rel_box;
bool result = CheckPenetrationAABBVsPlane(aabb, plane, cinfo);
// TODO: transform values back into world space
cinfo.posA = shape_a_rot.transpose() * (cinfo.posA) + shape_a.pos;
cinfo.posB = shape_a_rot.transpose() * (cinfo.posB) + shape_a.pos;
cinfo.dir = shape_a_rot.transpose() * (cinfo.dir);
return result;
}
bool CheckPenetrationAABBVsPlane(
@ -246,7 +262,8 @@ bool CheckPenetrationAABBVsPlane(
assert(shape_b.mType == SimShape::Plane);
cinfo.mNumManifoldPoints = 0;
Vector3d plane_normal = shape_b.orientation.conjugate().rotate(Vector3d (0., 1., 0.));
Vector3d plane_normal =
shape_b.orientation.conjugate().rotate(Vector3d(0., 1., 0.));
Vector3d plane_pos = shape_b.pos;
Vector3d dir_min, dir_max;
@ -263,12 +280,15 @@ bool CheckPenetrationAABBVsPlane(
return false;
}
// Separation direction clear: we would need to push the plane along its
// negative plane normal for separation.
cinfo.dir = -plane_normal;
// If center is below plane, return that
double center_distance = (Vector3d::Zero() - plane_pos).dot(plane_normal);
if (center_distance < cCollisionEps) {
cinfo.posA = Vector3d::Zero();
cinfo.posB = Vector3d::Zero();
cinfo.dir = -plane_normal;
cinfo.depth = center_distance - cCollisionEps;
return true;
}
@ -340,9 +360,7 @@ bool CheckPenetrationAABBVsPlane(
cinfo.posA += cinfo.mManifoldPoints[i];
}
cinfo.posA = cinfo.posA / (double)cinfo.mNumManifoldPoints;
cinfo.posB = cinfo.posA;
cinfo.dir = -plane_normal;
cinfo.depth = max_depth;
return true;

8
tests/CollisionTests.cc
View File

@ -89,6 +89,7 @@ TEST_CASE ("AABB vs Plane", "[Collision]") {
REQUIRE(cresult == true);
REQUIRE((cinfo.posA - Vector3d(0, -0.5, -0.5)).norm() < 1.0e-12);
REQUIRE((cinfo.dir - Vector3d(0, -sqrt(2.) * 0.5, -sqrt(2.) * 0.5)).norm() < 1.0e-12);
}
SECTION("Unit AABB Intersecting Contact Rotated Plane") {
@ -100,14 +101,17 @@ TEST_CASE ("AABB vs Plane", "[Collision]") {
REQUIRE((cinfo.posA - Vector3d(0, -0.4, -0.4)).norm() < 1.0e-12);
}
SECTION("RotatedBox Touching Plane") {
SECTION("Rotated Unit Box Touching Plane") {
box.orientation = Quaternion::fromAxisAngle(Vector3d (1.0, 0.0, 0.0), M_PI * 0.25);
plane.pos.set(0., -sqrt(2.) * 0.5, -sqrt(2.) * 0.5);
plane.orientation = Quaternion(0., 0., 0., 1.);
cresult = CheckPenetrationBoxVsPlane(box, plane, cinfo);
REQUIRE(cresult == true);
// REQUIRE((cinfo.posA - Vector3d(0, -0.4, -0.4)).norm() < 1.0e-12);
REQUIRE(fabs(cinfo.depth) <= cCollisionEps);
REQUIRE((cinfo.posA - Vector3d(0, -sqrt(2.) * 0.5, 0.)).norm() < 1.0e-12);
REQUIRE((cinfo.posB - Vector3d(0, -sqrt(2.) * 0.5, 0.)).norm() < 1.0e-12);
REQUIRE((cinfo.dir - Vector3d(0, -1., 0.)).norm() < 1.0e-12);
}
}