mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Working on NarrowPhaseInfoBatch optimizations

Browse Source
This commit is contained in:
Daniel Chappuis 2020-07-26 16:55:07 +02:00
parent d6b88baee7
commit 92884e2486
29 changed files with 342 additions and 824 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
CMakeLists.txt
View File

@ -63,9 +63,6 @@ set (REACTPHYSICS3D_HEADERS
"include/reactphysics3d/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h"
"include/reactphysics3d/collision/narrowphase/NarrowPhaseInput.h"
"include/reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h"
"include/reactphysics3d/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h"
"include/reactphysics3d/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h"
"include/reactphysics3d/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h"
"include/reactphysics3d/collision/shapes/AABB.h"
"include/reactphysics3d/collision/shapes/ConvexShape.h"
"include/reactphysics3d/collision/shapes/ConvexPolyhedronShape.h"
@ -168,9 +165,6 @@ set (REACTPHYSICS3D_SOURCES
"src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp"
"src/collision/narrowphase/NarrowPhaseInput.cpp"
"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"

16
include/reactphysics3d/collision/ContactPointInfo.h
View File

@ -45,10 +45,6 @@ class CollisionBody;
*/
struct ContactPointInfo {
private:
// -------------------- Methods -------------------- //
public:
// -------------------- Attributes -------------------- //
@ -64,18 +60,6 @@ struct ContactPointInfo {
/// Contact point of body 2 in local space of body 2
Vector3 localPoint2;
// -------------------- Methods -------------------- //
/// Constructor
ContactPointInfo(const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2)
: normal(contactNormal), penetrationDepth(penDepth),
localPoint1(localPt1), localPoint2(localPt2) {
assert(contactNormal.lengthSquare() > decimal(0.8));
assert(penDepth > decimal(0.0));
}
};
}

4
include/reactphysics3d/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
View File

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

78
include/reactphysics3d/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h
View File

@ -1,78 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2020 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 <reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h>
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// 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
* capsule vs capsule 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, OverlappingPairs& overlappingPairs);
/// Destructor
virtual ~CapsuleVsCapsuleNarrowPhaseInfoBatch() override = default;
/// Add shapes to be tested during narrow-phase collision detection into the batch
virtual void addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1,
CollisionShape* shape2, const Transform& shape1Transform,
const Transform& shape2Transform, bool needToReportContacts, MemoryAllocator& shapeAllocator) override;
// Initialize the containers using cached capacity
virtual void reserveMemory() override;
/// Clear all the objects in the batch
virtual void clear() override;
};
}
#endif

1
include/reactphysics3d/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
View File

@ -34,6 +34,7 @@ namespace reactphysics3d {
// Declarations
class ContactPoint;
struct NarrowPhaseInfoBatch;
// Class CapsuleVsConvexPolyhedronAlgorithm
/**

1
include/reactphysics3d/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h
View File

@ -34,6 +34,7 @@ namespace reactphysics3d {
// Declarations
class ContactPoint;
struct NarrowPhaseInfoBatch;
// Class ConvexPolyhedronVsConvexPolyhedronAlgorithm
/**

122
include/reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h
View File

@ -28,6 +28,7 @@
// Libraries
#include <reactphysics3d/engine/OverlappingPairs.h>
#include <reactphysics3d/collision/ContactPointInfo.h>
/// Namespace ReactPhysics3D
namespace reactphysics3d {
@ -40,16 +41,68 @@ struct ContactPointInfo;
// Struct NarrowPhaseInfoBatch
/**
* This abstract structure collects all the potential collisions from the middle-phase algorithm
* that have to be tested during narrow-phase collision detection. There is an implementation of
* this class for each kind of collision detection test. For instance, one for sphere vs sphere,
* one for sphere vs capsule, ...
* This structure collects all the potential collisions from the middle-phase algorithm
* that have to be tested during narrow-phase collision detection.
*/
struct NarrowPhaseInfoBatch {
struct NarrowPhaseInfo {
/// Broadphase overlapping pairs ids
uint64 overlappingPairId;
/// Entity of the first collider to test collision with
Entity colliderEntity1;
/// Entity of the second collider to test collision with
Entity colliderEntity2;
/// Collision info of the previous frame
// TODO OPTI : Do we really need to use a pointer here ? why not flat object instead ?
LastFrameCollisionInfo* lastFrameCollisionInfo;
/// Memory allocator for the collision shape (Used to release TriangleShape memory in destructor)
MemoryAllocator* collisionShapeAllocator;
/// Shape local to world transform of sphere 1
Transform shape1ToWorldTransform;
/// Shape local to world transform of sphere 2
Transform shape2ToWorldTransform;
/// Pointer to the first collision shapes to test collision with
CollisionShape* collisionShape1;
/// Pointer to the second collision shapes to test collision with
CollisionShape* collisionShape2;
/// True if we need to report contacts (false for triggers for instance)
bool reportContacts;
/// Result of the narrow-phase collision detection test
bool isColliding;
/// Number of contact points
uint8 nbContactPoints;
/// List of contact points created during the narrow-phase
ContactPointInfo contactPoints[NB_MAX_CONTACT_POINTS_IN_NARROWPHASE_INFO];
/// Constructor
NarrowPhaseInfo(uint64 pairId, Entity collider1, Entity collider2, LastFrameCollisionInfo* lastFrameInfo, MemoryAllocator& shapeAllocator,
const Transform& shape1ToWorldTransform, const Transform& shape2ToWorldTransform, CollisionShape* shape1,
CollisionShape* shape2, bool needToReportContacts)
: overlappingPairId(pairId), colliderEntity1(collider1), colliderEntity2(collider2), lastFrameCollisionInfo(lastFrameInfo),
collisionShapeAllocator(&shapeAllocator), shape1ToWorldTransform(shape1ToWorldTransform),
shape2ToWorldTransform(shape2ToWorldTransform), collisionShape1(shape1),
collisionShape2(shape2), reportContacts(needToReportContacts), isColliding(false), nbContactPoints(0) {
}
};
protected:
/// Memory allocator
/// Reference to the memory allocator
MemoryAllocator& mMemoryAllocator;
/// Reference to all the broad-phase overlapping pairs
@ -60,73 +113,40 @@ struct NarrowPhaseInfoBatch {
public:
/// List of Broadphase overlapping pairs ids
List<uint64> overlappingPairIds;
/// List of pointers to the first colliders to test collision with
List<Entity> colliderEntities1;
/// List of pointers to the second colliders to test collision with
List<Entity> colliderEntities2;
/// List of pointers to the first collision shapes to test collision with
List<CollisionShape*> collisionShapes1;
/// List of pointers to the second collision shapes to test collision with
List<CollisionShape*> collisionShapes2;
/// List of transforms that maps from collision shape 1 local-space to world-space
List<Transform> shape1ToWorldTransforms;
/// List of transforms that maps from collision shape 2 local-space to world-space
List<Transform> shape2ToWorldTransforms;
/// True for each pair of objects that we need to report contacts (false for triggers for instance)
List<bool> reportContacts;
/// Result of the narrow-phase collision detection test
List<bool> isColliding;
/// List of contact points created during the narrow-phase
List<List<ContactPointInfo*>> contactPoints;
/// Memory allocators for the collision shape (Used to release TriangleShape memory in destructor)
List<MemoryAllocator*> collisionShapeAllocators;
/// Collision infos of the previous frame
List<LastFrameCollisionInfo*> lastFrameCollisionInfos;
/// For each collision test, we keep some meta data
List<NarrowPhaseInfo> narrowPhaseInfos;
/// Constructor
NarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs);
NarrowPhaseInfoBatch(OverlappingPairs& overlappingPairs, MemoryAllocator& allocator);
/// Destructor
virtual ~NarrowPhaseInfoBatch();
~NarrowPhaseInfoBatch();
/// Add shapes to be tested during narrow-phase collision detection into the batch
void addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1,
CollisionShape* shape2, const Transform& shape1Transform, const Transform& shape2Transform,
bool needToReportContacts, MemoryAllocator& shapeAllocator);
/// Return the number of objects in the batch
uint getNbObjects() const;
/// Add shapes to be tested during narrow-phase collision detection into the batch
virtual void addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1,
CollisionShape* shape2, const Transform& shape1Transform,
const Transform& shape2Transform, bool needToReportContacts, MemoryAllocator& shapeAllocator);
/// Add a new contact point
virtual void addContactPoint(uint index, const Vector3& contactNormal, decimal penDepth,
void addContactPoint(uint index, const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2);
/// Reset the remaining contact points
void resetContactPoints(uint index);
// Initialize the containers using cached capacity
virtual void reserveMemory();
void reserveMemory();
/// Clear all the objects in the batch
virtual void clear();
void clear();
};
/// Return the number of objects in the batch
inline uint NarrowPhaseInfoBatch::getNbObjects() const {
return overlappingPairIds.size();
return narrowPhaseInfos.size();
}
}

