Create CapsuleVsCapsuleNarrowPhaseInfoBatch class

CMakeLists.txt

@@ -97,6 +97,7 @@ SET (REACTPHYSICS3D_HEADERS
     "src/collision/narrowphase/NarrowPhaseInput.h"
     "src/collision/narrowphase/NarrowPhaseInfoBatch.h"
     "src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h"
+    "src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h"
     "src/collision/shapes/AABB.h"
     "src/collision/shapes/ConvexShape.h"
     "src/collision/shapes/ConvexPolyhedronShape.h"
@@ -182,6 +183,7 @@ SET (REACTPHYSICS3D_SOURCES
     "src/collision/narrowphase/NarrowPhaseInput.cpp"
     "src/collision/narrowphase/NarrowPhaseInfoBatch.cpp"
     "src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp"
+    "src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.cpp"
     "src/collision/shapes/AABB.cpp"
     "src/collision/shapes/ConvexShape.cpp"
     "src/collision/shapes/ConvexPolyhedronShape.cpp"

src/collision/CollisionDetection.cpp

@@ -254,7 +254,7 @@ bool CollisionDetection::testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseI
     // get the narrow-phase batches to test for collision
     SphereVsSphereNarrowPhaseInfoBatch& sphereVsSphereBatch = narrowPhaseInput.getSphereVsSphereBatch();
     NarrowPhaseInfoBatch& sphereVsCapsuleBatch = narrowPhaseInput.getSphereVsCapsuleBatch();
-    NarrowPhaseInfoBatch& capsuleVsCapsuleBatch = narrowPhaseInput.getCapsuleVsCapsuleBatch();
+    CapsuleVsCapsuleNarrowPhaseInfoBatch& capsuleVsCapsuleBatch = narrowPhaseInput.getCapsuleVsCapsuleBatch();
     NarrowPhaseInfoBatch& sphereVsConvexPolyhedronBatch = narrowPhaseInput.getSphereVsConvexPolyhedronBatch();
     NarrowPhaseInfoBatch& capsuleVsConvexPolyhedronBatch = narrowPhaseInput.getCapsuleVsConvexPolyhedronBatch();
     NarrowPhaseInfoBatch& convexPolyhedronVsConvexPolyhedronBatch = narrowPhaseInput.getConvexPolyhedronVsConvexPolyhedronBatch();

src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp

@@ -26,7 +26,7 @@
 // Libraries
 #include "CapsuleVsCapsuleAlgorithm.h"
 #include "collision/shapes/CapsuleShape.h"
-#include "collision/narrowphase/NarrowPhaseInfoBatch.h"
+#include "collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h"
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;  
@@ -34,7 +34,7 @@ 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(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
+bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
                                               bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) {
     
     bool isCollisionFound = false;
@@ -44,33 +44,31 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseI
         assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0);
 
         assert(!narrowPhaseInfoBatch.isColliding[batchIndex]);
-        assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE);
-        assert(narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CAPSULE);
-
-        // Get the capsule collision shapes
-        const CapsuleShape* capsuleShape1 = static_cast<const CapsuleShape*>(narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
-        const CapsuleShape* capsuleShape2 = static_cast<const CapsuleShape*>(narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
 
         // Get the transform from capsule 1 local-space to capsule 2 local-space
         const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getInverse() *
                                                            narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
 
         // Compute the end-points of the inner segment of the first capsule
-        Vector3 capsule1SegA(0, -capsuleShape1->getHeight() * decimal(0.5), 0);
+        const decimal capsule1HalfHeight = narrowPhaseInfoBatch.capsule1Heights[batchIndex] * decimal(0.5);
-        Vector3 capsule1SegB(0, capsuleShape1->getHeight() * decimal(0.5), 0);
+        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 Vector3 capsule2SegA(0, -capsuleShape2->getHeight() * decimal(0.5), 0);
+        const decimal capsule2HalfHeight = narrowPhaseInfoBatch.capsule2Heights[batchIndex] * decimal(0.5);
-        const Vector3 capsule2SegB(0, capsuleShape2->getHeight() * decimal(0.5), 0);
+        const Vector3 capsule2SegA(0, -capsule2HalfHeight, 0);
+        const Vector3 capsule2SegB(0, capsule2HalfHeight, 0);
 
         // The two inner capsule segments
         const Vector3 seg1 = capsule1SegB - capsule1SegA;
         const Vector3 seg2 = capsule2SegB - capsule2SegA;
 
         // Compute the sum of the radius of the two capsules (virtual spheres)
-        decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius();
+        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)
         bool areCapsuleInnerSegmentsParralel = areParallelVectors(seg1, seg2);
@@ -140,10 +138,10 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseI
                     }
 
                     Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse();
-                    const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsuleShape1->getRadius());
+                    const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsule1Radius);
-                    const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsuleShape1->getRadius());
+                    const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsule1Radius);
-                    const Vector3 contactPointACapsule2Local = clipPointA - normalCapsule2SpaceNormalized * capsuleShape2->getRadius();
+                    const Vector3 contactPointACapsule2Local = clipPointA - normalCapsule2SpaceNormalized * capsule2Radius;
-                    const Vector3 contactPointBCapsule2Local = clipPointB - normalCapsule2SpaceNormalized * capsuleShape2->getRadius();
+                    const Vector3 contactPointBCapsule2Local = clipPointB - normalCapsule2SpaceNormalized * capsule2Radius;
 
                     decimal penetrationDepth = sumRadius - segmentsPerpendicularDistance;
 
