From 05d05c3fd88b0293c5e5551a505bed9b97ae5c58 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 29 Nov 2018 17:33:27 +0100 Subject: [PATCH] Create SphereVsCapsuleNarrowPhaseInfoBatch --- CMakeLists.txt | 2 + src/collision/CollisionDetection.cpp | 2 +- .../CapsuleVsCapsuleNarrowPhaseInfoBatch.h | 4 +- .../narrowphase/NarrowPhaseInput.cpp | 2 +- src/collision/narrowphase/NarrowPhaseInput.h | 7 +- .../narrowphase/SphereVsCapsuleAlgorithm.cpp | 26 ++---- .../narrowphase/SphereVsCapsuleAlgorithm.h | 3 +- .../SphereVsCapsuleNarrowPhaseInfoBatch.cpp | 92 +++++++++++++++++++ .../SphereVsCapsuleNarrowPhaseInfoBatch.h | 78 ++++++++++++++++ 9 files changed, 192 insertions(+), 24 deletions(-) create mode 100644 src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.cpp create mode 100644 src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h diff --git a/CMakeLists.txt b/CMakeLists.txt index e28ff41f..9b5e9bb4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -98,6 +98,7 @@ SET (REACTPHYSICS3D_HEADERS "src/collision/narrowphase/NarrowPhaseInfoBatch.h" "src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h" "src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h" + "src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h" "src/collision/shapes/AABB.h" "src/collision/shapes/ConvexShape.h" "src/collision/shapes/ConvexPolyhedronShape.h" @@ -184,6 +185,7 @@ SET (REACTPHYSICS3D_SOURCES "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/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index f3e4f6d5..b1e80bda 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -253,7 +253,7 @@ bool CollisionDetection::testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseI // get the narrow-phase batches to test for collision SphereVsSphereNarrowPhaseInfoBatch& sphereVsSphereBatch = narrowPhaseInput.getSphereVsSphereBatch(); - NarrowPhaseInfoBatch& sphereVsCapsuleBatch = narrowPhaseInput.getSphereVsCapsuleBatch(); + SphereVsCapsuleNarrowPhaseInfoBatch& sphereVsCapsuleBatch = narrowPhaseInput.getSphereVsCapsuleBatch(); CapsuleVsCapsuleNarrowPhaseInfoBatch& capsuleVsCapsuleBatch = narrowPhaseInput.getCapsuleVsCapsuleBatch(); NarrowPhaseInfoBatch& sphereVsConvexPolyhedronBatch = narrowPhaseInput.getSphereVsConvexPolyhedronBatch(); NarrowPhaseInfoBatch& capsuleVsConvexPolyhedronBatch = narrowPhaseInput.getCapsuleVsConvexPolyhedronBatch(); diff --git a/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h b/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h index 7bb061fb..914e02b9 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h +++ b/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h @@ -32,11 +32,11 @@ /// Namespace ReactPhysics3D namespace reactphysics3d { -// Struct SphereVsSphereNarrowPhaseInfoBatch +// 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 - * sphere vs sphere collision detection tests. + * capsule vs capsule collision detection tests. */ struct CapsuleVsCapsuleNarrowPhaseInfoBatch : public NarrowPhaseInfoBatch { diff --git a/src/collision/narrowphase/NarrowPhaseInput.cpp b/src/collision/narrowphase/NarrowPhaseInput.cpp index a595abcf..b5004c93 100644 --- a/src/collision/narrowphase/NarrowPhaseInput.cpp +++ b/src/collision/narrowphase/NarrowPhaseInput.cpp @@ -47,7 +47,7 @@ void NarrowPhaseInput::addNarrowPhaseTest(OverlappingPair* pair, CollisionShape* mSphereVsSphereBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform); break; case NarrowPhaseAlgorithmType::SphereVsCapsule: - mSphereVsCapsuleBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator); + mSphereVsCapsuleBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform); break; case NarrowPhaseAlgorithmType::CapsuleVsCapsule: mCapsuleVsCapsuleBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform); diff --git a/src/collision/narrowphase/NarrowPhaseInput.h b/src/collision/narrowphase/NarrowPhaseInput.h index 0ececc74..ee1c3906 100644 --- a/src/collision/narrowphase/NarrowPhaseInput.h +++ b/src/collision/narrowphase/NarrowPhaseInput.h @@ -31,6 +31,7 @@ #include "collision/narrowphase/NarrowPhaseInfoBatch.h" #include "collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h" #include "collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h" +#include "collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h" /// Namespace ReactPhysics3D namespace reactphysics3d { @@ -55,7 +56,7 @@ class NarrowPhaseInput { private: SphereVsSphereNarrowPhaseInfoBatch mSphereVsSphereBatch; - NarrowPhaseInfoBatch mSphereVsCapsuleBatch; + SphereVsCapsuleNarrowPhaseInfoBatch mSphereVsCapsuleBatch; CapsuleVsCapsuleNarrowPhaseInfoBatch mCapsuleVsCapsuleBatch; NarrowPhaseInfoBatch mSphereVsConvexPolyhedronBatch; NarrowPhaseInfoBatch mCapsuleVsConvexPolyhedronBatch; @@ -75,7 +76,7 @@ class NarrowPhaseInput { SphereVsSphereNarrowPhaseInfoBatch& getSphereVsSphereBatch(); /// Get a reference to the sphere vs capsule batch - NarrowPhaseInfoBatch& getSphereVsCapsuleBatch(); + SphereVsCapsuleNarrowPhaseInfoBatch& getSphereVsCapsuleBatch(); /// Get a reference to the capsule vs capsule batch CapsuleVsCapsuleNarrowPhaseInfoBatch& getCapsuleVsCapsuleBatch(); @@ -103,7 +104,7 @@ inline SphereVsSphereNarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsSphereBa } // Get a reference to the sphere vs capsule batch -inline NarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsCapsuleBatch() { +inline SphereVsCapsuleNarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsCapsuleBatch() { return mSphereVsCapsuleBatch; } diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index 8b6f9165..68a02509 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/narrowphase/NarrowPhaseInfoBatch.h" +#include "collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; @@ -35,23 +35,17 @@ 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(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, +bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) { bool isCollisionFound = false; for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { + assert(!narrowPhaseInfoBatch.isColliding[batchIndex]); assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0); - bool isSphereShape1 = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE; - - assert(!narrowPhaseInfoBatch.isColliding[batchIndex]); - assert(isSphereShape1 || narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE); - - // Get the collision shapes - const SphereShape* sphereShape = static_cast(isSphereShape1 ? narrowPhaseInfoBatch.collisionShapes1[batchIndex] : narrowPhaseInfoBatch.collisionShapes2[batchIndex]); - const CapsuleShape* capsuleShape = static_cast(isSphereShape1 ? narrowPhaseInfoBatch.collisionShapes2[batchIndex] : narrowPhaseInfoBatch.collisionShapes1[batchIndex]); + const bool isSphereShape1 = narrowPhaseInfoBatch.isSpheresShape1[batchIndex]; // Get the transform from sphere local-space to capsule local-space const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex]; @@ -63,7 +57,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseIn const Vector3 sphereCenter = sphereToCapsuleSpaceTransform.getPosition(); // Compute the end-points of the inner segment of the capsule - const decimal capsuleHalfHeight = capsuleShape->getHeight() * decimal(0.5); + const decimal capsuleHalfHeight = narrowPhaseInfoBatch.capsuleHeights[batchIndex] * decimal(0.5); const Vector3 capsuleSegA(0, -capsuleHalfHeight, 0); const Vector3 capsuleSegB(0, capsuleHalfHeight, 0); @@ -75,7 +69,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseIn const decimal sphereSegmentDistanceSquare = sphereCenterToSegment.lengthSquare(); // Compute the sum of the radius of the sphere and the capsule (virtual sphere) - decimal sumRadius = sphereShape->getRadius() + capsuleShape->getRadius(); + decimal sumRadius = narrowPhaseInfoBatch.sphereRadiuses[batchIndex] + narrowPhaseInfoBatch.capsuleRadiuses[batchIndex]; // If the collision shapes overlap if (sphereSegmentDistanceSquare < sumRadius * sumRadius) { @@ -93,8 +87,8 @@ bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseIn decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare); sphereCenterToSegment /= sphereSegmentDistance; - contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius()); - contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius(); + contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * narrowPhaseInfoBatch.sphereRadiuses[batchIndex]); + contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * narrowPhaseInfoBatch.capsuleRadiuses[batchIndex]; normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment; @@ -125,8 +119,8 @@ bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseIn normalWorld = capsuleToWorldTransform.getOrientation() * normalCapsuleSpace; // Compute the two local contact points - contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + normalCapsuleSpace * sphereShape->getRadius()); - contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * capsuleShape->getRadius(); + contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + normalCapsuleSpace * narrowPhaseInfoBatch.sphereRadiuses[batchIndex]); + contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * narrowPhaseInfoBatch.capsuleRadiuses[batchIndex]; } if (penetrationDepth <= decimal(0.0)) { diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h index 573e5756..7394bfb2 100644 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h @@ -36,6 +36,7 @@ namespace reactphysics3d { // Declarations class Body; class ContactPoint; +struct SphereVsCapsuleNarrowPhaseInfoBatch; // Class SphereVsCapsuleAlgorithm /** @@ -66,7 +67,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(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, + bool testCollision(SphereVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator); }; diff --git a/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.cpp b/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.cpp new file mode 100644 index 00000000..ec8ac168 --- /dev/null +++ b/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.cpp @@ -0,0 +1,92 @@ +/******************************************************************************** +* 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 "SphereVsCapsuleNarrowPhaseInfoBatch.h" +#include "collision/shapes/SphereShape.h" +#include "collision/shapes/CapsuleShape.h" + +using namespace reactphysics3d; + +// Constructor +SphereVsCapsuleNarrowPhaseInfoBatch::SphereVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator) + : NarrowPhaseInfoBatch(allocator), isSpheresShape1(allocator), sphereRadiuses(allocator), capsuleRadiuses(allocator), + capsuleHeights(allocator) { + +} + +// Add shapes to be tested during narrow-phase collision detection into the batch +void SphereVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2, + const Transform& shape1Transform, const Transform& shape2Transform) { + + 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()); + shape1ToWorldTransforms.add(shape1Transform); + shape2ToWorldTransforms.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 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() { + + 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) + + isSpheresShape1.clear(true); + sphereRadiuses.clear(true); + capsuleRadiuses.clear(true); + capsuleHeights.clear(true); +} diff --git a/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h b/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h new file mode 100644 index 00000000..beaa9bf1 --- /dev/null +++ b/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.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_CAPSULE_NARROW_PHASE_INFO_BATCH_H +#define REACTPHYSICS3D_SPHERE_VS_CAPSULE_NARROW_PHASE_INFO_BATCH_H + +// Libraries +#include "collision/narrowphase/NarrowPhaseInfoBatch.h" + +/// 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); + + /// Destructor + virtual ~SphereVsCapsuleNarrowPhaseInfoBatch() = 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 +