mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Improve GJK robustness for spheres by integrating the radius into the object margin

Browse Source
This commit is contained in:
Daniel Chappuis 2013-03-03 16:24:46 +01:00
parent 5a373cf27b
commit b43f875cef
9 changed files with 158 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
src/collision/narrowphase/EPA/EPAAlgorithm.cpp
View File

@ -158,24 +158,21 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
Vector3 v3 = rotationMat * v2;
// Compute the support point in the direction of v1
suppPointsA[2] = collisionShape1->getLocalSupportPoint(v1, OBJECT_MARGIN);
suppPointsB[2] = body2Tobody1 * collisionShape2->getLocalSupportPoint(rotateToBody2 *
(-v1),
OBJECT_MARGIN);
suppPointsA[2] = collisionShape1->getLocalSupportPointWithMargin(v1);
suppPointsB[2] = body2Tobody1 *
collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1));
points[2] = suppPointsA[2] - suppPointsB[2];
// Compute the support point in the direction of v2
suppPointsA[3] = collisionShape1->getLocalSupportPoint(v2, OBJECT_MARGIN);
suppPointsB[3] = body2Tobody1 * collisionShape2->getLocalSupportPoint(rotateToBody2 *
(-v2),
OBJECT_MARGIN);
suppPointsA[3] = collisionShape1->getLocalSupportPointWithMargin(v2);
suppPointsB[3] = body2Tobody1 *
collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2));
points[3] = suppPointsA[3] - suppPointsB[3];
// Compute the support point in the direction of v3
suppPointsA[4] = collisionShape1->getLocalSupportPoint(v3, OBJECT_MARGIN);
suppPointsB[4] = body2Tobody1 * collisionShape2->getLocalSupportPoint(rotateToBody2 *
(-v3),
OBJECT_MARGIN);
suppPointsA[4] = collisionShape1->getLocalSupportPointWithMargin(v3);
suppPointsB[4] = body2Tobody1 *
collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3));
points[4] = suppPointsA[4] - suppPointsB[4];
// Now we have an hexahedron (two tetrahedron glued together). We can simply keep the
@ -272,14 +269,13 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
Vector3 n = v1.cross(v2);
// Compute the two new vertices to obtain a hexahedron
suppPointsA[3] = collisionShape1->getLocalSupportPoint(n, OBJECT_MARGIN);
suppPointsB[3] = body2Tobody1 * collisionShape2->getLocalSupportPoint(rotateToBody2 *
(-n),
OBJECT_MARGIN);
suppPointsA[3] = collisionShape1->getLocalSupportPointWithMargin(n);
suppPointsB[3] = body2Tobody1 *
collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-n));
points[3] = suppPointsA[3] - suppPointsB[3];
suppPointsA[4] = collisionShape1->getLocalSupportPoint(-n, OBJECT_MARGIN);
suppPointsB[4] = body2Tobody1 * collisionShape2->getLocalSupportPoint(rotateToBody2 * n,
OBJECT_MARGIN);
suppPointsA[4] = collisionShape1->getLocalSupportPointWithMargin(-n);
suppPointsB[4] = body2Tobody1 *
collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * n);
points[4] = suppPointsA[4] - suppPointsB[4];
// Construct the triangle faces
@ -350,11 +346,11 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// Compute the support point of the Minkowski
// difference (A-B) in the closest point direction
suppPointsA[nbVertices] = collisionShape1->getLocalSupportPoint(
triangle->getClosestPoint(), OBJECT_MARGIN);
suppPointsB[nbVertices] = body2Tobody1 * collisionShape2->getLocalSupportPoint(
rotateToBody2 * (-triangle->getClosestPoint()),
OBJECT_MARGIN);
suppPointsA[nbVertices] = collisionShape1->getLocalSupportPointWithMargin(
triangle->getClosestPoint());
suppPointsB[nbVertices] = body2Tobody1 *
collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 *
(-triangle->getClosestPoint()));
points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
int indexNewVertex = nbVertices;

28
src/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

@ -81,7 +81,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
transform1.getOrientation().getMatrix();
// Initialize the margin (sum of margins of both objects)
decimal margin = 2 * OBJECT_MARGIN;
decimal margin = collisionShape1->getMargin() + collisionShape2->getMargin();
decimal marginSquare = margin * margin;
assert(margin > 0.0);
@ -97,8 +97,9 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
do {
// Compute the support points for original objects (without margins) A and B
suppA = collisionShape1->getLocalSupportPoint(-v);
suppB = body2Tobody1 * collisionShape2->getLocalSupportPoint(rotateToBody2 * v);
suppA = collisionShape1->getLocalSupportPointWithoutMargin(-v);
suppB = body2Tobody1 *
collisionShape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
@ -125,8 +126,8 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (OBJECT_MARGIN / dist) * v);
pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
@ -157,8 +158,8 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (OBJECT_MARGIN / dist) * v);
pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
@ -187,8 +188,8 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (OBJECT_MARGIN / dist) * v);
pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
@ -224,8 +225,8 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (OBJECT_MARGIN / dist) * v);
pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
@ -282,9 +283,8 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
do {
// Compute the support points for the enlarged object A and B
suppA = collisionShape1->getLocalSupportPoint(-v, OBJECT_MARGIN);
suppB = body2ToBody1 * collisionShape2->getLocalSupportPoint(rotateToBody2 * v,
OBJECT_MARGIN);
suppA = collisionShape1->getLocalSupportPointWithMargin(-v);
suppB = body2ToBody1 * collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * v);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;

