From bd5668ed51ee8ea58d492cd5a72246fe234d9cad Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 1 Aug 2014 12:36:32 +0200 Subject: [PATCH] Work on the testPointInside() method --- src/body/CollisionBody.cpp | 15 +- src/body/CollisionBody.h | 8 +- src/body/RigidBody.cpp | 2 +- src/body/RigidBody.h | 2 +- src/collision/CollisionDetection.h | 1 + src/collision/RaycastInfo.h | 7 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 210 +++++++++++++++++- src/collision/narrowphase/GJK/GJKAlgorithm.h | 3 + src/collision/shapes/BoxShape.cpp | 30 +++ src/collision/shapes/BoxShape.h | 32 +++ src/collision/shapes/CapsuleShape.cpp | 30 +++ src/collision/shapes/CapsuleShape.h | 32 +++ src/collision/shapes/CollisionShape.h | 16 +- src/collision/shapes/ConeShape.cpp | 30 +++ src/collision/shapes/ConeShape.h | 32 +++ src/collision/shapes/ConvexMeshShape.cpp | 25 +++ src/collision/shapes/ConvexMeshShape.h | 31 +++ src/collision/shapes/CylinderShape.cpp | 30 +++ src/collision/shapes/CylinderShape.h | 31 +++ src/collision/shapes/SphereShape.cpp | 30 +++ src/collision/shapes/SphereShape.h | 32 +++ src/configuration.h | 3 + src/engine/CollisionWorld.h | 5 +- test/tests/collision/TestPointInside.h | 43 ++-- 24 files changed, 643 insertions(+), 37 deletions(-) diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index 9c10370a..fee4f84c 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -61,7 +61,7 @@ CollisionBody::~CollisionBody() { /// the local-space of the body. By default, the second parameter is the identity transform. /// This method will return a pointer to the proxy collision shape that links the body with /// the collision shape you have added. -const ProxyShape* CollisionBody::addCollisionShape(const CollisionShape& collisionShape, +ProxyShape* CollisionBody::addCollisionShape(const CollisionShape& collisionShape, const Transform& transform) { // Create an internal copy of the collision shape into the world (if it does not exist yet) @@ -199,3 +199,16 @@ void CollisionBody::askForBroadPhaseCollisionCheck() const { mWorld.mCollisionDetection.askForBroadPhaseCollisionCheck(shape); } } + +// Return true if a point is inside the collision body +bool CollisionBody::testPointInside(const Vector3& worldPoint) const { + + // For each collision shape of the body + for(ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { + + // Test if the point is inside the collision shape + if (shape->testPointInside(worldPoint)) return true; + } + + return false; +} diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index 0356bce2..296f4d78 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -141,7 +141,7 @@ class CollisionBody : public Body { void setTransform(const Transform& transform); /// Add a collision shape to the body. - const ProxyShape* addCollisionShape(const CollisionShape& collisionShape, + ProxyShape* addCollisionShape(const CollisionShape& collisionShape, const Transform& transform = Transform::identity()); /// Remove a collision shape from the body @@ -163,17 +163,16 @@ class CollisionBody : public Body { const ContactManifoldListElement* getContactManifoldsLists() const; /// Return true if a point is inside the collision body - // TODO : Implement this method bool testPointInside(const Vector3& worldPoint) const; /// Raycast method // TODO : Implement this method - bool raycast(const Ray& ray, decimal distance = INFINITY_DISTANCE) const; + bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; /// Raycast method with feedback information // TODO : Implement this method bool raycast(const Ray& ray, RaycastInfo& raycastInfo, - decimal distance = INFINITY_DISTANCE) const; + decimal distance = RAYCAST_INFINITY_DISTANCE) const; // -------------------- Friendship -------------------- // @@ -181,6 +180,7 @@ class CollisionBody : public Body { friend class DynamicsWorld; friend class CollisionDetection; friend class BroadPhaseAlgorithm; + friend class ProxyConvexMeshShape; }; // Return the type of the body diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 224adcee..f890e410 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -175,7 +175,7 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons /// total mass of the rigid body and its inertia tensor). The mass must be positive. The third /// parameter is the transformation that transform the local-space of the collision shape into /// the local-space of the body. By default, the second parameter is the identity transform. -const ProxyShape* RigidBody::addCollisionShape(const CollisionShape& collisionShape, +ProxyShape* RigidBody::addCollisionShape(const CollisionShape& collisionShape, decimal mass, const Transform& transform) { assert(mass > decimal(0.0)); diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 3111733f..6a8191ee 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -204,7 +204,7 @@ class RigidBody : public CollisionBody { void applyTorque(const Vector3& torque); /// Add a collision shape to the body. - const ProxyShape* addCollisionShape(const CollisionShape& collisionShape, + ProxyShape* addCollisionShape(const CollisionShape& collisionShape, decimal mass, const Transform& transform = Transform::identity()); diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index ded6ae4b..533484db 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -148,6 +148,7 @@ class CollisionDetection { // -------------------- Friendship -------------------- // friend class DynamicsWorld; + friend class ProxyConvexMeshShape; }; // Select the narrow-phase collision algorithm to use given two collision shapes diff --git a/src/collision/RaycastInfo.h b/src/collision/RaycastInfo.h index 39d15c65..34443697 100644 --- a/src/collision/RaycastInfo.h +++ b/src/collision/RaycastInfo.h @@ -28,12 +28,15 @@ // Libraries #include "../mathematics/Vector3.h" -#include "../body/CollisionBody.h" -#include "shapes/CollisionShape.h" /// ReactPhysics3D namespace namespace reactphysics3d { +// Declarations +class CollisionBody; +class ProxyShape; +class CollisionShape; + // Structure RaycastInfo /** * This structure contains the information about a raycast hit. diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 67b86de1..74d67d80 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -47,7 +47,7 @@ GJKAlgorithm::~GJKAlgorithm() { } -// Return true and compute a contact info if the two bounding volumes collide. +// Return true and compute a contact info if the two collision shapes collide. /// This method implements the Hybrid Technique for computing the penetration depth by /// running the GJK algorithm on original objects (without margin). /// If the objects don't intersect, this method returns false. If they intersect @@ -330,3 +330,211 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(ProxyShape* collisi transform1, collisionShape2, transform2, v, contactInfo); } + +// Use the GJK Algorithm to find if a point is inside a convex collision shape +bool GJKAlgorithm::testPointInside(const Vector3& worldPoint, 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; + + // Create a simplex set + Simplex simplex; + + // Get the previous point V (last cached separating axis) + // TODO : Cache separating axis + Vector3 v(1, 1, 1); + + // Initialize the upper bound for the square distance + decimal distSquare = DECIMAL_LARGEST; + + do { + + // 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); + + // If the simplex is affinely dependent + 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 + // If the computation of the closest point fail + 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 + prevDistSquare = distSquare; + distSquare = v.lengthSquare(); + + // If the distance to the closest point doesn't improve a lot + 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); +} diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index e807a45b..db77a621 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -93,6 +93,9 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { /// Return true and compute a contact info if the two bounding volumes collide. virtual bool testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2, ContactPointInfo*& contactInfo); + + /// Use the GJK Algorithm to find if a point is inside a convex collision shape + bool testPointInside(const Vector3& worldPoint, ProxyShape *collisionShape); }; } diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index 5e838724..4c8ccdaf 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -62,6 +62,24 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const 0.0, 0.0, factor * (xSquare + ySquare)); } +// Raycast method +bool BoxShape::raycast(const Ray& ray, decimal distance) const { + // TODO : Implement this method + return false; +} + +// Raycast method with feedback information +bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const { + // 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; +} + // Constructor ProxyBoxShape::ProxyBoxShape(BoxShape* shape, CollisionBody* body, const Transform& transform, decimal mass) @@ -73,3 +91,15 @@ ProxyBoxShape::ProxyBoxShape(BoxShape* shape, CollisionBody* body, ProxyBoxShape::~ProxyBoxShape() { } + +// Raycast method +bool ProxyBoxShape::raycast(const Ray& ray, decimal distance) const { + // TODO : Implement this method + return false; +} + +// Raycast method with feedback information +bool ProxyBoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const { + // TODO : Implement this method + return false; +} diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index 0e4e4279..1f8ea740 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -29,6 +29,7 @@ // Libraries #include #include "CollisionShape.h" +#include "../../body/CollisionBody.h" #include "../../mathematics/mathematics.h" @@ -66,6 +67,9 @@ class BoxShape : public CollisionShape { /// Private assignment operator BoxShape& operator=(const BoxShape& shape); + /// Return true if a point is inside the collision shape + bool testPointInside(const Vector3& localPoint) const; + public : // -------------------- Methods -------------------- // @@ -103,6 +107,17 @@ class BoxShape : public CollisionShape { /// Create a proxy collision shape for the collision shape virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body, const Transform& transform, decimal mass); + + /// Raycast method + virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, + decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + // -------------------- Friendship -------------------- // + + friend class ProxyBoxShape; }; // Class ProxyBoxShape @@ -155,6 +170,16 @@ class ProxyBoxShape : public ProxyShape { /// Return the current collision shape margin virtual decimal getMargin() const; + + /// Raycast method + virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, + decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + /// Return true if a point is inside the collision shape + virtual bool testPointInside(const Vector3& worldPoint); }; // Allocate and return a copy of the object @@ -244,6 +269,13 @@ inline decimal ProxyBoxShape::getMargin() const { return mCollisionShape->getMargin(); } +// Return true if a point is inside the collision shape +inline bool ProxyBoxShape::testPointInside(const Vector3& worldPoint) { + const Transform localToWorld = mBody->getTransform() * mLocalToBodyTransform; + const Vector3 localPoint = localToWorld.getInverse() * worldPoint; + return mCollisionShape->testPointInside(localPoint); +} + } #endif diff --git a/src/collision/shapes/CapsuleShape.cpp b/src/collision/shapes/CapsuleShape.cpp index eb4d6815..5867c536 100644 --- a/src/collision/shapes/CapsuleShape.cpp +++ b/src/collision/shapes/CapsuleShape.cpp @@ -124,6 +124,24 @@ void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) co 0.0, 0.0, IxxAndzz); } +// Raycast method +bool CapsuleShape::raycast(const Ray& ray, decimal distance) const { + // TODO : Implement this method + return false; +} + +// Raycast method with feedback information +bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const { + // TODO : Implement this method + return false; +} + +// Return true if a point is inside the collision shape +bool CapsuleShape::testPointInside(const Vector3& localPoint) const { + // TODO : Implement this method + return false; +} + // Constructor ProxyCapsuleShape::ProxyCapsuleShape(CapsuleShape* shape, CollisionBody* body, const Transform& transform, decimal mass) @@ -135,3 +153,15 @@ ProxyCapsuleShape::ProxyCapsuleShape(CapsuleShape* shape, CollisionBody* body, ProxyCapsuleShape::~ProxyCapsuleShape() { } + +// Raycast method +bool ProxyCapsuleShape::raycast(const Ray& ray, decimal distance) const { + // TODO : Implement this method + return false; +} + +// Raycast method with feedback information +bool ProxyCapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const { + // TODO : Implement this method + return false; +} diff --git a/src/collision/shapes/CapsuleShape.h b/src/collision/shapes/CapsuleShape.h index 16214104..d40eef01 100644 --- a/src/collision/shapes/CapsuleShape.h +++ b/src/collision/shapes/CapsuleShape.h @@ -28,6 +28,7 @@ // Libraries #include "CollisionShape.h" +#include "../../body/CollisionBody.h" #include "../../mathematics/mathematics.h" // ReactPhysics3D namespace @@ -63,6 +64,9 @@ class CapsuleShape : public CollisionShape { /// Private assignment operator CapsuleShape& operator=(const CapsuleShape& shape); + /// Return true if a point is inside the collision shape + bool testPointInside(const Vector3& localPoint) const; + public : // -------------------- Methods -------------------- // @@ -103,6 +107,17 @@ class CapsuleShape : public CollisionShape { /// Create a proxy collision shape for the collision shape virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body, const Transform& transform, decimal mass); + + /// Raycast method + virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, + decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + // -------------------- Friendship -------------------- // + + friend class ProxyCapsuleShape; }; // Class ProxyCapsuleShape @@ -155,6 +170,16 @@ class ProxyCapsuleShape : public ProxyShape { /// Return the current collision shape margin virtual decimal getMargin() const; + + /// Raycast method + virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, + decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + /// Return true if a point is inside the collision shape + virtual bool testPointInside(const Vector3& worldPoint); }; /// Allocate and return a copy of the object @@ -235,6 +260,13 @@ inline decimal ProxyCapsuleShape::getMargin() const { return mCollisionShape->getMargin(); } +// Return true if a point is inside the collision shape +inline bool ProxyCapsuleShape::testPointInside(const Vector3& worldPoint) { + const Transform localToWorld = mBody->getTransform() * mLocalToBodyTransform; + const Vector3 localPoint = localToWorld.getInverse() * worldPoint; + return mCollisionShape->testPointInside(localPoint); +} + } #endif diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index db17621b..87b57a31 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -123,6 +123,13 @@ class CollisionShape { /// Create a proxy collision shape for the collision shape virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body, const Transform& transform, decimal mass)=0; + + /// Raycast method + virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const=0; + + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, + decimal distance = RAYCAST_INFINITY_DISTANCE) const=0; }; @@ -138,7 +145,7 @@ class CollisionShape { */ class ProxyShape { - private: + protected: // -------------------- Attributes -------------------- // @@ -202,12 +209,15 @@ class ProxyShape { /// Return the current object margin virtual decimal getMargin() const=0; + /// Return true if a point is inside the collision shape + virtual bool testPointInside(const Vector3& worldPoint)=0; + /// Raycast method - virtual bool raycast(const Ray& ray, decimal distance = INFINITY_DISTANCE) const=0; + virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const=0; /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, - decimal distance = INFINITY_DISTANCE) const=0; + decimal distance = RAYCAST_INFINITY_DISTANCE) const=0; // -------------------- Friendship -------------------- // diff --git a/src/collision/shapes/ConeShape.cpp b/src/collision/shapes/ConeShape.cpp index 3ea602c4..e0d77914 100644 --- a/src/collision/shapes/ConeShape.cpp +++ b/src/collision/shapes/ConeShape.cpp @@ -93,6 +93,24 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction) c return supportPoint; } +// Raycast method +bool ConeShape::raycast(const Ray& ray, decimal distance) const { + // TODO : Implement this method + return false; +} + +// Raycast method with feedback information +bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const { + // 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; +} + // Constructor ProxyConeShape::ProxyConeShape(ConeShape* shape, CollisionBody* body, const Transform& transform, decimal mass) @@ -104,3 +122,15 @@ ProxyConeShape::ProxyConeShape(ConeShape* shape, CollisionBody* body, ProxyConeShape::~ProxyConeShape() { } + +// Raycast method +bool ProxyConeShape::raycast(const Ray& ray, decimal distance) const { + // TODO : Implement this method + return false; +} + +// Raycast method with feedback information +bool ProxyConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const { + // TODO : Implement this method + return false; +} diff --git a/src/collision/shapes/ConeShape.h b/src/collision/shapes/ConeShape.h index f9f632b8..cd68a23a 100644 --- a/src/collision/shapes/ConeShape.h +++ b/src/collision/shapes/ConeShape.h @@ -28,6 +28,7 @@ // Libraries #include "CollisionShape.h" +#include "../../body/CollisionBody.h" #include "../../mathematics/mathematics.h" /// ReactPhysics3D namespace @@ -70,6 +71,9 @@ class ConeShape : public CollisionShape { /// Private assignment operator ConeShape& operator=(const ConeShape& shape); + + /// Return true if a point is inside the collision shape + bool testPointInside(const Vector3& localPoint) const; public : @@ -111,6 +115,17 @@ class ConeShape : public CollisionShape { /// Create a proxy collision shape for the collision shape virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body, const Transform& transform, decimal mass); + + /// Raycast method + virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, + decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + // -------------------- Friendship -------------------- // + + friend class ProxyConeShape; }; // Class ProxyConeShape @@ -163,6 +178,16 @@ class ProxyConeShape : public ProxyShape { /// Return the current collision shape margin virtual decimal getMargin() const; + + /// Raycast method + virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, + decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + /// Return true if a point is inside the collision body + virtual bool testPointInside(const Vector3& worldPoint); }; // Allocate and return a copy of the object @@ -251,6 +276,13 @@ inline decimal ProxyConeShape::getMargin() const { return mCollisionShape->getMargin(); } +// Return true if a point is inside the collision shape +inline bool ProxyConeShape::testPointInside(const Vector3& worldPoint) { + const Transform localToWorld = mBody->getTransform() * mLocalToBodyTransform; + const Vector3 localPoint = localToWorld.getInverse() * worldPoint; + return mCollisionShape->testPointInside(localPoint); +} + } #endif diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp index 943b2fc6..76578cee 100644 --- a/src/collision/shapes/ConvexMeshShape.cpp +++ b/src/collision/shapes/ConvexMeshShape.cpp @@ -224,6 +224,18 @@ bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const return true; } +// Raycast method +bool ConvexMeshShape::raycast(const Ray& ray, decimal distance) const { + // TODO : Implement this method + return false; +} + +// Raycast method with feedback information +bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const { + // TODO : Implement this method + return false; +} + // Constructor ProxyConvexMeshShape::ProxyConvexMeshShape(ConvexMeshShape* shape, CollisionBody* body, const Transform& transform, decimal mass) @@ -236,3 +248,16 @@ ProxyConvexMeshShape::ProxyConvexMeshShape(ConvexMeshShape* shape, CollisionBody ProxyConvexMeshShape::~ProxyConvexMeshShape() { } + +// Raycast method +bool ProxyConvexMeshShape::raycast(const Ray& ray, decimal distance) const { + // TODO : Implement this method + return false; +} + +// Raycast method with feedback information +bool ProxyConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, + decimal distance) const { + // TODO : Implement this method + return false; +} diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index 220afa3f..89cfce79 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -28,7 +28,9 @@ // Libraries #include "CollisionShape.h" +#include "../../engine/CollisionWorld.h" #include "../../mathematics/mathematics.h" +#include "../narrowphase/GJK/GJKAlgorithm.h" #include #include #include @@ -36,6 +38,9 @@ /// ReactPhysics3D namespace namespace reactphysics3d { +// Declaration +class CollisionWorld; + // Class ConvexMeshShape /** * This class represents a convex mesh shape. In order to create a convex mesh shape, you @@ -141,6 +146,17 @@ class ConvexMeshShape : public CollisionShape { /// Create a proxy collision shape for the collision shape virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body, const Transform& transform, decimal mass); + + /// Raycast method + virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, + decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + // -------------------- Friendship -------------------- // + + friend class ProxyConvexMeshShape; }; @@ -196,6 +212,16 @@ class ProxyConvexMeshShape : public ProxyShape { /// Return the current collision shape margin virtual decimal getMargin() const; + + /// Raycast method + virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, + decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + /// Return true if a point is inside the collision shape + virtual bool testPointInside(const Vector3& worldPoint); }; // Allocate and return a copy of the object @@ -319,6 +345,11 @@ inline decimal ProxyConvexMeshShape::getMargin() const { return mCollisionShape->getMargin(); } +// Return true if a point is inside the collision shape +inline bool ProxyConvexMeshShape::testPointInside(const Vector3& worldPoint) { + return mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.testPointInside(worldPoint,this); +} + } #endif diff --git a/src/collision/shapes/CylinderShape.cpp b/src/collision/shapes/CylinderShape.cpp index 5691d5e7..78cfa9e2 100644 --- a/src/collision/shapes/CylinderShape.cpp +++ b/src/collision/shapes/CylinderShape.cpp @@ -86,6 +86,24 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio return supportPoint; } +// Raycast method +bool CylinderShape::raycast(const Ray& ray, decimal distance) const { + // TODO : Implement this method + return false; +} + +// Raycast method with feedback information +bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const { + // 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; +} + // Constructor ProxyCylinderShape::ProxyCylinderShape(CylinderShape* cylinderShape, CollisionBody* body, const Transform& transform, decimal mass) @@ -97,3 +115,15 @@ ProxyCylinderShape::ProxyCylinderShape(CylinderShape* cylinderShape, CollisionBo ProxyCylinderShape::~ProxyCylinderShape() { } + +// Raycast method +bool ProxyCylinderShape::raycast(const Ray& ray, decimal distance) const { + // TODO : Implement this method + return false; +} + +// Raycast method with feedback information +bool ProxyCylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const { + // TODO : Implement this method + return false; +} diff --git a/src/collision/shapes/CylinderShape.h b/src/collision/shapes/CylinderShape.h index a290a51d..d89fc992 100644 --- a/src/collision/shapes/CylinderShape.h +++ b/src/collision/shapes/CylinderShape.h @@ -28,6 +28,7 @@ // Libraries #include "CollisionShape.h" +#include "../../body/CollisionBody.h" #include "../../mathematics/mathematics.h" @@ -68,6 +69,9 @@ class CylinderShape : public CollisionShape { /// Private assignment operator CylinderShape& operator=(const CylinderShape& shape); + /// Return true if a point is inside the collision shape + bool testPointInside(const Vector3& localPoint) const; + public : // -------------------- Methods -------------------- // @@ -109,6 +113,16 @@ class CylinderShape : public CollisionShape { virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body, const Transform& transform, decimal mass); + /// Raycast method + virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, + decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + // -------------------- Friendship -------------------- // + + friend class ProxyCylinderShape; }; // Class ProxyCylinderShape @@ -160,6 +174,16 @@ class ProxyCylinderShape : public ProxyShape { /// Return the current collision shape margin virtual decimal getMargin() const; + + /// Raycast method + virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, + decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + /// Return true if a point is inside the collision shape + virtual bool testPointInside(const Vector3& worldPoint); }; /// Allocate and return a copy of the object @@ -248,6 +272,13 @@ inline decimal ProxyCylinderShape::getMargin() const { return mCollisionShape->getMargin(); } +// Return true if a point is inside the collision shape +inline bool ProxyCylinderShape::testPointInside(const Vector3& worldPoint) { + const Transform localToWorld = mBody->getTransform() * mLocalToBodyTransform; + const Vector3 localPoint = localToWorld.getInverse() * worldPoint; + return mCollisionShape->testPointInside(localPoint); +} + } #endif diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp index 777b4bb4..f01dd463 100644 --- a/src/collision/shapes/SphereShape.cpp +++ b/src/collision/shapes/SphereShape.cpp @@ -46,6 +46,24 @@ SphereShape::~SphereShape() { } +// Raycast method +bool SphereShape::raycast(const Ray& ray, decimal distance) const { + // TODO : Implement this method + return false; +} + +// Raycast method with feedback information +bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const { + // 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; +} + // Constructor ProxySphereShape::ProxySphereShape(SphereShape* shape, CollisionBody* body, const Transform& transform, decimal mass) @@ -57,3 +75,15 @@ ProxySphereShape::ProxySphereShape(SphereShape* shape, CollisionBody* body, ProxySphereShape::~ProxySphereShape() { } + +// Raycast method +bool ProxySphereShape::raycast(const Ray& ray, decimal distance) const { + // TODO : Implement this method + return false; +} + +// Raycast method with feedback information +bool ProxySphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const { + // TODO : Implement this method + return false; +} diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h index 489adf73..374a8a92 100644 --- a/src/collision/shapes/SphereShape.h +++ b/src/collision/shapes/SphereShape.h @@ -28,6 +28,7 @@ // Libraries #include "CollisionShape.h" +#include "../../body/CollisionBody.h" #include "../../mathematics/mathematics.h" // ReactPhysics3D namespace @@ -58,6 +59,9 @@ class SphereShape : public CollisionShape { /// Private assignment operator SphereShape& operator=(const SphereShape& shape); + /// Return true if a point is inside the collision shape + bool testPointInside(const Vector3& localPoint) const; + public : // -------------------- Methods -------------------- // @@ -98,6 +102,17 @@ class SphereShape : public CollisionShape { /// Create a proxy collision shape for the collision shape virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body, const Transform& transform, decimal mass); + + /// Raycast method + virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, + decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + // -------------------- Friendship -------------------- // + + friend class ProxySphereShape; }; @@ -151,6 +166,16 @@ class ProxySphereShape : public ProxyShape { /// Return the current collision shape margin virtual decimal getMargin() const; + + /// Raycast method + virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, + decimal distance = RAYCAST_INFINITY_DISTANCE) const; + + /// Return true if a point is inside the collision shape + virtual bool testPointInside(const Vector3& worldPoint); }; /// Allocate and return a copy of the object @@ -267,6 +292,13 @@ inline decimal ProxySphereShape::getMargin() const { return mCollisionShape->getMargin(); } +// Return true if a point is inside the collision shape +inline bool ProxySphereShape::testPointInside(const Vector3& worldPoint) { + const Transform localToWorld = mBody->getTransform() * mLocalToBodyTransform; + const Vector3 localPoint = localToWorld.getInverse() * worldPoint; + return mCollisionShape->testPointInside(localPoint); +} + } #endif diff --git a/src/configuration.h b/src/configuration.h index 5a024e9c..61d5d9e2 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -131,6 +131,9 @@ const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1); /// followin constant with the linear velocity and the elapsed time between two frames. const decimal DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = decimal(1.7); +/// Raycasting infinity distance constant +const decimal RAYCAST_INFINITY_DISTANCE = decimal(-1.0); + } #endif diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index e143b30c..59f62c9b 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -122,18 +122,19 @@ class CollisionWorld { /// Raycast method // TODO : Implement this method - bool raycast(const Ray& ray, decimal distance = INFINITY_DISTANCE) const; + bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; /// Raycast method with feedback information // TODO : Implement this method bool raycast(const Ray& ray, RaycastInfo& raycastInfo, - decimal distance = INFINITY_DISTANCE) const; + decimal distance = RAYCAST_INFINITY_DISTANCE) const; // -------------------- Friendship -------------------- // friend class CollisionDetection; friend class CollisionBody; friend class RigidBody; + friend class ProxyConvexMeshShape; }; // Return an iterator to the beginning of the bodies of the physics world diff --git a/test/tests/collision/TestPointInside.h b/test/tests/collision/TestPointInside.h index 19820636..abb87a2f 100644 --- a/test/tests/collision/TestPointInside.h +++ b/test/tests/collision/TestPointInside.h @@ -27,14 +27,13 @@ #define TEST_POINT_INSIDE_H // Libraries -#include "../../Test.h" -#include "../../src/engine/CollisionWorld.h" -#include "../../src/collision/shapes/BoxShape.h" -#include "../../src/collision/shapes/SphereShape.h" -#include "../../src/collision/shapes/CapsuleShape.h" -#include "../../src/collision/shapes/ConeShape.h" -#include "../../src/collision/shapes/ConvexMeshShape.h" -#include "../../src/collision/shapes/CylinderShape.h" +#include "Test.h" +#include "collision/shapes/BoxShape.h" +#include "collision/shapes/SphereShape.h" +#include "collision/shapes/CapsuleShape.h" +#include "collision/shapes/ConeShape.h" +#include "collision/shapes/ConvexMeshShape.h" +#include "collision/shapes/CylinderShape.h" /// Reactphysics3D namespace namespace reactphysics3d { @@ -50,7 +49,7 @@ class TestPointInside : public Test { // ---------- Atributes ---------- // // Physics world - DynamicsWorld* mWorld; + CollisionWorld* mWorld; // Bodies CollisionBody* mBoxBody; @@ -70,7 +69,7 @@ class TestPointInside : public Test { // Collision Shapes ProxyBoxShape* mBoxShape; - ProxySphereShape* mSpherShape; + ProxySphereShape* mSphereShape; ProxyCapsuleShape* mCapsuleShape; ProxyConeShape* mConeShape; ProxyConvexMeshShape* mConvexMeshShape; @@ -85,7 +84,7 @@ class TestPointInside : public Test { TestPointInside() { // Create the world - mWorld = new rp3d::CollisionWorld(); + mWorld = new CollisionWorld(); // Body transform Vector3 position(-3, 2, 7); @@ -93,13 +92,13 @@ class TestPointInside : public Test { mBodyTransform = Transform(position, orientation); // Create the bodies - mBoxBody = mWorld->createCollisionBody(bodyTransform); - mSphereBody = mWorld->createCollisionBody(bodyTransform); - mCapsuleBody = mWorld->createCollisionBody(bodyTransform); - mConeBody = mWorld->createCollisionBody(bodyTransform); - mConvexMeshBody = mWorld->createCollisionBody(bodyTransform); - mConvexMeshBodyEdgesInfo = mWorld->createCollisionBody(bodyTransform); - mCylinderBody = mWorld->createCollisionBody(bodyTransform); + mBoxBody = mWorld->createCollisionBody(mBodyTransform); + mSphereBody = mWorld->createCollisionBody(mBodyTransform); + mCapsuleBody = mWorld->createCollisionBody(mBodyTransform); + mConeBody = mWorld->createCollisionBody(mBodyTransform); + mConvexMeshBody = mWorld->createCollisionBody(mBodyTransform); + mConvexMeshBodyEdgesInfo = mWorld->createCollisionBody(mBodyTransform); + mCylinderBody = mWorld->createCollisionBody(mBodyTransform); // Collision shape transform Vector3 shapePosition(1, -4, -3); @@ -111,16 +110,16 @@ class TestPointInside : public Test { // Create collision shapes BoxShape boxShape(Vector3(2, 3, 4), 0); - mBoxShape = mBoxBody->addCollisionShape(boxShape, shapeTransform); + mBoxShape = mBoxBody->addCollisionShape(boxShape, mShapeTransform); SphereShape sphereShape(3); - mSphereShape = mSphereBody->addCollisionShape(sphereShape, shapeTransform); + mSphereShape = mSphereBody->addCollisionShape(sphereShape, mShapeTransform); CapsuleShape capsuleShape(2, 10); - mCapsuleShape = mCapsuleBody->addCollisionShape(capsuleShape, shapeTransform); + mCapsuleShape = mCapsuleBody->addCollisionShape(capsuleShape, mShapeTransform); ConeShape coneShape(2, 6, 0); - mConeShape = mConeBody->addCollisionShape(coneShape, shapeTransform); + mConeShape = mConeBody->addCollisionShape(coneShape, mShapeTransform); ConvexMeshShape convexMeshShape(0); // Box of dimension (2, 3, 4) convexMeshShape.addVertex(Vector3(-2, -3, 4));