diff --git a/CMakeLists.txt b/CMakeLists.txt index 169476c9..0566a735 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -199,6 +199,8 @@ SET (REACTPHYSICS3D_SOURCES "src/containers/LinkedList.h" "src/containers/List.h" "src/containers/Map.h" + "src/containers/Set.h" + "src/containers/Pair.h" ) # Create the library diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 66cf1eb5..32269336 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -37,19 +37,19 @@ #include "collision/OverlapCallback.h" #include #include -#include #include #include -#include // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; using namespace std; + // Constructor CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryManager& memoryManager) - : mMemoryManager(memoryManager), mWorld(world), mNarrowPhaseInfoList(nullptr), mBroadPhaseAlgorithm(*this), - mIsCollisionShapesAdded(false) { + : mMemoryManager(memoryManager), mWorld(world), mNarrowPhaseInfoList(nullptr), + mOverlappingPairs(mMemoryManager.getPoolAllocator()), mBroadPhaseAlgorithm(*this), + mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mIsCollisionShapesAdded(false) { // Set the default collision dispatch configuration setCollisionDispatch(&mDefaultCollisionDispatch); @@ -104,7 +104,7 @@ void CollisionDetection::computeMiddlePhase() { PROFILE("CollisionDetection::computeMiddlePhase()", mProfiler); // For each possible collision pair of bodies - map::iterator it; + Map, OverlappingPair*>::Iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { OverlappingPair* pair = it->second; @@ -126,14 +126,11 @@ void CollisionDetection::computeMiddlePhase() { // overlapping pair if (!mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { - std::map::iterator itToRemove = it; - ++it; - // Destroy the overlapping pair - itToRemove->second->~OverlappingPair(); + it->second->~OverlappingPair(); - mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, itToRemove->second, sizeof(OverlappingPair)); - mOverlappingPairs.erase(itToRemove); + mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, it->second, sizeof(OverlappingPair)); + it = mOverlappingPairs.remove(it); continue; } else { @@ -154,7 +151,7 @@ void CollisionDetection::computeMiddlePhase() { // Check if the bodies are in the set of bodies that cannot collide between each other bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); - if (mNoCollisionPairs.count(bodiesIndex) > 0) continue; + if (mNoCollisionPairs.contains(bodiesIndex) > 0) continue; bool isShape1Convex = shape1->getCollisionShape()->isConvex(); bool isShape2Convex = shape2->getCollisionShape()->isConvex(); @@ -320,10 +317,10 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return; // Compute the overlapping pair ID - overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2); + Pair pairID = OverlappingPair::computeID(shape1, shape2); // Check if the overlapping pair already exists - if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return; + if (mOverlappingPairs.containsKey(pairID)) return; // Create the overlapping pair and add it into the set of overlapping pairs OverlappingPair* newPair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair))) @@ -331,11 +328,7 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro mMemoryManager.getSingleFrameAllocator()); assert(newPair != nullptr); -#ifndef NDEBUG - std::pair::iterator, bool> check = -#endif - mOverlappingPairs.insert(make_pair(pairID, newPair)); - assert(check.second); + mOverlappingPairs.add(Pair, OverlappingPair*>(pairID, newPair)); // Wake up the two bodies shape1->getBody()->setIsSleeping(false); @@ -348,19 +341,17 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { assert(proxyShape->mBroadPhaseID != -1); // Remove all the overlapping pairs involving this proxy shape - std::map::iterator it; + Map, OverlappingPair*>::Iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { if (it->second->getShape1()->mBroadPhaseID == proxyShape->mBroadPhaseID|| it->second->getShape2()->mBroadPhaseID == proxyShape->mBroadPhaseID) { - std::map::iterator itToRemove = it; - ++it; // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved // Destroy the overlapping pair - itToRemove->second->~OverlappingPair(); - mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, itToRemove->second, sizeof(OverlappingPair)); - mOverlappingPairs.erase(itToRemove); + it->second->~OverlappingPair(); + mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, it->second, sizeof(OverlappingPair)); + it = mOverlappingPairs.remove(it); } else { ++it; @@ -376,7 +367,7 @@ void CollisionDetection::addAllContactManifoldsToBodies() { PROFILE("CollisionDetection::addAllContactManifoldsToBodies()", mProfiler); // For each overlapping pairs in contact during the narrow-phase - std::map::iterator it; + Map, OverlappingPair*>::Iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { // Add all the contact manifolds of the pair into the list of contact manifolds @@ -427,7 +418,7 @@ void CollisionDetection::processAllPotentialContacts() { PROFILE("CollisionDetection::processAllPotentialContacts()", mProfiler); // For each overlapping pairs in contact during the narrow-phase - std::map::iterator it; + Map, OverlappingPair*>::Iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { // Process the potential contacts of the overlapping pair @@ -466,7 +457,7 @@ void CollisionDetection::reportAllContacts() { PROFILE("CollisionDetection::reportAllContacts()", mProfiler); // For each overlapping pairs in contact during the narrow-phase - std::map::iterator it; + Map, OverlappingPair*>::Iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { // If there is a user callback @@ -524,7 +515,7 @@ void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* over unsigned short categoryMaskBits) { assert(overlapCallback != nullptr); - std::unordered_set reportedBodies; + Set reportedBodies(mMemoryManager.getPoolAllocator()); // Ask the broad-phase to get all the overlapping shapes LinkedList overlappingNodes(mMemoryManager.getPoolAllocator()); @@ -547,7 +538,7 @@ void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* over if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { // Add the body into the set of reported bodies - reportedBodies.insert(overlapBody->getID()); + reportedBodies.add(overlapBody->getID()); // Notify the overlap to the user overlapCallback->notifyOverlap(overlapBody); @@ -640,7 +631,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl assert(overlapCallback != nullptr); - std::unordered_set reportedBodies; + Set reportedBodies(mMemoryManager.getPoolAllocator()); // For each proxy shape proxy shape of the body ProxyShape* bodyProxyShape = body->getProxyShapesList(); @@ -720,7 +711,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl CollisionBody* overlapBody = proxyShape->getBody(); // Add the body into the set of reported bodies - reportedBodies.insert(overlapBody->getID()); + reportedBodies.add(overlapBody->getID()); // Notify the overlap to the user overlapCallback->notifyOverlap(overlapBody); @@ -921,7 +912,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { computeBroadPhase(); // For each possible collision pair of bodies - map::iterator it; + Map, OverlappingPair*>::Iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { OverlappingPair* originalPair = it->second; diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 6aeac709..4dc45634 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -34,10 +34,9 @@ #include "narrowphase/DefaultCollisionDispatch.h" #include "memory/MemoryManager.h" #include "constraint/ContactPoint.h" -#include -#include +#include "containers/Map.h" +#include "containers/Set.h" #include -#include /// ReactPhysics3D namespace namespace reactphysics3d { @@ -80,7 +79,7 @@ class CollisionDetection { NarrowPhaseInfo* mNarrowPhaseInfoList; /// Broad-phase overlapping pairs - std::map mOverlappingPairs; + Map, OverlappingPair*> mOverlappingPairs; /// Broad-phase algorithm BroadPhaseAlgorithm mBroadPhaseAlgorithm; @@ -89,9 +88,8 @@ class CollisionDetection { // TODO : Delete this GJKAlgorithm mNarrowPhaseGJKAlgorithm; - // TODO : Maybe delete this set (what is the purpose ?) /// Set of pair of bodies that cannot collide between each other - std::set mNoCollisionPairs; + Set mNoCollisionPairs; /// True if some collision shapes have been added previously bool mIsCollisionShapesAdded; @@ -213,6 +211,9 @@ class CollisionDetection { /// Allow the broadphase to notify the collision detection about an overlapping pair. void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2); + /// Return a reference to the memory manager + MemoryManager& getMemoryManager() const; + /// Return a pointer to the world CollisionWorld* getWorld(); @@ -264,13 +265,13 @@ inline void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape, // Add a pair of bodies that cannot collide with each other inline void CollisionDetection::addNoCollisionPair(CollisionBody* body1, CollisionBody* body2) { - mNoCollisionPairs.insert(OverlappingPair::computeBodiesIndexPair(body1, body2)); + mNoCollisionPairs.add(OverlappingPair::computeBodiesIndexPair(body1, body2)); } // Remove a pair of bodies that cannot collide with each other inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1, CollisionBody* body2) { - mNoCollisionPairs.erase(OverlappingPair::computeBodiesIndexPair(body1, body2)); + mNoCollisionPairs.remove(OverlappingPair::computeBodiesIndexPair(body1, body2)); } // Ask for a collision shape to be tested again during broad-phase. @@ -327,6 +328,11 @@ inline CollisionWorld* CollisionDetection::getWorld() { return mWorld; } +// Return a reference to the memory manager +inline MemoryManager& CollisionDetection::getMemoryManager() const { + return mMemoryManager; +} + #ifdef IS_PROFILING_ACTIVE // Set the profiler diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index f513ad82..eea392d5 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -27,7 +27,6 @@ #define REACTPHYSICS3D_CONTACT_MANIFOLD_H // Libraries -#include #include "body/CollisionBody.h" #include "collision/ProxyShape.h" #include "constraint/ContactPoint.h" diff --git a/src/collision/HalfEdgeStructure.cpp b/src/collision/HalfEdgeStructure.cpp index 70a731d5..b5fd6140 100644 --- a/src/collision/HalfEdgeStructure.cpp +++ b/src/collision/HalfEdgeStructure.cpp @@ -25,37 +25,37 @@ // Libraries #include "HalfEdgeStructure.h" -#include +#include "containers/Map.h" +#include "containers/Pair.h" +#include "containers/containers_common.h" using namespace reactphysics3d; // Initialize the structure (when all vertices and faces have been added) void HalfEdgeStructure::init() { - using edgeKey = std::pair; + Map edges(mAllocator); + Map nextEdges(mAllocator); + Map mapEdgeToStartVertex(mAllocator); + Map mapEdgeToIndex(mAllocator); + Map mapEdgeIndexToKey(mAllocator); + Map mapFaceIndexToEdgeKey(mAllocator); - std::map edges; - std::map nextEdges; - std::map mapEdgeToStartVertex; - std::map mapEdgeToIndex; - std::map mapEdgeIndexToKey; - std::map mapFaceIndexToEdgeKey; + List currentFaceEdges(mAllocator, mFaces[0].faceVertices.size()); // For each face for (uint f=0; f currentFaceEdges; - - edgeKey firstEdgeKey; + VerticesPair firstEdgeKey(0, 0); // For each vertex of the face for (uint v=0; v < face.faceVertices.size(); v++) { uint v1Index = face.faceVertices[v]; uint v2Index = face.faceVertices[v == (face.faceVertices.size() - 1) ? 0 : v + 1]; - const edgeKey pairV1V2 = std::make_pair(v1Index, v2Index); + const VerticesPair pairV1V2 = VerticesPair(v1Index, v2Index); // Create a new half-edge Edge edge; @@ -65,19 +65,19 @@ void HalfEdgeStructure::init() { firstEdgeKey = pairV1V2; } else if (v >= 1) { - nextEdges.insert(std::make_pair(currentFaceEdges[currentFaceEdges.size() - 1], pairV1V2)); + nextEdges.add(Pair(currentFaceEdges[currentFaceEdges.size() - 1], pairV1V2)); } if (v == (face.faceVertices.size() - 1)) { - nextEdges.insert(std::make_pair(pairV1V2, firstEdgeKey)); + nextEdges.add(Pair(pairV1V2, firstEdgeKey)); } - edges.insert(std::make_pair(pairV1V2, edge)); + edges.add(Pair(pairV1V2, edge)); - const edgeKey pairV2V1 = std::make_pair(v2Index, v1Index); + const VerticesPair pairV2V1(v2Index, v1Index); - mapEdgeToStartVertex.insert(std::make_pair(pairV1V2, v1Index)); - mapEdgeToStartVertex.insert(std::make_pair(pairV2V1, v2Index)); + mapEdgeToStartVertex.add(Pair(pairV1V2, v1Index), true); + mapEdgeToStartVertex.add(Pair(pairV2V1, v2Index), true); - mapFaceIndexToEdgeKey.insert(std::make_pair(f, pairV1V2)); + mapFaceIndexToEdgeKey.add(Pair(f, pairV1V2), true); auto itEdge = edges.find(pairV2V1); if (itEdge != edges.end()) { @@ -87,21 +87,23 @@ void HalfEdgeStructure::init() { itEdge->second.twinEdgeIndex = edgeIndex + 1; edge.twinEdgeIndex = edgeIndex; - mapEdgeIndexToKey[edgeIndex] = pairV2V1; - mapEdgeIndexToKey[edgeIndex + 1] = pairV1V2; + mapEdgeIndexToKey.add(Pair(edgeIndex, pairV2V1)); + mapEdgeIndexToKey.add(Pair(edgeIndex + 1, pairV1V2)); mVertices[v1Index].edgeIndex = edgeIndex + 1; mVertices[v2Index].edgeIndex = edgeIndex; - mapEdgeToIndex.insert(std::make_pair(pairV1V2, edgeIndex + 1)); - mapEdgeToIndex.insert(std::make_pair(pairV2V1, edgeIndex)); + mapEdgeToIndex.add(Pair(pairV1V2, edgeIndex + 1)); + mapEdgeToIndex.add(Pair(pairV2V1, edgeIndex)); mEdges.add(itEdge->second); mEdges.add(edge); } - currentFaceEdges.push_back(pairV1V2); + currentFaceEdges.add(pairV1V2); } + + currentFaceEdges.clear(); } // Set next edges diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h index 15658a93..3f351533 100644 --- a/src/collision/HalfEdgeStructure.h +++ b/src/collision/HalfEdgeStructure.h @@ -28,7 +28,6 @@ // Libraries #include "mathematics/mathematics.h" -#include namespace reactphysics3d { @@ -42,6 +41,9 @@ class HalfEdgeStructure { public: + using VerticesPair = Pair; + + /// Edge struct Edge { uint vertexIndex; // Index of the vertex at the beginning of the edge uint twinEdgeIndex; // Index of the twin edge @@ -49,6 +51,7 @@ class HalfEdgeStructure { uint nextEdgeIndex; // Index of the next edge }; + /// Face struct Face { uint edgeIndex; // Index of an half-edge of the face List faceVertices; // Index of the vertices of the face @@ -60,6 +63,7 @@ class HalfEdgeStructure { Face(List vertices) : faceVertices(vertices) {} }; + /// Vertex struct Vertex { uint vertexPointIndex; // Index of the vertex point in the origin vertex array uint edgeIndex; // Index of one edge emanting from this vertex diff --git a/src/collision/PolyhedronMesh.h b/src/collision/PolyhedronMesh.h index b459723a..8720a8db 100644 --- a/src/collision/PolyhedronMesh.h +++ b/src/collision/PolyhedronMesh.h @@ -31,7 +31,6 @@ #include "HalfEdgeStructure.h" #include "collision/PolygonVertexArray.h" #include "memory/DefaultAllocator.h" -#include namespace reactphysics3d { @@ -86,6 +85,9 @@ class PolyhedronMesh { /// Return a vertex Vector3 getVertex(uint index) const; + /// Return the number of faces + uint getNbFaces() const; + /// Return a face normal Vector3 getFaceNormal(uint faceIndex) const; @@ -101,6 +103,11 @@ inline uint PolyhedronMesh::getNbVertices() const { return mHalfEdgeStructure.getNbVertices(); } +// Return the number of faces +inline uint PolyhedronMesh::getNbFaces() const { + return mHalfEdgeStructure.getNbFaces(); +} + // Return a face normal inline Vector3 PolyhedronMesh::getFaceNormal(uint faceIndex) const { assert(faceIndex < mHalfEdgeStructure.getNbFaces()); diff --git a/src/collision/ProxyShape.cpp b/src/collision/ProxyShape.cpp index 8847d094..e20ae485 100644 --- a/src/collision/ProxyShape.cpp +++ b/src/collision/ProxyShape.cpp @@ -62,7 +62,7 @@ bool ProxyShape::testPointInside(const Vector3& worldPoint) { * @param ray Ray to use for the raycasting * @param[out] raycastInfo Result of the raycasting that is valid only if the * methods returned true - * @return True if the ray hit the collision shape + * @return True if the ray hits the collision shape */ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) { diff --git a/src/collision/TriangleMesh.h b/src/collision/TriangleMesh.h index e33fad67..69196d22 100644 --- a/src/collision/TriangleMesh.h +++ b/src/collision/TriangleMesh.h @@ -27,9 +27,10 @@ #define REACTPHYSICS3D_TRIANGLE_MESH_H // Libraries -#include #include #include "TriangleVertexArray.h" +#include "memory/MemoryManager.h" +#include "containers/List.h" namespace reactphysics3d { @@ -46,12 +47,14 @@ class TriangleMesh { protected: /// All the triangle arrays of the mesh (one triangle array per part) - std::vector mTriangleArrays; + List mTriangleArrays; public: /// Constructor - TriangleMesh() = default; + TriangleMesh() : mTriangleArrays(MemoryManager::getBaseAllocator()) { + + } /// Destructor ~TriangleMesh() = default; @@ -68,7 +71,7 @@ class TriangleMesh { // Add a subpart of the mesh inline void TriangleMesh::addSubpart(TriangleVertexArray* triangleVertexArray) { - mTriangleArrays.push_back(triangleVertexArray ); + mTriangleArrays.add(triangleVertexArray ); } // Return a pointer to a given subpart (triangle vertex array) of the mesh diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.cpp b/src/collision/broadphase/BroadPhaseAlgorithm.cpp index 8998859e..5f76e2c2 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/src/collision/broadphase/BroadPhaseAlgorithm.cpp @@ -33,16 +33,19 @@ using namespace reactphysics3d; // Constructor BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection) - :mDynamicAABBTree(DYNAMIC_TREE_AABB_GAP), mNbMovedShapes(0), mNbAllocatedMovedShapes(8), + :mDynamicAABBTree(collisionDetection.getMemoryManager().getPoolAllocator(), DYNAMIC_TREE_AABB_GAP), + mNbMovedShapes(0), mNbAllocatedMovedShapes(8), mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8), mCollisionDetection(collisionDetection) { + PoolAllocator& poolAllocator = collisionDetection.getMemoryManager().getPoolAllocator(); + // Allocate memory for the array of non-static proxy shapes IDs - mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int)); + mMovedShapes = static_cast(poolAllocator.allocate(mNbAllocatedMovedShapes * sizeof(int))); assert(mMovedShapes != nullptr); // Allocate memory for the array of potential overlapping pairs - mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); + mPotentialPairs = static_cast(poolAllocator.allocate(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair))); assert(mPotentialPairs != nullptr); #ifdef IS_PROFILING_ACTIVE @@ -56,25 +59,34 @@ BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection) // Destructor BroadPhaseAlgorithm::~BroadPhaseAlgorithm() { + // Get the memory pool allocatory + PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator(); + // Release the memory for the array of non-static proxy shapes IDs - free(mMovedShapes); + poolAllocator.release(mMovedShapes, mNbAllocatedMovedShapes * sizeof (int)); // Release the memory for the array of potential overlapping pairs - free(mPotentialPairs); + poolAllocator.release(mPotentialPairs, mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); } // Add a collision shape in the array of shapes that have moved in the last simulation step // and that need to be tested again for broad-phase overlapping. void BroadPhaseAlgorithm::addMovedCollisionShape(int broadPhaseID) { + // Allocate more elements in the array of shapes that have moved if necessary if (mNbAllocatedMovedShapes == mNbMovedShapes) { + + // Get the memory pool allocatory + PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator(); + + uint oldNbAllocatedMovedShapes = mNbAllocatedMovedShapes; mNbAllocatedMovedShapes *= 2; int* oldArray = mMovedShapes; - mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int)); + mMovedShapes = static_cast(poolAllocator.allocate(mNbAllocatedMovedShapes * sizeof(int))); assert(mMovedShapes != nullptr); - memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int)); - free(oldArray); + std::memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int)); + poolAllocator.release(oldArray, oldNbAllocatedMovedShapes * sizeof(int)); } // Store the broad-phase ID into the array of shapes that have moved @@ -95,9 +107,13 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) { if ((mNbMovedShapes - mNbNonUsedMovedShapes) < mNbAllocatedMovedShapes / 4 && mNbAllocatedMovedShapes > 8) { + // Get the memory pool allocatory + PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator(); + + uint oldNbAllocatedMovedShapes = mNbAllocatedMovedShapes; mNbAllocatedMovedShapes /= 2; int* oldArray = mMovedShapes; - mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int)); + mMovedShapes = static_cast(poolAllocator.allocate(mNbAllocatedMovedShapes * sizeof(int))); assert(mMovedShapes != nullptr); uint nbElements = 0; for (uint i=0; i 8) { + PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator(); + // Reduce the number of allocated potential overlapping pairs BroadPhasePair* oldPairs = mPotentialPairs; + uint oldNbAllocatedPotentialPairs = mNbAllocatedPotentialPairs; mNbAllocatedPotentialPairs /= 2; - mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); + mPotentialPairs = static_cast(poolAllocator.allocate(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair))); assert(mPotentialPairs); memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair)); - free(oldPairs); + poolAllocator.release(oldPairs, oldNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); } } @@ -289,13 +308,16 @@ void BroadPhaseAlgorithm::addOverlappingNodes(int referenceNodeId, const LinkedL // If we need to allocate more memory for the array of potential overlapping pairs if (mNbPotentialPairs == mNbAllocatedPotentialPairs) { + PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator(); + // Allocate more memory for the array of potential pairs BroadPhasePair* oldPairs = mPotentialPairs; + uint oldNbAllocatedPotentialPairs = mNbAllocatedPotentialPairs; mNbAllocatedPotentialPairs *= 2; - mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); + mPotentialPairs = static_cast(poolAllocator.allocate(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair))); assert(mPotentialPairs); memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair)); - free(oldPairs); + poolAllocator.release(oldPairs, oldNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); } // Add the new potential pair into the array of potential overlapping pairs diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h index 30a67920..3ae18677 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.h +++ b/src/collision/broadphase/BroadPhaseAlgorithm.h @@ -27,7 +27,6 @@ #define REACTPHYSICS3D_BROAD_PHASE_ALGORITHM_H // Libraries -#include #include "body/CollisionBody.h" #include "collision/ProxyShape.h" #include "DynamicAABBTree.h" diff --git a/src/collision/broadphase/DynamicAABBTree.cpp b/src/collision/broadphase/DynamicAABBTree.cpp index 5fa33d9d..bbc4b109 100644 --- a/src/collision/broadphase/DynamicAABBTree.cpp +++ b/src/collision/broadphase/DynamicAABBTree.cpp @@ -35,7 +35,8 @@ using namespace reactphysics3d; const int TreeNode::NULL_TREE_NODE = -1; // Constructor -DynamicAABBTree::DynamicAABBTree(decimal extraAABBGap) : mExtraAABBGap(extraAABBGap) { +DynamicAABBTree::DynamicAABBTree(MemoryAllocator& allocator, decimal extraAABBGap) + : mAllocator(allocator), mExtraAABBGap(extraAABBGap) { init(); } @@ -44,7 +45,7 @@ DynamicAABBTree::DynamicAABBTree(decimal extraAABBGap) : mExtraAABBGap(extraAABB DynamicAABBTree::~DynamicAABBTree() { // Free the allocated memory for the nodes - free(mNodes); + mAllocator.release(mNodes, mNbAllocatedNodes * sizeof(TreeNode)); } // Initialize the tree @@ -55,9 +56,9 @@ void DynamicAABBTree::init() { mNbAllocatedNodes = 8; // Allocate memory for the nodes of the tree - mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode)); + mNodes = static_cast(mAllocator.allocate(mNbAllocatedNodes * sizeof(TreeNode))); assert(mNodes); - memset(mNodes, 0, mNbAllocatedNodes * sizeof(TreeNode)); + std::memset(mNodes, 0, mNbAllocatedNodes * sizeof(TreeNode)); // Initialize the allocated nodes for (int i=0; i(mAllocator.allocate(mNbAllocatedNodes * sizeof(TreeNode))); assert(mNodes); memcpy(mNodes, oldNodes, mNbNodes * sizeof(TreeNode)); - free(oldNodes); + mAllocator.release(oldNodes, oldNbAllocatedNodes * sizeof(TreeNode)); // Initialize the allocated nodes for (int i=mNbNodes; i stack; + Stack stack(mAllocator); stack.push(mRootNodeID); // While there are still nodes to visit @@ -637,7 +639,7 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &ca decimal maxFraction = ray.maxFraction; - Stack stack; + Stack stack(mAllocator); stack.push(mRootNodeID); // Walk through the tree from the root looking for proxy shapes diff --git a/src/collision/broadphase/DynamicAABBTree.h b/src/collision/broadphase/DynamicAABBTree.h index 54751138..936595a5 100644 --- a/src/collision/broadphase/DynamicAABBTree.h +++ b/src/collision/broadphase/DynamicAABBTree.h @@ -30,6 +30,7 @@ #include "configuration.h" #include "collision/shapes/AABB.h" #include "body/CollisionBody.h" +#include "memory/MemoryAllocator.h" /// Namespace ReactPhysics3D namespace reactphysics3d { @@ -136,6 +137,9 @@ class DynamicAABBTree { // -------------------- Attributes -------------------- // + /// Memory allocator + MemoryAllocator& mAllocator; + /// Pointer to the memory location of the nodes of the tree TreeNode* mNodes; @@ -203,7 +207,7 @@ class DynamicAABBTree { // -------------------- Methods -------------------- // /// Constructor - DynamicAABBTree(decimal extraAABBGap = decimal(0.0)); + DynamicAABBTree(MemoryAllocator& allocator, decimal extraAABBGap = decimal(0.0)); /// Destructor ~DynamicAABBTree(); diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 62685867..a7bc8667 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -212,171 +212,3 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase return GJKResult::INTERPENETRATE; } - - -// Use the GJK Algorithm to find if a point is inside a convex collision shape -bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) { - - Vector3 suppA; // Support point of object A - Vector3 w; // Support point of Minkowski difference A-B - decimal prevDistSquare; - - assert(proxyShape->getCollisionShape()->isConvex()); - - const ConvexShape* shape = static_cast(proxyShape->getCollisionShape()); - - // Support point of object B (object B is a single point) - const Vector3 suppB(localPoint); - - // Create a simplex set - VoronoiSimplex simplex; - - // Initial supporting direction - 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 = shape->getLocalSupportPointWithoutMargin(-v); - - // Compute the support point for the Minkowski difference A-B - w = suppA - suppB; - - // 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 point of the simplex closest to the origin - // If the computation of the closest point fail - if (!simplex.computeClosestPoint(v)) { - - return false; - } - - // 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; - } - } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * - simplex.getMaxLengthSquareOfAPoint()); - - // The point is inside the collision shape - return true; -} - -// Ray casting algorithm agains a convex collision shape using the GJK Algorithm -/// This method implements the GJK ray casting algorithm described by Gino Van Den Bergen in -/// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection". -bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo) { - - assert(proxyShape->getCollisionShape()->isConvex()); - - const ConvexShape* shape = static_cast(proxyShape->getCollisionShape()); - - Vector3 suppA; // Current lower bound point on the ray (starting at ray's origin) - Vector3 suppB; // Support point on the collision shape - const decimal machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON; - const decimal epsilon = decimal(0.0001); - - // Convert the ray origin and direction into the local-space of the collision shape - Vector3 rayDirection = ray.point2 - ray.point1; - - // If the points of the segment are two close, return no hit - if (rayDirection.lengthSquare() < machineEpsilonSquare) return false; - - Vector3 w; - - // Create a simplex set - VoronoiSimplex simplex; - - Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0)); - decimal lambda = decimal(0.0); - suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin) - suppB = shape->getLocalSupportPointWithoutMargin(rayDirection); - Vector3 v = suppA - suppB; - decimal vDotW, vDotR; - decimal distSquare = v.lengthSquare(); - int nbIterations = 0; - - // GJK Algorithm loop - while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) { - - // Compute the support points - suppB = shape->getLocalSupportPointWithoutMargin(v); - w = suppA - suppB; - - vDotW = v.dot(w); - - if (vDotW > decimal(0)) { - - vDotR = v.dot(rayDirection); - - if (vDotR >= -machineEpsilonSquare) { - return false; - } - else { - - // We have found a better lower bound for the hit point along the ray - lambda = lambda - vDotW / vDotR; - suppA = ray.point1 + lambda * rayDirection; - w = suppA - suppB; - n = v; - } - } - - // Add the new support point to the simplex - if (!simplex.isPointInSimplex(w)) { - simplex.addPoint(w, suppA, suppB); - } - - // Compute the closest point - if (simplex.computeClosestPoint(v)) { - - distSquare = v.lengthSquare(); - } - else { - distSquare = decimal(0.0); - } - - // If the current lower bound distance is larger than the maximum raycasting distance - if (lambda > ray.maxFraction) return false; - - nbIterations++; - } - - // If the origin was inside the shape, we return no hit - if (lambda < MACHINE_EPSILON) return false; - - // Compute the closet points of both objects (without the margins) - Vector3 pointA; - Vector3 pointB; - simplex.computeClosestPointsOfAandB(pointA, pointB); - - // A raycast hit has been found, we fill in the raycast info - raycastInfo.hitFraction = lambda; - raycastInfo.worldPoint = pointB; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - - if (n.lengthSquare() >= machineEpsilonSquare) { // The normal vector is valid - raycastInfo.worldNormal = n; - } - else { // Degenerated normal vector, we return a zero normal vector - raycastInfo.worldNormal = Vector3(decimal(0), decimal(0), decimal(0)); - } - - return true; -} diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index a9daa897..ac9cd652 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -96,12 +96,6 @@ class GJKAlgorithm { /// Compute a contact info if the two bounding volumes collide. GJKResult testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts); - /// Use the GJK Algorithm to find if a point is inside a convex collision shape - bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape); - - /// Ray casting algorithm agains a convex collision shape using the GJK Algorithm - bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo); - #ifdef IS_PROFILING_ACTIVE /// Set the profiler diff --git a/src/collision/narrowphase/GJK/VoronoiSimplex.h b/src/collision/narrowphase/GJK/VoronoiSimplex.h index 9cbee47e..47f1b2c2 100644 --- a/src/collision/narrowphase/GJK/VoronoiSimplex.h +++ b/src/collision/narrowphase/GJK/VoronoiSimplex.h @@ -28,7 +28,6 @@ // Libraries #include "mathematics/mathematics.h" -#include /// ReactPhysics3D namespace namespace reactphysics3d { diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index 2e86dfe3..fa5d68b9 100755 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -121,6 +121,10 @@ bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, b contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * capsuleShape->getRadius(); } + if (penetrationDepth <= decimal(0.0)) { + return false; + } + // Create the contact info object narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal, diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index c13e32f4..109b736a 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -28,7 +28,6 @@ #include "collision/ProxyShape.h" #include "configuration.h" #include "memory/MemoryManager.h" -#include #include using namespace reactphysics3d; diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp index 8f215fc2..d2046adc 100644 --- a/src/collision/shapes/ConcaveMeshShape.cpp +++ b/src/collision/shapes/ConcaveMeshShape.cpp @@ -25,12 +25,13 @@ // Libraries #include "ConcaveMeshShape.h" +#include "memory/MemoryManager.h" using namespace reactphysics3d; // Constructor ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh) - : ConcaveShape(CollisionShapeName::TRIANGLE_MESH) { + : ConcaveShape(CollisionShapeName::TRIANGLE_MESH), mDynamicAABBTree(MemoryManager::getBaseAllocator()) { mTriangleMesh = triangleMesh; mRaycastTestType = TriangleRaycastSide::FRONT; @@ -155,7 +156,7 @@ uint ConcaveMeshShape::computeTriangleShapeId(uint subPart, uint triangleIndex) decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) { // Add the id of the hit AABB node into - mHitAABBNodes.push_back(nodeId); + mHitAABBNodes.add(nodeId); return ray.maxFraction; } @@ -163,7 +164,7 @@ decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const R // Raycast all collision shapes that have been collected void ConcaveMeshRaycastCallback::raycastTriangles() { - std::vector::const_iterator it; + List::Iterator it; decimal smallestHitFraction = mRay.maxFraction; for (it = mHitAABBNodes.begin(); it != mHitAABBNodes.end(); ++it) { diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h index c8c91972..2ecefb94 100644 --- a/src/collision/shapes/ConcaveMeshShape.h +++ b/src/collision/shapes/ConcaveMeshShape.h @@ -31,6 +31,7 @@ #include "collision/broadphase/DynamicAABBTree.h" #include "collision/TriangleMesh.h" #include "collision/shapes/TriangleShape.h" +#include "containers/List.h" #include "engine/Profiler.h" namespace reactphysics3d { @@ -70,7 +71,7 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { private : - std::vector mHitAABBNodes; + List mHitAABBNodes; const DynamicAABBTree& mDynamicAABBTree; const ConcaveMeshShape& mConcaveMeshShape; ProxyShape* mProxyShape; @@ -91,7 +92,7 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { // Constructor ConcaveMeshRaycastCallback(const DynamicAABBTree& dynamicAABBTree, const ConcaveMeshShape& concaveMeshShape, ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray, MemoryAllocator& allocator) - : mDynamicAABBTree(dynamicAABBTree), mConcaveMeshShape(concaveMeshShape), mProxyShape(proxyShape), + : mHitAABBNodes(allocator), mDynamicAABBTree(dynamicAABBTree), mConcaveMeshShape(concaveMeshShape), mProxyShape(proxyShape), mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false), mAllocator(allocator) { } diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp index 5b6c70d6..d4037a8c 100644 --- a/src/collision/shapes/ConvexMeshShape.cpp +++ b/src/collision/shapes/ConvexMeshShape.cpp @@ -30,9 +30,6 @@ using namespace reactphysics3d; -// TODO : Check in every collision shape that localScalling is used correctly and even with SAT -// algorithm (not only in getLocalSupportPoint***() methods) - // Constructor to initialize with an array of 3D vertices. /// This method creates an internal copy of the input vertices. /** @@ -58,14 +55,14 @@ ConvexMeshShape::ConvexMeshShape(PolyhedronMesh* polyhedronMesh) /// runs in almost constant time. Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { - double maxDotProduct = DECIMAL_SMALLEST; + decimal maxDotProduct = DECIMAL_SMALLEST; uint indexMaxDotProduct = 0; // For each vertex of the mesh for (uint i=0; igetNbVertices(); i++) { // Compute the dot product of the current vertex - double dotProduct = direction.dot(mPolyhedronMesh->getVertex(i)); + decimal dotProduct = direction.dot(mPolyhedronMesh->getVertex(i)); // If the current dot product is larger than the maximum one if (dotProduct > maxDotProduct) { @@ -83,14 +80,11 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct // Recompute the bounds of the mesh void ConvexMeshShape::recalculateBounds() { - // TODO : Only works if the local origin is inside the mesh - // => Make it more robust (init with first vertex of mesh instead) - - mMinBounds.setToZero(); - mMaxBounds.setToZero(); + mMinBounds = mPolyhedronMesh->getVertex(0); + mMaxBounds = mPolyhedronMesh->getVertex(0); // For each vertex of the mesh - for (uint i=0; igetNbVertices(); i++) { + for (uint i=1; igetNbVertices(); i++) { if (mPolyhedronMesh->getVertex(i).x > mMaxBounds.x) mMaxBounds.x = mPolyhedronMesh->getVertex(i).x; if (mPolyhedronMesh->getVertex(i).x < mMinBounds.x) mMinBounds.x = mPolyhedronMesh->getVertex(i).x; @@ -105,14 +99,105 @@ void ConvexMeshShape::recalculateBounds() { // Apply the local scaling factor mMaxBounds = mMaxBounds * mScaling; mMinBounds = mMinBounds * mScaling; - - // Add the object margin to the bounds - mMaxBounds += Vector3(mMargin, mMargin, mMargin); - mMinBounds -= Vector3(mMargin, mMargin, mMargin); } // Raycast method with feedback information +/// This method implements the technique in the book "Real-time Collision Detection" by +/// Christer Ericson. bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const { - return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast( - ray, proxyShape, raycastInfo); + + // Ray direction + Vector3 direction = ray.point2 - ray.point1; + + decimal tMin = decimal(0.0); + decimal tMax = ray.maxFraction; + Vector3 currentFaceNormal; + bool isIntersectionFound = false; + + const HalfEdgeStructure& halfEdgeStructure = mPolyhedronMesh->getHalfEdgeStructure(); + + // For each face of the convex mesh + for (uint f=0; f < mPolyhedronMesh->getNbFaces(); f++) { + + const HalfEdgeStructure::Face& face = halfEdgeStructure.getFace(f); + const Vector3 faceNormal = mPolyhedronMesh->getFaceNormal(f); + const HalfEdgeStructure::Vertex& faceVertex = halfEdgeStructure.getVertex(face.faceVertices[0]); + const Vector3 facePoint = mPolyhedronMesh->getVertex(faceVertex.vertexPointIndex); + decimal denom = faceNormal.dot(direction); + decimal planeD = faceNormal.dot(facePoint); + decimal dist = planeD - faceNormal.dot(ray.point1); + + // If ray is parallel to the face + if (denom == decimal(0.0)) { + + // If ray is outside the clipping face, we return no intersection + if (dist < decimal(0.0)) return false; + } + else { + + // Compute the intersection between the ray and the current face plane + decimal t = dist / denom; + + // Update the current ray intersection by clipping it with the current face plane + // If the place faces the ray + if (denom < decimal(0.0)) { + // Clip the current ray intersection as it enters the convex mesh + if (t > tMin) { + tMin = t; + currentFaceNormal = faceNormal; + isIntersectionFound = true; + } + } + else { + // Clip the current ray intersection as it exits the convex mesh + if (t < tMax) tMax = t; + } + + // If the ray intersection with the convex mesh becomes empty, report no intersection + if (tMin > tMax) return false; + } + } + + if (isIntersectionFound) { + + // The ray intersects with the convex mesh + assert(tMin >= decimal(0.0)); + assert(tMax <= ray.maxFraction); + assert(tMin <= tMax); + assert(currentFaceNormal.lengthSquare() > decimal(0.0)); + + // The ray intersects the three slabs, we compute the hit point + Vector3 localHitPoint = ray.point1 + tMin * direction; + + raycastInfo.hitFraction = tMin; + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.worldPoint = localHitPoint; + raycastInfo.worldNormal = currentFaceNormal; + + return true; + } + + return false; } + +// Return true if a point is inside the collision shape +bool ConvexMeshShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { + + const HalfEdgeStructure& halfEdgeStructure = mPolyhedronMesh->getHalfEdgeStructure(); + + // For each face plane of the convex mesh + for (uint f=0; f < mPolyhedronMesh->getNbFaces(); f++) { + + const HalfEdgeStructure::Face& face = halfEdgeStructure.getFace(f); + const Vector3 faceNormal = mPolyhedronMesh->getFaceNormal(f); + const HalfEdgeStructure::Vertex& faceVertex = halfEdgeStructure.getVertex(face.faceVertices[0]); + const Vector3 facePoint = mPolyhedronMesh->getVertex(faceVertex.vertexPointIndex); + + // If the point is out of the face plane, it is outside of the convex mesh + if (computePointToPlaneDistance(localPoint, faceNormal, facePoint) > decimal(0.0)) return false; + } + + return true; +} + diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index 44d01df4..49d3f773 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -33,9 +33,6 @@ #include "collision/TriangleMesh.h" #include "collision/PolyhedronMesh.h" #include "collision/narrowphase/GJK/GJKAlgorithm.h" -#include -#include -#include /// ReactPhysics3D namespace namespace reactphysics3d { @@ -176,15 +173,6 @@ inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decima 0.0, 0.0, factor * (xSquare + ySquare)); } -// Return true if a point is inside the collision shape -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); -} - // Return the number of faces of the polyhedron inline uint ConvexMeshShape::getNbFaces() const { return mPolyhedronMesh->getHalfEdgeStructure().getNbFaces(); diff --git a/src/configuration.h b/src/configuration.h index 96391e76..3cde138c 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -32,6 +32,7 @@ #include #include #include "decimal.h" +#include "containers/Pair.h" // Windows platform #if defined(WIN32) ||defined(_WIN32) || defined(_WIN64) ||defined(__WIN32__) || defined(__WINDOWS__) @@ -52,7 +53,7 @@ using uchar = unsigned char; using ushort = unsigned short; using luint = long unsigned int; using bodyindex = luint; -using bodyindexpair = std::pair; +using bodyindexpair = Pair; using int8 = std::int8_t; using uint8 = std::uint8_t; diff --git a/src/containers/List.h b/src/containers/List.h index 6391d52b..49bd2d21 100644 --- a/src/containers/List.h +++ b/src/containers/List.h @@ -57,9 +57,6 @@ class List { /// Memory allocator MemoryAllocator& mAllocator; - // -------------------- Methods -------------------- // - - public: /// Class Iterator @@ -87,14 +84,14 @@ class List { Iterator() = default; /// Constructor - Iterator(T* buffer, size_t index, size_t size) - :mCurrentIndex(index), mBuffer(buffer), mSize(size) { + Iterator(void* buffer, size_t index, size_t size) + :mCurrentIndex(index), mBuffer(static_cast(buffer)), mSize(size) { } /// Copy constructor Iterator(const Iterator& it) - :mCurrentIndex(it.mCurrentIndex), mBuffer(it.mBuffer), mSize(it.size) { + :mCurrentIndex(it.mCurrentIndex), mBuffer(it.mBuffer), mSize(it.mSize) { } @@ -112,14 +109,14 @@ class List { /// Post increment (it++) Iterator& operator++() { - assert(mCurrentIndex < mSize - 1); + assert(mCurrentIndex < mSize); mCurrentIndex++; return *this; } /// Pre increment (++it) Iterator operator++(int number) { - assert(mCurrentIndex < mSize - 1); + assert(mCurrentIndex < mSize); Iterator tmp = *this; mCurrentIndex++; return tmp; @@ -149,13 +146,17 @@ class List { return true; } - return &(mBuffer[mCurrentIndex]) == &(iterator.mBuffer[mCurrentIndex]); + return &(mBuffer[mCurrentIndex]) == &(iterator.mBuffer[iterator.mCurrentIndex]); } /// Inequality operator (it != end()) bool operator!=(const Iterator& iterator) const { return !(*this == iterator); } + + /// Frienship + friend class List; + }; // -------------------- Methods -------------------- // @@ -229,8 +230,35 @@ class List { mSize++; } - /// Remove an element from the list at a given index - void remove(uint index) { + /// Try to find a given item of the list and return an iterator + /// pointing to that element if it exists in the list. Otherwise, + /// this method returns the end() iterator + Iterator find(const T& element) { + + for (uint i=0; i(mBuffer)[i]) { + return Iterator(mBuffer, i, mSize); + } + } + + return end(); + } + + /// Look for an element in the list and remove it + Iterator remove(const T& element) { + return remove(find(element)); + } + + /// Remove an element from the list and return a iterator + /// pointing to the element after the removed one (or end() if none) + Iterator remove(const Iterator& it) { + assert(it.mBuffer == mBuffer); + return removeAt(it.mCurrentIndex); + } + + /// Remove an element from the list at a given index and return an + /// iterator pointing to the element after the removed one (or end() if none) + Iterator removeAt(uint index) { assert(index >= 0 && index < mSize); @@ -246,6 +274,9 @@ class List { char* src = dest + sizeof(T); std::memcpy(static_cast(dest), static_cast(src), (mSize - index) * sizeof(T)); } + + // Return an iterator pointing to the element after the removed one + return Iterator(mBuffer, index, mSize); } /// Append another list at the end of the current one @@ -334,6 +365,16 @@ class List { return *this; } + + /// Return a begin iterator + Iterator begin() const { + return Iterator(mBuffer, 0, mSize); + } + + /// Return a end iterator + Iterator end() const { + return Iterator(mBuffer, mSize, mSize); + } }; } diff --git a/src/containers/Map.h b/src/containers/Map.h index 5510b98d..cea9342c 100644 --- a/src/containers/Map.h +++ b/src/containers/Map.h @@ -29,14 +29,19 @@ // Libraries #include "memory/MemoryAllocator.h" #include "mathematics/mathematics_functions.h" +#include "containers/Pair.h" #include +#include +#include #include + namespace reactphysics3d { // Class Map /** - * This class represents a simple generic associative map + * This class represents a simple generic associative map. This map is + * implemented with a hash table. */ template class Map { @@ -48,7 +53,7 @@ class Map { size_t hashCode; // Hash code of the entry int next; // Index of the next entry - std::pair* keyValue; // Pointer to the pair with key and value + Pair* keyValue; // Pointer to the pair with key and value /// Constructor Entry() { @@ -56,6 +61,13 @@ class Map { keyValue = nullptr; } + /// Constructor + Entry(size_t hashcode, int nextEntry) { + hashCode = hashcode; + next = nextEntry; + keyValue = nullptr; + } + /// Destructor ~Entry() { @@ -220,7 +232,7 @@ class Map { // If elements have been allocated if (mCapacity > 0) { - // Clear the list + // Clear the map clear(); // Destroy the entries @@ -242,6 +254,109 @@ class Map { public: + /// Class Iterator + /** + * This class represents an iterator for the Map + */ + class Iterator { + + private: + + /// Array of entries + const Entry* mEntries; + + /// Capacity of the map + int mCapacity; + + /// Number of used entries in the map + int mNbUsedEntries; + + /// Index of the current entry + int mCurrentEntry; + + /// Advance the iterator + void advance() { + + // If we are trying to move past the end + assert(mCurrentEntry < mNbUsedEntries); + + for (mCurrentEntry += 1; mCurrentEntry < mNbUsedEntries; mCurrentEntry++) { + + // If the entry is not empty + if (mEntries[mCurrentEntry].keyValue != nullptr) { + + // We have found the next non empty entry + return; + } + } + + // We have not find a non empty entry, we return an iterator to the end + mCurrentEntry = mCapacity; + } + + public: + + // Iterator traits + using value_type = Pair; + using difference_type = std::ptrdiff_t; + using pointer = Pair*; + using reference = Pair&; + using iterator_category = std::forward_iterator_tag; + + /// Constructor + Iterator() = default; + + /// Constructor + Iterator(const Entry* entries, int capacity, int nbUsedEntries, int currentEntry) + :mEntries(entries), mCapacity(capacity), mNbUsedEntries(nbUsedEntries), mCurrentEntry(currentEntry) { + + } + + /// Copy constructor + Iterator(const Iterator& it) + :mEntries(it.mEntries), mCapacity(it.mCapacity), mNbUsedEntries(it.mNbUsedEntries), mCurrentEntry(it.mCurrentEntry) { + + } + + /// Deferencable + reference operator*() const { + assert(mCurrentEntry >= 0 && mCurrentEntry < mNbUsedEntries); + assert(mEntries[mCurrentEntry].keyValue != nullptr); + return *(mEntries[mCurrentEntry].keyValue); + } + + /// Deferencable + pointer operator->() const { + assert(mCurrentEntry >= 0 && mCurrentEntry < mNbUsedEntries); + assert(mEntries[mCurrentEntry].keyValue != nullptr); + return mEntries[mCurrentEntry].keyValue; + } + + /// Post increment (it++) + Iterator& operator++() { + advance(); + return *this; + } + + /// Pre increment (++it) + Iterator operator++(int number) { + Iterator tmp = *this; + advance(); + return tmp; + } + + /// Equality operator (it == end()) + bool operator==(const Iterator& iterator) const { + return mCurrentEntry == iterator.mCurrentEntry && mEntries == iterator.mEntries; + } + + /// Inequality operator (it != end()) + bool operator!=(const Iterator& iterator) const { + return !(*this == iterator); + } + }; + + // -------------------- Methods -------------------- // /// Constructor @@ -277,7 +392,15 @@ class Map { std::memcpy(mBuckets, map.mBuckets, mCapacity * sizeof(int)); // Copy the entries - std::memcpy(mEntries, map.mEntries, mCapacity * sizeof(Entry)); + for (int i=0; i < mCapacity; i++) { + + new (&mEntries[i]) Entry(map.mEntries[i].hashCode, map.mEntries[i].next); + + if (map.mEntries[i].keyValue != nullptr) { + mEntries[i].keyValue = static_cast*>(mAllocator.allocate(sizeof(Pair))); + new (mEntries[i].keyValue) Pair(*(map.mEntries[i].keyValue)); + } + } } /// Destructor @@ -307,7 +430,7 @@ class Map { } /// Add an element into the map - void add(const std::pair& keyValue) { + void add(const Pair& keyValue, bool insertIfAlreadyPresent = false) { if (mCapacity == 0) { initialize(0); @@ -325,7 +448,19 @@ class Map { // If there is already an item with the same key in the map if (mEntries[i].hashCode == hashCode && mEntries[i].keyValue->first == keyValue.first) { - throw std::runtime_error("The key and value pair already exists in the map"); + if (insertIfAlreadyPresent) { + + // Destruct the previous key/value + mEntries[i].keyValue->~Pair(); + + // Copy construct the new key/value + new (mEntries[i].keyValue) Pair(keyValue); + + return; + } + else { + throw std::runtime_error("The key and value pair already exists in the map"); + } } } @@ -357,14 +492,25 @@ class Map { assert(mEntries[entryIndex].keyValue == nullptr); mEntries[entryIndex].hashCode = hashCode; mEntries[entryIndex].next = mBuckets[bucket]; - mEntries[entryIndex].keyValue = static_cast*>(mAllocator.allocate(sizeof(std::pair))); + mEntries[entryIndex].keyValue = static_cast*>(mAllocator.allocate(sizeof(Pair))); assert(mEntries[entryIndex].keyValue != nullptr); - new (mEntries[entryIndex].keyValue) std::pair(keyValue); + new (mEntries[entryIndex].keyValue) Pair(keyValue); mBuckets[bucket] = entryIndex; } + /// Remove the element pointed by some iterator + /// This method returns an iterator pointing to the element after + /// the one that has been removed + Iterator remove(const Iterator& it) { + + const K& key = it->first; + return remove(key); + } + /// Remove the element from the map with a given key - bool remove(const K& key) { + /// This method returns an iterator pointing to the element after + /// the one that has been removed + Iterator remove(const K& key) { if (mCapacity > 0) { @@ -384,8 +530,8 @@ class Map { // Release memory for the key/value pair if any if (mEntries[i].keyValue != nullptr) { - mEntries[i].keyValue->~pair(); - mAllocator.release(mEntries[i].keyValue, sizeof(std::pair)); + mEntries[i].keyValue->~Pair(); + mAllocator.release(mEntries[i].keyValue, sizeof(Pair)); mEntries[i].keyValue = nullptr; } mEntries[i].hashCode = -1; @@ -393,15 +539,27 @@ class Map { mFreeIndex = i; mNbFreeEntries++; - return true; + // Find the next entry to return the iterator + for (i += 1; i < mNbUsedEntries; i++) { + + // If the entry is not empty + if (mEntries[i].keyValue != nullptr) { + + // We have found the next non empty entry + return Iterator(mEntries, mCapacity, mNbUsedEntries, i); + } + } + + return end(); } } } - return false; + // Return the end iterator + return end(); } - /// Clear the list + /// Clear the map void clear() { if (mNbUsedEntries > 0) { @@ -410,8 +568,8 @@ class Map { mBuckets[i] = -1; mEntries[i].next = -1; if (mEntries[i].keyValue != nullptr) { - mEntries[i].keyValue->~pair(); - mAllocator.release(mEntries[i].keyValue, sizeof(std::pair)); + mEntries[i].keyValue->~Pair(); + mAllocator.release(mEntries[i].keyValue, sizeof(Pair)); mEntries[i].keyValue = nullptr; } } @@ -434,6 +592,36 @@ class Map { return mCapacity; } + /// Try to find an item of the map given a key. + /// The method returns an iterator to the found item or + /// an iterator pointing to the end if not found + Iterator find(const K& key) const { + + int bucket; + int entry = -1; + + if (mCapacity > 0) { + + size_t hashCode = std::hash()(key); + bucket = hashCode % mCapacity; + + for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) { + if (mEntries[i].hashCode == hashCode && mEntries[i].keyValue->first == key) { + entry = i; + break; + } + } + } + + if (entry == -1) { + return end(); + } + + assert(mEntries[entry].keyValue != nullptr); + + return Iterator(mEntries, mCapacity, mNbUsedEntries, entry); + } + /// Overloaded index operator V& operator[](const K& key) { @@ -465,14 +653,31 @@ class Map { throw std::runtime_error("No item with given key has been found in the map"); } - return mEntries[entry]; + assert(mEntries[entry].keyValue != nullptr); + + return mEntries[entry].keyValue->second; } /// Overloaded equality operator bool operator==(const Map& map) const { - // TODO : Implement this - return false; + if (size() != map.size()) return false; + + for (auto it = begin(); it != end(); ++it) { + auto it2 = map.find(it->first); + if (it2 == map.end() || it2->second != it->second) { + return false; + } + } + + for (auto it = map.begin(); it != map.end(); ++it) { + auto it2 = find(it->first); + if (it2 == end() || it2->second != it->second) { + return false; + } + } + + return true; } /// Overloaded not equal operator @@ -505,7 +710,15 @@ class Map { std::memcpy(mBuckets, map.mBuckets, mCapacity * sizeof(int)); // Copy the entries - std::memcpy(mEntries, map.mEntries, mCapacity * sizeof(Entry)); + for (int i=0; i < mCapacity; i++) { + + new (&mEntries[i]) Entry(map.mEntries[i].hashCode, map.mEntries[i].next); + + if (map.mEntries[i].keyValue != nullptr) { + mEntries[i].keyValue = static_cast*>(mAllocator.allocate(sizeof(Pair))); + new (mEntries[i].keyValue) Pair(*(map.mEntries[i].keyValue)); + } + } mNbUsedEntries = map.mNbUsedEntries; mNbFreeEntries = map.mNbFreeEntries; @@ -515,6 +728,32 @@ class Map { return *this; } + + /// Return a begin iterator + Iterator begin() const { + + // If the map is empty + if (size() == 0) { + + // Return an iterator to the end + return end(); + } + + // Find the first used entry + int entry; + for (entry=0; entry < mNbUsedEntries; entry++) { + if (mEntries[entry].keyValue != nullptr) { + return Iterator(mEntries, mCapacity, mNbUsedEntries, entry); + } + } + + assert(false); + } + + /// Return a end iterator + Iterator end() const { + return Iterator(mEntries, mCapacity, mNbUsedEntries, mCapacity); + } }; template diff --git a/src/containers/Pair.h b/src/containers/Pair.h new file mode 100644 index 00000000..8c34b4ac --- /dev/null +++ b/src/containers/Pair.h @@ -0,0 +1,106 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_PAIR_H +#define REACTPHYSICS3D_PAIR_H + +// Libraries +#include "configuration.h" +#include "memory/MemoryAllocator.h" +#include "containers/containers_common.h" +#include +#include + +namespace reactphysics3d { + +// Class Pair +/** + * This class represents a simple generic pair + */ +template +class Pair { + + public: + + // -------------------- Attributes -------------------- // + + /// First element of the pair + T1 first; + + /// Second element of the pair + T2 second; + + // -------------------- Methods -------------------- // + + /// Constructor + Pair(const T1& item1, const T2& item2) : first(item1), second(item2) { + + } + + /// Copy constructor + Pair(const Pair& pair) : first(pair.first), second(pair.second) { + + } + + /// Destructor + ~Pair() = default; + + /// Overloaded equality operator + bool operator==(const Pair& pair) const { + return first == pair.first && second == pair.second; + } + + /// Overloaded not equal operator + bool operator!=(const Pair& pair) const { + return !((*this) == pair); + } + + /// Overloaded assignment operator + Pair& operator=(const Pair& pair) { + first = pair.first; + second = pair.second; + } +}; + +} + +// Hash function for struct VerticesPair +namespace std { + + template struct hash> { + + size_t operator()(const reactphysics3d::Pair& pair) const { + + std::size_t seed = 0; + reactphysics3d::hash_combine(seed, pair.first); + reactphysics3d::hash_combine(seed, pair.second); + + return seed; + } + }; +} + + +#endif diff --git a/src/containers/Set.h b/src/containers/Set.h new file mode 100644 index 00000000..646507a1 --- /dev/null +++ b/src/containers/Set.h @@ -0,0 +1,712 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_SET_H +#define REACTPHYSICS3D_SET_H + +// Libraries +#include "memory/MemoryAllocator.h" +#include "mathematics/mathematics_functions.h" +#include +#include +#include +#include + + +namespace reactphysics3d { + +// Class Set +/** + * This class represents a simple generic set. This set is implemented + * with a hash table. + */ +template +class Set { + + private: + + /// An entry of the set + struct Entry { + + size_t hashCode; // Hash code of the entry + int next; // Index of the next entry + V* value; // Pointer to the value stored in the entry + + /// Constructor + Entry() { + next = -1; + value = nullptr; + } + + /// Constructor + Entry(size_t hashcode, int nextEntry) { + hashCode = hashcode; + next = nextEntry; + value = nullptr; + } + + /// Destructor + ~Entry() { + + assert(value == nullptr); + } + + }; + + // -------------------- Constants -------------------- // + + /// Number of prime numbers in array + static constexpr int NB_PRIMES = 70; + + /// Array of prime numbers for the size of the set + static const int PRIMES[NB_PRIMES]; + + /// Largest prime number + static int LARGEST_PRIME; + + // -------------------- Attributes -------------------- // + + /// Current number of used entries in the set + int mNbUsedEntries; + + /// Number of free entries among the used ones + int mNbFreeEntries; + + /// Current capacity of the set + int mCapacity; + + /// Array with all the buckets + int* mBuckets; + + /// Array with all the entries + Entry* mEntries; + + /// Memory allocator + MemoryAllocator& mAllocator; + + /// Index to the fist free entry + int mFreeIndex; + + // -------------------- Methods -------------------- // + + /// Initialize the set + void initialize(int capacity) { + + // Compute the next larger prime size + mCapacity = getPrimeSize(capacity); + + // Allocate memory for the buckets + mBuckets = static_cast(mAllocator.allocate(mCapacity * sizeof(int))); + + // Allocate memory for the entries + mEntries = static_cast(mAllocator.allocate(mCapacity * sizeof(Entry))); + + // Initialize the buckets and entries + for (int i=0; i mCapacity); + assert(isPrimeNumber(newCapacity)); + + // Allocate memory for the buckets + int* newBuckets = static_cast(mAllocator.allocate(newCapacity * sizeof(int))); + + // Allocate memory for the entries + Entry* newEntries = static_cast(mAllocator.allocate(newCapacity * sizeof(Entry))); + + // Initialize the new buckets + for (int i=0; i(&newEntries[i])) Entry(); + } + + // For each used entry + for (int i=0; i 0) { + + size_t hashCode = std::hash()(value); + int bucket = hashCode % mCapacity; + + for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) { + if (mEntries[i].hashCode == hashCode && (*mEntries[i].value) == value) { + return i; + } + } + } + + return -1; + } + + /// Return the prime number that is larger or equal to the number in parameter + /// for the size of the set + static int getPrimeSize(int number) { + + // Check if the next larger prime number is in the precomputed array of primes + for (int i = 0; i < NB_PRIMES; i++) { + if (PRIMES[i] >= number) return PRIMES[i]; + } + + // Manually compute the next larger prime number + for (int i = (number | 1); i < std::numeric_limits::max(); i+=2) { + + if (isPrimeNumber(i)) { + return i; + } + } + + return number; + } + + /// Clear and reset the set + void reset() { + + // If elements have been allocated + if (mCapacity > 0) { + + // Clear the list + clear(); + + // Destroy the entries + for (int i=0; i < mCapacity; i++) { + mEntries[i].~Entry(); + } + + mAllocator.release(mBuckets, mCapacity * sizeof(int)); + mAllocator.release(mEntries, mCapacity * sizeof(Entry)); + + mNbUsedEntries = 0; + mNbFreeEntries = 0; + mCapacity = 0; + mBuckets = nullptr; + mEntries = nullptr; + mFreeIndex = -1; + } + } + + public: + + /// Class Iterator + /** + * This class represents an iterator for the Set + */ + class Iterator { + + private: + + /// Array of entries + const Entry* mEntries; + + /// Capacity of the map + int mCapacity; + + /// Number of used entries in the map + int mNbUsedEntries; + + /// Index of the current entry + int mCurrentEntry; + + /// Advance the iterator + void advance() { + + // If we are trying to move past the end + assert(mCurrentEntry < mNbUsedEntries); + + for (mCurrentEntry += 1; mCurrentEntry < mNbUsedEntries; mCurrentEntry++) { + + // If the entry is not empty + if (mEntries[mCurrentEntry].value != nullptr) { + + // We have found the next non empty entry + return; + } + } + + // We have not find a non empty entry, we return an iterator to the end + mCurrentEntry = mCapacity; + } + + public: + + // Iterator traits + using value_type = V; + using difference_type = std::ptrdiff_t; + using pointer = V*; + using reference = V&; + using iterator_category = std::forward_iterator_tag; + + /// Constructor + Iterator() = default; + + /// Constructor + Iterator(const Entry* entries, int capacity, int nbUsedEntries, int currentEntry) + :mEntries(entries), mCapacity(capacity), mNbUsedEntries(nbUsedEntries), mCurrentEntry(currentEntry) { + + } + + /// Copy constructor + Iterator(const Iterator& it) + :mEntries(it.mEntries), mCapacity(it.mCapacity), mNbUsedEntries(it.mNbUsedEntries), mCurrentEntry(it.mCurrentEntry) { + + } + + /// Deferencable + reference operator*() const { + assert(mCurrentEntry >= 0 && mCurrentEntry < mNbUsedEntries); + assert(mEntries[mCurrentEntry].value != nullptr); + return *(mEntries[mCurrentEntry].value); + } + + /// Deferencable + pointer operator->() const { + assert(mCurrentEntry >= 0 && mCurrentEntry < mNbUsedEntries); + assert(mEntries[mCurrentEntry].value != nullptr); + return mEntries[mCurrentEntry].value; + } + + /// Post increment (it++) + Iterator& operator++() { + advance(); + return *this; + } + + /// Pre increment (++it) + Iterator operator++(int number) { + Iterator tmp = *this; + advance(); + return tmp; + } + + /// Equality operator (it == end()) + bool operator==(const Iterator& iterator) const { + return mCurrentEntry == iterator.mCurrentEntry && mEntries == iterator.mEntries; + } + + /// Inequality operator (it != end()) + bool operator!=(const Iterator& iterator) const { + return !(*this == iterator); + } + }; + + + // -------------------- Methods -------------------- // + + /// Constructor + Set(MemoryAllocator& allocator, size_t capacity = 0) + : mNbUsedEntries(0), mNbFreeEntries(0), mCapacity(0), mBuckets(nullptr), + mEntries(nullptr), mAllocator(allocator), mFreeIndex(-1) { + + // If the largest prime has not been computed yet + if (LARGEST_PRIME == -1) { + + // Compute the largest prime number (largest map capacity) + LARGEST_PRIME = getPrimeSize(PRIMES[NB_PRIMES - 1] + 2); + } + + if (capacity > 0) { + + initialize(capacity); + } + } + + /// Copy constructor + Set(const Set& set) + :mNbUsedEntries(set.mNbUsedEntries), mNbFreeEntries(set.mNbFreeEntries), mCapacity(set.mCapacity), + mAllocator(set.mAllocator), mFreeIndex(set.mFreeIndex) { + + // Allocate memory for the buckets + mBuckets = static_cast(mAllocator.allocate(mCapacity * sizeof(int))); + + // Allocate memory for the entries + mEntries = static_cast(mAllocator.allocate(mCapacity * sizeof(Entry))); + + // Copy the buckets + std::memcpy(mBuckets, set.mBuckets, mCapacity * sizeof(int)); + + // Copy the entries + for (int i=0; i < mCapacity; i++) { + + new (&mEntries[i]) Entry(set.mEntries[i].hashCode, set.mEntries[i].next); + + if (set.mEntries[i].value != nullptr) { + mEntries[i].value = static_cast(mAllocator.allocate(sizeof(V))); + new (mEntries[i].value) V(*(set.mEntries[i].value)); + } + } + } + + /// Destructor + ~Set() { + + reset(); + } + + /// Allocate memory for a given number of elements + void reserve(size_t capacity) { + + if (capacity <= mCapacity) return; + + if (capacity > LARGEST_PRIME && LARGEST_PRIME > mCapacity) { + capacity = LARGEST_PRIME; + } + else { + capacity = getPrimeSize(capacity); + } + + expand(capacity); + } + + /// Return true if the set contains a given value + bool contains(const V& value) const { + return findEntry(value) != -1; + } + + /// Add a value into the set + void add(const V& value) { + + if (mCapacity == 0) { + initialize(0); + } + + // Compute the hash code of the value + size_t hashCode = std::hash()(value); + + // Compute the corresponding bucket index + int bucket = hashCode % mCapacity; + + // Check if the item is already in the set + for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) { + + // If there is already an item with the same value in the set + if (mEntries[i].hashCode == hashCode && (*mEntries[i].value) == value) { + + return; + } + } + + size_t entryIndex; + + // If there are free entries to use + if (mNbFreeEntries > 0) { + assert(mFreeIndex >= 0); + entryIndex = mFreeIndex; + mFreeIndex = mEntries[entryIndex].next; + mNbFreeEntries--; + } + else { + + // If we need to allocator more entries + if (mNbUsedEntries == mCapacity) { + + // Allocate more memory + reserve(mCapacity * 2); + + // Recompute the bucket index + bucket = hashCode % mCapacity; + } + + entryIndex = mNbUsedEntries; + mNbUsedEntries++; + } + + assert(mEntries[entryIndex].value == nullptr); + mEntries[entryIndex].hashCode = hashCode; + mEntries[entryIndex].next = mBuckets[bucket]; + mEntries[entryIndex].value = static_cast(mAllocator.allocate(sizeof(V))); + assert(mEntries[entryIndex].value != nullptr); + new (mEntries[entryIndex].value) V(value); + mBuckets[bucket] = entryIndex; + } + + /// Remove the element pointed by some iterator + /// This method returns an iterator pointing to the + /// element after the one that has been removed + Iterator remove(const Iterator& it) { + + return remove(*it); + } + + /// Remove the element from the set with a given value + /// This method returns an iterator pointing to the + /// element after the one that has been removed + Iterator remove(const V& value) { + + if (mCapacity > 0) { + + size_t hashcode = std::hash()(value); + int bucket = hashcode % mCapacity; + int last = -1; + for (int i = mBuckets[bucket]; i >= 0; last = i, i = mEntries[i].next) { + + if (mEntries[i].hashCode == hashcode && (*mEntries[i].value) == value) { + + if (last < 0 ) { + mBuckets[bucket] = mEntries[i].next; + } + else { + mEntries[last].next = mEntries[i].next; + } + + // Release memory for the value if any + if (mEntries[i].value != nullptr) { + mEntries[i].value->~V(); + mAllocator.release(mEntries[i].value, sizeof(V)); + mEntries[i].value = nullptr; + } + mEntries[i].hashCode = -1; + mEntries[i].next = mFreeIndex; + mFreeIndex = i; + mNbFreeEntries++; + + // Find the next valid entry to return an iterator + for (i += 1; i < mNbUsedEntries; i++) { + + // If the entry is not empty + if (mEntries[i].value != nullptr) { + + // We have found the next non empty entry + return Iterator(mEntries, mCapacity, mNbUsedEntries, i); + } + } + + return end(); + } + } + } + + return end(); + } + + /// Clear the set + void clear() { + + if (mNbUsedEntries > 0) { + + for (int i=0; i < mCapacity; i++) { + mBuckets[i] = -1; + mEntries[i].next = -1; + if (mEntries[i].value != nullptr) { + mEntries[i].value->~V(); + mAllocator.release(mEntries[i].value, sizeof(V)); + mEntries[i].value = nullptr; + } + } + + mFreeIndex = -1; + mNbUsedEntries = 0; + mNbFreeEntries = 0; + } + + assert(size() == 0); + } + + /// Return the number of elements in the set + int size() const { + return mNbUsedEntries - mNbFreeEntries; + } + + /// Return the capacity of the set + int capacity() const { + return mCapacity; + } + + /// Try to find an item of the set given a key. + /// The method returns an iterator to the found item or + /// an iterator pointing to the end if not found + Iterator find(const V& value) const { + + int bucket; + int entry = -1; + + if (mCapacity > 0) { + + size_t hashCode = std::hash()(value); + bucket = hashCode % mCapacity; + + for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) { + if (mEntries[i].hashCode == hashCode && *(mEntries[i].value) == value) { + entry = i; + break; + } + } + } + + if (entry == -1) { + return end(); + } + + assert(mEntries[entry].value != nullptr); + + return Iterator(mEntries, mCapacity, mNbUsedEntries, entry); + } + + /// Overloaded equality operator + bool operator==(const Set& set) const { + + if (size() != set.size()) return false; + + for (auto it = begin(); it != end(); ++it) { + if(!set.contains(*it)) { + return false; + } + } + + return true; + } + + /// Overloaded not equal operator + bool operator!=(const Set& set) const { + + return !((*this) == set); + } + + /// Overloaded assignment operator + Set& operator=(const Set& set) { + + // Check for self assignment + if (this != &set) { + + // Reset the set + reset(); + + if (set.mCapacity > 0) { + + // Compute the next larger prime size + mCapacity = getPrimeSize(set.mCapacity); + + // Allocate memory for the buckets + mBuckets = static_cast(mAllocator.allocate(mCapacity * sizeof(int))); + + // Allocate memory for the entries + mEntries = static_cast(mAllocator.allocate(mCapacity * sizeof(Entry))); + + // Copy the buckets + std::memcpy(mBuckets, set.mBuckets, mCapacity * sizeof(int)); + + // Copy the entries + for (int i=0; i < mCapacity; i++) { + + new (&mEntries[i]) Entry(set.mEntries[i].hashCode, set.mEntries[i].next); + + if (set.mEntries[i].value != nullptr) { + mEntries[i].value = static_cast(mAllocator.allocate(sizeof(V))); + new (mEntries[i].value) V(*(set.mEntries[i].value)); + } + } + + mNbUsedEntries = set.mNbUsedEntries; + mNbFreeEntries = set.mNbFreeEntries; + mFreeIndex = set.mFreeIndex; + } + } + + return *this; + } + + /// Return a begin iterator + Iterator begin() const { + + // If the map is empty + if (size() == 0) { + + // Return an iterator to the end + return end(); + } + + // Find the first used entry + int entry; + for (entry=0; entry < mNbUsedEntries; entry++) { + if (mEntries[entry].value != nullptr) { + return Iterator(mEntries, mCapacity, mNbUsedEntries, entry); + } + } + + assert(false); + } + + /// Return a end iterator + Iterator end() const { + return Iterator(mEntries, mCapacity, mNbUsedEntries, mCapacity); + } +}; + +template +const int Set::PRIMES[NB_PRIMES] = {3, 7, 11, 17, 23, 29, 37, 47, 59, 71, 89, 107, 131, 163, 197, 239, 293, 353, 431, 521, 631, 761, 919, + 1103, 1327, 1597, 1931, 2333, 2801, 3371, 4049, 4861, 5839, 7013, 8419, 10103, 12143, 14591, + 17519, 21023, 25229, 30293, 36353, 43627, 52361, 62851, 75431, 90523, 108631, 130363, 156437, + 187751, 225307, 270371, 324449, 389357, 467237, 560689, 672827, 807403, 968897, 1162687, 1395263, + 1674319, 2009191, 2411033, 2893249, 3471899, 4166287, 4999559}; + +template +int Set::LARGEST_PRIME = -1; + +} + +#endif diff --git a/src/containers/Stack.h b/src/containers/Stack.h index 904486f1..84e470ba 100644 --- a/src/containers/Stack.h +++ b/src/containers/Stack.h @@ -28,6 +28,7 @@ // Libraries #include "configuration.h" +#include "memory/MemoryAllocator.h" namespace reactphysics3d { @@ -43,6 +44,9 @@ class Stack { // -------------------- Attributes -------------------- // + /// Reference to the memory allocator + MemoryAllocator& mAllocator; + /// Initial array that contains the elements of the stack T mInitArray[capacity]; @@ -60,7 +64,8 @@ class Stack { // -------------------- Methods -------------------- // /// Constructor - Stack() : mElements(mInitArray), mNbElements(0), mNbAllocatedElements(capacity) { + Stack(MemoryAllocator& allocator) + :mAllocator(allocator), mElements(mInitArray), mNbElements(0), mNbAllocatedElements(capacity) { } @@ -71,7 +76,7 @@ class Stack { if (mInitArray != mElements) { // Release the memory allocated on the heap - free(mElements); + mAllocator.release(mElements, mNbAllocatedElements * sizeof(T)); } } @@ -93,12 +98,13 @@ inline void Stack::push(const T& element) { // If we need to allocate more elements if (mNbElements == mNbAllocatedElements) { T* oldElements = mElements; + uint oldNbAllocatedElements = mNbAllocatedElements; mNbAllocatedElements *= 2; - mElements = (T*) malloc(mNbAllocatedElements * sizeof(T)); + mElements = static_cast(mAllocator.allocate(mNbAllocatedElements * sizeof(T))); assert(mElements); memcpy(mElements, oldElements, mNbElements * sizeof(T)); if (oldElements != mInitArray) { - free(oldElements); + mAllocator.release(oldElements, oldNbAllocatedElements * sizeof(T)); } } diff --git a/src/containers/containers_common.h b/src/containers/containers_common.h new file mode 100644 index 00000000..bdaa07f8 --- /dev/null +++ b/src/containers/containers_common.h @@ -0,0 +1,44 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_CONTAINERS_COMMON_H +#define REACTPHYSICS3D_CONTAINERS_COMMON_H + +// Libraries +#include +#include + +namespace reactphysics3d { + +/// This method is used to combine two hash values +template +inline void hash_combine(std::size_t& seed, const T& v) { + std::hash hasher; + seed ^= hasher(v) + 0x9e3779b9 + (seed<<6) + (seed>>2); +} + +} + +#endif diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index da5bf609..9388acda 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -33,8 +33,8 @@ using namespace std; // Constructor CollisionWorld::CollisionWorld() - : mCollisionDetection(this, mMemoryManager), mCurrentBodyID(0), - mEventListener(nullptr) { + : mCollisionDetection(this, mMemoryManager), mBodies(mMemoryManager.getPoolAllocator()), mCurrentBodyID(0), + mFreeBodiesIDs(mMemoryManager.getPoolAllocator()), mEventListener(nullptr) { #ifdef IS_PROFILING_ACTIVE @@ -49,14 +49,11 @@ CollisionWorld::CollisionWorld() CollisionWorld::~CollisionWorld() { // Destroy all the collision bodies that have not been removed - std::set::iterator itBodies; - for (itBodies = mBodies.begin(); itBodies != mBodies.end(); ) { - std::set::iterator itToRemove = itBodies; - ++itBodies; - destroyCollisionBody(*itToRemove); + for (int i=mBodies.size() - 1 ; i >= 0; i--) { + destroyCollisionBody(mBodies[i]); } - assert(mBodies.empty()); + assert(mBodies.size() == 0); } // Create a collision body and add it to the world @@ -80,7 +77,7 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) { assert(collisionBody != nullptr); // Add the collision body to the world - mBodies.insert(collisionBody); + mBodies.add(collisionBody); #ifdef IS_PROFILING_ACTIVE @@ -102,13 +99,13 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) { collisionBody->removeAllCollisionShapes(); // Add the body ID to the list of free IDs - mFreeBodiesIDs.push_back(collisionBody->getID()); + mFreeBodiesIDs.add(collisionBody->getID()); // Call the destructor of the collision body collisionBody->~CollisionBody(); // Remove the collision body from the list of bodies - mBodies.erase(collisionBody); + mBodies.remove(collisionBody); // Free the object from the memory allocator mMemoryManager.release(MemoryManager::AllocationType::Pool, collisionBody, sizeof(CollisionBody)); @@ -119,9 +116,9 @@ bodyindex CollisionWorld::computeNextAvailableBodyID() { // Compute the body ID bodyindex bodyID; - if (!mFreeBodiesIDs.empty()) { - bodyID = mFreeBodiesIDs.back(); - mFreeBodiesIDs.pop_back(); + if (mFreeBodiesIDs.size() != 0) { + bodyID = mFreeBodiesIDs[mFreeBodiesIDs.size() - 1]; + mFreeBodiesIDs.removeAt(mFreeBodiesIDs.size() - 1); } else { bodyID = mCurrentBodyID; @@ -135,7 +132,7 @@ bodyindex CollisionWorld::computeNextAvailableBodyID() { void CollisionWorld::resetContactManifoldListsOfBodies() { // For each rigid body of the world - for (std::set::iterator it = mBodies.begin(); it != mBodies.end(); ++it) { + for (List::Iterator it = mBodies.begin(); it != mBodies.end(); ++it) { // Reset the contact manifold list of the body (*it)->resetContactManifoldsList(); diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 9f1fea58..1cc501e2 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -27,11 +27,9 @@ #define REACTPHYSICS3D_COLLISION_WORLD_H // Libraries -#include -#include -#include #include #include "mathematics/mathematics.h" +#include "containers/List.h" #include "Profiler.h" #include "body/CollisionBody.h" #include "collision/RaycastInfo.h" @@ -68,13 +66,13 @@ class CollisionWorld { CollisionDetection mCollisionDetection; /// All the bodies (rigid and soft) of the world - std::set mBodies; + List mBodies; /// Current body ID bodyindex mCurrentBodyID; /// List of free ID for rigid bodies - std::vector mFreeBodiesIDs; + List mFreeBodiesIDs; /// Pointer to an event listener object EventListener* mEventListener; @@ -109,12 +107,6 @@ class CollisionWorld { /// Deleted assignment operator CollisionWorld& operator=(const CollisionWorld& world) = delete; - /// Return an iterator to the beginning of the bodies of the physics world - std::set::iterator getBodiesBeginIterator(); - - /// Return an iterator to the end of the bodies of the physics world - std::set::iterator getBodiesEndIterator(); - /// Create a collision body CollisionBody* createCollisionBody(const Transform& transform); @@ -167,22 +159,6 @@ class CollisionWorld { friend class ConvexMeshShape; }; -// Return an iterator to the beginning of the bodies of the physics world -/** - * @return An starting iterator to the set of bodies of the world - */ -inline std::set::iterator CollisionWorld::getBodiesBeginIterator() { - return mBodies.begin(); -} - -// Return an iterator to the end of the bodies of the physics world -/** - * @return An ending iterator to the set of bodies of the world - */ -inline std::set::iterator CollisionWorld::getBodiesEndIterator() { - return mBodies.end(); -} - // Set the collision dispatch configuration /// This can be used to replace default collision detection algorithms by your /// custom algorithm for instance. diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 105045e0..7c3994fc 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -31,8 +31,6 @@ #include "mathematics/mathematics.h" #include "constraint/Joint.h" #include "Island.h" -#include -#include namespace reactphysics3d { diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 8799a1ad..aecab4fe 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -32,8 +32,6 @@ #include "constraint/Joint.h" #include "collision/ContactManifold.h" #include "Island.h" -#include -#include /// ReactPhysics3D namespace namespace reactphysics3d { diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 35b1ba04..ee270519 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -44,7 +44,8 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity) mContactSolver(mMemoryManager), mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), - mIsSleepingEnabled(SLEEPING_ENABLED), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)), + mIsSleepingEnabled(SLEEPING_ENABLED), mRigidBodies(mMemoryManager.getPoolAllocator()), + mJoints(mMemoryManager.getPoolAllocator()), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)), mIsGravityEnabled(true), mConstrainedLinearVelocities(nullptr), mConstrainedAngularVelocities(nullptr), mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr), mConstrainedPositions(nullptr), @@ -67,19 +68,13 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity) DynamicsWorld::~DynamicsWorld() { // Destroy all the joints that have not been removed - std::set::iterator itJoints; - for (itJoints = mJoints.begin(); itJoints != mJoints.end();) { - std::set::iterator itToRemove = itJoints; - ++itJoints; - destroyJoint(*itToRemove); + for (int i=mJoints.size() - 1; i >= 0; i--) { + destroyJoint(mJoints[i]); } // Destroy all the rigid bodies that have not been removed - std::set::iterator itRigidBodies; - for (itRigidBodies = mRigidBodies.begin(); itRigidBodies != mRigidBodies.end();) { - std::set::iterator itToRemove = itRigidBodies; - ++itRigidBodies; - destroyRigidBody(*itToRemove); + for (int i=mRigidBodies.size() - 1; i >= 0; i--) { + destroyRigidBody(mRigidBodies[i]); } assert(mJoints.size() == 0); @@ -257,7 +252,7 @@ void DynamicsWorld::initVelocityArrays() { // Initialize the map of body indexes in the velocity arrays uint i = 0; - for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + for (List::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { mSplitLinearVelocities[i].setToZero(); mSplitAngularVelocities[i].setToZero(); @@ -388,7 +383,7 @@ void DynamicsWorld::solvePositionCorrection() { PROFILE("DynamicsWorld::solvePositionCorrection()", &mProfiler); // Do not continue if there is no constraints - if (mJoints.empty()) return; + if (mJoints.size() == 0) return; // For each island of the world for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) { @@ -423,8 +418,8 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) { assert(rigidBody != nullptr); // Add the rigid body to the physics world - mBodies.insert(rigidBody); - mRigidBodies.insert(rigidBody); + mBodies.add(rigidBody); + mRigidBodies.add(rigidBody); #ifdef IS_PROFILING_ACTIVE @@ -446,7 +441,7 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { rigidBody->removeAllCollisionShapes(); // Add the body ID to the list of free IDs - mFreeBodiesIDs.push_back(rigidBody->getID()); + mFreeBodiesIDs.add(rigidBody->getID()); // Destroy all the joints in which the rigid body to be destroyed is involved JointListElement* element; @@ -461,8 +456,8 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { rigidBody->~RigidBody(); // Remove the rigid body from the list of rigid bodies - mBodies.erase(rigidBody); - mRigidBodies.erase(rigidBody); + mBodies.remove(rigidBody); + mRigidBodies.remove(rigidBody); // Free the object from the memory allocator mMemoryManager.release(MemoryManager::AllocationType::Pool, rigidBody, sizeof(RigidBody)); @@ -536,7 +531,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { } // Add the joint into the world - mJoints.insert(newJoint); + mJoints.add(newJoint); // Add the joint into the joint list of the bodies involved in the joint addJointToBody(newJoint); @@ -565,7 +560,7 @@ void DynamicsWorld::destroyJoint(Joint* joint) { joint->getBody2()->setIsSleeping(false); // Remove the joint from the world - mJoints.erase(joint); + mJoints.remove(joint); // Remove the joint from the joint list of the bodies involved in the joint joint->mBody1->removeJointFromJointsList(mMemoryManager, joint); @@ -622,11 +617,11 @@ void DynamicsWorld::computeIslands() { int nbContactManifolds = 0; // Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds - for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + for (List::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { int nbBodyManifolds = (*it)->resetIsAlreadyInIslandAndCountManifolds(); nbContactManifolds += nbBodyManifolds; } - for (std::set::iterator it = mJoints.begin(); it != mJoints.end(); ++it) { + for (List::Iterator it = mJoints.begin(); it != mJoints.end(); ++it) { (*it)->mIsAlreadyInIsland = false; } @@ -636,7 +631,7 @@ void DynamicsWorld::computeIslands() { nbBytesStack)); // For each rigid body of the world - for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + for (List::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { RigidBody* body = *it; @@ -818,7 +813,7 @@ void DynamicsWorld::enableSleeping(bool isSleepingEnabled) { if (!mIsSleepingEnabled) { // For each body of the world - std::set::iterator it; + List::Iterator it; for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { // Wake up the rigid body @@ -828,12 +823,12 @@ void DynamicsWorld::enableSleeping(bool isSleepingEnabled) { } /// Return the list of all contacts of the world -std::vector DynamicsWorld::getContactsList() const { +List DynamicsWorld::getContactsList() { - std::vector contactManifolds; + List contactManifolds(mMemoryManager.getPoolAllocator()); // For each currently overlapping pair of bodies - std::map::const_iterator it; + Map, OverlappingPair*>::Iterator it; for (it = mCollisionDetection.mOverlappingPairs.begin(); it != mCollisionDetection.mOverlappingPairs.end(); ++it) { @@ -845,7 +840,7 @@ std::vector DynamicsWorld::getContactsList() const { while (manifold != nullptr) { // Get the contact manifold - contactManifolds.push_back(manifold); + contactManifolds.add(manifold); manifold = manifold->getNext(); } diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 0c614b65..af2fb659 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -66,10 +66,10 @@ class DynamicsWorld : public CollisionWorld { bool mIsSleepingEnabled; /// All the rigid bodies of the physics world - std::set mRigidBodies; + List mRigidBodies; /// All the joints of the world - std::set mJoints; + List mJoints; /// Gravity vector of the world Vector3 mGravity; @@ -215,12 +215,6 @@ class DynamicsWorld : public CollisionWorld { /// Return the number of joints in the world uint getNbJoints() const; - /// Return an iterator to the beginning of the rigid bodies of the physics world - std::set::iterator getRigidBodiesBeginIterator(); - - /// Return an iterator to the end of the rigid bodies of the physics world - std::set::iterator getRigidBodiesEndIterator(); - /// Return true if the sleeping technique is enabled bool isSleepingEnabled() const; @@ -249,7 +243,7 @@ class DynamicsWorld : public CollisionWorld { void setEventListener(EventListener* eventListener); /// Return the list of all contacts of the world - std::vector getContactsList() const; + List getContactsList(); // -------------------- Friendship -------------------- // @@ -260,7 +254,7 @@ class DynamicsWorld : public CollisionWorld { inline void DynamicsWorld::resetBodiesForceAndTorque() { // For each body of the world - std::set::iterator it; + List::Iterator it; for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { (*it)->mExternalForce.setToZero(); (*it)->mExternalTorque.setToZero(); @@ -370,22 +364,6 @@ inline uint DynamicsWorld::getNbJoints() const { return mJoints.size(); } -// Return an iterator to the beginning of the bodies of the physics world -/** - * @return Starting iterator of the set of rigid bodies - */ -inline std::set::iterator DynamicsWorld::getRigidBodiesBeginIterator() { - return mRigidBodies.begin(); -} - -// Return an iterator to the end of the bodies of the physics world -/** - * @return Ending iterator of the set of rigid bodies - */ -inline std::set::iterator DynamicsWorld::getRigidBodiesEndIterator() { - return mRigidBodies.end(); -} - // Return true if the sleeping technique is enabled /** * @return True if the sleeping technique is enabled and false otherwise diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index 7a95be95..16e077e9 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -28,14 +28,17 @@ #include "OverlappingPair.h" #include "collision/ContactManifoldInfo.h" #include "collision/NarrowPhaseInfo.h" +#include "containers/containers_common.h" using namespace reactphysics3d; + // Constructor OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator) : mContactManifoldSet(shape1, shape2, persistentMemoryAllocator), mPotentialContactManifolds(nullptr), - mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator) { + mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator), + mLastFrameCollisionInfos(mPersistentAllocator) { } @@ -150,7 +153,8 @@ void OverlappingPair::reducePotentialContactManifolds() { void OverlappingPair::addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2) { // Try to get the corresponding last frame collision info - auto it = mLastFrameCollisionInfos.find(std::make_pair(shapeId1, shapeId2)); + const ShapeIdPair shapeIdPair(shapeId1, shapeId2); + auto it = mLastFrameCollisionInfos.find(shapeIdPair); // If there is no collision info for those two shapes already if (it == mLastFrameCollisionInfos.end()) { @@ -160,9 +164,7 @@ void OverlappingPair::addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2) LastFrameCollisionInfo(); // Add it into the map of collision infos - std::map, LastFrameCollisionInfo*>::iterator it; - auto ret = mLastFrameCollisionInfos.insert(std::make_pair(std::make_pair(shapeId1, shapeId2), collisionInfo)); - assert(ret.second); + mLastFrameCollisionInfos.add(Pair(shapeIdPair, collisionInfo)); } else { @@ -185,7 +187,7 @@ void OverlappingPair::clearObsoleteLastFrameCollisionInfos() { it->second->~LastFrameCollisionInfo(); mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo)); - mLastFrameCollisionInfos.erase(it++); + it = mLastFrameCollisionInfos.remove(it); } else { ++it; diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index f3631775..e93658d0 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -30,14 +30,13 @@ #include "collision/ContactManifoldSet.h" #include "collision/ProxyShape.h" #include "collision/shapes/CollisionShape.h" -#include +#include "containers/Map.h" +#include "containers/Pair.h" +#include "containers/containers_common.h" /// ReactPhysics3D namespace namespace reactphysics3d { -// Type for the overlapping pair ID -using overlappingpairid = std::pair; - // Structure LastFrameCollisionInfo /** * This structure contains collision info about the last frame. @@ -95,6 +94,11 @@ struct LastFrameCollisionInfo { */ class OverlappingPair { + public: + + using OverlappingPairId = Pair; + using ShapeIdPair = Pair; + private: // -------------------- Attributes -------------------- // @@ -102,13 +106,6 @@ class OverlappingPair { /// Set of persistent contact manifolds ContactManifoldSet mContactManifoldSet; - /// Temporal coherence collision data for each overlapping collision shapes of this pair. - /// Temporal coherence data store collision information about the last frame. - /// If two convex shapes overlap, we have a single collision data but if one shape is concave, - /// we might have collision data for several overlapping triangles. The key in the map is the - /// shape Ids of the two collision shapes. - std::map, LastFrameCollisionInfo*> mLastFrameCollisionInfos; - /// Linked-list of potential contact manifold ContactManifoldInfo* mPotentialContactManifolds; @@ -118,6 +115,13 @@ class OverlappingPair { /// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo MemoryAllocator& mTempMemoryAllocator; + /// Temporal coherence collision data for each overlapping collision shapes of this pair. + /// Temporal coherence data store collision information about the last frame. + /// If two convex shapes overlap, we have a single collision data but if one shape is concave, + /// we might have collision data for several overlapping triangles. The key in the map is the + /// shape Ids of the two collision shapes. + Map mLastFrameCollisionInfos; + public: // -------------------- Methods -------------------- // @@ -142,7 +146,7 @@ class OverlappingPair { ProxyShape* getShape2() const; /// Return the last frame collision info - LastFrameCollisionInfo* getLastFrameCollisionInfo(std::pair shapeIds); + LastFrameCollisionInfo* getLastFrameCollisionInfo(ShapeIdPair& shapeIds); /// Return the a reference to the contact manifold set const ContactManifoldSet& getContactManifoldSet(); @@ -193,7 +197,7 @@ class OverlappingPair { void makeLastFrameCollisionInfosObsolete(); /// Return the pair of bodies index - static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2); + static OverlappingPairId computeID(ProxyShape* shape1, ProxyShape* shape2); /// Return the pair of bodies index of the pair static bodyindexpair computeBodiesIndexPair(CollisionBody* body1, CollisionBody* body2); @@ -219,8 +223,8 @@ inline void OverlappingPair::addContactManifold(const ContactManifoldInfo* conta } // Return the last frame collision info for a given shape id or nullptr if none is found -inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(std::pair shapeIds) { - std::map, LastFrameCollisionInfo*>::iterator it = mLastFrameCollisionInfos.find(shapeIds); +inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(ShapeIdPair& shapeIds) { + Map::Iterator it = mLastFrameCollisionInfos.find(shapeIds); if (it != mLastFrameCollisionInfos.end()) { return it->second; } @@ -240,13 +244,13 @@ inline void OverlappingPair::makeContactsObsolete() { } // Return the pair of bodies index -inline overlappingpairid OverlappingPair::computeID(ProxyShape* shape1, ProxyShape* shape2) { +inline OverlappingPair::OverlappingPairId OverlappingPair::computeID(ProxyShape* shape1, ProxyShape* shape2) { assert(shape1->mBroadPhaseID >= 0 && shape2->mBroadPhaseID >= 0); // Construct the pair of body index - overlappingpairid pairID = shape1->mBroadPhaseID < shape2->mBroadPhaseID ? - std::make_pair(shape1->mBroadPhaseID, shape2->mBroadPhaseID) : - std::make_pair(shape2->mBroadPhaseID, shape1->mBroadPhaseID); + OverlappingPairId pairID = shape1->mBroadPhaseID < shape2->mBroadPhaseID ? + OverlappingPairId(shape1->mBroadPhaseID, shape2->mBroadPhaseID) : + OverlappingPairId(shape2->mBroadPhaseID, shape1->mBroadPhaseID); assert(pairID.first != pairID.second); return pairID; } @@ -257,8 +261,8 @@ inline bodyindexpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body // Construct the pair of body index bodyindexpair indexPair = body1->getID() < body2->getID() ? - std::make_pair(body1->getID(), body2->getID()) : - std::make_pair(body2->getID(), body1->getID()); + bodyindexpair(body1->getID(), body2->getID()) : + bodyindexpair(body2->getID(), body1->getID()); assert(indexPair.first != indexPair.second); return indexPair; } @@ -296,7 +300,7 @@ inline void OverlappingPair::reduceContactManifolds() { // Return the last frame collision info for a given pair of shape ids inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(uint shapeId1, uint shapeId2) const { - return mLastFrameCollisionInfos.at(std::make_pair(shapeId1, shapeId2)); + return mLastFrameCollisionInfos[ShapeIdPair(shapeId1, shapeId2)]; } } diff --git a/src/mathematics/Ray.h b/src/mathematics/Ray.h index 938e8960..8efc7416 100644 --- a/src/mathematics/Ray.h +++ b/src/mathematics/Ray.h @@ -44,10 +44,10 @@ struct Ray { // -------------------- Attributes -------------------- // - /// First point of the ray (origin) + /// First point of the ray (origin) in world-space Vector3 point1; - /// Second point of the ray + /// Second point of the ray in world-space Vector3 point2; /// Maximum fraction value diff --git a/src/mathematics/Vector2.cpp b/src/mathematics/Vector2.cpp index 271fd82f..eed109dc 100644 --- a/src/mathematics/Vector2.cpp +++ b/src/mathematics/Vector2.cpp @@ -25,7 +25,6 @@ // Libraries #include "Vector2.h" -#include // Namespaces using namespace reactphysics3d; diff --git a/src/mathematics/Vector3.cpp b/src/mathematics/Vector3.cpp index 33b760ba..9a0cc082 100644 --- a/src/mathematics/Vector3.cpp +++ b/src/mathematics/Vector3.cpp @@ -26,7 +26,6 @@ // Libraries #include "Vector3.h" #include -#include // Namespaces using namespace reactphysics3d; diff --git a/src/mathematics/mathematics.h b/src/mathematics/mathematics.h index b2f2bd1f..9fac1571 100644 --- a/src/mathematics/mathematics.h +++ b/src/mathematics/mathematics.h @@ -36,7 +36,6 @@ #include "Ray.h" #include "configuration.h" #include "mathematics_functions.h" -#include #include #include #include diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index 7e4a1427..4494e1ec 100755 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -28,7 +28,6 @@ #include "Vector3.h" #include "Vector2.h" #include -#include using namespace reactphysics3d; @@ -226,30 +225,26 @@ List reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const V const List& planesPoints, const List& planesNormals, MemoryAllocator& allocator) { - assert(planesPoints.size() == planesNormals.size()); - List list1(allocator, 2); - List list2(allocator, 2); + List inputVertices(allocator, 2); + List outputVertices(allocator, 2); - List* inputVertices = &list1; - List* outputVertices = &list2; - - inputVertices->add(segA); - inputVertices->add(segB); + inputVertices.add(segA); + inputVertices.add(segB); // For each clipping plane for (uint p=0; psize() == 0) return *inputVertices; + if (inputVertices.size() == 0) return inputVertices; - assert(inputVertices->size() == 2); + assert(inputVertices.size() == 2); - outputVertices->clear(); + outputVertices.clear(); - Vector3& v1 = (*inputVertices)[0]; - Vector3& v2 = (*inputVertices)[1]; + Vector3& v1 = inputVertices[0]; + Vector3& v2 = inputVertices[1]; decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]); decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]); @@ -264,40 +259,39 @@ List reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const V decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]); if (t >= decimal(0) && t <= decimal(1.0)) { - outputVertices->add(v1 + t * (v2 - v1)); + outputVertices.add(v1 + t * (v2 - v1)); } else { - outputVertices->add(v2); + outputVertices.add(v2); } } else { - outputVertices->add(v1); + outputVertices.add(v1); } // Add the second vertex - outputVertices->add(v2); + outputVertices.add(v2); } else { // If the second vertex is behind the clipping plane // If the first vertex is in front of the clippling plane if (v1DotN >= decimal(0.0)) { - outputVertices->add(v1); + outputVertices.add(v1); // The first point we keep is the intersection between the segment v1, v2 and the clipping plane decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]); if (t >= decimal(0.0) && t <= decimal(1.0)) { - outputVertices->add(v1 + t * (v2 - v1)); + outputVertices.add(v1 + t * (v2 - v1)); } } } inputVertices = outputVertices; - outputVertices = p % 2 == 0 ? &list1 : &list2; } - return *outputVertices; + return outputVertices; } // Clip a polygon against multiple planes and return the clipped polygon vertices @@ -307,75 +301,73 @@ List reactphysics3d::clipPolygonWithPlanes(const List& polygon assert(planesPoints.size() == planesNormals.size()); - uint nbMaxElements = polygonVertices.size() + planesPoints.size(); - List list1(allocator, nbMaxElements); - List list2(allocator, nbMaxElements); + uint nbMaxElements = polygonVertices.size() + planesPoints.size(); + List inputVertices(allocator, nbMaxElements); + List outputVertices(allocator, nbMaxElements); - const List* inputVertices = &polygonVertices; - List* outputVertices = &list2; + inputVertices.addRange(polygonVertices); - // For each clipping plane - for (uint p=0; pclear(); + outputVertices.clear(); - uint nbInputVertices = inputVertices->size(); - uint vStart = nbInputVertices - 1; + uint nbInputVertices = inputVertices.size(); + uint vStart = nbInputVertices - 1; - // For each edge of the polygon - for (uint vEnd = 0; vEnd= decimal(0.0)) { + // If the second vertex is in front of the clippling plane + if (v2DotN >= decimal(0.0)) { - // If the first vertex is not in front of the clippling plane - if (v1DotN < decimal(0.0)) { + // If the first vertex is not in front of the clippling plane + if (v1DotN < decimal(0.0)) { - // The second point we keep is the intersection between the segment v1, v2 and the clipping plane - decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]); + // The second point we keep is the intersection between the segment v1, v2 and the clipping plane + decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]); - if (t >= decimal(0) && t <= decimal(1.0)) { - outputVertices->add(v1 + t * (v2 - v1)); + if (t >= decimal(0) && t <= decimal(1.0)) { + outputVertices.add(v1 + t * (v2 - v1)); + } + else { + outputVertices.add(v2); + } + } + + // Add the second vertex + outputVertices.add(v2); + } + else { // If the second vertex is behind the clipping plane + + // If the first vertex is in front of the clippling plane + if (v1DotN >= decimal(0.0)) { + + // The first point we keep is the intersection between the segment v1, v2 and the clipping plane + decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]); + + if (t >= decimal(0.0) && t <= decimal(1.0)) { + outputVertices.add(v1 + t * (v2 - v1)); + } + else { + outputVertices.add(v1); + } } - else { - outputVertices->add(v2); - } } - // Add the second vertex - outputVertices->add(v2); - } - else { // If the second vertex is behind the clipping plane - - // If the first vertex is in front of the clippling plane - if (v1DotN >= decimal(0.0)) { - - // The first point we keep is the intersection between the segment v1, v2 and the clipping plane - decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]); - - if (t >= decimal(0.0) && t <= decimal(1.0)) { - outputVertices->add(v1 + t * (v2 - v1)); - } - else { - outputVertices->add(v1); - } - } + vStart = vEnd; } - vStart = vEnd; + inputVertices = outputVertices; } - inputVertices = outputVertices; - outputVertices = p % 2 == 0 ? &list1 : &list2; - } - - return *outputVertices; + return outputVertices; } // Project a point onto a plane that is given by a point and its unit length normal @@ -383,6 +375,11 @@ Vector3 reactphysics3d::projectPointOntoPlane(const Vector3& point, const Vector return point - unitPlaneNormal.dot(point - planePoint) * unitPlaneNormal; } +// Return the distance between a point and a plane (the plane normal must be normalized) +decimal reactphysics3d::computePointToPlaneDistance(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint) { + return planeNormal.dot(point - planePoint); +} + // Return true if the given number is prime bool reactphysics3d::isPrimeNumber(int number) { diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index a72b0077..62c10c74 100755 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -32,7 +32,6 @@ #include #include #include -#include #include "containers/List.h" /// ReactPhysics3D namespace @@ -124,6 +123,9 @@ List clipPolygonWithPlanes(const List& polygonVertices, const /// Project a point onto a plane that is given by a point and its unit length normal Vector3 projectPointOntoPlane(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint); +/// Return the distance between a point and a plane (the plane normal must be normalized) +decimal computePointToPlaneDistance(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint); + /// Return true if the given number is prime bool isPrimeNumber(int number); diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index dd2a6265..f2ea5abe 100644 --- a/src/reactphysics3d.h +++ b/src/reactphysics3d.h @@ -64,6 +64,7 @@ #include "constraint/SliderJoint.h" #include "constraint/HingeJoint.h" #include "constraint/FixedJoint.h" +#include "containers/List.h" /// Alias to the ReactPhysics3D namespace namespace rp3d = reactphysics3d; diff --git a/test/main.cpp b/test/main.cpp index 8bb62ea4..24b9f003 100644 --- a/test/main.cpp +++ b/test/main.cpp @@ -41,6 +41,7 @@ #include "tests/collision/TestTriangleVertexArray.h" #include "tests/containers/TestList.h" #include "tests/containers/TestMap.h" +#include "tests/containers/TestSet.h" using namespace reactphysics3d; @@ -52,6 +53,7 @@ int main() { testSuite.addTest(new TestList("List")); testSuite.addTest(new TestMap("Map")); + testSuite.addTest(new TestSet("Set")); // ---------- Mathematics tests ---------- // diff --git a/test/tests/collision/TestCollisionWorld.h b/test/tests/collision/TestCollisionWorld.h old mode 100755 new mode 100644 index ad0c92cf..64abab25 --- a/test/tests/collision/TestCollisionWorld.h +++ b/test/tests/collision/TestCollisionWorld.h @@ -29,6 +29,7 @@ // Libraries #include "reactphysics3d.h" #include "Test.h" +#include /// Reactphysics3D namespace namespace reactphysics3d { @@ -53,7 +54,7 @@ struct CollisionPointData { penetrationDepth = penDepth; } - bool isContactPointSimilarTo(const Vector3& pointBody1, const Vector3& pointBody2, decimal penDepth, decimal epsilon = 0.001) { + bool isContactPointSimilarTo(const Vector3& pointBody1, const Vector3& pointBody2, decimal penDepth, decimal epsilon = 0.001) const { return approxEqual(pointBody1, localPointBody1, epsilon) && approxEqual(pointBody2, localPointBody2, epsilon) && @@ -70,11 +71,12 @@ struct CollisionManifoldData { return contactPoints.size(); } - bool hasContactPointSimilarTo(const Vector3& localPointBody1, const Vector3& localPointBody2, decimal penetrationDepth, decimal epsilon = 0.001) { + bool hasContactPointSimilarTo(const Vector3& localPointBody1, const Vector3& localPointBody2, decimal penetrationDepth, decimal epsilon = 0.001) const { - std::vector::iterator it; - for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { + std::vector::const_iterator it; + for (it = contactPoints.cbegin(); it != contactPoints.cend(); ++it) { + Vector3 vec = it->localPointBody1; if (it->isContactPointSimilarTo(localPointBody1, localPointBody2, penetrationDepth)) { return true; } @@ -109,10 +111,18 @@ struct CollisionData { return nbPoints; } - bool hasContactPointSimilarTo(const Vector3& localPointBody1, const Vector3& localPointBody2, decimal penetrationDepth, decimal epsilon = 0.001) { + const CollisionBody* getBody1() const { + return bodies.first; + } - std::vector::iterator it; - for (it = contactManifolds.begin(); it != contactManifolds.end(); ++it) { + const CollisionBody* getBody2() const { + return bodies.second; + } + + bool hasContactPointSimilarTo(const Vector3& localPointBody1, const Vector3& localPointBody2, decimal penetrationDepth, decimal epsilon = 0.001) const { + + std::vector::const_iterator it; + for (it = contactManifolds.cbegin(); it != contactManifolds.cend(); ++it) { if (it->hasContactPointSimilarTo(localPointBody1, localPointBody2, penetrationDepth)) { return true; @@ -129,7 +139,7 @@ class WorldCollisionCallback : public CollisionCallback { private: - std::vector> mCollisionData; + std::map, CollisionData> mCollisionDatas; std::pair getCollisionKeyPair(std::pair pair) const { @@ -149,15 +159,25 @@ class WorldCollisionCallback : public CollisionCallback void reset() { - mCollisionData.clear(); + mCollisionDatas.clear(); } bool hasContacts() const { - return mCollisionData.size() > 0; + return mCollisionDatas.size() > 0; } bool areProxyShapesColliding(const ProxyShape* proxyShape1, const ProxyShape* proxyShape2) { - return std::find(mCollisionData.begin(), mCollisionData.end(), getCollisionKeyPair(std::make_pair(proxyShape1, proxyShape2))) != mCollisionData.end(); + return mCollisionDatas.find(getCollisionKeyPair(std::make_pair(proxyShape1, proxyShape2))) != mCollisionDatas.end(); + } + + const CollisionData* getCollisionData(const ProxyShape* proxyShape1, const ProxyShape* proxyShape2) const { + std::map, CollisionData>::const_iterator it = mCollisionDatas.find(getCollisionKeyPair(std::make_pair(proxyShape1, proxyShape2))); + if (it != mCollisionDatas.end()) { + return &(it->second); + } + else { + return nullptr; + } } // This method will be called for each contact @@ -184,6 +204,7 @@ class WorldCollisionCallback : public CollisionCallback } collisionData.contactManifolds.push_back(collisionManifold); + mCollisionDatas.insert(std::make_pair(getCollisionKeyPair(collisionData.proxyShapes), collisionData)); element = element->getNext(); } @@ -195,7 +216,7 @@ class WorldOverlapCallback : public OverlapCallback { private: - CollisionBody* mOverlapBody; + std::vector mOverlapBodies; public: @@ -206,19 +227,19 @@ class WorldOverlapCallback : public OverlapCallback { /// This method will be called for each reported overlapping bodies virtual void notifyOverlap(CollisionBody* collisionBody) override { - + mOverlapBodies.push_back(collisionBody); } void reset() { - mOverlapBody = nullptr; + mOverlapBodies.clear(); } bool hasOverlap() const { - return mOverlapBody != nullptr; + return !mOverlapBodies.empty(); } - CollisionBody* getOverlapBody() { - return mOverlapBody; + std::vector& getOverlapBodies() { + return mOverlapBodies; } }; @@ -240,18 +261,48 @@ class TestCollisionWorld : public Test { CollisionBody* mBoxBody2; CollisionBody* mSphereBody1; CollisionBody* mSphereBody2; + CollisionBody* mCapsuleBody1; + CollisionBody* mCapsuleBody2; + CollisionBody* mConvexMeshBody1; + CollisionBody* mConvexMeshBody2; + CollisionBody* mConcaveMeshBody; // Collision shapes BoxShape* mBoxShape1; BoxShape* mBoxShape2; SphereShape* mSphereShape1; SphereShape* mSphereShape2; + CapsuleShape* mCapsuleShape1; + CapsuleShape* mCapsuleShape2; + ConvexMeshShape* mConvexMeshShape1; + ConvexMeshShape* mConvexMeshShape2; + ConcaveMeshShape* mConcaveMeshShape; // Proxy shapes ProxyShape* mBoxProxyShape1; ProxyShape* mBoxProxyShape2; ProxyShape* mSphereProxyShape1; ProxyShape* mSphereProxyShape2; + ProxyShape* mCapsuleProxyShape1; + ProxyShape* mCapsuleProxyShape2; + ProxyShape* mConvexMeshProxyShape1; + ProxyShape* mConvexMeshProxyShape2; + ProxyShape* mConcaveMeshProxyShape; + + PolygonVertexArray* mConvexMesh1PolygonVertexArray; + PolygonVertexArray* mConvexMesh2PolygonVertexArray; + PolyhedronMesh* mConvexMesh1PolyhedronMesh; + PolyhedronMesh* mConvexMesh2PolyhedronMesh; + PolygonVertexArray::PolygonFace* mConvexMeshPolygonFaces; + + TriangleVertexArray* mConcaveMeshTriangleVertexArray; + Vector3 mConvexMesh1CubeVertices[8]; + Vector3 mConvexMesh2CubeVertices[8]; + int mConvexMeshCubeIndices[24]; + + Vector3 mConcaveMeshPlaneVertices[36]; + int mConcaveMeshPlaneIndices[25 * 2 * 3]; + TriangleMesh* mConcaveTriangleMesh; // Collision callback WorldCollisionCallback mCollisionCallback; @@ -277,8 +328,8 @@ class TestCollisionWorld : public Test { Transform boxTransform2(Vector3(-10, 20, 0), Quaternion::identity()); mBoxBody2 = mWorld->createCollisionBody(boxTransform2); - mBoxShape2 = new BoxShape(Vector3(2, 2, 2)); - mBoxProxyShape2 = mBoxBody2->addCollisionShape(mBoxShape1, Transform::identity()); + mBoxShape2 = new BoxShape(Vector3(4, 2, 8)); + mBoxProxyShape2 = mBoxBody2->addCollisionShape(mBoxShape2, Transform::identity()); // ---------- Spheres ---------- // mSphereShape1 = new SphereShape(3.0); @@ -290,6 +341,107 @@ class TestCollisionWorld : public Test { Transform sphereTransform2(Vector3(20, 20, 0), Quaternion::identity()); mSphereBody2 = mWorld->createCollisionBody(sphereTransform2); mSphereProxyShape2 = mSphereBody2->addCollisionShape(mSphereShape2, Transform::identity()); + + // ---------- Capsules ---------- // + mCapsuleShape1 = new CapsuleShape(2, 6); + Transform capsuleTransform1(Vector3(-10, 0, 0), Quaternion::identity()); + mCapsuleBody1 = mWorld->createCollisionBody(capsuleTransform1); + mCapsuleProxyShape1 = mCapsuleBody1->addCollisionShape(mCapsuleShape1, Transform::identity()); + + mCapsuleShape2 = new CapsuleShape(3, 4); + Transform capsuleTransform2(Vector3(-20, 0, 0), Quaternion::identity()); + mCapsuleBody2 = mWorld->createCollisionBody(capsuleTransform2); + mCapsuleProxyShape2 = mCapsuleBody2->addCollisionShape(mCapsuleShape2, Transform::identity()); + + // ---------- Convex Meshes ---------- // + mConvexMesh1CubeVertices[0] = Vector3(-3, -3, 3); + mConvexMesh1CubeVertices[1] = Vector3(3, -3, 3); + mConvexMesh1CubeVertices[2] = Vector3(3, -3, -3); + mConvexMesh1CubeVertices[3] = Vector3(-3, -3, -3); + mConvexMesh1CubeVertices[4] = Vector3(-3, 3, 3); + mConvexMesh1CubeVertices[5] = Vector3(3, 3, 3); + mConvexMesh1CubeVertices[6] = Vector3(3, 3, -3); + mConvexMesh1CubeVertices[7] = Vector3(-3, 3, -3); + + mConvexMeshCubeIndices[0] = 0; mConvexMeshCubeIndices[1] = 3; mConvexMeshCubeIndices[2] = 2; mConvexMeshCubeIndices[3] = 1; + mConvexMeshCubeIndices[4] = 4; mConvexMeshCubeIndices[5] = 5; mConvexMeshCubeIndices[6] = 6; mConvexMeshCubeIndices[7] = 7; + mConvexMeshCubeIndices[8] = 0; mConvexMeshCubeIndices[9] = 1; mConvexMeshCubeIndices[10] = 5; mConvexMeshCubeIndices[11] = 4; + mConvexMeshCubeIndices[12] = 1; mConvexMeshCubeIndices[13] = 2; mConvexMeshCubeIndices[14] = 6; mConvexMeshCubeIndices[15] = 5; + mConvexMeshCubeIndices[16] = 2; mConvexMeshCubeIndices[17] = 3; mConvexMeshCubeIndices[18] = 7; mConvexMeshCubeIndices[19] = 6; + mConvexMeshCubeIndices[20] = 0; mConvexMeshCubeIndices[21] = 4; mConvexMeshCubeIndices[22] = 7; mConvexMeshCubeIndices[23] = 3; + + mConvexMeshPolygonFaces = new rp3d::PolygonVertexArray::PolygonFace[6]; + rp3d::PolygonVertexArray::PolygonFace* face = mConvexMeshPolygonFaces; + for (int f = 0; f < 6; f++) { + face->indexBase = f * 4; + face->nbVertices = 4; + face++; + } + mConvexMesh1PolygonVertexArray = new rp3d::PolygonVertexArray(8, &(mConvexMesh1CubeVertices[0]), sizeof(Vector3), + &(mConvexMeshCubeIndices[0]), sizeof(int), 6, mConvexMeshPolygonFaces, + rp3d::PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, + rp3d::PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + mConvexMesh1PolyhedronMesh = new rp3d::PolyhedronMesh(mConvexMesh1PolygonVertexArray); + mConvexMeshShape1 = new rp3d::ConvexMeshShape(mConvexMesh1PolyhedronMesh); + Transform convexMeshTransform1(Vector3(10, 0, 0), Quaternion::identity()); + mConvexMeshBody1 = mWorld->createCollisionBody(convexMeshTransform1); + mConvexMeshProxyShape1 = mConvexMeshBody1->addCollisionShape(mConvexMeshShape1, Transform::identity()); + + mConvexMesh2CubeVertices[0] = Vector3(-4, -2, 8); + mConvexMesh2CubeVertices[1] = Vector3(4, -2, 8); + mConvexMesh2CubeVertices[2] = Vector3(4, -2, -8); + mConvexMesh2CubeVertices[3] = Vector3(-4, -2, -8); + mConvexMesh2CubeVertices[4] = Vector3(-4, 2, 8); + mConvexMesh2CubeVertices[5] = Vector3(4, 2, 8); + mConvexMesh2CubeVertices[6] = Vector3(4, 2, -8); + mConvexMesh2CubeVertices[7] = Vector3(-4, 2, -8); + + mConvexMesh2PolygonVertexArray = new rp3d::PolygonVertexArray(8, &(mConvexMesh2CubeVertices[0]), sizeof(Vector3), + &(mConvexMeshCubeIndices[0]), sizeof(int), 6, mConvexMeshPolygonFaces, + rp3d::PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, + rp3d::PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + mConvexMesh2PolyhedronMesh = new rp3d::PolyhedronMesh(mConvexMesh2PolygonVertexArray); + mConvexMeshShape2 = new rp3d::ConvexMeshShape(mConvexMesh2PolyhedronMesh); + Transform convexMeshTransform2(Vector3(20, 0, 0), Quaternion::identity()); + mConvexMeshBody2 = mWorld->createCollisionBody(convexMeshTransform2); + mConvexMeshProxyShape2 = mConvexMeshBody2->addCollisionShape(mConvexMeshShape2, Transform::identity()); + + // ---------- Concave Meshes ---------- // + for (int i = 0; i < 6; i++) { + for (int j = 0; j < 6; j++) { + mConcaveMeshPlaneVertices[i * 6 + j] = Vector3(-2.5f + i, 0, -2.5f + j); + } + } + int triangleIndex = 0; + for (int i = 0; i < 5; i++) { + for (int j = 0; j < 5; j++) { + + // Triangle 1 + mConcaveMeshPlaneIndices[triangleIndex * 3] = i * 6 + j; + mConcaveMeshPlaneIndices[triangleIndex * 3 + 1] = (i+1) * 6 + (j+1); + mConcaveMeshPlaneIndices[triangleIndex * 3 + 2] = i * 6 + (j+1); + triangleIndex++; + + // Triangle 2 + mConcaveMeshPlaneIndices[triangleIndex * 3] = i * 6 + j; + mConcaveMeshPlaneIndices[triangleIndex * 3 + 1] = (i+1) * 6 + j; + mConcaveMeshPlaneIndices[triangleIndex * 3 + 2] = (i+1) * 6 + (j+1); + triangleIndex++; + } + } + + mConcaveMeshTriangleVertexArray = new rp3d::TriangleVertexArray(36, &(mConcaveMeshPlaneVertices[0]), sizeof(Vector3), + 25, &(mConcaveMeshPlaneIndices[0]), 3 * sizeof(int), + rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, + rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + + // Add the triangle vertex array of the subpart to the triangle mesh + Transform concaveMeshTransform(Vector3(0, -20, 0), Quaternion::identity()); + mConcaveTriangleMesh = new TriangleMesh(); + mConcaveTriangleMesh->addSubpart(mConcaveMeshTriangleVertexArray); + mConcaveMeshShape = new rp3d::ConcaveMeshShape(mConcaveTriangleMesh); + mConcaveMeshBody = mWorld->createCollisionBody(concaveMeshTransform); + mConcaveMeshProxyShape = mConcaveMeshBody->addCollisionShape(mConcaveMeshShape, rp3d::Transform::identity()); } /// Destructor @@ -301,6 +453,21 @@ class TestCollisionWorld : public Test { delete mSphereShape1; delete mSphereShape2; + delete mCapsuleShape1; + delete mCapsuleShape2; + + delete mConvexMeshShape1; + delete mConvexMeshShape2; + delete mConvexMesh1PolyhedronMesh; + delete mConvexMesh2PolyhedronMesh; + delete mConvexMesh1PolygonVertexArray; + delete mConvexMesh2PolygonVertexArray; + delete mConvexMeshPolygonFaces; + + delete mConcaveMeshShape; + delete mConcaveTriangleMesh; + delete mConcaveMeshTriangleVertexArray; + delete mWorld; } @@ -311,12 +478,23 @@ class TestCollisionWorld : public Test { testNoOverlap(); testNoAABBOverlap(); - testAABBOverlap(); - testSphereVsSphereCollision(); testSphereVsBoxCollision(); + testSphereVsCapsuleCollision(); + testSphereVsConvexMeshCollision(); + testSphereVsConcaveMeshCollision(); - testMultipleCollisions(); + testBoxVsBoxCollision(); + testBoxVsConvexMeshCollision(); + testBoxVsCapsuleCollision(); + testBoxVsConcaveMeshCollision(); + + testCapsuleVsCapsuleCollision(); + testCapsuleVsConcaveMeshCollision(); + + testConvexMeshVsConvexMeshCollision(); + testConvexMeshVsCapsuleCollision(); + testConvexMeshVsConcaveMeshCollision(); } void testNoCollisions() { @@ -373,7 +551,6 @@ class TestCollisionWorld : public Test { mCollisionCallback.reset(); mWorld->testCollision(mBoxBody2, mSphereBody2, &mCollisionCallback); test(!mCollisionCallback.hasContacts()); - } void testNoOverlap() { @@ -424,85 +601,2459 @@ class TestCollisionWorld : public Test { test(!mWorld->testAABBOverlap(mBoxBody2, mSphereBody2)); } - void testAABBOverlap() { - - // TODO : Test the CollisionWorld::testAABBOverlap() method here - } - void testSphereVsSphereCollision() { + Transform initTransform1 = mSphereBody1->getTransform(); + Transform initTransform2 = mSphereBody2->getTransform(); + Transform transform1(Vector3(10, 20, 50), Quaternion::identity()); + Transform transform2(Vector3(17, 20, 50), Quaternion::fromEulerAngles(rp3d::PI / 8.0f, rp3d::PI / 4.0f, rp3d::PI / 16.0f)); - // Move sphere 1 to collide with sphere 2 - mSphereBody1->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); + // Move spheres to collide with each other + mSphereBody1->setTransform(transform1); + mSphereBody2->setTransform(transform2); + // ----- Test AABB overlap ----- // + + test(mWorld->testAABBOverlap(mSphereBody1, mSphereBody2)); + + mOverlapCallback.reset(); + mWorld->testOverlap(mSphereBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mSphereBody2, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + // ----- Test global collision test ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mSphereProxyShape2)); + + // Get collision data + const CollisionData* collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mSphereProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + bool swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + Vector3 localBody1Point(3, 0, 0); + Vector3 localBody2Point = transform2.getInverse() * Vector3(12, 20, 50); + decimal penetrationDepth = 1.0f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mSphereProxyShape2)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mSphereProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody2, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mSphereProxyShape2)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mSphereProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, mSphereBody2, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mSphereProxyShape2)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mSphereProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // Reset the init transforms + mSphereBody1->setTransform(initTransform1); + mSphereBody2->setTransform(initTransform2); } void testSphereVsBoxCollision() { - // Move sphere 1 to collide with box - mSphereBody1->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); + Transform initTransform1 = mSphereBody1->getTransform(); + Transform initTransform2 = mBoxBody1->getTransform(); - // --------- Test collision with inactive bodies --------- // + /******************************************************************************** + * Test Sphere vs Box Face collision * + *********************************************************************************/ - mCollisionCallback.reset(); - mBoxBody1->setIsActive(false); - mSphereBody1->setIsActive(false); - mSphereBody2->setIsActive(false); - mWorld->testCollision(&mCollisionCallback); - + Transform transform1(Vector3(10, 20, 50), Quaternion::identity()); + Transform transform2(Vector3(14, 20, 50), Quaternion::identity()); - mBoxBody1->setIsActive(true); - mSphereBody1->setIsActive(true); - mSphereBody2->setIsActive(true); + // Move spheres to collide with each other + mSphereBody1->setTransform(transform1); + mBoxBody1->setTransform(transform2); - // --------- Test collision with collision filtering -------- // + // ----- Test AABB overlap ----- // - //mBoxProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_3); - //mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_2); - //mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_1); + test(mWorld->testAABBOverlap(mSphereBody1, mBoxBody1)); - //mCollisionCallback.reset(); - //mWorld->testCollision(&mCollisionCallback); - //test(mCollisionCallback.boxCollideWithSphere1); - //test(!mCollisionCallback.sphere1CollideWithSphere2); + mOverlapCallback.reset(); + mWorld->testOverlap(mSphereBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); - //// Move sphere 1 to collide with sphere 2 - //mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); + mOverlapCallback.reset(); + mWorld->testOverlap(mBoxBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); - //mCollisionCallback.reset(); - //mWorld->testCollision(&mCollisionCallback); - //test(!mCollisionCallback.boxCollideWithSphere1); - //test(mCollisionCallback.sphere1CollideWithSphere2); + // ----- Test global collision test ----- // - //mBoxProxyShape->setCollideWithMaskBits(CATEGORY_2); - //mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_2); - //mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_3); + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); - //mCollisionCallback.reset(); - //mWorld->testCollision(&mCollisionCallback); - //test(!mCollisionCallback.boxCollideWithSphere1); - //test(!mCollisionCallback.sphere1CollideWithSphere2); + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1)); - //// Move sphere 1 to collide with box - //mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); + // Get collision data + const CollisionData* collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); - //mBoxProxyShape->setCollideWithMaskBits(0xFFFF); - //mSphere1ProxyShape->setCollideWithMaskBits(0xFFFF); - //mSphere2ProxyShape->setCollideWithMaskBits(0xFFFF); + // True if the bodies are swapped in the collision callback response + bool swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + Vector3 localBody1Point(3, 0, 0); + Vector3 localBody2Point(-3, 0, 0); + decimal penetrationDepth = 2.0f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, mBoxBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + /******************************************************************************** + * Test Sphere vs Box Edge collision * + *********************************************************************************/ + + transform1 = Transform(Vector3(10, 20, 50), Quaternion::identity()); + transform2 = Transform(Vector3(14, 16, 50), Quaternion::identity()); + + // Move spheres to collide with each other + mSphereBody1->setTransform(transform1); + mBoxBody1->setTransform(transform2); + + // ----- Test AABB overlap ----- // + + test(mWorld->testAABBOverlap(mSphereBody1, mBoxBody1)); + + mOverlapCallback.reset(); + mWorld->testOverlap(mSphereBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mBoxBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + // ----- Test global collision test ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + localBody1Point = std::sqrt(4.5f) * Vector3(1, -1, 0); + localBody2Point = Vector3(-3, 3, 0); + penetrationDepth = decimal(3.0) - std::sqrt(2); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, mBoxBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + /******************************************************************************** + * Test Sphere vs Box Vertex collision * + *********************************************************************************/ + + transform1 = Transform(Vector3(10, 20, 50), Quaternion::identity()); + transform2 = Transform(Vector3(14, 16, 46), Quaternion::identity()); + + // Move spheres to collide with each other + mSphereBody1->setTransform(transform1); + mBoxBody1->setTransform(transform2); + + // ----- Test AABB overlap ----- // + + test(mWorld->testAABBOverlap(mSphereBody1, mBoxBody1)); + + mOverlapCallback.reset(); + mWorld->testOverlap(mSphereBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mBoxBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + // ----- Test global collision test ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + localBody1Point = std::sqrt(9.0f / 3.0f) * Vector3(1, -1, -1); + localBody2Point = Vector3(-3, 3, 3); + penetrationDepth = decimal(3.0) - std::sqrt(3); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, mBoxBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // Reset the init transforms + mSphereBody1->setTransform(initTransform1); + mBoxBody1->setTransform(initTransform2); } - void testMultipleCollisions() { + void testSphereVsCapsuleCollision() { - // TODO : Test collisions without categories set + Transform initTransform1 = mSphereBody1->getTransform(); + Transform initTransform2 = mCapsuleBody1->getTransform(); - // TODO : Test colliisons with categories set + /******************************************************************************** + * Test Sphere vs Capsule (sphere side) collision * + *********************************************************************************/ - // Assign collision categories to proxy shapes - //mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1); - //mSphere1ProxyShape->setCollisionCategoryBits(CATEGORY_1); - //mSphere2ProxyShape->setCollisionCategoryBits(CATEGORY_2); + Transform transform1(Vector3(10, 20, 50), Quaternion::identity()); + Transform transform2(Vector3(10, 14, 50), Quaternion::identity()); + + // Move spheres to collide with each other + mSphereBody1->setTransform(transform1); + mCapsuleBody1->setTransform(transform2); + + // ----- Test AABB overlap ----- // + + test(mWorld->testAABBOverlap(mSphereBody1, mCapsuleBody1)); + + mOverlapCallback.reset(); + mWorld->testOverlap(mSphereBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mCapsuleBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + // ----- Test global collision test ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mCapsuleProxyShape1)); + + // Get collision data + const CollisionData* collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mCapsuleProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + bool swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + Vector3 localBody1Point(0, -3, 0); + Vector3 localBody2Point(0, 5, 0); + decimal penetrationDepth = 2.0f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mCapsuleProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mCapsuleProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mCapsuleBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mCapsuleProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mCapsuleProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, mCapsuleBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mCapsuleProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mCapsuleProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + /******************************************************************************** + * Test Sphere vs Box Capsule (cylinder side) collision * + *********************************************************************************/ + + transform1 = Transform(Vector3(10, 20, 50), Quaternion::identity()); + transform2 = Transform(Vector3(14, 19, 50), Quaternion::identity()); + + // Move spheres to collide with each other + mSphereBody1->setTransform(transform1); + mCapsuleBody1->setTransform(transform2); + + // ----- Test AABB overlap ----- // + + test(mWorld->testAABBOverlap(mSphereBody1, mCapsuleBody1)); + + mOverlapCallback.reset(); + mWorld->testOverlap(mSphereBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mCapsuleBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + // ----- Test global collision test ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mCapsuleProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mCapsuleProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + localBody1Point = Vector3(3, 0, 0); + localBody2Point = Vector3(-2, 1, 0); + penetrationDepth = decimal(1.0); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mCapsuleProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mCapsuleProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mCapsuleBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mCapsuleProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mCapsuleProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, mCapsuleBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mCapsuleProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mCapsuleProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // Reset the init transforms + mSphereBody1->setTransform(initTransform1); + mCapsuleBody1->setTransform(initTransform2); } + + void testSphereVsConvexMeshCollision() { + + Transform initTransform1 = mSphereBody1->getTransform(); + Transform initTransform2 = mConvexMeshBody1->getTransform(); + + /******************************************************************************** + * Test Sphere vs Convex Mesh (Cube Face) collision * + *********************************************************************************/ + + Transform transform1(Vector3(10, 20, 50), Quaternion::identity()); + Transform transform2(Vector3(14, 20, 50), Quaternion::identity()); + + // Move spheres to collide with each other + mSphereBody1->setTransform(transform1); + mConvexMeshBody1->setTransform(transform2); + + // ----- Test AABB overlap ----- // + + test(mWorld->testAABBOverlap(mSphereBody1, mConvexMeshBody1)); + + mOverlapCallback.reset(); + mWorld->testOverlap(mSphereBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mConvexMeshBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + // ----- Test global collision test ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1)); + + // Get collision data + const CollisionData* collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + bool swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + Vector3 localBody1Point(3, 0, 0); + Vector3 localBody2Point(-3, 0, 0); + decimal penetrationDepth = 2.0f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mConvexMeshBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, mConvexMeshBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + /******************************************************************************** + * Test Sphere vs Convex Mesh (Cube Edge) collision * + *********************************************************************************/ + + transform1 = Transform(Vector3(10, 20, 50), Quaternion::identity()); + transform2 = Transform(Vector3(14, 16, 50), Quaternion::identity()); + + // Move spheres to collide with each other + mSphereBody1->setTransform(transform1); + mConvexMeshBody1->setTransform(transform2); + + // ----- Test AABB overlap ----- // + + test(mWorld->testAABBOverlap(mSphereBody1, mConvexMeshBody1)); + + mOverlapCallback.reset(); + mWorld->testOverlap(mSphereBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mConvexMeshBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + // ----- Test global collision test ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + localBody1Point = std::sqrt(4.5f) * Vector3(1, -1, 0); + localBody2Point = Vector3(-3, 3, 0); + penetrationDepth = decimal(3.0) - std::sqrt(2); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mConvexMeshBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, mConvexMeshBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + /******************************************************************************** + * Test Sphere vs ConvexMesh (Cube Vertex) collision * + *********************************************************************************/ + + transform1 = Transform(Vector3(10, 20, 50), Quaternion::identity()); + transform2 = Transform(Vector3(14, 16, 46), Quaternion::identity()); + + // Move spheres to collide with each other + mSphereBody1->setTransform(transform1); + mConvexMeshBody1->setTransform(transform2); + + // ----- Test AABB overlap ----- // + + test(mWorld->testAABBOverlap(mSphereBody1, mConvexMeshBody1)); + + mOverlapCallback.reset(); + mWorld->testOverlap(mSphereBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mConvexMeshBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + // ----- Test global collision test ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + localBody1Point = std::sqrt(9.0f / 3.0f) * Vector3(1, -1, -1); + localBody2Point = Vector3(-3, 3, 3); + penetrationDepth = decimal(3.0) - std::sqrt(3); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mConvexMeshBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, mConvexMeshBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // Reset the init transforms + mSphereBody1->setTransform(initTransform1); + mConvexMeshBody1->setTransform(initTransform2); + } + + void testSphereVsConcaveMeshCollision() { + + Transform initTransform1 = mSphereBody1->getTransform(); + Transform initTransform2 = mConcaveMeshBody->getTransform(); + + /******************************************************************************** + * Test Sphere vs Concave Mesh + *********************************************************************************/ + + Transform transform1(Vector3(10, 22.98f, 50), Quaternion::identity()); + Transform transform2(Vector3(10, 20, 50), Quaternion::identity()); + + // Move spheres to collide with each other + mSphereBody1->setTransform(transform1); + mConcaveMeshBody->setTransform(transform2); + + // ----- Test AABB overlap ----- // + + test(mWorld->testAABBOverlap(mSphereBody1, mConcaveMeshBody)); + + mOverlapCallback.reset(); + mWorld->testOverlap(mSphereBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mConcaveMeshBody, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + // ----- Test global collision test ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConcaveMeshProxyShape)); + + // Get collision data + const CollisionData* collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConcaveMeshProxyShape); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + bool swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + Vector3 localBody1Point(0, -3, 0); + Vector3 localBody2Point(0, 0, 0); + decimal penetrationDepth = 0.02f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConcaveMeshProxyShape)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConcaveMeshProxyShape); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mConcaveMeshBody, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConcaveMeshProxyShape)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConcaveMeshProxyShape); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, mConcaveMeshBody, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConcaveMeshProxyShape)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConcaveMeshProxyShape); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mSphereBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // Reset the init transforms + mSphereBody1->setTransform(initTransform1); + mConcaveMeshBody->setTransform(initTransform2); + } + + void testBoxVsBoxCollision() { + + Transform initTransform1 = mBoxBody1->getTransform(); + Transform initTransform2 = mBoxBody2->getTransform(); + + /******************************************************************************** + * Test Box vs Box Face collision * + *********************************************************************************/ + + Transform transform1(Vector3(11, 20, 50), Quaternion::identity()); + Transform transform2(Vector3(4.5, 16, 40), Quaternion::identity()); + + // Move spheres to collide with each other + mBoxBody1->setTransform(transform1); + mBoxBody2->setTransform(transform2); + + // ----- Test AABB overlap ----- // + + test(mWorld->testAABBOverlap(mBoxBody1, mBoxBody2)); + + mOverlapCallback.reset(); + mWorld->testOverlap(mBoxBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mBoxBody2, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + // ----- Test global collision test ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mBoxProxyShape2)); + + // Get collision data + const CollisionData* collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mBoxProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + bool swappedBodiesCollisionData = collisionData->getBody1()->getID() != mBoxBody1->getID(); + + // Test contact points + Vector3 localBody1Point1(-3, -2, -2); + Vector3 localBody2Point1(4, 2, 8); + decimal penetrationDepth1 = 0.5f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + Vector3 localBody1Point2(-3, -2, -3); + Vector3 localBody2Point2(4, 2, 7); + decimal penetrationDepth2 = 0.5f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point2 : localBody1Point2, + swappedBodiesCollisionData ? localBody1Point2 : localBody2Point2, + penetrationDepth2)); + Vector3 localBody1Point3(-3, -3, -2); + Vector3 localBody2Point3(4, 1, 8); + decimal penetrationDepth3 = 0.5f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point3 : localBody1Point3, + swappedBodiesCollisionData ? localBody1Point3 : localBody2Point3, + penetrationDepth3)); + Vector3 localBody1Point4(-3, -3, -3); + Vector3 localBody2Point4(4, 1, 7); + decimal penetrationDepth4 = 0.5f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point4 : localBody1Point4, + swappedBodiesCollisionData ? localBody1Point4 : localBody2Point4, + penetrationDepth4)); + + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mBoxProxyShape2)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mBoxProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mBoxBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point2 : localBody1Point2, + swappedBodiesCollisionData ? localBody1Point2 : localBody2Point2, + penetrationDepth2)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point3 : localBody1Point3, + swappedBodiesCollisionData ? localBody1Point3 : localBody2Point3, + penetrationDepth3)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point4 : localBody1Point4, + swappedBodiesCollisionData ? localBody1Point4 : localBody2Point4, + penetrationDepth4)); + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody2, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mBoxProxyShape2)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mBoxProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mBoxBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point2 : localBody1Point2, + swappedBodiesCollisionData ? localBody1Point2 : localBody2Point2, + penetrationDepth2)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point3 : localBody1Point3, + swappedBodiesCollisionData ? localBody1Point3 : localBody2Point3, + penetrationDepth3)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point4 : localBody1Point4, + swappedBodiesCollisionData ? localBody1Point4 : localBody2Point4, + penetrationDepth4)); + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, mBoxBody2, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mBoxProxyShape2)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mBoxProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mBoxBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point2 : localBody1Point2, + swappedBodiesCollisionData ? localBody1Point2 : localBody2Point2, + penetrationDepth2)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point3 : localBody1Point3, + swappedBodiesCollisionData ? localBody1Point3 : localBody2Point3, + penetrationDepth3)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point4 : localBody1Point4, + swappedBodiesCollisionData ? localBody1Point4 : localBody2Point4, + penetrationDepth4)); + + // reset the init transforms + mBoxBody1->setTransform(initTransform1); + mBoxBody2->setTransform(initTransform2); + } + + void testBoxVsConvexMeshCollision() { + + Transform initTransform1 = mBoxBody1->getTransform(); + Transform initTransform2 = mConvexMeshBody2->getTransform(); + + /******************************************************************************** + * Test Box vs Convex Mesh collision * + *********************************************************************************/ + + Transform transform1(Vector3(11, 20, 50), Quaternion::identity()); + Transform transform2(Vector3(4.5, 16, 40), Quaternion::identity()); + + // Move spheres to collide with each other + mBoxBody1->setTransform(transform1); + mConvexMeshBody2->setTransform(transform2); + + // ----- Test AABB overlap ----- // + + test(mWorld->testAABBOverlap(mBoxBody1, mConvexMeshBody2)); + + mOverlapCallback.reset(); + mWorld->testOverlap(mBoxBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mConvexMeshBody2, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + // ----- Test global collision test ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mConvexMeshProxyShape2)); + + // Get collision data + const CollisionData* collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mConvexMeshProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + bool swappedBodiesCollisionData = collisionData->getBody1()->getID() != mBoxBody1->getID(); + + // Test contact points + Vector3 localBody1Point1(-3, -2, -2); + Vector3 localBody2Point1(4, 2, 8); + decimal penetrationDepth1 = 0.5f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + Vector3 localBody1Point2(-3, -2, -3); + Vector3 localBody2Point2(4, 2, 7); + decimal penetrationDepth2 = 0.5f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point2 : localBody1Point2, + swappedBodiesCollisionData ? localBody1Point2 : localBody2Point2, + penetrationDepth2)); + Vector3 localBody1Point3(-3, -3, -2); + Vector3 localBody2Point3(4, 1, 8); + decimal penetrationDepth3 = 0.5f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point3 : localBody1Point3, + swappedBodiesCollisionData ? localBody1Point3 : localBody2Point3, + penetrationDepth3)); + Vector3 localBody1Point4(-3, -3, -3); + Vector3 localBody2Point4(4, 1, 7); + decimal penetrationDepth4 = 0.5f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point4 : localBody1Point4, + swappedBodiesCollisionData ? localBody1Point4 : localBody2Point4, + penetrationDepth4)); + + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mConvexMeshProxyShape2)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mConvexMeshProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mBoxBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point2 : localBody1Point2, + swappedBodiesCollisionData ? localBody1Point2 : localBody2Point2, + penetrationDepth2)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point3 : localBody1Point3, + swappedBodiesCollisionData ? localBody1Point3 : localBody2Point3, + penetrationDepth3)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point4 : localBody1Point4, + swappedBodiesCollisionData ? localBody1Point4 : localBody2Point4, + penetrationDepth4)); + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mConvexMeshBody2, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mConvexMeshProxyShape2)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mConvexMeshProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mBoxBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point2 : localBody1Point2, + swappedBodiesCollisionData ? localBody1Point2 : localBody2Point2, + penetrationDepth2)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point3 : localBody1Point3, + swappedBodiesCollisionData ? localBody1Point3 : localBody2Point3, + penetrationDepth3)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point4 : localBody1Point4, + swappedBodiesCollisionData ? localBody1Point4 : localBody2Point4, + penetrationDepth4)); + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, mConvexMeshBody2, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mConvexMeshProxyShape2)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mConvexMeshProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mBoxBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point2 : localBody1Point2, + swappedBodiesCollisionData ? localBody1Point2 : localBody2Point2, + penetrationDepth2)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point3 : localBody1Point3, + swappedBodiesCollisionData ? localBody1Point3 : localBody2Point3, + penetrationDepth3)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point4 : localBody1Point4, + swappedBodiesCollisionData ? localBody1Point4 : localBody2Point4, + penetrationDepth4)); + + // reset the init transforms + mBoxBody1->setTransform(initTransform1); + mConvexMeshBody2->setTransform(initTransform2); + } + + void testConvexMeshVsConvexMeshCollision() { + + Transform initTransform1 = mConvexMeshBody1->getTransform(); + Transform initTransform2 = mConvexMeshBody2->getTransform(); + + /******************************************************************************** + * Test Convex Mesh vs Convex Mesh collision * + *********************************************************************************/ + + Transform transform1(Vector3(11, 20, 50), Quaternion::identity()); + Transform transform2(Vector3(4.5, 16, 40), Quaternion::identity()); + + // Move spheres to collide with each other + mConvexMeshBody1->setTransform(transform1); + mConvexMeshBody2->setTransform(transform2); + + // ----- Test AABB overlap ----- // + + test(mWorld->testAABBOverlap(mConvexMeshBody1, mConvexMeshBody2)); + + mOverlapCallback.reset(); + mWorld->testOverlap(mConvexMeshBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mConvexMeshBody2, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + // ----- Test global collision test ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mConvexMeshProxyShape2)); + + // Get collision data + const CollisionData* collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mConvexMeshProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + bool swappedBodiesCollisionData = collisionData->getBody1()->getID() != mConvexMeshBody1->getID(); + + // Test contact points + Vector3 localBody1Point1(-3, -2, -2); + Vector3 localBody2Point1(4, 2, 8); + decimal penetrationDepth1 = 0.5f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + Vector3 localBody1Point2(-3, -2, -3); + Vector3 localBody2Point2(4, 2, 7); + decimal penetrationDepth2 = 0.5f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point2 : localBody1Point2, + swappedBodiesCollisionData ? localBody1Point2 : localBody2Point2, + penetrationDepth2)); + Vector3 localBody1Point3(-3, -3, -2); + Vector3 localBody2Point3(4, 1, 8); + decimal penetrationDepth3 = 0.5f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point3 : localBody1Point3, + swappedBodiesCollisionData ? localBody1Point3 : localBody2Point3, + penetrationDepth3)); + Vector3 localBody1Point4(-3, -3, -3); + Vector3 localBody2Point4(4, 1, 7); + decimal penetrationDepth4 = 0.5f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point4 : localBody1Point4, + swappedBodiesCollisionData ? localBody1Point4 : localBody2Point4, + penetrationDepth4)); + + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mConvexMeshBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mConvexMeshProxyShape2)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mConvexMeshProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mConvexMeshBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point2 : localBody1Point2, + swappedBodiesCollisionData ? localBody1Point2 : localBody2Point2, + penetrationDepth2)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point3 : localBody1Point3, + swappedBodiesCollisionData ? localBody1Point3 : localBody2Point3, + penetrationDepth3)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point4 : localBody1Point4, + swappedBodiesCollisionData ? localBody1Point4 : localBody2Point4, + penetrationDepth4)); + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mConvexMeshBody2, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mConvexMeshProxyShape2)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mConvexMeshProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mConvexMeshBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point2 : localBody1Point2, + swappedBodiesCollisionData ? localBody1Point2 : localBody2Point2, + penetrationDepth2)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point3 : localBody1Point3, + swappedBodiesCollisionData ? localBody1Point3 : localBody2Point3, + penetrationDepth3)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point4 : localBody1Point4, + swappedBodiesCollisionData ? localBody1Point4 : localBody2Point4, + penetrationDepth4)); + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mConvexMeshBody1, mConvexMeshBody2, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mConvexMeshProxyShape2)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mConvexMeshProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mConvexMeshBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point2 : localBody1Point2, + swappedBodiesCollisionData ? localBody1Point2 : localBody2Point2, + penetrationDepth2)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point3 : localBody1Point3, + swappedBodiesCollisionData ? localBody1Point3 : localBody2Point3, + penetrationDepth3)); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point4 : localBody1Point4, + swappedBodiesCollisionData ? localBody1Point4 : localBody2Point4, + penetrationDepth4)); + + // reset the init transforms + mConvexMeshBody1->setTransform(initTransform1); + mConvexMeshBody2->setTransform(initTransform2); + } + + void testBoxVsCapsuleCollision() { + + Transform initTransform1 = mBoxBody1->getTransform(); + Transform initTransform2 = mCapsuleBody1->getTransform(); + + /******************************************************************************** + * Test Box vs Capsule collision * + *********************************************************************************/ + + Transform transform1(Vector3(10, 20, 50), Quaternion::identity()); + Transform transform2(Vector3(17, 21, 50), Quaternion::fromEulerAngles(0, 0, rp3d::PI * 0.5f)); + + // Move spheres to collide with each other + mBoxBody1->setTransform(transform1); + mCapsuleBody1->setTransform(transform2); + + // ----- Test AABB overlap ----- // + + test(mWorld->testAABBOverlap(mBoxBody1, mCapsuleBody1)); + + mOverlapCallback.reset(); + mWorld->testOverlap(mBoxBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mCapsuleBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + // ----- Test global collision test ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mCapsuleProxyShape1)); + + // Get collision data + const CollisionData* collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mCapsuleProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + bool swappedBodiesCollisionData = collisionData->getBody1()->getID() != mBoxBody1->getID(); + + // Test contact points + Vector3 localBody1Point1(3, 1, 0); + Vector3 localBody2Point1(0, 5, 0); + decimal penetrationDepth1 = 1.0f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mCapsuleProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mCapsuleProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mBoxBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mCapsuleBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mCapsuleProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mCapsuleProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mBoxBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, mCapsuleBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mCapsuleProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mCapsuleProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mBoxBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + + // reset the init transforms + mBoxBody1->setTransform(initTransform1); + mCapsuleBody1->setTransform(initTransform2); + } + + void testConvexMeshVsCapsuleCollision() { + + Transform initTransform1 = mConvexMeshBody1->getTransform(); + Transform initTransform2 = mCapsuleBody1->getTransform(); + + /******************************************************************************** + * Test Convex Mesh vs Capsule collision * + *********************************************************************************/ + + Transform transform1(Vector3(10, 20, 50), Quaternion::identity()); + Transform transform2(Vector3(17, 21, 50), Quaternion::fromEulerAngles(0, 0, rp3d::PI * 0.5f)); + + // Move spheres to collide with each other + mConvexMeshBody1->setTransform(transform1); + mCapsuleBody1->setTransform(transform2); + + // ----- Test AABB overlap ----- // + + test(mWorld->testAABBOverlap(mConvexMeshBody1, mCapsuleBody1)); + + mOverlapCallback.reset(); + mWorld->testOverlap(mConvexMeshBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mCapsuleBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + // ----- Test global collision test ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mCapsuleProxyShape1)); + + // Get collision data + const CollisionData* collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mCapsuleProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + bool swappedBodiesCollisionData = collisionData->getBody1()->getID() != mConvexMeshBody1->getID(); + + // Test contact points + Vector3 localBody1Point1(3, 1, 0); + Vector3 localBody2Point1(0, 5, 0); + decimal penetrationDepth1 = 1.0f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mConvexMeshBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mCapsuleProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mCapsuleProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mConvexMeshBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mCapsuleBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mCapsuleProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mCapsuleProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mConvexMeshBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mConvexMeshBody1, mCapsuleBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mCapsuleProxyShape1)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mCapsuleProxyShape1); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mConvexMeshBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1, + swappedBodiesCollisionData ? localBody1Point1 : localBody2Point1, + penetrationDepth1)); + + // reset the init transforms + mConvexMeshBody1->setTransform(initTransform1); + mCapsuleBody1->setTransform(initTransform2); + } + + void testBoxVsConcaveMeshCollision() { + + Transform initTransform1 = mBoxBody1->getTransform(); + Transform initTransform2 = mConcaveMeshBody->getTransform(); + + /******************************************************************************** + * Test Box vs Concave Mesh + *********************************************************************************/ + + Transform transform1(Vector3(10, 22, 50), Quaternion::identity()); + Transform transform2(Vector3(10, 20, 50), Quaternion::identity()); + + // Move spheres to collide with each other + mBoxBody1->setTransform(transform1); + mConcaveMeshBody->setTransform(transform2); + + // ----- Test AABB overlap ----- // + + test(mWorld->testAABBOverlap(mBoxBody1, mConcaveMeshBody)); + + mOverlapCallback.reset(); + mWorld->testOverlap(mBoxBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mConcaveMeshBody, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + // ----- Test global collision test ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mConcaveMeshProxyShape)); + + // Get collision data + const CollisionData* collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mConcaveMeshProxyShape); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + bool swappedBodiesCollisionData = collisionData->getBody1()->getID() != mBoxBody1->getID(); + + for (int i=0; icontactManifolds[0].contactPoints.size(); i++) { + test(approxEqual(collisionData->contactManifolds[0].contactPoints[i].penetrationDepth, 1.0f)); + } + + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mConcaveMeshProxyShape)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mConcaveMeshProxyShape); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mBoxBody1->getID(); + + for (int i=0; icontactManifolds[0].contactPoints.size(); i++) { + test(approxEqual(collisionData->contactManifolds[0].contactPoints[i].penetrationDepth, 1.0f)); + } + + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mConcaveMeshBody, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mConcaveMeshProxyShape)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mConcaveMeshProxyShape); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mBoxBody1->getID(); + + for (int i=0; icontactManifolds[0].contactPoints.size(); i++) { + test(approxEqual(collisionData->contactManifolds[0].contactPoints[i].penetrationDepth, 1.0f)); + } + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, mConcaveMeshBody, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mConcaveMeshProxyShape)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mConcaveMeshProxyShape); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mBoxBody1->getID(); + + // Test contact points + for (int i=0; icontactManifolds[0].contactPoints.size(); i++) { + test(approxEqual(collisionData->contactManifolds[0].contactPoints[i].penetrationDepth, 1.0f)); + } + + // Reset the init transforms + mBoxBody1->setTransform(initTransform1); + mConcaveMeshBody->setTransform(initTransform2); + } + + void testConvexMeshVsConcaveMeshCollision() { + + Transform initTransform1 = mConvexMeshBody1->getTransform(); + Transform initTransform2 = mConcaveMeshBody->getTransform(); + + /******************************************************************************** + * Test Box vs Concave Mesh + *********************************************************************************/ + + Transform transform1(Vector3(10, 22, 50), Quaternion::identity()); + Transform transform2(Vector3(10, 20, 50), Quaternion::identity()); + + // Move spheres to collide with each other + mConvexMeshBody1->setTransform(transform1); + mConcaveMeshBody->setTransform(transform2); + + // ----- Test AABB overlap ----- // + + test(mWorld->testAABBOverlap(mConvexMeshBody1, mConcaveMeshBody)); + + mOverlapCallback.reset(); + mWorld->testOverlap(mConvexMeshBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mConcaveMeshBody, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + // ----- Test global collision test ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mConcaveMeshProxyShape)); + + // Get collision data + const CollisionData* collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mConcaveMeshProxyShape); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + bool swappedBodiesCollisionData = collisionData->getBody1()->getID() != mConvexMeshBody1->getID(); + + for (int i=0; icontactManifolds[0].contactPoints.size(); i++) { + test(approxEqual(collisionData->contactManifolds[0].contactPoints[i].penetrationDepth, 1.0f)); + } + + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mConvexMeshBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mConcaveMeshProxyShape)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mConcaveMeshProxyShape); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mConvexMeshBody1->getID(); + + for (int i=0; icontactManifolds[0].contactPoints.size(); i++) { + test(approxEqual(collisionData->contactManifolds[0].contactPoints[i].penetrationDepth, 1.0f)); + } + + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mConcaveMeshBody, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mConcaveMeshProxyShape)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mConcaveMeshProxyShape); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mConvexMeshBody1->getID(); + + for (int i=0; icontactManifolds[0].contactPoints.size(); i++) { + test(approxEqual(collisionData->contactManifolds[0].contactPoints[i].penetrationDepth, 1.0f)); + } + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mConvexMeshBody1, mConcaveMeshBody, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mConcaveMeshProxyShape)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mConcaveMeshProxyShape); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 4); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mConvexMeshBody1->getID(); + + // Test contact points + for (int i=0; icontactManifolds[0].contactPoints.size(); i++) { + test(approxEqual(collisionData->contactManifolds[0].contactPoints[i].penetrationDepth, 1.0f)); + } + + // Reset the init transforms + mConvexMeshBody1->setTransform(initTransform1); + mConcaveMeshBody->setTransform(initTransform2); + } + + void testCapsuleVsCapsuleCollision() { + + Transform initTransform1 = mCapsuleBody1->getTransform(); + Transform initTransform2 = mCapsuleBody2->getTransform(); + + /******************************************************************************** + * Test Capsule (sphere cap) vs Capsule (sphere cap) collision * + *********************************************************************************/ + + Transform transform1(Vector3(10, 20, 50), Quaternion::identity()); + Transform transform2(Vector3(16, 23, 50), Quaternion::fromEulerAngles(0, 0, rp3d::PI * 0.5f)); + + // Move spheres to collide with each other + mCapsuleBody1->setTransform(transform1); + mCapsuleBody2->setTransform(transform2); + + // ----- Test AABB overlap ----- // + + test(mWorld->testAABBOverlap(mCapsuleBody1, mCapsuleBody2)); + + mOverlapCallback.reset(); + mWorld->testOverlap(mCapsuleBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mCapsuleBody2, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + // ----- Test global collision test ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mCapsuleProxyShape2)); + + // Get collision data + const CollisionData* collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mCapsuleProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + bool swappedBodiesCollisionData = collisionData->getBody1()->getID() != mCapsuleBody1->getID(); + + // Test contact points + Vector3 localBody1Point(2, 3, 0); + Vector3 localBody2Point(0, 5, 0); + decimal penetrationDepth = 1.0f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mCapsuleBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mCapsuleProxyShape2)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mCapsuleProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mCapsuleBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mCapsuleBody2, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mCapsuleProxyShape2)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mCapsuleProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mCapsuleBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mCapsuleBody1, mCapsuleBody2, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mCapsuleProxyShape2)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mCapsuleProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mCapsuleBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + /******************************************************************************** + * Test Capsule (sphere cap) vs Capsule (cylinder side) collision * + *********************************************************************************/ + + transform1 = Transform(Vector3(10, 20, 50), Quaternion::identity()); + transform2 = Transform(Vector3(10, 27, 50), Quaternion::fromEulerAngles(0, 0, rp3d::PI * 0.5f)); + + // Move spheres to collide with each other + mCapsuleBody1->setTransform(transform1); + mCapsuleBody2->setTransform(transform2); + + // ----- Test AABB overlap ----- // + + test(mWorld->testAABBOverlap(mCapsuleBody1, mCapsuleBody2)); + + mOverlapCallback.reset(); + mWorld->testOverlap(mCapsuleBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mCapsuleBody2, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + // ----- Test global collision test ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mCapsuleProxyShape2)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mCapsuleProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mCapsuleBody1->getID(); + + // Test contact points + localBody1Point = Vector3(0, 5, 0); + localBody2Point = Vector3(-3, 0, 0); + penetrationDepth = decimal(1.0); + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mCapsuleBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mCapsuleProxyShape2)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mCapsuleProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mCapsuleBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mCapsuleBody2, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mCapsuleProxyShape2)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mCapsuleProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mCapsuleBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mCapsuleBody1, mCapsuleBody2, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mCapsuleProxyShape2)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mCapsuleProxyShape2); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mCapsuleBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // Reset the init transforms + mCapsuleBody1->setTransform(initTransform1); + mCapsuleBody2->setTransform(initTransform2); + } + + void testCapsuleVsConcaveMeshCollision() { + + Transform initTransform1 = mCapsuleBody1->getTransform(); + Transform initTransform2 = mConcaveMeshBody->getTransform(); + + /******************************************************************************** + * Test Capsule vs Concave Mesh + *********************************************************************************/ + + Transform transform1(Vector3(10, 24.98f, 50), Quaternion::identity()); + Transform transform2(Vector3(10, 20, 50), Quaternion::identity()); + + // Move spheres to collide with each other + mCapsuleBody1->setTransform(transform1); + mConcaveMeshBody->setTransform(transform2); + + // ----- Test AABB overlap ----- // + + test(mWorld->testAABBOverlap(mCapsuleBody1, mConcaveMeshBody)); + + mOverlapCallback.reset(); + mWorld->testOverlap(mCapsuleBody1, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mConcaveMeshBody, &mOverlapCallback); + test(mOverlapCallback.hasOverlap()); + + // ----- Test global collision test ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mConcaveMeshProxyShape)); + + // Get collision data + const CollisionData* collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mConcaveMeshProxyShape); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + bool swappedBodiesCollisionData = collisionData->getBody1()->getID() != mCapsuleBody1->getID(); + + // Test contact points + Vector3 localBody1Point(0, -5, 0); + Vector3 localBody2Point(0, 0, 0); + decimal penetrationDepth = 0.02f; + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 1 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mCapsuleBody1, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mConcaveMeshProxyShape)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mConcaveMeshProxyShape); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mCapsuleBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against body 2 only ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mConcaveMeshBody, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mConcaveMeshProxyShape)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mConcaveMeshProxyShape); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mCapsuleBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // ----- Test collision against selected body 1 and 2 ----- // + + mCollisionCallback.reset(); + mWorld->testCollision(mCapsuleBody1, mConcaveMeshBody, &mCollisionCallback); + + test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mConcaveMeshProxyShape)); + + // Get collision data + collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mConcaveMeshProxyShape); + test(collisionData != nullptr); + test(collisionData->getNbContactManifolds() == 1); + test(collisionData->getTotalNbContactPoints() == 1); + + // True if the bodies are swapped in the collision callback response + swappedBodiesCollisionData = collisionData->getBody1()->getID() != mCapsuleBody1->getID(); + + // Test contact points + test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point, + swappedBodiesCollisionData ? localBody1Point : localBody2Point, + penetrationDepth)); + + // Reset the init transforms + mCapsuleBody1->setTransform(initTransform1); + mConcaveMeshBody->setTransform(initTransform2); + } }; } diff --git a/test/tests/collision/TestDynamicAABBTree.h b/test/tests/collision/TestDynamicAABBTree.h index 53f5e618..a117df57 100755 --- a/test/tests/collision/TestDynamicAABBTree.h +++ b/test/tests/collision/TestDynamicAABBTree.h @@ -29,7 +29,7 @@ // Libraries #include "Test.h" #include "collision/broadphase/DynamicAABBTree.h" -#include +#include "memory/MemoryManager.h" /// Reactphysics3D namespace namespace reactphysics3d { @@ -113,7 +113,7 @@ class TestDynamicAABBTree : public Test { // ------------ Create tree ---------- // // Dynamic AABB Tree - DynamicAABBTree tree; + DynamicAABBTree tree(MemoryManager::getBaseAllocator()); #ifdef IS_PROFILING_ACTIVE /// Pointer to the profiler @@ -169,7 +169,7 @@ class TestDynamicAABBTree : public Test { // ------------- Create tree ----------- // // Dynamic AABB Tree - DynamicAABBTree tree; + DynamicAABBTree tree(MemoryManager::getBaseAllocator()); #ifdef IS_PROFILING_ACTIVE /// Pointer to the profiler @@ -368,7 +368,7 @@ class TestDynamicAABBTree : public Test { // ------------- Create tree ----------- // // Dynamic AABB Tree - DynamicAABBTree tree; + DynamicAABBTree tree(MemoryManager::getBaseAllocator()); #ifdef IS_PROFILING_ACTIVE /// Pointer to the profiler diff --git a/test/tests/collision/TestPointInside.h b/test/tests/collision/TestPointInside.h index afdb415c..540f9f03 100644 --- a/test/tests/collision/TestPointInside.h +++ b/test/tests/collision/TestPointInside.h @@ -59,12 +59,17 @@ class TestPointInside : public Test { CollisionBody* mCylinderBody; CollisionBody* mCompoundBody; + Vector3 mConvexMeshCubeVertices[8]; + int mConvexMeshCubeIndices[24]; + PolygonVertexArray* mConvexMeshPolygonVertexArray; + PolyhedronMesh* mConvexMeshPolyhedronMesh; + PolygonVertexArray::PolygonFace* mConvexMeshPolygonFaces; + // Collision shapes BoxShape* mBoxShape; SphereShape* mSphereShape; CapsuleShape* mCapsuleShape; ConvexMeshShape* mConvexMeshShape; - ConvexMeshShape* mConvexMeshShapeBodyEdgesInfo; // Transform Transform mBodyTransform; @@ -76,10 +81,7 @@ class TestPointInside : public Test { ProxyShape* mBoxProxyShape; ProxyShape* mSphereProxyShape; ProxyShape* mCapsuleProxyShape; - ProxyShape* mConeProxyShape; ProxyShape* mConvexMeshProxyShape; - ProxyShape* mConvexMeshProxyShapeEdgesInfo; - ProxyShape* mCylinderProxyShape; public : @@ -104,6 +106,7 @@ class TestPointInside : public Test { mConvexMeshBody = mWorld->createCollisionBody(mBodyTransform); mConvexMeshBodyEdgesInfo = mWorld->createCollisionBody(mBodyTransform); mCylinderBody = mWorld->createCollisionBody(mBodyTransform); + mConvexMeshBody = mWorld->createCollisionBody(mBodyTransform); mCompoundBody = mWorld->createCollisionBody(mBodyTransform); // Collision shape transform @@ -121,51 +124,44 @@ class TestPointInside : public Test { mSphereShape = new SphereShape(3); mSphereProxyShape = mSphereBody->addCollisionShape(mSphereShape, mShapeTransform); - mCapsuleShape = new CapsuleShape(2, 10); + mCapsuleShape = new CapsuleShape(3, 10); mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform); - // TODO : Create convex mesh shape with new way (polyhedron mesh) to add test again - /*mConvexMeshShape = new ConvexMeshShape(0.0); // Box of dimension (2, 3, 4) - mConvexMeshShape->addVertex(Vector3(-2, -3, -4)); - mConvexMeshShape->addVertex(Vector3(2, -3, -4)); - mConvexMeshShape->addVertex(Vector3(2, -3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, -3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, 3, -4)); - mConvexMeshShape->addVertex(Vector3(2, 3, -4)); - mConvexMeshShape->addVertex(Vector3(2, 3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, 3, 4)); - mConvexMeshProxyShape = mConvexMeshBody->addCollisionShape(mConvexMeshShape, mShapeTransform); + mConvexMeshCubeVertices[0] = Vector3(-2, -3, 4); + mConvexMeshCubeVertices[1] = Vector3(2, -3, 4); + mConvexMeshCubeVertices[2] = Vector3(2, -3, -4); + mConvexMeshCubeVertices[3] = Vector3(-2, -3, -4); + mConvexMeshCubeVertices[4] = Vector3(-2, 3, 4); + mConvexMeshCubeVertices[5] = Vector3(2, 3, 4); + mConvexMeshCubeVertices[6] = Vector3(2, 3, -4); + mConvexMeshCubeVertices[7] = Vector3(-2, 3, -4); - mConvexMeshShapeBodyEdgesInfo = new ConvexMeshShape(0.0); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, -3, -4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, -3, -4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, -3, 4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, -3, 4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, 3, -4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, 3, -4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, 3, 4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, 3, 4)); - mConvexMeshShapeBodyEdgesInfo->addEdge(0, 1); - mConvexMeshShapeBodyEdgesInfo->addEdge(1, 2); - mConvexMeshShapeBodyEdgesInfo->addEdge(2, 3); - mConvexMeshShapeBodyEdgesInfo->addEdge(0, 3); - mConvexMeshShapeBodyEdgesInfo->addEdge(4, 5); - mConvexMeshShapeBodyEdgesInfo->addEdge(5, 6); - mConvexMeshShapeBodyEdgesInfo->addEdge(6, 7); - mConvexMeshShapeBodyEdgesInfo->addEdge(4, 7); - mConvexMeshShapeBodyEdgesInfo->addEdge(0, 4); - mConvexMeshShapeBodyEdgesInfo->addEdge(1, 5); - mConvexMeshShapeBodyEdgesInfo->addEdge(2, 6); - mConvexMeshShapeBodyEdgesInfo->addEdge(3, 7); - mConvexMeshShapeBodyEdgesInfo->setIsEdgesInformationUsed(true); - mConvexMeshProxyShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape( - mConvexMeshShapeBodyEdgesInfo, - mShapeTransform); - */ + mConvexMeshCubeIndices[0] = 0; mConvexMeshCubeIndices[1] = 3; mConvexMeshCubeIndices[2] = 2; mConvexMeshCubeIndices[3] = 1; + mConvexMeshCubeIndices[4] = 4; mConvexMeshCubeIndices[5] = 5; mConvexMeshCubeIndices[6] = 6; mConvexMeshCubeIndices[7] = 7; + mConvexMeshCubeIndices[8] = 0; mConvexMeshCubeIndices[9] = 1; mConvexMeshCubeIndices[10] = 5; mConvexMeshCubeIndices[11] = 4; + mConvexMeshCubeIndices[12] = 1; mConvexMeshCubeIndices[13] = 2; mConvexMeshCubeIndices[14] = 6; mConvexMeshCubeIndices[15] = 5; + mConvexMeshCubeIndices[16] = 2; mConvexMeshCubeIndices[17] = 3; mConvexMeshCubeIndices[18] = 7; mConvexMeshCubeIndices[19] = 6; + mConvexMeshCubeIndices[20] = 0; mConvexMeshCubeIndices[21] = 4; mConvexMeshCubeIndices[22] = 7; mConvexMeshCubeIndices[23] = 3; + + mConvexMeshPolygonFaces = new PolygonVertexArray::PolygonFace[6]; + PolygonVertexArray::PolygonFace* face = mConvexMeshPolygonFaces; + for (int f = 0; f < 6; f++) { + face->indexBase = f * 4; + face->nbVertices = 4; + face++; + } + mConvexMeshPolygonVertexArray = new PolygonVertexArray(8, &(mConvexMeshCubeVertices[0]), sizeof(Vector3), + &(mConvexMeshCubeIndices[0]), sizeof(int), 6, mConvexMeshPolygonFaces, + PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, + PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + mConvexMeshPolyhedronMesh = new PolyhedronMesh(mConvexMeshPolygonVertexArray); + mConvexMeshShape = new ConvexMeshShape(mConvexMeshPolyhedronMesh); + Transform convexMeshTransform(Vector3(10, 0, 0), Quaternion::identity()); + mConvexMeshProxyShape = mConvexMeshBody->addCollisionShape(mConvexMeshShape, mShapeTransform); // Compound shape is a capsule and a sphere Vector3 positionShape2(Vector3(4, 2, -3)); - Quaternion orientationShape2 = Quaternion::fromEulerAngles(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); + Quaternion orientationShape2 = Quaternion::fromEulerAngles(-3 * PI / 8, 1.5 * PI/ 3, PI / 13); Transform shapeTransform2(positionShape2, orientationShape2); mLocalShape2ToWorld = mBodyTransform * shapeTransform2; mCompoundBody->addCollisionShape(mCapsuleShape, mShapeTransform); @@ -177,8 +173,10 @@ class TestPointInside : public Test { delete mBoxShape; delete mSphereShape; delete mCapsuleShape; - //delete mConvexMeshShape; - //delete mConvexMeshShapeBodyEdgesInfo; + delete mConvexMeshShape; + delete mConvexMeshPolygonFaces; + delete mConvexMeshPolygonVertexArray; + delete mConvexMeshPolyhedronMesh; } /// Run the tests @@ -328,24 +326,24 @@ class TestPointInside : public Test { test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, 0.4))); test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.3, 1, 1.5))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -7.1, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 7.1, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 5, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 5, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, 1.6))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, -1.7))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, -5, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -5, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, 1.6))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, -1.7))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -13.1, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 13.1, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 3.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -3.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 5, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 5, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.5, 5, 2.6))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.5, 5, -2.7))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 3.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -3.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, -5, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -5, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.5, -5, 2.6))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.5, -5, -2.7))); // Tests with ProxyCapsuleShape test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); @@ -375,33 +373,30 @@ class TestPointInside : public Test { test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, 0.4))); test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.3, 1, 1.5))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -7.1, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 7.1, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 5, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 5, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, 1.6))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, -1.7))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, -5, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -5, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, 1.6))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, -1.7))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -13.1, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 13.1, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 3.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -3.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 5, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 5, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.5, 5, 2.6))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.5, 5, -2.7))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 3.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -3.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, -5, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -5, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.5, -5, 2.6))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.5, -5, -2.7))); } /// Test the ProxyConvexMeshShape::testPointInside() and /// CollisionBody::testPointInside() methods void testConvexMesh() { - // ----- Tests without using edges information ----- // - - /* // Tests with CollisionBody test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); @@ -453,68 +448,12 @@ class TestPointInside : public Test { test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); - - // ----- Tests using edges information ----- // - - // Tests with CollisionBody - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); - - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); - - // Tests with ProxyConvexMeshShape - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); - - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); - */ } /// Test the CollisionBody::testPointInside() method void testCompound() { // Points on the capsule - // TODO : Previous it was a cylinder (not a capsule). Maybe those tests are wrong now test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0))); test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0))); diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index d02ab847..60d3068b 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -211,29 +211,28 @@ class TestRaycast : public Test { mCapsuleShape = new CapsuleShape(2, 5); mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform); - // TODO : Create convex mesh shape with new way (polyhedron mesh) to add test again - // Box of extents (2, 3, 4) - mPolyhedronVertices[0] = Vector3(-2, -3, -4); - mPolyhedronVertices[1] = Vector3(2, -3, -4); - mPolyhedronVertices[2] = Vector3(2, -3, 4); - mPolyhedronVertices[3] = Vector3(-2, -3, 4); - mPolyhedronVertices[4] = Vector3(-2, 3, -4); - mPolyhedronVertices[5] = Vector3(2, 3, -4); - mPolyhedronVertices[6] = Vector3(2, 3, 4); - mPolyhedronVertices[7] = Vector3(-2, 3, 4); + mPolyhedronVertices[0] = Vector3(-2, -3, 4); + mPolyhedronVertices[1] = Vector3(2, -3, 4); + mPolyhedronVertices[2] = Vector3(2, -3, -4); + mPolyhedronVertices[3] = Vector3(-2, -3, -4); + mPolyhedronVertices[4] = Vector3(-2, 3, 4); + mPolyhedronVertices[5] = Vector3(2, 3, 4); + mPolyhedronVertices[6] = Vector3(2, 3, -4); + mPolyhedronVertices[7] = Vector3(-2, 3, -4); - mPolyhedronIndices[0] = 0; mPolyhedronIndices[1] = 1; mPolyhedronIndices[2] = 2; mPolyhedronIndices[3] = 3; - mPolyhedronIndices[4] = 1; mPolyhedronIndices[5] = 5; mPolyhedronIndices[6] = 6; mPolyhedronIndices[7] = 2; - mPolyhedronIndices[8] = 0; mPolyhedronIndices[9] = 4; mPolyhedronIndices[10] = 5; mPolyhedronIndices[11] = 1; - mPolyhedronIndices[12] = 0; mPolyhedronIndices[13] = 3; mPolyhedronIndices[14] = 7; mPolyhedronIndices[15] = 4; - mPolyhedronIndices[16] = 3; mPolyhedronIndices[17] = 2; mPolyhedronIndices[18] = 6; mPolyhedronIndices[19] = 7; - mPolyhedronIndices[20] = 2; mPolyhedronIndices[21] = 5; mPolyhedronIndices[22] = 4; mPolyhedronIndices[23] = 7; + mPolyhedronIndices[0] = 0; mPolyhedronIndices[1] = 3; mPolyhedronIndices[2] = 2; mPolyhedronIndices[3] = 1; + mPolyhedronIndices[4] = 4; mPolyhedronIndices[5] = 5; mPolyhedronIndices[6] = 6; mPolyhedronIndices[7] = 7; + mPolyhedronIndices[8] = 0; mPolyhedronIndices[9] = 1; mPolyhedronIndices[10] = 5; mPolyhedronIndices[11] = 4; + mPolyhedronIndices[12] = 1; mPolyhedronIndices[13] = 2; mPolyhedronIndices[14] = 6; mPolyhedronIndices[15] = 5; + mPolyhedronIndices[16] = 2; mPolyhedronIndices[17] = 3; mPolyhedronIndices[18] = 7; mPolyhedronIndices[19] = 6; + mPolyhedronIndices[20] = 0; mPolyhedronIndices[21] = 4; mPolyhedronIndices[22] = 7; mPolyhedronIndices[23] = 3; // Polygon faces descriptions for the polyhedron - for (int f=0; f < 8; f++) { - PolygonVertexArray::PolygonFace& face = mPolygonFaces[f]; - face.indexBase = f * 4; - face.nbVertices = 4; + PolygonVertexArray::PolygonFace* face = mPolygonFaces; + for (int f = 0; f < 6; f++) { + face->indexBase = f * 4; + face->nbVertices = 4; + face++; } // Create the polygon vertex array @@ -1304,12 +1303,14 @@ class TestRaycast : public Test { Vector3 point2 = mLocalShapeToWorld * Vector3(1, 2, -4); Ray ray(point1, point2); Vector3 hitPoint = mLocalShapeToWorld * Vector3(1, 2, 4); + Transform inverse = mLocalShapeToWorld.getInverse(); mCallback.shapeToTest = mConvexMeshProxyShape; // CollisionWorld::raycast() mCallback.reset(); mWorld->raycast(ray, &mCallback); + Vector3 localTest = inverse * mCallback.raycastInfo.worldPoint; test(mCallback.isHit); test(mCallback.raycastInfo.body == mConvexMeshBody); test(mCallback.raycastInfo.proxyShape == mConvexMeshProxyShape); diff --git a/test/tests/containers/TestList.h b/test/tests/containers/TestList.h index ba1fbf61..750d0788 100644 --- a/test/tests/containers/TestList.h +++ b/test/tests/containers/TestList.h @@ -62,9 +62,10 @@ class TestList : public Test { testAddRemoveClear(); testAssignment(); testIndexing(); + testFind(); testEquality(); testReserve(); - testIteration(); + testIterators(); } void testConstructors() { @@ -142,27 +143,52 @@ class TestList : public Test { list3.add(3); list3.add(4); - list3.remove(3); + auto it = list3.removeAt(3); test(list3.size() == 3); test(list3.capacity() == 4); + test(it == list3.end()); test(list3[0] = 1); test(list3[1] = 2); test(list3[2] = 3); - list3.remove(1); + it = list3.removeAt(1); test(list3.size() == 2); test(list3.capacity() == 4); test(list3[0] = 1); test(list3[1] = 3); + test(*it = 3); - list3.remove(0); + list3.removeAt(0); test(list3.size() == 1); test(list3.capacity() == 4); test(list3[0] = 3); - list3.remove(0); + it = list3.removeAt(0); test(list3.size() == 0); test(list3.capacity() == 4); + test(it == list3.end()); + + list3.add(1); + list3.add(2); + list3.add(3); + it = list3.begin(); + list3.remove(it); + test(list3.size() == 2); + test(list3[0] == 2); + test(list3[1] == 3); + it = list3.find(3); + list3.remove(it); + test(list3.size() == 1); + test(list3[0] == 2); + + list3.add(5); + list3.add(6); + list3.add(7); + it = list3.remove(7); + test(it == list3.end()); + test(list3.size() == 3); + it = list3.remove(5); + test((*it) == 6); // ----- Test addRange() ----- // @@ -270,6 +296,20 @@ class TestList : public Test { test(list1[1] == 8); } + void testFind() { + + List list1(mAllocator); + list1.add(1); + list1.add(2); + list1.add(3); + list1.add(4); + list1.add(5); + + test(list1.find(1) == list1.begin()); + test(*(list1.find(2)) == 2); + test(*(list1.find(5)) == 5); + } + void testEquality() { List list1(mAllocator); @@ -320,8 +360,45 @@ class TestList : public Test { test(list1[1] == 2); } - void testIteration() { - // TODO : Implement this + void testIterators() { + + List list1(mAllocator); + + test(list1.begin() == list1.end()); + + list1.add(5); + list1.add(6); + list1.add(8); + list1.add(-1); + + List::Iterator itBegin = list1.begin(); + List::Iterator itEnd = list1.end(); + List::Iterator it = list1.begin(); + + test(itBegin == it); + test(*it == 5); + test(*(it++) == 5); + test(*it == 6); + test(*(it--) == 6); + test(*it == 5); + test(*(++it) == 6); + test(*it == 6); + test(*(--it) == 5); + test(*it == 5); + test(it == itBegin); + + it = list1.end(); + test(it == itEnd); + it--; + test(*it == -1); + it++; + test(it == itEnd); + + List list2(mAllocator); + for (auto it = list1.begin(); it != list1.end(); ++it) { + list2.add(*it); + } + test(list1 == list2); } }; diff --git a/test/tests/containers/TestMap.h b/test/tests/containers/TestMap.h index d7e084dc..d75de2aa 100644 --- a/test/tests/containers/TestMap.h +++ b/test/tests/containers/TestMap.h @@ -31,6 +31,30 @@ #include "containers/Map.h" #include "memory/DefaultAllocator.h" +// Key to test map with always same hash values +namespace reactphysics3d { + struct TestKey { + int key; + + TestKey(int k) :key(k) {} + + bool operator==(const TestKey& testKey) const { + return key == testKey.key; + } + }; +} + +// Hash function for struct VerticesPair +namespace std { + + template <> struct hash { + + size_t operator()(const reactphysics3d::TestKey& key) const { + return 1; + } + }; +} + /// Reactphysics3D namespace namespace reactphysics3d { @@ -62,10 +86,11 @@ class TestMap : public Test { testReserve(); testAddRemoveClear(); testContainsKey(); + testFind(); testIndexing(); testEquality(); testAssignment(); - testIteration(); + testIterators(); } void testConstructors() { @@ -81,15 +106,14 @@ class TestMap : public Test { test(map2.size() == 0); // ----- Copy Constructors ----- // -/* Map map3(map1); test(map3.capacity() == map1.capacity()); test(map3.size() == map1.size()); Map map4(mAllocator); - map4.add(std::make_pair(1, 10)); - map4.add(std::make_pair(2, 20)); - map4.add(std::make_pair(3, 30)); + map4.add(Pair(1, 10)); + map4.add(Pair(2, 20)); + map4.add(Pair(3, 30)); test(map4.capacity() >= 3); test(map4.size() == 3); @@ -99,7 +123,6 @@ class TestMap : public Test { test(map5[1] == 10); test(map5[2] == 20); test(map5[3] == 30); - */ } void testReserve() { @@ -107,8 +130,8 @@ class TestMap : public Test { Map map1(mAllocator); map1.reserve(15); test(map1.capacity() >= 15); - map1.add(std::make_pair(1, "test1")); - map1.add(std::make_pair(2, "test2")); + map1.add(Pair(1, "test1")); + map1.add(Pair(2, "test2")); test(map1.capacity() >= 15); map1.reserve(10); @@ -125,9 +148,9 @@ class TestMap : public Test { // ----- Test add() ----- // Map map1(mAllocator); - map1.add(std::make_pair(1, 10)); - map1.add(std::make_pair(8, 80)); - map1.add(std::make_pair(13, 130)); + map1.add(Pair(1, 10)); + map1.add(Pair(8, 80)); + map1.add(Pair(13, 130)); test(map1[1] == 10); test(map1[8] == 80); test(map1[13] == 130); @@ -135,7 +158,7 @@ class TestMap : public Test { Map map2(mAllocator, 15); for (int i = 0; i < 1000000; i++) { - map2.add(std::make_pair(i, i * 100)); + map2.add(Pair(i, i * 100)); } bool isValid = true; for (int i = 0; i < 1000000; i++) { @@ -144,26 +167,38 @@ class TestMap : public Test { test(isValid); map1.remove(1); - map1.add(std::make_pair(1, 10)); + map1.add(Pair(1, 10)); test(map1.size() == 3); test(map1[1] == 10); + map1.add(Pair(56, 34)); + test(map1[56] == 34); + test(map1.size() == 4); + map1.add(Pair(56, 13), true); + test(map1[56] == 13); + test(map1.size() == 4); + // ----- Test remove() ----- // map1.remove(1); test(!map1.containsKey(1)); test(map1.containsKey(8)); test(map1.containsKey(13)); - test(map1.size() == 2); + test(map1.size() == 3); map1.remove(13); - test(!map1.containsKey(8)); - test(map1.containsKey(13)); - test(map1.size() == 1); + test(map1.containsKey(8)); + test(!map1.containsKey(13)); + test(map1.size() == 2); map1.remove(8); test(!map1.containsKey(8)); + test(map1.size() == 1); + + auto it = map1.remove(56); + test(!map1.containsKey(56)); test(map1.size() == 0); + test(it == map1.end()); isValid = true; for (int i = 0; i < 1000000; i++) { @@ -177,19 +212,36 @@ class TestMap : public Test { Map map3(mAllocator); for (int i=0; i < 1000000; i++) { - map3.add(std::make_pair(i, i * 10)); + map3.add(Pair(i, i * 10)); map3.remove(i); } + map3.add(Pair(1, 10)); + map3.add(Pair(2, 20)); + map3.add(Pair(3, 30)); + test(map3.size() == 3); + it = map3.begin(); + map3.remove(it++); + test(!map3.containsKey(1)); + test(map3.size() == 2); + test(it->second == 20); + + map3.add(Pair(56, 32)); + map3.add(Pair(23, 89)); + for (it = map3.begin(); it != map3.end();) { + it = map3.remove(it); + } + test(map3.size() == 0); + // ----- Test clear() ----- // Map map4(mAllocator); - map4.add(std::make_pair(2, 20)); - map4.add(std::make_pair(4, 40)); - map4.add(std::make_pair(6, 60)); + map4.add(Pair(2, 20)); + map4.add(Pair(4, 40)); + map4.add(Pair(6, 60)); map4.clear(); test(map4.size() == 0); - map4.add(std::make_pair(2, 20)); + map4.add(Pair(2, 20)); test(map4.size() == 1); test(map4[2] == 20); map4.clear(); @@ -198,6 +250,24 @@ class TestMap : public Test { Map map5(mAllocator); map5.clear(); test(map5.size() == 0); + + // ----- Test map with always same hash value for keys ----- // + + Map map6(mAllocator); + for (int i=0; i < 1000; i++) { + map6.add(Pair(TestKey(i), i)); + } + bool isTestValid = true; + for (int i=0; i < 1000; i++) { + if (map6[TestKey(i)] != i) { + isTestValid = false; + } + } + test(isTestValid); + for (int i=0; i < 1000; i++) { + map6.remove(TestKey(i)); + } + test(map6.size() == 0); } void testContainsKey() { @@ -208,9 +278,9 @@ class TestMap : public Test { test(!map1.containsKey(4)); test(!map1.containsKey(6)); - map1.add(std::make_pair(2, 20)); - map1.add(std::make_pair(4, 40)); - map1.add(std::make_pair(6, 60)); + map1.add(Pair(2, 20)); + map1.add(Pair(4, 40)); + map1.add(Pair(6, 60)); test(map1.containsKey(2)); test(map1.containsKey(4)); @@ -229,9 +299,9 @@ class TestMap : public Test { void testIndexing() { Map map1(mAllocator); - map1.add(std::make_pair(2, 20)); - map1.add(std::make_pair(4, 40)); - map1.add(std::make_pair(6, 60)); + map1.add(Pair(2, 20)); + map1.add(Pair(4, 40)); + map1.add(Pair(6, 60)); test(map1[2] == 20); test(map1[4] == 40); test(map1[6] == 60); @@ -245,6 +315,26 @@ class TestMap : public Test { test(map1[6] == 30); } + void testFind() { + + Map map1(mAllocator); + map1.add(Pair(2, 20)); + map1.add(Pair(4, 40)); + map1.add(Pair(6, 60)); + test(map1.find(2)->second == 20); + test(map1.find(4)->second == 40); + test(map1.find(6)->second == 60); + test(map1.find(45) == map1.end()); + + map1[2] = 10; + map1[4] = 20; + map1[6] = 30; + + test(map1.find(2)->second == 10); + test(map1.find(4)->second == 20); + test(map1.find(6)->second == 30); + } + void testEquality() { Map map1(mAllocator, 10); @@ -252,13 +342,13 @@ class TestMap : public Test { test(map1 == map2); - map1.add(std::make_pair("a", 1)); - map1.add(std::make_pair("b", 2)); - map1.add(std::make_pair("c", 3)); + map1.add(Pair("a", 1)); + map1.add(Pair("b", 2)); + map1.add(Pair("c", 3)); - map2.add(std::make_pair("a", 1)); - map2.add(std::make_pair("b", 2)); - map2.add(std::make_pair("c", 4)); + map2.add(Pair("a", 1)); + map2.add(Pair("b", 2)); + map2.add(Pair("c", 4)); test(map1 == map1); test(map2 == map2); @@ -269,7 +359,7 @@ class TestMap : public Test { test(map1 == map2); Map map3(mAllocator); - map3.add(std::make_pair("a", 1)); + map3.add(Pair("a", 1)); test(map1 != map3); test(map2 != map3); @@ -278,13 +368,14 @@ class TestMap : public Test { void testAssignment() { Map map1(mAllocator); - map1.add(std::make_pair(1, 3)); - map1.add(std::make_pair(2, 6)); - map1.add(std::make_pair(10, 30)); -/* + map1.add(Pair(1, 3)); + map1.add(Pair(2, 6)); + map1.add(Pair(10, 30)); + Map map2(mAllocator); map2 = map1; test(map2.size() == map1.size()); + test(map1 == map2); test(map2[1] == 3); test(map2[2] == 6); test(map2[10] == 30); @@ -292,6 +383,7 @@ class TestMap : public Test { Map map3(mAllocator, 100); map3 = map1; test(map3.size() == map1.size()); + test(map3 == map1); test(map3[1] == 3); test(map3[2] == 6); test(map3[10] == 30); @@ -299,18 +391,40 @@ class TestMap : public Test { Map map4(mAllocator); map3 = map4; test(map3.size() == 0); -*/ + test(map3 == map4); + Map map5(mAllocator); - map5.add(std::make_pair(7, 8)); - map5.add(std::make_pair(19, 70)); + map5.add(Pair(7, 8)); + map5.add(Pair(19, 70)); map1 = map5; test(map5.size() == map1.size()); + test(map5 == map1); test(map1[7] == 8); test(map1[19] == 70); } - void testIteration() { + void testIterators() { + Map map1(mAllocator); + + test(map1.begin() == map1.end()); + + map1.add(Pair(1, 5)); + map1.add(Pair(2, 6)); + map1.add(Pair(3, 8)); + map1.add(Pair(4, -1)); + + Map::Iterator itBegin = map1.begin(); + Map::Iterator it = map1.begin(); + + test(itBegin == it); + + int size = 0; + for (auto it = map1.begin(); it != map1.end(); ++it) { + test(map1.containsKey(it->first)); + size++; + } + test(map1.size() == size); } }; diff --git a/test/tests/containers/TestSet.h b/test/tests/containers/TestSet.h new file mode 100644 index 00000000..ddf2c245 --- /dev/null +++ b/test/tests/containers/TestSet.h @@ -0,0 +1,417 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef TEST_SET_H +#define TEST_SET_H + +// Libraries +#include "Test.h" +#include "containers/Set.h" +#include "memory/DefaultAllocator.h" + +// Key to test map with always same hash values +namespace reactphysics3d { + struct TestValueSet { + int key; + + TestValueSet(int k) :key(k) {} + + bool operator==(const TestValueSet& testValue) const { + return key == testValue.key; + } + }; +} + +// Hash function for struct VerticesPair +namespace std { + + template <> struct hash { + + size_t operator()(const reactphysics3d::TestValueSet& value) const { + return 1; + } + }; +} + +/// Reactphysics3D namespace +namespace reactphysics3d { + +// Class TestSet +/** + * Unit test for the Set class + */ +class TestSet : public Test { + + private : + + // ---------- Atributes ---------- // + + DefaultAllocator mAllocator; + + public : + + // ---------- Methods ---------- // + + /// Constructor + TestSet(const std::string& name) : Test(name) { + + } + + /// Run the tests + void run() { + + testConstructors(); + testReserve(); + testAddRemoveClear(); + testContains(); + testFind(); + testEquality(); + testAssignment(); + testIterators(); + } + + void testConstructors() { + + // ----- Constructors ----- // + + Set set1(mAllocator); + test(set1.capacity() == 0); + test(set1.size() == 0); + + Set set2(mAllocator, 100); + test(set2.capacity() >= 100); + test(set2.size() == 0); + + // ----- Copy Constructors ----- // + Set set3(set1); + test(set3.capacity() == set1.capacity()); + test(set3.size() == set1.size()); + + Set set4(mAllocator); + set4.add(10); + set4.add(20); + set4.add(30); + test(set4.capacity() >= 3); + test(set4.size() == 3); + set4.add(30); + test(set4.size() == 3); + + Set set5(set4); + test(set5.capacity() == set4.capacity()); + test(set5.size() == set4.size()); + test(set5.contains(10)); + test(set5.contains(20)); + test(set5.contains(30)); + } + + void testReserve() { + + Set set1(mAllocator); + set1.reserve(15); + test(set1.capacity() >= 15); + set1.add("test1"); + set1.add("test2"); + test(set1.capacity() >= 15); + + set1.reserve(10); + test(set1.capacity() >= 15); + + set1.reserve(100); + test(set1.capacity() >= 100); + test(set1.contains("test1")); + test(set1.contains("test2")); + } + + void testAddRemoveClear() { + + // ----- Test add() ----- // + + Set set1(mAllocator); + set1.add(10); + set1.add(80); + set1.add(130); + test(set1.contains(10)); + test(set1.contains(80)); + test(set1.contains(130)); + test(set1.size() == 3); + + Set set2(mAllocator, 15); + for (int i = 0; i < 1000000; i++) { + set2.add(i); + } + bool isValid = true; + for (int i = 0; i < 1000000; i++) { + if (!set2.contains(i)) isValid = false; + } + test(isValid); + + set1.remove(10); + set1.add(10); + test(set1.size() == 3); + test(set1.contains(10)); + + set1.add(34); + test(set1.contains(34)); + test(set1.size() == 4); + + // ----- Test remove() ----- // + + set1.remove(10); + test(!set1.contains(10)); + test(set1.contains(80)); + test(set1.contains(130)); + test(set1.contains(34)); + test(set1.size() == 3); + + set1.remove(80); + test(!set1.contains(80)); + test(set1.contains(130)); + test(set1.contains(34)); + test(set1.size() == 2); + + set1.remove(130); + test(!set1.contains(130)); + test(set1.contains(34)); + test(set1.size() == 1); + + set1.remove(34); + test(!set1.contains(34)); + test(set1.size() == 0); + + isValid = true; + for (int i = 0; i < 1000000; i++) { + set2.remove(i); + } + for (int i = 0; i < 1000000; i++) { + if (set2.contains(i)) isValid = false; + } + test(isValid); + test(set2.size() == 0); + + Set set3(mAllocator); + for (int i=0; i < 1000000; i++) { + set3.add(i); + set3.remove(i); + } + + set3.add(1); + set3.add(2); + set3.add(3); + test(set3.size() == 3); + auto it = set3.begin(); + set3.remove(it++); + test(!set3.contains(1)); + test(set3.size() == 2); + test(*it == 2); + + set3.add(6); + set3.add(7); + set3.add(8); + for (it = set3.begin(); it != set3.end();) { + it = set3.remove(it); + } + test(set3.size() == 0); + + // ----- Test clear() ----- // + + Set set4(mAllocator); + set4.add(2); + set4.add(4); + set4.add(6); + set4.clear(); + test(set4.size() == 0); + set4.add(2); + test(set4.size() == 1); + test(set4.contains(2)); + set4.clear(); + test(set4.size() == 0); + + Set set5(mAllocator); + set5.clear(); + test(set5.size() == 0); + + // ----- Test map with always same hash value for keys ----- // + + Set set6(mAllocator); + for (int i=0; i < 1000; i++) { + set6.add(TestValueSet(i)); + } + bool isTestValid = true; + for (int i=0; i < 1000; i++) { + if (!set6.contains(TestValueSet(i))) { + isTestValid = false; + } + } + test(isTestValid); + for (int i=0; i < 1000; i++) { + set6.remove(TestValueSet(i)); + } + test(set6.size() == 0); + } + + void testContains() { + + Set set1(mAllocator); + + test(!set1.contains(2)); + test(!set1.contains(4)); + test(!set1.contains(6)); + + set1.add(2); + set1.add(4); + set1.add(6); + + test(set1.contains(2)); + test(set1.contains(4)); + test(set1.contains(6)); + + set1.remove(4); + test(!set1.contains(4)); + test(set1.contains(2)); + test(set1.contains(6)); + + set1.clear(); + test(!set1.contains(2)); + test(!set1.contains(6)); + } + + void testFind() { + + Set set1(mAllocator); + set1.add(2); + set1.add(4); + set1.add(6); + test(set1.find(2) != set1.end()); + test(set1.find(4) != set1.end()); + test(set1.find(6) != set1.end()); + test(set1.find(45) == set1.end()); + + set1.remove(2); + + test(set1.find(2) == set1.end()); + } + + void testEquality() { + + Set set1(mAllocator, 10); + Set set2(mAllocator, 2); + + test(set1 == set2); + + set1.add("a"); + set1.add("b"); + set1.add("c"); + + set2.add("a"); + set2.add("b"); + set2.add("h"); + + test(set1 == set1); + test(set2 == set2); + test(set1 != set2); + test(set2 != set1); + + set1.add("a"); + set2.remove("h"); + set2.add("c"); + + test(set1 == set2); + test(set2 == set1); + + Set set3(mAllocator); + set3.add("a"); + + test(set1 != set3); + test(set2 != set3); + test(set3 != set1); + test(set3 != set2); + } + + void testAssignment() { + + Set set1(mAllocator); + set1.add(1); + set1.add(2); + set1.add(10); + + Set set2(mAllocator); + set2 = set1; + test(set2.size() == set1.size()); + test(set2.contains(1)); + test(set2.contains(2)); + test(set2.contains(10)); + test(set1 == set2); + + Set set3(mAllocator, 100); + set3 = set1; + test(set3.size() == set1.size()); + test(set3 == set1); + test(set3.contains(1)); + test(set3.contains(2)); + test(set3.contains(10)); + + Set set4(mAllocator); + set3 = set4; + test(set3.size() == 0); + test(set3 == set4); + + Set set5(mAllocator); + set5.add(7); + set5.add(19); + set1 = set5; + test(set5.size() == set1.size()); + test(set1 == set5); + test(set1.contains(7)); + test(set1.contains(19)); + } + + void testIterators() { + + Set set1(mAllocator); + + test(set1.begin() == set1.end()); + + set1.add(1); + set1.add(2); + set1.add(3); + set1.add(4); + + Set::Iterator itBegin = set1.begin(); + Set::Iterator it = set1.begin(); + + test(itBegin == it); + + int size = 0; + for (auto it = set1.begin(); it != set1.end(); ++it) { + test(set1.contains(*it)); + size++; + } + test(set1.size() == size); + } + }; + +} + +#endif diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h index b76cb78c..5af12e60 100644 --- a/test/tests/mathematics/TestMathematicsFunctions.h +++ b/test/tests/mathematics/TestMathematicsFunctions.h @@ -103,6 +103,16 @@ class TestMathematicsFunctions : public Test { test(!sameSign(4, -7)); test(!sameSign(-4, 53)); + // Test computePointToPlaneDistance() + Vector3 p(8, 4, 0); + Vector3 n1(1, 0, 0); + Vector3 n2(-1, 0, 0); + Vector3 q1(1, 54, 0); + Vector3 q2(8, 17, 0); + test(approxEqual(computePointToPlaneDistance(q1, n1, p), decimal(-7))); + test(approxEqual(computePointToPlaneDistance(q1, n2, p), decimal(7))); + test(approxEqual(computePointToPlaneDistance(q2, n2, p), decimal(0.0))); + // Test computeBarycentricCoordinatesInTriangle() Vector3 a(0, 0, 0); Vector3 b(5, 0, 0); diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h index 9a90f25c..e60893df 100644 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -186,7 +186,7 @@ class CollisionDetectionScene : public SceneDemo { virtual void setIsContactPointsDisplayed(bool display) override; /// Return all the contact points of the scene - virtual std::vector getContactPoints() const override; + virtual std::vector getContactPoints() override; }; // Display or not the surface normals at hit points @@ -205,7 +205,7 @@ inline void CollisionDetectionScene::setIsContactPointsDisplayed(bool display) { } // Return all the contact points of the scene -inline std::vector CollisionDetectionScene::getContactPoints() const { +inline std::vector CollisionDetectionScene::getContactPoints() { return mContactManager.getContactPoints(); } diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.h b/testbed/scenes/collisionshapes/CollisionShapesScene.h index b22d8429..dbb49539 100644 --- a/testbed/scenes/collisionshapes/CollisionShapesScene.h +++ b/testbed/scenes/collisionshapes/CollisionShapesScene.h @@ -101,11 +101,11 @@ class CollisionShapesScene : public SceneDemo { virtual void reset() override; /// Return all the contact points of the scene - virtual std::vector getContactPoints() const override; + virtual std::vector getContactPoints() override; }; // Return all the contact points of the scene -inline std::vector CollisionShapesScene::getContactPoints() const { +inline std::vector CollisionShapesScene::getContactPoints() { return computeContactPointsOfWorld(getDynamicsWorld()); } diff --git a/testbed/scenes/concavemesh/ConcaveMeshScene.h b/testbed/scenes/concavemesh/ConcaveMeshScene.h index beca8f71..91ede1bb 100644 --- a/testbed/scenes/concavemesh/ConcaveMeshScene.h +++ b/testbed/scenes/concavemesh/ConcaveMeshScene.h @@ -98,11 +98,11 @@ class ConcaveMeshScene : public SceneDemo { virtual void reset() override; /// Return all the contact points of the scene - virtual std::vector getContactPoints() const override; + virtual std::vector getContactPoints() override; }; // Return all the contact points of the scene -inline std::vector ConcaveMeshScene::getContactPoints() const { +inline std::vector ConcaveMeshScene::getContactPoints() { return computeContactPointsOfWorld(getDynamicsWorld()); } diff --git a/testbed/scenes/cubes/CubesScene.h b/testbed/scenes/cubes/CubesScene.h index 6d553835..e284a91d 100755 --- a/testbed/scenes/cubes/CubesScene.h +++ b/testbed/scenes/cubes/CubesScene.h @@ -69,11 +69,11 @@ class CubesScene : public SceneDemo { virtual void reset() override; /// Return all the contact points of the scene - virtual std::vector getContactPoints() const override; + virtual std::vector getContactPoints() override; }; // Return all the contact points of the scene -inline std::vector CubesScene::getContactPoints() const { +inline std::vector CubesScene::getContactPoints() { return computeContactPointsOfWorld(getDynamicsWorld()); } diff --git a/testbed/scenes/cubestack/CubeStackScene.h b/testbed/scenes/cubestack/CubeStackScene.h index 9bbc1c01..d4a4dace 100644 --- a/testbed/scenes/cubestack/CubeStackScene.h +++ b/testbed/scenes/cubestack/CubeStackScene.h @@ -69,11 +69,11 @@ class CubeStackScene : public SceneDemo { virtual void reset() override; /// Return all the contact points of the scene - virtual std::vector getContactPoints() const override; + virtual std::vector getContactPoints() override; }; // Return all the contact points of the scene -inline std::vector CubeStackScene::getContactPoints() const { +inline std::vector CubeStackScene::getContactPoints() { return computeContactPointsOfWorld(getDynamicsWorld()); } diff --git a/testbed/scenes/heightfield/HeightFieldScene.h b/testbed/scenes/heightfield/HeightFieldScene.h index 2a3e68f5..90edda93 100644 --- a/testbed/scenes/heightfield/HeightFieldScene.h +++ b/testbed/scenes/heightfield/HeightFieldScene.h @@ -100,11 +100,11 @@ class HeightFieldScene : public SceneDemo { virtual void reset() override ; /// Return all the contact points of the scene - virtual std::vector getContactPoints() const override ; + virtual std::vector getContactPoints() override ; }; // Return all the contact points of the scene -inline std::vector HeightFieldScene::getContactPoints() const { +inline std::vector HeightFieldScene::getContactPoints() { return computeContactPointsOfWorld(getDynamicsWorld()); } diff --git a/testbed/scenes/joints/JointsScene.h b/testbed/scenes/joints/JointsScene.h index dad9af38..f101bf32 100644 --- a/testbed/scenes/joints/JointsScene.h +++ b/testbed/scenes/joints/JointsScene.h @@ -127,11 +127,11 @@ class JointsScene : public SceneDemo { virtual void reset() override; /// Return all the contact points of the scene - virtual std::vector getContactPoints() const override; + virtual std::vector getContactPoints() override; }; // Return all the contact points of the scene -inline std::vector JointsScene::getContactPoints() const { +inline std::vector JointsScene::getContactPoints() { return computeContactPointsOfWorld(getDynamicsWorld()); } diff --git a/testbed/scenes/raycast/RaycastScene.h b/testbed/scenes/raycast/RaycastScene.h index 8024ff1a..e8e4a854 100644 --- a/testbed/scenes/raycast/RaycastScene.h +++ b/testbed/scenes/raycast/RaycastScene.h @@ -198,7 +198,7 @@ class RaycastScene : public SceneDemo { virtual void setIsContactPointsDisplayed(bool display) override; /// Return all the contact points of the scene - virtual std::vector getContactPoints() const override; + virtual std::vector getContactPoints() override; }; // Display or not the surface normals at hit points @@ -217,7 +217,7 @@ inline void RaycastScene::setIsContactPointsDisplayed(bool display) { } // Return all the contact points of the scene -inline std::vector RaycastScene::getContactPoints() const { +inline std::vector RaycastScene::getContactPoints() { return mRaycastManager.getHitPoints(); } diff --git a/testbed/src/Scene.h b/testbed/src/Scene.h index 89a9afdd..6ccc3254 100644 --- a/testbed/src/Scene.h +++ b/testbed/src/Scene.h @@ -230,7 +230,7 @@ class Scene { void setIsWireframeEnabled(bool isEnabled); /// Return all the contact points of the scene - std::vector virtual getContactPoints() const; + std::vector virtual getContactPoints(); }; // Called when a keyboard event occurs @@ -303,7 +303,7 @@ inline void Scene::setIsWireframeEnabled(bool isEnabled) { } // Return all the contact points of the scene -inline std::vector Scene::getContactPoints() const { +inline std::vector Scene::getContactPoints() { // Return an empty list of contact points return std::vector(); diff --git a/testbed/src/SceneDemo.cpp b/testbed/src/SceneDemo.cpp index 5ef6fb95..8e2a92da 100644 --- a/testbed/src/SceneDemo.cpp +++ b/testbed/src/SceneDemo.cpp @@ -418,15 +418,15 @@ void SceneDemo::removeAllContactPoints() { } // Return all the contact points of the scene -std::vector SceneDemo::computeContactPointsOfWorld(const rp3d::DynamicsWorld* world) const { +std::vector SceneDemo::computeContactPointsOfWorld(rp3d::DynamicsWorld* world) { std::vector contactPoints; // Get the list of contact manifolds from the world - std::vector manifolds = world->getContactsList(); + rp3d::List manifolds = world->getContactsList(); // For each contact manifold - std::vector::const_iterator it; + rp3d::List::Iterator it; for (it = manifolds.begin(); it != manifolds.end(); ++it) { const rp3d::ContactManifold* manifold = *it; diff --git a/testbed/src/SceneDemo.h b/testbed/src/SceneDemo.h index 359c2374..1eaa30b9 100644 --- a/testbed/src/SceneDemo.h +++ b/testbed/src/SceneDemo.h @@ -158,7 +158,7 @@ class SceneDemo : public Scene { virtual void setIsShadowMappingEnabled(bool isShadowMappingEnabled) override; /// Return all the contact points of the scene - std::vector computeContactPointsOfWorld(const rp3d::DynamicsWorld* world) const; + std::vector computeContactPointsOfWorld(reactphysics3d::DynamicsWorld *world); }; // Enabled/Disable the shadow mapping