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

Implement the testPointInside() methods in the collision shapes

Browse Source
This commit is contained in:
Daniel Chappuis 2014-08-09 10:28:37 +02:00
parent 79c126eac9
commit 3c1b819fda
19 changed files with 60 additions and 191 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/body/CollisionBody.h
View File

@ -178,7 +178,7 @@ class CollisionBody : public Body {
friend class DynamicsWorld;
friend class CollisionDetection;
friend class BroadPhaseAlgorithm;
friend class ProxyConvexMeshShape;
friend class ConvexMeshShape;
};
// Return the type of the body

2
src/collision/CollisionDetection.h
View File

@ -148,7 +148,7 @@ class CollisionDetection {
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
friend class ProxyConvexMeshShape;
friend class ConvexMeshShape;
};
// Select the narrow-phase collision algorithm to use given two collision shapes

2
src/collision/ProxyShape.cpp
View File

@ -25,6 +25,6 @@ ProxyShape::~ProxyShape() {
bool ProxyShape::testPointInside(const Vector3& worldPoint) {
const Transform localToWorld = mBody->getTransform() * mLocalToBodyTransform;
const Vector3 localPoint = localToWorld.getInverse() * worldPoint;
return mCollisionShape->testPointInside(localPoint);
return mCollisionShape->testPointInside(localPoint, this);
}

1
src/collision/ProxyShape.h
View File

@ -113,6 +113,7 @@ class ProxyShape {
friend class CollisionDetection;
friend class EPAAlgorithm;
friend class GJKAlgorithm;
friend class ConvexMeshShape;
};
/// Return the collision shape

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

@ -332,28 +332,20 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(ProxyShape* collisi
}
// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool GJKAlgorithm::testPointInside(const Vector3& worldPoint, ProxyShape* collisionShape) {
bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* collisionShape) {
Vector3 suppA; // Support point of object A
Vector3 w; // Support point of Minkowski difference A-B
//Vector3 pA; // Closest point of object A
//Vector3 pB; // Closest point of object B
decimal vDotw;
decimal prevDistSquare;
// Get the local-space to world-space transforms
const Transform localToWorldTransform = collisionShape->getBody()->getTransform() *
collisionShape->getLocalToBodyTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
// Support point of object B (object B is a single point)
const Vector3 suppB = worldToLocalTransform * worldPoint;
const Vector3 suppB(localPoint);
// Create a simplex set
Simplex simplex;
// Get the previous point V (last cached separating axis)
// TODO : Cache separating axis
// Initial supporting direction
Vector3 v(1, 1, 1);
// Initialize the upper bound for the square distance
@ -363,57 +355,12 @@ bool GJKAlgorithm::testPointInside(const Vector3& worldPoint, ProxyShape* collis
// Compute the support points for original objects (without margins) A and B
suppA = collisionShape->getLocalSupportPointWithoutMargin(-v);
//suppB = body2Tobody1 *
// collisionShape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
vDotw = v.dot(w);
/*
// If the enlarge objects (with margins) do not intersect
if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
// Cache the current separating axis for frame coherence
mCurrentOverlappingPair->setCachedSeparatingAxis(v);
// No intersection, we return false
return false;
}
*/
/*
// If the objects intersect only in the margins
if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) {
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(collisionShape1, collisionShape2, normal,
penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
}
*/
// Add the new support point to the simplex
simplex.addPoint(w, suppA, suppB);
@ -421,33 +368,6 @@ bool GJKAlgorithm::testPointInside(const Vector3& worldPoint, ProxyShape* collis
if (simplex.isAffinelyDependent()) {
return false;
/*
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(collisionShape1, collisionShape2, normal,
penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
*/
}
// Compute the point of the simplex closest to the origin
@ -455,33 +375,6 @@ bool GJKAlgorithm::testPointInside(const Vector3& worldPoint, ProxyShape* collis
if (!simplex.computeClosestPoint(v)) {
return false;
/*
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(collisionShape1, collisionShape2, normal,
penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
*/
}
// Store and update the squared distance of the closest point
@ -492,49 +385,10 @@ bool GJKAlgorithm::testPointInside(const Vector3& worldPoint, ProxyShape* collis
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
return false;
/*
simplex.backupClosestPointInSimplex(v);
// Get the new squared distance
distSquare = v.lengthSquare();
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(collisionShape1, collisionShape2, normal,
penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
*/
}
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
simplex.getMaxLengthSquareOfAPoint());
// The point is inside the collision shape
return true;
// The objects (without margins) intersect. Therefore, we run the GJK algorithm
// again but on the enlarged objects to compute a simplex polytope that contains
// the origin. Then, we give that simplex polytope to the EPA algorithm to compute
// the correct penetration depth and contact points between the enlarged objects.
//return computePenetrationDepthForEnlargedObjects(collisionShape1, transform1, collisionShape2,
// transform2, contactInfo, v);
}

2
src/collision/narrowphase/GJK/GJKAlgorithm.h
View File

@ -95,7 +95,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
ContactPointInfo*& contactInfo);
/// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool testPointInside(const Vector3& worldPoint, ProxyShape *collisionShape);
bool testPointInside(const Vector3& localPoint, ProxyShape *collisionShape);
};
}

6
src/collision/shapes/BoxShape.cpp
View File

