atria-soft/ephysics
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

[DEV] clean some unuse of container

Browse Source
This commit is contained in:
Edouard DUPIN 2017-10-01 22:35:26 +02:00
parent cccb93fd34
commit 8d77c326de
7 changed files with 86 additions and 219 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp
View File

@ -11,45 +11,31 @@
using namespace ephysics; using namespace ephysics;
BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection) BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& _collisionDetection):
:m_dynamicAABBTree(DYNAMIC_TREE_AABB_GAP), m_numberMovedShapes(0), m_numberAllocatedMovedShapes(8), m_dynamicAABBTree(DYNAMIC_TREE_AABB_GAP),
m_numberNonUsedMovedShapes(0), m_collisionDetection(_collisionDetection) {
m_collisionDetection(collisionDetection) { m_movedShapes.reserve(8);
// Allocate memory for the array of non-static proxy shapes IDs
m_movedShapes = (int32_t*) malloc(m_numberAllocatedMovedShapes * sizeof(int32_t));
assert(m_movedShapes != NULL);
m_potentialPairs.reserve(8); m_potentialPairs.reserve(8);
} }
BroadPhaseAlgorithm::~BroadPhaseAlgorithm() { BroadPhaseAlgorithm::~BroadPhaseAlgorithm() {
// Release the memory for the array of non-static proxy shapes IDs
free(m_movedShapes);
} }
void BroadPhaseAlgorithm::addMovedCollisionShape(int32_t broadPhaseID) { void BroadPhaseAlgorithm::addMovedCollisionShape(int32_t _broadPhaseID) {
m_movedShapes.pushBack(_broadPhaseID);
}
// Allocate more elements in the array of shapes that have moved if necessary void BroadPhaseAlgorithm::removeMovedCollisionShape(int32_t _broadPhaseID) {
if (m_numberAllocatedMovedShapes == m_numberMovedShapes) { auto it = m_movedShapes.begin();
m_numberAllocatedMovedShapes *= 2; while (it != m_movedShapes.end()) {
int32_t* oldArray = m_movedShapes; if (*it == _broadPhaseID) {
m_movedShapes = (int32_t*) malloc(m_numberAllocatedMovedShapes * sizeof(int32_t)); it = m_movedShapes.erase(it);
assert(m_movedShapes != NULL); } else {
memcpy(m_movedShapes, oldArray, m_numberMovedShapes * sizeof(int32_t)); ++it;
free(oldArray);
} }
}
// Store the broad-phase ID int32_to the array of shapes that have moved /*
assert(m_numberMovedShapes < m_numberAllocatedMovedShapes);
assert(m_movedShapes != NULL);
m_movedShapes[m_numberMovedShapes] = broadPhaseID;
m_numberMovedShapes++;
}
void BroadPhaseAlgorithm::removeMovedCollisionShape(int32_t broadPhaseID) {
assert(m_numberNonUsedMovedShapes <= m_numberMovedShapes); assert(m_numberNonUsedMovedShapes <= m_numberMovedShapes);
// If less than the quarter of allocated elements of the non-static shapes IDs array // If less than the quarter of allocated elements of the non-static shapes IDs array
@ -81,6 +67,7 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int32_t broadPhaseID) {
break; break;
} }
} }
*/
} }
void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) { void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) {
@ -129,28 +116,27 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
m_potentialPairs.clear(); m_potentialPairs.clear();
// For all collision shapes that have moved (or have been created) during the // For all collision shapes that have moved (or have been created) during the
// last simulation step // last simulation step
for (uint32_t i=0; i<m_numberMovedShapes; i++) { for (auto &it: m_movedShapes) {
int32_t shapeID = m_movedShapes[i]; if (it == -1) {
if (shapeID == -1) { // impossible case ...
continue; continue;
} }
// Get the AABB of the shape // Get the AABB of the shape
const AABB& shapeAABB = m_dynamicAABBTree.getFatAABB(shapeID); const AABB& shapeAABB = m_dynamicAABBTree.getFatAABB(it);
// Ask the dynamic AABB tree to report all collision shapes that overlap with // Ask the dynamic AABB tree to report all collision shapes that overlap with
// this AABB. The method BroadPhase::notifiyOverlappingPair() will be called // this AABB. The method BroadPhase::notifiyOverlappingPair() will be called
// by the dynamic AABB tree for each potential overlapping pair. // by the dynamic AABB tree for each potential overlapping pair.
m_dynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, [&](int32_t nodeId) mutable { m_dynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, [&](int32_t nodeId) mutable {
// If both the nodes are the same, we do not create store the overlapping pair // If both the nodes are the same, we do not create store the overlapping pair
if (shapeID == nodeId) { if (it == nodeId) {
return; return;
} }
// Add the new potential pair int32_to the array of potential overlapping pairs // Add the new potential pair int32_to the array of potential overlapping pairs
m_potentialPairs.pushBack(etk::makePair(etk::min(shapeID, nodeId), etk::max(shapeID, nodeId) )); m_potentialPairs.pushBack(etk::makePair(etk::min(it, nodeId), etk::max(it, nodeId) ));
}); });
} }
// Reset the array of collision shapes that have move (or have been created) during the // Reset the array of collision shapes that have move (or have been created) during the last simulation step
// last simulation step m_movedShapes.clear();
m_numberMovedShapes = 0;
// Sort the array of potential overlapping pairs in order to remove duplicate pairs // Sort the array of potential overlapping pairs in order to remove duplicate pairs
m_potentialPairs.sort(0, m_potentialPairs.sort(0,

9
ephysics/collision/broadphase/BroadPhaseAlgorithm.hpp
View File

@ -50,16 +50,13 @@ namespace ephysics {
class BroadPhaseAlgorithm { class BroadPhaseAlgorithm {
protected : protected :
DynamicAABBTree m_dynamicAABBTree; //!< Dynamic AABB tree DynamicAABBTree m_dynamicAABBTree; //!< Dynamic AABB tree
int32_t* m_movedShapes; //!< Array with the broad-phase IDs of all collision shapes that have moved (or have been created) during the last simulation step. Those are the shapes that need to be tested for overlapping in the next simulation step. etk::Vector<int32_t> m_movedShapes; //!< Array with the broad-phase IDs of all collision shapes that have moved (or have been created) during the last simulation step. Those are the shapes that need to be tested for overlapping in the next simulation step.
uint32_t m_numberMovedShapes; //!< Number of collision shapes in the array of shapes that have moved during the last simulation step.
uint32_t m_numberAllocatedMovedShapes; //!< Number of allocated elements for the array of shapes that have moved during the last simulation step.
uint32_t m_numberNonUsedMovedShapes; //!< Number of non-used elements in the array of shapes that have moved during the last simulation step.
etk::Vector<etk::Pair<int32_t,int32_t>> m_potentialPairs; //!< Temporary array of potential overlapping pairs (with potential duplicates) etk::Vector<etk::Pair<int32_t,int32_t>> m_potentialPairs; //!< Temporary array of potential overlapping pairs (with potential duplicates)
CollisionDetection& m_collisionDetection; //!< Reference to the collision detection object CollisionDetection& m_collisionDetection; //!< Reference to the collision detection object
/// Private copy-constructor /// Private copy-constructor
BroadPhaseAlgorithm(const BroadPhaseAlgorithm& algorithm); BroadPhaseAlgorithm(const BroadPhaseAlgorithm& _obj);
/// Private assignment operator /// Private assignment operator
BroadPhaseAlgorithm& operator=(const BroadPhaseAlgorithm& algorithm); BroadPhaseAlgorithm& operator=(const BroadPhaseAlgorithm& _obj);
public : public :
/// Constructor /// Constructor
BroadPhaseAlgorithm(CollisionDetection& _collisionDetection); BroadPhaseAlgorithm(CollisionDetection& _collisionDetection);

180
ephysics/collision/broadphase/DynamicAABBTree.cpp
View File

@ -14,28 +14,24 @@ using namespace ephysics;
const int32_t TreeNode::NULL_TREE_NODE = -1; const int32_t TreeNode::NULL_TREE_NODE = -1;
DynamicAABBTree::DynamicAABBTree(float extraAABBGap) : m_extraAABBGap(extraAABBGap) { DynamicAABBTree::DynamicAABBTree(float _extraAABBGap):
m_extraAABBGap(_extraAABBGap) {
init(); init();
} }
DynamicAABBTree::~DynamicAABBTree() { DynamicAABBTree::~DynamicAABBTree() {
free(m_nodes); free(m_nodes);
} }
// Initialize the tree // Initialize the tree
void DynamicAABBTree::init() { void DynamicAABBTree::init() {
m_rootNodeID = TreeNode::NULL_TREE_NODE; m_rootNodeID = TreeNode::NULL_TREE_NODE;
m_numberNodes = 0; m_numberNodes = 0;
m_numberAllocatedNodes = 8; m_numberAllocatedNodes = 8;
// Allocate memory for the nodes of the tree // Allocate memory for the nodes of the tree
m_nodes = (TreeNode*) malloc(m_numberAllocatedNodes * sizeof(TreeNode)); m_nodes = (TreeNode*) malloc(m_numberAllocatedNodes * sizeof(TreeNode));
assert(m_nodes); assert(m_nodes);
memset(m_nodes, 0, m_numberAllocatedNodes * sizeof(TreeNode)); memset(m_nodes, 0, m_numberAllocatedNodes * sizeof(TreeNode));
// Initialize the allocated nodes // Initialize the allocated nodes
for (int32_t i=0; i<m_numberAllocatedNodes - 1; i++) { for (int32_t i=0; i<m_numberAllocatedNodes - 1; i++) {
m_nodes[i].nextNodeID = i + 1; m_nodes[i].nextNodeID = i + 1;
@ -48,10 +44,8 @@ void DynamicAABBTree::init() {
// Clear all the nodes and reset the tree // Clear all the nodes and reset the tree
void DynamicAABBTree::reset() { void DynamicAABBTree::reset() {
// Free the allocated memory for the nodes // Free the allocated memory for the nodes
free(m_nodes); free(m_nodes);
// Initialize the tree // Initialize the tree
init(); init();
} }
@ -88,7 +82,6 @@ int32_t DynamicAABBTree::allocateNode() {
// Release a node // Release a node
void DynamicAABBTree::releaseNode(int32_t _nodeID) { void DynamicAABBTree::releaseNode(int32_t _nodeID) {
assert(m_numberNodes > 0); assert(m_numberNodes > 0);
assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes); assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes);
assert(m_nodes[_nodeID].height >= 0); assert(m_nodes[_nodeID].height >= 0);
@ -100,34 +93,26 @@ void DynamicAABBTree::releaseNode(int32_t _nodeID) {
// Internally add an object int32_to the tree // Internally add an object int32_to the tree
int32_t DynamicAABBTree::addObjectInternal(const AABB& aabb) { int32_t DynamicAABBTree::addObjectInternal(const AABB& aabb) {
// Get the next available node (or allocate new ones if necessary) // Get the next available node (or allocate new ones if necessary)
int32_t _nodeID = allocateNode(); int32_t _nodeID = allocateNode();
// Create the fat aabb to use in the tree // Create the fat aabb to use in the tree
const vec3 gap(m_extraAABBGap, m_extraAABBGap, m_extraAABBGap); const vec3 gap(m_extraAABBGap, m_extraAABBGap, m_extraAABBGap);
m_nodes[_nodeID].aabb.setMin(aabb.getMin() - gap); m_nodes[_nodeID].aabb.setMin(aabb.getMin() - gap);
m_nodes[_nodeID].aabb.setMax(aabb.getMax() + gap); m_nodes[_nodeID].aabb.setMax(aabb.getMax() + gap);
// Set the height of the node in the tree // Set the height of the node in the tree
m_nodes[_nodeID].height = 0; m_nodes[_nodeID].height = 0;
// Insert the new leaf node in the tree // Insert the new leaf node in the tree
insertLeafNode(_nodeID); insertLeafNode(_nodeID);
assert(m_nodes[_nodeID].isLeaf()); assert(m_nodes[_nodeID].isLeaf());
assert(_nodeID >= 0); assert(_nodeID >= 0);
// Return the Id of the node // Return the Id of the node
return _nodeID; return _nodeID;
} }
// Remove an object from the tree // Remove an object from the tree
void DynamicAABBTree::removeObject(int32_t _nodeID) { void DynamicAABBTree::removeObject(int32_t _nodeID) {
assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes); assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes);
assert(m_nodes[_nodeID].isLeaf()); assert(m_nodes[_nodeID].isLeaf());
// Remove the node from the tree // Remove the node from the tree
removeLeafNode(_nodeID); removeLeafNode(_nodeID);
releaseNode(_nodeID); releaseNode(_nodeID);
@ -142,13 +127,10 @@ void DynamicAABBTree::removeObject(int32_t _nodeID) {
/// frames. If the "forceReinsert" parameter is true, we force a removal and reinsertion of the node /// frames. If the "forceReinsert" parameter is true, we force a removal and reinsertion of the node
/// (this can be useful if the shape AABB has become much smaller than the previous one for instance). /// (this can be useful if the shape AABB has become much smaller than the previous one for instance).
bool DynamicAABBTree::updateObject(int32_t _nodeID, const AABB& _newAABB, const vec3& _displacement, bool _forceReinsert) { bool DynamicAABBTree::updateObject(int32_t _nodeID, const AABB& _newAABB, const vec3& _displacement, bool _forceReinsert) {
PROFILE("DynamicAABBTree::updateObject()"); PROFILE("DynamicAABBTree::updateObject()");
assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes); assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes);
assert(m_nodes[_nodeID].isLeaf()); assert(m_nodes[_nodeID].isLeaf());
assert(m_nodes[_nodeID].height >= 0); assert(m_nodes[_nodeID].height >= 0);
EPHY_INFO(" compare : " << m_nodes[_nodeID].aabb.m_minCoordinates << " " << m_nodes[_nodeID].aabb.m_maxCoordinates); EPHY_INFO(" compare : " << m_nodes[_nodeID].aabb.m_minCoordinates << " " << m_nodes[_nodeID].aabb.m_maxCoordinates);
EPHY_INFO(" : " << _newAABB.m_minCoordinates << " " << _newAABB.m_maxCoordinates); EPHY_INFO(" : " << _newAABB.m_minCoordinates << " " << _newAABB.m_maxCoordinates);
// If the new AABB is still inside the fat AABB of the node // If the new AABB is still inside the fat AABB of the node
@ -156,16 +138,13 @@ bool DynamicAABBTree::updateObject(int32_t _nodeID, const AABB& _newAABB, const
&& m_nodes[_nodeID].aabb.contains(_newAABB)) { && m_nodes[_nodeID].aabb.contains(_newAABB)) {
return false; return false;
} }
// If the new AABB is outside the fat AABB, we remove the corresponding node // If the new AABB is outside the fat AABB, we remove the corresponding node
removeLeafNode(_nodeID); removeLeafNode(_nodeID);
// Compute the fat AABB by inflating the AABB with a constant gap // Compute the fat AABB by inflating the AABB with a constant gap
m_nodes[_nodeID].aabb = _newAABB; m_nodes[_nodeID].aabb = _newAABB;
const vec3 gap(m_extraAABBGap, m_extraAABBGap, m_extraAABBGap); const vec3 gap(m_extraAABBGap, m_extraAABBGap, m_extraAABBGap);
m_nodes[_nodeID].aabb.m_minCoordinates -= gap; m_nodes[_nodeID].aabb.m_minCoordinates -= gap;
m_nodes[_nodeID].aabb.m_maxCoordinates += gap; m_nodes[_nodeID].aabb.m_maxCoordinates += gap;
// Inflate the fat AABB in direction of the linear motion of the AABB // Inflate the fat AABB in direction of the linear motion of the AABB
if (_displacement.x() < 0.0f) { if (_displacement.x() < 0.0f) {
m_nodes[_nodeID].aabb.m_minCoordinates.setX(m_nodes[_nodeID].aabb.m_minCoordinates.x() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *_displacement.x()); m_nodes[_nodeID].aabb.m_minCoordinates.setX(m_nodes[_nodeID].aabb.m_minCoordinates.x() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *_displacement.x());
@ -188,10 +167,8 @@ bool DynamicAABBTree::updateObject(int32_t _nodeID, const AABB& _newAABB, const
//EPHY_CRITICAL("ERROR"); //EPHY_CRITICAL("ERROR");
} }
assert(m_nodes[_nodeID].aabb.contains(_newAABB)); assert(m_nodes[_nodeID].aabb.contains(_newAABB));
// Reinsert the node int32_to the tree // Reinsert the node int32_to the tree
insertLeafNode(_nodeID); insertLeafNode(_nodeID);
return true; return true;
} }
@ -199,75 +176,61 @@ bool DynamicAABBTree::updateObject(int32_t _nodeID, const AABB& _newAABB, const
// in the dynamic tree is described in the book "Introduction to Game Physics // in the dynamic tree is described in the book "Introduction to Game Physics
// with Box2D" by Ian Parberry. // with Box2D" by Ian Parberry.
void DynamicAABBTree::insertLeafNode(int32_t _nodeID) { void DynamicAABBTree::insertLeafNode(int32_t _nodeID) {
// If the tree is empty // If the tree is empty
if (m_rootNodeID == TreeNode::NULL_TREE_NODE) { if (m_rootNodeID == TreeNode::NULL_TREE_NODE) {
m_rootNodeID = _nodeID; m_rootNodeID = _nodeID;
m_nodes[m_rootNodeID].parentID = TreeNode::NULL_TREE_NODE; m_nodes[m_rootNodeID].parentID = TreeNode::NULL_TREE_NODE;
return; return;
} }
assert(m_rootNodeID != TreeNode::NULL_TREE_NODE); assert(m_rootNodeID != TreeNode::NULL_TREE_NODE);
// Find the best sibling node for the new node // Find the best sibling node for the new node
AABB newNodeAABB = m_nodes[_nodeID].aabb; AABB newNodeAABB = m_nodes[_nodeID].aabb;
int32_t currentNodeID = m_rootNodeID; int32_t currentNodeID = m_rootNodeID;
while (!m_nodes[currentNodeID].isLeaf()) { while (!m_nodes[currentNodeID].isLeaf()) {
int32_t leftChild = m_nodes[currentNodeID].children[0]; int32_t leftChild = m_nodes[currentNodeID].children[0];
int32_t rightChild = m_nodes[currentNodeID].children[1]; int32_t rightChild = m_nodes[currentNodeID].children[1];
// Compute the merged AABB // Compute the merged AABB
float volumeAABB = m_nodes[currentNodeID].aabb.getVolume(); float volumeAABB = m_nodes[currentNodeID].aabb.getVolume();
AABB mergedAABBs; AABB mergedAABBs;
mergedAABBs.mergeTwoAABBs(m_nodes[currentNodeID].aabb, newNodeAABB); mergedAABBs.mergeTwoAABBs(m_nodes[currentNodeID].aabb, newNodeAABB);
float mergedVolume = mergedAABBs.getVolume(); float mergedVolume = mergedAABBs.getVolume();
// Compute the cost of making the current node the sibbling of the new node // Compute the cost of making the current node the sibbling of the new node
float costS = float(2.0) * mergedVolume; float costS = float(2.0) * mergedVolume;
// Compute the minimum cost of pushing the new node further down the tree (inheritance cost) // Compute the minimum cost of pushing the new node further down the tree (inheritance cost)
float costI = float(2.0) * (mergedVolume - volumeAABB); float costI = float(2.0) * (mergedVolume - volumeAABB);
// Compute the cost of descending int32_to the left child // Compute the cost of descending int32_to the left child
float costLeft; float costLeft;
AABB currentAndLeftAABB; AABB currentAndLeftAABB;
currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, m_nodes[leftChild].aabb); currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, m_nodes[leftChild].aabb);
if (m_nodes[leftChild].isLeaf()) { // If the left child is a leaf if (m_nodes[leftChild].isLeaf()) { // If the left child is a leaf
costLeft = currentAndLeftAABB.getVolume() + costI; costLeft = currentAndLeftAABB.getVolume() + costI;
} } else {
else {
float leftChildVolume = m_nodes[leftChild].aabb.getVolume(); float leftChildVolume = m_nodes[leftChild].aabb.getVolume();
costLeft = costI + currentAndLeftAABB.getVolume() - leftChildVolume; costLeft = costI + currentAndLeftAABB.getVolume() - leftChildVolume;
} }
// Compute the cost of descending int32_to the right child // Compute the cost of descending int32_to the right child
float costRight; float costRight;
AABB currentAndRightAABB; AABB currentAndRightAABB;
currentAndRightAABB.mergeTwoAABBs(newNodeAABB, m_nodes[rightChild].aabb); currentAndRightAABB.mergeTwoAABBs(newNodeAABB, m_nodes[rightChild].aabb);
if (m_nodes[rightChild].isLeaf()) { // If the right child is a leaf if (m_nodes[rightChild].isLeaf()) { // If the right child is a leaf
costRight = currentAndRightAABB.getVolume() + costI; costRight = currentAndRightAABB.getVolume() + costI;
} } else {
else {
float rightChildVolume = m_nodes[rightChild].aabb.getVolume(); float rightChildVolume = m_nodes[rightChild].aabb.getVolume();
costRight = costI + currentAndRightAABB.getVolume() - rightChildVolume; costRight = costI + currentAndRightAABB.getVolume() - rightChildVolume;
} }
// If the cost of making the current node a sibbling of the new node is smaller than // If the cost of making the current node a sibbling of the new node is smaller than
// the cost of going down int32_to the left or right child // the cost of going down int32_to the left or right child
if (costS < costLeft && costS < costRight) break; if (costS < costLeft && costS < costRight) {
break;
}
// It is cheaper to go down int32_to a child of the current node, choose the best child // It is cheaper to go down int32_to a child of the current node, choose the best child
if (costLeft < costRight) { if (costLeft < costRight) {
currentNodeID = leftChild; currentNodeID = leftChild;
} } else {
else {
currentNodeID = rightChild; currentNodeID = rightChild;
} }
} }
int32_t siblingNode = currentNodeID; int32_t siblingNode = currentNodeID;
// Create a new parent for the new node and the sibling node // Create a new parent for the new node and the sibling node
int32_t oldParentNode = m_nodes[siblingNode].parentID; int32_t oldParentNode = m_nodes[siblingNode].parentID;
int32_t newParentNode = allocateNode(); int32_t newParentNode = allocateNode();
@ -275,120 +238,96 @@ void DynamicAABBTree::insertLeafNode(int32_t _nodeID) {
m_nodes[newParentNode].aabb.mergeTwoAABBs(m_nodes[siblingNode].aabb, newNodeAABB); m_nodes[newParentNode].aabb.mergeTwoAABBs(m_nodes[siblingNode].aabb, newNodeAABB);
m_nodes[newParentNode].height = m_nodes[siblingNode].height + 1; m_nodes[newParentNode].height = m_nodes[siblingNode].height + 1;
assert(m_nodes[newParentNode].height > 0); assert(m_nodes[newParentNode].height > 0);
// If the sibling node was not the root node // If the sibling node was not the root node
if (oldParentNode != TreeNode::NULL_TREE_NODE) { if (oldParentNode != TreeNode::NULL_TREE_NODE) {
assert(!m_nodes[oldParentNode].isLeaf()); assert(!m_nodes[oldParentNode].isLeaf());
if (m_nodes[oldParentNode].children[0] == siblingNode) { if (m_nodes[oldParentNode].children[0] == siblingNode) {
m_nodes[oldParentNode].children[0] = newParentNode; m_nodes[oldParentNode].children[0] = newParentNode;
} } else {
else {
m_nodes[oldParentNode].children[1] = newParentNode; m_nodes[oldParentNode].children[1] = newParentNode;
} }
m_nodes[newParentNode].children[0] = siblingNode; m_nodes[newParentNode].children[0] = siblingNode;
m_nodes[newParentNode].children[1] = _nodeID; m_nodes[newParentNode].children[1] = _nodeID;
m_nodes[siblingNode].parentID = newParentNode; m_nodes[siblingNode].parentID = newParentNode;
m_nodes[_nodeID].parentID = newParentNode; m_nodes[_nodeID].parentID = newParentNode;
} } else {
else { // If the sibling node was the root node // If the sibling node was the root node
m_nodes[newParentNode].children[0] = siblingNode; m_nodes[newParentNode].children[0] = siblingNode;
m_nodes[newParentNode].children[1] = _nodeID; m_nodes[newParentNode].children[1] = _nodeID;
m_nodes[siblingNode].parentID = newParentNode; m_nodes[siblingNode].parentID = newParentNode;
m_nodes[_nodeID].parentID = newParentNode; m_nodes[_nodeID].parentID = newParentNode;
m_rootNodeID = newParentNode; m_rootNodeID = newParentNode;
} }
// Move up in the tree to change the AABBs that have changed // Move up in the tree to change the AABBs that have changed
currentNodeID = m_nodes[_nodeID].parentID; currentNodeID = m_nodes[_nodeID].parentID;
assert(!m_nodes[currentNodeID].isLeaf()); assert(!m_nodes[currentNodeID].isLeaf());
while (currentNodeID != TreeNode::NULL_TREE_NODE) { while (currentNodeID != TreeNode::NULL_TREE_NODE) {
// Balance the sub-tree of the current node if it is not balanced // Balance the sub-tree of the current node if it is not balanced
currentNodeID = balanceSubTreeAtNode(currentNodeID); currentNodeID = balanceSubTreeAtNode(currentNodeID);
assert(m_nodes[_nodeID].isLeaf()); assert(m_nodes[_nodeID].isLeaf());
assert(!m_nodes[currentNodeID].isLeaf()); assert(!m_nodes[currentNodeID].isLeaf());
int32_t leftChild = m_nodes[currentNodeID].children[0]; int32_t leftChild = m_nodes[currentNodeID].children[0];
int32_t rightChild = m_nodes[currentNodeID].children[1]; int32_t rightChild = m_nodes[currentNodeID].children[1];
assert(leftChild != TreeNode::NULL_TREE_NODE); assert(leftChild != TreeNode::NULL_TREE_NODE);
assert(rightChild != TreeNode::NULL_TREE_NODE); assert(rightChild != TreeNode::NULL_TREE_NODE);
// Recompute the height of the node in the tree // Recompute the height of the node in the tree
m_nodes[currentNodeID].height = etk::max(m_nodes[leftChild].height, m_nodes[currentNodeID].height = etk::max(m_nodes[leftChild].height,
m_nodes[rightChild].height) + 1; m_nodes[rightChild].height) + 1;
assert(m_nodes[currentNodeID].height > 0); assert(m_nodes[currentNodeID].height > 0);
// Recompute the AABB of the node // Recompute the AABB of the node
m_nodes[currentNodeID].aabb.mergeTwoAABBs(m_nodes[leftChild].aabb, m_nodes[rightChild].aabb); m_nodes[currentNodeID].aabb.mergeTwoAABBs(m_nodes[leftChild].aabb, m_nodes[rightChild].aabb);
currentNodeID = m_nodes[currentNodeID].parentID; currentNodeID = m_nodes[currentNodeID].parentID;
} }
assert(m_nodes[_nodeID].isLeaf()); assert(m_nodes[_nodeID].isLeaf());
} }
// Remove a leaf node from the tree // Remove a leaf node from the tree
void DynamicAABBTree::removeLeafNode(int32_t _nodeID) { void DynamicAABBTree::removeLeafNode(int32_t _nodeID) {
assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes); assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes);
assert(m_nodes[_nodeID].isLeaf()); assert(m_nodes[_nodeID].isLeaf());
// If we are removing the root node (root node is a leaf in this case) // If we are removing the root node (root node is a leaf in this case)
if (m_rootNodeID == _nodeID) { if (m_rootNodeID == _nodeID) {
m_rootNodeID = TreeNode::NULL_TREE_NODE; m_rootNodeID = TreeNode::NULL_TREE_NODE;
return; return;
} }
int32_t parentNodeID = m_nodes[_nodeID].parentID; int32_t parentNodeID = m_nodes[_nodeID].parentID;
int32_t grandParentNodeID = m_nodes[parentNodeID].parentID; int32_t grandParentNodeID = m_nodes[parentNodeID].parentID;
int32_t siblingNodeID; int32_t siblingNodeID;
if (m_nodes[parentNodeID].children[0] == _nodeID) { if (m_nodes[parentNodeID].children[0] == _nodeID) {
siblingNodeID = m_nodes[parentNodeID].children[1]; siblingNodeID = m_nodes[parentNodeID].children[1];
} } else {
else {
siblingNodeID = m_nodes[parentNodeID].children[0]; siblingNodeID = m_nodes[parentNodeID].children[0];
} }
// If the parent of the node to remove is not the root node // If the parent of the node to remove is not the root node
if (grandParentNodeID != TreeNode::NULL_TREE_NODE) { if (grandParentNodeID != TreeNode::NULL_TREE_NODE) {
// Destroy the parent node // Destroy the parent node
if (m_nodes[grandParentNodeID].children[0] == parentNodeID) { if (m_nodes[grandParentNodeID].children[0] == parentNodeID) {
m_nodes[grandParentNodeID].children[0] = siblingNodeID; m_nodes[grandParentNodeID].children[0] = siblingNodeID;
} } else {
else {
assert(m_nodes[grandParentNodeID].children[1] == parentNodeID); assert(m_nodes[grandParentNodeID].children[1] == parentNodeID);
m_nodes[grandParentNodeID].children[1] = siblingNodeID; m_nodes[grandParentNodeID].children[1] = siblingNodeID;
} }
m_nodes[siblingNodeID].parentID = grandParentNodeID; m_nodes[siblingNodeID].parentID = grandParentNodeID;
releaseNode(parentNodeID); releaseNode(parentNodeID);
// Now, we need to recompute the AABBs of the node on the path back to the root // Now, we need to recompute the AABBs of the node on the path back to the root
// and make sure that the tree is still balanced // and make sure that the tree is still balanced
int32_t currentNodeID = grandParentNodeID; int32_t currentNodeID = grandParentNodeID;
while(currentNodeID != TreeNode::NULL_TREE_NODE) { while(currentNodeID != TreeNode::NULL_TREE_NODE) {
// Balance the current sub-tree if necessary // Balance the current sub-tree if necessary
currentNodeID = balanceSubTreeAtNode(currentNodeID); currentNodeID = balanceSubTreeAtNode(currentNodeID);
assert(!m_nodes[currentNodeID].isLeaf()); assert(!m_nodes[currentNodeID].isLeaf());
// Get the two children of the current node // Get the two children of the current node
int32_t leftChildID = m_nodes[currentNodeID].children[0]; int32_t leftChildID = m_nodes[currentNodeID].children[0];
int32_t rightChildID = m_nodes[currentNodeID].children[1]; int32_t rightChildID = m_nodes[currentNodeID].children[1];
// Recompute the AABB and the height of the current node // Recompute the AABB and the height of the current node
m_nodes[currentNodeID].aabb.mergeTwoAABBs(m_nodes[leftChildID].aabb, m_nodes[currentNodeID].aabb.mergeTwoAABBs(m_nodes[leftChildID].aabb,
m_nodes[rightChildID].aabb); m_nodes[rightChildID].aabb);
m_nodes[currentNodeID].height = etk::max(m_nodes[leftChildID].height, m_nodes[currentNodeID].height = etk::max(m_nodes[leftChildID].height,
m_nodes[rightChildID].height) + 1; m_nodes[rightChildID].height) + 1;
assert(m_nodes[currentNodeID].height > 0); assert(m_nodes[currentNodeID].height > 0);
currentNodeID = m_nodes[currentNodeID].parentID; currentNodeID = m_nodes[currentNodeID].parentID;
} }
} } else { // If the parent of the node to remove is the root node
else { // If the parent of the node to remove is the root node
// The sibling node becomes the new root node // The sibling node becomes the new root node
m_rootNodeID = siblingNodeID; m_rootNodeID = siblingNodeID;
m_nodes[siblingNodeID].parentID = TreeNode::NULL_TREE_NODE; m_nodes[siblingNodeID].parentID = TreeNode::NULL_TREE_NODE;
@ -400,18 +339,13 @@ void DynamicAABBTree::removeLeafNode(int32_t _nodeID) {
/// The rotation schemes are described in the book "Introduction to Game Physics /// The rotation schemes are described in the book "Introduction to Game Physics
/// with Box2D" by Ian Parberry. This method returns the new root node ID. /// with Box2D" by Ian Parberry. This method returns the new root node ID.
int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t _nodeID) { int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t _nodeID) {
assert(_nodeID != TreeNode::NULL_TREE_NODE); assert(_nodeID != TreeNode::NULL_TREE_NODE);
TreeNode* nodeA = m_nodes + _nodeID; TreeNode* nodeA = m_nodes + _nodeID;
// If the node is a leaf or the height of A's sub-tree is less than 2 // If the node is a leaf or the height of A's sub-tree is less than 2
if (nodeA->isLeaf() || nodeA->height < 2) { if (nodeA->isLeaf() || nodeA->height < 2) {
// Do not perform any rotation // Do not perform any rotation
return _nodeID; return _nodeID;
} }
// Get the two children nodes // Get the two children nodes
int32_t nodeBID = nodeA->children[0]; int32_t nodeBID = nodeA->children[0];
int32_t nodeCID = nodeA->children[1]; int32_t nodeCID = nodeA->children[1];
@ -419,150 +353,116 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t _nodeID) {
assert(nodeCID >= 0 && nodeCID < m_numberAllocatedNodes); assert(nodeCID >= 0 && nodeCID < m_numberAllocatedNodes);
TreeNode* nodeB = m_nodes + nodeBID; TreeNode* nodeB = m_nodes + nodeBID;
TreeNode* nodeC = m_nodes + nodeCID; TreeNode* nodeC = m_nodes + nodeCID;
// Compute the factor of the left and right sub-trees // Compute the factor of the left and right sub-trees
int32_t balanceFactor = nodeC->height - nodeB->height; int32_t balanceFactor = nodeC->height - nodeB->height;
// If the right node C is 2 higher than left node B // If the right node C is 2 higher than left node B
if (balanceFactor > 1) { if (balanceFactor > 1) {
assert(!nodeC->isLeaf()); assert(!nodeC->isLeaf());
int32_t nodeFID = nodeC->children[0]; int32_t nodeFID = nodeC->children[0];
int32_t nodeGID = nodeC->children[1]; int32_t nodeGID = nodeC->children[1];
assert(nodeFID >= 0 && nodeFID < m_numberAllocatedNodes); assert(nodeFID >= 0 && nodeFID < m_numberAllocatedNodes);
assert(nodeGID >= 0 && nodeGID < m_numberAllocatedNodes); assert(nodeGID >= 0 && nodeGID < m_numberAllocatedNodes);
TreeNode* nodeF = m_nodes + nodeFID; TreeNode* nodeF = m_nodes + nodeFID;
TreeNode* nodeG = m_nodes + nodeGID; TreeNode* nodeG = m_nodes + nodeGID;
nodeC->children[0] = _nodeID; nodeC->children[0] = _nodeID;
nodeC->parentID = nodeA->parentID; nodeC->parentID = nodeA->parentID;
nodeA->parentID = nodeCID; nodeA->parentID = nodeCID;
if (nodeC->parentID != TreeNode::NULL_TREE_NODE) { if (nodeC->parentID != TreeNode::NULL_TREE_NODE) {
if (m_nodes[nodeC->parentID].children[0] == _nodeID) { if (m_nodes[nodeC->parentID].children[0] == _nodeID) {
m_nodes[nodeC->parentID].children[0] = nodeCID; m_nodes[nodeC->parentID].children[0] = nodeCID;
} } else {
else {
assert(m_nodes[nodeC->parentID].children[1] == _nodeID); assert(m_nodes[nodeC->parentID].children[1] == _nodeID);
m_nodes[nodeC->parentID].children[1] = nodeCID; m_nodes[nodeC->parentID].children[1] = nodeCID;
} }
} } else {
else {
m_rootNodeID = nodeCID; m_rootNodeID = nodeCID;
} }
assert(!nodeC->isLeaf()); assert(!nodeC->isLeaf());
assert(!nodeA->isLeaf()); assert(!nodeA->isLeaf());
// If the right node C was higher than left node B because of the F node // If the right node C was higher than left node B because of the F node
if (nodeF->height > nodeG->height) { if (nodeF->height > nodeG->height) {
nodeC->children[1] = nodeFID; nodeC->children[1] = nodeFID;
nodeA->children[1] = nodeGID; nodeA->children[1] = nodeGID;
nodeG->parentID = _nodeID; nodeG->parentID = _nodeID;
// Recompute the AABB of node A and C // Recompute the AABB of node A and C
nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeG->aabb); nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeG->aabb);
nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb); nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb);
// Recompute the height of node A and C // Recompute the height of node A and C
nodeA->height = etk::max(nodeB->height, nodeG->height) + 1; nodeA->height = etk::max(nodeB->height, nodeG->height) + 1;
nodeC->height = etk::max(nodeA->height, nodeF->height) + 1; nodeC->height = etk::max(nodeA->height, nodeF->height) + 1;
assert(nodeA->height > 0); assert(nodeA->height > 0);
assert(nodeC->height > 0); assert(nodeC->height > 0);
} } else {
else { // If the right node C was higher than left node B because of node G // If the right node C was higher than left node B because of node G
nodeC->children[1] = nodeGID; nodeC->children[1] = nodeGID;
nodeA->children[1] = nodeFID; nodeA->children[1] = nodeFID;
nodeF->parentID = _nodeID; nodeF->parentID = _nodeID;
// Recompute the AABB of node A and C // Recompute the AABB of node A and C
nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeF->aabb); nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeF->aabb);
nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb); nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb);
// Recompute the height of node A and C // Recompute the height of node A and C
nodeA->height = etk::max(nodeB->height, nodeF->height) + 1; nodeA->height = etk::max(nodeB->height, nodeF->height) + 1;
nodeC->height = etk::max(nodeA->height, nodeG->height) + 1; nodeC->height = etk::max(nodeA->height, nodeG->height) + 1;
assert(nodeA->height > 0); assert(nodeA->height > 0);
assert(nodeC->height > 0); assert(nodeC->height > 0);
} }
// Return the new root of the sub-tree // Return the new root of the sub-tree
return nodeCID; return nodeCID;
} }
// If the left node B is 2 higher than right node C // If the left node B is 2 higher than right node C
if (balanceFactor < -1) { if (balanceFactor < -1) {
assert(!nodeB->isLeaf()); assert(!nodeB->isLeaf());
int32_t nodeFID = nodeB->children[0]; int32_t nodeFID = nodeB->children[0];
int32_t nodeGID = nodeB->children[1]; int32_t nodeGID = nodeB->children[1];
assert(nodeFID >= 0 && nodeFID < m_numberAllocatedNodes); assert(nodeFID >= 0 && nodeFID < m_numberAllocatedNodes);
assert(nodeGID >= 0 && nodeGID < m_numberAllocatedNodes); assert(nodeGID >= 0 && nodeGID < m_numberAllocatedNodes);
TreeNode* nodeF = m_nodes + nodeFID; TreeNode* nodeF = m_nodes + nodeFID;
TreeNode* nodeG = m_nodes + nodeGID; TreeNode* nodeG = m_nodes + nodeGID;
nodeB->children[0] = _nodeID; nodeB->children[0] = _nodeID;
nodeB->parentID = nodeA->parentID; nodeB->parentID = nodeA->parentID;
nodeA->parentID = nodeBID; nodeA->parentID = nodeBID;
if (nodeB->parentID != TreeNode::NULL_TREE_NODE) { if (nodeB->parentID != TreeNode::NULL_TREE_NODE) {
if (m_nodes[nodeB->parentID].children[0] == _nodeID) { if (m_nodes[nodeB->parentID].children[0] == _nodeID) {
m_nodes[nodeB->parentID].children[0] = nodeBID; m_nodes[nodeB->parentID].children[0] = nodeBID;
} } else {
else {
assert(m_nodes[nodeB->parentID].children[1] == _nodeID); assert(m_nodes[nodeB->parentID].children[1] == _nodeID);
m_nodes[nodeB->parentID].children[1] = nodeBID; m_nodes[nodeB->parentID].children[1] = nodeBID;
} }
} } else {
else {
m_rootNodeID = nodeBID; m_rootNodeID = nodeBID;
} }
assert(!nodeB->isLeaf()); assert(!nodeB->isLeaf());
assert(!nodeA->isLeaf()); assert(!nodeA->isLeaf());
// If the left node B was higher than right node C because of the F node // If the left node B was higher than right node C because of the F node
if (nodeF->height > nodeG->height) { if (nodeF->height > nodeG->height) {
nodeB->children[1] = nodeFID; nodeB->children[1] = nodeFID;
nodeA->children[0] = nodeGID; nodeA->children[0] = nodeGID;
nodeG->parentID = _nodeID; nodeG->parentID = _nodeID;
// Recompute the AABB of node A and B // Recompute the AABB of node A and B
nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeG->aabb); nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeG->aabb);
nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb); nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb);
// Recompute the height of node A and B // Recompute the height of node A and B
nodeA->height = etk::max(nodeC->height, nodeG->height) + 1; nodeA->height = etk::max(nodeC->height, nodeG->height) + 1;
nodeB->height = etk::max(nodeA->height, nodeF->height) + 1; nodeB->height = etk::max(nodeA->height, nodeF->height) + 1;
assert(nodeA->height > 0); assert(nodeA->height > 0);
assert(nodeB->height > 0); assert(nodeB->height > 0);
} } else {
else { // If the left node B was higher than right node C because of node G // If the left node B was higher than right node C because of node G
nodeB->children[1] = nodeGID; nodeB->children[1] = nodeGID;
nodeA->children[0] = nodeFID; nodeA->children[0] = nodeFID;
nodeF->parentID = _nodeID; nodeF->parentID = _nodeID;
// Recompute the AABB of node A and B // Recompute the AABB of node A and B
nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeF->aabb); nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeF->aabb);
nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb); nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb);
// Recompute the height of node A and B // Recompute the height of node A and B
nodeA->height = etk::max(nodeC->height, nodeF->height) + 1; nodeA->height = etk::max(nodeC->height, nodeF->height) + 1;
nodeB->height = etk::max(nodeA->height, nodeG->height) + 1; nodeB->height = etk::max(nodeA->height, nodeG->height) + 1;
assert(nodeA->height > 0); assert(nodeA->height > 0);
assert(nodeB->height > 0); assert(nodeB->height > 0);
} }
// Return the new root of the sub-tree // Return the new root of the sub-tree
return nodeBID; return nodeBID;
} }
// If the sub-tree is balanced, return the current root node // If the sub-tree is balanced, return the current root node
return _nodeID; return _nodeID;
} }
@ -592,7 +492,8 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& _aabb, etk:
if (nodeToVisit->isLeaf()) { if (nodeToVisit->isLeaf()) {
// Notify the broad-phase about a new potential overlapping pair // Notify the broad-phase about a new potential overlapping pair
_callback(nodeIDToVisit); _callback(nodeIDToVisit);
} else { // If the node is not a leaf } else {
// If the node is not a leaf
// We need to visit its children // We need to visit its children
stack.push(nodeToVisit->children[0]); stack.push(nodeToVisit->children[0]);
stack.push(nodeToVisit->children[1]); stack.push(nodeToVisit->children[1]);
@ -687,95 +588,75 @@ AABB DynamicAABBTree::getRootAABB() const {
// Add an object int32_to the tree. This method creates a new leaf node in the tree and // Add an object int32_to the tree. This method creates a new leaf node in the tree and
// returns the ID of the corresponding node. // returns the ID of the corresponding node.
int32_t DynamicAABBTree::addObject(const AABB& aabb, int32_t data1, int32_t data2) { int32_t DynamicAABBTree::addObject(const AABB& aabb, int32_t data1, int32_t data2) {
int32_t nodeId = addObjectInternal(aabb); int32_t nodeId = addObjectInternal(aabb);
m_nodes[nodeId].dataInt[0] = data1; m_nodes[nodeId].dataInt[0] = data1;
m_nodes[nodeId].dataInt[1] = data2; m_nodes[nodeId].dataInt[1] = data2;
return nodeId; return nodeId;
} }
// Add an object int32_to the tree. This method creates a new leaf node in the tree and // Add an object int32_to the tree. This method creates a new leaf node in the tree and
// returns the ID of the corresponding node. // returns the ID of the corresponding node.
int32_t DynamicAABBTree::addObject(const AABB& aabb, void* data) { int32_t DynamicAABBTree::addObject(const AABB& aabb, void* data) {
int32_t nodeId = addObjectInternal(aabb); int32_t nodeId = addObjectInternal(aabb);
m_nodes[nodeId].dataPointer = data; m_nodes[nodeId].dataPointer = data;
return nodeId; return nodeId;
} }
#ifndef NDEBUG #ifdef DEBUG
// Check if the tree structure is valid (for debugging purpose) // Check if the tree structure is valid (for debugging purpose)
void DynamicAABBTree::check() const { void DynamicAABBTree::check() const {
// Recursively check each node // Recursively check each node
checkNode(m_rootNodeID); checkNode(m_rootNodeID);
int32_t nbFreeNodes = 0; int32_t nbFreeNodes = 0;
int32_t freeNodeID = m_freeNodeID; int32_t freeNodeID = m_freeNodeID;
// Check the free nodes // Check the free nodes
while(freeNodeID != TreeNode::NULL_TREE_NODE) { while(freeNodeID != TreeNode::NULL_TREE_NODE) {
assert(0 <= freeNodeID && freeNodeID < m_numberAllocatedNodes); assert(0 <= freeNodeID && freeNodeID < m_numberAllocatedNodes);
freeNodeID = m_nodes[freeNodeID].nextNodeID; freeNodeID = m_nodes[freeNodeID].nextNodeID;
nbFreeNodes++; nbFreeNodes++;
} }
assert(m_numberNodes + nbFreeNodes == m_numberAllocatedNodes); assert(m_numberNodes + nbFreeNodes == m_numberAllocatedNodes);
} }
// Check if the node structure is valid (for debugging purpose) // Check if the node structure is valid (for debugging purpose)
void DynamicAABBTree::checkNode(int32_t _nodeID) const { void DynamicAABBTree::checkNode(int32_t _nodeID) const {
if (_nodeID == TreeNode::NULL_TREE_NODE) {
if (_nodeID == TreeNode::NULL_TREE_NODE) return; return;
}
// If it is the root // If it is the root
if (_nodeID == m_rootNodeID) { if (_nodeID == m_rootNodeID) {
assert(m_nodes[_nodeID].parentID == TreeNode::NULL_TREE_NODE); assert(m_nodes[_nodeID].parentID == TreeNode::NULL_TREE_NODE);
} }
// Get the children nodes // Get the children nodes
TreeNode* pNode = m_nodes + _nodeID; TreeNode* pNode = m_nodes + _nodeID;
assert(!pNode->isLeaf()); assert(!pNode->isLeaf());
int32_t leftChild = pNode->children[0]; int32_t leftChild = pNode->children[0];
int32_t rightChild = pNode->children[1]; int32_t rightChild = pNode->children[1];
assert(pNode->height >= 0); assert(pNode->height >= 0);
assert(pNode->aabb.getVolume() > 0); assert(pNode->aabb.getVolume() > 0);
// If the current node is a leaf // If the current node is a leaf
if (pNode->isLeaf()) { if (pNode->isLeaf()) {
// Check that there are no children // Check that there are no children
assert(leftChild == TreeNode::NULL_TREE_NODE); assert(leftChild == TreeNode::NULL_TREE_NODE);
assert(rightChild == TreeNode::NULL_TREE_NODE); assert(rightChild == TreeNode::NULL_TREE_NODE);
assert(pNode->height == 0); assert(pNode->height == 0);
} } else {
else {
// Check that the children node IDs are valid // Check that the children node IDs are valid
assert(0 <= leftChild && leftChild < m_numberAllocatedNodes); assert(0 <= leftChild && leftChild < m_numberAllocatedNodes);
assert(0 <= rightChild && rightChild < m_numberAllocatedNodes); assert(0 <= rightChild && rightChild < m_numberAllocatedNodes);
// Check that the children nodes have the correct parent node // Check that the children nodes have the correct parent node
assert(m_nodes[leftChild].parentID == _nodeID); assert(m_nodes[leftChild].parentID == _nodeID);
assert(m_nodes[rightChild].parentID == _nodeID); assert(m_nodes[rightChild].parentID == _nodeID);
// Check the height of node // Check the height of node
int32_t height = 1 + etk::max(m_nodes[leftChild].height, m_nodes[rightChild].height); int32_t height = 1 + etk::max(m_nodes[leftChild].height, m_nodes[rightChild].height);
assert(m_nodes[_nodeID].height == height); assert(m_nodes[_nodeID].height == height);
// Check the AABB of the node // Check the AABB of the node
AABB aabb; AABB aabb;
aabb.mergeTwoAABBs(m_nodes[leftChild].aabb, m_nodes[rightChild].aabb); aabb.mergeTwoAABBs(m_nodes[leftChild].aabb, m_nodes[rightChild].aabb);
assert(aabb.getMin() == m_nodes[_nodeID].aabb.getMin()); assert(aabb.getMin() == m_nodes[_nodeID].aabb.getMin());
assert(aabb.getMax() == m_nodes[_nodeID].aabb.getMax()); assert(aabb.getMax() == m_nodes[_nodeID].aabb.getMax());
// Recursively check the children nodes // Recursively check the children nodes
checkNode(leftChild); checkNode(leftChild);
checkNode(rightChild); checkNode(rightChild);
@ -791,16 +672,13 @@ int32_t DynamicAABBTree::computeHeight() {
int32_t DynamicAABBTree::computeHeight(int32_t _nodeID) { int32_t DynamicAABBTree::computeHeight(int32_t _nodeID) {
assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes); assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes);
TreeNode* node = m_nodes + _nodeID; TreeNode* node = m_nodes + _nodeID;
// If the node is a leaf, its height is zero // If the node is a leaf, its height is zero
if (node->isLeaf()) { if (node->isLeaf()) {
return 0; return 0;
} }
// Compute the height of the left and right sub-tree // Compute the height of the left and right sub-tree
int32_t leftHeight = computeHeight(node->children[0]); int32_t leftHeight = computeHeight(node->children[0]);
int32_t rightHeight = computeHeight(node->children[1]); int32_t rightHeight = computeHeight(node->children[1]);
// Return the height of the node // Return the height of the node
return 1 + etk::max(leftHeight, rightHeight); return 1 + etk::max(leftHeight, rightHeight);
} }

4
ephysics/collision/broadphase/DynamicAABBTree.hpp
View File

@ -11,9 +11,7 @@
#include <etk/Function.hpp> #include <etk/Function.hpp>
namespace ephysics { namespace ephysics {
class BroadPhaseAlgorithm; // TODO: to replace this, create a Tree<T> template (multiple child) or TreeRedBlack<T>
class BroadPhaseRaycastTestCallback;
struct RaycastTest;
/** /**
* @brief It represents a node of the dynamic AABB tree. * @brief It represents a node of the dynamic AABB tree.
*/ */

7
ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp
View File

@ -297,7 +297,10 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& _simpl
nbVertices++; nbVertices++;
// Update the upper bound of the penetration depth // Update the upper bound of the penetration depth
float wDotv = points[indexNewVertex].dot(triangle->getClosestPoint()); float wDotv = points[indexNewVertex].dot(triangle->getClosestPoint());
EPHY_ASSERT(wDotv > 0.0, "depth penetration error"); EPHY_INFO(" point=" << points[indexNewVertex]);
EPHY_INFO("close point=" << triangle->getClosestPoint());
EPHY_INFO(" ==>" << wDotv);
EPHY_ASSERT(wDotv >= 0.0, "depth penetration error");
float wDotVSquare = wDotv * wDotv / triangle->getDistSquare(); float wDotVSquare = wDotv * wDotv / triangle->getDistSquare();
if (wDotVSquare < upperBoundSquarePenDepth) { if (wDotVSquare < upperBoundSquarePenDepth) {
upperBoundSquarePenDepth = wDotVSquare; upperBoundSquarePenDepth = wDotVSquare;
@ -333,7 +336,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& _simpl
vec3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB); vec3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
vec3 normal = _vector.safeNormalized(); vec3 normal = _vector.safeNormalized();
float penetrationDepth = _vector.length(); float penetrationDepth = _vector.length();
assert(penetrationDepth > 0.0); EPHY_ASSERT(penetrationDepth >= 0.0, "penetration depth <0");
if (normal.length2() < FLT_EPSILON) { if (normal.length2() < FLT_EPSILON) {
return; return;
} }

8
ephysics/collision/narrowphase/EPA/TriangleEPA.hpp
View File

@ -27,7 +27,9 @@ namespace ephysics {
public: public:
/// Private copy-constructor /// Private copy-constructor
TriangleEPA(const TriangleEPA& _triangle) { TriangleEPA(const TriangleEPA& _triangle) {
memcpy(m_indicesVertices, _triangle.m_indicesVertices, sizeof(m_indicesVertices)); m_indicesVertices[0] = _triangle.m_indicesVertices[0];
m_indicesVertices[1] = _triangle.m_indicesVertices[1];
m_indicesVertices[2] = _triangle.m_indicesVertices[2];
m_adjacentEdges[0] = _triangle.m_adjacentEdges[0]; m_adjacentEdges[0] = _triangle.m_adjacentEdges[0];
m_adjacentEdges[1] = _triangle.m_adjacentEdges[1]; m_adjacentEdges[1] = _triangle.m_adjacentEdges[1];
m_adjacentEdges[2] = _triangle.m_adjacentEdges[2]; m_adjacentEdges[2] = _triangle.m_adjacentEdges[2];
@ -40,7 +42,9 @@ namespace ephysics {
} }
/// Private assignment operator /// Private assignment operator
TriangleEPA& operator=(const TriangleEPA& _triangle) { TriangleEPA& operator=(const TriangleEPA& _triangle) {
memcpy(m_indicesVertices, _triangle.m_indicesVertices, sizeof(m_indicesVertices)); m_indicesVertices[0] = _triangle.m_indicesVertices[0];
m_indicesVertices[1] = _triangle.m_indicesVertices[1];
m_indicesVertices[2] = _triangle.m_indicesVertices[2];
m_adjacentEdges[0] = _triangle.m_adjacentEdges[0]; m_adjacentEdges[0] = _triangle.m_adjacentEdges[0];
m_adjacentEdges[1] = _triangle.m_adjacentEdges[1]; m_adjacentEdges[1] = _triangle.m_adjacentEdges[1];
m_adjacentEdges[2] = _triangle.m_adjacentEdges[2]; m_adjacentEdges[2] = _triangle.m_adjacentEdges[2];

27
ephysics/constraint/ContactPoint.cpp
View File

@ -12,24 +12,25 @@ using namespace ephysics;
using namespace std; using namespace std;
// Constructor // Constructor
ContactPoint::ContactPoint(const ContactPointInfo& contactInfo) ContactPoint::ContactPoint(const ContactPointInfo& _contactInfo):
: m_body1(contactInfo.shape1->getBody()), m_body2(contactInfo.shape2->getBody()), m_body1(_contactInfo.shape1->getBody()),
m_normal(contactInfo.normal), m_body2(_contactInfo.shape2->getBody()),
m_penetrationDepth(contactInfo.penetrationDepth), m_normal(_contactInfo.normal),
m_localPointOnBody1(contactInfo.localPoint1), m_penetrationDepth(_contactInfo.penetrationDepth),
m_localPointOnBody2(contactInfo.localPoint2), m_localPointOnBody1(_contactInfo.localPoint1),
m_worldPointOnBody1(contactInfo.shape1->getBody()->getTransform() * m_localPointOnBody2(_contactInfo.localPoint2),
contactInfo.shape1->getLocalToBodyTransform() * m_worldPointOnBody1(_contactInfo.shape1->getBody()->getTransform() *
contactInfo.localPoint1), _contactInfo.shape1->getLocalToBodyTransform() *
m_worldPointOnBody2(contactInfo.shape2->getBody()->getTransform() * _contactInfo.localPoint1),
contactInfo.shape2->getLocalToBodyTransform() * m_worldPointOnBody2(_contactInfo.shape2->getBody()->getTransform() *
contactInfo.localPoint2), _contactInfo.shape2->getLocalToBodyTransform() *
_contactInfo.localPoint2),
m_isRestingContact(false) { m_isRestingContact(false) {
m_frictionVectors[0] = vec3(0, 0, 0); m_frictionVectors[0] = vec3(0, 0, 0);
m_frictionVectors[1] = vec3(0, 0, 0); m_frictionVectors[1] = vec3(0, 0, 0);
assert(m_penetrationDepth > 0.0); assert(m_penetrationDepth >= 0.0);
} }