Changes and bug fixes in ContactManifold and ContactManifoldSet
This commit is contained in:
parent
d5617526ff
commit
de494bb0fd
|
@ -104,7 +104,7 @@ void CollisionDetection::computeMiddlePhase() {
|
||||||
OverlappingPair* pair = it->second;
|
OverlappingPair* pair = it->second;
|
||||||
|
|
||||||
// Make all the contact manifolds and contact points of the pair obsolete
|
// Make all the contact manifolds and contact points of the pair obsolete
|
||||||
pair->makeContactsObselete();
|
pair->makeContactsObsolete();
|
||||||
|
|
||||||
ProxyShape* shape1 = pair->getShape1();
|
ProxyShape* shape1 = pair->getShape1();
|
||||||
ProxyShape* shape2 = pair->getShape2();
|
ProxyShape* shape2 = pair->getShape2();
|
||||||
|
@ -410,12 +410,12 @@ void CollisionDetection::processPotentialContacts(OverlappingPair* pair) {
|
||||||
potentialManifold = potentialManifold->mNext;
|
potentialManifold = potentialManifold->mNext;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Reset the isNew status of the manifolds
|
|
||||||
pair->resetIsNewManifoldStatus();
|
|
||||||
|
|
||||||
// Clear the obsolete contact manifolds and contact points
|
// Clear the obsolete contact manifolds and contact points
|
||||||
pair->clearObsoleteManifoldsAndContactPoints();
|
pair->clearObsoleteManifoldsAndContactPoints();
|
||||||
|
|
||||||
|
// Reduce the contact manifolds and contact points if there are too many of them
|
||||||
|
pair->reduceContactManifolds();
|
||||||
|
|
||||||
// Reset the potential contacts of the pair
|
// Reset the potential contacts of the pair
|
||||||
pair->clearPotentialContactManifolds();
|
pair->clearPotentialContactManifolds();
|
||||||
}
|
}
|
||||||
|
|
|
@ -34,20 +34,14 @@ ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyS
|
||||||
: mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mContactNormalId(manifoldInfo->getContactNormalId()),
|
: mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mContactNormalId(manifoldInfo->getContactNormalId()),
|
||||||
mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
|
mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
|
||||||
mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
|
mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
|
||||||
mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false), mIsNew(true) {
|
mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false) {
|
||||||
|
|
||||||
// For each contact point info in the manifold
|
// For each contact point info in the manifold
|
||||||
const ContactPointInfo* pointInfo = manifoldInfo->getFirstContactPointInfo();
|
const ContactPointInfo* pointInfo = manifoldInfo->getFirstContactPointInfo();
|
||||||
while(pointInfo != nullptr) {
|
while(pointInfo != nullptr) {
|
||||||
|
|
||||||
// Create the new contact point
|
// Add the new contact point
|
||||||
ContactPoint* contact = new (mMemoryAllocator.allocate(sizeof(ContactPoint)))
|
addContactPoint(pointInfo);
|
||||||
ContactPoint(pointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
|
|
||||||
|
|
||||||
// Add the new contact point into the manifold
|
|
||||||
contact->setNext(mContactPoints);
|
|
||||||
mContactPoints = contact;
|
|
||||||
mNbContactPoints++;
|
|
||||||
|
|
||||||
pointInfo = pointInfo->next;
|
pointInfo = pointInfo->next;
|
||||||
}
|
}
|
||||||
|
@ -73,52 +67,122 @@ ContactManifold::~ContactManifold() {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Remove a contact point
|
||||||
|
void ContactManifold::removeContactPoint(ContactPoint* contactPoint) {
|
||||||
|
|
||||||
|
assert(mNbContactPoints > 0);
|
||||||
|
assert(mContactPoints != nullptr);
|
||||||
|
assert(contactPoint != nullptr);
|
||||||
|
|
||||||
|
ContactPoint* previous = contactPoint->getPrevious();
|
||||||
|
ContactPoint* next = contactPoint->getNext();
|
||||||
|
|
||||||
|
if (previous != nullptr) {
|
||||||
|
previous->setNext(next);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
mContactPoints = next;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (next != nullptr) {
|
||||||
|
next->setPrevious(previous);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Delete the contact point
|
||||||
|
contactPoint->~ContactPoint();
|
||||||
|
mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
|
||||||
|
|
||||||
|
mNbContactPoints--;
|
||||||
|
assert(mNbContactPoints >= 0);
|
||||||
|
}
|
||||||
|
|
||||||
// Add a contact point
|
// Add a contact point
|
||||||
void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) {
|
void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) {
|
||||||
|
|
||||||
|
assert(contactPointInfo != nullptr);
|
||||||
|
|
||||||
// Create the new contact point
|
// Create the new contact point
|
||||||
ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint)))
|
ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(contactPointInfo);
|
||||||
ContactPoint(contactPointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
|
|
||||||
|
|
||||||
// Add the new contact point into the manifold
|
// Add the new contact point into the manifold
|
||||||
contactPoint->setNext(mContactPoints);
|
contactPoint->setNext(mContactPoints);
|
||||||
|
contactPoint->setPrevious(nullptr);
|
||||||
|
if (mContactPoints != nullptr) {
|
||||||
|
mContactPoints->setPrevious(contactPoint);
|
||||||
|
}
|
||||||
mContactPoints = contactPoint;
|
mContactPoints = contactPoint;
|
||||||
|
|
||||||
mNbContactPoints++;
|
mNbContactPoints++;
|
||||||
assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Clear the obsolete contact points
|
// Clear the obsolete contact points
|
||||||
void ContactManifold::clearObseleteContactPoints() {
|
void ContactManifold::clearObsoleteContactPoints() {
|
||||||
|
|
||||||
|
assert(mContactPoints != nullptr);
|
||||||
|
|
||||||
|
// For each contact point of the manifold
|
||||||
ContactPoint* contactPoint = mContactPoints;
|
ContactPoint* contactPoint = mContactPoints;
|
||||||
ContactPoint* previousContactPoint = nullptr;
|
|
||||||
while (contactPoint != nullptr) {
|
while (contactPoint != nullptr) {
|
||||||
|
|
||||||
ContactPoint* nextContactPoint = contactPoint->getNext();
|
ContactPoint* nextContactPoint = contactPoint->getNext();
|
||||||
|
|
||||||
|
// If the contact point is obsolete
|
||||||
if (contactPoint->getIsObsolete()) {
|
if (contactPoint->getIsObsolete()) {
|
||||||
|
|
||||||
// Delete the contact point
|
// Remove the contact point
|
||||||
contactPoint->~ContactPoint();
|
removeContactPoint(contactPoint);
|
||||||
mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
|
|
||||||
|
|
||||||
if (previousContactPoint != nullptr) {
|
|
||||||
previousContactPoint->setNext(nextContactPoint);
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
mContactPoints = nextContactPoint;
|
|
||||||
}
|
|
||||||
|
|
||||||
mNbContactPoints--;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
previousContactPoint = contactPoint;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
contactPoint = nextContactPoint;
|
contactPoint = nextContactPoint;
|
||||||
}
|
}
|
||||||
|
|
||||||
assert(mNbContactPoints >= 0);
|
assert(mNbContactPoints > 0);
|
||||||
assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
|
assert(mContactPoints != nullptr);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Make sure we do not have too much contact points by keeping only the best
|
||||||
|
// contact points of the manifold (with largest penetration depth)
|
||||||
|
void ContactManifold::reduce() {
|
||||||
|
|
||||||
|
assert(mContactPoints != nullptr);
|
||||||
|
|
||||||
|
// Remove contact points while there is too much contact points
|
||||||
|
while (mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) {
|
||||||
|
removeNonOptimalContactPoint();
|
||||||
|
}
|
||||||
|
|
||||||
|
assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD && mNbContactPoints > 0);
|
||||||
|
assert(mContactPoints != nullptr);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Remove a contact point that is not optimal (with a small penetration depth)
|
||||||
|
void ContactManifold::removeNonOptimalContactPoint() {
|
||||||
|
|
||||||
|
assert(mContactPoints != nullptr);
|
||||||
|
assert(mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD);
|
||||||
|
|
||||||
|
// Get the contact point with the minimum penetration depth among all points
|
||||||
|
ContactPoint* contactPoint = mContactPoints;
|
||||||
|
ContactPoint* minContactPoint = nullptr;
|
||||||
|
decimal minPenetrationDepth = DECIMAL_LARGEST;
|
||||||
|
while (contactPoint != nullptr) {
|
||||||
|
|
||||||
|
ContactPoint* nextContactPoint = contactPoint->getNext();
|
||||||
|
|
||||||
|
if (contactPoint->getPenetrationDepth() < minPenetrationDepth) {
|
||||||
|
|
||||||
|
minContactPoint = contactPoint;
|
||||||
|
minPenetrationDepth = contactPoint->getPenetrationDepth();
|
||||||
|
}
|
||||||
|
|
||||||
|
contactPoint = nextContactPoint;
|
||||||
|
}
|
||||||
|
|
||||||
|
assert(minContactPoint != nullptr);
|
||||||
|
|
||||||
|
// Remove the non optimal contact point
|
||||||
|
removeContactPoint(minContactPoint);
|
||||||
|
|
||||||
|
assert(mNbContactPoints > 0);
|
||||||
|
assert(mContactPoints != nullptr);
|
||||||
}
|
}
|
||||||
|
|
|
@ -143,9 +143,6 @@ class ContactManifold {
|
||||||
/// True if the contact manifold is obsolete
|
/// True if the contact manifold is obsolete
|
||||||
bool mIsObsolete;
|
bool mIsObsolete;
|
||||||
|
|
||||||
/// True if the contact manifold is new (has just been added from potential contacts)
|
|
||||||
bool mIsNew;
|
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
/// Return true if the contact manifold has already been added into an island
|
/// Return true if the contact manifold has already been added into an island
|
||||||
|
@ -161,7 +158,7 @@ class ContactManifold {
|
||||||
void setIsObsolete(bool isObselete, bool setContactPoints);
|
void setIsObsolete(bool isObselete, bool setContactPoints);
|
||||||
|
|
||||||
/// Clear the obsolete contact points
|
/// Clear the obsolete contact points
|
||||||
void clearObseleteContactPoints();
|
void clearObsoleteContactPoints();
|
||||||
|
|
||||||
/// Return the contact normal direction Id of the manifold
|
/// Return the contact normal direction Id of the manifold
|
||||||
short getContactNormalId() const;
|
short getContactNormalId() const;
|
||||||
|
@ -184,6 +181,15 @@ class ContactManifold {
|
||||||
/// Add a contact point
|
/// Add a contact point
|
||||||
void addContactPoint(const ContactPointInfo* contactPointInfo);
|
void addContactPoint(const ContactPointInfo* contactPointInfo);
|
||||||
|
|
||||||
|
/// Make sure we do not have too much contact points by keeping only the best ones
|
||||||
|
void reduce();
|
||||||
|
|
||||||
|
/// Remove a contact point that is not optimal (with a small penetration depth)
|
||||||
|
void removeNonOptimalContactPoint();
|
||||||
|
|
||||||
|
/// Remove a contact point
|
||||||
|
void removeContactPoint(ContactPoint* contactPoint);
|
||||||
|
|
||||||
/// Set the friction twist accumulated impulse
|
/// Set the friction twist accumulated impulse
|
||||||
void setFrictionTwistImpulse(decimal frictionTwistImpulse);
|
void setFrictionTwistImpulse(decimal frictionTwistImpulse);
|
||||||
|
|
||||||
|
@ -208,12 +214,6 @@ class ContactManifold {
|
||||||
/// Return the friction twist accumulated impulse
|
/// Return the friction twist accumulated impulse
|
||||||
decimal getFrictionTwistImpulse() const;
|
decimal getFrictionTwistImpulse() const;
|
||||||
|
|
||||||
/// Return true if the manifold has just been created
|
|
||||||
bool getIsNew() const;
|
|
||||||
|
|
||||||
/// Set the isNew attribute
|
|
||||||
void setIsNew(bool isNew);
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
@ -417,18 +417,6 @@ inline short ContactManifold::getContactNormalId() const {
|
||||||
return mContactNormalId;
|
return mContactNormalId;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// Return true if the manifold has just been created
|
|
||||||
inline bool ContactManifold::getIsNew() const {
|
|
||||||
return mIsNew;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Set the isNew attribute
|
|
||||||
inline void ContactManifold::setIsNew(bool isNew) {
|
|
||||||
mIsNew = isNew;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -60,43 +60,16 @@ void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactMa
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
|
|
||||||
bool addNewManifold = true;
|
// Create a new contact manifold
|
||||||
|
createManifold(contactManifoldInfo);
|
||||||
// If there are too much contact manifolds in the set
|
|
||||||
if (mNbManifolds >= mNbMaxManifolds) {
|
|
||||||
|
|
||||||
// We need to remove a manifold from the set.
|
|
||||||
// We search for an old manifold with the smallest maximum penetration depth among its contact points.
|
|
||||||
// If we do not find an old manifold, we select a new one that has the smallest contact penetration depth.
|
|
||||||
ContactManifold* minDepthManifold = selectManifoldToRemove(contactManifoldInfo->getLargestPenetrationDepth());
|
|
||||||
|
|
||||||
// If the manifold with the minimum penetration depth is an existing one
|
|
||||||
if (minDepthManifold != nullptr) {
|
|
||||||
|
|
||||||
// Remove the manifold
|
|
||||||
removeManifold(minDepthManifold);
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
|
|
||||||
// The manifold we do not want to get is the new one. Therefore, we do not add it to the set
|
|
||||||
addNewManifold = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// If we need to add the new contact manifold
|
|
||||||
if (addNewManifold) {
|
|
||||||
|
|
||||||
// Create a new contact manifold
|
|
||||||
createManifold(contactManifoldInfo);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Update a previous similar manifold with a new one
|
// Update a previous similar manifold with a new one
|
||||||
void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold) {
|
void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold) {
|
||||||
|
|
||||||
assert(oldManifold != nullptr);
|
assert(oldManifold != nullptr);
|
||||||
assert(newManifold != nullptr);
|
assert(newManifold != nullptr);
|
||||||
|
|
||||||
// For each contact point of the new manifold
|
// For each contact point of the new manifold
|
||||||
ContactPointInfo* contactPointInfo = newManifold->getFirstContactPointInfo();
|
ContactPointInfo* contactPointInfo = newManifold->getFirstContactPointInfo();
|
||||||
|
@ -114,7 +87,7 @@ void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold,
|
||||||
if (oldContactPoint->isSimilarWithContactPoint(contactPointInfo)) {
|
if (oldContactPoint->isSimilarWithContactPoint(contactPointInfo)) {
|
||||||
|
|
||||||
// Replace (update) the old contact point with the new one
|
// Replace (update) the old contact point with the new one
|
||||||
oldContactPoint->update(contactPointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
|
oldContactPoint->update(contactPointInfo);
|
||||||
isSimilarPointFound = true;
|
isSimilarPointFound = true;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -136,53 +109,34 @@ void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold,
|
||||||
oldManifold->setIsObsolete(false, false);
|
oldManifold->setIsObsolete(false, false);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the manifold to remove (because it is too old or has not the largest penetration depth)
|
// Remove a contact manifold that is the least optimal (smaller penetration depth)
|
||||||
ContactManifold* ContactManifoldSet::selectManifoldToRemove(decimal penDepthNewManifold) const {
|
void ContactManifoldSet::removeNonOptimalManifold() {
|
||||||
|
|
||||||
assert(mNbManifolds == mNbMaxManifolds);
|
assert(mNbManifolds > mNbMaxManifolds);
|
||||||
assert(mManifolds != nullptr);
|
assert(mManifolds != nullptr);
|
||||||
|
|
||||||
// Look for a manifold that is not new and with the smallest contact penetration depth.
|
// Look for a manifold that is not new and with the smallest contact penetration depth.
|
||||||
// At the same time, we also look for a new manifold with the smallest contact penetration depth
|
// At the same time, we also look for a new manifold with the smallest contact penetration depth
|
||||||
// in case no old manifold exists.
|
// in case no old manifold exists.
|
||||||
ContactManifold* minDepthOldManifold = nullptr;
|
ContactManifold* minDepthManifold = nullptr;
|
||||||
ContactManifold* minDepthNewManifold = nullptr;
|
decimal minDepth = DECIMAL_LARGEST;
|
||||||
decimal minDepthOld = DECIMAL_LARGEST;
|
|
||||||
decimal minDepthNew = penDepthNewManifold;
|
|
||||||
ContactManifold* manifold = mManifolds;
|
ContactManifold* manifold = mManifolds;
|
||||||
while (manifold != nullptr) {
|
while (manifold != nullptr) {
|
||||||
|
|
||||||
// Get the largest contact point penetration depth of the manifold
|
// Get the largest contact point penetration depth of the manifold
|
||||||
const decimal depth = manifold->getLargestContactDepth();
|
const decimal depth = manifold->getLargestContactDepth();
|
||||||
|
|
||||||
// If it is a new manifold
|
if (depth < minDepth) {
|
||||||
if (manifold->getIsNew()) {
|
minDepth = depth;
|
||||||
|
minDepthManifold = manifold;
|
||||||
if (depth < minDepthNew) {
|
|
||||||
minDepthNew = depth;
|
|
||||||
minDepthNewManifold = manifold;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
|
|
||||||
if (depth < minDepthOld) {
|
|
||||||
minDepthOld = depth;
|
|
||||||
minDepthOldManifold = manifold;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
manifold = manifold->getNext();
|
manifold = manifold->getNext();
|
||||||
}
|
}
|
||||||
|
|
||||||
// If there was a contact manifold that was not new
|
// Remove the non optimal manifold
|
||||||
if (minDepthOldManifold != nullptr) {
|
assert(minDepthManifold != nullptr);
|
||||||
|
removeManifold(minDepthManifold);
|
||||||
// Return the old manifold with the smallest penetration depth
|
|
||||||
return minDepthOldManifold;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Otherwise, we return the new manifold with the smallest penetration depth
|
|
||||||
return minDepthNewManifold;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the contact manifold with a similar average normal.
|
// Return the contact manifold with a similar average normal.
|
||||||
|
@ -262,7 +216,6 @@ void ContactManifoldSet::clear() {
|
||||||
|
|
||||||
// Create a new contact manifold and add it to the set
|
// Create a new contact manifold and add it to the set
|
||||||
void ContactManifoldSet::createManifold(const ContactManifoldInfo* manifoldInfo) {
|
void ContactManifoldSet::createManifold(const ContactManifoldInfo* manifoldInfo) {
|
||||||
assert(mNbManifolds < mNbMaxManifolds);
|
|
||||||
|
|
||||||
ContactManifold* manifold = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
|
ContactManifold* manifold = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
|
||||||
ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator);
|
ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator);
|
||||||
|
@ -333,9 +286,26 @@ void ContactManifoldSet::clearObsoleteManifoldsAndContactPoints() {
|
||||||
else {
|
else {
|
||||||
|
|
||||||
// Clear the obsolete contact points of the manifold
|
// Clear the obsolete contact points of the manifold
|
||||||
manifold->clearObseleteContactPoints();
|
manifold->clearObsoleteContactPoints();
|
||||||
}
|
}
|
||||||
|
|
||||||
manifold = nextManifold;
|
manifold = nextManifold;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Remove some contact manifolds and contact points if there are too many of them
|
||||||
|
void ContactManifoldSet::reduce() {
|
||||||
|
|
||||||
|
// Remove non optimal contact manifold while there are too many manifolds in the set
|
||||||
|
while (mNbManifolds > mNbMaxManifolds) {
|
||||||
|
removeNonOptimalManifold();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Reduce all the contact manifolds in case they have too many contact points
|
||||||
|
ContactManifold* manifold = mManifolds;
|
||||||
|
while (manifold != nullptr) {
|
||||||
|
manifold->reduce();
|
||||||
|
manifold = manifold->getNext();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
|
@ -74,8 +74,8 @@ class ContactManifoldSet {
|
||||||
// Return the contact manifold with a similar average normal.
|
// Return the contact manifold with a similar average normal.
|
||||||
ContactManifold* selectManifoldWithSimilarNormal(short int normalDirectionId) const;
|
ContactManifold* selectManifoldWithSimilarNormal(short int normalDirectionId) const;
|
||||||
|
|
||||||
/// Return the manifold to remove (because it is too old or has not the largest penetration depth)
|
/// Remove a contact manifold that is the least optimal (smaller penetration depth)
|
||||||
ContactManifold* selectManifoldToRemove(decimal penDepthNewManifold) const;
|
void removeNonOptimalManifold();
|
||||||
|
|
||||||
/// Update a previous similar manifold with a new one
|
/// Update a previous similar manifold with a new one
|
||||||
void updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold);
|
void updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold);
|
||||||
|
@ -124,8 +124,8 @@ class ContactManifoldSet {
|
||||||
/// Clear the obsolete contact manifolds and contact points
|
/// Clear the obsolete contact manifolds and contact points
|
||||||
void clearObsoleteManifoldsAndContactPoints();
|
void clearObsoleteManifoldsAndContactPoints();
|
||||||
|
|
||||||
/// Reset the isNew status of all the manifolds
|
// Remove some contact manifolds and contact points if there are too many of them
|
||||||
void resetIsNewManifoldStatus();
|
void reduce();
|
||||||
|
|
||||||
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
|
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
|
||||||
// Each face of the cube is divided into 4x4 buckets. This method maps the
|
// Each face of the cube is divided into 4x4 buckets. This method maps the
|
||||||
|
@ -180,17 +180,6 @@ inline int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// Reset the isNew status of all the manifolds
|
|
||||||
inline void ContactManifoldSet::resetIsNewManifoldStatus() {
|
|
||||||
|
|
||||||
ContactManifold* manifold = mManifolds;
|
|
||||||
while (manifold != nullptr) {
|
|
||||||
manifold->setIsNew(false);
|
|
||||||
manifold = manifold->getNext();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -77,6 +77,8 @@ struct ContactPointInfo {
|
||||||
: normal(contactNormal), penetrationDepth(penDepth),
|
: normal(contactNormal), penetrationDepth(penDepth),
|
||||||
localPoint1(localPt1), localPoint2(localPt2), next(nullptr), isUsed(false) {
|
localPoint1(localPt1), localPoint2(localPt2), next(nullptr), isUsed(false) {
|
||||||
|
|
||||||
|
assert(contactNormal.lengthSquare() > decimal(0.8));
|
||||||
|
assert(penDepth > decimal(0.0));
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Destructor
|
/// Destructor
|
||||||
|
|
|
@ -31,26 +31,22 @@ using namespace reactphysics3d;
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, const Transform& body1Transform,
|
ContactPoint::ContactPoint(const ContactPointInfo* contactInfo)
|
||||||
const Transform& body2Transform)
|
|
||||||
: mNormal(contactInfo->normal),
|
: mNormal(contactInfo->normal),
|
||||||
mPenetrationDepth(contactInfo->penetrationDepth),
|
mPenetrationDepth(contactInfo->penetrationDepth),
|
||||||
mLocalPointOnBody1(contactInfo->localPoint1),
|
mLocalPointOnBody1(contactInfo->localPoint1),
|
||||||
mLocalPointOnBody2(contactInfo->localPoint2),
|
mLocalPointOnBody2(contactInfo->localPoint2),
|
||||||
mIsRestingContact(false), mIsObsolete(false), mNext(nullptr) {
|
mIsRestingContact(false), mIsObsolete(false), mNext(nullptr), mPrevious(nullptr) {
|
||||||
|
|
||||||
assert(mPenetrationDepth > decimal(0.0));
|
assert(mPenetrationDepth > decimal(0.0));
|
||||||
|
assert(mNormal.lengthSquare() > decimal(0.8));
|
||||||
// Compute the world position of the contact points
|
|
||||||
mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
|
|
||||||
mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
|
|
||||||
|
|
||||||
mIsObsolete = false;
|
mIsObsolete = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Update the contact point with a new one that is similar (very close)
|
// Update the contact point with a new one that is similar (very close)
|
||||||
/// The idea is to keep the cache impulse (for warm starting the contact solver)
|
/// The idea is to keep the cache impulse (for warm starting the contact solver)
|
||||||
void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& body1Transform, const Transform& body2Transform) {
|
void ContactPoint::update(const ContactPointInfo* contactInfo) {
|
||||||
|
|
||||||
assert(isSimilarWithContactPoint(contactInfo));
|
assert(isSimilarWithContactPoint(contactInfo));
|
||||||
assert(contactInfo->penetrationDepth > decimal(0.0));
|
assert(contactInfo->penetrationDepth > decimal(0.0));
|
||||||
|
@ -60,9 +56,5 @@ void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform&
|
||||||
mLocalPointOnBody1 = contactInfo->localPoint1;
|
mLocalPointOnBody1 = contactInfo->localPoint1;
|
||||||
mLocalPointOnBody2 = contactInfo->localPoint2;
|
mLocalPointOnBody2 = contactInfo->localPoint2;
|
||||||
|
|
||||||
// Compute the world position of the contact points
|
|
||||||
mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
|
|
||||||
mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
|
|
||||||
|
|
||||||
mIsObsolete = false;
|
mIsObsolete = false;
|
||||||
}
|
}
|
||||||
|
|
|
@ -59,12 +59,6 @@ class ContactPoint {
|
||||||
/// Contact point on body 2 in local space of body 2
|
/// Contact point on body 2 in local space of body 2
|
||||||
Vector3 mLocalPointOnBody2;
|
Vector3 mLocalPointOnBody2;
|
||||||
|
|
||||||
/// Contact point on body 1 in world space
|
|
||||||
Vector3 mWorldPointOnBody1;
|
|
||||||
|
|
||||||
/// Contact point on body 2 in world space
|
|
||||||
Vector3 mWorldPointOnBody2;
|
|
||||||
|
|
||||||
/// True if the contact is a resting contact (exists for more than one time step)
|
/// True if the contact is a resting contact (exists for more than one time step)
|
||||||
bool mIsRestingContact;
|
bool mIsRestingContact;
|
||||||
|
|
||||||
|
@ -74,14 +68,16 @@ class ContactPoint {
|
||||||
/// True if the contact point is obsolete
|
/// True if the contact point is obsolete
|
||||||
bool mIsObsolete;
|
bool mIsObsolete;
|
||||||
|
|
||||||
/// Pointer to the next contact point in the linked-list
|
/// Pointer to the next contact point in the double linked-list
|
||||||
ContactPoint* mNext;
|
ContactPoint* mNext;
|
||||||
|
|
||||||
|
/// Pointer to the previous contact point in the double linked-list
|
||||||
|
ContactPoint* mPrevious;
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
/// Update the contact point with a new one that is similar (very close)
|
/// Update the contact point with a new one that is similar (very close)
|
||||||
void update(const ContactPointInfo* contactInfo, const Transform& body1Transform,
|
void update(const ContactPointInfo* contactInfo);
|
||||||
const Transform& body2Transform);
|
|
||||||
|
|
||||||
/// Return true if the contact point is similar (close enougth) to another given contact point
|
/// Return true if the contact point is similar (close enougth) to another given contact point
|
||||||
bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const;
|
bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const;
|
||||||
|
@ -99,6 +95,9 @@ class ContactPoint {
|
||||||
/// Set the next contact point in the linked list
|
/// Set the next contact point in the linked list
|
||||||
void setNext(ContactPoint* next);
|
void setNext(ContactPoint* next);
|
||||||
|
|
||||||
|
/// Set the previous contact point in the linked list
|
||||||
|
void setPrevious(ContactPoint* previous);
|
||||||
|
|
||||||
/// Return true if the contact point is obsolete
|
/// Return true if the contact point is obsolete
|
||||||
bool getIsObsolete() const;
|
bool getIsObsolete() const;
|
||||||
|
|
||||||
|
@ -107,8 +106,7 @@ class ContactPoint {
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
/// Constructor
|
/// Constructor
|
||||||
ContactPoint(const ContactPointInfo* contactInfo, const Transform& body1Transform,
|
ContactPoint(const ContactPointInfo* contactInfo);
|
||||||
const Transform& body2Transform);
|
|
||||||
|
|
||||||
/// Destructor
|
/// Destructor
|
||||||
~ContactPoint() = default;
|
~ContactPoint() = default;
|
||||||
|
@ -128,18 +126,15 @@ class ContactPoint {
|
||||||
/// Return the contact local point on body 2
|
/// Return the contact local point on body 2
|
||||||
Vector3 getLocalPointOnBody2() const;
|
Vector3 getLocalPointOnBody2() const;
|
||||||
|
|
||||||
/// Return the contact world point on body 1
|
|
||||||
Vector3 getWorldPointOnBody1() const;
|
|
||||||
|
|
||||||
/// Return the contact world point on body 2
|
|
||||||
Vector3 getWorldPointOnBody2() const;
|
|
||||||
|
|
||||||
/// Return the cached penetration impulse
|
/// Return the cached penetration impulse
|
||||||
decimal getPenetrationImpulse() const;
|
decimal getPenetrationImpulse() const;
|
||||||
|
|
||||||
/// Return true if the contact is a resting contact
|
/// Return true if the contact is a resting contact
|
||||||
bool getIsRestingContact() const;
|
bool getIsRestingContact() const;
|
||||||
|
|
||||||
|
/// Return the previous contact point in the linked list
|
||||||
|
inline ContactPoint* getPrevious() const;
|
||||||
|
|
||||||
/// Return the next contact point in the linked list
|
/// Return the next contact point in the linked list
|
||||||
ContactPoint* getNext() const;
|
ContactPoint* getNext() const;
|
||||||
|
|
||||||
|
@ -170,16 +165,6 @@ inline Vector3 ContactPoint::getLocalPointOnBody2() const {
|
||||||
return mLocalPointOnBody2;
|
return mLocalPointOnBody2;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the contact world point on body 1
|
|
||||||
inline Vector3 ContactPoint::getWorldPointOnBody1() const {
|
|
||||||
return mWorldPointOnBody1;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return the contact world point on body 2
|
|
||||||
inline Vector3 ContactPoint::getWorldPointOnBody2() const {
|
|
||||||
return mWorldPointOnBody2;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return the cached penetration impulse
|
// Return the cached penetration impulse
|
||||||
inline decimal ContactPoint::getPenetrationImpulse() const {
|
inline decimal ContactPoint::getPenetrationImpulse() const {
|
||||||
return mPenetrationImpulse;
|
return mPenetrationImpulse;
|
||||||
|
@ -226,6 +211,16 @@ inline void ContactPoint::setNext(ContactPoint* next) {
|
||||||
mNext = next;
|
mNext = next;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Return the previous contact point in the linked list
|
||||||
|
inline ContactPoint* ContactPoint::getPrevious() const {
|
||||||
|
return mPrevious;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set the previous contact point in the linked list
|
||||||
|
inline void ContactPoint::setPrevious(ContactPoint* previous) {
|
||||||
|
mPrevious = previous;
|
||||||
|
}
|
||||||
|
|
||||||
// Return the penetration depth of the contact
|
// Return the penetration depth of the contact
|
||||||
inline decimal ContactPoint::getPenetrationDepth() const {
|
inline decimal ContactPoint::getPenetrationDepth() const {
|
||||||
return mPenetrationDepth;
|
return mPenetrationDepth;
|
||||||
|
|
|
@ -120,6 +120,10 @@ void ContactSolver::initializeForIsland(Island* island) {
|
||||||
assert(body1 != nullptr);
|
assert(body1 != nullptr);
|
||||||
assert(body2 != nullptr);
|
assert(body2 != nullptr);
|
||||||
|
|
||||||
|
// Get the two contact shapes
|
||||||
|
const ProxyShape* shape1 = externalManifold->getShape1();
|
||||||
|
const ProxyShape* shape2 = externalManifold->getShape2();
|
||||||
|
|
||||||
// Get the position of the two bodies
|
// Get the position of the two bodies
|
||||||
const Vector3& x1 = body1->mCenterOfMassWorld;
|
const Vector3& x1 = body1->mCenterOfMassWorld;
|
||||||
const Vector3& x2 = body2->mCenterOfMassWorld;
|
const Vector3& x2 = body2->mCenterOfMassWorld;
|
||||||
|
@ -149,11 +153,12 @@ void ContactSolver::initializeForIsland(Island* island) {
|
||||||
|
|
||||||
// For each contact point of the contact manifold
|
// For each contact point of the contact manifold
|
||||||
ContactPoint* externalContact = externalManifold->getContactPoints();
|
ContactPoint* externalContact = externalManifold->getContactPoints();
|
||||||
|
assert(externalContact != nullptr);
|
||||||
while (externalContact != nullptr) {
|
while (externalContact != nullptr) {
|
||||||
|
|
||||||
// Get the contact point on the two bodies
|
// Get the contact point on the two bodies
|
||||||
Vector3 p1 = externalContact->getWorldPointOnBody1();
|
Vector3 p1 = shape1->getLocalToWorldTransform() * externalContact->getLocalPointOnBody1();
|
||||||
Vector3 p2 = externalContact->getWorldPointOnBody2();
|
Vector3 p2 = shape2->getLocalToWorldTransform() * externalContact->getLocalPointOnBody2();
|
||||||
|
|
||||||
new (mContactPoints + mNbContactPoints) ContactPointSolver();
|
new (mContactPoints + mNbContactPoints) ContactPointSolver();
|
||||||
mContactPoints[mNbContactPoints].externalContact = externalContact;
|
mContactPoints[mNbContactPoints].externalContact = externalContact;
|
||||||
|
|
|
@ -136,9 +136,6 @@ class OverlappingPair {
|
||||||
/// Return the last frame collision info
|
/// Return the last frame collision info
|
||||||
LastFrameCollisionInfo& getLastFrameCollisionInfo();
|
LastFrameCollisionInfo& getLastFrameCollisionInfo();
|
||||||
|
|
||||||
/// Return the number of contacts in the cache
|
|
||||||
uint getNbContactPoints() const;
|
|
||||||
|
|
||||||
/// Return the a reference to the contact manifold set
|
/// Return the a reference to the contact manifold set
|
||||||
const ContactManifoldSet& getContactManifoldSet();
|
const ContactManifoldSet& getContactManifoldSet();
|
||||||
|
|
||||||
|
@ -167,13 +164,13 @@ class OverlappingPair {
|
||||||
void reducePotentialContactManifolds();
|
void reducePotentialContactManifolds();
|
||||||
|
|
||||||
/// Make the contact manifolds and contact points obsolete
|
/// Make the contact manifolds and contact points obsolete
|
||||||
void makeContactsObselete();
|
void makeContactsObsolete();
|
||||||
|
|
||||||
/// Clear the obsolete contact manifold and contact points
|
/// Clear the obsolete contact manifold and contact points
|
||||||
void clearObsoleteManifoldsAndContactPoints();
|
void clearObsoleteManifoldsAndContactPoints();
|
||||||
|
|
||||||
/// Reset the isNew status of all the manifolds
|
/// Reduce the contact manifolds that have too many contact points
|
||||||
void resetIsNewManifoldStatus();
|
void reduceContactManifolds();
|
||||||
|
|
||||||
/// Return the pair of bodies index
|
/// Return the pair of bodies index
|
||||||
static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2);
|
static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2);
|
||||||
|
@ -206,18 +203,13 @@ inline LastFrameCollisionInfo& OverlappingPair::getLastFrameCollisionInfo() {
|
||||||
return mLastFrameCollisionInfo;
|
return mLastFrameCollisionInfo;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the number of contact points in the contact manifold
|
|
||||||
inline uint OverlappingPair::getNbContactPoints() const {
|
|
||||||
return mContactManifoldSet.getTotalNbContactPoints();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return the contact manifold
|
// Return the contact manifold
|
||||||
inline const ContactManifoldSet& OverlappingPair::getContactManifoldSet() {
|
inline const ContactManifoldSet& OverlappingPair::getContactManifoldSet() {
|
||||||
return mContactManifoldSet;
|
return mContactManifoldSet;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Make the contact manifolds and contact points obsolete
|
// Make the contact manifolds and contact points obsolete
|
||||||
inline void OverlappingPair::makeContactsObselete() {
|
inline void OverlappingPair::makeContactsObsolete() {
|
||||||
|
|
||||||
mContactManifoldSet.makeContactsObsolete();
|
mContactManifoldSet.makeContactsObsolete();
|
||||||
}
|
}
|
||||||
|
@ -272,10 +264,9 @@ inline void OverlappingPair::clearObsoleteManifoldsAndContactPoints() {
|
||||||
mContactManifoldSet.clearObsoleteManifoldsAndContactPoints();
|
mContactManifoldSet.clearObsoleteManifoldsAndContactPoints();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Reduce the contact manifolds that have too many contact points
|
||||||
// Reset the isNew status of all the manifolds
|
inline void OverlappingPair::reduceContactManifolds() {
|
||||||
inline void OverlappingPair::resetIsNewManifoldStatus() {
|
mContactManifoldSet.reduce();
|
||||||
mContactManifoldSet.resetIsNewManifoldStatus();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue
Block a user