23
include/reactphysics3d/collision/narrowphase/NarrowPhaseInput.h
View File

@ -29,9 +29,6 @@
// Libraries
#include <reactphysics3d/containers/List.h>
#include <reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h>
#include <reactphysics3d/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h>
#include <reactphysics3d/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h>
#include <reactphysics3d/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h>
/// Namespace ReactPhysics3D
namespace reactphysics3d {
@ -55,9 +52,9 @@ class NarrowPhaseInput {
private:
SphereVsSphereNarrowPhaseInfoBatch mSphereVsSphereBatch;
SphereVsCapsuleNarrowPhaseInfoBatch mSphereVsCapsuleBatch;
CapsuleVsCapsuleNarrowPhaseInfoBatch mCapsuleVsCapsuleBatch;
NarrowPhaseInfoBatch mSphereVsSphereBatch;
NarrowPhaseInfoBatch mSphereVsCapsuleBatch;
NarrowPhaseInfoBatch mCapsuleVsCapsuleBatch;
NarrowPhaseInfoBatch mSphereVsConvexPolyhedronBatch;
NarrowPhaseInfoBatch mCapsuleVsConvexPolyhedronBatch;
NarrowPhaseInfoBatch mConvexPolyhedronVsConvexPolyhedronBatch;
@ -74,13 +71,13 @@ class NarrowPhaseInput {
MemoryAllocator& shapeAllocator);
/// Get a reference to the sphere vs sphere batch
SphereVsSphereNarrowPhaseInfoBatch& getSphereVsSphereBatch();
NarrowPhaseInfoBatch& getSphereVsSphereBatch();
/// Get a reference to the sphere vs capsule batch
SphereVsCapsuleNarrowPhaseInfoBatch& getSphereVsCapsuleBatch();
NarrowPhaseInfoBatch& getSphereVsCapsuleBatch();
/// Get a reference to the capsule vs capsule batch
CapsuleVsCapsuleNarrowPhaseInfoBatch& getCapsuleVsCapsuleBatch();
NarrowPhaseInfoBatch& getCapsuleVsCapsuleBatch();
/// Get a reference to the sphere vs convex polyhedron batch
NarrowPhaseInfoBatch& getSphereVsConvexPolyhedronBatch();
@ -100,17 +97,17 @@ class NarrowPhaseInput {
// Get a reference to the sphere vs sphere batch contacts
inline SphereVsSphereNarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsSphereBatch() {
inline NarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsSphereBatch() {
return mSphereVsSphereBatch;
}
// Get a reference to the sphere vs capsule batch contacts
inline SphereVsCapsuleNarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsCapsuleBatch() {
inline NarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsCapsuleBatch() {
return mSphereVsCapsuleBatch;
}
// Get a reference to the capsule vs capsule batch contacts
inline CapsuleVsCapsuleNarrowPhaseInfoBatch& NarrowPhaseInput::getCapsuleVsCapsuleBatch() {
inline NarrowPhaseInfoBatch& NarrowPhaseInput::getCapsuleVsCapsuleBatch() {
return mCapsuleVsCapsuleBatch;
}
@ -125,7 +122,7 @@ inline NarrowPhaseInfoBatch& NarrowPhaseInput::getCapsuleVsConvexPolyhedronBatch
}
// Get a reference to the convex polyhedron vs convex polyhedron batch contacts
inline NarrowPhaseInfoBatch& NarrowPhaseInput::getConvexPolyhedronVsConvexPolyhedronBatch() {
inline NarrowPhaseInfoBatch &NarrowPhaseInput::getConvexPolyhedronVsConvexPolyhedronBatch() {
return mConvexPolyhedronVsConvexPolyhedronBatch;
}

4
include/reactphysics3d/collision/narrowphase/SphereVsCapsuleAlgorithm.h
View File

@ -34,7 +34,7 @@ namespace reactphysics3d {
// Declarations
class ContactPoint;
struct SphereVsCapsuleNarrowPhaseInfoBatch;
struct NarrowPhaseInfoBatch;
// Class SphereVsCapsuleAlgorithm
/**
@ -65,7 +65,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(SphereVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
uint batchNbItems, MemoryAllocator& memoryAllocator);
};

78
include/reactphysics3d/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h
View File

@ -1,78 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2020 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 <reactphysics3d/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, OverlappingPairs& overlappingPairs);
/// Destructor
virtual ~SphereVsCapsuleNarrowPhaseInfoBatch() override = default;
/// Add shapes to be tested during narrow-phase collision detection into the batch
virtual void addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1,
CollisionShape* shape2, const Transform& shape1Transform,
const Transform& shape2Transform, bool needToReportContacts, MemoryAllocator& shapeAllocator) override;
// Initialize the containers using cached capacity
virtual void reserveMemory() override;
/// Clear all the objects in the batch
virtual void clear() override;
};
}
#endif

1
include/reactphysics3d/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
View File

@ -34,6 +34,7 @@ namespace reactphysics3d {
// Declarations
class ContactPoint;
struct NarrowPhaseInfoBatch;
// Class SphereVsConvexPolyhedronAlgorithm
/**

4
include/reactphysics3d/collision/narrowphase/SphereVsSphereAlgorithm.h
View File

@ -34,7 +34,7 @@ namespace reactphysics3d {
// Declarations
class ContactPoint;
struct SphereVsSphereNarrowPhaseInfoBatch;
struct NarrowPhaseInfoBatch;
// Class SphereVsSphereAlgorithm
/**
@ -65,7 +65,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volume collide
bool testCollision(SphereVsSphereNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
uint batchNbItems, MemoryAllocator& memoryAllocator);
};

72
include/reactphysics3d/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h
View File

@ -1,72 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2020 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_SPHERE_VS_SPHERE_NARROW_PHASE_INFO_BATCH_H
#define REACTPHYSICS3D_SPHERE_VS_SPHERE_NARROW_PHASE_INFO_BATCH_H
// Libraries
#include <reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h>
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Struct SphereVsSphereNarrowPhaseInfoBatch
/**
* This structure collects all the potential collisions from the middle-phase algorithm
* that have to be tested during narrow-phase collision detection. This class collects all the
* sphere vs sphere collision detection tests.
*/
struct SphereVsSphereNarrowPhaseInfoBatch : public NarrowPhaseInfoBatch {
public:
/// List of radiuses for the first spheres
List<decimal> sphere1Radiuses;
/// List of radiuses for the second spheres
List<decimal> sphere2Radiuses;
/// Constructor
SphereVsSphereNarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs);
/// Destructor
virtual ~SphereVsSphereNarrowPhaseInfoBatch() override = default;
/// Add shapes to be tested during narrow-phase collision detection into the batch
virtual void addNarrowPhaseInfo(uint64 airId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1,
CollisionShape* shape2, const Transform& shape1Transform,
const Transform& shape2Transform, bool needToReportContacts, MemoryAllocator& shapeAllocator) override;
// Initialize the containers using cached capacity
virtual void reserveMemory() override;
/// Clear all the objects in the batch
virtual void clear() override;
};
}
#endif

29
include/reactphysics3d/configuration.h
View File

@ -35,15 +35,35 @@
#include <reactphysics3d/decimal.h>
#include <reactphysics3d/containers/Pair.h>
// Windows platform
// Platforms
#if defined(WIN32) ||defined(_WIN32) || defined(_WIN64) ||defined(__WIN32__) || defined(__WINDOWS__)
#define WINDOWS_OS
#elif defined(__APPLE__) // Apple platform
#elif defined(__APPLE__)
#define APPLE_OS
#elif defined(__linux__) || defined(linux) || defined(__linux) // Linux platform
#define LINUX_OS
#endif
// Compilers
#if defined(_MSC_VER)
#define RP3D_COMPILER_VISUAL_STUDIO
#elif defined(__clang__)
#define RP3D_COMPILER_CLANG
#elif defined(__GNUC__)
#define RP3D_COMPILER_GCC
#else
#define RP3D_COMPILER_UNKNOWN
#endif
// Force inline macro
#if defined(RP3D_COMPILER_VISUAL_STUDIO)
#define RP3D_FORCE_INLINE __forceinline
#elif defined(RP3D_COMPILER_GCC) || defined(RP3D_COMPILER_CLANG)
#define RP3D_FORCE_INLINE inline __attribute__((always_inline))
#else
#define RP3D_FORCE_INLINE inline
#endif
/// Namespace reactphysics3d
namespace reactphysics3d {
@ -103,6 +123,9 @@ constexpr decimal PI_TIMES_2 = decimal(6.28318530);
/// without triggering a large modification of the tree each frame which can be costly
constexpr decimal DYNAMIC_TREE_FAT_AABB_INFLATE_PERCENTAGE = decimal(0.08);
/// Maximum number of contact points in a narrow phase info object
constexpr uint8 NB_MAX_CONTACT_POINTS_IN_NARROWPHASE_INFO = 16;
/// Maximum number of contact manifolds in an overlapping pair
constexpr uint8 NB_MAX_CONTACT_MANIFOLDS = 3;
@ -110,7 +133,7 @@ constexpr uint8 NB_MAX_CONTACT_MANIFOLDS = 3;
constexpr uint8 NB_MAX_POTENTIAL_CONTACT_MANIFOLDS = 4 * NB_MAX_CONTACT_MANIFOLDS;
/// Maximum number of contact points in potential contact manifold
constexpr uint8 NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD = 12;
constexpr uint8 NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD = 16;
/// Current version of ReactPhysics3D
const std::string RP3D_VERSION = std::string("0.8.0");

10
include/reactphysics3d/engine/OverlappingPairs.h
View File

@ -54,6 +54,8 @@ class CollisionDispatch;
*/
struct LastFrameCollisionInfo {
// TODO OPTI : Use bit flags instead of bools here
/// True if we have information about the previous frame
bool isValid;
@ -77,9 +79,9 @@ struct LastFrameCollisionInfo {
// SAT Algorithm
bool satIsAxisFacePolyhedron1;
bool satIsAxisFacePolyhedron2;
uint satMinAxisFaceIndex;
uint satMinEdge1Index;
uint satMinEdge2Index;
uint8 satMinAxisFaceIndex;
uint8 satMinEdge1Index;
uint8 satMinEdge2Index;
/// Constructor
LastFrameCollisionInfo() {
@ -118,7 +120,7 @@ class OverlappingPairs {
MemoryAllocator& mPersistentAllocator;
/// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo
// TODO : Do we need to keep this ?
// TODO OPTI : Do we need to keep this ?
MemoryAllocator& mTempMemoryAllocator;
/// Current number of components

43
src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
View File

@ -26,7 +26,7 @@
// Libraries
#include <reactphysics3d/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h>
#include <reactphysics3d/collision/shapes/CapsuleShape.h>
#include <reactphysics3d/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h>
#include <reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
@ -34,30 +34,37 @@ 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(CapsuleVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
MemoryAllocator& memoryAllocator) {
bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, MemoryAllocator& memoryAllocator) {
bool isCollisionFound = false;
for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0);
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].nbContactPoints == 0);
assert(!narrowPhaseInfoBatch.isColliding[batchIndex]);
assert(!narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding);
// Get the transform from capsule 1 local-space to capsule 2 local-space
const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getInverse() *
narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getInverse() *
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform;
const CapsuleShape* capsuleShape1 = static_cast<CapsuleShape*>(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1);
const CapsuleShape* capsuleShape2 = static_cast<CapsuleShape*>(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2);
const decimal capsule1Height = capsuleShape1->getHeight();
const decimal capsule2Height = capsuleShape2->getHeight();
const decimal capsule1Radius = capsuleShape1->getRadius();
const decimal capsule2Radius = capsuleShape2->getRadius();
// Compute the end-points of the inner segment of the first capsule
const decimal capsule1HalfHeight = narrowPhaseInfoBatch.capsule1Heights[batchIndex] * decimal(0.5);
const decimal capsule1HalfHeight = capsule1Height * 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 decimal capsule2HalfHeight = narrowPhaseInfoBatch.capsule2Heights[batchIndex] * decimal(0.5);
const decimal capsule2HalfHeight = capsule2Height * decimal(0.5);
const Vector3 capsule2SegA(0, -capsule2HalfHeight, 0);
const Vector3 capsule2SegB(0, capsule2HalfHeight, 0);
@ -66,8 +73,6 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat
const Vector3 seg2 = capsule2SegB - capsule2SegA;
// Compute the sum of the radius of the two capsules (virtual spheres)
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)
@ -95,7 +100,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat
// If the segments were overlapping (the clip segment is valid)
if (t1 > decimal(0.0) && t2 > decimal(0.0)) {
if (narrowPhaseInfoBatch.reportContacts[batchIndex]) {
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
// Clip the inner segment of capsule 2
if (t1 > decimal(1.0)) t1 = decimal(1.0);
@ -145,14 +150,14 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat
decimal penetrationDepth = sumRadius - segmentsPerpendicularDistance;
const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsule2SpaceNormalized;
const Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * normalCapsule2SpaceNormalized;
// Create the contact info object
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
}
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
isCollisionFound = true;
continue;
}
@ -171,7 +176,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat
// If the collision shapes overlap
if (closestPointsDistanceSquare < sumRadius * sumRadius) {
if (narrowPhaseInfoBatch.reportContacts[batchIndex]) {
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
// If the distance between the inner segments is not zero
if (closestPointsDistanceSquare > MACHINE_EPSILON) {
@ -182,7 +187,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsule1Radius);
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsule2Radius;
const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * closestPointsSeg1ToSeg2;
const Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2;
decimal penetrationDepth = sumRadius - closestPointsDistance;
@ -205,7 +210,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsule1Radius);
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsule2Radius;
const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsuleSpace2;
const Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2;
// Create the contact info object
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local);
@ -221,7 +226,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsule1Radius);
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsule2Radius;
const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsuleSpace2;
const Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2;
// Create the contact info object
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local);
@ -229,7 +234,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat
}
}
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
isCollisionFound = true;
}
}

84
src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.cpp
View File

@ -1,84 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2020 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 <reactphysics3d/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h>
#include <reactphysics3d/collision/shapes/CapsuleShape.h>
using namespace reactphysics3d;
// Constructor
CapsuleVsCapsuleNarrowPhaseInfoBatch::CapsuleVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator,
OverlappingPairs& overlappingPairs)
: NarrowPhaseInfoBatch(allocator, overlappingPairs), capsule1Radiuses(allocator), capsule2Radiuses(allocator),
capsule1Heights(allocator), capsule2Heights(allocator) {
}
// Add shapes to be tested during narrow-phase collision detection into the batch
void CapsuleVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, CollisionShape* shape2,
const Transform& shape1Transform, const Transform& shape2Transform, bool needToReportContacts, MemoryAllocator &shapeAllocator) {
NarrowPhaseInfoBatch::addNarrowPhaseInfo(pairId, pairIndex, collider1, collider2, shape1, shape2, shape1Transform,
shape2Transform, needToReportContacts, shapeAllocator);
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());
}
// 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() {
// 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)
NarrowPhaseInfoBatch::clear();
capsule1Radiuses.clear(true);
capsule2Radiuses.clear(true);
capsule1Heights.clear(true);
capsule2Heights.clear(true);
}

