Added Collision Test Sphere vs Sphere, Collision returns for each body a separate contact point.
Martin Felis
parent
c02255d64c
commit
a881ad5ae2
|
|
@ -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;
|
||||
|
|
@ -32,13 +28,13 @@ struct SimShape {
|
|||
struct SimBody {
|
||||
VectorNd q, qdot, qddot, tau;
|
||||
Model mModel;
|
||||
|
||||
|
||||
typedef std::pair<unsigned int, SimShape> BodyCollisionInfo;
|
||||
std::vector<BodyCollisionInfo> mCollisionShapes;
|
||||
|
||||
void step(double ts);
|
||||
void resolveCollisions (double dt, std::vector<CollisionInfo>& collisions);
|
||||
void calcNextPositions (double dt, const VectorNd& in_qdot, VectorNd& out_q);
|
||||
void resolveCollisions(double dt, std::vector<CollisionInfo>& collisions);
|
||||
void calcNextPositions(double dt, const VectorNd& in_qdot, VectorNd& out_q);
|
||||
void updateCollisionShapes();
|
||||
};
|
||||
|
||||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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_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;
|
||||
}
|
||||
|
||||
|
|
@ -204,7 +241,7 @@ bool SolveGaussSeidelProj(
|
|||
return true;
|
||||
}
|
||||
|
||||
if (iter > maxiter) {
|
||||
if (iter > maxiter) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
@ -247,7 +284,7 @@ void SimBody::resolveCollisions(
|
|||
mModel,
|
||||
q,
|
||||
collisions[i].mBodyAIndex,
|
||||
collisions[i].pos,
|
||||
collisions[i].posA,
|
||||
false));
|
||||
}
|
||||
|
||||
|
|
@ -278,16 +315,16 @@ void SimBody::resolveCollisions(
|
|||
|
||||
// Solve for the impules hlambda
|
||||
A = G * Minv * G.transpose();
|
||||
b = (constr_value ) * 1. / dt + G * (qdot + Minv * dt * (tau - N));
|
||||
b = (constr_value)*1. / dt + G * (qdot + Minv * dt * (tau - N));
|
||||
|
||||
VectorNd hlambda (VectorNd::Zero(nconstraints));
|
||||
VectorNd hlambda_lo (VectorNd::Constant(nconstraints, 0.));
|
||||
VectorNd hlambda_hi (VectorNd::Constant(nconstraints, 1.0e9));
|
||||
VectorNd hlambda(VectorNd::Zero(nconstraints));
|
||||
VectorNd hlambda_lo(VectorNd::Constant(nconstraints, 0.));
|
||||
VectorNd hlambda_hi(VectorNd::Constant(nconstraints, 1.0e9));
|
||||
|
||||
bool solve_result = false;
|
||||
solve_result = SolveGaussSeidelProj(A, b, hlambda, hlambda_lo, hlambda_hi);
|
||||
|
||||
// VectorNd hlambda = A.colPivHouseholderQr().solve(b) * -1.;
|
||||
// VectorNd hlambda = A.colPivHouseholderQr().solve(b) * -1.;
|
||||
|
||||
if (!solve_result) {
|
||||
cout << "Impulse Solve Failed!! " << endl;
|
||||
|
|
@ -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
|
||||
|
|
@ -45,40 +45,104 @@ TEST_CASE("Simple Box vs Sphere Collision", "[Collision]") {
|
|||
}
|
||||
}
|
||||
|
||||
TEST_CASE ("CheckCollisionSphereVsPlane", "[Collision]") {
|
||||
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.);
|
||||
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.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.);
|
||||
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.);
|
||||
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 ("Sphere penetration") {
|
||||
sphere.pos = Vector3d (0., -1., 0.);
|
||||
cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);
|
||||
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 == false);
|
||||
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));
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue