Working on contacts reduction

This commit is contained in:
Daniel Chappuis 2017-06-05 00:05:49 +02:00
parent b1597c508f
commit 95db87fd62
5 changed files with 306 additions and 61 deletions

View File

@ -62,6 +62,7 @@ SET (REACTPHYSICS3D_SOURCES
"src/body/RigidBody.cpp"
"src/collision/ContactPointInfo.h"
"src/collision/ContactManifoldInfo.h"
"src/collision/ContactManifoldInfo.cpp"
"src/collision/broadphase/BroadPhaseAlgorithm.h"
"src/collision/broadphase/BroadPhaseAlgorithm.cpp"
"src/collision/broadphase/DynamicAABBTree.h"

View File

@ -248,8 +248,8 @@ void CollisionDetection::computeNarrowPhase() {
ContactManifoldInfo contactManifoldInfo(mSingleFrameAllocator);
if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, contactManifoldInfo)) {
// Reduce the number of points in the contact manifold
contactManifoldInfo.reduce();
// Reduce the number of points in the contact manifold (if necessary)
contactManifoldInfo.reduce(currentNarrowPhaseInfo->shape1ToWorldTransform);
// If it is the first contact since the pairs are overlapping
if (currentNarrowPhaseInfo->overlappingPair->getNbContactPoints() == 0) {
@ -696,6 +696,9 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
// Reduce the number of points in the contact manifold (if necessary)
contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform);
CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body1, body2,
body1ProxyShape, body2ProxyShape);
@ -777,6 +780,9 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
// Reduce the number of points in the contact manifold (if necessary)
contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform);
CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body,
proxyShape->getBody(), bodyProxyShape,
proxyShape);
@ -848,6 +854,9 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
// Reduce the number of points in the contact manifold (if necessary)
contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform);
CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, shape1->getBody(),
shape2->getBody(), shape1, shape2);

View File

@ -37,9 +37,6 @@
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Constants
const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold
// Class declarations
class ContactManifold;

View File