42
src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
View File

@ -66,21 +66,21 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar
for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
// Get the last frame collision info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.lastFrameCollisionInfos[batchIndex];
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].lastFrameCollisionInfo;
lastFrameCollisionInfo->wasUsingGJK = true;
lastFrameCollisionInfo->wasUsingSAT = false;
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE ||
narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CAPSULE);
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CAPSULE ||
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CAPSULE);
// If we have found a contact point inside the margins (shallow penetration)
if (gjkResults[batchIndex] == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
// If we need to report contacts
if (narrowPhaseInfoBatch.reportContacts[batchIndex]) {
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
bool noContact = false;
@ -89,20 +89,20 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar
// two contact points instead of a single one (as in the deep contact case with SAT algorithm)
// Get the contact point created by GJK
assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() > 0);
ContactPointInfo*& contactPoint = narrowPhaseInfoBatch.contactPoints[batchIndex][0];
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].nbContactPoints > 0);
ContactPointInfo& contactPoint = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].contactPoints[0];
bool isCapsuleShape1 = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE;
bool isCapsuleShape1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CAPSULE;
// Get the collision shapes
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes1[batchIndex] : narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes2[batchIndex] : narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1);
// For each face of the polyhedron
for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform;
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform;
// Get the face normal
const Vector3 faceNormal = polyhedron->getFaceNormal(f);
@ -113,18 +113,18 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar
Vector3 capsuleInnerSegmentDirection = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA);
capsuleInnerSegmentDirection.normalize();
bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint->normal) > decimal(0.0);
bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint.normal) > decimal(0.0);
bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal);
// If the polyhedron face normal is orthogonal to the capsule inner segment and parallel to the contact point normal and the face normal
// is in direction of the contact normal (from the polyhedron point of view).
if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentDirection)
&& areParallelVectors(faceNormalWorld, contactPoint->normal)) {
&& areParallelVectors(faceNormalWorld, contactPoint.normal)) {
// Remove the previous contact point computed by GJK
narrowPhaseInfoBatch.resetContactPoints(batchIndex);
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform;
const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
// Compute the end-points of the inner segment of the capsule
@ -143,13 +143,13 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar
}
// Compute and create two contact points
bool contactsFound = satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
bool contactsFound = satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint.penetrationDepth,
polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
narrowPhaseInfoBatch, batchIndex, isCapsuleShape1);
if (!contactsFound) {
noContact = true;
narrowPhaseInfoBatch.isColliding[batchIndex] = false;
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = false;
break;
}
@ -166,7 +166,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar
lastFrameCollisionInfo->wasUsingGJK = false;
// Colision found
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
isCollisionFound = true;
continue;
}
@ -175,12 +175,12 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar
if (gjkResults[batchIndex] == GJKAlgorithm::GJKResult::INTERPENETRATE) {
// Run the SAT algorithm to find the separating axis and compute contact point
narrowPhaseInfoBatch.isColliding[batchIndex] = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfoBatch, batchIndex);
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfoBatch, batchIndex);
lastFrameCollisionInfo->wasUsingGJK = false;
lastFrameCollisionInfo->wasUsingSAT = true;
if (narrowPhaseInfoBatch.isColliding[batchIndex]) {
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding) {
isCollisionFound = true;
}
}

