Refactor islands creation
This commit is contained in:
Daniel Chappuis
parent
d9342c55f5
commit
1c91ef7d48
|
|
@ -137,6 +137,7 @@ SET (REACTPHYSICS3D_HEADERS
|
|||
"src/engine/DynamicsWorld.h"
|
||||
"src/engine/EventListener.h"
|
||||
"src/engine/Island.h"
|
||||
"src/engine/Islands.h"
|
||||
"src/engine/Material.h"
|
||||
"src/engine/OverlappingPair.h"
|
||||
"src/engine/Timer.h"
|
||||
|
|
|
|||
|
|
@ -42,6 +42,7 @@
|
|||
#include "utils/Profiler.h"
|
||||
#include "engine/EventListener.h"
|
||||
#include "collision/RaycastInfo.h"
|
||||
#include "engine/Islands.h"
|
||||
#include <cassert>
|
||||
#include <iostream>
|
||||
|
||||
|
|
@ -53,22 +54,23 @@ using namespace std;
|
|||
// Constructor
|
||||
CollisionDetection::CollisionDetection(CollisionWorld* world, ProxyShapeComponents& proxyShapesComponents, TransformComponents& transformComponents,
|
||||
DynamicsComponents& dynamicsComponents, MemoryManager& memoryManager)
|
||||
: mMemoryManager(memoryManager), mProxyShapesComponents(proxyShapesComponents), mTransformComponents(transformComponents),
|
||||
: mMemoryManager(memoryManager), mProxyShapesComponents(proxyShapesComponents),
|
||||
mCollisionDispatch(mMemoryManager.getPoolAllocator()), mWorld(world),
|
||||
mOverlappingPairs(mMemoryManager.getPoolAllocator()),
|
||||
mBroadPhaseSystem(*this, mProxyShapesComponents, mTransformComponents, dynamicsComponents),
|
||||
mBroadPhaseSystem(*this, mProxyShapesComponents, transformComponents, dynamicsComponents),
|
||||
mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mMapBroadPhaseIdToProxyShapeEntity(memoryManager.getPoolAllocator()),
|
||||
mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator()), mPotentialContactPoints(mMemoryManager.getPoolAllocator()),
|
||||
mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator()), mPotentialContactPoints(mMemoryManager.getSingleFrameAllocator()),
|
||||
// TODO : We should probably use single frame allocator for mPotentialContactPoints, mPotentialContactManifolds, mMapPairIdToOverlappingPairContacts
|
||||
mPotentialContactManifolds(mMemoryManager.getPoolAllocator()), mContactPairs1(mMemoryManager.getPoolAllocator()),
|
||||
mPotentialContactManifolds(mMemoryManager.getSingleFrameAllocator()), mContactPairs1(mMemoryManager.getPoolAllocator()),
|
||||
mContactPairs2(mMemoryManager.getPoolAllocator()), mPreviousContactPairs(&mContactPairs1), mCurrentContactPairs(&mContactPairs2),
|
||||
mMapPairIdToContactPairIndex1(mMemoryManager.getPoolAllocator()), mMapPairIdToContactPairIndex2(mMemoryManager.getPoolAllocator()),
|
||||
mPreviousMapPairIdToContactPairIndex(&mMapPairIdToContactPairIndex1), mCurrentMapPairIdToContactPairIndex(&mMapPairIdToContactPairIndex2),
|
||||
mContactPairsIndicesOrderingForContacts(memoryManager.getSingleFrameAllocator()),
|
||||
mContactManifolds1(mMemoryManager.getPoolAllocator()), mContactManifolds2(mMemoryManager.getPoolAllocator()),
|
||||
mPreviousContactManifolds(&mContactManifolds1), mCurrentContactManifolds(&mContactManifolds2),
|
||||
mContactPoints1(mMemoryManager.getPoolAllocator()),
|
||||
mContactPoints2(mMemoryManager.getPoolAllocator()), mPreviousContactPoints(&mContactPoints1),
|
||||
mCurrentContactPoints(&mContactPoints2) {
|
||||
mCurrentContactPoints(&mContactPoints2), mMapBodyToContactPairs(mMemoryManager.getSingleFrameAllocator()) {
|
||||
|
||||
#ifdef IS_PROFILING_ACTIVE
|
||||
|
||||
|
|
@ -420,22 +422,18 @@ void CollisionDetection::computeNarrowPhase() {
|
|||
// Reduce the number of contact points in the manifolds
|
||||
reducePotentialContactManifolds(mOverlappingPairs);
|
||||
|
||||
// Create the actual contact manifolds and contacts (from potential contacts)
|
||||
createContacts();
|
||||
|
||||
// Initialize the current contacts with the contacts from the previous frame (for warmstarting)
|
||||
initContactsWithPreviousOnes();
|
||||
|
||||
// Add all the contact manifolds (between colliding bodies) to the bodies
|
||||
addAllContactManifoldsToBodies();
|
||||
|
||||
// Report contacts to the user
|
||||
reportAllContacts();
|
||||
|
||||
// Clear the list of narrow-phase infos
|
||||
mNarrowPhaseInput.clear();
|
||||
assert(mCurrentContactManifolds->size() == 0);
|
||||
assert(mCurrentContactPoints->size() == 0);
|
||||
|
||||
// TODO : Do not forget to clear all the contact pair, contact manifolds and contact points lists
|
||||
mContactPairsIndicesOrderingForContacts.reserve(mCurrentContactPairs->size());
|
||||
|
||||
mNarrowPhaseInput.clear();
|
||||
}
|
||||
|
||||
// Swap the previous and current contacts lists
|
||||
|
|
@ -467,24 +465,26 @@ void CollisionDetection::swapPreviousAndCurrentContacts() {
|
|||
}
|
||||
}
|
||||
|
||||
// Create the actual contact manifolds and contacts (from potential contacts)
|
||||
// Create the actual contact manifolds and contacts points
|
||||
/// List of the indices of the contact pairs (in mCurrentContacPairs array) with contact pairs of
|
||||
/// same islands packed together linearly and contact pairs that are not part of islands at the end.
|
||||
/// This is used when we create contact manifolds and contact points so that there are also packed
|
||||
/// together linearly if they are part of the same island.
|
||||
void CollisionDetection::createContacts() {
|
||||
|
||||
RP3D_PROFILE("CollisionDetection::createContacts()", mProfiler);
|
||||
|
||||
assert(mCurrentContactManifolds->size() == 0);
|
||||
assert(mCurrentContactPoints->size() == 0);
|
||||
assert(mCurrentContactPairs->size() == mContactPairsIndicesOrderingForContacts.size());
|
||||
|
||||
mCurrentContactManifolds->reserve(mCurrentContactPairs->size());
|
||||
mCurrentContactPoints->reserve(mCurrentContactManifolds->size());
|
||||
|
||||
// For each contact pair
|
||||
for (auto it = mCurrentMapPairIdToContactPairIndex->begin(); it != mCurrentMapPairIdToContactPairIndex->end(); ++it) {
|
||||
|
||||
const uint contactPairIndex = it->second;
|
||||
assert(contactPairIndex < mCurrentContactPairs->size());
|
||||
ContactPair& contactPair = (*mCurrentContactPairs)[contactPairIndex];
|
||||
for (uint p=0; p < mContactPairsIndicesOrderingForContacts.size(); p++) {
|
||||
|
||||
ContactPair& contactPair = (*mCurrentContactPairs)[mContactPairsIndicesOrderingForContacts[p]];
|
||||
assert(contactPair.potentialContactManifoldsIndices.size() > 0);
|
||||
|
||||
// Start index and numnber of contact manifolds for this contact pair
|
||||
contactPair.contactManifoldsIndex = mCurrentContactManifolds->size();
|
||||
contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size();
|
||||
contactPair.contactPointsIndex = mCurrentContactPoints->size();
|
||||
|
|
@ -496,7 +496,8 @@ void CollisionDetection::createContacts() {
|
|||
|
||||
// Start index and number of contact points for this manifold
|
||||
const uint contactPointsIndex = mCurrentContactPoints->size();
|
||||
const int8 nbContactPoints = potentialManifold.potentialContactPointsIndices.size();
|
||||
const int8 nbContactPoints = static_cast<int8>(potentialManifold.potentialContactPointsIndices.size());
|
||||
contactPair.nbToTalContactPoints += nbContactPoints;
|
||||
|
||||
// We create a new contact manifold
|
||||
ContactManifold contactManifold(contactPointsIndex, nbContactPoints, mMemoryManager.getPoolAllocator(), mWorld->mConfig);
|
||||
|
|
@ -504,9 +505,6 @@ void CollisionDetection::createContacts() {
|
|||
// Add the contact manifold
|
||||
mCurrentContactManifolds->add(contactManifold);
|
||||
|
||||
// Increase the number of total contact point of the contact pair
|
||||
contactPair.nbToTalContactPoints += potentialManifold.potentialContactPointsIndices.size();
|
||||
|
||||
assert(potentialManifold.potentialContactPointsIndices.size() > 0);
|
||||
|
||||
// For each contact point of the manifold
|
||||
|
|
@ -523,9 +521,13 @@ void CollisionDetection::createContacts() {
|
|||
}
|
||||
}
|
||||
|
||||
// Initialize the current contacts with the contacts from the previous frame (for warmstarting)
|
||||
initContactsWithPreviousOnes();
|
||||
|
||||
// Reset the potential contacts
|
||||
mPotentialContactPoints.clear();
|
||||
mPotentialContactManifolds.clear();
|
||||
mPotentialContactPoints.clear(true);
|
||||
mPotentialContactManifolds.clear(true);
|
||||
mContactPairsIndicesOrderingForContacts.clear(true);
|
||||
}
|
||||
|
||||
// Initialize the current contacts with the contacts from the previous frame (for warmstarting)
|
||||
|
|
@ -800,6 +802,7 @@ void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPh
|
|||
const uint contactPointIndex = static_cast<uint>(mPotentialContactPoints.size());
|
||||
|
||||
// TODO : We should probably use single frame allocator here for mPotentialContactPoints
|
||||
// If so, do not forget to call mPotentialContactPoints.clear(true) at the end of frame
|
||||
mPotentialContactPoints.add(contactPoint);
|
||||
|
||||
bool similarManifoldFound = false;
|
||||
|
|
@ -846,6 +849,7 @@ void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPh
|
|||
|
||||
// Create a new contact manifold for the overlapping pair
|
||||
// TODO : We should probably use single frame allocator here
|
||||
// If so, do not forget to call mPotentialContactPoints.clear(true) at the end of frame
|
||||
ContactManifoldInfo contactManifoldInfo(pairId, mMemoryManager.getPoolAllocator());
|
||||
|
||||
// Add the contact point to the manifold
|
||||
|
|
@ -854,12 +858,34 @@ void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPh
|
|||
// If the overlapping pair contact does not exists yet
|
||||
if (pairContact == nullptr) {
|
||||
|
||||
Entity body1Entity = narrowPhaseInfoBatch.overlappingPairs[i]->getShape1()->getBody()->getEntity();
|
||||
Entity body2Entity = narrowPhaseInfoBatch.overlappingPairs[i]->getShape2()->getBody()->getEntity();
|
||||
|
||||
// TODO : We should probably use a single frame allocator here
|
||||
ContactPair overlappingPairContact(pairId, mMemoryManager.getPoolAllocator());
|
||||
const uint newContactPairIndex = mCurrentContactPairs->size();
|
||||
ContactPair overlappingPairContact(pairId, body1Entity, body2Entity, newContactPairIndex, mMemoryManager.getPoolAllocator());
|
||||
mCurrentContactPairs->add(overlappingPairContact);
|
||||
pairContact = &((*mCurrentContactPairs)[newContactPairIndex]);
|
||||
mCurrentMapPairIdToContactPairIndex->add(Pair<OverlappingPair::OverlappingPairId, uint>(pairId, newContactPairIndex));
|
||||
|
||||
auto itbodyContactPairs = mMapBodyToContactPairs.find(body1Entity);
|
||||
if (itbodyContactPairs != mMapBodyToContactPairs.end()) {
|
||||
itbodyContactPairs->second.add(newContactPairIndex);
|
||||
}
|
||||
else {
|
||||
List<uint> contactPairs(mMemoryManager.getSingleFrameAllocator(), 1);
|
||||
contactPairs.add(newContactPairIndex);
|
||||
mMapBodyToContactPairs.add(Pair<Entity, List<uint>>(body1Entity, contactPairs));
|
||||
}
|
||||
itbodyContactPairs = mMapBodyToContactPairs.find(body2Entity);
|
||||
if (itbodyContactPairs != mMapBodyToContactPairs.end()) {
|
||||
itbodyContactPairs->second.add(newContactPairIndex);
|
||||
}
|
||||
else {
|
||||
List<uint> contactPairs(mMemoryManager.getSingleFrameAllocator(), 1);
|
||||
contactPairs.add(newContactPairIndex);
|
||||
mMapBodyToContactPairs.add(Pair<Entity, List<uint>>(body2Entity, contactPairs));
|
||||
}
|
||||
}
|
||||
|
||||
assert(pairContact != nullptr);
|
||||
|
|
|
|||
|
|
@ -82,9 +82,6 @@ class CollisionDetection {
|
|||
/// Reference the proxy-shape components
|
||||
ProxyShapeComponents& mProxyShapesComponents;
|
||||
|
||||
/// Reference the transform components
|
||||
TransformComponents& mTransformComponents;
|
||||
|
||||
/// Collision Detection Dispatch configuration
|
||||
CollisionDispatch mCollisionDispatch;
|
||||
|
||||
|
|
@ -138,6 +135,12 @@ class CollisionDetection {
|
|||
/// (either mMapPairIdToContactPairIndex1 or mMapPairIdToContactPairIndex2)
|
||||
Map<OverlappingPair::OverlappingPairId, uint>* mCurrentMapPairIdToContactPairIndex;
|
||||
|
||||
/// List of the indices of the contact pairs (in mCurrentContacPairs array) with contact pairs of
|
||||
/// same islands packed together linearly and contact pairs that are not part of islands at the end.
|
||||
/// This is used when we create contact manifolds and contact points so that there are also packed
|
||||
/// together linearly if they are part of the same island.
|
||||
List<uint> mContactPairsIndicesOrderingForContacts;
|
||||
|
||||
/// First list with the contact manifolds
|
||||
List<ContactManifold> mContactManifolds1;
|
||||
|
||||
|
|
@ -162,6 +165,9 @@ class CollisionDetection {
|
|||
/// Pointer to the contact points of the current frame (either mContactPoints1 or mContactPoints2)
|
||||
List<ContactPoint>* mCurrentContactPoints;
|
||||
|
||||
/// Map a body entity to the list of contact pairs in which it is involved
|
||||
Map<Entity, List<uint>> mMapBodyToContactPairs;
|
||||
|
||||
#ifdef IS_PROFILING_ACTIVE
|
||||
|
||||
/// Pointer to the profiler
|
||||
|
|
@ -214,7 +220,7 @@ class CollisionDetection {
|
|||
/// Reduce the potential contact manifolds and contact points of the overlapping pair contacts
|
||||
void reducePotentialContactManifolds(const OverlappingPairMap& overlappingPairs);
|
||||
|
||||
/// Create the actual contact manifolds and contacts (from potential contacts)
|
||||
/// Create the actual contact manifolds and contacts points (from potential contacts) for a given contact pair
|
||||
void createContacts();
|
||||
|
||||
/// Initialize the current contacts with the contacts from the previous frame (for warmstarting)
|
||||
|
|
|
|||
|
|
@ -50,6 +50,18 @@ struct ContactPair {
|
|||
/// Indices of the potential contact manifolds
|
||||
List<uint> potentialContactManifoldsIndices;
|
||||
|
||||
/// Entity of the first body of the manifold
|
||||
Entity body1Entity;
|
||||
|
||||
/// Entity of the second body of the manifold
|
||||
Entity body2Entity;
|
||||
|
||||
/// True if the manifold is already in an island
|
||||
bool isAlreadyInIsland;
|
||||
|
||||
/// Index of the contact pair in the array of pairs
|
||||
uint contactPairIndex;
|
||||
|
||||
/// Index of the first contact manifold in the array
|
||||
uint contactManifoldsIndex;
|
||||
|
||||
|
|
@ -65,8 +77,9 @@ struct ContactPair {
|
|||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
ContactPair(OverlappingPair::OverlappingPairId pairId, MemoryAllocator& allocator)
|
||||
: pairId(pairId), potentialContactManifoldsIndices(allocator), contactManifoldsIndex(0), nbContactManifolds(0),
|
||||
ContactPair(OverlappingPair::OverlappingPairId pairId, Entity body1Entity, Entity body2Entity, uint contactPairIndex, MemoryAllocator& allocator)
|
||||
: pairId(pairId), potentialContactManifoldsIndices(allocator), body1Entity(body1Entity), body2Entity(body2Entity),
|
||||
isAlreadyInIsland(false), contactPairIndex(contactPairIndex), contactManifoldsIndex(0), nbContactManifolds(0),
|
||||
contactPointsIndex(0), nbToTalContactPoints(0) {
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -35,7 +35,7 @@ using namespace reactphysics3d;
|
|||
|
||||
// Constructor
|
||||
DynamicsComponents::DynamicsComponents(MemoryAllocator& allocator)
|
||||
:Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof (Vector3)) {
|
||||
:Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof (Vector3) + sizeof(bool)) {
|
||||
|
||||
// Allocate memory for the components data
|
||||
allocate(INIT_NB_ALLOCATED_COMPONENTS);
|
||||
|
|
@ -57,6 +57,7 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
|
|||
Entity* newBodies = static_cast<Entity*>(newBuffer);
|
||||
Vector3* newLinearVelocities = reinterpret_cast<Vector3*>(newBodies + nbComponentsToAllocate);
|
||||
Vector3* newAngularVelocities = reinterpret_cast<Vector3*>(newLinearVelocities + nbComponentsToAllocate);
|
||||
bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(newAngularVelocities + nbComponentsToAllocate);
|
||||
|
||||
// If there was already components before
|
||||
if (mNbComponents > 0) {
|
||||
|
|
@ -65,6 +66,7 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
|
|||
memcpy(newBodies, mBodies, mNbComponents * sizeof(Entity));
|
||||
memcpy(newLinearVelocities, mLinearVelocities, mNbComponents * sizeof(Vector3));
|
||||
memcpy(newAngularVelocities, mAngularVelocities, mNbComponents * sizeof(Vector3));
|
||||
memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool));
|
||||
|
||||
// Deallocate previous memory
|
||||
mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize);
|
||||
|
|
@ -74,6 +76,7 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
|
|||
mBodies = newBodies;
|
||||
mLinearVelocities = newLinearVelocities;
|
||||
mAngularVelocities = newAngularVelocities;
|
||||
mIsAlreadyInIsland = newIsAlreadyInIsland;
|
||||
mNbAllocatedComponents = nbComponentsToAllocate;
|
||||
}
|
||||
|
||||
|
|
@ -87,6 +90,7 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const
|
|||
new (mBodies + index) Entity(bodyEntity);
|
||||
new (mLinearVelocities + index) Vector3(component.linearVelocity);
|
||||
new (mAngularVelocities + index) Vector3(component.angularVelocity);
|
||||
mIsAlreadyInIsland[index] = false;
|
||||
|
||||
// Map the entity with the new component lookup index
|
||||
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(bodyEntity, index));
|
||||
|
|
@ -107,6 +111,7 @@ void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex)
|
|||
new (mBodies + destIndex) Entity(mBodies[srcIndex]);
|
||||
new (mLinearVelocities + destIndex) Vector3(mLinearVelocities[srcIndex]);
|
||||
new (mAngularVelocities + destIndex) Vector3(mAngularVelocities[srcIndex]);
|
||||
mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex];
|
||||
|
||||
// Destroy the source component
|
||||
destroyComponent(srcIndex);
|
||||
|
|
@ -129,6 +134,7 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
|
|||
Entity entity1(mBodies[index1]);
|
||||
Vector3 linearVelocity1(mLinearVelocities[index1]);
|
||||
Vector3 angularVelocity1(mAngularVelocities[index1]);
|
||||
bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1];
|
||||
|
||||
// Destroy component 1
|
||||
destroyComponent(index1);
|
||||
|
|
@ -139,6 +145,7 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
|
|||
new (mBodies + index2) Entity(entity1);
|
||||
new (mLinearVelocities + index2) Vector3(linearVelocity1);
|
||||
new (mAngularVelocities + index2) Vector3(angularVelocity1);
|
||||
mIsAlreadyInIsland[index2] = isAlreadyInIsland1;
|
||||
|
||||
// Update the entity to component index mapping
|
||||
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(entity1, index2));
|
||||
|
|
|
|||
|
|
@ -59,6 +59,9 @@ class DynamicsComponents : public Components {
|
|||
/// Array with the angular velocity of each component
|
||||
Vector3* mAngularVelocities;
|
||||
|
||||
/// Array with the boolean value to know if the body has already been added into an island
|
||||
bool* mIsAlreadyInIsland;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Allocate memory for a given number of components
|
||||
|
|
@ -105,15 +108,22 @@ class DynamicsComponents : public Components {
|
|||
/// Return the angular velocity of an entity
|
||||
Vector3& getAngularVelocity(Entity bodyEntity) const;
|
||||
|
||||
/// Return true if the entity is already in an island
|
||||
bool getIsAlreadyInIsland(Entity bodyEntity) const;
|
||||
|
||||
/// Set the linear velocity of an entity
|
||||
void setLinearVelocity(Entity bodyEntity, const Vector3& linearVelocity);
|
||||
|
||||
/// Set the angular velocity of an entity
|
||||
void setAngularVelocity(Entity bodyEntity, const Vector3& angularVelocity);
|
||||
|
||||
/// Set the value to know if the entity is already in an island
|
||||
bool setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland);
|
||||
|
||||
// -------------------- Friendship -------------------- //
|
||||
|
||||
friend class BroadPhaseSystem;
|
||||
friend class DynamicsWorld;
|
||||
};
|
||||
|
||||
// Return the linear velocity of an entity
|
||||
|
|
@ -148,6 +158,22 @@ inline void DynamicsComponents::setAngularVelocity(Entity bodyEntity, const Vect
|
|||
mAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = angularVelocity;
|
||||
}
|
||||
|
||||
// Return true if the entity is already in an island
|
||||
inline bool DynamicsComponents::getIsAlreadyInIsland(Entity bodyEntity) const {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
|
||||
|
||||
return mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]];
|
||||
}
|
||||
|
||||
/// Set the value to know if the entity is already in an island
|
||||
inline bool DynamicsComponents::setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
|
||||
|
||||
mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]] = isAlreadyInIsland;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -32,7 +32,9 @@
|
|||
#include "utils/Profiler.h"
|
||||
#include "engine/EventListener.h"
|
||||
#include "engine/Island.h"
|
||||
#include "engine/Islands.h"
|
||||
#include "collision/ContactManifold.h"
|
||||
#include "containers/Stack.h"
|
||||
|
||||
// Namespaces
|
||||
using namespace reactphysics3d;
|
||||
|
|
@ -60,7 +62,8 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS
|
|||
mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity),
|
||||
mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity),
|
||||
mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep),
|
||||
mFreeJointsIDs(mMemoryManager.getPoolAllocator()), mCurrentJointId(0) {
|
||||
mFreeJointsIDs(mMemoryManager.getPoolAllocator()), mCurrentJointId(0),
|
||||
mIslands2(mMemoryManager.getSingleFrameAllocator()) {
|
||||
|
||||
#ifdef IS_PROFILING_ACTIVE
|
||||
|
||||
|
|
@ -128,6 +131,32 @@ void DynamicsWorld::update(decimal timeStep) {
|
|||
// Compute the islands (separate groups of bodies with constraints between each others)
|
||||
computeIslands();
|
||||
|
||||
// Create the islands
|
||||
createIslands();
|
||||
|
||||
// TODO : DELETE THIS
|
||||
/*
|
||||
std::cout << "--------------------- FRAME ------------------------ " << std::endl;
|
||||
std::cout << " ---- OLD ISLANDS -----" << std::endl;
|
||||
for (uint i=0; i < mNbIslands; i++) {
|
||||
std::cout << "Island " << i << std::endl;
|
||||
for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
|
||||
std::cout << "Body Id : " << mIslands[i]->getBodies()[b]->getId() << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
std::cout << " ---- NEW ISLANDS -----" << std::endl;
|
||||
for (uint i=0; i < mIslands2.getNbIslands(); i++) {
|
||||
std::cout << "Island " << i << std::endl;
|
||||
for (uint b=0; b < mIslands2.bodyEntities[i].size(); b++) {
|
||||
std::cout << "Body Id : " << mBodyComponents.getBody(mIslands2.bodyEntities[i][b])->getId() << std::endl;
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
// Create the actual narrow-phase contacts
|
||||
mCollisionDetection.createContacts();
|
||||
|
||||
// Integrate the velocities
|
||||
integrateRigidBodiesVelocities();
|
||||
|
||||
|
|
@ -151,6 +180,9 @@ void DynamicsWorld::update(decimal timeStep) {
|
|||
// Reset the external force and torque applied to the bodies
|
||||
resetBodiesForceAndTorque();
|
||||
|
||||
// Reset the islands
|
||||
mIslands2.clear();
|
||||
|
||||
// Reset the single frame memory allocator
|
||||
mMemoryManager.resetFrameAllocator();
|
||||
}
|
||||
|
|
@ -672,6 +704,7 @@ uint DynamicsWorld::computeNextAvailableJointId() {
|
|||
return jointId;
|
||||
}
|
||||
|
||||
// TODO : DELETE THIS
|
||||
// Compute the islands of awake bodies.
|
||||
/// An island is an isolated group of rigid bodies that have constraints (joints or contacts)
|
||||
/// between each other. This method computes the islands at each time step as follows: For each
|
||||
|
|
@ -828,6 +861,179 @@ void DynamicsWorld::computeIslands() {
|
|||
}
|
||||
}
|
||||
|
||||
// Compute the islands using potential contacts and joints
|
||||
/// We compute the islands before creating the actual contacts here because we want all
|
||||
/// the contact manifolds and contact points of the same island
|
||||
/// to be packed together into linear arrays of manifolds and contacts for better caching.
|
||||
/// An island is an isolated group of rigid bodies that have constraints (joints or contacts)
|
||||
/// between each other. This method computes the islands at each time step as follows: For each
|
||||
/// awake rigid body, we run a Depth First Search (DFS) through the constraint graph of that body
|
||||
/// (graph where nodes are the bodies and where the edges are the constraints between the bodies) to
|
||||
/// find all the bodies that are connected with it (the bodies that share joints or contacts with
|
||||
/// it). Then, we create an island with this group of connected bodies.
|
||||
void DynamicsWorld::createIslands() {
|
||||
|
||||
// TODO : Check if we handle kinematic bodies correctly in islands creation
|
||||
|
||||
// list of contact pairs involving a non-rigid body
|
||||
List<uint> nonRigidBodiesContactPairs(mMemoryManager.getSingleFrameAllocator());
|
||||
|
||||
RP3D_PROFILE("DynamicsWorld::createIslandsAndContacts()", mProfiler);
|
||||
|
||||
// Reset all the isAlreadyInIsland variables of bodies and joints
|
||||
for (uint b=0; b < mDynamicsComponents.getNbEnabledComponents(); b++) {
|
||||
mDynamicsComponents.mIsAlreadyInIsland[b] = false;
|
||||
}
|
||||
for (List<Joint*>::Iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
|
||||
(*it)->mIsAlreadyInIsland = false;
|
||||
}
|
||||
|
||||
// Create a stack for the bodies to visit during the Depth First Search
|
||||
Stack<Entity> bodyEntityIndicesToVisit(mMemoryManager.getSingleFrameAllocator());
|
||||
|
||||
uint nbTotalManifolds = 0;
|
||||
|
||||
// For each dynamic component
|
||||
// TODO : Here we iterate on dynamic component where we can have static, kinematic and dynamic bodies. Maybe we should
|
||||
// not use a dynamic component for a static body.
|
||||
for (uint b=0; b < mDynamicsComponents.getNbEnabledComponents(); b++) {
|
||||
|
||||
// If the body has already been added to an island, we go to the next body
|
||||
if (mDynamicsComponents.mIsAlreadyInIsland[b]) continue;
|
||||
|
||||
// If the body is static, we go to the next body
|
||||
// TODO : Do not use pointer to rigid body here (maybe move getType() into a component)
|
||||
CollisionBody* body = static_cast<CollisionBody*>(mBodyComponents.getBody(mDynamicsComponents.mBodies[b]));
|
||||
if (body->getType() == BodyType::STATIC) continue;
|
||||
|
||||
// Reset the stack of bodies to visit
|
||||
bodyEntityIndicesToVisit.clear();
|
||||
|
||||
// Add the body into the stack of bodies to visit
|
||||
mDynamicsComponents.mIsAlreadyInIsland[b] = true;
|
||||
bodyEntityIndicesToVisit.push(mDynamicsComponents.mBodies[b]);
|
||||
|
||||
// Create the new island
|
||||
uint32 islandIndex = mIslands2.addIsland(nbTotalManifolds);
|
||||
|
||||
// While there are still some bodies to visit in the stack
|
||||
while (bodyEntityIndicesToVisit.size() > 0) {
|
||||
|
||||
// Get the next body to visit from the stack
|
||||
const Entity bodyToVisitEntity = bodyEntityIndicesToVisit.pop();
|
||||
|
||||
// Awake the body if it is sleeping
|
||||
notifyBodyDisabled(bodyToVisitEntity, false);
|
||||
|
||||
// Add the body into the island
|
||||
mIslands2.bodyEntities[islandIndex].add(bodyToVisitEntity);
|
||||
|
||||
// If the current body is static, we do not want to perform the DFS
|
||||
// search across that body
|
||||
// TODO : Do not use pointer to rigid body here (maybe move getType() into a component)
|
||||
RigidBody* rigidBodyToVisit = static_cast<RigidBody*>(mBodyComponents.getBody(bodyToVisitEntity));
|
||||
if (rigidBodyToVisit->getType() == BodyType::STATIC) continue;
|
||||
|
||||
// If the body is involved in contacts with other bodies
|
||||
auto itBodyContactPairs = mCollisionDetection.mMapBodyToContactPairs.find(bodyToVisitEntity);
|
||||
if (itBodyContactPairs != mCollisionDetection.mMapBodyToContactPairs.end()) {
|
||||
|
||||
// For each contact pair in which the current body is involded
|
||||
List<uint>& contactPairs = itBodyContactPairs->second;
|
||||
for (uint p=0; p < contactPairs.size(); p++) {
|
||||
|
||||
ContactPair& pair = (*mCollisionDetection.mCurrentContactPairs)[contactPairs[p]];
|
||||
assert(pair.potentialContactManifoldsIndices.size() > 0);
|
||||
|
||||
// Check if the current contact pair has already been added into an island
|
||||
if (pair.isAlreadyInIsland) continue;
|
||||
|
||||
// Get the other body of the contact manifold
|
||||
// TODO : Maybe avoid those casts here
|
||||
RigidBody* body1 = dynamic_cast<RigidBody*>(mBodyComponents.getBody(pair.body1Entity));
|
||||
RigidBody* body2 = dynamic_cast<RigidBody*>(mBodyComponents.getBody(pair.body2Entity));
|
||||
|
||||
// If the colliding body is a RigidBody (and not a CollisionBody instead)
|
||||
if (body1 != nullptr && body2 != nullptr) {
|
||||
|
||||
nbTotalManifolds += pair.potentialContactManifoldsIndices.size();
|
||||
|
||||
// Add the pair into the list of pair to process to create contacts
|
||||
mCollisionDetection.mContactPairsIndicesOrderingForContacts.add(pair.contactPairIndex);
|
||||
|
||||
// Add the contact manifold into the island
|
||||
mIslands2.nbContactManifolds[islandIndex] += pair.potentialContactManifoldsIndices.size();
|
||||
pair.isAlreadyInIsland = true;
|
||||
|
||||
const Entity otherBodyEntity = pair.body1Entity == bodyToVisitEntity ? pair.body2Entity : pair.body1Entity;
|
||||
|
||||
// Check if the other body has already been added to the island
|
||||
if (mDynamicsComponents.getIsAlreadyInIsland(otherBodyEntity)) continue;
|
||||
|
||||
// Insert the other body into the stack of bodies to visit
|
||||
bodyEntityIndicesToVisit.push(otherBodyEntity);
|
||||
mDynamicsComponents.setIsAlreadyInIsland(otherBodyEntity, true);
|
||||
}
|
||||
else {
|
||||
|
||||
// Add the contact pair index in the list of contact pairs that won't be part of islands
|
||||
nonRigidBodiesContactPairs.add(pair.contactPairIndex);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// For each joint in which the current body is involved
|
||||
JointListElement* jointElement;
|
||||
for (jointElement = rigidBodyToVisit->mJointsList; jointElement != nullptr;
|
||||
jointElement = jointElement->next) {
|
||||
|
||||
Joint* joint = jointElement->joint;
|
||||
|
||||
// Check if the current joint has already been added into an island
|
||||
if (joint->isAlreadyInIsland()) continue;
|
||||
|
||||
// Add the joint into the island
|
||||
mIslands2.joints.add(joint);
|
||||
joint->mIsAlreadyInIsland = true;
|
||||
|
||||
const Entity body1Entity = joint->getBody1()->getEntity();
|
||||
const Entity body2Entity = joint->getBody2()->getEntity();
|
||||
const Entity otherBodyEntity = body1Entity == bodyToVisitEntity ? body2Entity : body1Entity;
|
||||
|
||||
// TODO : Maybe avoid those casts here
|
||||
// Get the other body of the contact manifold
|
||||
RigidBody* body1 = static_cast<RigidBody*>(joint->getBody1());
|
||||
RigidBody* body2 = static_cast<RigidBody*>(joint->getBody2());
|
||||
RigidBody* otherBody = (body1->getId() == rigidBodyToVisit->getId()) ? body2 : body1;
|
||||
|
||||
// Check if the other body has already been added to the island
|
||||
if (otherBody->mIsAlreadyInIsland) continue;
|
||||
|
||||
// Insert the other body into the stack of bodies to visit
|
||||
bodyEntityIndicesToVisit.push(otherBodyEntity);
|
||||
otherBody->mIsAlreadyInIsland = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Reset the isAlreadyIsland variable of the static bodies so that they
|
||||
// can also be included in the other islands
|
||||
for (uint b=0; b < mDynamicsComponents.getNbEnabledComponents(); b++) {
|
||||
|
||||
// If the body is static, we go to the next body
|
||||
// TODO : Do not use pointer to rigid body here (maybe move getType() into a component)
|
||||
CollisionBody* body = static_cast<CollisionBody*>(mBodyComponents.getBody(mDynamicsComponents.mBodies[b]));
|
||||
if (body->getType() == BodyType::STATIC) {
|
||||
mDynamicsComponents.mIsAlreadyInIsland[b] = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Add the contact pairs that are not part of islands at the end of the array of pairs for contacts creations
|
||||
mCollisionDetection.mContactPairsIndicesOrderingForContacts.addRange(nonRigidBodiesContactPairs);
|
||||
|
||||
mCollisionDetection.mMapBodyToContactPairs.clear(true);
|
||||
}
|
||||
|
||||
// Put bodies to sleep if needed.
|
||||
/// For each island, if all the bodies have been almost still for a long enough period of
|
||||
/// time, we put all the bodies of the island to sleep.
|
||||
|
|
|
|||
|
|
@ -33,6 +33,7 @@
|
|||
#include "utils/Logger.h"
|
||||
#include "engine/ContactSolver.h"
|
||||
#include "components/DynamicsComponents.h"
|
||||
#include "engine/Islands.h"
|
||||
|
||||
/// Namespace ReactPhysics3D
|
||||
namespace reactphysics3d {
|
||||
|
|
@ -126,6 +127,9 @@ class DynamicsWorld : public CollisionWorld {
|
|||
/// Current joint id
|
||||
uint mCurrentJointId;
|
||||
|
||||
/// All the islands of bodies of the current frame
|
||||
Islands mIslands2;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Integrate the positions and orientations of rigid bodies.
|
||||
|
|
@ -149,6 +153,9 @@ class DynamicsWorld : public CollisionWorld {
|
|||
/// Compute the islands of awake bodies.
|
||||
void computeIslands();
|
||||
|
||||
/// Compute the islands using potential contacts and joints and create the actual contacts.
|
||||
void createIslands();
|
||||
|
||||
/// Update the postion/orientation of the bodies
|
||||
void updateBodiesState();
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user