From 6413d479ad95e4e083cf5701c5c09d133ca3355a Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 27 Nov 2018 07:27:38 +0100 Subject: [PATCH] Create SphereVsSphereNarrowPhaseInfoBatch class --- CMakeLists.txt | 8 +- src/collision/CollisionDetection.cpp | 4 +- src/collision/ContactManifoldSet.cpp | 2 +- .../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 2 +- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 2 +- ...xPolyhedronVsConvexPolyhedronAlgorithm.cpp | 2 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 2 +- .../NarrowPhaseInfoBatch.cpp | 7 +- .../{ => narrowphase}/NarrowPhaseInfoBatch.h | 9 +- .../narrowphase/NarrowPhaseInput.cpp | 2 +- src/collision/narrowphase/NarrowPhaseInput.h | 9 +- .../narrowphase/SAT/SATAlgorithm.cpp | 2 +- .../narrowphase/SphereVsCapsuleAlgorithm.cpp | 2 +- .../SphereVsConvexPolyhedronAlgorithm.cpp | 2 +- .../narrowphase/SphereVsSphereAlgorithm.cpp | 40 ++++----- .../narrowphase/SphereVsSphereAlgorithm.h | 3 +- .../SphereVsSphereNarrowPhaseInfoBatch.cpp | 88 +++++++++++++++++++ .../SphereVsSphereNarrowPhaseInfoBatch.h | 78 ++++++++++++++++ 18 files changed, 218 insertions(+), 46 deletions(-) rename src/collision/{ => narrowphase}/NarrowPhaseInfoBatch.cpp (95%) rename src/collision/{ => narrowphase}/NarrowPhaseInfoBatch.h (95%) create mode 100644 src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp create mode 100644 src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h diff --git a/CMakeLists.txt b/CMakeLists.txt index ce20060a..e17e0e35 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -94,8 +94,9 @@ SET (REACTPHYSICS3D_HEADERS "src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h" "src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h" "src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h" - "src/collision/narrowphase/SphereVsSphereNarrowPhaseInputBatch.h" "src/collision/narrowphase/NarrowPhaseInput.h" + "src/collision/narrowphase/NarrowPhaseInfoBatch.h" + "src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h" "src/collision/shapes/AABB.h" "src/collision/shapes/ConvexShape.h" "src/collision/shapes/ConvexPolyhedronShape.h" @@ -116,7 +117,6 @@ SET (REACTPHYSICS3D_HEADERS "src/collision/PolyhedronMesh.h" "src/collision/HalfEdgeStructure.h" "src/collision/CollisionDetection.h" - "src/collision/NarrowPhaseInfoBatch.h" "src/collision/ContactManifold.h" "src/collision/ContactManifoldSet.h" "src/collision/MiddlePhaseTriangleCallback.h" @@ -179,8 +179,9 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp" "src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp" "src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp" - "src/collision/narrowphase/SphereVsSphereNarrowPhaseInputBatch.cpp" "src/collision/narrowphase/NarrowPhaseInput.cpp" + "src/collision/narrowphase/NarrowPhaseInfoBatch.cpp" + "src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp" "src/collision/shapes/AABB.cpp" "src/collision/shapes/ConvexShape.cpp" "src/collision/shapes/ConvexPolyhedronShape.cpp" @@ -201,7 +202,6 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/PolyhedronMesh.cpp" "src/collision/HalfEdgeStructure.cpp" "src/collision/CollisionDetection.cpp" - "src/collision/NarrowPhaseInfoBatch.cpp" "src/collision/ContactManifold.cpp" "src/collision/ContactManifoldSet.cpp" "src/collision/MiddlePhaseTriangleCallback.cpp" diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 8d7d29b5..f8d82082 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -35,7 +35,7 @@ #include "collision/CollisionCallback.h" #include "collision/MiddlePhaseTriangleCallback.h" #include "collision/OverlapCallback.h" -#include "collision/NarrowPhaseInfoBatch.h" +#include "collision/narrowphase/NarrowPhaseInfoBatch.h" #include "collision/ContactManifold.h" #include "utils/Profiler.h" #include "engine/EventListener.h" @@ -252,7 +252,7 @@ bool CollisionDetection::testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseI ConvexPolyhedronVsConvexPolyhedronAlgorithm* convexPolyVsConvexPolyAlgo = mCollisionDispatch.getConvexPolyhedronVsConvexPolyhedronAlgorithm(); // get the narrow-phase batches to test for collision - NarrowPhaseInfoBatch& sphereVsSphereBatch = narrowPhaseInput.getSphereVsSphereBatch(); + SphereVsSphereNarrowPhaseInfoBatch& sphereVsSphereBatch = narrowPhaseInput.getSphereVsSphereBatch(); NarrowPhaseInfoBatch& sphereVsCapsuleBatch = narrowPhaseInput.getSphereVsCapsuleBatch(); NarrowPhaseInfoBatch& capsuleVsCapsuleBatch = narrowPhaseInput.getCapsuleVsCapsuleBatch(); NarrowPhaseInfoBatch& sphereVsConvexPolyhedronBatch = narrowPhaseInput.getSphereVsConvexPolyhedronBatch(); diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index 7ced9552..5c6f8d7f 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -25,7 +25,7 @@ // Libraries #include "ContactManifoldSet.h" -#include "NarrowPhaseInfoBatch.h" +#include "narrowphase/NarrowPhaseInfoBatch.h" #include "constraint/ContactPoint.h" #include "ProxyShape.h" #include "collision/ContactManifold.h" diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index 3412a4ca..af26f26a 100755 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -26,7 +26,7 @@ // Libraries #include "CapsuleVsCapsuleAlgorithm.h" #include "collision/shapes/CapsuleShape.h" -#include "collision/NarrowPhaseInfoBatch.h" +#include "collision/narrowphase/NarrowPhaseInfoBatch.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index e611109b..d425b80e 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -29,7 +29,7 @@ #include "GJK/GJKAlgorithm.h" #include "collision/shapes/CapsuleShape.h" #include "collision/shapes/ConvexPolyhedronShape.h" -#include "collision/NarrowPhaseInfoBatch.h" +#include "collision/narrowphase/NarrowPhaseInfoBatch.h" #include "collision/ContactPointInfo.h" #include diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp index fe47ac3e..3ed15c74 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp @@ -27,7 +27,7 @@ #include "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h" #include "GJK/GJKAlgorithm.h" #include "SAT/SATAlgorithm.h" -#include "collision/NarrowPhaseInfoBatch.h" +#include "collision/narrowphase/NarrowPhaseInfoBatch.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index bbda9a62..317145b9 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -31,7 +31,7 @@ #include "configuration.h" #include "utils/Profiler.h" #include "containers/List.h" -#include "collision/NarrowPhaseInfoBatch.h" +#include "collision/narrowphase/NarrowPhaseInfoBatch.h" #include "collision/narrowphase/GJK/VoronoiSimplex.h" #include diff --git a/src/collision/NarrowPhaseInfoBatch.cpp b/src/collision/narrowphase/NarrowPhaseInfoBatch.cpp similarity index 95% rename from src/collision/NarrowPhaseInfoBatch.cpp rename to src/collision/narrowphase/NarrowPhaseInfoBatch.cpp index 08466945..bb7c0d7e 100644 --- a/src/collision/NarrowPhaseInfoBatch.cpp +++ b/src/collision/narrowphase/NarrowPhaseInfoBatch.cpp @@ -25,7 +25,7 @@ // Libraries #include "NarrowPhaseInfoBatch.h" -#include "ContactPointInfo.h" +#include "collision/ContactPointInfo.h" #include "collision/shapes/TriangleShape.h" #include "engine/OverlappingPair.h" @@ -125,11 +125,12 @@ void NarrowPhaseInfoBatch::clear() { // Release the memory of the TriangleShape (this memory was allocated in the // MiddlePhaseTriangleCallback::testTriangle() method) - if (collisionShapes1[i]->getName() == CollisionShapeName::TRIANGLE) { + // TODO DOD : Try to move this code + if (collisionShapes1.size() > 0 && collisionShapes1[i]->getName() == CollisionShapeName::TRIANGLE) { collisionShapes1[i]->~CollisionShape(); collisionShapeAllocators[i]->release(collisionShapes1[i], sizeof(TriangleShape)); } - if (collisionShapes2[i]->getName() == CollisionShapeName::TRIANGLE) { + if (collisionShapes2.size() > 0 && collisionShapes2[i]->getName() == CollisionShapeName::TRIANGLE) { collisionShapes2[i]->~CollisionShape(); collisionShapeAllocators[i]->release(collisionShapes2[i], sizeof(TriangleShape)); } diff --git a/src/collision/NarrowPhaseInfoBatch.h b/src/collision/narrowphase/NarrowPhaseInfoBatch.h similarity index 95% rename from src/collision/NarrowPhaseInfoBatch.h rename to src/collision/narrowphase/NarrowPhaseInfoBatch.h index 48c23221..abf5d7b0 100644 --- a/src/collision/NarrowPhaseInfoBatch.h +++ b/src/collision/narrowphase/NarrowPhaseInfoBatch.h @@ -47,7 +47,7 @@ struct ContactPointInfo; */ struct NarrowPhaseInfoBatch { - private: + protected: /// Memory allocator MemoryAllocator& mMemoryAllocator; @@ -57,6 +57,8 @@ struct NarrowPhaseInfoBatch { public: + // TODO DOD : Try to remove most of the following lists + /// List of Broadphase overlapping pairs List overlappingPairs; @@ -88,12 +90,13 @@ struct NarrowPhaseInfoBatch { NarrowPhaseInfoBatch(MemoryAllocator& allocator); /// Destructor - ~NarrowPhaseInfoBatch(); + virtual ~NarrowPhaseInfoBatch(); /// Return the number of objects in the batch uint getNbObjects() const; /// Add shapes to be tested during narrow-phase collision detection into the batch + // TODO DOD : Remove this method (only use the one in specialized classed) void addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2, const Transform& shape1Transform, const Transform& shape2Transform, MemoryAllocator& shapeAllocator); @@ -109,7 +112,7 @@ struct NarrowPhaseInfoBatch { void reserveMemory(); /// Clear all the objects in the batch - void clear(); + virtual void clear(); }; /// Return the number of objects in the batch diff --git a/src/collision/narrowphase/NarrowPhaseInput.cpp b/src/collision/narrowphase/NarrowPhaseInput.cpp index 88d6cb14..19e85925 100644 --- a/src/collision/narrowphase/NarrowPhaseInput.cpp +++ b/src/collision/narrowphase/NarrowPhaseInput.cpp @@ -44,7 +44,7 @@ void NarrowPhaseInput::addNarrowPhaseTest(OverlappingPair* pair, CollisionShape* switch (narrowPhaseAlgorithmType) { case NarrowPhaseAlgorithmType::SphereVsSphere: - mSphereVsSphereBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator); + mSphereVsSphereBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform); break; case NarrowPhaseAlgorithmType::SphereVsCapsule: mSphereVsCapsuleBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator); diff --git a/src/collision/narrowphase/NarrowPhaseInput.h b/src/collision/narrowphase/NarrowPhaseInput.h index e71e327e..da9e34fa 100644 --- a/src/collision/narrowphase/NarrowPhaseInput.h +++ b/src/collision/narrowphase/NarrowPhaseInput.h @@ -28,7 +28,8 @@ // Libraries #include "containers/List.h" -#include "collision/NarrowPhaseInfoBatch.h" +#include "collision/narrowphase/NarrowPhaseInfoBatch.h" +#include "collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h" /// Namespace ReactPhysics3D namespace reactphysics3d { @@ -52,7 +53,7 @@ class NarrowPhaseInput { private: - NarrowPhaseInfoBatch mSphereVsSphereBatch; + SphereVsSphereNarrowPhaseInfoBatch mSphereVsSphereBatch; NarrowPhaseInfoBatch mSphereVsCapsuleBatch; NarrowPhaseInfoBatch mCapsuleVsCapsuleBatch; NarrowPhaseInfoBatch mSphereVsConvexPolyhedronBatch; @@ -70,7 +71,7 @@ class NarrowPhaseInput { MemoryAllocator& shapeAllocator); /// Get a reference to the sphere vs sphere batch - NarrowPhaseInfoBatch& getSphereVsSphereBatch(); + SphereVsSphereNarrowPhaseInfoBatch& getSphereVsSphereBatch(); /// Get a reference to the sphere vs capsule batch NarrowPhaseInfoBatch& getSphereVsCapsuleBatch(); @@ -96,7 +97,7 @@ class NarrowPhaseInput { // Get a reference to the sphere vs sphere batch -inline NarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsSphereBatch() { +inline SphereVsSphereNarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsSphereBatch() { return mSphereVsSphereBatch; } diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 9224290a..3c467ea3 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -30,7 +30,7 @@ #include "collision/shapes/CapsuleShape.h" #include "collision/shapes/SphereShape.h" #include "engine/OverlappingPair.h" -#include "collision/NarrowPhaseInfoBatch.h" +#include "collision/narrowphase/NarrowPhaseInfoBatch.h" #include "collision/shapes/TriangleShape.h" #include "configuration.h" #include "utils/Profiler.h" diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index 0f98cd27..8b6f9165 100755 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -27,7 +27,7 @@ #include "SphereVsCapsuleAlgorithm.h" #include "collision/shapes/SphereShape.h" #include "collision/shapes/CapsuleShape.h" -#include "collision/NarrowPhaseInfoBatch.h" +#include "collision/narrowphase/NarrowPhaseInfoBatch.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index b1416afd..a8e638be 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -27,7 +27,7 @@ #include "SphereVsConvexPolyhedronAlgorithm.h" #include "GJK/GJKAlgorithm.h" #include "SAT/SATAlgorithm.h" -#include "collision/NarrowPhaseInfoBatch.h" +#include "collision/narrowphase/NarrowPhaseInfoBatch.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index fa042687..03b8a393 100755 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -26,12 +26,12 @@ // Libraries #include "SphereVsSphereAlgorithm.h" #include "collision/shapes/SphereShape.h" -#include "collision/NarrowPhaseInfoBatch.h" +#include "collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, +bool SphereVsSphereAlgorithm::testCollision(SphereVsSphereNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) { bool isCollisionFound = false; @@ -42,32 +42,29 @@ bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInf assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0); assert(!narrowPhaseInfoBatch.isColliding[batchIndex]); - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE); - assert(narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::SPHERE); - - // Get the sphere collision shapes - const SphereShape* sphereShape1 = static_cast(narrowPhaseInfoBatch.collisionShapes1[batchIndex]); - const SphereShape* sphereShape2 = static_cast(narrowPhaseInfoBatch.collisionShapes2[batchIndex]); - // Get the local-space to world-space transforms - const Transform& transform1 = narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex]; - const Transform& transform2 = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex]; + const Transform& transform1 = narrowPhaseInfoBatch.sphere1WorldTransforms[batchIndex]; + const Transform& transform2 = narrowPhaseInfoBatch.sphere2WorldTransforms[batchIndex]; // Compute the distance between the centers Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare(); // Compute the sum of the radius - decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius(); + decimal sumRadiuses = narrowPhaseInfoBatch.sphere1Radiuses[batchIndex] + narrowPhaseInfoBatch.sphere2Radiuses[batchIndex]; + + // Compute the product of the sum of the radius + decimal sumRadiusesProducts = sumRadiuses * sumRadiuses; // If the sphere collision shapes intersect - if (squaredDistanceBetweenCenters < sumRadius * sumRadius) { + if (squaredDistanceBetweenCenters < sumRadiusesProducts) { if (reportContacts) { - Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); - Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); - decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); + Transform transform1Inverse = narrowPhaseInfoBatch.sphere1WorldTransforms[batchIndex].getInverse(); + Transform transform2Inverse = narrowPhaseInfoBatch.sphere2WorldTransforms[batchIndex].getInverse(); + + decimal penetrationDepth = sumRadiuses - std::sqrt(squaredDistanceBetweenCenters); Vector3 intersectionOnBody1; Vector3 intersectionOnBody2; Vector3 normal; @@ -75,8 +72,11 @@ bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInf // If the two sphere centers are not at the same position if (squaredDistanceBetweenCenters > MACHINE_EPSILON) { - intersectionOnBody1 = sphereShape1->getRadius() * centerSphere2InBody1LocalSpace.getUnit(); - intersectionOnBody2 = sphereShape2->getRadius() * centerSphere1InBody2LocalSpace.getUnit(); + Vector3 centerSphere2InBody1LocalSpace = transform1Inverse * narrowPhaseInfoBatch.sphere1WorldTransforms[batchIndex].getPosition(); + Vector3 centerSphere1InBody2LocalSpace = transform2Inverse * narrowPhaseInfoBatch.sphere2WorldTransforms[batchIndex].getPosition(); + + intersectionOnBody1 = narrowPhaseInfoBatch.sphere1Radiuses[batchIndex] * centerSphere2InBody1LocalSpace.getUnit(); + intersectionOnBody2 = narrowPhaseInfoBatch.sphere2Radiuses[batchIndex] * centerSphere1InBody2LocalSpace.getUnit(); normal = vectorBetweenCenters.getUnit(); } else { // If the sphere centers are at the same position (degenerate case) @@ -84,8 +84,8 @@ bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInf // Take any contact normal direction normal.setAllValues(0, 1, 0); - intersectionOnBody1 = sphereShape1->getRadius() * (transform1.getInverse().getOrientation() * normal); - intersectionOnBody2 = sphereShape2->getRadius() * (transform2.getInverse().getOrientation() * normal); + intersectionOnBody1 = narrowPhaseInfoBatch.sphere1Radiuses[batchIndex] * (transform1Inverse.getOrientation() * normal); + intersectionOnBody2 = narrowPhaseInfoBatch.sphere2Radiuses[batchIndex] * (transform2Inverse.getOrientation() * normal); } // Create the contact info object diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h index 8099b608..e0f4c5be 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -34,6 +34,7 @@ namespace reactphysics3d { // Declarations class ContactPoint; +struct SphereVsSphereNarrowPhaseInfoBatch; class Body; // Class SphereVsSphereAlgorithm @@ -65,7 +66,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete; /// Compute a contact info if the two bounding volume collide - bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, + bool testCollision(SphereVsSphereNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator); }; diff --git a/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp b/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp new file mode 100644 index 00000000..3b9d2fc1 --- /dev/null +++ b/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp @@ -0,0 +1,88 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2018 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 "SphereVsSphereNarrowPhaseInfoBatch.h" +#include "collision/shapes/SphereShape.h" + +using namespace reactphysics3d; + +// Constructor +SphereVsSphereNarrowPhaseInfoBatch::SphereVsSphereNarrowPhaseInfoBatch(MemoryAllocator& allocator) + : NarrowPhaseInfoBatch(allocator), sphere1Radiuses(allocator), sphere2Radiuses(allocator), + sphere1WorldTransforms(allocator), sphere2WorldTransforms(allocator) { + +} + +// Add shapes to be tested during narrow-phase collision detection into the batch +void SphereVsSphereNarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2, + const Transform& shape1Transform, const Transform& shape2Transform) { + + 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()); + sphere1WorldTransforms.add(shape1Transform); + sphere2WorldTransforms.add(shape2Transform); + overlappingPairs.add(pair); + contactPoints.add(List(mMemoryAllocator)); + isColliding.add(false); + + // Add a collision info for the two collision shapes into the overlapping pair (if not present yet) + LastFrameCollisionInfo* lastFrameInfo = pair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId()); + lastFrameCollisionInfos.add(lastFrameInfo); +} + +// Initialize the containers using cached capacity +void SphereVsSphereNarrowPhaseInfoBatch::reserveMemory() { + + NarrowPhaseInfoBatch::reserveMemory(); + + sphere1Radiuses.reserve(mCachedCapacity); + sphere2Radiuses.reserve(mCachedCapacity); + sphere1WorldTransforms.reserve(mCachedCapacity); + sphere2WorldTransforms.reserve(mCachedCapacity); +} + +// Clear all the objects in the batch +void SphereVsSphereNarrowPhaseInfoBatch::clear() { + + NarrowPhaseInfoBatch::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) + + sphere1Radiuses.clear(true); + sphere2Radiuses.clear(true); + sphere1WorldTransforms.clear(true); + sphere2WorldTransforms.clear(true); +} + diff --git a/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h b/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h new file mode 100644 index 00000000..ac0a37c9 --- /dev/null +++ b/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h @@ -0,0 +1,78 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2018 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 "collision/narrowphase/NarrowPhaseInfoBatch.h" + +/// 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; + + /// List of the world-space positions for the center of the first spheres + List sphere1WorldTransforms; + + /// List of the world-space positions for the center of the second spheres + List sphere2WorldTransforms; + + /// Constructor + SphereVsSphereNarrowPhaseInfoBatch(MemoryAllocator& allocator); + + /// Destructor + virtual ~SphereVsSphereNarrowPhaseInfoBatch() = default; + + /// Add shapes to be tested during narrow-phase collision detection into the batch + virtual void addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, + CollisionShape* shape2, const Transform& shape1Transform, + const Transform& shape2Transform); + + // Initialize the containers using cached capacity + void reserveMemory(); + + /// Clear all the objects in the batch + virtual void clear(); +}; + +} + +#endif +