4
src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
View File

@ -35,7 +35,7 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between two convex polyhedra
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch &narrowPhaseInfoBatch,
uint batchStartIndex, uint batchNbItems,
bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator) {
@ -54,7 +54,7 @@ bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoB
for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
// Get the last frame collision info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.lastFrameCollisionInfos[batchIndex];
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].lastFrameCollisionInfo;
lastFrameCollisionInfo->wasUsingSAT = true;
lastFrameCollisionInfo->wasUsingGJK = false;

16
src/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

@ -64,15 +64,15 @@ void GJKAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uin
decimal prevDistSquare;
bool contactFound = false;
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->isConvex());
assert(narrowPhaseInfoBatch.collisionShapes2[batchIndex]->isConvex());
assert(narrowPhaseInfoBatch.narrowPhaseInfo[[batchIndex].collisionShape1->isConvex());
assert(narrowPhaseInfoBatch.narrowPhaseInfo[[batchIndex].collisionShape2->isConvex());
const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2);
// Get the local-space to world-space transforms
const Transform& transform1 = narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform& transform2 = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
const Transform& transform1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform;
const Transform& transform2 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform;
// Transform a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1)
@ -92,7 +92,7 @@ void GJKAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uin
VoronoiSimplex simplex;
// Get the last collision frame info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.lastFrameCollisionInfos[batchIndex];
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].lastFrameCollisionInfo;
// Get the previous point V (last cached separating axis)
Vector3 v;
@ -215,7 +215,7 @@ void GJKAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uin
}
// If we need to report contacts
if (narrowPhaseInfoBatch.reportContacts[batchIndex]) {
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(shape1, shape2, pA, pB, transform1, transform2,

113
src/collision/narrowphase/NarrowPhaseInfoBatch.cpp
View File

@ -33,12 +33,8 @@
using namespace reactphysics3d;
// Constructor
NarrowPhaseInfoBatch::NarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs)
: mMemoryAllocator(allocator), mOverlappingPairs(overlappingPairs), overlappingPairIds(allocator),
colliderEntities1(allocator), colliderEntities2(allocator), collisionShapes1(allocator), collisionShapes2(allocator),
shape1ToWorldTransforms(allocator), shape2ToWorldTransforms(allocator), reportContacts(allocator),
isColliding(allocator), contactPoints(allocator), collisionShapeAllocators(allocator),
lastFrameCollisionInfos(allocator) {
NarrowPhaseInfoBatch::NarrowPhaseInfoBatch(OverlappingPairs& overlappingPairs, MemoryAllocator& allocator)
: mMemoryAllocator(allocator), mOverlappingPairs(overlappingPairs), narrowPhaseInfos(allocator){
}
@ -48,99 +44,65 @@ NarrowPhaseInfoBatch::~NarrowPhaseInfoBatch() {
}
// Add shapes to be tested during narrow-phase collision detection into the batch
void NarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, CollisionShape* shape2,
const Transform& shape1Transform, const Transform& shape2Transform, bool needToReportContacts,
MemoryAllocator& shapeAllocator) {
overlappingPairIds.add(pairId);
colliderEntities1.add(collider1);
colliderEntities2.add(collider2);
collisionShapes1.add(shape1);
collisionShapes2.add(shape2);
shape1ToWorldTransforms.add(shape1Transform);
shape2ToWorldTransforms.add(shape2Transform);
reportContacts.add(needToReportContacts);
collisionShapeAllocators.add(&shapeAllocator);
contactPoints.add(List<ContactPointInfo*>(mMemoryAllocator));
isColliding.add(false);
void NarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1,
CollisionShape* shape2, const Transform& shape1Transform, const Transform& shape2Transform,
bool needToReportContacts, MemoryAllocator& shapeAllocator) {
// Add a collision info for the two collision shapes into the overlapping pair (if not present yet)
// TODO OPTI : Can we better manage this
LastFrameCollisionInfo* lastFrameInfo = mOverlappingPairs.addLastFrameInfoIfNecessary(pairIndex, shape1->getId(), shape2->getId());
lastFrameCollisionInfos.add(lastFrameInfo);
// Create a meta data object
narrowPhaseInfos.emplace(pairId, collider1, collider2, lastFrameInfo, shapeAllocator, shape1Transform, shape2Transform, shape1, shape2, needToReportContacts);
}
// Add a new contact point
void NarrowPhaseInfoBatch::addContactPoint(uint index, const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2) {
void NarrowPhaseInfoBatch::addContactPoint(uint index, const Vector3& contactNormal, decimal penDepth, const Vector3& localPt1, const Vector3& localPt2) {
assert(reportContacts[index]);
assert(penDepth > decimal(0.0));
// Get the memory allocator
MemoryAllocator& allocator = mOverlappingPairs.getTemporaryAllocator();
if (narrowPhaseInfos[index].nbContactPoints < NB_MAX_CONTACT_POINTS_IN_NARROWPHASE_INFO) {
// Create the contact point info
ContactPointInfo* contactPointInfo = new (allocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(contactNormal, penDepth, localPt1, localPt2);
assert(contactNormal.length() > 0.8f);
// Add it into the list of contact points
contactPoints[index].add(contactPointInfo);
// Add it into the array of contact points
narrowPhaseInfos[index].contactPoints[narrowPhaseInfos[index].nbContactPoints].normal = contactNormal;
narrowPhaseInfos[index].contactPoints[narrowPhaseInfos[index].nbContactPoints].penetrationDepth = penDepth;
narrowPhaseInfos[index].contactPoints[narrowPhaseInfos[index].nbContactPoints].localPoint1 = localPt1;
narrowPhaseInfos[index].contactPoints[narrowPhaseInfos[index].nbContactPoints].localPoint2 = localPt2;
narrowPhaseInfos[index].nbContactPoints++;
}
}
// Reset the remaining contact points
void NarrowPhaseInfoBatch::resetContactPoints(uint index) {
// Get the memory allocator
MemoryAllocator& allocator = mOverlappingPairs.getTemporaryAllocator();
// For each remaining contact point info
for (uint i=0; i < contactPoints[index].size(); i++) {
ContactPointInfo* contactPoint = contactPoints[index][i];
// Call the destructor
contactPoint->~ContactPointInfo();
// Delete the current element
allocator.release(contactPoint, sizeof(ContactPointInfo));
}
contactPoints[index].clear();
narrowPhaseInfos[index].nbContactPoints = 0;
}
// Initialize the containers using cached capacity
void NarrowPhaseInfoBatch::reserveMemory() {
overlappingPairIds.reserve(mCachedCapacity);
colliderEntities1.reserve(mCachedCapacity);
colliderEntities2.reserve(mCachedCapacity);
collisionShapes1.reserve(mCachedCapacity);
collisionShapes2.reserve(mCachedCapacity);
shape1ToWorldTransforms.reserve(mCachedCapacity);
shape2ToWorldTransforms.reserve(mCachedCapacity);
reportContacts.reserve(mCachedCapacity);
collisionShapeAllocators.reserve(mCachedCapacity);
lastFrameCollisionInfos.reserve(mCachedCapacity);
isColliding.reserve(mCachedCapacity);
contactPoints.reserve(mCachedCapacity);
narrowPhaseInfos.reserve(mCachedCapacity);
}
// Clear all the objects in the batch
void NarrowPhaseInfoBatch::clear() {
for (uint i=0; i < overlappingPairIds.size(); i++) {
// TODO OPTI : Better manage this
for (uint i=0; i < narrowPhaseInfos.size(); i++) {
assert(contactPoints[i].size() == 0);
assert(narrowPhaseOutputInfos[i].nbContactPoints == 0);
// Release the memory of the TriangleShape (this memory was allocated in the
// MiddlePhaseTriangleCallback::testTriangle() method)
if (collisionShapes1.size() > 0 && collisionShapes1[i]->getName() == CollisionShapeName::TRIANGLE) {
collisionShapes1[i]->~CollisionShape();
collisionShapeAllocators[i]->release(collisionShapes1[i], sizeof(TriangleShape));
if (narrowPhaseInfos[i].collisionShape1->getName() == CollisionShapeName::TRIANGLE) {
narrowPhaseInfos[i].collisionShape1->~CollisionShape();
narrowPhaseInfos[i].collisionShapeAllocator->release(narrowPhaseInfos[i].collisionShape1, sizeof(TriangleShape));
}
if (collisionShapes2.size() > 0 && collisionShapes2[i]->getName() == CollisionShapeName::TRIANGLE) {
collisionShapes2[i]->~CollisionShape();
collisionShapeAllocators[i]->release(collisionShapes2[i], sizeof(TriangleShape));
if (narrowPhaseInfos[i].collisionShape2->getName() == CollisionShapeName::TRIANGLE) {
narrowPhaseInfos[i].collisionShape2->~CollisionShape();
narrowPhaseInfos[i].collisionShapeAllocator->release(narrowPhaseInfos[i].collisionShape2, sizeof(TriangleShape));
}
}
@ -149,18 +111,7 @@ void NarrowPhaseInfoBatch::clear() {
// 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)
mCachedCapacity = overlappingPairIds.size();
mCachedCapacity = narrowPhaseInfos.size();
overlappingPairIds.clear(true);
colliderEntities1.clear(true);
colliderEntities2.clear(true);
collisionShapes1.clear(true);
collisionShapes2.clear(true);
shape1ToWorldTransforms.clear(true);
shape2ToWorldTransforms.clear(true);
reportContacts.clear(true);
collisionShapeAllocators.clear(true);
lastFrameCollisionInfos.clear(true);
isColliding.clear(true);
contactPoints.clear(true);
narrowPhaseInfos.clear(true);
}

8
src/collision/narrowphase/NarrowPhaseInput.cpp
View File

@ -31,10 +31,10 @@ using namespace reactphysics3d;
/// Constructor
NarrowPhaseInput::NarrowPhaseInput(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs)
:mSphereVsSphereBatch(allocator, overlappingPairs), mSphereVsCapsuleBatch(allocator, overlappingPairs),
mCapsuleVsCapsuleBatch(allocator, overlappingPairs), mSphereVsConvexPolyhedronBatch(allocator, overlappingPairs),
mCapsuleVsConvexPolyhedronBatch(allocator, overlappingPairs),
mConvexPolyhedronVsConvexPolyhedronBatch(allocator, overlappingPairs) {
:mSphereVsSphereBatch(overlappingPairs, allocator), mSphereVsCapsuleBatch(overlappingPairs, allocator),
mCapsuleVsCapsuleBatch(overlappingPairs, allocator), mSphereVsConvexPolyhedronBatch(overlappingPairs, allocator),
mCapsuleVsConvexPolyhedronBatch(overlappingPairs, allocator),
mConvexPolyhedronVsConvexPolyhedronBatch(overlappingPairs, allocator) {
}

104
src/collision/narrowphase/SAT/SATAlgorithm.cpp
View File

@ -64,19 +64,19 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& n
for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
bool isSphereShape1 = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE;
bool isSphereShape1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::SPHERE;
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE ||
narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::SPHERE);
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::SPHERE ||
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::SPHERE);
// Get the capsule collision shapes
const SphereShape* sphere = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfoBatch.collisionShapes1[batchIndex] : narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isSphereShape1 ? narrowPhaseInfoBatch.collisionShapes2[batchIndex] : narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const SphereShape* sphere = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1);
const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
const Transform& polyhedronToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform;
const Transform& polyhedronToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform;
// Get the transform from sphere local-space to polyhedron local-space
const Transform worldToPolyhedronTransform = polyhedronToWorldTransform.getInverse();
@ -115,7 +115,7 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& n
}
// If we need to report contacts
if (narrowPhaseInfoBatch.reportContacts[batchIndex]) {
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex);
Vector3 minFaceNormalWorld = polyhedronToWorldTransform.getOrientation() * minFaceNormal;
@ -125,10 +125,10 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& n
Vector3 normalWorld = isSphereShape1 ? -minFaceNormalWorld : minFaceNormalWorld;
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2,
isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal,
narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform,
minPenetrationDepth, normalWorld);
// Create the contact info object
@ -137,7 +137,7 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& n
isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
}
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
isCollisionFound = true;
}
@ -167,19 +167,19 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfoBatch&
RP3D_PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()", mProfiler);
bool isCapsuleShape1 = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE;
bool isCapsuleShape1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CAPSULE;
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE ||
narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CAPSULE);
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CAPSULE ||
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CAPSULE);
// Get the collision shapes
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes1[batchIndex] : narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes2[batchIndex] : narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1);
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform;
const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform;
const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
@ -279,7 +279,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfoBatch&
if (isMinPenetrationFaceNormal) {
// If we need to report contacts
if (narrowPhaseInfoBatch.reportContacts[batchIndex]) {
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
return computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth,
polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace,
@ -290,7 +290,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfoBatch&
else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment
// If we need to report contacts
if (narrowPhaseInfoBatch.reportContacts[batchIndex]) {
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
// Compute the closest points between the inner capsule segment and the
// edge of the polyhedron in polyhedron local-space
@ -303,10 +303,10 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfoBatch&
Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius;
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2,
isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule,
narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform,
minPenetrationDepth, normalWorld);
// Create the contact point
@ -448,10 +448,10 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2,
isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron,
isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule,
narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform,
penetrationDepth, normalWorld);
@ -486,14 +486,14 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0);
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].nbContactPoints == 0);
const ConvexPolyhedronShape* polyhedron1 = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const ConvexPolyhedronShape* polyhedron2 = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
const ConvexPolyhedronShape* polyhedron1 = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1);
const ConvexPolyhedronShape* polyhedron2 = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2);
const Transform polyhedron1ToPolyhedron2 = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getInverse() * narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform polyhedron1ToPolyhedron2 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getInverse() * narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform;
const Transform polyhedron2ToPolyhedron1 = polyhedron1ToPolyhedron2.getInverse();
decimal minPenetrationDepth = DECIMAL_LARGEST;
@ -507,7 +507,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
const bool isShape1Triangle = polyhedron1->getName() == CollisionShapeName::TRIANGLE;
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.lastFrameCollisionInfos[batchIndex];
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].lastFrameCollisionInfo;
// If the last frame collision info is valid and was also using SAT algorithm
if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingSAT) {
@ -549,7 +549,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// The shapes are still overlapping in the previous axis (the contact manifold is not empty).
// Therefore, we can return without running the whole SAT algorithm
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
isCollisionFound = true;
continue;
}
@ -589,7 +589,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// The shapes are still overlapping in the previous axis (the contact manifold is not empty).
// Therefore, we can return without running the whole SAT algorithm
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
isCollisionFound = true;
continue;
}
@ -651,18 +651,18 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
if (t1 >= decimal(0.0) && t1 <= decimal(1) && t2 >= decimal(0.0) && t2 <= decimal(1.0)) {
// If we need to report contact points
if (narrowPhaseInfoBatch.reportContacts[batchIndex]) {
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
// Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1
Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
// Compute the world normal
Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * separatingAxisPolyhedron2Space;
Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * separatingAxisPolyhedron2Space;
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge,
narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform,
penetrationDepth, normalWorld);
// Create the contact point
@ -673,7 +673,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// The shapes are overlapping on the previous axis (the contact manifold is not empty). Therefore
// we return without running the whole SAT algorithm
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
isCollisionFound = true;
continue;
}
@ -848,7 +848,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
else { // If we have an edge vs edge contact
// If we need to report contacts
if (narrowPhaseInfoBatch.reportContacts[batchIndex]) {
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
// Compute the closest points between the two edges (in the local-space of poylhedron 2)
Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge;
@ -859,12 +859,12 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
// Compute the world normal
Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge,
narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform,
minPenetrationDepth, normalWorld);
// Create the contact point
@ -878,7 +878,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
lastFrameCollisionInfo->satMinEdge2Index = minSeparatingEdge2Index;
}
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
isCollisionFound = true;
}
@ -903,8 +903,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace;
// Compute the world normal
Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex].getOrientation() * axisReferenceSpace :
-(narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * axisReferenceSpace);
Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform.getOrientation() * axisReferenceSpace :
-(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * axisReferenceSpace);
// Get the reference face
const HalfEdgeStructure::Face& referenceFace = referencePolyhedron->getFace(minFaceIndex);
@ -975,7 +975,7 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
contactPointsFound = true;
// If we need to report contacts
if (narrowPhaseInfoBatch.reportContacts[batchIndex]) {
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
Vector3 outWorldNormal = normalWorld;
@ -986,10 +986,10 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(clipPolygonVertices[i], axisReferenceSpace, referenceFaceVertex);
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron,
narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform,
penetrationDepth, outWorldNormal);
// Create a new contact point

