Inital works of Box vs Plan collision.
Martin Felis
parent
159400b722
commit
cd57d8aa7e
|
|
@ -15,7 +15,7 @@ struct SimShape;
|
|||
struct SimBody;
|
||||
struct CollisionInfo;
|
||||
|
||||
const double cCollisionEps = 1.0e-4;
|
||||
const double cCollisionEps = 1.0e-5;
|
||||
|
||||
struct SimShape {
|
||||
enum ShapeType { Box = 0, Sphere = 1, Plane = 2 };
|
||||
|
|
@ -47,6 +47,8 @@ struct CollisionInfo {
|
|||
const SimShape* mShapeB = nullptr;
|
||||
int mBodyAIndex;
|
||||
int mBodyBIndex;
|
||||
Vector3d mManifoldPoints[4];
|
||||
int mNumManifoldPoints = 0;
|
||||
Vector3d posA = Vector3d::Zero();
|
||||
Vector3d posB = Vector3d::Zero();
|
||||
double biasVelocityA = 0.;
|
||||
|
|
@ -93,6 +95,16 @@ bool CheckPenetrationSphereVsPlane(
|
|||
const SimShape& shape_b,
|
||||
CollisionInfo& cinfo);
|
||||
|
||||
bool CheckPenetrationBoxVsPlane (
|
||||
const SimShape& shape_a,
|
||||
const SimShape& shape_b,
|
||||
CollisionInfo& cinfo);
|
||||
|
||||
bool CheckPenetrationAABBVsPlane(
|
||||
const SimShape& shape_a,
|
||||
const SimShape& shape_b,
|
||||
CollisionInfo& cinfo);
|
||||
|
||||
SimBody CreateSphereBody(
|
||||
double mass,
|
||||
double radius,
|
||||
|
|
|
|||
162
src/rbdlsim.cc
162
src/rbdlsim.cc
|
|
@ -1,5 +1,4 @@
|
|||
#include "rbdlsim.h"
|
||||
#include "utils.h"
|
||||
|
||||
#include <ccd/ccd.h>
|
||||
#include <ccd/quat.h>
|
||||
|
|
@ -11,6 +10,8 @@
|
|||
#include <iostream>
|
||||
#include <vector>
|
||||
|
||||
#include "utils.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace RBDLSim {
|
||||
|
|
@ -113,6 +114,9 @@ bool CheckPenetration(
|
|||
if (shape_a.mType == SimShape::Sphere && shape_b.mType == SimShape::Sphere) {
|
||||
return CheckPenetrationSphereVsSphere(shape_a, shape_b, cinfo);
|
||||
}
|
||||
if (shape_a.mType == SimShape::Box && shape_b.mType == SimShape::Plane) {
|
||||
return CheckPenetrationBoxVsPlane(shape_a, shape_b, cinfo);
|
||||
}
|
||||
|
||||
ccd_t ccd;
|
||||
CCD_INIT(&ccd);
|
||||
|
|
@ -177,7 +181,7 @@ bool CheckPenetrationSphereVsPlane(
|
|||
(shape_a.orientation - Quaternion(0., 0., 0., 1.)).squaredNorm()
|
||||
< cCollisionEps);
|
||||
|
||||
Vector3d plane_normal = shape_b.orientation.toMatrix().block(0, 1, 3, 1);
|
||||
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;
|
||||
|
|
@ -197,6 +201,153 @@ bool CheckPenetrationSphereVsPlane(
|
|||
return 0;
|
||||
}
|
||||
|
||||
void CalcIntersectionLineSegmentPlane(
|
||||
const Vector3d& vA,
|
||||
const Vector3d& vB,
|
||||
const Vector3d& plane_point,
|
||||
const Vector3d& plane_normal,
|
||||
Vector3d& result) {
|
||||
double dA = (vA - plane_point).dot(plane_normal);
|
||||
double dB = (vB - plane_point).dot(plane_normal);
|
||||
|
||||
double s = (-dA) / (dB - dA);
|
||||
assert(s >= 0);
|
||||
assert(s <= 1.);
|
||||
result = vA + s * (vB - vA);
|
||||
}
|
||||
|
||||
bool CheckPenetrationBoxVsPlane (
|
||||
const SimShape& shape_a,
|
||||
const SimShape& shape_b,
|
||||
CollisionInfo& cinfo) {
|
||||
assert(shape_a.mType == SimShape::Box);
|
||||
assert(shape_b.mType == SimShape::Plane);
|
||||
|
||||
SimShape aabb = shape_a;
|
||||
SimShape plane = shape_b;
|
||||
|
||||
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;
|
||||
plane.orientation = rot_rel_box;
|
||||
|
||||
bool result = CheckPenetrationAABBVsPlane(aabb, plane, cinfo);
|
||||
|
||||
// TODO: transform values back into world space
|
||||
}
|
||||
|
||||
bool CheckPenetrationAABBVsPlane(
|
||||
const SimShape& shape_a,
|
||||
const SimShape& shape_b,
|
||||
CollisionInfo& cinfo) {
|
||||
assert(shape_a.mType == SimShape::Box);
|
||||
assert(shape_b.mType == SimShape::Plane);
|
||||
cinfo.mNumManifoldPoints = 0;
|
||||
|
||||
Vector3d plane_normal = shape_b.orientation.conjugate().rotate(Vector3d (0., 1., 0.));
|
||||
Vector3d plane_pos = shape_b.pos;
|
||||
Vector3d dir_min, dir_max;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
double sign = plane_normal[i] >= 0 ? -1. : 1.;
|
||||
|
||||
dir_min[i] = sign * shape_a.scale[i] * 0.5;
|
||||
dir_max[i] = -sign * shape_a.scale[i] * 0.5;
|
||||
}
|
||||
|
||||
double distance = (dir_min - plane_pos).dot(plane_normal);
|
||||
if (distance > cCollisionEps) {
|
||||
// early out: all points above plane
|
||||
return false;
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
const Vector3d& scale = shape_a.scale;
|
||||
// clang-format off
|
||||
Vector3d vertices[8] = {
|
||||
Vector3d( scale[0] * 0.5, -scale[0] * 0.5, scale[0] * 0.5),
|
||||
Vector3d( scale[0] * 0.5, -scale[0] * 0.5, -scale[0] * 0.5),
|
||||
Vector3d( scale[0] * 0.5, scale[0] * 0.5, scale[0] * 0.5),
|
||||
Vector3d( scale[0] * 0.5, scale[0] * 0.5, -scale[0] * 0.5),
|
||||
Vector3d(-scale[0] * 0.5, -scale[0] * 0.5, scale[0] * 0.5),
|
||||
Vector3d(-scale[0] * 0.5, -scale[0] * 0.5, -scale[0] * 0.5),
|
||||
Vector3d(-scale[0] * 0.5, scale[0] * 0.5, scale[0] * 0.5),
|
||||
Vector3d(-scale[0] * 0.5, scale[0] * 0.5, -scale[0] * 0.5)
|
||||
};
|
||||
// clang-format on
|
||||
|
||||
// Check whether any vertices are touching the plane.
|
||||
double distances[8];
|
||||
double max_depth = -cCollisionEps;
|
||||
for (int i = 0; i < 8; i++) {
|
||||
distances[i] = (vertices[i] - plane_pos).dot(plane_normal);
|
||||
if (distances[i] >= 0. && distances[i] < cCollisionEps) {
|
||||
cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] = vertices[i];
|
||||
}
|
||||
max_depth = distances[i] < max_depth ? distances[i] : max_depth;
|
||||
}
|
||||
|
||||
if (cinfo.mNumManifoldPoints == 0) {
|
||||
// We have no vertex contacts so we have to compute points on the edges.
|
||||
const int num_edges = 12;
|
||||
char edge_pairs[num_edges][2] = {
|
||||
{0, 1},
|
||||
{0, 2},
|
||||
{0, 4},
|
||||
{1, 3},
|
||||
{1, 5},
|
||||
{2, 3},
|
||||
{2, 6},
|
||||
{3, 7},
|
||||
{4, 5},
|
||||
{4, 6},
|
||||
{5, 7},
|
||||
{6, 7}};
|
||||
|
||||
for (int i = 0; i < num_edges; i++) {
|
||||
const double& d0 = distances[edge_pairs[i][0]];
|
||||
const double& d1 = distances[edge_pairs[i][1]];
|
||||
|
||||
if (d0 * d1 < 0) {
|
||||
// we have an intersection on this edge, compute the contact point on
|
||||
// this edge.
|
||||
const Vector3d& v0 = vertices[edge_pairs[i][0]];
|
||||
const Vector3d& v1 = vertices[edge_pairs[i][1]];
|
||||
|
||||
double s = (-d0) / (d1 - d0);
|
||||
assert(s >= 0);
|
||||
assert(s <= 1.);
|
||||
|
||||
cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] = v0 + s * (v1 - v0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Average manifold points to compute the central contact point.
|
||||
cinfo.posA.setZero();
|
||||
for (int i = 0; i < cinfo.mNumManifoldPoints; i++) {
|
||||
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;
|
||||
}
|
||||
|
||||
void SimBody::updateCollisionShapes() {
|
||||
UpdateKinematicsCustom(mModel, &q, nullptr, nullptr);
|
||||
|
||||
|
|
@ -393,13 +544,12 @@ void ApplyConstraintImpulse(
|
|||
SimBody* body_b,
|
||||
CollisionInfo& cinfo) {
|
||||
if (body_a && !body_a->mIsStatic) {
|
||||
body_a->qdot += cinfo.MInvA * cinfo.jacA.transpose()
|
||||
* (-cinfo.deltaImpulse);
|
||||
body_a->qdot +=
|
||||
cinfo.MInvA * cinfo.jacA.transpose() * (-cinfo.deltaImpulse);
|
||||
}
|
||||
|
||||
if (body_b && !body_b->mIsStatic) {
|
||||
body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose()
|
||||
* (cinfo.deltaImpulse);
|
||||
body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose() * (cinfo.deltaImpulse);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -45,11 +45,77 @@ TEST_CASE("Simple Box vs Sphere Collision", "[Collision]") {
|
|||
}
|
||||
}
|
||||
|
||||
TEST_CASE ("AABB vs Plane", "[Collision]") {
|
||||
SimShape plane;
|
||||
plane.mType = SimShape::Plane;
|
||||
plane.pos = Vector3d(0., 0., 0.);
|
||||
plane.orientation = Quaternion(0., 0., 0., 1.);
|
||||
plane.scale = Vector3d(1., 1., 1.);
|
||||
|
||||
SimShape box;
|
||||
box.mType = SimShape::Box;
|
||||
box.pos.set(0.0, 0.0, 0.);
|
||||
box.scale.set(1., 1., 1.);
|
||||
box.orientation.set(0., 0., 0., 1.);
|
||||
|
||||
bool cresult = false;
|
||||
CollisionInfo cinfo;
|
||||
|
||||
SECTION("Unit AABB above Plane") {
|
||||
plane.pos.set(0.0, -0.6, 0.);
|
||||
cresult = CheckPenetration(box, plane, cinfo);
|
||||
|
||||
REQUIRE(cresult == false);
|
||||
}
|
||||
|
||||
SECTION("Unit AABB below Plane") {
|
||||
plane.pos.set(0.0, 0.6, 0.);
|
||||
cresult = CheckPenetration(box, plane, cinfo);
|
||||
|
||||
REQUIRE(cresult == true);
|
||||
}
|
||||
|
||||
SECTION("Unit AABB on Plane") {
|
||||
plane.pos.set(0.0, 0.5, 0.);
|
||||
cresult = CheckPenetration(box, plane, cinfo);
|
||||
|
||||
REQUIRE(cresult == true);
|
||||
}
|
||||
|
||||
SECTION("Unit AABB Edge Contact Rotated Plane") {
|
||||
plane.pos.set(10., -0.5, -0.5);
|
||||
plane.orientation = Quaternion::fromAxisAngle(Vector3d (1.0, 0., 0.), M_PI * 0.25);
|
||||
cresult = CheckPenetrationBoxVsPlane (box, plane, cinfo);
|
||||
|
||||
REQUIRE(cresult == true);
|
||||
REQUIRE((cinfo.posA - Vector3d(0, -0.5, -0.5)).norm() < 1.0e-12);
|
||||
}
|
||||
|
||||
SECTION("Unit AABB Intersecting Contact Rotated Plane") {
|
||||
plane.pos.set(10., -0.4, -0.4);
|
||||
plane.orientation = Quaternion::fromAxisAngle(Vector3d (1.0, 0., 0.), M_PI * 0.25);
|
||||
cresult = CheckPenetrationBoxVsPlane(box, plane, cinfo);
|
||||
|
||||
REQUIRE(cresult == true);
|
||||
REQUIRE((cinfo.posA - Vector3d(0, -0.4, -0.4)).norm() < 1.0e-12);
|
||||
}
|
||||
|
||||
SECTION("RotatedBox 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);
|
||||
}
|
||||
}
|
||||
|
||||
TEST_CASE("CheckCollisionSphereVsPlane", "[Collision]") {
|
||||
SimShape plane;
|
||||
plane.mType = SimShape::Plane;
|
||||
plane.pos = Vector3d(0., 0., 0.);
|
||||
plane.orientation = Quaternion(0., 1., 0., 1.);
|
||||
plane.orientation = Quaternion(0., 0., 0., 1.);
|
||||
plane.scale = Vector3d(1., 1., 1.);
|
||||
|
||||
SimShape sphere;
|
||||
|
|
@ -94,7 +160,6 @@ TEST_CASE("CheckCollisionSphereVsPlane", "[Collision]") {
|
|||
|
||||
REQUIRE(cresult == true);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
TEST_CASE("CheckCollisionSphereVsSphere", "[Collision]") {
|
||||
|
|
|
|||
Loading…
Reference in New Issue