198 lines
6.5 KiB
C++
198 lines
6.5 KiB
C++
/** @file
|
|
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
* @author Daniel CHAPPUIS
|
|
* @author Edouard DUPIN
|
|
* @copyright 2010-2016, Daniel Chappuis
|
|
* @copyright 2017, Edouard DUPIN
|
|
* @license MPL v2.0 (see license file)
|
|
*/
|
|
#include <ephysics/collision/ContactManifoldSet.hpp>
|
|
|
|
using namespace ephysics;
|
|
|
|
ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1,
|
|
ProxyShape* shape2,
|
|
int nbMaxManifolds):
|
|
this.nbMaxManifolds(nbMaxManifolds),
|
|
this.nbManifolds(0),
|
|
this.shape1(shape1),
|
|
this.shape2(shape2) {
|
|
assert(nbMaxManifolds >= 1);
|
|
}
|
|
|
|
ContactManifoldSet::~ContactManifoldSet() {
|
|
clear();
|
|
}
|
|
|
|
void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
|
|
// Compute an Id corresponding to the normal direction (using a cubemap)
|
|
int16t normalDirectionId = computeCubemapNormalId(contact.getNormal());
|
|
// If there is no contact manifold yet
|
|
if (this.nbManifolds == 0) {
|
|
createManifold(normalDirectionId);
|
|
this.manifolds[0].addContactPoint(contact);
|
|
assert(this.manifolds[this.nbManifolds-1].getNbContactPoints() > 0);
|
|
for (int i=0; i<this.nbManifolds; i++) {
|
|
assert(this.manifolds[i].getNbContactPoints() > 0);
|
|
}
|
|
return;
|
|
}
|
|
// Select the manifold with the most similar normal (if exists)
|
|
int similarManifoldIndex = 0;
|
|
if (this.nbMaxManifolds > 1) {
|
|
similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId);
|
|
}
|
|
// If a similar manifold has been found
|
|
if (similarManifoldIndex != -1) {
|
|
// Add the contact point to that similar manifold
|
|
this.manifolds[similarManifoldIndex].addContactPoint(contact);
|
|
assert(this.manifolds[similarManifoldIndex].getNbContactPoints() > 0);
|
|
return;
|
|
}
|
|
// If the maximum number of manifold has not been reached yet
|
|
if (this.nbManifolds < this.nbMaxManifolds) {
|
|
// Create a new manifold for the contact point
|
|
createManifold(normalDirectionId);
|
|
this.manifolds[this.nbManifolds-1].addContactPoint(contact);
|
|
for (int i=0; i<this.nbManifolds; i++) {
|
|
assert(this.manifolds[i].getNbContactPoints() > 0);
|
|
}
|
|
return;
|
|
}
|
|
// The contact point will be in a new contact manifold, we now have too much
|
|
// manifolds condidates. We need to remove one. We choose to keep the manifolds
|
|
// with the largest contact depth among their points
|
|
int smallestDepthIndex = -1;
|
|
float minDepth = contact.getPenetrationDepth();
|
|
assert(this.nbManifolds == this.nbMaxManifolds);
|
|
for (int i=0; i<this.nbManifolds; i++) {
|
|
float depth = this.manifolds[i].getLargestContactDepth();
|
|
if (depth < minDepth) {
|
|
minDepth = depth;
|
|
smallestDepthIndex = i;
|
|
}
|
|
}
|
|
// If we do not want to keep to new manifold (not created yet) with the
|
|
// new contact point
|
|
if (smallestDepthIndex == -1) {
|
|
// Delete the new contact
|
|
ETKDELETE(ContactPoint, contact);
|
|
contact = null;
|
|
return;
|
|
}
|
|
assert(smallestDepthIndex >= 0 && smallestDepthIndex < this.nbManifolds);
|
|
// Here we need to replace an existing manifold with a new one (that contains
|
|
// the new contact point)
|
|
removeManifold(smallestDepthIndex);
|
|
createManifold(normalDirectionId);
|
|
this.manifolds[this.nbManifolds-1].addContactPoint(contact);
|
|
assert(this.manifolds[this.nbManifolds-1].getNbContactPoints() > 0);
|
|
for (int i=0; i<this.nbManifolds; i++) {
|
|
assert(this.manifolds[i].getNbContactPoints() > 0);
|
|
}
|
|
return;
|
|
}
|
|
|
|
int ContactManifoldSet::selectManifoldWithSimilarNormal(int16t normalDirectionId) {
|
|
// Return the Id of the manifold with the same normal direction id (if exists)
|
|
for (int i=0; i<this.nbManifolds; i++) {
|
|
if (normalDirectionId == this.manifolds[i].getNormalDirectionId()) {
|
|
return i;
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
int16t ContactManifoldSet::computeCubemapNormalId( Vector3f normal) {
|
|
assert(normal.length2() > FLTEPSILON);
|
|
int faceNo;
|
|
float u, v;
|
|
float max = max3(fabs(normal.x()), fabs(normal.y()), fabs(normal.z()));
|
|
Vector3f normalScaled = normal / max;
|
|
if (normalScaled.x() >= normalScaled.y() && normalScaled.x() >= normalScaled.z()) {
|
|
faceNo = normalScaled.x() > 0 ? 0 : 1;
|
|
u = normalScaled.y();
|
|
v = normalScaled.z();
|
|
} else if (normalScaled.y() >= normalScaled.x() && normalScaled.y() >= normalScaled.z()) {
|
|
faceNo = normalScaled.y() > 0 ? 2 : 3;
|
|
u = normalScaled.x();
|
|
v = normalScaled.z();
|
|
} else {
|
|
faceNo = normalScaled.z() > 0 ? 4 : 5;
|
|
u = normalScaled.x();
|
|
v = normalScaled.y();
|
|
}
|
|
int indexU = floor(((u + 1)/2) * CONTACTCUBEMAPFACENBSUBDIVISIONS);
|
|
int indexV = floor(((v + 1)/2) * CONTACTCUBEMAPFACENBSUBDIVISIONS);
|
|
if (indexU == CONTACTCUBEMAPFACENBSUBDIVISIONS) {
|
|
indexU--;
|
|
}
|
|
if (indexV == CONTACTCUBEMAPFACENBSUBDIVISIONS) {
|
|
indexV--;
|
|
}
|
|
int nbSubDivInFace = CONTACTCUBEMAPFACENBSUBDIVISIONS * CONTACTCUBEMAPFACENB_SUBDIVISIONS;
|
|
return faceNo * 200 + indexU * nbSubDivInFace + indexV;
|
|
}
|
|
|
|
void ContactManifoldSet::update() {
|
|
for (int i=this.nbManifolds-1; i>=0; i--) {
|
|
// Update the contact manifold
|
|
this.manifolds[i].update(this.shape1.getBody().getTransform() * this.shape1.getLocalToBodyTransform(),
|
|
this.shape2.getBody().getTransform() * this.shape2.getLocalToBodyTransform());
|
|
// Remove the contact manifold if has no contact points anymore
|
|
if (this.manifolds[i].getNbContactPoints() == 0) {
|
|
removeManifold(i);
|
|
}
|
|
}
|
|
}
|
|
|
|
void ContactManifoldSet::clear() {
|
|
for (int i=this.nbManifolds-1; i>=0; i--) {
|
|
removeManifold(i);
|
|
}
|
|
assert(this.nbManifolds == 0);
|
|
}
|
|
|
|
void ContactManifoldSet::createManifold(int16t normalDirectionId) {
|
|
assert(this.nbManifolds < this.nbMaxManifolds);
|
|
this.manifolds[this.nbManifolds] = ETKNEW(ContactManifold, this.shape1, this.shape2, normalDirectionId);
|
|
this.nbManifolds++;
|
|
}
|
|
|
|
void ContactManifoldSet::removeManifold(int index) {
|
|
assert(this.nbManifolds > 0);
|
|
assert(index >= 0 && index < this.nbManifolds);
|
|
// Delete the new contact
|
|
ETKDELETE(ContactManifold, this.manifolds[index]);
|
|
this.manifolds[index] = null;
|
|
for (int i=index; (i+1) < this.nbManifolds; i++) {
|
|
this.manifolds[i] = this.manifolds[i+1];
|
|
}
|
|
this.nbManifolds--;
|
|
}
|
|
|
|
ProxyShape* ContactManifoldSet::getShape1() {
|
|
return this.shape1;
|
|
}
|
|
|
|
ProxyShape* ContactManifoldSet::getShape2() {
|
|
return this.shape2;
|
|
}
|
|
|
|
int ContactManifoldSet::getNbContactManifolds() {
|
|
return this.nbManifolds;
|
|
}
|
|
|
|
ContactManifold* ContactManifoldSet::getContactManifold(int index) {
|
|
assert(index >= 0 && index < this.nbManifolds);
|
|
return this.manifolds[index];
|
|
}
|
|
|
|
int ContactManifoldSet::getTotalNbContactPoints() {
|
|
int nbPoints = 0;
|
|
for (int i=0; i<this.nbManifolds; i++) {
|
|
nbPoints += this.manifolds[i].getNbContactPoints();
|
|
}
|
|
return nbPoints;
|
|
}
|