@@ -184,8 +182,8 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseI
                     decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
                     closestPointsSeg1ToSeg2 /= closestPointsDistance;
 
-                    const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius());
+                    const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsule1Radius);
-                    const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius();
+                    const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsule2Radius;
 
                     const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * closestPointsSeg1ToSeg2;
 
@@ -207,8 +205,8 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseI
                         Vector3 normalCapsuleSpace2 = (closestPointCapsule2Seg - capsule1SegmentMostExtremePoint);
                         normalCapsuleSpace2.normalize();
 
-                        const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius());
+                        const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsule1Radius);
-                        const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius();
+                        const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsule2Radius;
 
                         const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsuleSpace2;
 
@@ -223,8 +221,8 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseI
                         normalCapsuleSpace2.normalize();
 
                         // Compute the contact points on both shapes
-                        const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius());
+                        const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsule1Radius);
-                        const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius();
+                        const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsule2Radius;
 
                         const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsuleSpace2;
 

src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h

@@ -34,7 +34,7 @@
 namespace reactphysics3d {
 
 // Declarations
-struct NarrowPhaseInfoBatch;
+struct CapsuleVsCapsuleNarrowPhaseInfoBatch;
 class Body;
 class ContactPoint;
 
@@ -67,7 +67,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
 		CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
 
         /// Compute the narrow-phase collision detection between two capsules
-        bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
+        bool testCollision(CapsuleVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
                                    uint batchNbItems, bool reportContacts, bool stopFirstContactFound,
                                    MemoryAllocator& memoryAllocator);
 };

src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.cpp