38
src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
View File

@ -27,7 +27,7 @@
#include <reactphysics3d/collision/narrowphase/SphereVsCapsuleAlgorithm.h>
#include <reactphysics3d/collision/shapes/SphereShape.h>
#include <reactphysics3d/collision/shapes/CapsuleShape.h>
#include <reactphysics3d/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h>
#include <reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
@ -35,21 +35,27 @@ 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(SphereVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
MemoryAllocator& memoryAllocator) {
bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, MemoryAllocator& memoryAllocator) {
bool isCollisionFound = false;
for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
assert(!narrowPhaseInfoBatch.isColliding[batchIndex]);
assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0);
assert(!narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding);
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].nbContactPoints == 0);
const bool isSphereShape1 = narrowPhaseInfoBatch.isSpheresShape1[batchIndex];
const bool isSphereShape1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::SPHERE;
const SphereShape* sphereShape = static_cast<SphereShape*>(isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2);
const CapsuleShape* capsuleShape = static_cast<CapsuleShape*>(isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1);
const decimal capsuleHeight = capsuleShape->getHeight();
const decimal sphereRadius = sphereShape->getRadius();
const decimal capsuleRadius = capsuleShape->getRadius();
// Get the transform from sphere local-space to capsule local-space
const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
const Transform& capsuleToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform;
const Transform& capsuleToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform;
const Transform worldToCapsuleTransform = capsuleToWorldTransform.getInverse();
const Transform sphereToCapsuleSpaceTransform = worldToCapsuleTransform * sphereToWorldTransform;
@ -57,7 +63,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch
const Vector3 sphereCenter = sphereToCapsuleSpaceTransform.getPosition();
// Compute the end-points of the inner segment of the capsule
const decimal capsuleHalfHeight = narrowPhaseInfoBatch.capsuleHeights[batchIndex] * decimal(0.5);
const decimal capsuleHalfHeight = capsuleHeight * decimal(0.5);
const Vector3 capsuleSegA(0, -capsuleHalfHeight, 0);
const Vector3 capsuleSegB(0, capsuleHalfHeight, 0);
@ -69,7 +75,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch
const decimal sphereSegmentDistanceSquare = sphereCenterToSegment.lengthSquare();
// Compute the sum of the radius of the sphere and the capsule (virtual sphere)
decimal sumRadius = narrowPhaseInfoBatch.sphereRadiuses[batchIndex] + narrowPhaseInfoBatch.capsuleRadiuses[batchIndex];
decimal sumRadius = sphereRadius + capsuleRadius;
// If the collision shapes overlap
if (sphereSegmentDistanceSquare < sumRadius * sumRadius) {
@ -80,7 +86,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch
Vector3 contactPointCapsuleLocal;
// If we need to report contacts
if (narrowPhaseInfoBatch.reportContacts[batchIndex]) {
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
// If the sphere center is not on the capsule inner segment
if (sphereSegmentDistanceSquare > MACHINE_EPSILON) {
@ -88,8 +94,8 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch
decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare);
sphereCenterToSegment /= sphereSegmentDistance;
contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * narrowPhaseInfoBatch.sphereRadiuses[batchIndex]);
contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * narrowPhaseInfoBatch.capsuleRadiuses[batchIndex];
contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereRadius);
contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleRadius;
normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment;
@ -120,8 +126,8 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch
normalWorld = capsuleToWorldTransform.getOrientation() * normalCapsuleSpace;
// Compute the two local contact points
contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + normalCapsuleSpace * narrowPhaseInfoBatch.sphereRadiuses[batchIndex]);
contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * narrowPhaseInfoBatch.capsuleRadiuses[batchIndex];
contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + normalCapsuleSpace * sphereRadius);
contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * capsuleRadius;
}
if (penetrationDepth <= decimal(0.0)) {
@ -136,7 +142,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch
isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal);
}
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
isCollisionFound = true;
continue;
}