@ -0,0 +1,278 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 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 "ContactManifoldInfo.h"
using namespace reactphysics3d;
// Constructor
ContactManifoldInfo::ContactManifoldInfo(Allocator& allocator)
: mContactPointsList(nullptr), mAllocator(allocator), mNbContactPoints(0) {}
// Destructor
ContactManifoldInfo::~ContactManifoldInfo() {
// Remove all the contact points
reset();
}
// Add a new contact point into the manifold
void ContactManifoldInfo::addContactPoint(const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2) {
assert(penDepth > decimal(0.0));
// Create the contact point info
ContactPointInfo* contactPointInfo = new (mAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(contactNormal, penDepth, localPt1, localPt2);
// Add it into the linked list of contact points
contactPointInfo->next = mContactPointsList;
mContactPointsList = contactPointInfo;
mNbContactPoints++;
}
// Remove all the contact points
void ContactManifoldInfo::reset() {
// Delete all the contact points in the linked list
ContactPointInfo* element = mContactPointsList;
while(element != nullptr) {
ContactPointInfo* elementToDelete = element;
element = element->next;
// Delete the current element
mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
}
mContactPointsList = nullptr;
mNbContactPoints = 0;
}
// Reduce the number of points in the contact manifold
// This is based on the technique described by Dirk Gregorius in his
// "Contacts Creation" GDC presentation
void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) {
assert(mContactPointsList != nullptr);
// TODO : Implement this (do not forget to deallocate removed points)
// The following algorithm only works to reduce to 4 contact points
assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4);
// If there are too many contact points in the manifold
if (mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) {
ContactPointInfo* pointsToKeep[MAX_CONTACT_POINTS_IN_MANIFOLD];
// Compute the initial contact point we need to keep.
// The first point we keep is always the point in a given
// constant direction (in order to always have same contact points
// between frames for better stability)
const Transform worldToShape1Transform = shape1ToWorldTransform.getInverse();
// Compute the contact normal of the manifold (we use the first contact point)
// in the local-space of the first collision shape
const Vector3 contactNormalShape1Space = worldToShape1Transform.getOrientation() * mContactPointsList->normal;
// Compute a search direction
const Vector3 searchDirection(1, 1, 1);
ContactPointInfo* element = mContactPointsList;
pointsToKeep[0] = element;
decimal maxDotProduct = searchDirection.dot(element->localPoint1);
element = element->next;
while(element != nullptr) {
decimal dotProduct = searchDirection.dot(element->localPoint1);
if (dotProduct > maxDotProduct) {
maxDotProduct = dotProduct;
pointsToKeep[0] = element;
}
element = element->next;
}
// Compute the second contact point we need to keep.
// The second point we keep is the one farthest away from the first point.
decimal maxDistance = decimal(0.0);
element = mContactPointsList;
while(element != nullptr) {
if (element == pointsToKeep[0]) {
element = element->next;
continue;
}
decimal distance = (pointsToKeep[0]->localPoint1 - element->localPoint1).lengthSquare();
if (distance >= maxDistance) {
maxDistance = distance;
pointsToKeep[1] = element;
}
element = element->next;
}
assert(pointsToKeep[1] != nullptr);
// Compute the third contact point we need to keep.
// The second point is the one producing the triangle with the larger area
// with first and second point.
// We compute the most positive or most negative triangle area (depending on winding)
ContactPointInfo* thirdPointMaxArea = nullptr;
ContactPointInfo* thirdPointMinArea = nullptr;
decimal minArea = decimal(0.0);
decimal maxArea = decimal(0.0);
bool isPreviousAreaPositive = true;
element = mContactPointsList;
while(element != nullptr) {
if (element == pointsToKeep[0] || element == pointsToKeep[1]) {
element = element->next;
continue;
}
const Vector3 newToFirst = pointsToKeep[0]->localPoint1 - element->localPoint1;
const Vector3 newToSecond = pointsToKeep[1]->localPoint1 - element->localPoint1;
// Compute the triangle area
decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space);
if (area >= maxArea) {
maxArea = area;
thirdPointMaxArea = element;
}
if (area <= minArea) {
minArea = area;
thirdPointMinArea = element;
}
element = element->next;
}
assert(minArea <= decimal(0.0));
assert(maxArea >= decimal(0.0));
if (maxArea > (-minArea)) {
isPreviousAreaPositive = true;
pointsToKeep[2] = thirdPointMaxArea;
}
else {
isPreviousAreaPositive = false;
pointsToKeep[2] = thirdPointMinArea;
}
assert(pointsToKeep[2] != nullptr);
// Compute the 4th point by choosing the triangle that add the most
// triangle area to the previous triangle and has opposite sign area (opposite winding)
decimal largestArea = decimal(0.0); // Largest area (positive or negative)
element = mContactPointsList;
// For each remaining point
while(element != nullptr) {
if (element == pointsToKeep[0] || element == pointsToKeep[1] || element == pointsToKeep[2]) {
element = element->next;
continue;
}
// For each edge of the triangle made by the first three points
for (uint i=0; i<3; i++) {
uint edgeVertex1Index = i;
uint edgeVertex2Index = i < 2 ? i + 1 : 0;
const Vector3 newToFirst = pointsToKeep[edgeVertex1Index]->localPoint1 - element->localPoint1;
const Vector3 newToSecond = pointsToKeep[edgeVertex2Index]->localPoint1 - element->localPoint1;
// Compute the triangle area
decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space);
// We are looking at the triangle with maximal area (positive or negative).
// If the previous area is positive, we are looking at negative area now.
// If the previous area is negative, we are looking at the positive area now.
if (isPreviousAreaPositive && area < largestArea) {
largestArea = area;
pointsToKeep[3] = element;
}
else if (!isPreviousAreaPositive && area > largestArea) {
largestArea = area;
pointsToKeep[3] = element;
}
element = element->next;
}
}
assert(pointsToKeep[3] != nullptr);
// Delete the contact points we do not want to keep from the linked list
element = mContactPointsList;
ContactPointInfo* previousElement = nullptr;
while(element != nullptr) {
// Skip the points we want to keep
if (element == pointsToKeep[0] || element == pointsToKeep[1] ||
element == pointsToKeep[2] || element == pointsToKeep[3]) {
previousElement = element;
element = element->next;
continue;
}
ContactPointInfo* elementToDelete = element;
if (previousElement != nullptr) {
previousElement->next = elementToDelete->next;
}
else {
mContactPointsList = elementToDelete->next;
}
element = element->next;
// Delete the current element
mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
}
mNbContactPoints = 4;
}
}
/// Return the largest penetration depth among the contact points
decimal ContactManifoldInfo::getLargestPenetrationDepth() const {
decimal maxDepth = decimal(0.0);
ContactPointInfo* element = mContactPointsList;
while(element != nullptr) {
if (element->penetrationDepth > maxDepth) {
maxDepth = element->penetrationDepth;
}
element = element->next;
}
return maxDepth;
}

