jege_proto/src/org/atriaSoft/ephysics/collision/ContactManifoldSet.cpp

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;
}