@@ -0,0 +1,89 @@
+/********************************************************************************
+* 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 "CapsuleVsCapsuleNarrowPhaseInfoBatch.h"
+#include "collision/shapes/CapsuleShape.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+CapsuleVsCapsuleNarrowPhaseInfoBatch::CapsuleVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator)
+      : NarrowPhaseInfoBatch(allocator), capsule1Radiuses(allocator), capsule2Radiuses(allocator),
+        capsule1Heights(allocator), capsule2Heights(allocator) {
+
+}
+
+// Add shapes to be tested during narrow-phase collision detection into the batch
+void CapsuleVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2,
+                                                            const Transform& shape1Transform, const Transform& shape2Transform) {
+
+    assert(shape1->getType() == CollisionShapeType::CAPSULE);
+    assert(shape2->getType() == CollisionShapeType::CAPSULE);
+
+    const CapsuleShape* capsule1 = static_cast<const CapsuleShape*>(shape1);
+    const CapsuleShape* capsule2 = static_cast<const CapsuleShape*>(shape2);
+
+    capsule1Radiuses.add(capsule1->getRadius());
+    capsule2Radiuses.add(capsule2->getRadius());
+    capsule1Heights.add(capsule1->getHeight());
+    capsule2Heights.add(capsule2->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 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() {
+
+    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)
+
+    capsule1Radiuses.clear(true);
+    capsule2Radiuses.clear(true);
+    capsule1Heights.clear(true);
+    capsule2Heights.clear(true);
+}

src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.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_CAPSULE_VS_CAPSULE_NARROW_PHASE_INFO_BATCH_H
+#define REACTPHYSICS3D_CAPSULE_VS_CAPSULE_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 CapsuleVsCapsuleNarrowPhaseInfoBatch : public NarrowPhaseInfoBatch {
+
+    public:
+
+        /// List of radiuses for the first capsules
+        List<decimal> capsule1Radiuses;
+
+        /// List of radiuses for the second capsules
+        List<decimal> capsule2Radiuses;
+
+        /// List of heights for the first capsules
+        List<decimal> capsule1Heights;
+
+        /// List of heights for the second capsules
+        List<decimal> capsule2Heights;
+
+        /// Constructor
+        CapsuleVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator);
+
+        /// Destructor
+        virtual ~CapsuleVsCapsuleNarrowPhaseInfoBatch() = 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
+

src/collision/narrowphase/NarrowPhaseInput.cpp

@@ -50,7 +50,7 @@ void NarrowPhaseInput::addNarrowPhaseTest(OverlappingPair* pair, CollisionShape*
             mSphereVsCapsuleBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator);
             break;
         case NarrowPhaseAlgorithmType::CapsuleVsCapsule:
-            mCapsuleVsCapsuleBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator);
+            mCapsuleVsCapsuleBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform);
             break;
         case NarrowPhaseAlgorithmType::SphereVsConvexPolyhedron:
             mSphereVsConvexPolyhedronBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator);

src/collision/narrowphase/NarrowPhaseInput.h

@@ -30,6 +30,7 @@
 #include "containers/List.h"
 #include "collision/narrowphase/NarrowPhaseInfoBatch.h"
 #include "collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h"
+#include "collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h"
 
 /// Namespace ReactPhysics3D
 namespace reactphysics3d {
@@ -55,7 +56,7 @@ class NarrowPhaseInput {
 
         SphereVsSphereNarrowPhaseInfoBatch mSphereVsSphereBatch;
         NarrowPhaseInfoBatch mSphereVsCapsuleBatch;
-        NarrowPhaseInfoBatch mCapsuleVsCapsuleBatch;
+        CapsuleVsCapsuleNarrowPhaseInfoBatch mCapsuleVsCapsuleBatch;
         NarrowPhaseInfoBatch mSphereVsConvexPolyhedronBatch;
         NarrowPhaseInfoBatch mCapsuleVsConvexPolyhedronBatch;
         NarrowPhaseInfoBatch mConvexPolyhedronVsConvexPolyhedronBatch;
@@ -77,7 +78,7 @@ class NarrowPhaseInput {
         NarrowPhaseInfoBatch& getSphereVsCapsuleBatch();
 
         /// Get a reference to the capsule vs capsule batch
-        NarrowPhaseInfoBatch& getCapsuleVsCapsuleBatch();
+        CapsuleVsCapsuleNarrowPhaseInfoBatch& getCapsuleVsCapsuleBatch();
 
         /// Get a reference to the sphere vs convex polyhedron batch
         NarrowPhaseInfoBatch& getSphereVsConvexPolyhedronBatch();
@@ -107,7 +108,7 @@ inline NarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsCapsuleBatch() {
 }
 
 // Get a reference to the capsule vs capsule batch
-inline NarrowPhaseInfoBatch& NarrowPhaseInput::getCapsuleVsCapsuleBatch() {
+inline CapsuleVsCapsuleNarrowPhaseInfoBatch& NarrowPhaseInput::getCapsuleVsCapsuleBatch() {
    return mCapsuleVsCapsuleBatch;
 }
 

src/collision/narrowphase/SphereVsSphereAlgorithm.cpp

@@ -43,8 +43,8 @@ bool SphereVsSphereAlgorithm::testCollision(SphereVsSphereNarrowPhaseInfoBatch&
         assert(!narrowPhaseInfoBatch.isColliding[batchIndex]);
 
         // Get the local-space to world-space transforms
-        const Transform& transform1 = narrowPhaseInfoBatch.sphere1WorldTransforms[batchIndex];
+        const Transform& transform1 = narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
-        const Transform& transform2 = narrowPhaseInfoBatch.sphere2WorldTransforms[batchIndex];
+        const Transform& transform2 = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
 
         // Compute the distance between the centers
         Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
@@ -61,8 +61,8 @@ bool SphereVsSphereAlgorithm::testCollision(SphereVsSphereNarrowPhaseInfoBatch&
 
             if (reportContacts) {
 
-                Transform transform1Inverse = narrowPhaseInfoBatch.sphere1WorldTransforms[batchIndex].getInverse();
+                Transform transform1Inverse = transform1.getInverse();
-                Transform transform2Inverse = narrowPhaseInfoBatch.sphere2WorldTransforms[batchIndex].getInverse();
+                Transform transform2Inverse = transform2.getInverse();
 
                 decimal penetrationDepth = sumRadiuses - std::sqrt(squaredDistanceBetweenCenters);
                 Vector3 intersectionOnBody1;
@@ -72,8 +72,8 @@ bool SphereVsSphereAlgorithm::testCollision(SphereVsSphereNarrowPhaseInfoBatch&
                 // If the two sphere centers are not at the same position
                 if (squaredDistanceBetweenCenters > MACHINE_EPSILON) {
 
-                    Vector3 centerSphere2InBody1LocalSpace = transform1Inverse * narrowPhaseInfoBatch.sphere1WorldTransforms[batchIndex].getPosition();
+                    Vector3 centerSphere2InBody1LocalSpace = transform1Inverse * transform2.getPosition();
-                    Vector3 centerSphere1InBody2LocalSpace = transform2Inverse * narrowPhaseInfoBatch.sphere2WorldTransforms[batchIndex].getPosition();
+                    Vector3 centerSphere1InBody2LocalSpace = transform2Inverse * transform1.getPosition();
 
                     intersectionOnBody1 = narrowPhaseInfoBatch.sphere1Radiuses[batchIndex] * centerSphere2InBody1LocalSpace.getUnit();
                     intersectionOnBody2 = narrowPhaseInfoBatch.sphere2Radiuses[batchIndex] * centerSphere1InBody2LocalSpace.getUnit();

src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp

@@ -31,8 +31,7 @@ using namespace reactphysics3d;
 
 // Constructor
 SphereVsSphereNarrowPhaseInfoBatch::SphereVsSphereNarrowPhaseInfoBatch(MemoryAllocator& allocator)
-      : NarrowPhaseInfoBatch(allocator), sphere1Radiuses(allocator), sphere2Radiuses(allocator),
+      : NarrowPhaseInfoBatch(allocator), sphere1Radiuses(allocator), sphere2Radiuses(allocator) {
-        sphere1WorldTransforms(allocator), sphere2WorldTransforms(allocator) {
 
 }
 
@@ -48,8 +47,8 @@ void SphereVsSphereNarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pai
 
     sphere1Radiuses.add(sphere1->getRadius());
     sphere2Radiuses.add(sphere2->getRadius());
-    sphere1WorldTransforms.add(shape1Transform);
+    shape1ToWorldTransforms.add(shape1Transform);
-    sphere2WorldTransforms.add(shape2Transform);
+    shape2ToWorldTransforms.add(shape2Transform);
     overlappingPairs.add(pair);
     contactPoints.add(List<ContactPointInfo*>(mMemoryAllocator));
     isColliding.add(false);
@@ -66,8 +65,6 @@ void SphereVsSphereNarrowPhaseInfoBatch::reserveMemory() {
 
     sphere1Radiuses.reserve(mCachedCapacity);
     sphere2Radiuses.reserve(mCachedCapacity);
-    sphere1WorldTransforms.reserve(mCachedCapacity);
-    sphere2WorldTransforms.reserve(mCachedCapacity);
 }
 
 // Clear all the objects in the batch
@@ -82,7 +79,5 @@ void SphereVsSphereNarrowPhaseInfoBatch::clear() {
 
     sphere1Radiuses.clear(true);
     sphere2Radiuses.clear(true);
-    sphere1WorldTransforms.clear(true);
-    sphere2WorldTransforms.clear(true);
 }
 

src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h

@@ -48,12 +48,6 @@ struct SphereVsSphereNarrowPhaseInfoBatch : public NarrowPhaseInfoBatch {
         /// List of radiuses for the second spheres
         List<decimal> sphere2Radiuses;
 
-        /// List of the world-space positions for the center of the first spheres
-        List<Transform> sphere1WorldTransforms;
-
-        /// List of the world-space positions for the center of the second spheres
-        List<Transform> sphere2WorldTransforms;
-
         /// Constructor
         SphereVsSphereNarrowPhaseInfoBatch(MemoryAllocator& allocator);