From 6ebad66acf0f2ce06ee2fa532021ba137b443349 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 2 Dec 2015 22:25:52 +0100 Subject: [PATCH] Working on triangles mesh raycasting --- src/collision/CollisionDetection.cpp | 1 - src/collision/ProxyShape.h | 1 + src/collision/RaycastInfo.h | 10 +- .../broadphase/BroadPhaseAlgorithm.cpp | 42 +++--- .../broadphase/BroadPhaseAlgorithm.h | 39 +++-- src/collision/broadphase/DynamicAABBTree.cpp | 7 +- src/collision/broadphase/DynamicAABBTree.h | 18 ++- .../narrowphase/ConcaveVsConvexAlgorithm.cpp | 1 - src/collision/shapes/ConcaveMeshShape.cpp | 62 +++++++- src/collision/shapes/ConcaveMeshShape.h | 46 ++++-- src/collision/shapes/ConcaveShape.h | 8 ++ src/collision/shapes/SphereShape.h | 4 +- src/collision/shapes/TriangleShape.cpp | 51 ++++++- src/collision/shapes/TriangleShape.h | 8 +- test/tests/collision/TestRaycast.h | 134 +++++++++++++++++- .../collisionshapes/CollisionShapesScene.cpp | 3 - .../scenes/concavemesh/ConcaveMeshScene.cpp | 2 +- testbed/scenes/raycast/RaycastScene.cpp | 29 +++- testbed/scenes/raycast/RaycastScene.h | 4 +- 19 files changed, 394 insertions(+), 76 deletions(-) diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 2fcf6888..d23a90d7 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -252,7 +252,6 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call // For each possible collision pair of bodies map::iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { - ContactPointInfo* contactInfo = NULL; OverlappingPair* pair = it->second; diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h index 405e20e4..fae65c45 100644 --- a/src/collision/ProxyShape.h +++ b/src/collision/ProxyShape.h @@ -174,6 +174,7 @@ class ProxyShape { friend class EPAAlgorithm; friend class GJKAlgorithm; friend class ConvexMeshShape; + }; // Return the pointer to the cached collision data diff --git a/src/collision/RaycastInfo.h b/src/collision/RaycastInfo.h index af6f33a1..11377bf3 100644 --- a/src/collision/RaycastInfo.h +++ b/src/collision/RaycastInfo.h @@ -68,6 +68,12 @@ struct RaycastInfo { /// The hit point "p" is such that p = point1 + hitFraction * (point2 - point1) decimal hitFraction; + /// Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise) + int meshSubpart; + + /// Hit triangle index (only used for triangles mesh and -1 otherwise) + int triangleIndex; + /// Pointer to the hit collision body CollisionBody* body; @@ -77,7 +83,7 @@ struct RaycastInfo { // -------------------- Methods -------------------- // /// Constructor - RaycastInfo() : body(NULL), proxyShape(NULL) { + RaycastInfo() : meshSubpart(-1), triangleIndex(-1), body(NULL), proxyShape(NULL) { } @@ -108,7 +114,7 @@ class RaycastCallback { /// This method will be called for each ProxyShape that is hit by the /// ray. You cannot make any assumptions about the order of the /// calls. You should use the return value to control the continuation - /// of the ray. The return value is the next maxFraction value to use. + /// of the ray. The returned value is the next maxFraction value to use. /// If you return a fraction of 0.0, it means that the raycast should /// terminate. If you return a fraction of 1.0, it indicates that the /// ray is not clipped and the ray cast should continue as if no hit diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.cpp b/src/collision/broadphase/BroadPhaseAlgorithm.cpp index ddce583b..7c1979f4 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/src/collision/broadphase/BroadPhaseAlgorithm.cpp @@ -264,31 +264,29 @@ void BroadPhaseAlgorithm::notifyOverlappingNodes(int node1ID, int node2ID) { mNbPotentialPairs++; } -// Called for a broad-phase shape that has to be tested for raycast -decimal BroadPhaseAlgorithm::raycastBroadPhaseShape(int32 nodeId, RaycastTest& raycastTest, - const Ray& ray, - unsigned short raycastWithCategoryMaskBits) const { - - decimal hitFraction = decimal(-1.0); - - // Get the proxy shape from the node - ProxyShape* proxyShape = static_cast(mDynamicAABBTree.getNodeDataPointer(nodeId)); - - // Check if the raycast filtering mask allows raycast against this shape - if ((raycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) { - - // Ask the collision detection to perform a ray cast test against - // the proxy shape of this node because the ray is overlapping - // with the shape in the broad-phase - hitFraction = raycastTest.raycastAgainstShape(proxyShape, ray); - } - - return hitFraction; -} - // Called when a overlapping node has been found during the call to // DynamicAABBTree:reportAllShapesOverlappingWithAABB() void AABBOverlapCallback::notifyOverlappingNode(int nodeId) { mBroadPhaseAlgorithm.notifyOverlappingNodes(mReferenceNodeId, nodeId); } + +// Called for a broad-phase shape that has to be tested for raycast +decimal BroadPhaseRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) { + + decimal hitFraction = decimal(-1.0); + + // Get the proxy shape from the node + ProxyShape* proxyShape = static_cast(mDynamicAABBTree.getNodeDataPointer(nodeId)); + + // Check if the raycast filtering mask allows raycast against this shape + if ((mRaycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) { + + // Ask the collision detection to perform a ray cast test against + // the proxy shape of this node because the ray is overlapping + // with the shape in the broad-phase + hitFraction = mRaycastTest.raycastAgainstShape(proxyShape, ray); + } + + return hitFraction; +} diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h index 2382a96c..bf893ab8 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.h +++ b/src/collision/broadphase/BroadPhaseAlgorithm.h @@ -83,18 +83,33 @@ class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback { }; -// Class BroadPhaseRaycastTestCallback +// Class BroadPhaseRaycastCallback /** - * Callback method called to raycast against a given broad-phase shape + * Callback called when the AABB of a leaf node is hit by a ray the + * broad-phase Dynamic AABB Tree. */ -class BroadPhaseRaycastTestCallback { +class BroadPhaseRaycastCallback : public DynamicAABBTreeRaycastCallback { + + private : + + const DynamicAABBTree& mDynamicAABBTree; + + unsigned short mRaycastWithCategoryMaskBits; + + RaycastTest& mRaycastTest; public: + // Constructor + BroadPhaseRaycastCallback(const DynamicAABBTree& dynamicAABBTree, unsigned short raycastWithCategoryMaskBits, + RaycastTest& raycastTest) + : mDynamicAABBTree(dynamicAABBTree), mRaycastWithCategoryMaskBits(raycastWithCategoryMaskBits), + mRaycastTest(raycastTest) { + + } + // Called for a broad-phase shape that has to be tested for raycast - virtual decimal raycastBroadPhaseShape(int32 nodeId, RaycastTest& raycastTest, - const Ray& ray, - unsigned short raycastWithCategoryMaskBits) const=0; + virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray); }; @@ -106,7 +121,7 @@ class BroadPhaseRaycastTestCallback { * later for collision during the narrow-phase collision detection. A dynamic AABB * tree data structure is used for fast broad-phase collision detection. */ -class BroadPhaseAlgorithm : BroadPhaseRaycastTestCallback { +class BroadPhaseAlgorithm { protected : @@ -183,11 +198,6 @@ class BroadPhaseAlgorithm : BroadPhaseRaycastTestCallback { /// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree void notifyOverlappingNodes(int broadPhaseId1, int broadPhaseId2); - /// Called for a broad-phase shape that has to be tested for raycast - virtual decimal raycastBroadPhaseShape(int32 nodeId, RaycastTest& raycastTest, - const Ray& ray, - unsigned short raycastWithCategoryMaskBits) const; - /// Compute all the overlapping pairs of collision shapes void computeOverlappingPairs(); @@ -223,7 +233,10 @@ inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1, // Ray casting method inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest, unsigned short raycastWithCategoryMaskBits) const { - mDynamicAABBTree.raycast(ray, raycastTest, raycastWithCategoryMaskBits, *this); + + BroadPhaseRaycastCallback broadPhaseRaycastCallback(mDynamicAABBTree, raycastWithCategoryMaskBits, raycastTest); + + mDynamicAABBTree.raycast(ray, broadPhaseRaycastCallback); } } diff --git a/src/collision/broadphase/DynamicAABBTree.cpp b/src/collision/broadphase/DynamicAABBTree.cpp index ba17f514..a1b19113 100644 --- a/src/collision/broadphase/DynamicAABBTree.cpp +++ b/src/collision/broadphase/DynamicAABBTree.cpp @@ -631,9 +631,7 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, } // Ray casting method -void DynamicAABBTree::raycast(const Ray& ray, RaycastTest& raycastTest, - unsigned short raycastWithCategoryMaskBits, - const BroadPhaseRaycastTestCallback& callback) const { +void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &callback) const { decimal maxFraction = ray.maxFraction; @@ -668,8 +666,7 @@ void DynamicAABBTree::raycast(const Ray& ray, RaycastTest& raycastTest, Ray rayTemp(ray.point1, ray.point2, maxFraction); // Call the callback that will raycast again the broad-phase shape - decimal hitFraction = callback.raycastBroadPhaseShape(nodeID, raycastTest, rayTemp, - raycastWithCategoryMaskBits); + decimal hitFraction = callback.raycastBroadPhaseShape(nodeID, rayTemp); // If the user returned a hitFraction of zero, it means that // the raycasting should stop here diff --git a/src/collision/broadphase/DynamicAABBTree.h b/src/collision/broadphase/DynamicAABBTree.h index 6b8707de..bdb4819a 100644 --- a/src/collision/broadphase/DynamicAABBTree.h +++ b/src/collision/broadphase/DynamicAABBTree.h @@ -103,6 +103,20 @@ class DynamicAABBTreeOverlapCallback { virtual void notifyOverlappingNode(int nodeId)=0; }; +// Class DynamicAABBTreeRaycastCallback +/** + * Raycast callback in the Dynamic AABB Tree called when the AABB of a leaf + * node is hit by the ray. + */ +class DynamicAABBTreeRaycastCallback { + + public: + + // Called when the AABB of a leaf node is hit by a ray + virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray)=0; + +}; + // Class DynamicAABBTree /** * This class implements a dynamic AABB tree that is used for broad-phase @@ -208,9 +222,7 @@ class DynamicAABBTree { DynamicAABBTreeOverlapCallback& callback) const; /// Ray casting method - void raycast(const Ray& ray, RaycastTest& raycastTest, - unsigned short raycastWithCategoryMaskBits, - const BroadPhaseRaycastTestCallback& callback) const; + void raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& callback) const; /// Compute the height of the tree int computeHeight(); diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp index bc55226b..de78d8f1 100644 --- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp +++ b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp @@ -85,7 +85,6 @@ void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Inf void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) { // Create a triangle collision shape - // TODO : Do we need to use a collision margin for a triangle ? TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2]); // Select the collision algorithm to use between the triangle and the convex shape diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp index 61d0e402..637f86d3 100644 --- a/src/collision/shapes/ConcaveMeshShape.cpp +++ b/src/collision/shapes/ConcaveMeshShape.cpp @@ -25,6 +25,7 @@ // Libraries #include "ConcaveMeshShape.h" +#include "collision/shapes/TriangleShape.h" using namespace reactphysics3d; @@ -157,10 +158,67 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& } // Raycast method with feedback information +/// Note that only the first triangle hit by the ray in the mesh will be returned, even if +/// the ray hits many triangles. bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { + // Create the callback object that will compute ray casting against triangles + ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray); - // TODO : Implement this + // Ask the Dynamic AABB Tree to report all AABB nodes that are hit by the ray. + // The raycastCallback object will then compute ray casting against the triangles + // in the hit AABBs. + mDynamicAABBTree.raycast(ray, raycastCallback); - return false; + return raycastCallback.getIsHit(); +} + +// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree +decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) { + + // Add the id of the hit AABB node into + mHitAABBNodes.push_back(nodeId); + + return ray.maxFraction; +} + +// Raycast all collision shapes that have been collected +void ConcaveMeshRaycastCallback::raycastTriangles() { + + std::vector::const_iterator it; + decimal smallestHitFraction = mRay.maxFraction; + + for (it = mHitAABBNodes.begin(); it != mHitAABBNodes.end(); ++it) { + + // Get the node data (triangle index and mesh subpart index) + int32* data = mDynamicAABBTree.getNodeDataInt(*it); + + // Get the triangle vertices for this node from the concave mesh shape + Vector3 trianglePoints[3]; + mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints); + + // Create a triangle collision shape + TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2]); + + // Ray casting test against the collision shape + RaycastInfo raycastInfo; + bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape); + + // If the ray hit the collision shape + if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) { + + assert(raycastInfo.hitFraction >= decimal(0.0)); + + mRaycastInfo.body = raycastInfo.body; + mRaycastInfo.proxyShape = raycastInfo.proxyShape; + mRaycastInfo.hitFraction = raycastInfo.hitFraction; + mRaycastInfo.worldPoint = raycastInfo.worldPoint; + mRaycastInfo.worldNormal = raycastInfo.worldNormal; + mRaycastInfo.meshSubpart = data[0]; + mRaycastInfo.triangleIndex = data[1]; + + smallestHitFraction = raycastInfo.hitFraction; + mIsHit = true; + } + } } diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h index cd574a69..e2782f05 100644 --- a/src/collision/shapes/ConcaveMeshShape.h +++ b/src/collision/shapes/ConcaveMeshShape.h @@ -63,6 +63,41 @@ class ConvexTriangleAABBOverlapCallback : public DynamicAABBTreeOverlapCallback }; +/// Class ConcaveMeshRaycastCallback +class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { + + private : + + std::vector mHitAABBNodes; + const DynamicAABBTree& mDynamicAABBTree; + const ConcaveMeshShape& mConcaveMeshShape; + ProxyShape* mProxyShape; + RaycastInfo& mRaycastInfo; + const Ray& mRay; + bool mIsHit; + + public: + + // Constructor + ConcaveMeshRaycastCallback(const DynamicAABBTree& dynamicAABBTree, const ConcaveMeshShape& concaveMeshShape, + ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray) + : mDynamicAABBTree(dynamicAABBTree), mConcaveMeshShape(concaveMeshShape), mProxyShape(proxyShape), + mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false) { + + } + + /// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree + virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray); + + /// Raycast all collision shapes that have been collected + void raycastTriangles(); + + /// Return true if a raycast hit has been found + bool getIsHit() const { + return mIsHit; + } +}; + // TODO : Implement raycasting with this collision shape // TODO : Make possible for the user to have a scaling factor on the mesh @@ -101,9 +136,6 @@ class ConcaveMeshShape : public ConcaveShape { virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const; - /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; - /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; @@ -141,6 +173,7 @@ class ConcaveMeshShape : public ConcaveShape { // ---------- Friendship ----------- // friend class ConvexTriangleAABBOverlapCallback; + friend class ConcaveMeshRaycastCallback; }; // Return the number of bytes used by the collision shape @@ -209,13 +242,6 @@ inline void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decim 0, 0, mass); } -// Return true if a point is inside the collision shape -inline bool ConcaveMeshShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { - - // TODO : Implement this - return false; -} - // Called when a overlapping node has been found during the call to // DynamicAABBTree:reportAllShapesOverlappingWithAABB() inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId) { diff --git a/src/collision/shapes/ConcaveShape.h b/src/collision/shapes/ConcaveShape.h index 2ff0f86b..e15ee22e 100644 --- a/src/collision/shapes/ConcaveShape.h +++ b/src/collision/shapes/ConcaveShape.h @@ -68,6 +68,9 @@ class ConcaveShape : public CollisionShape { /// Private assignment operator ConcaveShape& operator=(const ConcaveShape& shape); + /// Return true if a point is inside the collision shape + virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; + public : // -------------------- Methods -------------------- // @@ -90,6 +93,11 @@ inline bool ConcaveShape::isConvex() const { return false; } +// Return true if a point is inside the collision shape +inline bool ConcaveShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { + return false; +} + } #endif diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h index 58e3e1a0..27acc416 100644 --- a/src/collision/shapes/SphereShape.h +++ b/src/collision/shapes/SphereShape.h @@ -97,7 +97,7 @@ class SphereShape : public ConvexShape { virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; /// Update the AABB of a body using its collision shape - virtual void computeAABB(AABB& aabb, const Transform& transform); + virtual void computeAABB(AABB& aabb, const Transform& transform) const; }; // Get the radius of the sphere @@ -183,7 +183,7 @@ inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal ma * computed in world-space coordinates * @param transform Transform used to compute the AABB of the collision shape */ -inline void SphereShape::computeAABB(AABB& aabb, const Transform& transform) { +inline void SphereShape::computeAABB(AABB& aabb, const Transform& transform) const { // Get the local extents in x,y and z direction Vector3 extents(mMargin, mMargin, mMargin); diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 0a0d540c..12b34da2 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -51,10 +51,57 @@ TriangleShape::~TriangleShape() { } // Raycast method with feedback information +/// This method use the line vs triangle raycasting technique described in +/// Real-time Collision Detection by Christer Ericson. bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - // TODO : Implement this + // TODO : For all collision shape, try to perform raycasting in local-space and + // compute world-space hit point, normal in upper classes when local-space + // hit points are returned + const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform(); + const Transform worldToLocalTransform = localToWorldTransform.getInverse(); + const Vector3 point1 = worldToLocalTransform * ray.point1; + const Vector3 point2 = worldToLocalTransform * ray.point2; - return false; + const Vector3 pq = point2 - point1; + const Vector3 pa = mPoints[0] - point1; + const Vector3 pb = mPoints[1] - point1; + const Vector3 pc = mPoints[2] - point1; + + // Test if the line PQ is inside the eges BC, CA and AB. We use the triple + // product for this test. + const Vector3 m = pq.cross(pc); + decimal u = pb.dot(m); + if (u < decimal(0.0)) return false; + + decimal v = -pa.dot(m); + if (v < decimal(0.0)) return false; + + decimal w = pa.dot(pq.cross(pb)); + if (w < decimal(0.0)) return false; + + // If the line PQ is in the triangle plane (case where u=v=w=0) + if (u < MACHINE_EPSILON && u < MACHINE_EPSILON && v < MACHINE_EPSILON) return false; + + // Compute the barycentric coordinates (u, v, w) to determine the + // intersection point R, R = u * a + v * b + w * c + decimal denom = decimal(1.0) / (u + v + w); + u *= denom; + v *= denom; + w *= denom; + + // Compute the local hit point using the barycentric coordinates + const Vector3 localHitPoint = u * mPoints[0] + v * mPoints[1] + w * mPoints[2]; + + const Vector3 localHitNormal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]); + + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.worldPoint = localToWorldTransform * localHitPoint; + raycastInfo.hitFraction = (localHitPoint - point1).length() / pq.length(); + raycastInfo.worldNormal = localToWorldTransform.getOrientation() * localHitNormal; + raycastInfo.worldNormal.normalize(); + + return true; } diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h index 7ffdd380..bdcfcd6d 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -92,7 +92,11 @@ class TriangleShape : public ConvexShape { virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; /// Update the AABB of a body using its collision shape - virtual void computeAABB(AABB& aabb, const Transform& transform); + virtual void computeAABB(AABB& aabb, const Transform& transform) const; + + // ---------- Friendship ---------- // + + friend class ConcaveMeshRaycastCallback; }; // Return the number of bytes used by the collision shape @@ -155,7 +159,7 @@ inline void TriangleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal * computed in world-space coordinates * @param transform Transform used to compute the AABB of the collision shape */ -inline void TriangleShape::computeAABB(AABB& aabb, const Transform& transform) { +inline void TriangleShape::computeAABB(AABB& aabb, const Transform& transform) const { const Vector3 worldPoint1 = transform * mPoints[0]; const Vector3 worldPoint2 = transform * mPoints[1]; diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index 2c86a7db..c4e4de91 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -36,6 +36,7 @@ #include "collision/shapes/ConeShape.h" #include "collision/shapes/ConvexMeshShape.h" #include "collision/shapes/CylinderShape.h" +#include "collision/shapes/TriangleShape.h" // TODO : Add test for concave shape here @@ -115,6 +116,7 @@ class TestRaycast : public Test { CollisionBody* mConvexMeshBodyEdgesInfo; CollisionBody* mCylinderBody; CollisionBody* mCompoundBody; + CollisionBody* mTriangleBody; // Transform Transform mBodyTransform; @@ -130,7 +132,7 @@ class TestRaycast : public Test { ConvexMeshShape* mConvexMeshShape; ConvexMeshShape* mConvexMeshShapeEdgesInfo; CylinderShape* mCylinderShape; - + TriangleShape* mTriangleShape; // Proxy Shapes ProxyShape* mBoxProxyShape; @@ -142,6 +144,7 @@ class TestRaycast : public Test { ProxyShape* mCylinderProxyShape; ProxyShape* mCompoundSphereProxyShape; ProxyShape* mCompoundCylinderProxyShape; + ProxyShape* mTriangleProxyShape; public : @@ -156,8 +159,11 @@ class TestRaycast : public Test { mWorld = new CollisionWorld(); // Body transform + // TODO : Uncomment this Vector3 position(-3, 2, 7); Quaternion orientation(PI / 5, PI / 6, PI / 7); +// Vector3 position(0, 0, 0); +// Quaternion orientation = Quaternion::identity(); mBodyTransform = Transform(position, orientation); // Create the bodies @@ -169,10 +175,14 @@ class TestRaycast : public Test { mConvexMeshBodyEdgesInfo = mWorld->createCollisionBody(mBodyTransform); mCylinderBody = mWorld->createCollisionBody(mBodyTransform); mCompoundBody = mWorld->createCollisionBody(mBodyTransform); + mTriangleBody = mWorld->createCollisionBody(mBodyTransform); // Collision shape transform + // TODO : Uncomment this Vector3 shapePosition(1, -4, -3); Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3); +// Vector3 shapePosition(0, 0, 0); +// Quaternion shapeOrientation = Quaternion::identity(); mShapeTransform = Transform(shapePosition, shapeOrientation); // Compute the the transform from a local shape point to world-space @@ -185,6 +195,12 @@ class TestRaycast : public Test { mSphereShape = new SphereShape(3); mSphereProxyShape = mSphereBody->addCollisionShape(mSphereShape, mShapeTransform); + const Vector3 triangleVertex1(100, 100, 0); + const Vector3 triangleVertex2(105, 100, 0); + const Vector3 triangleVertex3(100, 103, 0); + mTriangleShape = new TriangleShape(triangleVertex1, triangleVertex2, triangleVertex3); + mTriangleProxyShape = mTriangleBody->addCollisionShape(mTriangleShape, mShapeTransform); + mCapsuleShape = new CapsuleShape(2, 5); mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform); @@ -249,6 +265,7 @@ class TestRaycast : public Test { mCylinderProxyShape->setCollisionCategoryBits(CATEGORY2); mCompoundSphereProxyShape->setCollisionCategoryBits(CATEGORY2); mCompoundCylinderProxyShape->setCollisionCategoryBits(CATEGORY2); + mTriangleProxyShape->setCollisionCategoryBits(CATEGORY1); } /// Destructor @@ -260,6 +277,7 @@ class TestRaycast : public Test { delete mConvexMeshShape; delete mConvexMeshShapeEdgesInfo; delete mCylinderShape; + delete mTriangleShape; } /// Run the tests @@ -271,6 +289,7 @@ class TestRaycast : public Test { testConvexMesh(); testCylinder(); testCompound(); + testTriangle(); } /// Test the ProxyBoxShape::raycast(), CollisionBody::raycast() and @@ -935,6 +954,119 @@ class TestRaycast : public Test { test(mCallback.isHit); } + /// Test the ProxySphereShape::raycast(), CollisionBody::raycast() and + /// CollisionWorld::raycast() methods. + void testTriangle() { + + // ----- Test feedback data ----- // + Vector3 point1 = mLocalShapeToWorld * Vector3(101, 101, 400); + Vector3 point2 = mLocalShapeToWorld * Vector3(101, 101, -200); + Ray ray(point1, point2); + Vector3 hitPoint = mLocalShapeToWorld * Vector3(101, 101, 0); + + mCallback.shapeToTest = mTriangleProxyShape; + + // CollisionWorld::raycast() + mCallback.reset(); + mWorld->raycast(ray, &mCallback); + test(mCallback.isHit); + test(mCallback.raycastInfo.body == mTriangleBody); + test(mCallback.raycastInfo.proxyShape == mTriangleProxyShape); + test(approxEqual(mCallback.raycastInfo.hitFraction, 0.2, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + + // Correct category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY1); + test(mCallback.isHit); + + // Wrong category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY2); + test(!mCallback.isHit); + + // CollisionBody::raycast() + RaycastInfo raycastInfo2; + test(mTriangleBody->raycast(ray, raycastInfo2)); + test(raycastInfo2.body == mTriangleBody); + test(raycastInfo2.proxyShape == mTriangleProxyShape); + test(approxEqual(raycastInfo2.hitFraction, 0.2, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo3; + test(mTriangleProxyShape->raycast(ray, raycastInfo3)); + test(raycastInfo3.body == mTriangleBody); + test(raycastInfo3.proxyShape == mTriangleProxyShape); + test(approxEqual(raycastInfo3.hitFraction, 0.2, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + + Ray ray1(mLocalShapeToWorld * Vector3(-10, 10, 4), mLocalShapeToWorld * Vector3(15, 6, -4)); + Ray ray2(mLocalShapeToWorld * Vector3(102, 107, 5), mLocalShapeToWorld * Vector3(102, 107, -5)); + Ray ray3(mLocalShapeToWorld * Vector3(106, 102, 6), mLocalShapeToWorld * Vector3(106, 102, -8)); + + Ray ray4(mLocalShapeToWorld * Vector3(100.2, 101, 5), mLocalShapeToWorld * Vector3(100.2, 101, -5)); + Ray ray5(mLocalShapeToWorld * Vector3(100.5, 101.5, 4), mLocalShapeToWorld * Vector3(100.5, 101.5, -54)); + Ray ray6(mLocalShapeToWorld * Vector3(102, 101, 1), mLocalShapeToWorld * Vector3(102, 102, -1)); + + // ----- Test raycast miss ----- // + test(!mTriangleBody->raycast(ray1, raycastInfo3)); + test(!mTriangleProxyShape->raycast(ray1, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray1, &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); + test(!mCallback.isHit); + + test(!mTriangleBody->raycast(ray2, raycastInfo3)); + test(!mTriangleProxyShape->raycast(ray2, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray2, &mCallback); + test(!mCallback.isHit); + + test(!mTriangleBody->raycast(ray3, raycastInfo3)); + test(!mTriangleProxyShape->raycast(ray3, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray3, &mCallback); + test(!mCallback.isHit); + + // ----- Test raycast hits ----- // + test(mTriangleBody->raycast(ray4, raycastInfo3)); + test(mTriangleProxyShape->raycast(ray4, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray4, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray4.point1, ray4.point2, decimal(0.8)), &mCallback); + test(mCallback.isHit); + + test(mTriangleBody->raycast(ray5, raycastInfo3)); + test(mTriangleProxyShape->raycast(ray5, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray5, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray5.point1, ray5.point2, decimal(0.8)), &mCallback); + test(mCallback.isHit); + + test(mTriangleBody->raycast(ray6, raycastInfo3)); + test(mTriangleProxyShape->raycast(ray6, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray6, &mCallback); + mCallback.reset(); + mWorld->raycast(Ray(ray6.point1, ray6.point2, decimal(0.8)), &mCallback); + } + /// Test the ProxyConeShape::raycast(), CollisionBody::raycast() and /// CollisionWorld::raycast() methods. void testCone() { diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp index b63cb03c..0a23fc05 100644 --- a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp +++ b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp @@ -45,9 +45,6 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Gravity vector in the dynamics world rp3d::Vector3 gravity(0, -9.81, 0); - // Time step for the physics simulation - rp3d::decimal timeStep = 1.0f / 60.0f; - // Create the dynamics world for the physics simulation mDynamicsWorld = new rp3d::DynamicsWorld(gravity); diff --git a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp index 01791334..ad0f62ef 100644 --- a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp +++ b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp @@ -165,7 +165,7 @@ void ConcaveMeshScene::reset() { // Reset the transform mConcaveMesh->resetTransform(rp3d::Transform::identity()); - rp3d::Vector3 spherePos(10.1, 10, 0); + rp3d::Vector3 spherePos(0, 15, 0); rp3d::Transform sphereTransform(spherePos, rp3d::Quaternion::identity()); mSphere->resetTransform(sphereTransform); } diff --git a/testbed/scenes/raycast/RaycastScene.cpp b/testbed/scenes/raycast/RaycastScene.cpp index b2760012..2f5846ed 100644 --- a/testbed/scenes/raycast/RaycastScene.cpp +++ b/testbed/scenes/raycast/RaycastScene.cpp @@ -32,9 +32,9 @@ using namespace raycastscene; // Constructor RaycastScene::RaycastScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS, false), mCurrentBodyIndex(-1), mAreNormalsDisplayed(false), - mMeshFolderPath("meshes/"), mRaycastManager(mPhongShader, mMeshFolderPath), - mVBOVertices(GL_ARRAY_BUFFER) { + : SceneDemo(name, SCENE_RADIUS, false), mMeshFolderPath("meshes/"), + mRaycastManager(mPhongShader, mMeshFolderPath), mCurrentBodyIndex(-1), + mAreNormalsDisplayed(false), mVBOVertices(GL_ARRAY_BUFFER) { mIsContactPointsDisplayed = true; @@ -122,6 +122,16 @@ RaycastScene::RaycastScene(const std::string& name) mConvexMesh->setColor(mGreyColorDemo); mConvexMesh->setSleepingColor(mRedColorDemo); + // ---------- Concave Mesh ---------- // + openglframework::Vector3 position8(0, 0, 0); + + // Create a convex mesh and a corresponding collision body in the dynamics world + mConcaveMesh = new ConcaveMesh(position8, mCollisionWorld, mMeshFolderPath); + + // Set the color + mConcaveMesh->setColor(mGreyColorDemo); + mConcaveMesh->setSleepingColor(mRedColorDemo); + // Create the lines that will be used for raycasting createLines(); @@ -173,6 +183,7 @@ void RaycastScene::changeBody() { mCapsule->getCollisionBody()->setIsActive(false); mConvexMesh->getCollisionBody()->setIsActive(false); mDumbbell->getCollisionBody()->setIsActive(false); + mConcaveMesh->getCollisionBody()->setIsActive(false); switch(mCurrentBodyIndex) { case 0: mSphere->getCollisionBody()->setIsActive(true); @@ -189,6 +200,8 @@ void RaycastScene::changeBody() { break; case 6: mDumbbell->getCollisionBody()->setIsActive(true); break; + case 7: mConcaveMesh->getCollisionBody()->setIsActive(true); + break; } } @@ -237,9 +250,15 @@ RaycastScene::~RaycastScene() { // Destroy the corresponding rigid body from the dynamics world mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody()); - // Destroy the convex mesh + // Destroy the dumbbell delete mDumbbell; + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody()); + + // Destroy the convex mesh + delete mConcaveMesh; + mRaycastManager.resetPoints(); // Destroy the static data for the visual contact points @@ -345,8 +364,8 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader, if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix); if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix); if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix); + if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix); - //mPhongShader.unbind(); shader.unbind(); } diff --git a/testbed/scenes/raycast/RaycastScene.h b/testbed/scenes/raycast/RaycastScene.h index ed0209af..fa62ab5a 100644 --- a/testbed/scenes/raycast/RaycastScene.h +++ b/testbed/scenes/raycast/RaycastScene.h @@ -39,6 +39,7 @@ #include "Capsule.h" #include "Line.h" #include "ConvexMesh.h" +#include "ConcaveMesh.h" #include "Dumbbell.h" #include "VisualContactPoint.h" #include "../common/Viewer.h" @@ -58,7 +59,7 @@ const float CAPSULE_HEIGHT = 5.0f; const float DUMBBELL_HEIGHT = 5.0f; const int NB_RAYS = 100; const float RAY_LENGTH = 30.0f; -const int NB_BODIES = 7; +const int NB_BODIES = 8; // Raycast manager class RaycastManager : public rp3d::RaycastCallback { @@ -145,6 +146,7 @@ class RaycastScene : public SceneDemo { Capsule* mCapsule; ConvexMesh* mConvexMesh; Dumbbell* mDumbbell; + ConcaveMesh* mConcaveMesh; /// Collision world used for the physics simulation rp3d::CollisionWorld* mCollisionWorld;