Create SphereVsCapsuleNarrowPhaseInfoBatch

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"

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();

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 {
 

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);

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;
 }
 

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;
+        const bool isSphereShape1 = narrowPhaseInfoBatch.isSpheresShape1[batchIndex];
-
-        assert(!narrowPhaseInfoBatch.isColliding[batchIndex]);
-        assert(isSphereShape1 || narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE);
-
-        // Get the collision shapes
-        const SphereShape* sphereShape = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfoBatch.collisionShapes1[batchIndex] : narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
-        const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isSphereShape1 ? narrowPhaseInfoBatch.collisionShapes2[batchIndex] : narrowPhaseInfoBatch.collisionShapes1[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());
+                    contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * narrowPhaseInfoBatch.sphereRadiuses[batchIndex]);
-                    contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius();
+                    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());
+                    contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + normalCapsuleSpace * narrowPhaseInfoBatch.sphereRadiuses[batchIndex]);
-                    contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * capsuleShape->getRadius();
+                    contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * narrowPhaseInfoBatch.capsuleRadiuses[batchIndex];
                 }
 
                 if (penetrationDepth <= decimal(0.0)) {

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);
 };

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<const SphereShape*>(isSphereShape1 ? shape1 : shape2);
+    const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(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<ContactPointInfo*>(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);
+}

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<bool> isSpheresShape1;
+
+        /// List of radiuses for the spheres
+        List<decimal> sphereRadiuses;
+
+        /// List of radiuses for the capsules
+        List<decimal> capsuleRadiuses;
+
+        /// List of heights for the capsules
+        List<decimal> 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
+