88
src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.cpp
View File

@ -1,88 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2020 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 <reactphysics3d/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h>
#include <reactphysics3d/collision/shapes/SphereShape.h>
#include <reactphysics3d/collision/shapes/CapsuleShape.h>
using namespace reactphysics3d;
// Constructor
SphereVsCapsuleNarrowPhaseInfoBatch::SphereVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator,
OverlappingPairs& overlappingPairs)
: NarrowPhaseInfoBatch(allocator, overlappingPairs), isSpheresShape1(allocator), sphereRadiuses(allocator), capsuleRadiuses(allocator),
capsuleHeights(allocator) {
}
// Add shapes to be tested during narrow-phase collision detection into the batch
void SphereVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, CollisionShape* shape2,
const Transform& shape1Transform, const Transform& shape2Transform,
bool needToReportContacts, MemoryAllocator& shapeAllocator) {
NarrowPhaseInfoBatch::addNarrowPhaseInfo(pairId, pairIndex, collider1, collider2, shape1, shape2, shape1Transform,
shape2Transform, needToReportContacts, shapeAllocator);
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());
}
// 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() {
// 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)
NarrowPhaseInfoBatch::clear();
isSpheresShape1.clear(true);
sphereRadiuses.clear(true);
capsuleRadiuses.clear(true);
capsuleHeights.clear(true);
}

