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

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

@ -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_numberMovedShapes; i++) {
int32_t shapeID = m_movedShapes[i];
if (shapeID == -1) {
for (auto &it: m_movedShapes) {
if (it == -1) {
// impossible case ...
continue;
}
// 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
// this AABB. The method BroadPhase::notifiyOverlappingPair() will be called
// by the dynamic AABB tree for each potential overlapping pair.
m_dynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, [&](int32_t nodeId) mutable {
// If both the nodes are the same, we do not create store the overlapping pair
if (shapeID == nodeId) {
if (it == nodeId) {
return;
}
// 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
// last simulation step
m_numberMovedShapes = 0;
// Reset the array of collision shapes that have move (or have been created) during the last simulation step
m_movedShapes.clear();
// Sort the array of potential overlapping pairs in order to remove duplicate pairs
m_potentialPairs.sort(0,

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

@ -50,16 +50,13 @@ namespace ephysics {
class BroadPhaseAlgorithm {
protected :
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.
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<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<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
/// 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);

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

@ -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<m_numberAllocatedNodes - 1; i++) {
m_nodes[i].nextNodeID = i + 1;
@ -48,10 +44,8 @@ void DynamicAABBTree::init() {
// Clear all the nodes and reset the tree
void DynamicAABBTree::reset() {
// Free the allocated memory for the nodes
free(m_nodes);
// Initialize the tree
init();
}
@ -88,7 +82,6 @@ int32_t DynamicAABBTree::allocateNode() {
// Release a node
void DynamicAABBTree::releaseNode(int32_t _nodeID) {
assert(m_numberNodes > 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);
}

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

@ -11,9 +11,7 @@
#include <etk/Function.hpp>
namespace ephysics {
class BroadPhaseAlgorithm;
class BroadPhaseRaycastTestCallback;
struct RaycastTest;
// TODO: to replace this, create a Tree<T> template (multiple child) or TreeRedBlack<T>
/**
* @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++;
// 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;
}

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

@ -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];

29
ephysics/constraint/ContactPoint.cpp
View File

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