diff --git a/CMakeLists.txt b/CMakeLists.txt index 11cbd844..c6f2e52a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,9 +63,6 @@ set (REACTPHYSICS3D_HEADERS "include/reactphysics3d/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h" "include/reactphysics3d/collision/narrowphase/NarrowPhaseInput.h" "include/reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h" - "include/reactphysics3d/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h" - "include/reactphysics3d/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h" - "include/reactphysics3d/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h" "include/reactphysics3d/collision/shapes/AABB.h" "include/reactphysics3d/collision/shapes/ConvexShape.h" "include/reactphysics3d/collision/shapes/ConvexPolyhedronShape.h" @@ -168,9 +165,6 @@ set (REACTPHYSICS3D_SOURCES "src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp" "src/collision/narrowphase/NarrowPhaseInput.cpp" "src/collision/narrowphase/NarrowPhaseInfoBatch.cpp" - "src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp" - "src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.cpp" - "src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.cpp" "src/collision/shapes/AABB.cpp" "src/collision/shapes/ConvexShape.cpp" "src/collision/shapes/ConvexPolyhedronShape.cpp" diff --git a/include/reactphysics3d/collision/ContactPointInfo.h b/include/reactphysics3d/collision/ContactPointInfo.h index 387f2363..b75b9b97 100644 --- a/include/reactphysics3d/collision/ContactPointInfo.h +++ b/include/reactphysics3d/collision/ContactPointInfo.h @@ -45,10 +45,6 @@ class CollisionBody; */ struct ContactPointInfo { - private: - - // -------------------- Methods -------------------- // - public: // -------------------- Attributes -------------------- // @@ -64,18 +60,6 @@ struct ContactPointInfo { /// Contact point of body 2 in local space of body 2 Vector3 localPoint2; - - // -------------------- Methods -------------------- // - - /// Constructor - ContactPointInfo(const Vector3& contactNormal, decimal penDepth, - const Vector3& localPt1, const Vector3& localPt2) - : normal(contactNormal), penetrationDepth(penDepth), - localPoint1(localPt1), localPoint2(localPt2) { - - assert(contactNormal.lengthSquare() > decimal(0.8)); - assert(penDepth > decimal(0.0)); - } }; } diff --git a/include/reactphysics3d/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/include/reactphysics3d/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h index a34d58bf..f065235c 100644 --- a/include/reactphysics3d/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h +++ b/include/reactphysics3d/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h @@ -34,7 +34,7 @@ namespace reactphysics3d { // Declarations -struct CapsuleVsCapsuleNarrowPhaseInfoBatch; +struct NarrowPhaseInfoBatch; class ContactPoint; // Class CapsuleVsCapsuleAlgorithm @@ -66,7 +66,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between two capsules - bool testCollision(CapsuleVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, + bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, MemoryAllocator& memoryAllocator); }; diff --git a/include/reactphysics3d/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h b/include/reactphysics3d/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h deleted file mode 100644 index 2312a2e2..00000000 --- a/include/reactphysics3d/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h +++ /dev/null @@ -1,78 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2020 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_CAPSULE_VS_CAPSULE_NARROW_PHASE_INFO_BATCH_H -#define REACTPHYSICS3D_CAPSULE_VS_CAPSULE_NARROW_PHASE_INFO_BATCH_H - -// Libraries -#include - -/// Namespace ReactPhysics3D -namespace reactphysics3d { - -// Struct CapsuleVsCapsuleNarrowPhaseInfoBatch -/** - * This structure collects all the potential collisions from the middle-phase algorithm - * that have to be tested during narrow-phase collision detection. This class collects all the - * capsule vs capsule collision detection tests. - */ -struct CapsuleVsCapsuleNarrowPhaseInfoBatch : public NarrowPhaseInfoBatch { - - public: - - /// List of radiuses for the first capsules - List capsule1Radiuses; - - /// List of radiuses for the second capsules - List capsule2Radiuses; - - /// List of heights for the first capsules - List capsule1Heights; - - /// List of heights for the second capsules - List capsule2Heights; - - /// Constructor - CapsuleVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs); - - /// Destructor - virtual ~CapsuleVsCapsuleNarrowPhaseInfoBatch() override = default; - - /// Add shapes to be tested during narrow-phase collision detection into the batch - virtual void addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, - CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, bool needToReportContacts, MemoryAllocator& shapeAllocator) override; - - // Initialize the containers using cached capacity - virtual void reserveMemory() override; - - /// Clear all the objects in the batch - virtual void clear() override; -}; - -} - -#endif - diff --git a/include/reactphysics3d/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h b/include/reactphysics3d/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h index 80987bf3..f72e7db2 100644 --- a/include/reactphysics3d/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h +++ b/include/reactphysics3d/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h @@ -34,6 +34,7 @@ namespace reactphysics3d { // Declarations class ContactPoint; +struct NarrowPhaseInfoBatch; // Class CapsuleVsConvexPolyhedronAlgorithm /** diff --git a/include/reactphysics3d/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h b/include/reactphysics3d/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h index c3cb6e0d..2b6525e7 100644 --- a/include/reactphysics3d/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h +++ b/include/reactphysics3d/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h @@ -34,6 +34,7 @@ namespace reactphysics3d { // Declarations class ContactPoint; +struct NarrowPhaseInfoBatch; // Class ConvexPolyhedronVsConvexPolyhedronAlgorithm /** diff --git a/include/reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h b/include/reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h index 73ce4e33..06b8e0cc 100644 --- a/include/reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h +++ b/include/reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h @@ -28,6 +28,7 @@ // Libraries #include +#include /// Namespace ReactPhysics3D namespace reactphysics3d { @@ -40,16 +41,68 @@ struct ContactPointInfo; // Struct NarrowPhaseInfoBatch /** - * This abstract structure collects all the potential collisions from the middle-phase algorithm - * that have to be tested during narrow-phase collision detection. There is an implementation of - * this class for each kind of collision detection test. For instance, one for sphere vs sphere, - * one for sphere vs capsule, ... + * This structure collects all the potential collisions from the middle-phase algorithm + * that have to be tested during narrow-phase collision detection. */ struct NarrowPhaseInfoBatch { + struct NarrowPhaseInfo { + + /// Broadphase overlapping pairs ids + uint64 overlappingPairId; + + /// Entity of the first collider to test collision with + Entity colliderEntity1; + + /// Entity of the second collider to test collision with + Entity colliderEntity2; + + /// Collision info of the previous frame + // TODO OPTI : Do we really need to use a pointer here ? why not flat object instead ? + LastFrameCollisionInfo* lastFrameCollisionInfo; + + /// Memory allocator for the collision shape (Used to release TriangleShape memory in destructor) + MemoryAllocator* collisionShapeAllocator; + + /// Shape local to world transform of sphere 1 + Transform shape1ToWorldTransform; + + /// Shape local to world transform of sphere 2 + Transform shape2ToWorldTransform; + + /// Pointer to the first collision shapes to test collision with + CollisionShape* collisionShape1; + + /// Pointer to the second collision shapes to test collision with + CollisionShape* collisionShape2; + + /// True if we need to report contacts (false for triggers for instance) + bool reportContacts; + + /// Result of the narrow-phase collision detection test + bool isColliding; + + /// Number of contact points + uint8 nbContactPoints; + + /// List of contact points created during the narrow-phase + ContactPointInfo contactPoints[NB_MAX_CONTACT_POINTS_IN_NARROWPHASE_INFO]; + + /// Constructor + NarrowPhaseInfo(uint64 pairId, Entity collider1, Entity collider2, LastFrameCollisionInfo* lastFrameInfo, MemoryAllocator& shapeAllocator, + const Transform& shape1ToWorldTransform, const Transform& shape2ToWorldTransform, CollisionShape* shape1, + CollisionShape* shape2, bool needToReportContacts) + : overlappingPairId(pairId), colliderEntity1(collider1), colliderEntity2(collider2), lastFrameCollisionInfo(lastFrameInfo), + collisionShapeAllocator(&shapeAllocator), shape1ToWorldTransform(shape1ToWorldTransform), + shape2ToWorldTransform(shape2ToWorldTransform), collisionShape1(shape1), + collisionShape2(shape2), reportContacts(needToReportContacts), isColliding(false), nbContactPoints(0) { + + } + }; + protected: - /// Memory allocator + /// Reference to the memory allocator MemoryAllocator& mMemoryAllocator; /// Reference to all the broad-phase overlapping pairs @@ -60,73 +113,40 @@ struct NarrowPhaseInfoBatch { public: - /// List of Broadphase overlapping pairs ids - List overlappingPairIds; - - /// List of pointers to the first colliders to test collision with - List colliderEntities1; - - /// List of pointers to the second colliders to test collision with - List colliderEntities2; - - /// List of pointers to the first collision shapes to test collision with - List collisionShapes1; - - /// List of pointers to the second collision shapes to test collision with - List collisionShapes2; - - /// List of transforms that maps from collision shape 1 local-space to world-space - List shape1ToWorldTransforms; - - /// List of transforms that maps from collision shape 2 local-space to world-space - List shape2ToWorldTransforms; - - /// True for each pair of objects that we need to report contacts (false for triggers for instance) - List reportContacts; - - /// Result of the narrow-phase collision detection test - List isColliding; - - /// List of contact points created during the narrow-phase - List> contactPoints; - - /// Memory allocators for the collision shape (Used to release TriangleShape memory in destructor) - List collisionShapeAllocators; - - /// Collision infos of the previous frame - List lastFrameCollisionInfos; + /// For each collision test, we keep some meta data + List narrowPhaseInfos; /// Constructor - NarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs); + NarrowPhaseInfoBatch(OverlappingPairs& overlappingPairs, MemoryAllocator& allocator); /// Destructor - virtual ~NarrowPhaseInfoBatch(); + ~NarrowPhaseInfoBatch(); + + /// Add shapes to be tested during narrow-phase collision detection into the batch + void addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, + CollisionShape* shape2, const Transform& shape1Transform, const Transform& shape2Transform, + bool needToReportContacts, MemoryAllocator& shapeAllocator); /// Return the number of objects in the batch uint getNbObjects() const; - /// Add shapes to be tested during narrow-phase collision detection into the batch - virtual void addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, - CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, bool needToReportContacts, MemoryAllocator& shapeAllocator); - /// Add a new contact point - virtual void addContactPoint(uint index, const Vector3& contactNormal, decimal penDepth, + void addContactPoint(uint index, const Vector3& contactNormal, decimal penDepth, const Vector3& localPt1, const Vector3& localPt2); /// Reset the remaining contact points void resetContactPoints(uint index); // Initialize the containers using cached capacity - virtual void reserveMemory(); + void reserveMemory(); /// Clear all the objects in the batch - virtual void clear(); + void clear(); }; /// Return the number of objects in the batch inline uint NarrowPhaseInfoBatch::getNbObjects() const { - return overlappingPairIds.size(); + return narrowPhaseInfos.size(); } } diff --git a/include/reactphysics3d/collision/narrowphase/NarrowPhaseInput.h b/include/reactphysics3d/collision/narrowphase/NarrowPhaseInput.h index 5e8b4d9b..4ad189da 100644 --- a/include/reactphysics3d/collision/narrowphase/NarrowPhaseInput.h +++ b/include/reactphysics3d/collision/narrowphase/NarrowPhaseInput.h @@ -29,9 +29,6 @@ // Libraries #include #include -#include -#include -#include /// Namespace ReactPhysics3D namespace reactphysics3d { @@ -55,9 +52,9 @@ class NarrowPhaseInput { private: - SphereVsSphereNarrowPhaseInfoBatch mSphereVsSphereBatch; - SphereVsCapsuleNarrowPhaseInfoBatch mSphereVsCapsuleBatch; - CapsuleVsCapsuleNarrowPhaseInfoBatch mCapsuleVsCapsuleBatch; + NarrowPhaseInfoBatch mSphereVsSphereBatch; + NarrowPhaseInfoBatch mSphereVsCapsuleBatch; + NarrowPhaseInfoBatch mCapsuleVsCapsuleBatch; NarrowPhaseInfoBatch mSphereVsConvexPolyhedronBatch; NarrowPhaseInfoBatch mCapsuleVsConvexPolyhedronBatch; NarrowPhaseInfoBatch mConvexPolyhedronVsConvexPolyhedronBatch; @@ -74,13 +71,13 @@ class NarrowPhaseInput { MemoryAllocator& shapeAllocator); /// Get a reference to the sphere vs sphere batch - SphereVsSphereNarrowPhaseInfoBatch& getSphereVsSphereBatch(); + NarrowPhaseInfoBatch& getSphereVsSphereBatch(); /// Get a reference to the sphere vs capsule batch - SphereVsCapsuleNarrowPhaseInfoBatch& getSphereVsCapsuleBatch(); + NarrowPhaseInfoBatch& getSphereVsCapsuleBatch(); /// Get a reference to the capsule vs capsule batch - CapsuleVsCapsuleNarrowPhaseInfoBatch& getCapsuleVsCapsuleBatch(); + NarrowPhaseInfoBatch& getCapsuleVsCapsuleBatch(); /// Get a reference to the sphere vs convex polyhedron batch NarrowPhaseInfoBatch& getSphereVsConvexPolyhedronBatch(); @@ -100,17 +97,17 @@ class NarrowPhaseInput { // Get a reference to the sphere vs sphere batch contacts -inline SphereVsSphereNarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsSphereBatch() { +inline NarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsSphereBatch() { return mSphereVsSphereBatch; } // Get a reference to the sphere vs capsule batch contacts -inline SphereVsCapsuleNarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsCapsuleBatch() { +inline NarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsCapsuleBatch() { return mSphereVsCapsuleBatch; } // Get a reference to the capsule vs capsule batch contacts -inline CapsuleVsCapsuleNarrowPhaseInfoBatch& NarrowPhaseInput::getCapsuleVsCapsuleBatch() { +inline NarrowPhaseInfoBatch& NarrowPhaseInput::getCapsuleVsCapsuleBatch() { return mCapsuleVsCapsuleBatch; } @@ -125,7 +122,7 @@ inline NarrowPhaseInfoBatch& NarrowPhaseInput::getCapsuleVsConvexPolyhedronBatch } // Get a reference to the convex polyhedron vs convex polyhedron batch contacts -inline NarrowPhaseInfoBatch& NarrowPhaseInput::getConvexPolyhedronVsConvexPolyhedronBatch() { +inline NarrowPhaseInfoBatch &NarrowPhaseInput::getConvexPolyhedronVsConvexPolyhedronBatch() { return mConvexPolyhedronVsConvexPolyhedronBatch; } diff --git a/include/reactphysics3d/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/include/reactphysics3d/collision/narrowphase/SphereVsCapsuleAlgorithm.h index 21c85918..49d27a4f 100644 --- a/include/reactphysics3d/collision/narrowphase/SphereVsCapsuleAlgorithm.h +++ b/include/reactphysics3d/collision/narrowphase/SphereVsCapsuleAlgorithm.h @@ -34,7 +34,7 @@ namespace reactphysics3d { // Declarations class ContactPoint; -struct SphereVsCapsuleNarrowPhaseInfoBatch; +struct NarrowPhaseInfoBatch; // Class SphereVsCapsuleAlgorithm /** @@ -65,7 +65,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a sphere and a capsule - bool testCollision(SphereVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, + bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, MemoryAllocator& memoryAllocator); }; diff --git a/include/reactphysics3d/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h b/include/reactphysics3d/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h deleted file mode 100644 index 4841a3cf..00000000 --- a/include/reactphysics3d/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h +++ /dev/null @@ -1,78 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2020 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_SPHERE_VS_CAPSULE_NARROW_PHASE_INFO_BATCH_H -#define REACTPHYSICS3D_SPHERE_VS_CAPSULE_NARROW_PHASE_INFO_BATCH_H - -// Libraries -#include - -/// Namespace ReactPhysics3D -namespace reactphysics3d { - -// Struct SphereVsCapsuleNarrowPhaseInfoBatch -/** - * This structure collects all the potential collisions from the middle-phase algorithm - * that have to be tested during narrow-phase collision detection. This class collects all the - * sphere vs capsule collision detection tests. - */ -struct SphereVsCapsuleNarrowPhaseInfoBatch : public NarrowPhaseInfoBatch { - - public: - - /// List of boolean values to know if the the sphere is the first or second shape - List isSpheresShape1; - - /// List of radiuses for the spheres - List sphereRadiuses; - - /// List of radiuses for the capsules - List capsuleRadiuses; - - /// List of heights for the capsules - List capsuleHeights; - - /// Constructor - SphereVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs); - - /// Destructor - virtual ~SphereVsCapsuleNarrowPhaseInfoBatch() override = default; - - /// Add shapes to be tested during narrow-phase collision detection into the batch - virtual void addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, - CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, bool needToReportContacts, MemoryAllocator& shapeAllocator) override; - - // Initialize the containers using cached capacity - virtual void reserveMemory() override; - - /// Clear all the objects in the batch - virtual void clear() override; -}; - -} - -#endif - diff --git a/include/reactphysics3d/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/include/reactphysics3d/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h index 264e9f5a..41a9a87e 100644 --- a/include/reactphysics3d/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h +++ b/include/reactphysics3d/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h @@ -34,6 +34,7 @@ namespace reactphysics3d { // Declarations class ContactPoint; +struct NarrowPhaseInfoBatch; // Class SphereVsConvexPolyhedronAlgorithm /** diff --git a/include/reactphysics3d/collision/narrowphase/SphereVsSphereAlgorithm.h b/include/reactphysics3d/collision/narrowphase/SphereVsSphereAlgorithm.h index e6f68e7a..85193f52 100644 --- a/include/reactphysics3d/collision/narrowphase/SphereVsSphereAlgorithm.h +++ b/include/reactphysics3d/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -34,7 +34,7 @@ namespace reactphysics3d { // Declarations class ContactPoint; -struct SphereVsSphereNarrowPhaseInfoBatch; +struct NarrowPhaseInfoBatch; // Class SphereVsSphereAlgorithm /** @@ -65,7 +65,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete; /// Compute a contact info if the two bounding volume collide - bool testCollision(SphereVsSphereNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, + bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, MemoryAllocator& memoryAllocator); }; diff --git a/include/reactphysics3d/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h b/include/reactphysics3d/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h deleted file mode 100644 index 63792c71..00000000 --- a/include/reactphysics3d/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h +++ /dev/null @@ -1,72 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2020 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_SPHERE_VS_SPHERE_NARROW_PHASE_INFO_BATCH_H -#define REACTPHYSICS3D_SPHERE_VS_SPHERE_NARROW_PHASE_INFO_BATCH_H - -// Libraries -#include - -/// Namespace ReactPhysics3D -namespace reactphysics3d { - -// Struct SphereVsSphereNarrowPhaseInfoBatch -/** - * This structure collects all the potential collisions from the middle-phase algorithm - * that have to be tested during narrow-phase collision detection. This class collects all the - * sphere vs sphere collision detection tests. - */ -struct SphereVsSphereNarrowPhaseInfoBatch : public NarrowPhaseInfoBatch { - - public: - - /// List of radiuses for the first spheres - List sphere1Radiuses; - - /// List of radiuses for the second spheres - List sphere2Radiuses; - - /// Constructor - SphereVsSphereNarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs); - - /// Destructor - virtual ~SphereVsSphereNarrowPhaseInfoBatch() override = default; - - /// Add shapes to be tested during narrow-phase collision detection into the batch - virtual void addNarrowPhaseInfo(uint64 airId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, - CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, bool needToReportContacts, MemoryAllocator& shapeAllocator) override; - - // Initialize the containers using cached capacity - virtual void reserveMemory() override; - - /// Clear all the objects in the batch - virtual void clear() override; -}; - -} - -#endif - diff --git a/include/reactphysics3d/configuration.h b/include/reactphysics3d/configuration.h index 0fbd2b6a..77832d0d 100644 --- a/include/reactphysics3d/configuration.h +++ b/include/reactphysics3d/configuration.h @@ -35,15 +35,35 @@ #include #include -// Windows platform +// Platforms #if defined(WIN32) ||defined(_WIN32) || defined(_WIN64) ||defined(__WIN32__) || defined(__WINDOWS__) #define WINDOWS_OS -#elif defined(__APPLE__) // Apple platform +#elif defined(__APPLE__) #define APPLE_OS #elif defined(__linux__) || defined(linux) || defined(__linux) // Linux platform #define LINUX_OS #endif +// Compilers +#if defined(_MSC_VER) + #define RP3D_COMPILER_VISUAL_STUDIO +#elif defined(__clang__) + #define RP3D_COMPILER_CLANG +#elif defined(__GNUC__) + #define RP3D_COMPILER_GCC +#else + #define RP3D_COMPILER_UNKNOWN +#endif + +// Force inline macro +#if defined(RP3D_COMPILER_VISUAL_STUDIO) + #define RP3D_FORCE_INLINE __forceinline +#elif defined(RP3D_COMPILER_GCC) || defined(RP3D_COMPILER_CLANG) + #define RP3D_FORCE_INLINE inline __attribute__((always_inline)) +#else + #define RP3D_FORCE_INLINE inline +#endif + /// Namespace reactphysics3d namespace reactphysics3d { @@ -103,6 +123,9 @@ constexpr decimal PI_TIMES_2 = decimal(6.28318530); /// without triggering a large modification of the tree each frame which can be costly constexpr decimal DYNAMIC_TREE_FAT_AABB_INFLATE_PERCENTAGE = decimal(0.08); +/// Maximum number of contact points in a narrow phase info object +constexpr uint8 NB_MAX_CONTACT_POINTS_IN_NARROWPHASE_INFO = 16; + /// Maximum number of contact manifolds in an overlapping pair constexpr uint8 NB_MAX_CONTACT_MANIFOLDS = 3; @@ -110,7 +133,7 @@ constexpr uint8 NB_MAX_CONTACT_MANIFOLDS = 3; constexpr uint8 NB_MAX_POTENTIAL_CONTACT_MANIFOLDS = 4 * NB_MAX_CONTACT_MANIFOLDS; /// Maximum number of contact points in potential contact manifold -constexpr uint8 NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD = 12; +constexpr uint8 NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD = 16; /// Current version of ReactPhysics3D const std::string RP3D_VERSION = std::string("0.8.0"); diff --git a/include/reactphysics3d/engine/OverlappingPairs.h b/include/reactphysics3d/engine/OverlappingPairs.h index 13dfad0c..a3920148 100644 --- a/include/reactphysics3d/engine/OverlappingPairs.h +++ b/include/reactphysics3d/engine/OverlappingPairs.h @@ -54,6 +54,8 @@ class CollisionDispatch; */ struct LastFrameCollisionInfo { + // TODO OPTI : Use bit flags instead of bools here + /// True if we have information about the previous frame bool isValid; @@ -77,9 +79,9 @@ struct LastFrameCollisionInfo { // SAT Algorithm bool satIsAxisFacePolyhedron1; bool satIsAxisFacePolyhedron2; - uint satMinAxisFaceIndex; - uint satMinEdge1Index; - uint satMinEdge2Index; + uint8 satMinAxisFaceIndex; + uint8 satMinEdge1Index; + uint8 satMinEdge2Index; /// Constructor LastFrameCollisionInfo() { @@ -118,7 +120,7 @@ class OverlappingPairs { MemoryAllocator& mPersistentAllocator; /// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo - // TODO : Do we need to keep this ? + // TODO OPTI : Do we need to keep this ? MemoryAllocator& mTempMemoryAllocator; /// Current number of components diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index 4fa4595f..aa201ac8 100755 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -26,7 +26,7 @@ // Libraries #include #include -#include +#include // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; @@ -34,30 +34,37 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between two capsules // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, - MemoryAllocator& memoryAllocator) { +bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, MemoryAllocator& memoryAllocator) { bool isCollisionFound = false; for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { - assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].nbContactPoints == 0); - assert(!narrowPhaseInfoBatch.isColliding[batchIndex]); + assert(!narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding); // Get the transform from capsule 1 local-space to capsule 2 local-space - const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getInverse() * - narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex]; + const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getInverse() * + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform; + + const CapsuleShape* capsuleShape1 = static_cast(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1); + const CapsuleShape* capsuleShape2 = static_cast(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2); + + const decimal capsule1Height = capsuleShape1->getHeight(); + const decimal capsule2Height = capsuleShape2->getHeight(); + const decimal capsule1Radius = capsuleShape1->getRadius(); + const decimal capsule2Radius = capsuleShape2->getRadius(); // Compute the end-points of the inner segment of the first capsule - const decimal capsule1HalfHeight = narrowPhaseInfoBatch.capsule1Heights[batchIndex] * decimal(0.5); + const decimal capsule1HalfHeight = capsule1Height * decimal(0.5); Vector3 capsule1SegA(0, -capsule1HalfHeight, 0); Vector3 capsule1SegB(0, capsule1HalfHeight, 0); capsule1SegA = capsule1ToCapsule2SpaceTransform * capsule1SegA; capsule1SegB = capsule1ToCapsule2SpaceTransform * capsule1SegB; // Compute the end-points of the inner segment of the second capsule - const decimal capsule2HalfHeight = narrowPhaseInfoBatch.capsule2Heights[batchIndex] * decimal(0.5); + const decimal capsule2HalfHeight = capsule2Height * decimal(0.5); const Vector3 capsule2SegA(0, -capsule2HalfHeight, 0); const Vector3 capsule2SegB(0, capsule2HalfHeight, 0); @@ -66,8 +73,6 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat const Vector3 seg2 = capsule2SegB - capsule2SegA; // Compute the sum of the radius of the two capsules (virtual spheres) - const decimal capsule1Radius = narrowPhaseInfoBatch.capsule1Radiuses[batchIndex]; - const decimal capsule2Radius = narrowPhaseInfoBatch.capsule2Radiuses[batchIndex]; const decimal sumRadius = capsule1Radius + capsule2Radius; // If the two capsules are parallel (we create two contact points) @@ -95,7 +100,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat // If the segments were overlapping (the clip segment is valid) if (t1 > decimal(0.0) && t2 > decimal(0.0)) { - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { // Clip the inner segment of capsule 2 if (t1 > decimal(1.0)) t1 = decimal(1.0); @@ -145,14 +150,14 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat decimal penetrationDepth = sumRadius - segmentsPerpendicularDistance; - const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsule2SpaceNormalized; + const Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * normalCapsule2SpaceNormalized; // Create the contact info object narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local); narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local); } - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; continue; } @@ -171,7 +176,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat // If the collision shapes overlap if (closestPointsDistanceSquare < sumRadius * sumRadius) { - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { // If the distance between the inner segments is not zero if (closestPointsDistanceSquare > MACHINE_EPSILON) { @@ -182,7 +187,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsule1Radius); const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsule2Radius; - const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * closestPointsSeg1ToSeg2; + const Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2; decimal penetrationDepth = sumRadius - closestPointsDistance; @@ -205,7 +210,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsule1Radius); const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsule2Radius; - const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsuleSpace2; + const Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2; // Create the contact info object narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local); @@ -221,7 +226,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsule1Radius); const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsule2Radius; - const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsuleSpace2; + const Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2; // Create the contact info object narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local); @@ -229,7 +234,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat } } - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; } } diff --git a/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.cpp b/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.cpp deleted file mode 100644 index 6af03018..00000000 --- a/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.cpp +++ /dev/null @@ -1,84 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2020 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. * -* * -********************************************************************************/ - -// Libraries -#include -#include - -using namespace reactphysics3d; - -// Constructor -CapsuleVsCapsuleNarrowPhaseInfoBatch::CapsuleVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator, - OverlappingPairs& overlappingPairs) - : NarrowPhaseInfoBatch(allocator, overlappingPairs), capsule1Radiuses(allocator), capsule2Radiuses(allocator), - capsule1Heights(allocator), capsule2Heights(allocator) { - -} - -// Add shapes to be tested during narrow-phase collision detection into the batch -void CapsuleVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, CollisionShape* shape2, - const Transform& shape1Transform, const Transform& shape2Transform, bool needToReportContacts, MemoryAllocator &shapeAllocator) { - - NarrowPhaseInfoBatch::addNarrowPhaseInfo(pairId, pairIndex, collider1, collider2, shape1, shape2, shape1Transform, - shape2Transform, needToReportContacts, shapeAllocator); - - assert(shape1->getType() == CollisionShapeType::CAPSULE); - assert(shape2->getType() == CollisionShapeType::CAPSULE); - - const CapsuleShape* capsule1 = static_cast(shape1); - const CapsuleShape* capsule2 = static_cast(shape2); - - capsule1Radiuses.add(capsule1->getRadius()); - capsule2Radiuses.add(capsule2->getRadius()); - capsule1Heights.add(capsule1->getHeight()); - capsule2Heights.add(capsule2->getHeight()); -} - -// Initialize the containers using cached capacity -void CapsuleVsCapsuleNarrowPhaseInfoBatch::reserveMemory() { - - NarrowPhaseInfoBatch::reserveMemory(); - - capsule1Radiuses.reserve(mCachedCapacity); - capsule2Radiuses.reserve(mCachedCapacity); - capsule1Heights.reserve(mCachedCapacity); - capsule2Heights.reserve(mCachedCapacity); -} - -// Clear all the objects in the batch -void CapsuleVsCapsuleNarrowPhaseInfoBatch::clear() { - - // Note that we clear the following containers and we release their allocated memory. Therefore, - // if the memory allocator is a single frame allocator, the memory is deallocated and will be - // allocated in the next frame at a possibly different location in memory (remember that the - // location of the allocated memory of a single frame allocator might change between two frames) - - NarrowPhaseInfoBatch::clear(); - - capsule1Radiuses.clear(true); - capsule2Radiuses.clear(true); - capsule1Heights.clear(true); - capsule2Heights.clear(true); -} diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index 8e082822..49dcbc0b 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -66,21 +66,21 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { // Get the last frame collision info - LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.lastFrameCollisionInfos[batchIndex]; + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].lastFrameCollisionInfo; lastFrameCollisionInfo->wasUsingGJK = true; lastFrameCollisionInfo->wasUsingSAT = false; - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON || - narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE || - narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CAPSULE); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CAPSULE || + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CAPSULE); // If we have found a contact point inside the margins (shallow penetration) if (gjkResults[batchIndex] == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { // If we need to report contacts - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { bool noContact = false; @@ -89,20 +89,20 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar // two contact points instead of a single one (as in the deep contact case with SAT algorithm) // Get the contact point created by GJK - assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() > 0); - ContactPointInfo*& contactPoint = narrowPhaseInfoBatch.contactPoints[batchIndex][0]; + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].nbContactPoints > 0); + ContactPointInfo& contactPoint = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].contactPoints[0]; - bool isCapsuleShape1 = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE; + bool isCapsuleShape1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CAPSULE; // Get the collision shapes - const CapsuleShape* capsuleShape = static_cast(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes1[batchIndex] : narrowPhaseInfoBatch.collisionShapes2[batchIndex]); - const ConvexPolyhedronShape* polyhedron = static_cast(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes2[batchIndex] : narrowPhaseInfoBatch.collisionShapes1[batchIndex]); + const CapsuleShape* capsuleShape = static_cast(isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2); + const ConvexPolyhedronShape* polyhedron = static_cast(isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1); // For each face of the polyhedron for (uint f = 0; f < polyhedron->getNbFaces(); f++) { - const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex]; - const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex]; + const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform; + const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform; // Get the face normal const Vector3 faceNormal = polyhedron->getFaceNormal(f); @@ -113,18 +113,18 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar Vector3 capsuleInnerSegmentDirection = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA); capsuleInnerSegmentDirection.normalize(); - bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint->normal) > decimal(0.0); + bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint.normal) > decimal(0.0); bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal); // If the polyhedron face normal is orthogonal to the capsule inner segment and parallel to the contact point normal and the face normal // is in direction of the contact normal (from the polyhedron point of view). if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentDirection) - && areParallelVectors(faceNormalWorld, contactPoint->normal)) { + && areParallelVectors(faceNormalWorld, contactPoint.normal)) { // Remove the previous contact point computed by GJK narrowPhaseInfoBatch.resetContactPoints(batchIndex); - const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex]; + const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform; const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld; // Compute the end-points of the inner segment of the capsule @@ -143,13 +143,13 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar } // Compute and create two contact points - bool contactsFound = satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth, + bool contactsFound = satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint.penetrationDepth, polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace, capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, narrowPhaseInfoBatch, batchIndex, isCapsuleShape1); if (!contactsFound) { noContact = true; - narrowPhaseInfoBatch.isColliding[batchIndex] = false; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = false; break; } @@ -166,7 +166,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar lastFrameCollisionInfo->wasUsingGJK = false; // Colision found - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; continue; } @@ -175,12 +175,12 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar if (gjkResults[batchIndex] == GJKAlgorithm::GJKResult::INTERPENETRATE) { // Run the SAT algorithm to find the separating axis and compute contact point - narrowPhaseInfoBatch.isColliding[batchIndex] = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfoBatch, batchIndex); + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfoBatch, batchIndex); lastFrameCollisionInfo->wasUsingGJK = false; lastFrameCollisionInfo->wasUsingSAT = true; - if (narrowPhaseInfoBatch.isColliding[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding) { isCollisionFound = true; } } diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp index a44ba93b..9630e975 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp @@ -35,7 +35,7 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between two convex polyhedra // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, +bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch &narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator) { @@ -54,7 +54,7 @@ bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoB for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { // Get the last frame collision info - LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.lastFrameCollisionInfos[batchIndex]; + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].lastFrameCollisionInfo; lastFrameCollisionInfo->wasUsingSAT = true; lastFrameCollisionInfo->wasUsingGJK = false; diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 28c3ff0f..f3d664a7 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -64,15 +64,15 @@ void GJKAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uin decimal prevDistSquare; bool contactFound = false; - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->isConvex()); - assert(narrowPhaseInfoBatch.collisionShapes2[batchIndex]->isConvex()); + assert(narrowPhaseInfoBatch.narrowPhaseInfo[[batchIndex].collisionShape1->isConvex()); + assert(narrowPhaseInfoBatch.narrowPhaseInfo[[batchIndex].collisionShape2->isConvex()); - const ConvexShape* shape1 = static_cast(narrowPhaseInfoBatch.collisionShapes1[batchIndex]); - const ConvexShape* shape2 = static_cast(narrowPhaseInfoBatch.collisionShapes2[batchIndex]); + const ConvexShape* shape1 = static_cast(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1); + const ConvexShape* shape2 = static_cast(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2); // Get the local-space to world-space transforms - const Transform& transform1 = narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex]; - const Transform& transform2 = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex]; + const Transform& transform1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform; + const Transform& transform2 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform; // Transform a point from local space of body 2 to local // space of body 1 (the GJK algorithm is done in local space of body 1) @@ -92,7 +92,7 @@ void GJKAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uin VoronoiSimplex simplex; // Get the last collision frame info - LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.lastFrameCollisionInfos[batchIndex]; + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].lastFrameCollisionInfo; // Get the previous point V (last cached separating axis) Vector3 v; @@ -215,7 +215,7 @@ void GJKAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uin } // If we need to report contacts - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle TriangleShape::computeSmoothTriangleMeshContact(shape1, shape2, pA, pB, transform1, transform2, diff --git a/src/collision/narrowphase/NarrowPhaseInfoBatch.cpp b/src/collision/narrowphase/NarrowPhaseInfoBatch.cpp index ff59d32b..754161fd 100644 --- a/src/collision/narrowphase/NarrowPhaseInfoBatch.cpp +++ b/src/collision/narrowphase/NarrowPhaseInfoBatch.cpp @@ -33,12 +33,8 @@ using namespace reactphysics3d; // Constructor -NarrowPhaseInfoBatch::NarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs) - : mMemoryAllocator(allocator), mOverlappingPairs(overlappingPairs), overlappingPairIds(allocator), - colliderEntities1(allocator), colliderEntities2(allocator), collisionShapes1(allocator), collisionShapes2(allocator), - shape1ToWorldTransforms(allocator), shape2ToWorldTransforms(allocator), reportContacts(allocator), - isColliding(allocator), contactPoints(allocator), collisionShapeAllocators(allocator), - lastFrameCollisionInfos(allocator) { +NarrowPhaseInfoBatch::NarrowPhaseInfoBatch(OverlappingPairs& overlappingPairs, MemoryAllocator& allocator) + : mMemoryAllocator(allocator), mOverlappingPairs(overlappingPairs), narrowPhaseInfos(allocator){ } @@ -48,99 +44,65 @@ NarrowPhaseInfoBatch::~NarrowPhaseInfoBatch() { } // Add shapes to be tested during narrow-phase collision detection into the batch -void NarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, CollisionShape* shape2, - const Transform& shape1Transform, const Transform& shape2Transform, bool needToReportContacts, - MemoryAllocator& shapeAllocator) { - - overlappingPairIds.add(pairId); - colliderEntities1.add(collider1); - colliderEntities2.add(collider2); - collisionShapes1.add(shape1); - collisionShapes2.add(shape2); - shape1ToWorldTransforms.add(shape1Transform); - shape2ToWorldTransforms.add(shape2Transform); - reportContacts.add(needToReportContacts); - collisionShapeAllocators.add(&shapeAllocator); - contactPoints.add(List(mMemoryAllocator)); - isColliding.add(false); +void NarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, + CollisionShape* shape2, const Transform& shape1Transform, const Transform& shape2Transform, + bool needToReportContacts, MemoryAllocator& shapeAllocator) { // Add a collision info for the two collision shapes into the overlapping pair (if not present yet) + // TODO OPTI : Can we better manage this LastFrameCollisionInfo* lastFrameInfo = mOverlappingPairs.addLastFrameInfoIfNecessary(pairIndex, shape1->getId(), shape2->getId()); - lastFrameCollisionInfos.add(lastFrameInfo); + + // Create a meta data object + narrowPhaseInfos.emplace(pairId, collider1, collider2, lastFrameInfo, shapeAllocator, shape1Transform, shape2Transform, shape1, shape2, needToReportContacts); } // Add a new contact point -void NarrowPhaseInfoBatch::addContactPoint(uint index, const Vector3& contactNormal, decimal penDepth, - const Vector3& localPt1, const Vector3& localPt2) { +void NarrowPhaseInfoBatch::addContactPoint(uint index, const Vector3& contactNormal, decimal penDepth, const Vector3& localPt1, const Vector3& localPt2) { - assert(reportContacts[index]); assert(penDepth > decimal(0.0)); - // Get the memory allocator - MemoryAllocator& allocator = mOverlappingPairs.getTemporaryAllocator(); + if (narrowPhaseInfos[index].nbContactPoints < NB_MAX_CONTACT_POINTS_IN_NARROWPHASE_INFO) { - // Create the contact point info - ContactPointInfo* contactPointInfo = new (allocator.allocate(sizeof(ContactPointInfo))) - ContactPointInfo(contactNormal, penDepth, localPt1, localPt2); + assert(contactNormal.length() > 0.8f); - // Add it into the list of contact points - contactPoints[index].add(contactPointInfo); + // Add it into the array of contact points + narrowPhaseInfos[index].contactPoints[narrowPhaseInfos[index].nbContactPoints].normal = contactNormal; + narrowPhaseInfos[index].contactPoints[narrowPhaseInfos[index].nbContactPoints].penetrationDepth = penDepth; + narrowPhaseInfos[index].contactPoints[narrowPhaseInfos[index].nbContactPoints].localPoint1 = localPt1; + narrowPhaseInfos[index].contactPoints[narrowPhaseInfos[index].nbContactPoints].localPoint2 = localPt2; + narrowPhaseInfos[index].nbContactPoints++; + } } // Reset the remaining contact points void NarrowPhaseInfoBatch::resetContactPoints(uint index) { - // Get the memory allocator - MemoryAllocator& allocator = mOverlappingPairs.getTemporaryAllocator(); - - // For each remaining contact point info - for (uint i=0; i < contactPoints[index].size(); i++) { - - ContactPointInfo* contactPoint = contactPoints[index][i]; - - // Call the destructor - contactPoint->~ContactPointInfo(); - - // Delete the current element - allocator.release(contactPoint, sizeof(ContactPointInfo)); - } - - contactPoints[index].clear(); + narrowPhaseInfos[index].nbContactPoints = 0; } // Initialize the containers using cached capacity void NarrowPhaseInfoBatch::reserveMemory() { - overlappingPairIds.reserve(mCachedCapacity); - colliderEntities1.reserve(mCachedCapacity); - colliderEntities2.reserve(mCachedCapacity); - collisionShapes1.reserve(mCachedCapacity); - collisionShapes2.reserve(mCachedCapacity); - shape1ToWorldTransforms.reserve(mCachedCapacity); - shape2ToWorldTransforms.reserve(mCachedCapacity); - reportContacts.reserve(mCachedCapacity); - collisionShapeAllocators.reserve(mCachedCapacity); - lastFrameCollisionInfos.reserve(mCachedCapacity); - isColliding.reserve(mCachedCapacity); - contactPoints.reserve(mCachedCapacity); + narrowPhaseInfos.reserve(mCachedCapacity); } // Clear all the objects in the batch void NarrowPhaseInfoBatch::clear() { - for (uint i=0; i < overlappingPairIds.size(); i++) { + // TODO OPTI : Better manage this + for (uint i=0; i < narrowPhaseInfos.size(); i++) { - assert(contactPoints[i].size() == 0); + assert(narrowPhaseOutputInfos[i].nbContactPoints == 0); // Release the memory of the TriangleShape (this memory was allocated in the // MiddlePhaseTriangleCallback::testTriangle() method) - if (collisionShapes1.size() > 0 && collisionShapes1[i]->getName() == CollisionShapeName::TRIANGLE) { - collisionShapes1[i]->~CollisionShape(); - collisionShapeAllocators[i]->release(collisionShapes1[i], sizeof(TriangleShape)); + if (narrowPhaseInfos[i].collisionShape1->getName() == CollisionShapeName::TRIANGLE) { + narrowPhaseInfos[i].collisionShape1->~CollisionShape(); + narrowPhaseInfos[i].collisionShapeAllocator->release(narrowPhaseInfos[i].collisionShape1, sizeof(TriangleShape)); } - if (collisionShapes2.size() > 0 && collisionShapes2[i]->getName() == CollisionShapeName::TRIANGLE) { - collisionShapes2[i]->~CollisionShape(); - collisionShapeAllocators[i]->release(collisionShapes2[i], sizeof(TriangleShape)); + if (narrowPhaseInfos[i].collisionShape2->getName() == CollisionShapeName::TRIANGLE) { + narrowPhaseInfos[i].collisionShape2->~CollisionShape(); + narrowPhaseInfos[i].collisionShapeAllocator->release(narrowPhaseInfos[i].collisionShape2, sizeof(TriangleShape)); } } @@ -149,18 +111,7 @@ void NarrowPhaseInfoBatch::clear() { // allocated in the next frame at a possibly different location in memory (remember that the // location of the allocated memory of a single frame allocator might change between two frames) - mCachedCapacity = overlappingPairIds.size(); + mCachedCapacity = narrowPhaseInfos.size(); - overlappingPairIds.clear(true); - colliderEntities1.clear(true); - colliderEntities2.clear(true); - collisionShapes1.clear(true); - collisionShapes2.clear(true); - shape1ToWorldTransforms.clear(true); - shape2ToWorldTransforms.clear(true); - reportContacts.clear(true); - collisionShapeAllocators.clear(true); - lastFrameCollisionInfos.clear(true); - isColliding.clear(true); - contactPoints.clear(true); + narrowPhaseInfos.clear(true); } diff --git a/src/collision/narrowphase/NarrowPhaseInput.cpp b/src/collision/narrowphase/NarrowPhaseInput.cpp index 767fd3c2..454717f3 100644 --- a/src/collision/narrowphase/NarrowPhaseInput.cpp +++ b/src/collision/narrowphase/NarrowPhaseInput.cpp @@ -31,10 +31,10 @@ using namespace reactphysics3d; /// Constructor NarrowPhaseInput::NarrowPhaseInput(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs) - :mSphereVsSphereBatch(allocator, overlappingPairs), mSphereVsCapsuleBatch(allocator, overlappingPairs), - mCapsuleVsCapsuleBatch(allocator, overlappingPairs), mSphereVsConvexPolyhedronBatch(allocator, overlappingPairs), - mCapsuleVsConvexPolyhedronBatch(allocator, overlappingPairs), - mConvexPolyhedronVsConvexPolyhedronBatch(allocator, overlappingPairs) { + :mSphereVsSphereBatch(overlappingPairs, allocator), mSphereVsCapsuleBatch(overlappingPairs, allocator), + mCapsuleVsCapsuleBatch(overlappingPairs, allocator), mSphereVsConvexPolyhedronBatch(overlappingPairs, allocator), + mCapsuleVsConvexPolyhedronBatch(overlappingPairs, allocator), + mConvexPolyhedronVsConvexPolyhedronBatch(overlappingPairs, allocator) { } diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 4271b900..22118b78 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -64,19 +64,19 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& n for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { - bool isSphereShape1 = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE; + bool isSphereShape1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::SPHERE; - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON || - narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE || - narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::SPHERE); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::SPHERE || + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::SPHERE); // Get the capsule collision shapes - const SphereShape* sphere = static_cast(isSphereShape1 ? narrowPhaseInfoBatch.collisionShapes1[batchIndex] : narrowPhaseInfoBatch.collisionShapes2[batchIndex]); - const ConvexPolyhedronShape* polyhedron = static_cast(isSphereShape1 ? narrowPhaseInfoBatch.collisionShapes2[batchIndex] : narrowPhaseInfoBatch.collisionShapes1[batchIndex]); + const SphereShape* sphere = static_cast(isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2); + const ConvexPolyhedronShape* polyhedron = static_cast(isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1); - const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex]; - const Transform& polyhedronToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex]; + const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform; + const Transform& polyhedronToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform; // Get the transform from sphere local-space to polyhedron local-space const Transform worldToPolyhedronTransform = polyhedronToWorldTransform.getInverse(); @@ -115,7 +115,7 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& n } // If we need to report contacts - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex); Vector3 minFaceNormalWorld = polyhedronToWorldTransform.getOrientation() * minFaceNormal; @@ -125,10 +125,10 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& n Vector3 normalWorld = isSphereShape1 ? -minFaceNormalWorld : minFaceNormalWorld; // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle - TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex], + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2, isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal, - narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex], + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform, minPenetrationDepth, normalWorld); // Create the contact info object @@ -137,7 +137,7 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& n isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal); } - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; } @@ -167,19 +167,19 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfoBatch& RP3D_PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()", mProfiler); - bool isCapsuleShape1 = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE; + bool isCapsuleShape1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CAPSULE; - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON || - narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE || - narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CAPSULE); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CAPSULE || + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CAPSULE); // Get the collision shapes - const CapsuleShape* capsuleShape = static_cast(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes1[batchIndex] : narrowPhaseInfoBatch.collisionShapes2[batchIndex]); - const ConvexPolyhedronShape* polyhedron = static_cast(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes2[batchIndex] : narrowPhaseInfoBatch.collisionShapes1[batchIndex]); + const CapsuleShape* capsuleShape = static_cast(isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2); + const ConvexPolyhedronShape* polyhedron = static_cast(isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1); - const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex]; - const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex]; + const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform; + const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform; const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld; @@ -279,7 +279,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfoBatch& if (isMinPenetrationFaceNormal) { // If we need to report contacts - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { return computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth, polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace, @@ -290,7 +290,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfoBatch& else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment // If we need to report contacts - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { // Compute the closest points between the inner capsule segment and the // edge of the polyhedron in polyhedron local-space @@ -303,10 +303,10 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfoBatch& Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius; // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle - TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex], + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2, isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge, isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule, - narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex], + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform, minPenetrationDepth, normalWorld); // Create the contact point @@ -448,10 +448,10 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle - TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex], + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2, isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron, isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule, - narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex], + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform, penetrationDepth, normalWorld); @@ -486,14 +486,14 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].nbContactPoints == 0); - const ConvexPolyhedronShape* polyhedron1 = static_cast(narrowPhaseInfoBatch.collisionShapes1[batchIndex]); - const ConvexPolyhedronShape* polyhedron2 = static_cast(narrowPhaseInfoBatch.collisionShapes2[batchIndex]); + const ConvexPolyhedronShape* polyhedron1 = static_cast(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1); + const ConvexPolyhedronShape* polyhedron2 = static_cast(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2); - const Transform polyhedron1ToPolyhedron2 = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getInverse() * narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex]; + const Transform polyhedron1ToPolyhedron2 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getInverse() * narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform; const Transform polyhedron2ToPolyhedron1 = polyhedron1ToPolyhedron2.getInverse(); decimal minPenetrationDepth = DECIMAL_LARGEST; @@ -507,7 +507,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space; const bool isShape1Triangle = polyhedron1->getName() == CollisionShapeName::TRIANGLE; - LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.lastFrameCollisionInfos[batchIndex]; + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].lastFrameCollisionInfo; // If the last frame collision info is valid and was also using SAT algorithm if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingSAT) { @@ -549,7 +549,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn // The shapes are still overlapping in the previous axis (the contact manifold is not empty). // Therefore, we can return without running the whole SAT algorithm - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; continue; } @@ -589,7 +589,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn // The shapes are still overlapping in the previous axis (the contact manifold is not empty). // Therefore, we can return without running the whole SAT algorithm - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; continue; } @@ -651,18 +651,18 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn if (t1 >= decimal(0.0) && t1 <= decimal(1) && t2 >= decimal(0.0) && t2 <= decimal(1.0)) { // If we need to report contact points - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { // Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1 Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge; // Compute the world normal - Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * separatingAxisPolyhedron2Space; + Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * separatingAxisPolyhedron2Space; // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle - TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex], + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2, closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge, - narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex], + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform, penetrationDepth, normalWorld); // Create the contact point @@ -673,7 +673,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn // The shapes are overlapping on the previous axis (the contact manifold is not empty). Therefore // we return without running the whole SAT algorithm - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; continue; } @@ -848,7 +848,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn else { // If we have an edge vs edge contact // If we need to report contacts - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { // Compute the closest points between the two edges (in the local-space of poylhedron 2) Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge; @@ -859,12 +859,12 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge; // Compute the world normal - Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space; + Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space; // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle - TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex], + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2, closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge, - narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex], + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform, minPenetrationDepth, normalWorld); // Create the contact point @@ -878,7 +878,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn lastFrameCollisionInfo->satMinEdge2Index = minSeparatingEdge2Index; } - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; } @@ -903,8 +903,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace; // Compute the world normal - Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex].getOrientation() * axisReferenceSpace : - -(narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * axisReferenceSpace); + Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform.getOrientation() * axisReferenceSpace : + -(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * axisReferenceSpace); // Get the reference face const HalfEdgeStructure::Face& referenceFace = referencePolyhedron->getFace(minFaceIndex); @@ -975,7 +975,7 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene contactPointsFound = true; // If we need to report contacts - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { Vector3 outWorldNormal = normalWorld; @@ -986,10 +986,10 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(clipPolygonVertices[i], axisReferenceSpace, referenceFaceVertex); // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle - TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex], + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2, isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron, - narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex], + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform, penetrationDepth, outWorldNormal); // Create a new contact point diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index a441cbca..d839f7cb 100755 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -27,7 +27,7 @@ #include #include #include -#include +#include // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; @@ -35,21 +35,27 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between a sphere and a capsule // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, - MemoryAllocator& memoryAllocator) { +bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, MemoryAllocator& memoryAllocator) { bool isCollisionFound = false; for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { - assert(!narrowPhaseInfoBatch.isColliding[batchIndex]); - assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0); + assert(!narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].nbContactPoints == 0); - const bool isSphereShape1 = narrowPhaseInfoBatch.isSpheresShape1[batchIndex]; + const bool isSphereShape1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::SPHERE; + + const SphereShape* sphereShape = static_cast(isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2); + const CapsuleShape* capsuleShape = static_cast(isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1); + + const decimal capsuleHeight = capsuleShape->getHeight(); + const decimal sphereRadius = sphereShape->getRadius(); + const decimal capsuleRadius = capsuleShape->getRadius(); // Get the transform from sphere local-space to capsule local-space - const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex]; - const Transform& capsuleToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex]; + const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform; + const Transform& capsuleToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform; const Transform worldToCapsuleTransform = capsuleToWorldTransform.getInverse(); const Transform sphereToCapsuleSpaceTransform = worldToCapsuleTransform * sphereToWorldTransform; @@ -57,7 +63,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch const Vector3 sphereCenter = sphereToCapsuleSpaceTransform.getPosition(); // Compute the end-points of the inner segment of the capsule - const decimal capsuleHalfHeight = narrowPhaseInfoBatch.capsuleHeights[batchIndex] * decimal(0.5); + const decimal capsuleHalfHeight = capsuleHeight * decimal(0.5); const Vector3 capsuleSegA(0, -capsuleHalfHeight, 0); const Vector3 capsuleSegB(0, capsuleHalfHeight, 0); @@ -69,7 +75,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch const decimal sphereSegmentDistanceSquare = sphereCenterToSegment.lengthSquare(); // Compute the sum of the radius of the sphere and the capsule (virtual sphere) - decimal sumRadius = narrowPhaseInfoBatch.sphereRadiuses[batchIndex] + narrowPhaseInfoBatch.capsuleRadiuses[batchIndex]; + decimal sumRadius = sphereRadius + capsuleRadius; // If the collision shapes overlap if (sphereSegmentDistanceSquare < sumRadius * sumRadius) { @@ -80,7 +86,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch Vector3 contactPointCapsuleLocal; // If we need to report contacts - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { // If the sphere center is not on the capsule inner segment if (sphereSegmentDistanceSquare > MACHINE_EPSILON) { @@ -88,8 +94,8 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare); sphereCenterToSegment /= sphereSegmentDistance; - contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * narrowPhaseInfoBatch.sphereRadiuses[batchIndex]); - contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * narrowPhaseInfoBatch.capsuleRadiuses[batchIndex]; + contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereRadius); + contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleRadius; normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment; @@ -120,8 +126,8 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch normalWorld = capsuleToWorldTransform.getOrientation() * normalCapsuleSpace; // Compute the two local contact points - contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + normalCapsuleSpace * narrowPhaseInfoBatch.sphereRadiuses[batchIndex]); - contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * narrowPhaseInfoBatch.capsuleRadiuses[batchIndex]; + contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + normalCapsuleSpace * sphereRadius); + contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * capsuleRadius; } if (penetrationDepth <= decimal(0.0)) { @@ -136,7 +142,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal); } - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; continue; } diff --git a/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.cpp b/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.cpp deleted file mode 100644 index e4433b0d..00000000 --- a/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.cpp +++ /dev/null @@ -1,88 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2020 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. * -* * -********************************************************************************/ - -// Libraries -#include -#include -#include - -using namespace reactphysics3d; - -// Constructor -SphereVsCapsuleNarrowPhaseInfoBatch::SphereVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator, - OverlappingPairs& overlappingPairs) - : NarrowPhaseInfoBatch(allocator, overlappingPairs), isSpheresShape1(allocator), sphereRadiuses(allocator), capsuleRadiuses(allocator), - capsuleHeights(allocator) { - -} - -// Add shapes to be tested during narrow-phase collision detection into the batch -void SphereVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, CollisionShape* shape2, - const Transform& shape1Transform, const Transform& shape2Transform, - bool needToReportContacts, MemoryAllocator& shapeAllocator) { - - NarrowPhaseInfoBatch::addNarrowPhaseInfo(pairId, pairIndex, collider1, collider2, shape1, shape2, shape1Transform, - shape2Transform, needToReportContacts, shapeAllocator); - - bool isSphereShape1 = shape1->getType() == CollisionShapeType::SPHERE; - isSpheresShape1.add(isSphereShape1); - - assert(isSphereShape1 || shape1->getType() == CollisionShapeType::CAPSULE); - - // Get the collision shapes - const SphereShape* sphereShape = static_cast(isSphereShape1 ? shape1 : shape2); - const CapsuleShape* capsuleShape = static_cast(isSphereShape1 ? shape2 : shape1); - - sphereRadiuses.add(sphereShape->getRadius()); - capsuleRadiuses.add(capsuleShape->getRadius()); - capsuleHeights.add(capsuleShape->getHeight()); -} - -// Initialize the containers using cached capacity -void SphereVsCapsuleNarrowPhaseInfoBatch::reserveMemory() { - - NarrowPhaseInfoBatch::reserveMemory(); - - isSpheresShape1.reserve(mCachedCapacity); - sphereRadiuses.reserve(mCachedCapacity); - capsuleRadiuses.reserve(mCachedCapacity); - capsuleHeights.reserve(mCachedCapacity); -} - -// Clear all the objects in the batch -void SphereVsCapsuleNarrowPhaseInfoBatch::clear() { - - // Note that we clear the following containers and we release their allocated memory. Therefore, - // if the memory allocator is a single frame allocator, the memory is deallocated and will be - // allocated in the next frame at a possibly different location in memory (remember that the - // location of the allocated memory of a single frame allocator might change between two frames) - - NarrowPhaseInfoBatch::clear(); - - isSpheresShape1.clear(true); - sphereRadiuses.clear(true); - capsuleRadiuses.clear(true); - capsuleHeights.clear(true); -} diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index 0c0f609b..6e732d12 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -57,13 +57,13 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narr // For each item in the batch for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON || - narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE || - narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::SPHERE); + assert(narrowPhaseInfoBatch.narrowPhaseInfo[[batchIndex].collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || + narrowPhaseInfoBatch.narrowPhaseInfo[[batchIndex].collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfoBatch.narrowPhaseInfo[[batchIndex].collisionShape1->getType() == CollisionShapeType::SPHERE || + narrowPhaseInfoBatch.narrowPhaseInfo[[batchIndex].collisionShape2->getType() == CollisionShapeType::SPHERE); // Get the last frame collision info - LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.lastFrameCollisionInfos[batchIndex]; + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].lastFrameCollisionInfo; lastFrameCollisionInfo->wasUsingGJK = true; lastFrameCollisionInfo->wasUsingSAT = false; @@ -72,7 +72,7 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narr if (gjkResults[batchIndex] == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { // Return true - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; continue; } diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 943ae89d..78f817db 100755 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -26,31 +26,37 @@ // Libraries #include #include -#include +#include // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -bool SphereVsSphereAlgorithm::testCollision(SphereVsSphereNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, MemoryAllocator& memoryAllocator) { +bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, MemoryAllocator& memoryAllocator) { bool isCollisionFound = false; // For each item in the batch for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { - assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0); - assert(!narrowPhaseInfoBatch.isColliding[batchIndex]); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].nbContactPoints == 0); + assert(!narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding); // Get the local-space to world-space transforms - const Transform& transform1 = narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex]; - const Transform& transform2 = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex]; + const Transform& transform1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform; + const Transform& transform2 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform; // Compute the distance between the centers Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare(); + const SphereShape* sphereShape1 = static_cast(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1); + const SphereShape* sphereShape2 = static_cast(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2); + + const decimal sphere1Radius = sphereShape1->getRadius(); + const decimal sphere2Radius = sphereShape2->getRadius(); + // Compute the sum of the radius - decimal sumRadiuses = narrowPhaseInfoBatch.sphere1Radiuses[batchIndex] + narrowPhaseInfoBatch.sphere2Radiuses[batchIndex]; + decimal sumRadiuses = sphere1Radius + sphere2Radius; // Compute the product of the sum of the radius decimal sumRadiusesProducts = sumRadiuses * sumRadiuses; @@ -59,7 +65,7 @@ bool SphereVsSphereAlgorithm::testCollision(SphereVsSphereNarrowPhaseInfoBatch& if (squaredDistanceBetweenCenters < sumRadiusesProducts) { // If we need to report contacts - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { const Transform transform1Inverse = transform1.getInverse(); const Transform transform2Inverse = transform2.getInverse(); @@ -75,8 +81,8 @@ bool SphereVsSphereAlgorithm::testCollision(SphereVsSphereNarrowPhaseInfoBatch& Vector3 centerSphere2InBody1LocalSpace = transform1Inverse * transform2.getPosition(); Vector3 centerSphere1InBody2LocalSpace = transform2Inverse * transform1.getPosition(); - intersectionOnBody1 = narrowPhaseInfoBatch.sphere1Radiuses[batchIndex] * centerSphere2InBody1LocalSpace.getUnit(); - intersectionOnBody2 = narrowPhaseInfoBatch.sphere2Radiuses[batchIndex] * centerSphere1InBody2LocalSpace.getUnit(); + intersectionOnBody1 = sphere1Radius * centerSphere2InBody1LocalSpace.getUnit(); + intersectionOnBody2 = sphere2Radius * centerSphere1InBody2LocalSpace.getUnit(); normal = vectorBetweenCenters.getUnit(); } else { // If the sphere centers are at the same position (degenerate case) @@ -84,15 +90,15 @@ bool SphereVsSphereAlgorithm::testCollision(SphereVsSphereNarrowPhaseInfoBatch& // Take any contact normal direction normal.setAllValues(0, 1, 0); - intersectionOnBody1 = narrowPhaseInfoBatch.sphere1Radiuses[batchIndex] * (transform1Inverse.getOrientation() * normal); - intersectionOnBody2 = narrowPhaseInfoBatch.sphere2Radiuses[batchIndex] * (transform2Inverse.getOrientation() * normal); + intersectionOnBody1 = sphere1Radius * (transform1Inverse.getOrientation() * normal); + intersectionOnBody2 = sphere2Radius * (transform2Inverse.getOrientation() * normal); } // Create the contact info object narrowPhaseInfoBatch.addContactPoint(batchIndex, normal, penetrationDepth, intersectionOnBody1, intersectionOnBody2); } - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; } } diff --git a/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp b/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp deleted file mode 100644 index 36970cd2..00000000 --- a/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2020 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. * -* * -********************************************************************************/ - -// Libraries -#include -#include - -using namespace reactphysics3d; - -// Constructor -SphereVsSphereNarrowPhaseInfoBatch::SphereVsSphereNarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs) - : NarrowPhaseInfoBatch(allocator, overlappingPairs), sphere1Radiuses(allocator), sphere2Radiuses(allocator) { - -} - -// Add shapes to be tested during narrow-phase collision detection into the batch -void SphereVsSphereNarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, CollisionShape* shape2, - const Transform& shape1Transform, const Transform& shape2Transform, bool needToReportContacts, MemoryAllocator &shapeAllocator) { - - NarrowPhaseInfoBatch::addNarrowPhaseInfo(pairId, pairIndex, collider1, collider2, shape1, shape2, shape1Transform, - shape2Transform, needToReportContacts, shapeAllocator); - - assert(shape1->getType() == CollisionShapeType::SPHERE); - assert(shape2->getType() == CollisionShapeType::SPHERE); - - const SphereShape* sphere1 = static_cast(shape1); - const SphereShape* sphere2 = static_cast(shape2); - - sphere1Radiuses.add(sphere1->getRadius()); - sphere2Radiuses.add(sphere2->getRadius()); -} - -// Initialize the containers using cached capacity -void SphereVsSphereNarrowPhaseInfoBatch::reserveMemory() { - - NarrowPhaseInfoBatch::reserveMemory(); - - sphere1Radiuses.reserve(mCachedCapacity); - sphere2Radiuses.reserve(mCachedCapacity); -} - -// Clear all the objects in the batch -void SphereVsSphereNarrowPhaseInfoBatch::clear() { - - // Note that we clear the following containers and we release their allocated memory. Therefore, - // if the memory allocator is a single frame allocator, the memory is deallocated and will be - // allocated in the next frame at a possibly different location in memory (remember that the - // location of the allocated memory of a single frame allocator might change between two frames) - - NarrowPhaseInfoBatch::clear(); - - sphere1Radiuses.clear(true); - sphere2Radiuses.clear(true); -} - diff --git a/src/systems/CollisionDetectionSystem.cpp b/src/systems/CollisionDetectionSystem.cpp index e11f7e86..4d28ae89 100644 --- a/src/systems/CollisionDetectionSystem.cpp +++ b/src/systems/CollisionDetectionSystem.cpp @@ -445,9 +445,9 @@ bool CollisionDetectionSystem::testNarrowPhaseCollision(NarrowPhaseInput& narrow ConvexPolyhedronVsConvexPolyhedronAlgorithm* convexPolyVsConvexPolyAlgo = mCollisionDispatch.getConvexPolyhedronVsConvexPolyhedronAlgorithm(); // get the narrow-phase batches to test for collision for contacts - SphereVsSphereNarrowPhaseInfoBatch& sphereVsSphereBatchContacts = narrowPhaseInput.getSphereVsSphereBatch(); - SphereVsCapsuleNarrowPhaseInfoBatch& sphereVsCapsuleBatchContacts = narrowPhaseInput.getSphereVsCapsuleBatch(); - CapsuleVsCapsuleNarrowPhaseInfoBatch& capsuleVsCapsuleBatchContacts = narrowPhaseInput.getCapsuleVsCapsuleBatch(); + NarrowPhaseInfoBatch& sphereVsSphereBatchContacts = narrowPhaseInput.getSphereVsSphereBatch(); + NarrowPhaseInfoBatch& sphereVsCapsuleBatchContacts = narrowPhaseInput.getSphereVsCapsuleBatch(); + NarrowPhaseInfoBatch& capsuleVsCapsuleBatchContacts = narrowPhaseInput.getCapsuleVsCapsuleBatch(); NarrowPhaseInfoBatch& sphereVsConvexPolyhedronBatchContacts = narrowPhaseInput.getSphereVsConvexPolyhedronBatch(); NarrowPhaseInfoBatch& capsuleVsConvexPolyhedronBatchContacts = narrowPhaseInput.getCapsuleVsConvexPolyhedronBatch(); NarrowPhaseInfoBatch& convexPolyhedronVsConvexPolyhedronBatchContacts = narrowPhaseInput.getConvexPolyhedronVsConvexPolyhedronBatch(); @@ -482,7 +482,6 @@ void CollisionDetectionSystem::processAllPotentialContacts(NarrowPhaseInput& nar List* contactPairs) { assert(contactPairs->size() == 0); - assert(mapPairIdToContactPairIndex->size() == 0); Map mapPairIdToContactPairIndex(mMemoryManager.getHeapAllocator(), mPreviousMapPairIdToContactPairIndex.size()); @@ -618,13 +617,13 @@ void CollisionDetectionSystem::computeOverlapSnapshotContactPairs(NarrowPhaseInf for(uint i=0; i < narrowPhaseInfoBatch.getNbObjects(); i++) { // If there is a collision - if (narrowPhaseInfoBatch.isColliding[i]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[i].isColliding) { // If the contact pair does not already exist - if (!setOverlapContactPairId.contains(narrowPhaseInfoBatch.overlappingPairIds[i])) { + if (!setOverlapContactPairId.contains(narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId)) { - const Entity collider1Entity = narrowPhaseInfoBatch.colliderEntities1[i]; - const Entity collider2Entity = narrowPhaseInfoBatch.colliderEntities2[i]; + const Entity collider1Entity = narrowPhaseInfoBatch.narrowPhaseInfos[i].colliderEntity1; + const Entity collider2Entity = narrowPhaseInfoBatch.narrowPhaseInfos[i].colliderEntity2; const uint32 collider1Index = mCollidersComponents.getEntityIndex(collider1Entity); const uint32 collider2Index = mCollidersComponents.getEntityIndex(collider2Entity); @@ -635,10 +634,10 @@ void CollisionDetectionSystem::computeOverlapSnapshotContactPairs(NarrowPhaseInf const bool isTrigger = mCollidersComponents.mIsTrigger[collider1Index] || mCollidersComponents.mIsTrigger[collider2Index]; // Create a new contact pair - ContactPair contactPair(narrowPhaseInfoBatch.overlappingPairIds[i], body1Entity, body2Entity, collider1Entity, collider2Entity, contactPairs.size(), isTrigger, false); + ContactPair contactPair(narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId, body1Entity, body2Entity, collider1Entity, collider2Entity, contactPairs.size(), isTrigger, false); contactPairs.add(contactPair); - setOverlapContactPairId.add(narrowPhaseInfoBatch.overlappingPairIds[i]); + setOverlapContactPairId.add(narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId); } } @@ -993,26 +992,31 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na RP3D_PROFILE("CollisionDetectionSystem::processPotentialContacts()", mProfiler); + if (updateLastFrameInfo) { + + // For each narrow phase info object + for(uint i=0; i < narrowPhaseInfoBatch.getNbObjects(); i++) { + + narrowPhaseInfoBatch.narrowPhaseInfos[i].lastFrameCollisionInfo->wasColliding = narrowPhaseInfoBatch.narrowPhaseInfos[i].isColliding; + + // The previous frame collision info is now valid + narrowPhaseInfoBatch.narrowPhaseInfos[i].lastFrameCollisionInfo->isValid = true; + } + } + // For each narrow phase info object for(uint i=0; i < narrowPhaseInfoBatch.getNbObjects(); i++) { - if (updateLastFrameInfo) { - narrowPhaseInfoBatch.lastFrameCollisionInfos[i]->wasColliding = narrowPhaseInfoBatch.isColliding[i]; - - // The previous frame collision info is now valid - narrowPhaseInfoBatch.lastFrameCollisionInfos[i]->isValid = true; - } - - const uint64 pairId = narrowPhaseInfoBatch.overlappingPairIds[i]; + const uint64 pairId = narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId; const uint64 pairIndex = mOverlappingPairs.mMapPairIdToPairIndex[pairId]; // If the two colliders are colliding - if (narrowPhaseInfoBatch.isColliding[i]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[i].isColliding) { mOverlappingPairs.mCollidingInCurrentFrame[pairIndex] = true; - const Entity collider1Entity = narrowPhaseInfoBatch.colliderEntities1[i]; - const Entity collider2Entity = narrowPhaseInfoBatch.colliderEntities2[i]; + const Entity collider1Entity = narrowPhaseInfoBatch.narrowPhaseInfos[i].colliderEntity1; + const Entity collider2Entity = narrowPhaseInfoBatch.narrowPhaseInfos[i].colliderEntity2; const uint32 collider1Index = mCollidersComponents.getEntityIndex(collider1Entity); const uint32 collider2Index = mCollidersComponents.getEntityIndex(collider2Entity); @@ -1045,7 +1049,7 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na const uint contactPointIndexStart = static_cast(potentialContactPoints.size()); // Add the potential contacts - for (uint j=0; j < narrowPhaseInfoBatch.contactPoints[i].size(); j++) { + for (uint j=0; j < narrowPhaseInfoBatch.narrowPhaseInfos[i].nbContactPoints; j++) { if (contactManifoldInfo.nbPotentialContactPoints < NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD) { @@ -1054,14 +1058,14 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na contactManifoldInfo.nbPotentialContactPoints++; // Add the contact point to the list of potential contact points - const ContactPointInfo& contactPoint = *(narrowPhaseInfoBatch.contactPoints[i][j]); + const ContactPointInfo& contactPoint = narrowPhaseInfoBatch.narrowPhaseInfos[i].contactPoints[j]; potentialContactPoints.add(contactPoint); } } // Add the contact manifold to the overlapping pair contact - assert(overlappingPairContact.pairId == contactManifoldInfo.pairId); + assert(pairId == contactManifoldInfo.pairId); pairContact->potentialContactManifoldsIndices[0] = contactManifoldIndex; pairContact->nbPotentialContactManifolds = 1; } @@ -1096,9 +1100,9 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na assert(pairContact != nullptr); // Add the potential contacts - for (uint j=0; j < narrowPhaseInfoBatch.contactPoints[i].size(); j++) { + for (uint j=0; j < narrowPhaseInfoBatch.narrowPhaseInfos[i].nbContactPoints; j++) { - const ContactPointInfo& contactPoint = *(narrowPhaseInfoBatch.contactPoints[i][j]); + const ContactPointInfo& contactPoint = narrowPhaseInfoBatch.narrowPhaseInfos[i].contactPoints[j]; // Add the contact point to the list of potential contact points const uint contactPointIndex = static_cast(potentialContactPoints.size());