12
src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
View File

@ -57,13 +57,13 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narr
// For each item in the batch
for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE ||
narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::SPHERE);
assert(narrowPhaseInfoBatch.narrowPhaseInfo[[batchIndex].collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfoBatch.narrowPhaseInfo[[batchIndex].collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfoBatch.narrowPhaseInfo[[batchIndex].collisionShape1->getType() == CollisionShapeType::SPHERE ||
narrowPhaseInfoBatch.narrowPhaseInfo[[batchIndex].collisionShape2->getType() == CollisionShapeType::SPHERE);
// Get the last frame collision info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.lastFrameCollisionInfos[batchIndex];
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].lastFrameCollisionInfo;
lastFrameCollisionInfo->wasUsingGJK = true;
lastFrameCollisionInfo->wasUsingSAT = false;
@ -72,7 +72,7 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narr
if (gjkResults[batchIndex] == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
// Return true
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
isCollisionFound = true;
continue;
}

32
src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
View File

@ -26,31 +26,37 @@
// Libraries
#include <reactphysics3d/collision/narrowphase/SphereVsSphereAlgorithm.h>
#include <reactphysics3d/collision/shapes/SphereShape.h>
#include <reactphysics3d/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h>
#include <reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
bool SphereVsSphereAlgorithm::testCollision(SphereVsSphereNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, MemoryAllocator& memoryAllocator) {
bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, MemoryAllocator& memoryAllocator) {
bool isCollisionFound = false;
// For each item in the batch
for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0);
assert(!narrowPhaseInfoBatch.isColliding[batchIndex]);
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].nbContactPoints == 0);
assert(!narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding);
// Get the local-space to world-space transforms
const Transform& transform1 = narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform& transform2 = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
const Transform& transform1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform;
const Transform& transform2 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform;
// Compute the distance between the centers
Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
const SphereShape* sphereShape1 = static_cast<SphereShape*>(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1);
const SphereShape* sphereShape2 = static_cast<SphereShape*>(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2);
const decimal sphere1Radius = sphereShape1->getRadius();
const decimal sphere2Radius = sphereShape2->getRadius();
// Compute the sum of the radius
decimal sumRadiuses = narrowPhaseInfoBatch.sphere1Radiuses[batchIndex] + narrowPhaseInfoBatch.sphere2Radiuses[batchIndex];
decimal sumRadiuses = sphere1Radius + sphere2Radius;
// Compute the product of the sum of the radius
decimal sumRadiusesProducts = sumRadiuses * sumRadiuses;
@ -59,7 +65,7 @@ bool SphereVsSphereAlgorithm::testCollision(SphereVsSphereNarrowPhaseInfoBatch&
if (squaredDistanceBetweenCenters < sumRadiusesProducts) {
// If we need to report contacts
if (narrowPhaseInfoBatch.reportContacts[batchIndex]) {
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
const Transform transform1Inverse = transform1.getInverse();
const Transform transform2Inverse = transform2.getInverse();
@ -75,8 +81,8 @@ bool SphereVsSphereAlgorithm::testCollision(SphereVsSphereNarrowPhaseInfoBatch&
Vector3 centerSphere2InBody1LocalSpace = transform1Inverse * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2Inverse * transform1.getPosition();
intersectionOnBody1 = narrowPhaseInfoBatch.sphere1Radiuses[batchIndex] * centerSphere2InBody1LocalSpace.getUnit();
intersectionOnBody2 = narrowPhaseInfoBatch.sphere2Radiuses[batchIndex] * centerSphere1InBody2LocalSpace.getUnit();
intersectionOnBody1 = sphere1Radius * centerSphere2InBody1LocalSpace.getUnit();
intersectionOnBody2 = sphere2Radius * centerSphere1InBody2LocalSpace.getUnit();
normal = vectorBetweenCenters.getUnit();
}
else { // If the sphere centers are at the same position (degenerate case)
@ -84,15 +90,15 @@ bool SphereVsSphereAlgorithm::testCollision(SphereVsSphereNarrowPhaseInfoBatch&
// Take any contact normal direction
normal.setAllValues(0, 1, 0);
intersectionOnBody1 = narrowPhaseInfoBatch.sphere1Radiuses[batchIndex] * (transform1Inverse.getOrientation() * normal);
intersectionOnBody2 = narrowPhaseInfoBatch.sphere2Radiuses[batchIndex] * (transform2Inverse.getOrientation() * normal);
intersectionOnBody1 = sphere1Radius * (transform1Inverse.getOrientation() * normal);
intersectionOnBody2 = sphere2Radius * (transform2Inverse.getOrientation() * normal);
}
// Create the contact info object
narrowPhaseInfoBatch.addContactPoint(batchIndex, normal, penetrationDepth, intersectionOnBody1, intersectionOnBody2);
}
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
isCollisionFound = true;
}
}

77
src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp
View File

@ -1,77 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2020 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 <reactphysics3d/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h>
#include <reactphysics3d/collision/shapes/SphereShape.h>
using namespace reactphysics3d;
// Constructor
SphereVsSphereNarrowPhaseInfoBatch::SphereVsSphereNarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs)
: NarrowPhaseInfoBatch(allocator, overlappingPairs), sphere1Radiuses(allocator), sphere2Radiuses(allocator) {
}
// Add shapes to be tested during narrow-phase collision detection into the batch
void SphereVsSphereNarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, CollisionShape* shape2,
const Transform& shape1Transform, const Transform& shape2Transform, bool needToReportContacts, MemoryAllocator &shapeAllocator) {
NarrowPhaseInfoBatch::addNarrowPhaseInfo(pairId, pairIndex, collider1, collider2, shape1, shape2, shape1Transform,
shape2Transform, needToReportContacts, shapeAllocator);
assert(shape1->getType() == CollisionShapeType::SPHERE);
assert(shape2->getType() == CollisionShapeType::SPHERE);
const SphereShape* sphere1 = static_cast<const SphereShape*>(shape1);
const SphereShape* sphere2 = static_cast<const SphereShape*>(shape2);
sphere1Radiuses.add(sphere1->getRadius());
sphere2Radiuses.add(sphere2->getRadius());
}
// Initialize the containers using cached capacity
void SphereVsSphereNarrowPhaseInfoBatch::reserveMemory() {
NarrowPhaseInfoBatch::reserveMemory();
sphere1Radiuses.reserve(mCachedCapacity);
sphere2Radiuses.reserve(mCachedCapacity);
}
// Clear all the objects in the batch
void SphereVsSphereNarrowPhaseInfoBatch::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)
NarrowPhaseInfoBatch::clear();
sphere1Radiuses.clear(true);
sphere2Radiuses.clear(true);
}

56
src/systems/CollisionDetectionSystem.cpp
View File

