diff --git a/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp b/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp index 26176b5..7f8a852 100644 --- a/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp @@ -11,45 +11,31 @@ using namespace ephysics; -BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection) - :m_dynamicAABBTree(DYNAMIC_TREE_AABB_GAP), m_numberMovedShapes(0), m_numberAllocatedMovedShapes(8), - m_numberNonUsedMovedShapes(0), - m_collisionDetection(collisionDetection) { - - // 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); - +BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& _collisionDetection): + m_dynamicAABBTree(DYNAMIC_TREE_AABB_GAP), + m_collisionDetection(_collisionDetection) { + m_movedShapes.reserve(8); m_potentialPairs.reserve(8); } 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 - if (m_numberAllocatedMovedShapes == m_numberMovedShapes) { - m_numberAllocatedMovedShapes *= 2; - int32_t* oldArray = m_movedShapes; - m_movedShapes = (int32_t*) malloc(m_numberAllocatedMovedShapes * sizeof(int32_t)); - assert(m_movedShapes != NULL); - memcpy(m_movedShapes, oldArray, m_numberMovedShapes * sizeof(int32_t)); - free(oldArray); +void BroadPhaseAlgorithm::removeMovedCollisionShape(int32_t _broadPhaseID) { + auto it = m_movedShapes.begin(); + while (it != m_movedShapes.end()) { + if (*it == _broadPhaseID) { + it = m_movedShapes.erase(it); + } else { + ++it; + } } - - // 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); // 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; } } + */ } void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) { @@ -129,28 +116,27 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() { m_potentialPairs.clear(); // For all collision shapes that have moved (or have been created) during the // last simulation step - for (uint32_t i=0; i 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> m_potentialPairs; //!< Temporary array of potential overlapping pairs (with potential duplicates) CollisionDetection& m_collisionDetection; //!< Reference to the collision detection object /// Private copy-constructor - BroadPhaseAlgorithm(const BroadPhaseAlgorithm& algorithm); + BroadPhaseAlgorithm(const BroadPhaseAlgorithm& _obj); /// Private assignment operator - BroadPhaseAlgorithm& operator=(const BroadPhaseAlgorithm& algorithm); + BroadPhaseAlgorithm& operator=(const BroadPhaseAlgorithm& _obj); public : /// Constructor BroadPhaseAlgorithm(CollisionDetection& _collisionDetection); diff --git a/ephysics/collision/broadphase/DynamicAABBTree.cpp b/ephysics/collision/broadphase/DynamicAABBTree.cpp index 2a703b6..70909e0 100644 --- a/ephysics/collision/broadphase/DynamicAABBTree.cpp +++ b/ephysics/collision/broadphase/DynamicAABBTree.cpp @@ -14,28 +14,24 @@ using namespace ephysics; const int32_t TreeNode::NULL_TREE_NODE = -1; -DynamicAABBTree::DynamicAABBTree(float extraAABBGap) : m_extraAABBGap(extraAABBGap) { - +DynamicAABBTree::DynamicAABBTree(float _extraAABBGap): + m_extraAABBGap(_extraAABBGap) { init(); } DynamicAABBTree::~DynamicAABBTree() { - free(m_nodes); } // Initialize the tree void DynamicAABBTree::init() { - m_rootNodeID = TreeNode::NULL_TREE_NODE; m_numberNodes = 0; m_numberAllocatedNodes = 8; - // Allocate memory for the nodes of the tree m_nodes = (TreeNode*) malloc(m_numberAllocatedNodes * sizeof(TreeNode)); assert(m_nodes); memset(m_nodes, 0, m_numberAllocatedNodes * sizeof(TreeNode)); - // Initialize the allocated nodes for (int32_t i=0; i 0); assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes); assert(m_nodes[_nodeID].height >= 0); @@ -100,34 +93,26 @@ void DynamicAABBTree::releaseNode(int32_t _nodeID) { // Internally add an object int32_to the tree int32_t DynamicAABBTree::addObjectInternal(const AABB& aabb) { - // Get the next available node (or allocate new ones if necessary) int32_t _nodeID = allocateNode(); - // Create the fat aabb to use in the tree const vec3 gap(m_extraAABBGap, m_extraAABBGap, m_extraAABBGap); m_nodes[_nodeID].aabb.setMin(aabb.getMin() - gap); m_nodes[_nodeID].aabb.setMax(aabb.getMax() + gap); - // Set the height of the node in the tree m_nodes[_nodeID].height = 0; - // Insert the new leaf node in the tree insertLeafNode(_nodeID); assert(m_nodes[_nodeID].isLeaf()); - assert(_nodeID >= 0); - // Return the Id of the node return _nodeID; } // Remove an object from the tree void DynamicAABBTree::removeObject(int32_t _nodeID) { - assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes); assert(m_nodes[_nodeID].isLeaf()); - // Remove the node from the tree removeLeafNode(_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 /// (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) { - PROFILE("DynamicAABBTree::updateObject()"); - assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes); assert(m_nodes[_nodeID].isLeaf()); assert(m_nodes[_nodeID].height >= 0); - EPHY_INFO(" compare : " << m_nodes[_nodeID].aabb.m_minCoordinates << " " << m_nodes[_nodeID].aabb.m_maxCoordinates); EPHY_INFO(" : " << _newAABB.m_minCoordinates << " " << _newAABB.m_maxCoordinates); // 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)) { return false; } - // If the new AABB is outside the fat AABB, we remove the corresponding node removeLeafNode(_nodeID); - // Compute the fat AABB by inflating the AABB with a constant gap m_nodes[_nodeID].aabb = _newAABB; const vec3 gap(m_extraAABBGap, m_extraAABBGap, m_extraAABBGap); m_nodes[_nodeID].aabb.m_minCoordinates -= gap; m_nodes[_nodeID].aabb.m_maxCoordinates += gap; - // Inflate the fat AABB in direction of the linear motion of the AABB 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()); @@ -188,10 +167,8 @@ bool DynamicAABBTree::updateObject(int32_t _nodeID, const AABB& _newAABB, const //EPHY_CRITICAL("ERROR"); } assert(m_nodes[_nodeID].aabb.contains(_newAABB)); - // Reinsert the node int32_to the tree insertLeafNode(_nodeID); - 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 // with Box2D" by Ian Parberry. void DynamicAABBTree::insertLeafNode(int32_t _nodeID) { - // If the tree is empty if (m_rootNodeID == TreeNode::NULL_TREE_NODE) { m_rootNodeID = _nodeID; m_nodes[m_rootNodeID].parentID = TreeNode::NULL_TREE_NODE; return; } - assert(m_rootNodeID != TreeNode::NULL_TREE_NODE); - // Find the best sibling node for the new node AABB newNodeAABB = m_nodes[_nodeID].aabb; int32_t currentNodeID = m_rootNodeID; while (!m_nodes[currentNodeID].isLeaf()) { - int32_t leftChild = m_nodes[currentNodeID].children[0]; int32_t rightChild = m_nodes[currentNodeID].children[1]; - // Compute the merged AABB float volumeAABB = m_nodes[currentNodeID].aabb.getVolume(); AABB mergedAABBs; mergedAABBs.mergeTwoAABBs(m_nodes[currentNodeID].aabb, newNodeAABB); float mergedVolume = mergedAABBs.getVolume(); - // Compute the cost of making the current node the sibbling of the new node float costS = float(2.0) * mergedVolume; - // Compute the minimum cost of pushing the new node further down the tree (inheritance cost) float costI = float(2.0) * (mergedVolume - volumeAABB); - // Compute the cost of descending int32_to the left child float costLeft; AABB currentAndLeftAABB; currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, m_nodes[leftChild].aabb); if (m_nodes[leftChild].isLeaf()) { // If the left child is a leaf costLeft = currentAndLeftAABB.getVolume() + costI; - } - else { + } else { float leftChildVolume = m_nodes[leftChild].aabb.getVolume(); costLeft = costI + currentAndLeftAABB.getVolume() - leftChildVolume; } - // Compute the cost of descending int32_to the right child float costRight; AABB currentAndRightAABB; currentAndRightAABB.mergeTwoAABBs(newNodeAABB, m_nodes[rightChild].aabb); if (m_nodes[rightChild].isLeaf()) { // If the right child is a leaf costRight = currentAndRightAABB.getVolume() + costI; - } - else { + } else { float rightChildVolume = m_nodes[rightChild].aabb.getVolume(); costRight = costI + currentAndRightAABB.getVolume() - rightChildVolume; } - // 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 - 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 if (costLeft < costRight) { currentNodeID = leftChild; - } - else { + } else { currentNodeID = rightChild; } } - int32_t siblingNode = currentNodeID; - // Create a new parent for the new node and the sibling node int32_t oldParentNode = m_nodes[siblingNode].parentID; 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].height = m_nodes[siblingNode].height + 1; assert(m_nodes[newParentNode].height > 0); - // If the sibling node was not the root node if (oldParentNode != TreeNode::NULL_TREE_NODE) { assert(!m_nodes[oldParentNode].isLeaf()); if (m_nodes[oldParentNode].children[0] == siblingNode) { m_nodes[oldParentNode].children[0] = newParentNode; - } - else { + } else { m_nodes[oldParentNode].children[1] = newParentNode; } m_nodes[newParentNode].children[0] = siblingNode; m_nodes[newParentNode].children[1] = _nodeID; m_nodes[siblingNode].parentID = newParentNode; m_nodes[_nodeID].parentID = newParentNode; - } - else { // If the sibling node was the root node + } else { + // If the sibling node was the root node m_nodes[newParentNode].children[0] = siblingNode; m_nodes[newParentNode].children[1] = _nodeID; m_nodes[siblingNode].parentID = newParentNode; m_nodes[_nodeID].parentID = newParentNode; m_rootNodeID = newParentNode; } - // Move up in the tree to change the AABBs that have changed currentNodeID = m_nodes[_nodeID].parentID; assert(!m_nodes[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(m_nodes[_nodeID].isLeaf()); - assert(!m_nodes[currentNodeID].isLeaf()); int32_t leftChild = m_nodes[currentNodeID].children[0]; int32_t rightChild = m_nodes[currentNodeID].children[1]; assert(leftChild != TreeNode::NULL_TREE_NODE); assert(rightChild != TreeNode::NULL_TREE_NODE); - // Recompute the height of the node in the tree m_nodes[currentNodeID].height = etk::max(m_nodes[leftChild].height, m_nodes[rightChild].height) + 1; assert(m_nodes[currentNodeID].height > 0); - // Recompute the AABB of the node m_nodes[currentNodeID].aabb.mergeTwoAABBs(m_nodes[leftChild].aabb, m_nodes[rightChild].aabb); - currentNodeID = m_nodes[currentNodeID].parentID; } - assert(m_nodes[_nodeID].isLeaf()); } // Remove a leaf node from the tree void DynamicAABBTree::removeLeafNode(int32_t _nodeID) { - assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes); assert(m_nodes[_nodeID].isLeaf()); - // If we are removing the root node (root node is a leaf in this case) if (m_rootNodeID == _nodeID) { m_rootNodeID = TreeNode::NULL_TREE_NODE; return; } - int32_t parentNodeID = m_nodes[_nodeID].parentID; int32_t grandParentNodeID = m_nodes[parentNodeID].parentID; int32_t siblingNodeID; if (m_nodes[parentNodeID].children[0] == _nodeID) { siblingNodeID = m_nodes[parentNodeID].children[1]; - } - else { + } else { siblingNodeID = m_nodes[parentNodeID].children[0]; } - // If the parent of the node to remove is not the root node if (grandParentNodeID != TreeNode::NULL_TREE_NODE) { - // Destroy the parent node if (m_nodes[grandParentNodeID].children[0] == parentNodeID) { m_nodes[grandParentNodeID].children[0] = siblingNodeID; - } - else { + } else { assert(m_nodes[grandParentNodeID].children[1] == parentNodeID); m_nodes[grandParentNodeID].children[1] = siblingNodeID; } m_nodes[siblingNodeID].parentID = grandParentNodeID; releaseNode(parentNodeID); - // 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 int32_t currentNodeID = grandParentNodeID; while(currentNodeID != TreeNode::NULL_TREE_NODE) { - // Balance the current sub-tree if necessary currentNodeID = balanceSubTreeAtNode(currentNodeID); - assert(!m_nodes[currentNodeID].isLeaf()); - // Get the two children of the current node int32_t leftChildID = m_nodes[currentNodeID].children[0]; int32_t rightChildID = m_nodes[currentNodeID].children[1]; - // Recompute the AABB and the height of the current node m_nodes[currentNodeID].aabb.mergeTwoAABBs(m_nodes[leftChildID].aabb, m_nodes[rightChildID].aabb); m_nodes[currentNodeID].height = etk::max(m_nodes[leftChildID].height, m_nodes[rightChildID].height) + 1; assert(m_nodes[currentNodeID].height > 0); - 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 m_rootNodeID = siblingNodeID; 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 /// with Box2D" by Ian Parberry. This method returns the new root node ID. int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t _nodeID) { - assert(_nodeID != TreeNode::NULL_TREE_NODE); - TreeNode* nodeA = m_nodes + _nodeID; - // If the node is a leaf or the height of A's sub-tree is less than 2 if (nodeA->isLeaf() || nodeA->height < 2) { - // Do not perform any rotation return _nodeID; } - // Get the two children nodes int32_t nodeBID = nodeA->children[0]; int32_t nodeCID = nodeA->children[1]; @@ -419,150 +353,116 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t _nodeID) { assert(nodeCID >= 0 && nodeCID < m_numberAllocatedNodes); TreeNode* nodeB = m_nodes + nodeBID; TreeNode* nodeC = m_nodes + nodeCID; - // Compute the factor of the left and right sub-trees int32_t balanceFactor = nodeC->height - nodeB->height; - // If the right node C is 2 higher than left node B if (balanceFactor > 1) { - assert(!nodeC->isLeaf()); - int32_t nodeFID = nodeC->children[0]; int32_t nodeGID = nodeC->children[1]; assert(nodeFID >= 0 && nodeFID < m_numberAllocatedNodes); assert(nodeGID >= 0 && nodeGID < m_numberAllocatedNodes); TreeNode* nodeF = m_nodes + nodeFID; TreeNode* nodeG = m_nodes + nodeGID; - nodeC->children[0] = _nodeID; nodeC->parentID = nodeA->parentID; nodeA->parentID = nodeCID; - if (nodeC->parentID != TreeNode::NULL_TREE_NODE) { - if (m_nodes[nodeC->parentID].children[0] == _nodeID) { m_nodes[nodeC->parentID].children[0] = nodeCID; - } - else { + } else { assert(m_nodes[nodeC->parentID].children[1] == _nodeID); m_nodes[nodeC->parentID].children[1] = nodeCID; } - } - else { + } else { m_rootNodeID = 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; - // Recompute the AABB of node A and C nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeG->aabb); nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb); - // Recompute the height of node A and C nodeA->height = etk::max(nodeB->height, nodeG->height) + 1; nodeC->height = etk::max(nodeA->height, nodeF->height) + 1; assert(nodeA->height > 0); assert(nodeC->height > 0); - } - else { // If the right node C was higher than left node B because of node G + } else { + // If the right node C was higher than left node B because of node G nodeC->children[1] = nodeGID; nodeA->children[1] = nodeFID; nodeF->parentID = _nodeID; - // Recompute the AABB of node A and C nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeF->aabb); nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb); - // Recompute the height of node A and C nodeA->height = etk::max(nodeB->height, nodeF->height) + 1; nodeC->height = etk::max(nodeA->height, nodeG->height) + 1; assert(nodeA->height > 0); assert(nodeC->height > 0); } - // Return the new root of the sub-tree return nodeCID; } - // If the left node B is 2 higher than right node C if (balanceFactor < -1) { - assert(!nodeB->isLeaf()); - int32_t nodeFID = nodeB->children[0]; int32_t nodeGID = nodeB->children[1]; assert(nodeFID >= 0 && nodeFID < m_numberAllocatedNodes); assert(nodeGID >= 0 && nodeGID < m_numberAllocatedNodes); TreeNode* nodeF = m_nodes + nodeFID; TreeNode* nodeG = m_nodes + nodeGID; - nodeB->children[0] = _nodeID; nodeB->parentID = nodeA->parentID; nodeA->parentID = nodeBID; - if (nodeB->parentID != TreeNode::NULL_TREE_NODE) { - if (m_nodes[nodeB->parentID].children[0] == _nodeID) { m_nodes[nodeB->parentID].children[0] = nodeBID; - } - else { + } else { assert(m_nodes[nodeB->parentID].children[1] == _nodeID); m_nodes[nodeB->parentID].children[1] = nodeBID; } - } - else { + } else { m_rootNodeID = 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; - // Recompute the AABB of node A and B nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeG->aabb); nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb); - // Recompute the height of node A and B nodeA->height = etk::max(nodeC->height, nodeG->height) + 1; nodeB->height = etk::max(nodeA->height, nodeF->height) + 1; assert(nodeA->height > 0); assert(nodeB->height > 0); - } - else { // If the left node B was higher than right node C because of node G + } else { + // If the left node B was higher than right node C because of node G nodeB->children[1] = nodeGID; nodeA->children[0] = nodeFID; nodeF->parentID = _nodeID; - // Recompute the AABB of node A and B nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeF->aabb); nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb); - // Recompute the height of node A and B nodeA->height = etk::max(nodeC->height, nodeF->height) + 1; nodeB->height = etk::max(nodeA->height, nodeG->height) + 1; assert(nodeA->height > 0); assert(nodeB->height > 0); } - // Return the new root of the sub-tree return nodeBID; } - // If the sub-tree is balanced, return the current root node return _nodeID; } @@ -592,7 +492,8 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& _aabb, etk: if (nodeToVisit->isLeaf()) { // Notify the broad-phase about a new potential overlapping pair _callback(nodeIDToVisit); - } else { // If the node is not a leaf + } else { + // If the node is not a leaf // We need to visit its children stack.push(nodeToVisit->children[0]); 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 // returns the ID of the corresponding node. int32_t DynamicAABBTree::addObject(const AABB& aabb, int32_t data1, int32_t data2) { - int32_t nodeId = addObjectInternal(aabb); - m_nodes[nodeId].dataInt[0] = data1; m_nodes[nodeId].dataInt[1] = data2; - return nodeId; } // 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. int32_t DynamicAABBTree::addObject(const AABB& aabb, void* data) { - int32_t nodeId = addObjectInternal(aabb); - m_nodes[nodeId].dataPointer = data; - return nodeId; } -#ifndef NDEBUG +#ifdef DEBUG // Check if the tree structure is valid (for debugging purpose) void DynamicAABBTree::check() const { - // Recursively check each node checkNode(m_rootNodeID); - int32_t nbFreeNodes = 0; int32_t freeNodeID = m_freeNodeID; - // Check the free nodes while(freeNodeID != TreeNode::NULL_TREE_NODE) { assert(0 <= freeNodeID && freeNodeID < m_numberAllocatedNodes); freeNodeID = m_nodes[freeNodeID].nextNodeID; nbFreeNodes++; } - assert(m_numberNodes + nbFreeNodes == m_numberAllocatedNodes); } // Check if the node structure is valid (for debugging purpose) void DynamicAABBTree::checkNode(int32_t _nodeID) const { - - if (_nodeID == TreeNode::NULL_TREE_NODE) return; - + if (_nodeID == TreeNode::NULL_TREE_NODE) { + return; + } // If it is the root if (_nodeID == m_rootNodeID) { assert(m_nodes[_nodeID].parentID == TreeNode::NULL_TREE_NODE); } - // Get the children nodes TreeNode* pNode = m_nodes + _nodeID; assert(!pNode->isLeaf()); int32_t leftChild = pNode->children[0]; int32_t rightChild = pNode->children[1]; - assert(pNode->height >= 0); assert(pNode->aabb.getVolume() > 0); - // If the current node is a leaf if (pNode->isLeaf()) { - // Check that there are no children assert(leftChild == TreeNode::NULL_TREE_NODE); assert(rightChild == TreeNode::NULL_TREE_NODE); assert(pNode->height == 0); - } - else { - + } else { // Check that the children node IDs are valid assert(0 <= leftChild && leftChild < m_numberAllocatedNodes); assert(0 <= rightChild && rightChild < m_numberAllocatedNodes); - // Check that the children nodes have the correct parent node assert(m_nodes[leftChild].parentID == _nodeID); assert(m_nodes[rightChild].parentID == _nodeID); - // Check the height of node int32_t height = 1 + etk::max(m_nodes[leftChild].height, m_nodes[rightChild].height); assert(m_nodes[_nodeID].height == height); - // Check the AABB of the node AABB aabb; aabb.mergeTwoAABBs(m_nodes[leftChild].aabb, m_nodes[rightChild].aabb); assert(aabb.getMin() == m_nodes[_nodeID].aabb.getMin()); assert(aabb.getMax() == m_nodes[_nodeID].aabb.getMax()); - // Recursively check the children nodes checkNode(leftChild); checkNode(rightChild); @@ -784,23 +665,20 @@ void DynamicAABBTree::checkNode(int32_t _nodeID) const { // Compute the height of the tree int32_t DynamicAABBTree::computeHeight() { - return computeHeight(m_rootNodeID); + return computeHeight(m_rootNodeID); } // Compute the height of a given node in the tree int32_t DynamicAABBTree::computeHeight(int32_t _nodeID) { assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes); TreeNode* node = m_nodes + _nodeID; - // If the node is a leaf, its height is zero if (node->isLeaf()) { return 0; } - // Compute the height of the left and right sub-tree int32_t leftHeight = computeHeight(node->children[0]); int32_t rightHeight = computeHeight(node->children[1]); - // Return the height of the node return 1 + etk::max(leftHeight, rightHeight); } diff --git a/ephysics/collision/broadphase/DynamicAABBTree.hpp b/ephysics/collision/broadphase/DynamicAABBTree.hpp index 233e502..b59ee77 100644 --- a/ephysics/collision/broadphase/DynamicAABBTree.hpp +++ b/ephysics/collision/broadphase/DynamicAABBTree.hpp @@ -11,9 +11,7 @@ #include namespace ephysics { - class BroadPhaseAlgorithm; - class BroadPhaseRaycastTestCallback; - struct RaycastTest; + // TODO: to replace this, create a Tree template (multiple child) or TreeRedBlack /** * @brief It represents a node of the dynamic AABB tree. */ diff --git a/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp b/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp index ea56241..6169e47 100644 --- a/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ b/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp @@ -297,7 +297,10 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& _simpl nbVertices++; // Update the upper bound of the penetration depth 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(); if (wDotVSquare < upperBoundSquarePenDepth) { upperBoundSquarePenDepth = wDotVSquare; @@ -333,7 +336,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& _simpl vec3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB); vec3 normal = _vector.safeNormalized(); float penetrationDepth = _vector.length(); - assert(penetrationDepth > 0.0); + EPHY_ASSERT(penetrationDepth >= 0.0, "penetration depth <0"); if (normal.length2() < FLT_EPSILON) { return; } diff --git a/ephysics/collision/narrowphase/EPA/TriangleEPA.hpp b/ephysics/collision/narrowphase/EPA/TriangleEPA.hpp index 8b46abc..7344c62 100644 --- a/ephysics/collision/narrowphase/EPA/TriangleEPA.hpp +++ b/ephysics/collision/narrowphase/EPA/TriangleEPA.hpp @@ -27,7 +27,9 @@ namespace ephysics { public: /// Private copy-constructor 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[1] = _triangle.m_adjacentEdges[1]; m_adjacentEdges[2] = _triangle.m_adjacentEdges[2]; @@ -40,7 +42,9 @@ namespace ephysics { } /// Private assignment operator 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[1] = _triangle.m_adjacentEdges[1]; m_adjacentEdges[2] = _triangle.m_adjacentEdges[2]; diff --git a/ephysics/constraint/ContactPoint.cpp b/ephysics/constraint/ContactPoint.cpp index 3b86380..631cd69 100644 --- a/ephysics/constraint/ContactPoint.cpp +++ b/ephysics/constraint/ContactPoint.cpp @@ -12,24 +12,25 @@ using namespace ephysics; using namespace std; // Constructor -ContactPoint::ContactPoint(const ContactPointInfo& contactInfo) - : m_body1(contactInfo.shape1->getBody()), m_body2(contactInfo.shape2->getBody()), - m_normal(contactInfo.normal), - m_penetrationDepth(contactInfo.penetrationDepth), - m_localPointOnBody1(contactInfo.localPoint1), - m_localPointOnBody2(contactInfo.localPoint2), - m_worldPointOnBody1(contactInfo.shape1->getBody()->getTransform() * - contactInfo.shape1->getLocalToBodyTransform() * - contactInfo.localPoint1), - m_worldPointOnBody2(contactInfo.shape2->getBody()->getTransform() * - contactInfo.shape2->getLocalToBodyTransform() * - contactInfo.localPoint2), - m_isRestingContact(false) { +ContactPoint::ContactPoint(const ContactPointInfo& _contactInfo): + m_body1(_contactInfo.shape1->getBody()), + m_body2(_contactInfo.shape2->getBody()), + m_normal(_contactInfo.normal), + m_penetrationDepth(_contactInfo.penetrationDepth), + m_localPointOnBody1(_contactInfo.localPoint1), + m_localPointOnBody2(_contactInfo.localPoint2), + m_worldPointOnBody1(_contactInfo.shape1->getBody()->getTransform() * + _contactInfo.shape1->getLocalToBodyTransform() * + _contactInfo.localPoint1), + m_worldPointOnBody2(_contactInfo.shape2->getBody()->getTransform() * + _contactInfo.shape2->getLocalToBodyTransform() * + _contactInfo.localPoint2), + m_isRestingContact(false) { m_frictionVectors[0] = vec3(0, 0, 0); m_frictionVectors[1] = vec3(0, 0, 0); - assert(m_penetrationDepth > 0.0); + assert(m_penetrationDepth >= 0.0); }