mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Continue working on triangle mesh collision detection

Browse Source
This commit is contained in:
Daniel Chappuis 2015-11-13 21:23:34 +01:00
parent 9fd98f8efc
commit 568f03461f
9 changed files with 309 additions and 144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -143,6 +143,8 @@ SET (REACTPHYSICS3D_SOURCES
"src/engine/OverlappingPair.cpp"
"src/engine/Profiler.h"
"src/engine/Profiler.cpp"
"src/engine/Timer.h"
"src/engine/Timer.cpp"
"src/mathematics/mathematics.h"
"src/mathematics/mathematics_functions.h"
"src/mathematics/Matrix2x2.h"

29
src/collision/broadphase/BroadPhaseAlgorithm.cpp
View File

@ -118,7 +118,7 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) {
// Add the collision shape into the dynamic AABB tree and get its broad-phase ID
int nodeId = mDynamicAABBTree.addObject(proxyShape, aabb);
int nodeId = mDynamicAABBTree.addObject(aabb, proxyShape);
// Set the broad-phase ID of the proxy shape
proxyShape->mBroadPhaseID = nodeId;
@ -175,13 +175,15 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
if (shapeID == -1) continue;
// Get the fat AABB of the collision shape from the dynamic AABB tree
AABBOverlapCallback callback(*this, shapeID);
// Get the AABB of the shape
const AABB& shapeAABB = mDynamicAABBTree.getFatAABB(shapeID);
// Ask the dynamic AABB tree to report all collision shapes that overlap with
// this AABB. The method BroadPhase::notifiyOverlappingPair() will be called
// by the dynamic AABB tree for each potential overlapping pair.
mDynamicAABBTree.reportAllShapesOverlappingWith(shapeID, shapeAABB, *this);
mDynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, callback);
}
// Reset the array of collision shapes that have move (or have been created) during the
@ -200,9 +202,11 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
BroadPhasePair* pair = mPotentialPairs + i;
i++;
assert(pair->collisionShape1ID != pair->collisionShape2ID);
// Get the two collision shapes of the pair
ProxyShape* shape1 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeData(pair->collisionShape1ID));
ProxyShape* shape2 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeData(pair->collisionShape2ID));
ProxyShape* shape1 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(pair->collisionShape1ID));
ProxyShape* shape2 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(pair->collisionShape2ID));
// Notify the collision detection about the overlapping pair
mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
@ -237,7 +241,7 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
}
// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
void BroadPhaseAlgorithm::notifyOverlappingAABBs(int node1ID, int node2ID) {
void BroadPhaseAlgorithm::notifyOverlappingNodes(int node1ID, int node2ID) {
// If both the nodes are the same, we do not create store the overlapping pair
if (node1ID == node2ID) return;
@ -261,14 +265,14 @@ void BroadPhaseAlgorithm::notifyOverlappingAABBs(int node1ID, int node2ID) {
}
// Called for a broad-phase shape that has to be tested for raycast
decimal BroadPhaseAlgorithm::raycastBroadPhaseShape(void* nodeData, RaycastTest& raycastTest,
decimal BroadPhaseAlgorithm::raycastBroadPhaseShape(int32 nodeId, RaycastTest& raycastTest,
const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const {
decimal hitFraction = decimal(-1.0);
// Get the proxy shape from the node
ProxyShape* proxyShape = static_cast<ProxyShape*>(nodeData);
ProxyShape* proxyShape = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(nodeId));
// Check if the raycast filtering mask allows raycast against this shape
if ((raycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) {
@ -280,4 +284,11 @@ decimal BroadPhaseAlgorithm::raycastBroadPhaseShape(void* nodeData, RaycastTest&
}
return hitFraction;
}
}
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
void AABBOverlapCallback::notifyOverlappingNode(int nodeId) {
mBroadPhaseAlgorithm.notifyOverlappingNodes(mReferenceNodeId, nodeId);
}

36
src/collision/broadphase/BroadPhaseAlgorithm.h
View File

@ -37,6 +37,7 @@ namespace reactphysics3d {
// Declarations
class CollisionDetection;
class BroadPhaseAlgorithm;
// Structure BroadPhasePair
/**
@ -59,17 +60,26 @@ struct BroadPhasePair {
static bool smallerThan(const BroadPhasePair& pair1, const BroadPhasePair& pair2);
};
// Class BroadPhaseOverlapCallback
/**
* Overlapping callback method that has to be used as parameter of the
* reportAllShapesOverlappingWith() method.
*/
class BroadPhaseOverlapCallback {
// class AABBOverlapCallback
class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
public :
private:
// Called when two overlapping AABBs have been found
virtual void notifyOverlappingAABBs(int broadPhaseId1, int broadPhaseId2)=0;
BroadPhaseAlgorithm& mBroadPhaseAlgorithm;
int mReferenceNodeId;
public:
// Constructor
AABBOverlapCallback(BroadPhaseAlgorithm& broadPhaseAlgo, int referenceNodeId)
: mBroadPhaseAlgorithm(broadPhaseAlgo), mReferenceNodeId(referenceNodeId) {
}
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int nodeId);
};
@ -82,7 +92,7 @@ class BroadPhaseRaycastTestCallback {
public:
// Called for a broad-phase shape that has to be tested for raycast
virtual decimal raycastBroadPhaseShape(void* nodeData, RaycastTest& raycastTest,
virtual decimal raycastBroadPhaseShape(int32 nodeId, RaycastTest& raycastTest,
const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const=0;
@ -96,7 +106,7 @@ class BroadPhaseRaycastTestCallback {
* later for collision during the narrow-phase collision detection. A dynamic AABB
* tree data structure is used for fast broad-phase collision detection.
*/
class BroadPhaseAlgorithm : BroadPhaseRaycastTestCallback, BroadPhaseOverlapCallback {
class BroadPhaseAlgorithm : BroadPhaseRaycastTestCallback {
protected :
@ -171,10 +181,10 @@ class BroadPhaseAlgorithm : BroadPhaseRaycastTestCallback, BroadPhaseOverlapCall
void removeMovedCollisionShape(int broadPhaseID);
/// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
void notifyOverlappingAABBs(int broadPhaseId1, int broadPhaseId2);
void notifyOverlappingNodes(int broadPhaseId1, int broadPhaseId2);
/// Called for a broad-phase shape that has to be tested for raycast
virtual decimal raycastBroadPhaseShape(void* nodeData, RaycastTest& raycastTest,
virtual decimal raycastBroadPhaseShape(int32 nodeId, RaycastTest& raycastTest,
const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const;

43
src/collision/broadphase/DynamicAABBTree.cpp
View File

@ -93,9 +93,6 @@ int DynamicAABBTree::allocateNode() {
int freeNodeID = mFreeNodeID;
mFreeNodeID = mNodes[freeNodeID].nextNodeID;
mNodes[freeNodeID].parentID = TreeNode::NULL_TREE_NODE;
mNodes[freeNodeID].children[0] = TreeNode::NULL_TREE_NODE;
mNodes[freeNodeID].children[1] = TreeNode::NULL_TREE_NODE;
mNodes[freeNodeID].data = NULL;
mNodes[freeNodeID].height = 0;
mNbNodes++;
@ -114,9 +111,8 @@ void DynamicAABBTree::releaseNode(int nodeID) {
mNbNodes--;
}
// Add an object into the tree. This method creates a new leaf node in the tree and
// returns the ID of the corresponding node.
int DynamicAABBTree::addObject(void* nodeData, const AABB& aabb) {
// Internally add an object into the tree
int DynamicAABBTree::addObjectInternal(const AABB& aabb) {
// Get the next available node (or allocate new ones if necessary)
int nodeID = allocateNode();
@ -126,9 +122,6 @@ int DynamicAABBTree::addObject(void* nodeData, const AABB& aabb) {
mNodes[nodeID].aabb.setMin(aabb.getMin() - gap);
mNodes[nodeID].aabb.setMax(aabb.getMax() + gap);
// Set the node data
mNodes[nodeID].data = nodeData;
// Set the height of the node in the tree
mNodes[nodeID].height = 0;
@ -286,13 +279,13 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
int oldParentNode = mNodes[siblingNode].parentID;
int newParentNode = allocateNode();
mNodes[newParentNode].parentID = oldParentNode;
mNodes[newParentNode].data = NULL;
mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB);
mNodes[newParentNode].height = mNodes[siblingNode].height + 1;
assert(mNodes[newParentNode].height > 0);
// If the sibling node was not the root node
if (oldParentNode != TreeNode::NULL_TREE_NODE) {
assert(!mNodes[oldParentNode].isLeaf());
if (mNodes[oldParentNode].children[0] == siblingNode) {
mNodes[oldParentNode].children[0] = newParentNode;
}
@ -314,12 +307,14 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
// Move up in the tree to change the AABBs that have changed
currentNodeID = mNodes[nodeID].parentID;
assert(!mNodes[currentNodeID].isLeaf());
while (currentNodeID != TreeNode::NULL_TREE_NODE) {
// Balance the sub-tree of the current node if it is not balanced
currentNodeID = balanceSubTreeAtNode(currentNodeID);
assert(mNodes[nodeID].isLeaf());
assert(!mNodes[currentNodeID].isLeaf());
int leftChild = mNodes[currentNodeID].children[0];
int rightChild = mNodes[currentNodeID].children[1];
assert(leftChild != TreeNode::NULL_TREE_NODE);
@ -383,6 +378,8 @@ void DynamicAABBTree::removeLeafNode(int nodeID) {
// Balance the current sub-tree if necessary
currentNodeID = balanceSubTreeAtNode(currentNodeID);
assert(!mNodes[currentNodeID].isLeaf());
// Get the two children of the current node
int leftChildID = mNodes[currentNodeID].children[0];
int rightChildID = mNodes[currentNodeID].children[1];
@ -436,6 +433,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
// If the right node C is 2 higher than left node B
if (balanceFactor > 1) {
assert(!nodeC->isLeaf());
int nodeFID = nodeC->children[0];
int nodeGID = nodeC->children[1];
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
@ -461,8 +460,12 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
mRootNodeID = nodeCID;
}
assert(!nodeC->isLeaf());
assert(!nodeA->isLeaf());
// If the right node C was higher than left node B because of the F node
if (nodeF->height > nodeG->height) {
nodeC->children[1] = nodeFID;
nodeA->children[1] = nodeGID;
nodeG->parentID = nodeID;
@ -500,6 +503,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
// If the left node B is 2 higher than right node C
if (balanceFactor < -1) {
assert(!nodeB->isLeaf());
int nodeFID = nodeB->children[0];
int nodeGID = nodeB->children[1];
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
@ -525,8 +530,12 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
mRootNodeID = nodeBID;
}
assert(!nodeB->isLeaf());
assert(!nodeA->isLeaf());
// If the left node B was higher than right node C because of the F node
if (nodeF->height > nodeG->height) {
nodeB->children[1] = nodeFID;
nodeA->children[0] = nodeGID;
nodeG->parentID = nodeID;
@ -565,12 +574,9 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
return nodeID;
}
// Report all shapes overlapping with the AABB given in parameter.
/// For each overlapping shape with the AABB given in parameter, the
/// BroadPhase::notifyOverlappingPair() method is called to store a
/// potential overlapping pair.
void DynamicAABBTree::reportAllShapesOverlappingWith(int nodeID, const AABB& aabb,
BroadPhaseOverlapCallback& callback) {
/// Report all shapes overlapping with the AABB given in parameter.
void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb,
DynamicAABBTreeOverlapCallback& callback) const {
// Create a stack with the nodes to visit
Stack<int, 64> stack;
@ -595,7 +601,7 @@ void DynamicAABBTree::reportAllShapesOverlappingWith(int nodeID, const AABB& aab
if (nodeToVisit->isLeaf()) {
// Notify the broad-phase about a new potential overlapping pair
callback.notifyOverlappingAABBs(nodeID, nodeIDToVisit);
callback.notifyOverlappingNode(nodeIDToVisit);
}
else { // If the node is not a leaf
@ -645,7 +651,7 @@ void DynamicAABBTree::raycast(const Ray& ray, RaycastTest& raycastTest,
Ray rayTemp(ray.point1, ray.point2, maxFraction);
// Call the callback that will raycast again the broad-phase shape
decimal hitFraction = callback.raycastBroadPhaseShape(node->data, raycastTest, rayTemp,
decimal hitFraction = callback.raycastBroadPhaseShape(nodeID, raycastTest, rayTemp,
raycastWithCategoryMaskBits);
// If the user returned a hitFraction of zero, it means that
@ -712,6 +718,7 @@ void DynamicAABBTree::checkNode(int nodeID) const {
// Get the children nodes
TreeNode* pNode = mNodes + nodeID;
assert(!pNode->isLeaf());
int leftChild = pNode->children[0];
int rightChild = pNode->children[1];

88
src/collision/broadphase/DynamicAABBTree.h
View File

@ -37,7 +37,7 @@ namespace reactphysics3d {
// Declarations
class BroadPhaseAlgorithm;
class BroadPhaseRaycastTestCallback;
class BroadPhaseOverlapCallback;
class DynamicAABBTreeOverlapCallback;
struct RaycastTest;
// Structure TreeNode
@ -70,8 +70,11 @@ struct TreeNode {
/// Left and right child of the node (children[0] = left child)
int32 children[2];
/// Pointer to the data stored at that node (in case the node is a leaf)
void* data;
/// Two pieces of data stored at that node (in case the node is a leaf)
union {
int32 dataInt[2];
void* dataPointer;
};
};
/// Height of the node in the tree
@ -86,6 +89,20 @@ struct TreeNode {
bool isLeaf() const;
};
// Class DynamicAABBTreeOverlapCallback
/**
* Overlapping callback method that has to be used as parameter of the
* reportAllShapesOverlappingWithNode() method.
*/
class DynamicAABBTreeOverlapCallback {
public :
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int nodeId)=0;
};
// Class DynamicAABBTree
/**
* This class implements a dynamic AABB tree that is used for broad-phase
@ -139,6 +156,9 @@ class DynamicAABBTree {
/// Compute the height of a given node in the tree
int computeHeight(int nodeID);
/// Internally add an object into the tree
int addObjectInternal(const AABB& aabb);
#ifndef NDEBUG
/// Check if the tree structure is valid (for debugging purpose)
@ -159,8 +179,11 @@ class DynamicAABBTree {
/// Destructor
~DynamicAABBTree();
/// Add an object into the tree
int addObject(void* nodeData, const AABB& aabb);
/// Add an object into the tree (where node data are two integers)
int addObject(const AABB& aabb, int32 data1, int32 data2);
/// Add an object into the tree (where node data is a pointer)
int addObject(const AABB& aabb, void* data);
/// Remove an object from the tree
void removeObject(int nodeID);
@ -171,12 +194,15 @@ class DynamicAABBTree {
/// Return the fat AABB corresponding to a given node ID
const AABB& getFatAABB(int nodeID) const;
/// Return the pointer to the data of a given leaf node of the tree
void* getNodeData(int nodeID) const;
/// Return the pointer to the data array of a given leaf node of the tree
int32* getNodeDataInt(int nodeID) const;
/// Return the data pointer of a given leaf node of the tree
void* getNodeDataPointer(int nodeID) const;
/// Report all shapes overlapping with the AABB given in parameter.
void reportAllShapesOverlappingWith(int nodeID, const AABB& aabb,
BroadPhaseOverlapCallback& callback);
void reportAllShapesOverlappingWithAABB(const AABB& aabb,
DynamicAABBTreeOverlapCallback& callback) const;
/// Ray casting method
void raycast(const Ray& ray, RaycastTest& raycastTest,
@ -185,6 +211,9 @@ class DynamicAABBTree {
/// Compute the height of the tree
int computeHeight();
/// Return the root AABB of the tree
AABB getRootAABB() const;
};
// Return true if the node is a leaf of the tree
@ -198,11 +227,46 @@ inline const AABB& DynamicAABBTree::getFatAABB(int nodeID) const {
return mNodes[nodeID].aabb;
}
// Return the pointer to the data of a given leaf node of the tree
inline void* DynamicAABBTree::getNodeData(int nodeID) const {
// Return the pointer to the data array of a given leaf node of the tree
inline int32* DynamicAABBTree::getNodeDataInt(int nodeID) const {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf());
return mNodes[nodeID].data;
return mNodes[nodeID].dataInt;
}
// Return the pointer to the data pointer of a given leaf node of the tree
inline void* DynamicAABBTree::getNodeDataPointer(int nodeID) const {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf());
return mNodes[nodeID].dataPointer;
}
// Return the root AABB of the tree
inline AABB DynamicAABBTree::getRootAABB() const {
return getFatAABB(mRootNodeID);
}
// Add an object into the tree. This method creates a new leaf node in the tree and
// returns the ID of the corresponding node.
inline int DynamicAABBTree::addObject(const AABB& aabb, int32 data1, int32 data2) {
int nodeId = addObjectInternal(aabb);
mNodes[nodeId].dataInt[0] = data1;
mNodes[nodeId].dataInt[1] = data2;
return nodeId;
}
// Add an object into the tree. This method creates a new leaf node in the tree and
// returns the ID of the corresponding node.
inline int DynamicAABBTree::addObject(const AABB& aabb, void* data) {
int nodeId = addObjectInternal(aabb);
mNodes[nodeId].dataPointer = data;
return nodeId;
}
}

24
src/collision/shapes/AABB.cpp
View File

@ -89,3 +89,27 @@ bool AABB::contains(const AABB& aabb) {
return isInside;
}
// Create and return an AABB for a triangle
AABB AABB::createAABBForTriangle(const Vector3* trianglePoints) {
Vector3 minCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z);
Vector3 maxCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z);
if (trianglePoints[1].x < minCoords.x) minCoords.x = trianglePoints[1].x;
if (trianglePoints[1].y < minCoords.y) minCoords.y = trianglePoints[1].y;
if (trianglePoints[1].z < minCoords.z) minCoords.z = trianglePoints[1].z;
if (trianglePoints[2].x < minCoords.x) minCoords.x = trianglePoints[2].x;
if (trianglePoints[2].y < minCoords.y) minCoords.y = trianglePoints[2].y;
if (trianglePoints[2].z < minCoords.z) minCoords.z = trianglePoints[2].z;
if (trianglePoints[1].x > maxCoords.x) maxCoords.x = trianglePoints[1].x;
if (trianglePoints[1].y > maxCoords.y) maxCoords.y = trianglePoints[1].y;
if (trianglePoints[1].z > maxCoords.z) maxCoords.z = trianglePoints[1].z;
if (trianglePoints[2].x > maxCoords.x) maxCoords.x = trianglePoints[2].x;
if (trianglePoints[2].y > maxCoords.y) maxCoords.y = trianglePoints[2].y;
if (trianglePoints[2].z > maxCoords.z) maxCoords.z = trianglePoints[2].z;
return AABB(minCoords, maxCoords);
}

3
src/collision/shapes/AABB.h
View File

@ -100,6 +100,9 @@ class AABB {
/// Return true if the AABB of a triangle intersects the AABB
bool testCollisionTriangleAABB(const Vector3* trianglePoints) const;
/// Create and return an AABB for a triangle
static AABB createAABBForTriangle(const Vector3* trianglePoints);
/// Assignment operator
AABB& operator=(const AABB& aabb);

154
src/collision/shapes/ConcaveMeshShape.cpp
View File

@ -25,6 +25,7 @@
// Libraries
#include "ConcaveMeshShape.h"
#include <iostream>
using namespace reactphysics3d;
@ -32,7 +33,8 @@ using namespace reactphysics3d;
ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh) : ConcaveShape(CONCAVE_MESH) {
mTriangleMesh = triangleMesh;
recalculateBounds();
// Insert all the triangles into the dynamic AABB tree
initBVHTree();
}
// Destructor
@ -40,14 +42,16 @@ ConcaveMeshShape::~ConcaveMeshShape() {
}
// Use a callback method on all triangles of the concave shape inside a given AABB
void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const {
// Insert all the triangles into the dynamic AABB tree
void ConcaveMeshShape::initBVHTree() {
// TODO : Try to randomly add the triangles into the tree to obtain a better tree
// For each sub-part of the mesh
for (int i=0; i<mTriangleMesh->getNbSubparts(); i++) {
for (int subPart=0; subPart<mTriangleMesh->getNbSubparts(); subPart++) {
// Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(i);
TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart);
TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
@ -57,8 +61,9 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB&
int indexStride = triangleVertexArray->getIndicesStride();
// For each triangle of the concave mesh
for (int j=0; j<triangleVertexArray->getNbTriangles(); j++) {
for (int triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) {
void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
Vector3 trianglePoints[3];
// For each vertex of the triangle
@ -67,10 +72,10 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB&
// Get the index of the current vertex in the triangle
int vertexIndex;
if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
vertexIndex = ((unsigned int*)(indicesStart + j * 3 * indexStride))[k];
vertexIndex = ((uint*)vertexIndexPointer)[k];
}
else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
vertexIndex = ((unsigned short*)(indicesStart + j * 3 * indexStride))[k];
vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
}
// Get the vertices components of the triangle
@ -88,16 +93,70 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB&
}
}
// If the triangle AABB intersects with the convex shape AABB
if (localAABB.testCollisionTriangleAABB(trianglePoints)) {
// Create the AABB for the triangle
AABB aabb = AABB::createAABBForTriangle(trianglePoints);
// Call the callback to report this triangle
callback.testTriangle(trianglePoints);
}
// Add the AABB with the index of the triangle into the dynamic AABB tree
mDynamicAABBTree.addObject(aabb, subPart, triangleIndex);
}
}
}
// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
// given the start vertex index pointer of the triangle
void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex,
Vector3* outTriangleVertices) const {
// Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart);
TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
int vertexStride = triangleVertexArray->getVerticesStride();
int indexStride = triangleVertexArray->getIndicesStride();
void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
// For each vertex of the triangle
for (int k=0; k < 3; k++) {
// Get the index of the current vertex in the triangle
int vertexIndex;
if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
vertexIndex = ((uint*)vertexIndexPointer)[k];
}
else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
}
// Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
outTriangleVertices[k][0] = decimal(vertices[0]);
outTriangleVertices[k][1] = decimal(vertices[1]);
outTriangleVertices[k][2] = decimal(vertices[2]);
}
else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
outTriangleVertices[k][0] = decimal(vertices[0]);
outTriangleVertices[k][1] = decimal(vertices[1]);
outTriangleVertices[k][2] = decimal(vertices[2]);
}
}
}
// Use a callback method on all triangles of the concave shape inside a given AABB
void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const {
ConvexTriangleAABBOverlapCallback overlapCallback(callback, *this, mDynamicAABBTree);
// Ask the Dynamic AABB Tree to report all the triangles that are overlapping
// with the AABB of the convex shape.
mDynamicAABBTree.reportAllShapesOverlappingWithAABB(localAABB, overlapCallback);
}
// Raycast method with feedback information
bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
@ -106,72 +165,3 @@ bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh
return false;
}
// Recompute the bounds of the mesh
void ConcaveMeshShape::recalculateBounds() {
bool isFirstVertex = true;
// For each sub-part of the mesh
for (int i=0; i<mTriangleMesh->getNbSubparts(); i++) {
// Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(i);
TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
int vertexStride = triangleVertexArray->getVerticesStride();
int indexStride = triangleVertexArray->getIndicesStride();
// For each triangle of the concave mesh
for (int j=0; j<triangleVertexArray->getNbTriangles(); j++) {
// For each vertex of the triangle
for (int k=0; k < 3; k++) {
// Get the index of the current vertex in the triangle
int vertexIndex;
if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
vertexIndex = ((unsigned int*)(indicesStart + j * 3 * indexStride))[k];
}
else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
vertexIndex = ((unsigned short*)(indicesStart + j * 3 * indexStride))[k];
}
Vector3 vertex;
// Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
vertex[0] = decimal(vertices[0]);
vertex[1] = decimal(vertices[1]);
vertex[2] = decimal(vertices[2]);
}
else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
vertex[0] = decimal(vertices[0]);
vertex[1] = decimal(vertices[1]);
vertex[2] = decimal(vertices[2]);
}
if (isFirstVertex) {
mMinBounds.setAllValues(vertex.x, vertex.y, vertex.z);
mMaxBounds.setAllValues(vertex.x, vertex.y, vertex.z);
isFirstVertex = false;
}
else {
if (vertex.x > mMaxBounds.x) mMaxBounds.x = vertex.x;
if (vertex.x < mMinBounds.x) mMinBounds.x = vertex.x;
if (vertex.y > mMaxBounds.y) mMaxBounds.y = vertex.y;
if (vertex.y < mMinBounds.y) mMinBounds.y = vertex.y;
if (vertex.z > mMaxBounds.z) mMaxBounds.z = vertex.z;
if (vertex.z < mMinBounds.z) mMinBounds.z = vertex.z;
}
}
}
}
}

74
src/collision/shapes/ConcaveMeshShape.h
View File

@ -28,10 +28,41 @@
// Libraries
#include "ConcaveShape.h"
#include "collision/broadphase/DynamicAABBTree.h"
#include "collision/TriangleMesh.h"
namespace reactphysics3d {
class ConcaveMeshShape;
// class ConvexTriangleAABBOverlapCallback
class ConvexTriangleAABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
private:
TriangleCallback& mTriangleTestCallback;
// Reference to the concave mesh shape
const ConcaveMeshShape& mConcaveMeshShape;
// Reference to the Dynamic AABB tree
const DynamicAABBTree& mDynamicAABBTree;
public:
// Constructor
ConvexTriangleAABBOverlapCallback(TriangleCallback& triangleCallback, const ConcaveMeshShape& concaveShape,
const DynamicAABBTree& dynamicAABBTree)
: mTriangleTestCallback(triangleCallback), mConcaveMeshShape(concaveShape), mDynamicAABBTree(dynamicAABBTree) {
}
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int nodeId);
};
// TODO : Implement raycasting with this collision shape
// TODO : Make possible for the user to have a scaling factor on the mesh
@ -51,11 +82,8 @@ class ConcaveMeshShape : public ConcaveShape {
/// Triangle mesh
TriangleMesh* mTriangleMesh;
/// Mesh minimum bounds in the three local x, y and z directions
Vector3 mMinBounds;
/// Mesh maximum bounds in the three local x, y and z directions
Vector3 mMaxBounds;
/// Dynamic AABB tree to accelerate collision with the triangles
DynamicAABBTree mDynamicAABBTree;
// -------------------- Methods -------------------- //
@ -82,9 +110,13 @@ class ConcaveMeshShape : public ConcaveShape {
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
/// Recompute the bounds of the mesh
// TODO : Check if we need this when AABB tree is used
void recalculateBounds();
/// Insert all the triangles into the dynamic AABB tree
void initBVHTree();
/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
/// given the start vertex index pointer of the triangle.
void getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex,
Vector3* outTriangleVertices) const;
public:
@ -102,6 +134,10 @@ class ConcaveMeshShape : public ConcaveShape {
/// Use a callback method on all triangles of the concave shape inside a given AABB
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const;
// ---------- Friendship ----------- //
friend class ConvexTriangleAABBOverlapCallback;
};
// Return the number of bytes used by the collision shape
@ -132,8 +168,11 @@ inline Vector3 ConcaveMeshShape::getLocalSupportPointWithoutMargin(const Vector3
*/
inline void ConcaveMeshShape::getLocalBounds(Vector3& min, Vector3& max) const {
min = mMinBounds;
max = mMaxBounds;
// Get the AABB of the whole tree
AABB treeAABB = mDynamicAABBTree.getRootAABB();
min = treeAABB.getMin();
max = treeAABB.getMax();
}
// Return the local inertia tensor of the sphere
@ -160,6 +199,21 @@ inline bool ConcaveMeshShape::testPointInside(const Vector3& localPoint, ProxySh
return false;
}
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId) {
// Get the node data (triangle index and mesh subpart index)
int32* data = mDynamicAABBTree.getNodeDataInt(nodeId);
// Get the triangle vertices for this node from the concave mesh shape
Vector3 trianglePoints[3];
mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
// Call the callback to test narrow-phase collision with this triangle
mTriangleTestCallback.testTriangle(trianglePoints);
}
}
#endif