31
src/collision/shapes/BoxShape.h
View File

@ -79,8 +79,14 @@ class BoxShape : public CollisionShape {
// Return the local extents in x,y and z direction
virtual Vector3 getLocalExtents(decimal margin=0.0) const;
// Return a local support point in a given direction
virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const;
// Return the margin distance around the shape
virtual decimal getMargin() const;
// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const;
// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const;
// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
@ -104,11 +110,18 @@ inline void BoxShape::setExtent(const Vector3& extent) {
// Return the local extents of the box (half-width) in x,y and z local direction
// This method is used to compute the AABB of the box
inline Vector3 BoxShape::getLocalExtents(decimal margin) const {
return mExtent + Vector3(margin, margin, margin);
return mExtent + Vector3(getMargin(), getMargin(), getMargin());
}
// Return a local support point in a given direction
inline Vector3 BoxShape::getLocalSupportPoint(const Vector3& direction, decimal margin) const {
// Return the margin distance around the shape
inline decimal BoxShape::getMargin() const {
return OBJECT_MARGIN;
}
// Return a local support point in a given direction with the object margin
inline Vector3 BoxShape::getLocalSupportPointWithMargin(const Vector3& direction) const {
decimal margin = getMargin();
assert(margin >= 0.0);
return Vector3(direction.x < 0.0 ? -mExtent.x - margin : mExtent.x + margin,
@ -116,6 +129,14 @@ inline Vector3 BoxShape::getLocalSupportPoint(const Vector3& direction, decimal
direction.z < 0.0 ? -mExtent.z - margin : mExtent.z + margin);
}
// Return a local support point in a given direction without the objec margin
inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
return Vector3(direction.x < 0.0 ? -mExtent.x : mExtent.x,
direction.y < 0.0 ? -mExtent.y : mExtent.y,
direction.z < 0.0 ? -mExtent.z : mExtent.z);
}
}
#endif

10
src/collision/shapes/CollisionShape.h
View File

@ -76,12 +76,18 @@ class CollisionShape {
// Return the type of the collision shapes
CollisionShapeType getType() const;
// Return a local support point in a given direction
virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const=0;
// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const=0;
// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const=0;
// Return the local extents in x,y and z direction
virtual Vector3 getLocalExtents(decimal margin=0.0) const=0;
// Return the margin distance around the shape
virtual decimal getMargin() const=0;
// Return the local inertia tensor of the collision shapes
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0;
};

30
src/collision/shapes/ConeShape.cpp
View File

@ -58,9 +58,24 @@ ConeShape::~ConeShape() {
}
// Return a local support point in a given direction
inline Vector3 ConeShape::getLocalSupportPoint(const Vector3& direction, decimal margin) const {
assert(margin >= 0.0);
// Return a local support point in a given direction with the object margin
inline Vector3 ConeShape::getLocalSupportPointWithMargin(const Vector3& direction) const {
// Compute the support point without the margin
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction);
// Add the margin to the support point
Vector3 unitVec(0.0, -1.0, 0.0);
if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) {
unitVec = direction.getUnit();
}
supportPoint += unitVec * getMargin();
return supportPoint;
}
// Return a local support point in a given direction without the object margin
inline Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
const Vector3& v = direction;
decimal sinThetaTimesLengthV = mSinTheta * v.length();
@ -80,15 +95,6 @@ inline Vector3 ConeShape::getLocalSupportPoint(const Vector3& direction, decimal
}
}
// Add the margin to the support point
if (margin != 0.0) {
Vector3 unitVec(0.0, -1.0, 0.0);
if (v.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) {
unitVec = v.getUnit();
}
supportPoint += unitVec * margin;
}
return supportPoint;
}

15
src/collision/shapes/ConeShape.h
View File