@ -72,9 +72,3 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distanc
// TODO : Implement this method
return false;
}
// Return true if a point is inside the collision shape
bool BoxShape::testPointInside(const Vector3& localPoint) const {
// TODO : Implement this method
return false;
}

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

@ -76,7 +76,7 @@ class BoxShape : public CollisionShape {
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint) const;
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
public :
@ -166,6 +166,13 @@ inline bool BoxShape::isEqualTo(const CollisionShape& otherCollisionShape) const
return (mExtent == otherShape.mExtent);
}
// Return true if a point is inside the collision shape
inline bool BoxShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
return (localPoint.x < mExtent[0] && localPoint.x > -mExtent[0] &&
localPoint.y < mExtent[1] && localPoint.y > -mExtent[1] &&
localPoint.z < mExtent[2] && localPoint.z > -mExtent[2]);
}
}
#endif

16
src/collision/shapes/CapsuleShape.cpp
View File

@ -139,7 +139,17 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal dis
}
// Return true if a point is inside the collision shape
bool CapsuleShape::testPointInside(const Vector3& localPoint) const {
// TODO : Implement this method
return false;
bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
const decimal diffYCenterSphere1 = localPoint.y - mHalfHeight;
const decimal diffYCenterSphere2 = localPoint.y + mHalfHeight;
const decimal xSquare = localPoint.x * localPoint.x;
const decimal zSquare = localPoint.z * localPoint.z;
const decimal squareRadius = mRadius * mRadius;
// Return true if the point is inside the cylinder or one of the two spheres of the capsule
return ((xSquare + zSquare) < squareRadius &&
localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) ||
(xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius ||
(xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius;
}

2
src/collision/shapes/CapsuleShape.h
View File

@ -73,7 +73,7 @@ class CapsuleShape : public CollisionShape {
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint) const;
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
public :

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

@ -83,7 +83,7 @@ class CollisionShape {
void** cachedCollisionData) const=0;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& worldPoint) const=0;
virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0;
/// Raycast method
virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const=0;

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

@ -105,9 +105,3 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distan
// TODO : Implement this method
return false;
}
// Return true if a point is inside the collision shape
bool ConeShape::testPointInside(const Vector3& localPoint) const {
// TODO : Implement this method
return false;
}

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

@ -81,7 +81,7 @@ class ConeShape : public CollisionShape {
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint) const;
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
public :
@ -171,6 +171,14 @@ inline bool ConeShape::isEqualTo(const CollisionShape& otherCollisionShape) cons
return (mRadius == otherShape.mRadius && mHalfHeight == otherShape.mHalfHeight);
}
// Return true if a point is inside the collision shape
inline bool ConeShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
const decimal radiusHeight = mRadius * (-localPoint.y + mHalfHeight) /
(mHalfHeight * decimal(2.0));
return (localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) &&
(localPoint.x * localPoint.x + localPoint.z * localPoint.z < radiusHeight *radiusHeight);
}
}
#endif

11
src/collision/shapes/ConvexMeshShape.h
View File

@ -102,7 +102,7 @@ class ConvexMeshShape : public CollisionShape {
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint) const;
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
public :
@ -237,9 +237,12 @@ inline void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) {
}
// Return true if a point is inside the collision shape
inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint) const {
// TODO : Implement this
return false;
inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint,
ProxyShape* proxyShape) const {
// Use the GJK algorithm to test if the point is inside the convex mesh
return proxyShape->mBody->mWorld.mCollisionDetection.
mNarrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape);
}
}

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

@ -98,9 +98,3 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal di
// TODO : Implement this method
return false;
}
// Return true if a point is inside the collision shape
bool CylinderShape::testPointInside(const Vector3& localPoint) const {
// TODO : Implement this method
return false;
}

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

@ -78,7 +78,7 @@ class CylinderShape : public CollisionShape {
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint) const;
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
public :
@ -168,6 +168,11 @@ inline bool CylinderShape::isEqualTo(const CollisionShape& otherCollisionShape)
return (mRadius == otherShape.mRadius && mHalfHeight == otherShape.mHalfHeight);
}
// Return true if a point is inside the collision shape
inline bool CylinderShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const{
return ((localPoint.x * localPoint.x + localPoint.z * localPoint.z) < mRadius * mRadius &&
localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight);
}
}

6
src/collision/shapes/SphereShape.cpp
View File

@ -57,9 +57,3 @@ bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal dist
// TODO : Implement this method
return false;
}
// Return true if a point is inside the collision shape
bool SphereShape::testPointInside(const Vector3& localPoint) const {
// TODO : Implement this method
return false;
}

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

@ -68,7 +68,7 @@ class SphereShape : public CollisionShape {
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint) const;
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
public :
@ -188,6 +188,11 @@ inline bool SphereShape::isEqualTo(const CollisionShape& otherCollisionShape) co
return (mRadius == otherShape.mRadius);
}
// Return true if a point is inside the collision shape
inline bool SphereShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
return (localPoint.lengthSquare() < mRadius * mRadius);
}
}
#endif

2
src/engine/CollisionWorld.h
View File

@ -132,7 +132,7 @@ class CollisionWorld {
friend class CollisionDetection;
friend class CollisionBody;
friend class RigidBody;
friend class ProxyConvexMeshShape;
friend class ConvexMeshShape;
};
// Return an iterator to the beginning of the bodies of the physics world