@ -445,9 +445,9 @@ bool CollisionDetectionSystem::testNarrowPhaseCollision(NarrowPhaseInput& narrow
ConvexPolyhedronVsConvexPolyhedronAlgorithm* convexPolyVsConvexPolyAlgo = mCollisionDispatch.getConvexPolyhedronVsConvexPolyhedronAlgorithm();
// get the narrow-phase batches to test for collision for contacts
SphereVsSphereNarrowPhaseInfoBatch& sphereVsSphereBatchContacts = narrowPhaseInput.getSphereVsSphereBatch();
SphereVsCapsuleNarrowPhaseInfoBatch& sphereVsCapsuleBatchContacts = narrowPhaseInput.getSphereVsCapsuleBatch();
CapsuleVsCapsuleNarrowPhaseInfoBatch& capsuleVsCapsuleBatchContacts = narrowPhaseInput.getCapsuleVsCapsuleBatch();
NarrowPhaseInfoBatch& sphereVsSphereBatchContacts = narrowPhaseInput.getSphereVsSphereBatch();
NarrowPhaseInfoBatch& sphereVsCapsuleBatchContacts = narrowPhaseInput.getSphereVsCapsuleBatch();
NarrowPhaseInfoBatch& capsuleVsCapsuleBatchContacts = narrowPhaseInput.getCapsuleVsCapsuleBatch();
NarrowPhaseInfoBatch& sphereVsConvexPolyhedronBatchContacts = narrowPhaseInput.getSphereVsConvexPolyhedronBatch();
NarrowPhaseInfoBatch& capsuleVsConvexPolyhedronBatchContacts = narrowPhaseInput.getCapsuleVsConvexPolyhedronBatch();
NarrowPhaseInfoBatch& convexPolyhedronVsConvexPolyhedronBatchContacts = narrowPhaseInput.getConvexPolyhedronVsConvexPolyhedronBatch();
@ -482,7 +482,6 @@ void CollisionDetectionSystem::processAllPotentialContacts(NarrowPhaseInput& nar
List<ContactPair>* contactPairs) {
assert(contactPairs->size() == 0);
assert(mapPairIdToContactPairIndex->size() == 0);
Map<uint64, uint> mapPairIdToContactPairIndex(mMemoryManager.getHeapAllocator(), mPreviousMapPairIdToContactPairIndex.size());
@ -618,13 +617,13 @@ void CollisionDetectionSystem::computeOverlapSnapshotContactPairs(NarrowPhaseInf
for(uint i=0; i < narrowPhaseInfoBatch.getNbObjects(); i++) {
// If there is a collision
if (narrowPhaseInfoBatch.isColliding[i]) {
if (narrowPhaseInfoBatch.narrowPhaseInfos[i].isColliding) {
// If the contact pair does not already exist
if (!setOverlapContactPairId.contains(narrowPhaseInfoBatch.overlappingPairIds[i])) {
if (!setOverlapContactPairId.contains(narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId)) {
const Entity collider1Entity = narrowPhaseInfoBatch.colliderEntities1[i];
const Entity collider2Entity = narrowPhaseInfoBatch.colliderEntities2[i];
const Entity collider1Entity = narrowPhaseInfoBatch.narrowPhaseInfos[i].colliderEntity1;
const Entity collider2Entity = narrowPhaseInfoBatch.narrowPhaseInfos[i].colliderEntity2;
const uint32 collider1Index = mCollidersComponents.getEntityIndex(collider1Entity);
const uint32 collider2Index = mCollidersComponents.getEntityIndex(collider2Entity);
@ -635,10 +634,10 @@ void CollisionDetectionSystem::computeOverlapSnapshotContactPairs(NarrowPhaseInf
const bool isTrigger = mCollidersComponents.mIsTrigger[collider1Index] || mCollidersComponents.mIsTrigger[collider2Index];
// Create a new contact pair
ContactPair contactPair(narrowPhaseInfoBatch.overlappingPairIds[i], body1Entity, body2Entity, collider1Entity, collider2Entity, contactPairs.size(), isTrigger, false);
ContactPair contactPair(narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId, body1Entity, body2Entity, collider1Entity, collider2Entity, contactPairs.size(), isTrigger, false);
contactPairs.add(contactPair);
setOverlapContactPairId.add(narrowPhaseInfoBatch.overlappingPairIds[i]);
setOverlapContactPairId.add(narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId);
}
}
@ -993,26 +992,31 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
RP3D_PROFILE("CollisionDetectionSystem::processPotentialContacts()", mProfiler);
if (updateLastFrameInfo) {
// For each narrow phase info object
for(uint i=0; i < narrowPhaseInfoBatch.getNbObjects(); i++) {
narrowPhaseInfoBatch.narrowPhaseInfos[i].lastFrameCollisionInfo->wasColliding = narrowPhaseInfoBatch.narrowPhaseInfos[i].isColliding;
// The previous frame collision info is now valid
narrowPhaseInfoBatch.narrowPhaseInfos[i].lastFrameCollisionInfo->isValid = true;
}
}
// For each narrow phase info object
for(uint i=0; i < narrowPhaseInfoBatch.getNbObjects(); i++) {
if (updateLastFrameInfo) {
narrowPhaseInfoBatch.lastFrameCollisionInfos[i]->wasColliding = narrowPhaseInfoBatch.isColliding[i];
// The previous frame collision info is now valid
narrowPhaseInfoBatch.lastFrameCollisionInfos[i]->isValid = true;
}
const uint64 pairId = narrowPhaseInfoBatch.overlappingPairIds[i];
const uint64 pairId = narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId;
const uint64 pairIndex = mOverlappingPairs.mMapPairIdToPairIndex[pairId];
// If the two colliders are colliding
if (narrowPhaseInfoBatch.isColliding[i]) {
if (narrowPhaseInfoBatch.narrowPhaseInfos[i].isColliding) {
mOverlappingPairs.mCollidingInCurrentFrame[pairIndex] = true;
const Entity collider1Entity = narrowPhaseInfoBatch.colliderEntities1[i];
const Entity collider2Entity = narrowPhaseInfoBatch.colliderEntities2[i];
const Entity collider1Entity = narrowPhaseInfoBatch.narrowPhaseInfos[i].colliderEntity1;
const Entity collider2Entity = narrowPhaseInfoBatch.narrowPhaseInfos[i].colliderEntity2;
const uint32 collider1Index = mCollidersComponents.getEntityIndex(collider1Entity);
const uint32 collider2Index = mCollidersComponents.getEntityIndex(collider2Entity);
@ -1045,7 +1049,7 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
const uint contactPointIndexStart = static_cast<uint>(potentialContactPoints.size());
// Add the potential contacts
for (uint j=0; j < narrowPhaseInfoBatch.contactPoints[i].size(); j++) {
for (uint j=0; j < narrowPhaseInfoBatch.narrowPhaseInfos[i].nbContactPoints; j++) {
if (contactManifoldInfo.nbPotentialContactPoints < NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD) {
@ -1054,14 +1058,14 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
contactManifoldInfo.nbPotentialContactPoints++;
// Add the contact point to the list of potential contact points
const ContactPointInfo& contactPoint = *(narrowPhaseInfoBatch.contactPoints[i][j]);
const ContactPointInfo& contactPoint = narrowPhaseInfoBatch.narrowPhaseInfos[i].contactPoints[j];
potentialContactPoints.add(contactPoint);
}
}
// Add the contact manifold to the overlapping pair contact
assert(overlappingPairContact.pairId == contactManifoldInfo.pairId);
assert(pairId == contactManifoldInfo.pairId);
pairContact->potentialContactManifoldsIndices[0] = contactManifoldIndex;
pairContact->nbPotentialContactManifolds = 1;
}
@ -1096,9 +1100,9 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
assert(pairContact != nullptr);
// Add the potential contacts
for (uint j=0; j < narrowPhaseInfoBatch.contactPoints[i].size(); j++) {
for (uint j=0; j < narrowPhaseInfoBatch.narrowPhaseInfos[i].nbContactPoints; j++) {
const ContactPointInfo& contactPoint = *(narrowPhaseInfoBatch.contactPoints[i][j]);
const ContactPointInfo& contactPoint = narrowPhaseInfoBatch.narrowPhaseInfos[i].contactPoints[j];
// Add the contact point to the list of potential contact points
const uint contactPointIndex = static_cast<uint>(potentialContactPoints.size());