@ -88,8 +88,11 @@ class ConeShape : public CollisionShape {
// Set the height
void setHeight(decimal height);
// Return a support point in a given direction
virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const;
// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const;
// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const;
// Return the local extents in x,y and z direction
virtual Vector3 getLocalExtents(decimal margin=0.0) const;
@ -97,6 +100,9 @@ class ConeShape : public CollisionShape {
// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
// Return the margin distance around the shape
virtual decimal getMargin() const;
#ifdef VISUAL_DEBUG
// Draw the sphere (only for testing purpose)
virtual void draw() const;
@ -143,6 +149,11 @@ inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass
0.0, 0.0, 0.0, diagXZ);
}
// Return the margin distance around the shape
inline decimal ConeShape::getMargin() const {
return OBJECT_MARGIN;
}
}
#endif

30
src/collision/shapes/CylinderShape.cpp
View File

@ -53,9 +53,24 @@ CylinderShape::~CylinderShape() {
}
// Return a local support point in a given direction
Vector3 CylinderShape::getLocalSupportPoint(const Vector3& direction, decimal margin) const {
assert(margin >= 0.0);
// Return a local support point in a given direction with the object margin
Vector3 CylinderShape::getLocalSupportPointWithMargin(const Vector3& direction) const {
// Compute the support point without the margin
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction);
// Add the margin to the support point
Vector3 unitVec(0.0, 1.0, 0.0);
if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) {
unitVec = direction.getUnit();
}
supportPoint += unitVec * getMargin();
return supportPoint;
}
// Return a local support point in a given direction without the object margin
Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
Vector3 supportPoint(0.0, 0.0, 0.0);
decimal uDotv = direction.y;
@ -72,15 +87,6 @@ Vector3 CylinderShape::getLocalSupportPoint(const Vector3& direction, decimal ma
else supportPoint.y = mHalfHeight;
}
// Add the margin to the support point
if (margin != 0.0) {
Vector3 unitVec(0.0, 1.0, 0.0);
if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) {
unitVec = direction.getUnit();
}
supportPoint += unitVec * margin;
}
return supportPoint;
}

15
src/collision/shapes/CylinderShape.h
View File

@ -84,8 +84,11 @@ class CylinderShape : public CollisionShape {
// Set the height
void setHeight(decimal height);
// Return a support point in a given direction
virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const;
// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const;
// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const;
// Return the local extents in x,y and z direction
virtual Vector3 getLocalExtents(decimal margin=0.0) const;
@ -93,6 +96,9 @@ class CylinderShape : public CollisionShape {
// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
// Return the margin distance around the shape
virtual decimal getMargin() const;
#ifdef VISUAL_DEBUG
// Draw the sphere (only for testing purpose)
virtual void draw() const;
@ -133,6 +139,11 @@ inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal
0.0, 0.0, diag);
}
// Return the margin distance around the shape
inline decimal CylinderShape::getMargin() const {
return OBJECT_MARGIN;
}
}
#endif

37
src/collision/shapes/SphereShape.h
View File

@ -72,8 +72,11 @@ class SphereShape : public CollisionShape {
// Set the radius of the sphere
void setRadius(decimal radius);
// Return a local support point in a given direction
virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const;
// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const;
// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const;
// Return the local extents in x,y and z direction
virtual Vector3 getLocalExtents(decimal margin=0.0) const;
@ -81,6 +84,9 @@ class SphereShape : public CollisionShape {
// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
// Return the margin distance around the shape
virtual decimal getMargin() const;
#ifdef VISUAL_DEBUG
// Draw the sphere (only for testing purpose)
virtual void draw() const;
@ -97,20 +103,28 @@ inline void SphereShape::setRadius(decimal radius) {
mRadius = radius;
}
// Return a local support point in a given direction
inline Vector3 SphereShape::getLocalSupportPoint(const Vector3& direction, decimal margin) const {
assert(margin >= 0.0);
decimal length = direction.length();
// Return a local support point in a given direction with the object margin
inline Vector3 SphereShape::getLocalSupportPointWithMargin(const Vector3& direction) const {
decimal margin = getMargin();
// If the direction vector is not the zero vector
if (length > 0.0) {
if (direction.lengthSquare() >= MACHINE_EPSILON * MACHINE_EPSILON) {
// Return the support point of the sphere in the given direction
return (mRadius + margin) * direction.getUnit();
return margin * direction.getUnit();
}
// If the direction vector is the zero vector we return a point on the
// boundary of the sphere
return Vector3(0, mRadius + margin, 0);
return Vector3(0, margin, 0);
}
// Return a local support point in a given direction without the object margin
inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
// Return the center of the sphere (the radius is taken into account in the object margin)
return Vector3(0.0, 0.0, 0.0);
}
// Return the local extents of the collision shape (half-width) in x,y and z local direction
@ -127,6 +141,11 @@ inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal ma
0.0, 0.0, diag);
}
// Return the margin distance around the shape
inline decimal SphereShape::getMargin() const {
return mRadius + OBJECT_MARGIN;
}
}
#endif