diff --git a/CMakeLists.txt b/CMakeLists.txt index e17e0e35..e28ff41f 100644 --- a/CMakeLists.txt +++ b/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" diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index f8d82082..f3e4f6d5 100644 --- a/src/collision/CollisionDetection.cpp +++ b/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(); diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index af26f26a..ecf69ee1 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/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(narrowPhaseInfoBatch.collisionShapes1[batchIndex]); - const CapsuleShape* capsuleShape2 = static_cast(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); - Vector3 capsule1SegB(0, capsuleShape1->getHeight() * decimal(0.5), 0); + const decimal capsule1HalfHeight = narrowPhaseInfoBatch.capsule1Heights[batchIndex] * decimal(0.5); + Vector3 capsule1SegA(0, -capsule1HalfHeight, 0); + Vector3 capsule1SegB(0, capsule1HalfHeight, 0); capsule1SegA = capsule1ToCapsule2SpaceTransform * capsule1SegA; capsule1SegB = capsule1ToCapsule2SpaceTransform * capsule1SegB; // Compute the end-points of the inner segment of the second capsule - const Vector3 capsule2SegA(0, -capsuleShape2->getHeight() * decimal(0.5), 0); - const Vector3 capsule2SegB(0, capsuleShape2->getHeight() * decimal(0.5), 0); + const decimal capsule2HalfHeight = narrowPhaseInfoBatch.capsule2Heights[batchIndex] * decimal(0.5); + 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 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsuleShape1->getRadius()); - const Vector3 contactPointACapsule2Local = clipPointA - normalCapsule2SpaceNormalized * capsuleShape2->getRadius(); - const Vector3 contactPointBCapsule2Local = clipPointB - normalCapsule2SpaceNormalized * capsuleShape2->getRadius(); + const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsule1Radius); + const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsule1Radius); + const Vector3 contactPointACapsule2Local = clipPointA - normalCapsule2SpaceNormalized * capsule2Radius; + 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 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius(); + const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsule1Radius); + 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 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius(); + const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsule1Radius); + 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 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius(); + const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsule1Radius); + const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsule2Radius; const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsuleSpace2; diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h index 8587316e..32f76abe 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h +++ b/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); }; diff --git a/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.cpp b/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.cpp new file mode 100644 index 00000000..1034e13d --- /dev/null +++ b/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(shape1); + const CapsuleShape* capsule2 = static_cast(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(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); +} diff --git a/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h b/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h new file mode 100644 index 00000000..7bb061fb --- /dev/null +++ b/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 capsule1Radiuses; + + /// List of radiuses for the second capsules + List capsule2Radiuses; + + /// List of heights for the first capsules + List capsule1Heights; + + /// List of heights for the second capsules + List capsule2Heights; + + /// Constructor + CapsuleVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator); + + /// 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 + diff --git a/src/collision/narrowphase/NarrowPhaseInput.cpp b/src/collision/narrowphase/NarrowPhaseInput.cpp index 19e85925..a595abcf 100644 --- a/src/collision/narrowphase/NarrowPhaseInput.cpp +++ b/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); diff --git a/src/collision/narrowphase/NarrowPhaseInput.h b/src/collision/narrowphase/NarrowPhaseInput.h index da9e34fa..0ececc74 100644 --- a/src/collision/narrowphase/NarrowPhaseInput.h +++ b/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; } diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 03b8a393..52bf5d96 100755 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/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& transform2 = narrowPhaseInfoBatch.sphere2WorldTransforms[batchIndex]; + const Transform& transform1 = narrowPhaseInfoBatch.shape1ToWorldTransforms[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 transform2Inverse = narrowPhaseInfoBatch.sphere2WorldTransforms[batchIndex].getInverse(); + Transform transform1Inverse = transform1.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 centerSphere1InBody2LocalSpace = transform2Inverse * narrowPhaseInfoBatch.sphere2WorldTransforms[batchIndex].getPosition(); + Vector3 centerSphere2InBody1LocalSpace = transform1Inverse * transform2.getPosition(); + Vector3 centerSphere1InBody2LocalSpace = transform2Inverse * transform1.getPosition(); intersectionOnBody1 = narrowPhaseInfoBatch.sphere1Radiuses[batchIndex] * centerSphere2InBody1LocalSpace.getUnit(); intersectionOnBody2 = narrowPhaseInfoBatch.sphere2Radiuses[batchIndex] * centerSphere1InBody2LocalSpace.getUnit(); diff --git a/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp b/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp index 3b9d2fc1..d0891389 100644 --- a/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp +++ b/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp @@ -31,8 +31,7 @@ using namespace reactphysics3d; // Constructor SphereVsSphereNarrowPhaseInfoBatch::SphereVsSphereNarrowPhaseInfoBatch(MemoryAllocator& allocator) - : NarrowPhaseInfoBatch(allocator), sphere1Radiuses(allocator), sphere2Radiuses(allocator), - sphere1WorldTransforms(allocator), sphere2WorldTransforms(allocator) { + : NarrowPhaseInfoBatch(allocator), sphere1Radiuses(allocator), sphere2Radiuses(allocator) { } @@ -48,8 +47,8 @@ void SphereVsSphereNarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pai sphere1Radiuses.add(sphere1->getRadius()); sphere2Radiuses.add(sphere2->getRadius()); - sphere1WorldTransforms.add(shape1Transform); - sphere2WorldTransforms.add(shape2Transform); + shape1ToWorldTransforms.add(shape1Transform); + shape2ToWorldTransforms.add(shape2Transform); overlappingPairs.add(pair); contactPoints.add(List(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); } diff --git a/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h b/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h index ac0a37c9..71d5ec5e 100644 --- a/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h +++ b/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h @@ -48,12 +48,6 @@ struct SphereVsSphereNarrowPhaseInfoBatch : public NarrowPhaseInfoBatch { /// 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);