View File

@ -33,6 +33,8 @@
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Constants
const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold
// Class ContactManifoldInfo
/**
@ -51,22 +53,18 @@ class ContactManifoldInfo {
/// Memory allocator used to allocate contact points
Allocator& mAllocator;
// -------------------- Methods -------------------- //
/// Number of contact points in the manifold
uint mNbContactPoints;
public:
// -------------------- Methods -------------------- //
/// Constructor
ContactManifoldInfo(Allocator& allocator) : mContactPointsList(nullptr), mAllocator(allocator) {}
ContactManifoldInfo(Allocator& allocator);
/// Destructor
~ContactManifoldInfo() {
// Remove all the contact points
reset();
}
~ContactManifoldInfo();
/// Deleted copy-constructor
ContactManifoldInfo(const ContactManifoldInfo& contactManifold) = delete;
@ -76,64 +74,26 @@ class ContactManifoldInfo {
/// Add a new contact point into the manifold
void addContactPoint(const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2) {
assert(penDepth > decimal(0.0));
// Create the contact point info
ContactPointInfo* contactPointInfo = new (mAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(contactNormal, penDepth, localPt1, localPt2);
// Add it into the linked list of contact points
contactPointInfo->next = mContactPointsList;
mContactPointsList = contactPointInfo;
}
const Vector3& localPt1, const Vector3& localPt2);
/// Remove all the contact points
void reset() {
// Delete all the contact points in the linked list
ContactPointInfo* element = mContactPointsList;
while(element != nullptr) {
ContactPointInfo* elementToDelete = element;
element = element->next;
// Delete the current element
mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
}
mContactPointsList = nullptr;
}
void reset();
/// Get the first contact point info of the linked list of contact points
ContactPointInfo* getFirstContactPointInfo() const {
return mContactPointsList;
}
ContactPointInfo* getFirstContactPointInfo() const;
/// Reduce the number of points in the contact manifold
void reduce() {
// TODO : Implement this (do not forget to deallocate removed points)
}
void reduce(const Transform& shape1ToWorldTransform);
/// Return the largest penetration depth among the contact points
decimal getLargestPenetrationDepth() const {
decimal maxDepth = decimal(0.0);
ContactPointInfo* element = mContactPointsList;
while(element != nullptr) {
if (element->penetrationDepth > maxDepth) {
maxDepth = element->penetrationDepth;
}
element = element->next;
}
return maxDepth;
}
decimal getLargestPenetrationDepth() const;
};
// Get the first contact point info of the linked list of contact points
inline ContactPointInfo* ContactManifoldInfo::getFirstContactPointInfo() const {
return mContactPointsList;
}
}
#endif