From 247f3e6c11401ffb4ac9456793b83258bc3d2071 Mon Sep 17 00:00:00 2001 From: Edouard DUPIN Date: Fri, 16 Jun 2017 21:34:05 +0200 Subject: [PATCH] [DEV] remove mathematic --- ephysics/body/RigidBody.cpp | 3 + .../broadphase/BroadPhaseAlgorithm.cpp | 15 +- .../collision/broadphase/DynamicAABBTree.cpp | 183 +++++++++--------- ephysics/collision/shapes/AABB.cpp | 14 +- ephysics/collision/shapes/AABB.h | 3 - ephysics/collision/shapes/BoxShape.cpp | 16 +- ephysics/collision/shapes/BoxShape.h | 19 +- ephysics/collision/shapes/CapsuleShape.cpp | 13 +- ephysics/collision/shapes/CapsuleShape.h | 14 +- ephysics/collision/shapes/CollisionShape.cpp | 20 +- ephysics/collision/shapes/CollisionShape.h | 35 +--- ephysics/collision/shapes/ConcaveMeshShape.h | 29 ++- ephysics/collision/shapes/ConeShape.cpp | 25 ++- ephysics/collision/shapes/ConeShape.h | 28 +-- ephysics/constraint/HingeJoint.cpp | 26 +-- ephysics/constraint/SliderJoint.cpp | 8 +- ephysics/debug.cpp | 12 ++ ephysics/debug.hpp | 37 ++++ ephysics/engine/ContactSolver.cpp | 56 +++--- ephysics/engine/DynamicsWorld.cpp | 65 ++++--- ephysics/mathematics/mathematics_functions.h | 1 + lutin_ephysics.py | 2 + test/tests/collision/TestDynamicAABBTree.h | 40 ++-- .../scenes/concavemesh/ConcaveMeshScene.cpp | 2 +- 24 files changed, 320 insertions(+), 346 deletions(-) create mode 100644 ephysics/debug.cpp create mode 100644 ephysics/debug.hpp diff --git a/ephysics/body/RigidBody.cpp b/ephysics/body/RigidBody.cpp index 5588087..2e34ccd 100644 --- a/ephysics/body/RigidBody.cpp +++ b/ephysics/body/RigidBody.cpp @@ -9,6 +9,7 @@ #include #include #include +#include // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; @@ -310,7 +311,9 @@ void RigidBody::updateBroadPhaseState() const { // Recompute the world-space AABB of the collision shape AABB aabb; + EPHY_ERROR(" : " << aabb.getMin() << " " << aabb.getMax()); shape->getCollisionShape()->computeAABB(aabb, m_transform *shape->getLocalToBodyTransform()); + EPHY_ERROR(" : " << aabb.getMin() << " " << aabb.getMax()); // Update the broad-phase state for the proxy collision shape m_world.m_collisionDetection.updateProxyCollisionShape(shape, aabb, displacement); diff --git a/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp b/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp index 0acc76d..cbf7c07 100644 --- a/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp @@ -123,20 +123,17 @@ void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) { } // Notify the broad-phase that a collision shape has moved and need to be updated -void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb, - const vec3& displacement, bool forceReinsert) { - - int32_t broadPhaseID = proxyShape->m_broadPhaseID; - +void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* _proxyShape, + const AABB& _aabb, + const vec3& _displacement, + bool _forceReinsert) { + int32_t broadPhaseID = _proxyShape->m_broadPhaseID; assert(broadPhaseID >= 0); - // Update the dynamic AABB tree according to the movement of the collision shape - bool hasBeenReInserted = m_dynamicAABBTree.updateObject(broadPhaseID, aabb, displacement, forceReinsert); - + bool hasBeenReInserted = m_dynamicAABBTree.updateObject(broadPhaseID, _aabb, _displacement, _forceReinsert); // If the collision shape has moved out of its fat AABB (and therefore has been reinserted // int32_to the tree). if (hasBeenReInserted) { - // Add the collision shape int32_to the array of shapes that have moved (or have been created) // during the last simulation step addMovedCollisionShape(broadPhaseID); diff --git a/ephysics/collision/broadphase/DynamicAABBTree.cpp b/ephysics/collision/broadphase/DynamicAABBTree.cpp index bf6b616..7b546a7 100644 --- a/ephysics/collision/broadphase/DynamicAABBTree.cpp +++ b/ephysics/collision/broadphase/DynamicAABBTree.cpp @@ -9,6 +9,7 @@ #include #include #include +#include using namespace reactphysics3d; @@ -62,12 +63,9 @@ void DynamicAABBTree::reset() { // Allocate and return a new node in the tree int32_t DynamicAABBTree::allocateNode() { - // If there is no more allocated node to use if (m_freeNodeID == TreeNode::NULL_TREE_NODE) { - assert(m_numberNodes == m_numberAllocatedNodes); - // Allocate more nodes in the tree m_numberAllocatedNodes *= 2; TreeNode* oldNodes = m_nodes; @@ -75,7 +73,6 @@ int32_t DynamicAABBTree::allocateNode() { assert(m_nodes); memcpy(m_nodes, oldNodes, m_numberNodes * sizeof(TreeNode)); free(oldNodes); - // Initialize the allocated nodes for (int32_t i=m_numberNodes; i 0); - assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes); - assert(m_nodes[nodeID].height >= 0); - m_nodes[nodeID].nextNodeID = m_freeNodeID; - m_nodes[nodeID].height = -1; - m_freeNodeID = nodeID; + assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes); + assert(m_nodes[_nodeID].height >= 0); + m_nodes[_nodeID].nextNodeID = m_freeNodeID; + m_nodes[_nodeID].height = -1; + m_freeNodeID = _nodeID; m_numberNodes--; } @@ -112,37 +107,38 @@ void DynamicAABBTree::releaseNode(int32_t nodeID) { int32_t DynamicAABBTree::addObjectInternal(const AABB& aabb) { // 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 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); + 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; + m_nodes[_nodeID].height = 0; // Insert the new leaf node in the tree - insertLeafNode(nodeID); - assert(m_nodes[nodeID].isLeaf()); + insertLeafNode(_nodeID); + assert(m_nodes[_nodeID].isLeaf()); - assert(nodeID >= 0); + assert(_nodeID >= 0); // Return the Id of the node - return nodeID; + return _nodeID; } // 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(m_nodes[nodeID].isLeaf()); + assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes); + assert(m_nodes[_nodeID].isLeaf()); // Remove the node from the tree - removeLeafNode(nodeID); - releaseNode(nodeID); + removeLeafNode(_nodeID); + releaseNode(_nodeID); } + // Update the dynamic tree after an object has moved. /// If the new AABB of the object that has moved is still inside its fat AABB, then /// nothing is done. Otherwise, the corresponding node is removed and reinserted int32_to the tree. @@ -150,49 +146,56 @@ void DynamicAABBTree::removeObject(int32_t nodeID) { /// argument is the linear velocity of the AABB multiplied by the elapsed time between two /// 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) { +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); + 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 - if (!forceReinsert && m_nodes[nodeID].aabb.contains(newAABB)) { + if ( _forceReinsert == false + && m_nodes[_nodeID].aabb.contains(_newAABB)) { return false; } // 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 - m_nodes[nodeID].aabb = newAABB; + 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; + 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()); + 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()); } else { - m_nodes[nodeID].aabb.m_maxCoordinates.setY(m_nodes[nodeID].aabb.m_maxCoordinates.x() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x()); + m_nodes[_nodeID].aabb.m_maxCoordinates.setX(m_nodes[_nodeID].aabb.m_maxCoordinates.x() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *_displacement.x()); } - if (displacement.y() < 0.0f) { - m_nodes[nodeID].aabb.m_minCoordinates.setY(m_nodes[nodeID].aabb.m_minCoordinates.y() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y()); + if (_displacement.y() < 0.0f) { + m_nodes[_nodeID].aabb.m_minCoordinates.setY(m_nodes[_nodeID].aabb.m_minCoordinates.y() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *_displacement.y()); } else { - m_nodes[nodeID].aabb.m_maxCoordinates.setY(m_nodes[nodeID].aabb.m_maxCoordinates.y() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y()); + m_nodes[_nodeID].aabb.m_maxCoordinates.setY(m_nodes[_nodeID].aabb.m_maxCoordinates.y() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *_displacement.y()); } - if (displacement.z() < 0.0f) { - m_nodes[nodeID].aabb.m_minCoordinates.setZ(m_nodes[nodeID].aabb.m_minCoordinates.z() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z()); + if (_displacement.z() < 0.0f) { + m_nodes[_nodeID].aabb.m_minCoordinates.setZ(m_nodes[_nodeID].aabb.m_minCoordinates.z() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *_displacement.z()); } else { - m_nodes[nodeID].aabb.m_maxCoordinates.setZ(m_nodes[nodeID].aabb.m_maxCoordinates.z() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z()); + m_nodes[_nodeID].aabb.m_maxCoordinates.setZ(m_nodes[_nodeID].aabb.m_maxCoordinates.z() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *_displacement.z()); } - - assert(m_nodes[nodeID].aabb.contains(newAABB)); + EPHY_ERROR(" compare : " << m_nodes[_nodeID].aabb.m_minCoordinates << " " << m_nodes[_nodeID].aabb.m_maxCoordinates); + EPHY_ERROR(" : " << _newAABB.m_minCoordinates << " " << _newAABB.m_maxCoordinates); + if (m_nodes[_nodeID].aabb.contains(_newAABB) == false) { + //EPHY_CRITICAL("ERROR"); + } + assert(m_nodes[_nodeID].aabb.contains(_newAABB)); // Reinsert the node int32_to the tree - insertLeafNode(nodeID); + insertLeafNode(_nodeID); return true; } @@ -200,11 +203,11 @@ bool DynamicAABBTree::updateObject(int32_t nodeID, const AABB& newAABB, const ve // Insert a leaf node in the tree. The process of inserting a new leaf node // in the dynamic tree is described in the book "Introduction to Game Physics // with Box2D" by Ian Parberry. -void DynamicAABBTree::insertLeafNode(int32_t nodeID) { +void DynamicAABBTree::insertLeafNode(int32_t _nodeID) { // If the tree is empty if (m_rootNodeID == TreeNode::NULL_TREE_NODE) { - m_rootNodeID = nodeID; + m_rootNodeID = _nodeID; m_nodes[m_rootNodeID].parentID = TreeNode::NULL_TREE_NODE; return; } @@ -212,7 +215,7 @@ void DynamicAABBTree::insertLeafNode(int32_t nodeID) { assert(m_rootNodeID != TreeNode::NULL_TREE_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; while (!m_nodes[currentNodeID].isLeaf()) { @@ -288,26 +291,26 @@ void DynamicAABBTree::insertLeafNode(int32_t nodeID) { m_nodes[oldParentNode].children[1] = newParentNode; } 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[nodeID].parentID = newParentNode; + m_nodes[_nodeID].parentID = newParentNode; } else { // If the sibling node was the root node 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[nodeID].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; + 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[_nodeID].isLeaf()); assert(!m_nodes[currentNodeID].isLeaf()); int32_t leftChild = m_nodes[currentNodeID].children[0]; @@ -326,25 +329,25 @@ void DynamicAABBTree::insertLeafNode(int32_t nodeID) { currentNodeID = m_nodes[currentNodeID].parentID; } - assert(m_nodes[nodeID].isLeaf()); + assert(m_nodes[_nodeID].isLeaf()); } // 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(m_nodes[nodeID].isLeaf()); + 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) { + if (m_rootNodeID == _nodeID) { m_rootNodeID = TreeNode::NULL_TREE_NODE; return; } - int32_t parentNodeID = m_nodes[nodeID].parentID; + int32_t parentNodeID = m_nodes[_nodeID].parentID; int32_t grandParentNodeID = m_nodes[parentNodeID].parentID; int32_t siblingNodeID; - if (m_nodes[parentNodeID].children[0] == nodeID) { + if (m_nodes[parentNodeID].children[0] == _nodeID) { siblingNodeID = m_nodes[parentNodeID].children[1]; } else { @@ -401,17 +404,17 @@ void DynamicAABBTree::removeLeafNode(int32_t nodeID) { // Balance the sub-tree of a given node using left or right rotations. /// 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) { +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 (nodeA->isLeaf() || nodeA->height < 2) { // Do not perform any rotation - return nodeID; + return _nodeID; } // Get the two children nodes @@ -437,17 +440,17 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) { TreeNode* nodeF = m_nodes + nodeFID; TreeNode* nodeG = m_nodes + nodeGID; - nodeC->children[0] = nodeID; + 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) { + if (m_nodes[nodeC->parentID].children[0] == _nodeID) { m_nodes[nodeC->parentID].children[0] = nodeCID; } else { - assert(m_nodes[nodeC->parentID].children[1] == nodeID); + assert(m_nodes[nodeC->parentID].children[1] == _nodeID); m_nodes[nodeC->parentID].children[1] = nodeCID; } } @@ -463,7 +466,7 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) { nodeC->children[1] = nodeFID; nodeA->children[1] = nodeGID; - nodeG->parentID = nodeID; + nodeG->parentID = _nodeID; // Recompute the AABB of node A and C nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeG->aabb); @@ -478,7 +481,7 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) { 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; + nodeF->parentID = _nodeID; // Recompute the AABB of node A and C nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeF->aabb); @@ -507,17 +510,17 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) { TreeNode* nodeF = m_nodes + nodeFID; TreeNode* nodeG = m_nodes + nodeGID; - nodeB->children[0] = nodeID; + 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) { + if (m_nodes[nodeB->parentID].children[0] == _nodeID) { m_nodes[nodeB->parentID].children[0] = nodeBID; } else { - assert(m_nodes[nodeB->parentID].children[1] == nodeID); + assert(m_nodes[nodeB->parentID].children[1] == _nodeID); m_nodes[nodeB->parentID].children[1] = nodeBID; } } @@ -533,7 +536,7 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) { nodeB->children[1] = nodeFID; nodeA->children[0] = nodeGID; - nodeG->parentID = nodeID; + nodeG->parentID = _nodeID; // Recompute the AABB of node A and B nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeG->aabb); @@ -548,7 +551,7 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) { 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; + nodeF->parentID = _nodeID; // Recompute the AABB of node A and B nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeF->aabb); @@ -566,7 +569,7 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) { } // If the sub-tree is balanced, return the current root node - return nodeID; + return _nodeID; } /// Report all shapes overlapping with the AABB given in parameter. @@ -692,17 +695,17 @@ void DynamicAABBTree::check() const { } // 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) 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); + if (_nodeID == m_rootNodeID) { + assert(m_nodes[_nodeID].parentID == TreeNode::NULL_TREE_NODE); } // Get the children nodes - TreeNode* pNode = m_nodes + nodeID; + TreeNode* pNode = m_nodes + _nodeID; assert(!pNode->isLeaf()); int32_t leftChild = pNode->children[0]; int32_t rightChild = pNode->children[1]; @@ -725,18 +728,18 @@ void DynamicAABBTree::checkNode(int32_t nodeID) const { 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); + assert(m_nodes[leftChild].parentID == _nodeID); + assert(m_nodes[rightChild].parentID == _nodeID); // Check the height of node int32_t height = 1 + std::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 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()); + assert(aabb.getMin() == m_nodes[_nodeID].aabb.getMin()); + assert(aabb.getMax() == m_nodes[_nodeID].aabb.getMax()); // Recursively check the children nodes checkNode(leftChild); @@ -750,9 +753,9 @@ int32_t DynamicAABBTree::computeHeight() { } // 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; +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()) { diff --git a/ephysics/collision/shapes/AABB.cpp b/ephysics/collision/shapes/AABB.cpp index 24eb4ee..b53e42a 100644 --- a/ephysics/collision/shapes/AABB.cpp +++ b/ephysics/collision/shapes/AABB.cpp @@ -13,30 +13,24 @@ using namespace reactphysics3d; using namespace std; -// Constructor -AABB::AABB() { - +AABB::AABB(): + m_minCoordinates(0,0,0), + m_maxCoordinates(0,0,0) { + } -// Constructor AABB::AABB(const vec3& minCoordinates, const vec3& maxCoordinates): m_minCoordinates(minCoordinates), m_maxCoordinates(maxCoordinates) { } -// Copy-constructor AABB::AABB(const AABB& aabb): m_minCoordinates(aabb.m_minCoordinates), m_maxCoordinates(aabb.m_maxCoordinates) { } -// Destructor -AABB::~AABB() { - -} - // Merge the AABB in parameter with the current one void AABB::mergeWithAABB(const AABB& aabb) { m_minCoordinates.setX(std::min(m_minCoordinates.x(), aabb.m_minCoordinates.x())); diff --git a/ephysics/collision/shapes/AABB.h b/ephysics/collision/shapes/AABB.h index d7e84b5..29bc8e1 100644 --- a/ephysics/collision/shapes/AABB.h +++ b/ephysics/collision/shapes/AABB.h @@ -43,9 +43,6 @@ class AABB { /// Copy-constructor AABB(const AABB& aabb); - /// Destructor - ~AABB(); - /// Return the center point vec3 getCenter() const; diff --git a/ephysics/collision/shapes/BoxShape.cpp b/ephysics/collision/shapes/BoxShape.cpp index 4a71586..144a9ee 100644 --- a/ephysics/collision/shapes/BoxShape.cpp +++ b/ephysics/collision/shapes/BoxShape.cpp @@ -18,16 +18,12 @@ using namespace reactphysics3d; * @param extent The vector with the three extents of the box (in meters) * @param margin The collision margin (in meters) around the collision shape */ -BoxShape::BoxShape(const vec3& extent, float margin) - : ConvexShape(BOX, margin), m_extent(extent - vec3(margin, margin, margin)) { - assert(extent.x() > 0.0f && extent.x() > margin); - assert(extent.y() > 0.0f && extent.y() > margin); - assert(extent.z() > 0.0f && extent.z() > margin); -} - -// Destructor -BoxShape::~BoxShape() { - +BoxShape::BoxShape(const vec3& _extent, float _margin): + ConvexShape(BOX, _margin), + m_extent(_extent - vec3(_margin, _margin, _margin)) { + assert(_extent.x() > 0.0f && _extent.x() > _margin); + assert(_extent.y() > 0.0f && _extent.y() > _margin); + assert(_extent.z() > 0.0f && _extent.z() > _margin); } // Return the local inertia tensor of the collision shape diff --git a/ephysics/collision/shapes/BoxShape.h b/ephysics/collision/shapes/BoxShape.h index 7647f1f..8c2cb07 100644 --- a/ephysics/collision/shapes/BoxShape.h +++ b/ephysics/collision/shapes/BoxShape.h @@ -32,14 +32,8 @@ namespace reactphysics3d { class BoxShape : public ConvexShape { protected : - - // -------------------- Attributes -------------------- // - /// Extent sizes of the box in the x, y and z direction vec3 m_extent; - - // -------------------- Methods -------------------- // - /// Private copy-constructor BoxShape(const BoxShape& shape); @@ -60,14 +54,11 @@ class BoxShape : public ConvexShape { virtual size_t getSizeInBytes() const; public : - - // -------------------- Methods -------------------- // - /// Constructor BoxShape(const vec3& extent, float margin = OBJECT_MARGIN); /// Destructor - virtual ~BoxShape(); + virtual ~BoxShape() = default; /// Return the extents of the box vec3 getExtent() const; @@ -76,7 +67,7 @@ class BoxShape : public ConvexShape { virtual void setLocalScaling(const vec3& scaling); /// Return the local bounds of the shape in x, y and z directions - virtual void getLocalBounds(vec3& min, vec3& max) const; + virtual void getLocalBounds(vec3& _min, vec3& _max) const; /// Return the local inertia tensor of the collision shape virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; @@ -104,13 +95,13 @@ inline void BoxShape::setLocalScaling(const vec3& scaling) { * @param min The minimum bounds of the shape in local-space coordinates * @param max The maximum bounds of the shape in local-space coordinates */ -inline void BoxShape::getLocalBounds(vec3& min, vec3& max) const { +inline void BoxShape::getLocalBounds(vec3& _min, vec3& _max) const { // Maximum bounds - max = m_extent + vec3(m_margin, m_margin, m_margin); + _max = m_extent + vec3(m_margin, m_margin, m_margin); // Minimum bounds - min = -max; + _min = -_max; } // Return the number of bytes used by the collision shape diff --git a/ephysics/collision/shapes/CapsuleShape.cpp b/ephysics/collision/shapes/CapsuleShape.cpp index dfe2195..036163b 100644 --- a/ephysics/collision/shapes/CapsuleShape.cpp +++ b/ephysics/collision/shapes/CapsuleShape.cpp @@ -14,13 +14,14 @@ using namespace reactphysics3d; // Constructor /** - * @param radius The radius of the capsule (in meters) - * @param height The height of the capsule (in meters) + * @param _radius The radius of the capsule (in meters) + * @param _height The height of the capsule (in meters) */ -CapsuleShape::CapsuleShape(float radius, float height) - : ConvexShape(CAPSULE, radius), m_halfHeight(height * 0.5f) { - assert(radius > 0.0f); - assert(height > 0.0f); +CapsuleShape::CapsuleShape(float _radius, float _height): + ConvexShape(CAPSULE, _radius), + m_halfHeight(_height * 0.5f) { + assert(_radius > 0.0f); + assert(_height > 0.0f); } // Destructor diff --git a/ephysics/collision/shapes/CapsuleShape.h b/ephysics/collision/shapes/CapsuleShape.h index 50bda94..c769edb 100644 --- a/ephysics/collision/shapes/CapsuleShape.h +++ b/ephysics/collision/shapes/CapsuleShape.h @@ -24,16 +24,9 @@ namespace reactphysics3d { * capsule shape. */ class CapsuleShape : public ConvexShape { - - protected : - - // -------------------- Attributes -------------------- // - + protected: /// Half height of the capsule (height = distance between the centers of the two spheres) float m_halfHeight; - - // -------------------- Methods -------------------- // - /// Private copy-constructor CapsuleShape(const CapsuleShape& shape); @@ -59,11 +52,8 @@ class CapsuleShape : public ConvexShape { virtual size_t getSizeInBytes() const; public : - - // -------------------- Methods -------------------- // - /// Constructor - CapsuleShape(float radius, float height); + CapsuleShape(float _radius, float _height); /// Destructor virtual ~CapsuleShape(); diff --git a/ephysics/collision/shapes/CollisionShape.cpp b/ephysics/collision/shapes/CollisionShape.cpp index c258582..5df8bec 100644 --- a/ephysics/collision/shapes/CollisionShape.cpp +++ b/ephysics/collision/shapes/CollisionShape.cpp @@ -12,14 +12,14 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -// Constructor -CollisionShape::CollisionShape(CollisionShapeType type) : m_type(type), m_scaling(1.0, 1.0, 1.0) { +CollisionShape::CollisionShape(CollisionShapeType type) : + m_type(type), + m_scaling(1.0f, 1.0f, 1.0f) { } -// Destructor CollisionShape::~CollisionShape() { - + } // Compute the world-space AABB of the collision shape given a transform @@ -33,18 +33,18 @@ void CollisionShape::computeAABB(AABB& aabb, const etk::Transform3D& transform) PROFILE("CollisionShape::computeAABB()"); // Get the local bounds in x,y and z direction - vec3 minBounds; - vec3 maxBounds; + vec3 minBounds(0,0,0); + vec3 maxBounds(0,0,0); getLocalBounds(minBounds, maxBounds); // Rotate the local bounds according to the orientation of the body etk::Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsolute(); vec3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds), - worldAxis.getColumn(1).dot(minBounds), - worldAxis.getColumn(2).dot(minBounds)); + worldAxis.getColumn(1).dot(minBounds), + worldAxis.getColumn(2).dot(minBounds)); vec3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds), - worldAxis.getColumn(1).dot(maxBounds), - worldAxis.getColumn(2).dot(maxBounds)); + worldAxis.getColumn(1).dot(maxBounds), + worldAxis.getColumn(2).dot(maxBounds)); // Compute the minimum and maximum coordinates of the rotated extents vec3 minCoordinates = transform.getPosition() + worldMinBounds; diff --git a/ephysics/collision/shapes/CollisionShape.h b/ephysics/collision/shapes/CollisionShape.h index e411303..59f386d 100644 --- a/ephysics/collision/shapes/CollisionShape.h +++ b/ephysics/collision/shapes/CollisionShape.h @@ -33,74 +33,43 @@ class CollisionBody; * body that is used during the narrow-phase collision detection. */ class CollisionShape { - protected : - - // -------------------- Attributes -------------------- // - - /// Type of the collision shape - CollisionShapeType m_type; - - /// Scaling vector of the collision shape - vec3 m_scaling; - - // -------------------- Methods -------------------- // - + CollisionShapeType m_type; //!< Type of the collision shape + vec3 m_scaling; //!< Scaling vector of the collision shape /// Private copy-constructor CollisionShape(const CollisionShape& shape); - /// Private assignment operator CollisionShape& operator=(const CollisionShape& shape); - /// Return true if a point is inside the collision shape virtual bool testPointInside(const vec3& worldPoint, ProxyShape* proxyShape) const=0; - /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const=0; - /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const = 0; - public : - - // -------------------- Methods -------------------- // - /// Constructor CollisionShape(CollisionShapeType type); - /// Destructor virtual ~CollisionShape(); - /// Return the type of the collision shapes CollisionShapeType getType() const; - /// Return true if the collision shape is convex, false if it is concave virtual bool isConvex() const=0; - /// Return the local bounds of the shape in x, y and z directions virtual void getLocalBounds(vec3& min, vec3& max) const=0; - /// Return the scaling vector of the collision shape vec3 getScaling() const; - /// Set the local scaling vector of the collision shape virtual void setLocalScaling(const vec3& scaling); - /// Return the local inertia tensor of the collision shapes virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const=0; - /// Compute the world-space AABB of the collision shape given a transform virtual void computeAABB(AABB& aabb, const etk::Transform3D& transform) const; - /// Return true if the collision shape type is a convex shape static bool isConvex(CollisionShapeType shapeType); - /// Return the maximum number of contact manifolds in an overlapping pair given two shape types static int32_t computeNbMaxContactManifolds(CollisionShapeType shapeType1, CollisionShapeType shapeType2); - - // -------------------- Friendship -------------------- // - friend class ProxyShape; friend class CollisionWorld; }; diff --git a/ephysics/collision/shapes/ConcaveMeshShape.h b/ephysics/collision/shapes/ConcaveMeshShape.h index c500a4d..14f3ae6 100644 --- a/ephysics/collision/shapes/ConcaveMeshShape.h +++ b/ephysics/collision/shapes/ConcaveMeshShape.h @@ -16,31 +16,24 @@ namespace reactphysics3d { class ConcaveMeshShape; -// class ConvexTriangleAABBOverlapCallback class ConvexTriangleAABBOverlapCallback : public DynamicAABBTreeOverlapCallback { - private: - - TriangleCallback& m_triangleTestCallback; - - // Reference to the concave mesh shape - const ConcaveMeshShape& m_concaveMeshShape; - - // Reference to the Dynamic AABB tree - const DynamicAABBTree& m_dynamicAABBTree; - + TriangleCallback& m_triangleTestCallback; //!< + const ConcaveMeshShape& m_concaveMeshShape; //!< Reference to the concave mesh shape + const DynamicAABBTree& m_dynamicAABBTree; //!< Reference to the Dynamic AABB tree public: - // Constructor - ConvexTriangleAABBOverlapCallback(TriangleCallback& triangleCallback, const ConcaveMeshShape& concaveShape, - const DynamicAABBTree& dynamicAABBTree) - : m_triangleTestCallback(triangleCallback), m_concaveMeshShape(concaveShape), m_dynamicAABBTree(dynamicAABBTree) { - + ConvexTriangleAABBOverlapCallback(TriangleCallback& _triangleCallback, + const ConcaveMeshShape& _concaveShape, + const DynamicAABBTree& _dynamicAABBTree): + m_triangleTestCallback(_triangleCallback), + m_concaveMeshShape(_concaveShape), + m_dynamicAABBTree(_dynamicAABBTree) { + } - // Called when a overlapping node has been found during the call to // DynamicAABBTree:reportAllShapesOverlappingWithAABB() - virtual void notifyOverlappingNode(int32_t nodeId); + virtual void notifyOverlappingNode(int32_t _nodeId); }; diff --git a/ephysics/collision/shapes/ConeShape.cpp b/ephysics/collision/shapes/ConeShape.cpp index 9da6154..7019306 100644 --- a/ephysics/collision/shapes/ConeShape.cpp +++ b/ephysics/collision/shapes/ConeShape.cpp @@ -18,18 +18,15 @@ using namespace reactphysics3d; * @param height Height of the cone (in meters) * @param margin Collision margin (in meters) around the collision shape */ -ConeShape::ConeShape(float radius, float height, float margin) - : ConvexShape(CONE, margin), mRadius(radius), m_halfHeight(height * 0.5f) { - assert(mRadius > 0.0f); +ConeShape::ConeShape(float radius, float height, float margin): + ConvexShape(CONE, margin), + m_radius(radius), + m_halfHeight(height * 0.5f) { + assert(m_radius > 0.0f); assert(m_halfHeight > 0.0f); // Compute the sine of the semi-angle at the apex point - mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height)); -} - -// Destructor -ConeShape::~ConeShape() { - + m_sinTheta = m_radius / (sqrt(m_radius * m_radius + height * height)); } // Return a local support point in a given direction without the object margin @@ -37,7 +34,7 @@ vec3 ConeShape::getLocalSupportPointWithoutMargin(const vec3& direction, void** cachedCollisionData) const { const vec3& v = direction; - float sinThetaTimesLengthV = mSinTheta * v.length(); + float sinThetaTimesLengthV = m_sinTheta * v.length(); vec3 supportPoint; if (v.y() > sinThetaTimesLengthV) { @@ -46,7 +43,7 @@ vec3 ConeShape::getLocalSupportPointWithoutMargin(const vec3& direction, else { float projectedLength = sqrt(v.x() * v.x() + v.z() * v.z()); if (projectedLength > MACHINE_EPSILON) { - float d = mRadius / projectedLength; + float d = m_radius / projectedLength; supportPoint = vec3(v.x() * d, -m_halfHeight, v.z() * d); } else { @@ -70,7 +67,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr vec3 centerBase(0, -m_halfHeight, 0); vec3 axis(0, float(-1.0), 0); float heightSquare = float(4.0) * m_halfHeight * m_halfHeight; - float cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius); + float cosThetaSquare = heightSquare / (heightSquare + m_radius * m_radius); float factor = 1.0f - cosThetaSquare; vec3 delta = ray.point1 - V; float c0 = -cosThetaSquare * delta.x() * delta.x() + factor * delta.y() * delta.y() - @@ -148,7 +145,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr // Only keep this int32_tersection if it is inside the cone radius localHitPoint[2] = ray.point1 + tHit[2] * r; - if ((localHitPoint[2] - centerBase).length2() > mRadius * mRadius) { + if ((localHitPoint[2] - centerBase).length2() > m_radius * m_radius) { tHit[2] = float(-1.0); } @@ -175,7 +172,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr float h = float(2.0) * m_halfHeight; float value1 = (localHitPoint[hitIndex].x() * localHitPoint[hitIndex].x() + localHitPoint[hitIndex].z() * localHitPoint[hitIndex].z()); - float rOverH = mRadius / h; + float rOverH = m_radius / h; float value2 = 1.0f + rOverH * rOverH; float factor = 1.0f / std::sqrt(value1 * value2); float x = localHitPoint[hitIndex].x() * factor; diff --git a/ephysics/collision/shapes/ConeShape.h b/ephysics/collision/shapes/ConeShape.h index 7b49599..f9be0c4 100644 --- a/ephysics/collision/shapes/ConeShape.h +++ b/ephysics/collision/shapes/ConeShape.h @@ -29,22 +29,13 @@ namespace reactphysics3d { * default margin distance by not using the "margin" parameter in the constructor. */ class ConeShape : public ConvexShape { - protected : - - // -------------------- Attributes -------------------- // - /// Radius of the base - float mRadius; - + float m_radius; /// Half height of the cone float m_halfHeight; - /// sine of the semi angle at the apex point - float mSinTheta; - - // -------------------- Methods -------------------- // - + float m_sinTheta; /// Private copy-constructor ConeShape(const ConeShape& shape); @@ -69,10 +60,7 @@ class ConeShape : public ConvexShape { // -------------------- Methods -------------------- // /// Constructor - ConeShape(float mRadius, float height, float margin = OBJECT_MARGIN); - - /// Destructor - virtual ~ConeShape(); + ConeShape(float m_radius, float height, float margin = OBJECT_MARGIN); /// Return the radius float getRadius() const; @@ -95,7 +83,7 @@ class ConeShape : public ConvexShape { * @return Radius of the cone (in meters) */ inline float ConeShape::getRadius() const { - return mRadius; + return m_radius; } // Return the height @@ -110,7 +98,7 @@ inline float ConeShape::getHeight() const { inline void ConeShape::setLocalScaling(const vec3& scaling) { m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y(); - mRadius = (mRadius / m_scaling.x()) * scaling.x(); + m_radius = (m_radius / m_scaling.x()) * scaling.x(); CollisionShape::setLocalScaling(scaling); } @@ -128,7 +116,7 @@ inline size_t ConeShape::getSizeInBytes() const { inline void ConeShape::getLocalBounds(vec3& min, vec3& max) const { // Maximum bounds - max.setX(mRadius + m_margin); + max.setX(m_radius + m_margin); max.setY(m_halfHeight + m_margin); max.setZ(max.x()); @@ -145,7 +133,7 @@ inline void ConeShape::getLocalBounds(vec3& min, vec3& max) const { * @param mass Mass to use to compute the inertia tensor of the collision shape */ inline void ConeShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { - float rSquare = mRadius * mRadius; + float rSquare = m_radius * m_radius; float diagXZ = float(0.15) * mass * (rSquare + m_halfHeight); tensor.setValue(diagXZ, 0.0, 0.0, 0.0, float(0.3) * mass * rSquare, @@ -154,7 +142,7 @@ inline void ConeShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float m // Return true if a point is inside the collision shape inline bool ConeShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { - const float radiusHeight = mRadius * (-localPoint.y() + m_halfHeight) / + const float radiusHeight = m_radius * (-localPoint.y() + m_halfHeight) / (m_halfHeight * float(2.0)); return (localPoint.y() < m_halfHeight && localPoint.y() > -m_halfHeight) && (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z() < radiusHeight *radiusHeight); diff --git a/ephysics/constraint/HingeJoint.cpp b/ephysics/constraint/HingeJoint.cpp index 72830ce..5b072f1 100644 --- a/ephysics/constraint/HingeJoint.cpp +++ b/ephysics/constraint/HingeJoint.cpp @@ -93,7 +93,7 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat vec3 a2 = orientationBody2 * mHingeLocalAxisBody2; mA1.normalize(); a2.normalize(); - const vec3 b2 = a2.getOneUnitOrthogonalVector(); + const vec3 b2 = a2.getOrthoVector(); const vec3 c2 = a2.cross(b2); mB2CrossA1 = b2.cross(mA1); mC2CrossA1 = c2.cross(mA1); @@ -104,11 +104,11 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix) float inverseMassBodies = m_body1->m_massInverse + m_body2->m_massInverse; - etk::Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, - 0, inverseMassBodies, 0, - 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose(); + etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0, + 0, inverseMassBodies, 0, + 0, 0, inverseMassBodies) + + skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() + + skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose(); m_inverseMassMatrixTranslation.setZero(); if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { m_inverseMassMatrixTranslation = massMatrix.getInverse(); @@ -423,7 +423,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS vec3 a2 = q2 * mHingeLocalAxisBody2; mA1.normalize(); a2.normalize(); - const vec3 b2 = a2.getOneUnitOrthogonalVector(); + const vec3 b2 = a2.getOrthoVector(); const vec3 c2 = a2.cross(b2); mB2CrossA1 = b2.cross(mA1); mC2CrossA1 = c2.cross(mA1); @@ -436,11 +436,11 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints float inverseMassBodies = m_body1->m_massInverse + m_body2->m_massInverse; - etk::Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, - 0, inverseMassBodies, 0, - 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose(); + etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0, + 0, inverseMassBodies, 0, + 0, 0, inverseMassBodies) + + skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() + + skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose(); m_inverseMassMatrixTranslation.setZero(); if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { m_inverseMassMatrixTranslation = massMatrix.getInverse(); @@ -762,7 +762,7 @@ float HingeJoint::computeCurrentHingeAngle(const etk::Quaternion& orientationBod // axis is not pointing in the same direction as the hinge axis, we use the rotation -q which // has the same |sin(theta/2)| value but the value cos(theta/2) is sign inverted. Some details // about this trick is explained in the source code of OpenTissue (http://www.opentissue.org). - float cosHalfAngle = relativeRotation.w; + float cosHalfAngle = relativeRotation.w(); float sinHalfAngleAbs = relativeRotation.getVectorV().length(); // Compute the dot product of the relative rotation axis and the hinge axis diff --git a/ephysics/constraint/SliderJoint.cpp b/ephysics/constraint/SliderJoint.cpp index 896fbb6..a9fd8fe 100644 --- a/ephysics/constraint/SliderJoint.cpp +++ b/ephysics/constraint/SliderJoint.cpp @@ -76,7 +76,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Compute the two orthogonal vectors to the slider axis in world-space mSliderAxisWorld = orientationBody1 * mSliderAxisBody1; mSliderAxisWorld.normalize(); - mN1 = mSliderAxisWorld.getOneUnitOrthogonalVector(); + mN1 = mSliderAxisWorld.getOrthoVector(); mN2 = mSliderAxisWorld.cross(mN1); // Check if the limit constraints are violated or not @@ -128,8 +128,8 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mBTranslation.setZero(); float biasFactor = (BETA / constraintSolverData.timeStep); if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBTranslation.x() = u.dot(mN1); - mBTranslation.y() = u.dot(mN2); + mBTranslation.setX(u.dot(mN1)); + mBTranslation.setY(u.dot(mN2)); mBTranslation *= biasFactor; } @@ -440,7 +440,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // Compute the two orthogonal vectors to the slider axis in world-space mSliderAxisWorld = q1 * mSliderAxisBody1; mSliderAxisWorld.normalize(); - mN1 = mSliderAxisWorld.getOneUnitOrthogonalVector(); + mN1 = mSliderAxisWorld.getOrthoVector(); mN2 = mSliderAxisWorld.cross(mN1); // Check if the limit constraints are violated or not diff --git a/ephysics/debug.cpp b/ephysics/debug.cpp new file mode 100644 index 0000000..41a816b --- /dev/null +++ b/ephysics/debug.cpp @@ -0,0 +1,12 @@ +/** @file + * @author Edouard DUPIN + * @copyright 2011, Edouard DUPIN, all right reserved + * @license MPL v2.0 (see license file) + */ + +#include + +int32_t ephysic::getLogId() { + static int32_t g_val = elog::registerInstance("ephysic"); + return g_val; +} diff --git a/ephysics/debug.hpp b/ephysics/debug.hpp new file mode 100644 index 0000000..ae851c4 --- /dev/null +++ b/ephysics/debug.hpp @@ -0,0 +1,37 @@ +/** @file + * @author Edouard DUPIN + * @copyright 2011, Edouard DUPIN, all right reserved + * @license MPL v2.0 (see license file) + */ +#pragma once + +#include + +namespace ephysic { + int32_t getLogId(); +}; +#define EPHY_BASE(info,data) ELOG_BASE(ephysic::getLogId(),info,data) + +#define EPHY_PRINT(data) EPHY_BASE(-1, data) +#define EPHY_CRITICAL(data) EPHY_BASE(1, data) +#define EPHY_ERROR(data) EPHY_BASE(2, data) +#define EPHY_WARNING(data) EPHY_BASE(3, data) +#define EPHY_INFO(data) EPHY_BASE(4, data) +#ifdef DEBUG + #define EPHY_DEBUG(data) EPHY_BASE(5, data) + #define EPHY_VERBOSE(data) EPHY_BASE(6, data) + #define EPHY_TODO(data) EPHY_BASE(4, "TODO : " << data) +#else + #define EPHY_DEBUG(data) do { } while(false) + #define EPHY_VERBOSE(data) do { } while(false) + #define EPHY_TODO(data) do { } while(false) +#endif + +#define EPHY_ASSERT(cond,data) \ + do { \ + if (!(cond)) { \ + EPHY_CRITICAL(data); \ + assert(!#cond); \ + } \ + } while (0) + diff --git a/ephysics/engine/ContactSolver.cpp b/ephysics/engine/ContactSolver.cpp index 3485998..ab35b61 100644 --- a/ephysics/engine/ContactSolver.cpp +++ b/ephysics/engine/ContactSolver.cpp @@ -21,8 +21,8 @@ const float ContactSolver::SLOP= float(0.01); // Constructor ContactSolver::ContactSolver(const std::map& mapBodyToVelocityIndex) - :m_splitLinearVelocities(NULL), m_splitAngularVelocities(NULL), - mContactConstraints(NULL), mLinearVelocities(NULL), mAngularVelocities(NULL), + :m_splitLinearVelocities(nullptr), m_splitAngularVelocities(nullptr), + mContactConstraints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr), m_mapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), mIsWarmStartingActive(true), mIsSplitImpulseActive(true), mIsSolveFrictionAtContactManifoldCenterActive(true) { @@ -39,11 +39,11 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { PROFILE("ContactSolver::initializeForIsland()"); - assert(island != NULL); + assert(island != nullptr); assert(island->getNbBodies() > 0); assert(island->getNbContactManifolds() > 0); - assert(m_splitLinearVelocities != NULL); - assert(m_splitAngularVelocities != NULL); + assert(m_splitLinearVelocities != nullptr); + assert(m_splitAngularVelocities != nullptr); // Set the current time step m_timeStep = dt; @@ -51,7 +51,7 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { m_numberContactManifolds = island->getNbContactManifolds(); mContactConstraints = new ContactManifoldSolver[m_numberContactManifolds]; - assert(mContactConstraints != NULL); + assert(mContactConstraints != nullptr); // For each contact manifold of the island ContactManifold** contactManifolds = island->getContactManifold(); @@ -66,8 +66,8 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { // Get the two bodies of the contact RigidBody* body1 = static_cast(externalManifold->getContactPoint(0)->getBody1()); RigidBody* body2 = static_cast(externalManifold->getContactPoint(0)->getBody2()); - assert(body1 != NULL); - assert(body2 != NULL); + assert(body1 != nullptr); + assert(body2 != nullptr); // Get the position of the two bodies const vec3& x1 = body1->m_centerOfMassWorld; @@ -91,8 +91,8 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { // If we solve the friction constraints at the center of the contact manifold if (mIsSolveFrictionAtContactManifoldCenterActive) { - int32_ternalManifold.frictionPointBody1 = vec3::zero(); - int32_ternalManifold.frictionPointBody2 = vec3::zero(); + int32_ternalManifold.frictionPointBody1 = vec3(0.0f,0.0f,0.0f); + int32_ternalManifold.frictionPointBody2 = vec3(0.0f,0.0f,0.0f); } // For each contact point of the contact manifold @@ -119,7 +119,7 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { contactPoint.penetrationImpulse = 0.0; contactPoint.friction1Impulse = 0.0; contactPoint.friction2Impulse = 0.0; - contactPoint.rollingResistanceImpulse = vec3::zero(); + contactPoint.rollingResistanceImpulse = vec3(0.0f,0.0f,0.0f); // If we solve the friction constraints at the center of the contact manifold if (mIsSolveFrictionAtContactManifoldCenterActive) { @@ -384,8 +384,8 @@ void ContactSolver::warmStart() { if (contactManifold.rollingResistanceFactor > 0) { // Compute the impulse P = J^T * lambda - const Impulse impulseRollingResistance(vec3::zero(), -contactPoint.rollingResistanceImpulse, - vec3::zero(), contactPoint.rollingResistanceImpulse); + const Impulse impulseRollingResistance(vec3(0.0f,0.0f,0.0f), -contactPoint.rollingResistanceImpulse, + vec3(0.0f,0.0f,0.0f), contactPoint.rollingResistanceImpulse); // Apply the impulses to the bodies of the constraint applyImpulse(impulseRollingResistance, contactManifold); @@ -398,7 +398,7 @@ void ContactSolver::warmStart() { contactPoint.penetrationImpulse = 0.0; contactPoint.friction1Impulse = 0.0; contactPoint.friction2Impulse = 0.0; - contactPoint.rollingResistanceImpulse = vec3::zero(); + contactPoint.rollingResistanceImpulse = vec3(0.0f,0.0f,0.0f); } } @@ -469,8 +469,8 @@ void ContactSolver::warmStart() { // Compute the impulse P = J^T * lambda angularImpulseBody1 = -contactManifold.rollingResistanceImpulse; angularImpulseBody2 = contactManifold.rollingResistanceImpulse; - const Impulse impulseRollingResistance(vec3::zero(), angularImpulseBody1, - vec3::zero(), angularImpulseBody2); + const Impulse impulseRollingResistance(vec3(0.0f,0.0f,0.0f), angularImpulseBody1, + vec3(0.0f,0.0f,0.0f), angularImpulseBody2); // Apply the impulses to the bodies of the constraint applyImpulse(impulseRollingResistance, contactManifold); @@ -481,7 +481,7 @@ void ContactSolver::warmStart() { contactManifold.friction1Impulse = 0.0; contactManifold.friction2Impulse = 0.0; contactManifold.frictionTwistImpulse = 0.0; - contactManifold.rollingResistanceImpulse = vec3::zero(); + contactManifold.rollingResistanceImpulse = vec3(0.0f,0.0f,0.0f); } } } @@ -640,8 +640,8 @@ void ContactSolver::solve() { deltaLambdaRolling = contactPoint.rollingResistanceImpulse - lambdaTempRolling; // Compute the impulse P=J^T * lambda - const Impulse impulseRolling(vec3::zero(), -deltaLambdaRolling, - vec3::zero(), deltaLambdaRolling); + const Impulse impulseRolling(vec3(0.0f,0.0f,0.0f), -deltaLambdaRolling, + vec3(0.0f,0.0f,0.0f), deltaLambdaRolling); // Apply the impulses to the bodies of the constraint applyImpulse(impulseRolling, contactManifold); @@ -742,15 +742,17 @@ void ContactSolver::solve() { vec3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); float rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse; vec3 lambdaTempRolling = contactManifold.rollingResistanceImpulse; - contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse + - deltaLambdaRolling, rollingLimit); + contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse + deltaLambdaRolling, + rollingLimit); deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling; // Compute the impulse P=J^T * lambda angularImpulseBody1 = -deltaLambdaRolling; angularImpulseBody2 = deltaLambdaRolling; - const Impulse impulseRolling(vec3::zero(), angularImpulseBody1, - vec3::zero(), angularImpulseBody2); + const Impulse impulseRolling(vec3(0.0f,0.0f,0.0f), + angularImpulseBody1, + vec3(0.0f,0.0f,0.0f), + angularImpulseBody2); // Apply the impulses to the bodies of the constraint applyImpulse(impulseRolling, contactManifold); @@ -846,7 +848,7 @@ void ContactSolver::computeFrictionVectors(const vec3& deltaVelocity, else { // Get any orthogonal vector to the normal as the first friction vector - contactPoint.frictionVector1 = contactPoint.normal.getOneUnitOrthogonalVector(); + contactPoint.frictionVector1 = contactPoint.normal.getOrthoVector(); } // The second friction vector is computed by the cross product of the firs @@ -876,7 +878,7 @@ void ContactSolver::computeFrictionVectors(const vec3& deltaVelocity, else { // Get any orthogonal vector to the normal as the first friction vector - contact.frictionVector1 = contact.normal.getOneUnitOrthogonalVector(); + contact.frictionVector1 = contact.normal.getOrthoVector(); } // The second friction vector is computed by the cross product of the firs @@ -887,8 +889,8 @@ void ContactSolver::computeFrictionVectors(const vec3& deltaVelocity, // Clean up the constraint solver void ContactSolver::cleanup() { - if (mContactConstraints != NULL) { + if (mContactConstraints != nullptr) { delete[] mContactConstraints; - mContactConstraints = NULL; + mContactConstraints = nullptr; } } diff --git a/ephysics/engine/DynamicsWorld.cpp b/ephysics/engine/DynamicsWorld.cpp index 678821b..f0b7d3f 100644 --- a/ephysics/engine/DynamicsWorld.cpp +++ b/ephysics/engine/DynamicsWorld.cpp @@ -26,11 +26,11 @@ DynamicsWorld::DynamicsWorld(const vec3 &gravity) m_nbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), m_nbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), m_isSleepingEnabled(SPLEEPING_ENABLED), m_gravity(gravity), - m_isGravityEnabled(true), m_constrainedLinearVelocities(NULL), - m_constrainedAngularVelocities(NULL), m_splitLinearVelocities(NULL), - m_splitAngularVelocities(NULL), m_constrainedPositions(NULL), - m_constrainedOrientations(NULL), m_numberIslands(0), - m_numberIslandsCapacity(0), m_islands(NULL), m_numberBodiesCapacity(0), + m_isGravityEnabled(true), m_constrainedLinearVelocities(nullptr), + m_constrainedAngularVelocities(nullptr), m_splitLinearVelocities(nullptr), + m_splitAngularVelocities(nullptr), m_constrainedPositions(nullptr), + m_constrainedOrientations(nullptr), m_numberIslands(0), + m_numberIslandsCapacity(0), m_islands(nullptr), m_numberBodiesCapacity(0), m_sleepLinearVelocity(DEFAULT_SLEEP_LINEAR_VELOCITY), m_sleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_VELOCITY), m_timeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP) { @@ -109,7 +109,7 @@ void DynamicsWorld::update(float timeStep) { m_timeStep = timeStep; // Notify the event listener about the beginning of an int32_ternal tick - if (m_eventListener != NULL) { + if (m_eventListener != nullptr) { m_eventListener->beginInternalTick(); } @@ -145,7 +145,7 @@ void DynamicsWorld::update(float timeStep) { if (m_isSleepingEnabled) updateSleepingBodies(); // Notify the event listener about the end of an int32_ternal tick - if (m_eventListener != NULL) m_eventListener->endInternalTick(); + if (m_eventListener != nullptr) m_eventListener->endInternalTick(); // Reset the external force and torque applied to the bodies resetBodiesForceAndTorque(); @@ -185,9 +185,11 @@ void DynamicsWorld::int32_tegrateRigidBodiesPositions() { // Update the new constrained position and orientation of the body m_constrainedPositions[indexArray] = currentPosition + newLinVelocity * m_timeStep; - m_constrainedOrientations[indexArray] = currentOrientation + - etk::Quaternion(0, newAngVelocity) * - currentOrientation * 0.5f * m_timeStep; + m_constrainedOrientations[indexArray] = currentOrientation; + m_constrainedOrientations[indexArray] += etk::Quaternion(0, newAngVelocity) + * currentOrientation + * 0.5f + * m_timeStep; } } } @@ -244,12 +246,12 @@ void DynamicsWorld::initVelocityArrays() { m_constrainedAngularVelocities = new vec3[m_numberBodiesCapacity]; m_constrainedPositions = new vec3[m_numberBodiesCapacity]; m_constrainedOrientations = new etk::Quaternion[m_numberBodiesCapacity]; - assert(m_splitLinearVelocities != NULL); - assert(m_splitAngularVelocities != NULL); - assert(m_constrainedLinearVelocities != NULL); - assert(m_constrainedAngularVelocities != NULL); - assert(m_constrainedPositions != NULL); - assert(m_constrainedOrientations != NULL); + assert(m_splitLinearVelocities != nullptr); + assert(m_splitAngularVelocities != nullptr); + assert(m_constrainedLinearVelocities != nullptr); + assert(m_constrainedAngularVelocities != nullptr); + assert(m_constrainedPositions != nullptr); + assert(m_constrainedOrientations != nullptr); } // Reset the velocities arrays @@ -297,11 +299,10 @@ void DynamicsWorld::int32_tegrateRigidBodiesVelocities() { assert(m_splitAngularVelocities[indexBody] == vec3(0, 0, 0)); // Integrate the external force to get the new velocity of the body - m_constrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() + - m_timeStep * bodies[b]->m_massInverse * bodies[b]->m_externalForce; - m_constrainedAngularVelocities[indexBody] = bodies[b]->getAngularVelocity() + - m_timeStep * bodies[b]->getInertiaTensorInverseWorld() * - bodies[b]->m_externalTorque; + m_constrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity(); + m_constrainedLinearVelocities[indexBody] += bodies[b]->m_massInverse * bodies[b]->m_externalForce * m_timeStep; + m_constrainedAngularVelocities[indexBody] = bodies[b]->getAngularVelocity(); + m_constrainedAngularVelocities[indexBody] += bodies[b]->getInertiaTensorInverseWorld() * bodies[b]->m_externalTorque * m_timeStep; // If the gravity has to be applied to this rigid body if (bodies[b]->isGravityEnabled() && m_isGravityEnabled) { @@ -436,7 +437,7 @@ RigidBody* DynamicsWorld::createRigidBody(const etk::Transform3D& transform) { // Create the rigid body RigidBody* rigidBody = new (m_memoryAllocator.allocate(sizeof(RigidBody))) RigidBody(transform, *this, bodyID); - assert(rigidBody != NULL); + assert(rigidBody != nullptr); // Add the rigid body to the physics world m_bodies.insert(rigidBody); @@ -460,7 +461,7 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { // Destroy all the joints in which the rigid body to be destroyed is involved JointListElement* element; - for (element = rigidBody->m_jointsList; element != NULL; element = element->next) { + for (element = rigidBody->m_jointsList; element != nullptr; element = element->next) { destroyJoint(element->joint); } @@ -485,7 +486,7 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { */ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { - Joint* newJoint = NULL; + Joint* newJoint = nullptr; // Allocate memory to create the new joint switch(jointInfo.type) { @@ -530,7 +531,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { default: { assert(false); - return NULL; + return nullptr; } } @@ -557,7 +558,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { */ void DynamicsWorld::destroyJoint(Joint* joint) { - assert(joint != NULL); + assert(joint != nullptr); // If the collision between the two bodies of the constraint was disabled if (!joint->isCollisionEnabled()) { @@ -589,7 +590,7 @@ void DynamicsWorld::destroyJoint(Joint* joint) { // Add the joint to the list of joints of the two bodies involved in the joint void DynamicsWorld::addJointToBody(Joint* joint) { - assert(joint != NULL); + assert(joint != nullptr); // Add the joint at the beginning of the linked list of joints of the first body void* allocatedMemory1 = m_memoryAllocator.allocate(sizeof(JointListElement)); @@ -698,7 +699,7 @@ void DynamicsWorld::computeIslands() { // For each contact manifold in which the current body is involded ContactManifoldListElement* contactElement; - for (contactElement = bodyToVisit->m_contactManifoldsList; contactElement != NULL; + for (contactElement = bodyToVisit->m_contactManifoldsList; contactElement != nullptr; contactElement = contactElement->next) { ContactManifold* contactManifold = contactElement->contactManifold; @@ -728,7 +729,7 @@ void DynamicsWorld::computeIslands() { // For each joint in which the current body is involved JointListElement* jointElement; - for (jointElement = bodyToVisit->m_jointsList; jointElement != NULL; + for (jointElement = bodyToVisit->m_jointsList; jointElement != nullptr; jointElement = jointElement->next) { Joint* joint = jointElement->joint; @@ -904,7 +905,7 @@ void DynamicsWorld::testCollision(const CollisionBody* body, std::set shapes1; // For each shape of the body - for (const ProxyShape* shape=body->getProxyShapesList(); shape != NULL; + for (const ProxyShape* shape=body->getProxyShapesList(); shape != nullptr; shape = shape->getNext()) { shapes1.insert(shape->m_broadPhaseID); } @@ -929,13 +930,13 @@ void DynamicsWorld::testCollision(const CollisionBody* body1, // Create the sets of shapes std::set shapes1; - for (const ProxyShape* shape=body1->getProxyShapesList(); shape != NULL; + for (const ProxyShape* shape=body1->getProxyShapesList(); shape != nullptr; shape = shape->getNext()) { shapes1.insert(shape->m_broadPhaseID); } std::set shapes2; - for (const ProxyShape* shape=body2->getProxyShapesList(); shape != NULL; + for (const ProxyShape* shape=body2->getProxyShapesList(); shape != nullptr; shape = shape->getNext()) { shapes2.insert(shape->m_broadPhaseID); } diff --git a/ephysics/mathematics/mathematics_functions.h b/ephysics/mathematics/mathematics_functions.h index 7ee39da..c5438f3 100644 --- a/ephysics/mathematics/mathematics_functions.h +++ b/ephysics/mathematics/mathematics_functions.h @@ -7,6 +7,7 @@ // Libraries #include +#include #include #include diff --git a/lutin_ephysics.py b/lutin_ephysics.py index ce777bf..c4476ef 100644 --- a/lutin_ephysics.py +++ b/lutin_ephysics.py @@ -28,6 +28,7 @@ def configure(target, my_module): my_module.add_extra_flags() # add the file to compile: my_module.add_src_file([ + 'ephysics/debug.cpp', 'ephysics/memory/MemoryAllocator.cpp', 'ephysics/constraint/Joint.cpp', 'ephysics/constraint/HingeJoint.cpp', @@ -83,6 +84,7 @@ def configure(target, my_module): ]) my_module.add_header_file([ + 'ephysics/debug.hpp', 'ephysics/memory/MemoryAllocator.h', 'ephysics/memory/Stack.h', 'ephysics/constraint/BallAndSocketJoint.h', diff --git a/test/tests/collision/TestDynamicAABBTree.h b/test/tests/collision/TestDynamicAABBTree.h index 57b82b5..5f31047 100644 --- a/test/tests/collision/TestDynamicAABBTree.h +++ b/test/tests/collision/TestDynamicAABBTree.h @@ -228,10 +228,10 @@ class TestDynamicAABBTree : public Test { // ---- Update the object AABBs with the initial AABBs (no reinsertion) ----- // - tree.updateObject(object1Id, aabb1, vec3::zero(), false); - tree.updateObject(object2Id, aabb2, vec3::zero(), false); - tree.updateObject(object3Id, aabb3, vec3::zero(), false); - tree.updateObject(object4Id, aabb4, vec3::zero(), false); + tree.updateObject(object1Id, aabb1, vec3(0.0f,0.0f,0.0f)(), false); + tree.updateObject(object2Id, aabb2, vec3(0.0f,0.0f,0.0f)(), false); + tree.updateObject(object3Id, aabb3, vec3(0.0f,0.0f,0.0f)(), false); + tree.updateObject(object4Id, aabb4, vec3(0.0f,0.0f,0.0f)(), false); // AABB overlapping nothing mOverlapCallback.reset(); @@ -275,10 +275,10 @@ class TestDynamicAABBTree : public Test { // ---- Update the object AABBs with the initial AABBs (with reinsertion) ----- // - tree.updateObject(object1Id, aabb1, vec3::zero(), true); - tree.updateObject(object2Id, aabb2, vec3::zero(), true); - tree.updateObject(object3Id, aabb3, vec3::zero(), true); - tree.updateObject(object4Id, aabb4, vec3::zero(), true); + tree.updateObject(object1Id, aabb1, vec3(0.0f,0.0f,0.0f)(), true); + tree.updateObject(object2Id, aabb2, vec3(0.0f,0.0f,0.0f)(), true); + tree.updateObject(object3Id, aabb3, vec3(0.0f,0.0f,0.0f)(), true); + tree.updateObject(object4Id, aabb4, vec3(0.0f,0.0f,0.0f)(), true); // AABB overlapping nothing mOverlapCallback.reset(); @@ -323,10 +323,10 @@ class TestDynamicAABBTree : public Test { // ---- Move objects 2 and 3 ----- // AABB newAABB2(vec3(-7, 10, -3), vec3(1, 13, 3)); - tree.updateObject(object2Id, newAABB2, vec3::zero()); + tree.updateObject(object2Id, newAABB2, vec3(0.0f,0.0f,0.0f)()); AABB newAABB3(vec3(7, -6, -3), vec3(9, 1, 3)); - tree.updateObject(object3Id, newAABB3, vec3::zero()); + tree.updateObject(object3Id, newAABB3, vec3(0.0f,0.0f,0.0f)()); // AABB overlapping object 3 mOverlapCallback.reset(); @@ -414,10 +414,10 @@ class TestDynamicAABBTree : public Test { // ---- Update the object AABBs with the initial AABBs (no reinsertion) ----- // - tree.updateObject(object1Id, aabb1, vec3::zero(), false); - tree.updateObject(object2Id, aabb2, vec3::zero(), false); - tree.updateObject(object3Id, aabb3, vec3::zero(), false); - tree.updateObject(object4Id, aabb4, vec3::zero(), false); + tree.updateObject(object1Id, aabb1, vec3(0.0f,0.0f,0.0f)(), false); + tree.updateObject(object2Id, aabb2, vec3(0.0f,0.0f,0.0f)(), false); + tree.updateObject(object3Id, aabb3, vec3(0.0f,0.0f,0.0f)(), false); + tree.updateObject(object4Id, aabb4, vec3(0.0f,0.0f,0.0f)(), false); // Ray with no hits m_raycastCallback.reset(); @@ -453,10 +453,10 @@ class TestDynamicAABBTree : public Test { // ---- Update the object AABBs with the initial AABBs (with reinsertion) ----- // - tree.updateObject(object1Id, aabb1, vec3::zero(), true); - tree.updateObject(object2Id, aabb2, vec3::zero(), true); - tree.updateObject(object3Id, aabb3, vec3::zero(), true); - tree.updateObject(object4Id, aabb4, vec3::zero(), true); + tree.updateObject(object1Id, aabb1, vec3(0.0f,0.0f,0.0f)(), true); + tree.updateObject(object2Id, aabb2, vec3(0.0f,0.0f,0.0f)(), true); + tree.updateObject(object3Id, aabb3, vec3(0.0f,0.0f,0.0f)(), true); + tree.updateObject(object4Id, aabb4, vec3(0.0f,0.0f,0.0f)(), true); // Ray with no hits m_raycastCallback.reset(); @@ -493,10 +493,10 @@ class TestDynamicAABBTree : public Test { // ---- Move objects 2 and 3 ----- // AABB newAABB2(vec3(-7, 10, -3), vec3(1, 13, 3)); - tree.updateObject(object2Id, newAABB2, vec3::zero()); + tree.updateObject(object2Id, newAABB2, vec3(0.0f,0.0f,0.0f)()); AABB newAABB3(vec3(7, -6, -3), vec3(9, 1, 3)); - tree.updateObject(object3Id, newAABB3, vec3::zero()); + tree.updateObject(object3Id, newAABB3, vec3(0.0f,0.0f,0.0f)()); // Ray that hits object 1, 2 Ray ray5(vec3(-4, -5, 0), vec3(-4, 12, 0)); diff --git a/tools/testbed/scenes/concavemesh/ConcaveMeshScene.cpp b/tools/testbed/scenes/concavemesh/ConcaveMeshScene.cpp index 8903c7f..43a31f0 100644 --- a/tools/testbed/scenes/concavemesh/ConcaveMeshScene.cpp +++ b/tools/testbed/scenes/concavemesh/ConcaveMeshScene.cpp @@ -177,7 +177,7 @@ void ConcaveMeshScene::renderSinglePass(Shader& shader, const openglframework::M void ConcaveMeshScene::reset() { // Reset the transform - rp3d::etk::Transform3D transform(rp3d::vec3::zero(), rp3d::etk::Quaternion::identity()); + rp3d::etk::Transform3D transform(rp3d::vec3(0.0f,0.0f,0.0f)(), rp3d::etk::Quaternion::identity()); mConcaveMesh->resetTransform(transform); for (int32_t i=0; i