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

[DEV] update the 2 callback herited in function lanmba availlable

Browse Source
This commit is contained in:
Edouard DUPIN 2017-09-30 21:56:04 +02:00
parent cdd0d3e453
commit 275f32d377
17 changed files with 267 additions and 419 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -13,25 +13,20 @@ using namespace ephysics;
BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
:m_dynamicAABBTree(DYNAMIC_TREE_AABB_GAP), m_numberMovedShapes(0), m_numberAllocatedMovedShapes(8),
m_numberNonUsedMovedShapes(0), m_numberPotentialPairs(0), m_numberAllocatedPotentialPairs(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);
// Allocate memory for the array of potential overlapping pairs
m_potentialPairs = (BroadPhasePair*) malloc(m_numberAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(m_potentialPairs != NULL);
m_potentialPairs.reserve(8);
}
BroadPhaseAlgorithm::~BroadPhaseAlgorithm() {
// Release the memory for the array of non-static proxy shapes IDs
free(m_movedShapes);
// Release the memory for the array of potential overlapping pairs
free(m_potentialPairs);
}
void BroadPhaseAlgorithm::addMovedCollisionShape(int32_t broadPhaseID) {
@ -131,111 +126,72 @@ void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* _proxyShape,
}
void BroadPhaseAlgorithm::computeOverlappingPairs() {
// Reset the potential overlapping pairs
m_numberPotentialPairs = 0;
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) continue;
AABBOverlapCallback callback(*this, shapeID);
if (shapeID == -1) {
continue;
}
// Get the AABB of the shape
const AABB& shapeAABB = m_dynamicAABBTree.getFatAABB(shapeID);
// Ask the dynamic AABB tree to report all collision shapes that overlap with
// this AABB. The method BroadPhase::notifiyOverlappingPair() will be called
// by the dynamic AABB tree for each potential overlapping pair.
m_dynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, callback);
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) {
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) ));
});
}
// Reset the array of collision shapes that have move (or have been created) during the
// last simulation step
m_numberMovedShapes = 0;
// Sort the array of potential overlapping pairs in order to remove duplicate pairs
std::sort(m_potentialPairs, m_potentialPairs + m_numberPotentialPairs, BroadPhasePair::smallerThan);
m_potentialPairs.sort(0,
m_potentialPairs.size()-1,
[](const etk::Pair<int32_t,int32_t>& _pair1, const etk::Pair<int32_t,int32_t>& _pair2) {
if (_pair1.first < _pair2.first) {
return true;
}
if (_pair1.first == _pair2.first) {
return _pair1.second < _pair2.second;
}
return false;
});
// Check all the potential overlapping pairs avoiding duplicates to report unique
// overlapping pairs
uint32_t i=0;
while (i < m_numberPotentialPairs) {
uint32_t iii=0;
while (iii < m_potentialPairs.size()) {
// Get a potential overlapping pair
BroadPhasePair* pair = m_potentialPairs + i;
i++;
assert(pair->collisionShape1ID != pair->collisionShape2ID);
const etk::Pair<int32_t,int32_t>& pair = (m_potentialPairs[iii]);
++iii;
// Get the two collision shapes of the pair
ProxyShape* shape1 = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(pair->collisionShape1ID));
ProxyShape* shape2 = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(pair->collisionShape2ID));
ProxyShape* shape1 = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(pair.first));
ProxyShape* shape2 = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(pair.second));
// Notify the collision detection about the overlapping pair
m_collisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
// Skip the duplicate overlapping pairs
while (i < m_numberPotentialPairs) {
while (iii < m_potentialPairs.size()) {
// Get the next pair
BroadPhasePair* nextPair = m_potentialPairs + i;
const etk::Pair<int32_t,int32_t>& nextPair = m_potentialPairs[iii];
// If the next pair is different from the previous one, we stop skipping pairs
if (nextPair->collisionShape1ID != pair->collisionShape1ID ||
nextPair->collisionShape2ID != pair->collisionShape2ID) {
if ( nextPair.first != pair.first
|| nextPair.second != pair.second) {
break;
}
i++;
++iii;
}
}
}
// If the number of potential overlapping pairs is less than the quarter of allocated
// number of overlapping pairs
if (m_numberPotentialPairs < m_numberAllocatedPotentialPairs / 4 && m_numberPotentialPairs > 8) {
// Reduce the number of allocated potential overlapping pairs
BroadPhasePair* oldPairs = m_potentialPairs;
m_numberAllocatedPotentialPairs /= 2;
m_potentialPairs = (BroadPhasePair*) malloc(m_numberAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(m_potentialPairs);
memcpy(m_potentialPairs, oldPairs, m_numberPotentialPairs * sizeof(BroadPhasePair));
free(oldPairs);
}
}
void BroadPhaseAlgorithm::notifyOverlappingNodes(int32_t node1ID, int32_t node2ID) {
// If both the nodes are the same, we do not create store the overlapping pair
if (node1ID == node2ID) return;
// If we need to allocate more memory for the array of potential overlapping pairs
if (m_numberPotentialPairs == m_numberAllocatedPotentialPairs) {
// Allocate more memory for the array of potential pairs
BroadPhasePair* oldPairs = m_potentialPairs;
m_numberAllocatedPotentialPairs *= 2;
m_potentialPairs = (BroadPhasePair*) malloc(m_numberAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(m_potentialPairs);
memcpy(m_potentialPairs, oldPairs, m_numberPotentialPairs * sizeof(BroadPhasePair));
free(oldPairs);
}
// Add the new potential pair int32_to the array of potential overlapping pairs
m_potentialPairs[m_numberPotentialPairs].collisionShape1ID = etk::min(node1ID, node2ID);
m_potentialPairs[m_numberPotentialPairs].collisionShape2ID = etk::max(node1ID, node2ID);
m_numberPotentialPairs++;
}
void AABBOverlapCallback::notifyOverlappingNode(int32_t nodeId) {
m_broadPhaseAlgorithm.notifyOverlappingNodes(m_referenceNodeId, nodeId);
}
float BroadPhaseRaycastCallback::raycastBroadPhaseShape(int32_t nodeId, const Ray& ray) {
float BroadPhaseRaycastCallback::operator()(int32_t nodeId, const Ray& ray) {
float hitFraction = float(-1.0);

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

@ -16,49 +16,12 @@ namespace ephysics {
class CollisionDetection;
class BroadPhaseAlgorithm;
/**
* @brief It represent a potential overlapping pair during the
* broad-phase collision detection.
*/
struct BroadPhasePair {
int32_t collisionShape1ID; //!< Broad-phase ID of the first collision shape
int32_t collisionShape2ID; //!< Broad-phase ID of the second collision shape
/**
* @brief Method used to compare two pairs for sorting algorithm
* @param[in] _pair1 first pair of element
* @param[in] _pair2 Second pair of element
* @return _pair1 is smaller than _pair2
*/
static bool smallerThan(const BroadPhasePair& _pair1, const BroadPhasePair& _pair2) {
if (_pair1.collisionShape1ID < _pair2.collisionShape1ID) return true;
if (_pair1.collisionShape1ID == _pair2.collisionShape1ID) {
return _pair1.collisionShape2ID < _pair2.collisionShape2ID;
}
return false;
}
};
class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
private:
BroadPhaseAlgorithm& m_broadPhaseAlgorithm;
int32_t m_referenceNodeId;
public:
// Constructor
AABBOverlapCallback(BroadPhaseAlgorithm& _broadPhaseAlgo, int32_t _referenceNodeId):
m_broadPhaseAlgorithm(_broadPhaseAlgo),
m_referenceNodeId(_referenceNodeId) {
}
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int32_t nodeId);
};
// TODO : remove this as callback ... DynamicAABBTreeOverlapCallback {
/**
* Callback called when the AABB of a leaf node is hit by a ray the
* broad-phase Dynamic AABB Tree.
*/
class BroadPhaseRaycastCallback : public DynamicAABBTreeRaycastCallback {
class BroadPhaseRaycastCallback {
private :
const DynamicAABBTree& m_dynamicAABBTree;
unsigned short m_raycastWithCategoryMaskBits;
@ -74,7 +37,7 @@ namespace ephysics {
}
// Called for a broad-phase shape that has to be tested for raycast
virtual float raycastBroadPhaseShape(int32_t _nodeId, const Ray& _ray);
float operator()(int32_t _nodeId, const Ray& _ray);
};
/**
@ -91,9 +54,7 @@ namespace ephysics {
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.
BroadPhasePair* m_potentialPairs; //!< Temporary array of potential overlapping pairs (with potential duplicates)
uint32_t m_numberPotentialPairs; //!< Number of potential overlapping pairs
uint32_t m_numberAllocatedPotentialPairs; //!< Number of allocated elements for the array of potential overlapping pairs
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);
@ -119,8 +80,6 @@ namespace ephysics {
/// Remove a collision shape from the array of shapes that have moved in the last simulation
/// step and that need to be tested again for broad-phase overlapping.
void removeMovedCollisionShape(int32_t _broadPhaseID);
/// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
void notifyOverlappingNodes(int32_t _broadPhaseId1, int32_t _broadPhaseId2);
/// Compute all the overlapping pairs of collision shapes
void computeOverlappingPairs();
/// Return true if the two broad-phase collision shapes are overlapping

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

@ -568,36 +568,31 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t _nodeID) {
}
/// Report all shapes overlapping with the AABB given in parameter.
void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb,
DynamicAABBTreeOverlapCallback& callback) const {
void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& _aabb, etk::Function<void(int32_t nodeId)> _callback) const {
if (_callback == nullptr) {
EPHY_ERROR("call with nullptr callback");
return;
}
// Create a stack with the nodes to visit
Stack<int32_t, 64> stack;
stack.push(m_rootNodeID);
// While there are still nodes to visit
while(stack.getNbElements() > 0) {
// Get the next node ID to visit
int32_t nodeIDToVisit = stack.pop();
// Skip it if it is a null node
if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) continue;
if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) {
continue;
}
// Get the corresponding node
const TreeNode* nodeToVisit = m_nodes + nodeIDToVisit;
// If the AABB in parameter overlaps with the AABB of the node to visit
if (aabb.testCollision(nodeToVisit->aabb)) {
if (_aabb.testCollision(nodeToVisit->aabb)) {
// If the node is a leaf
if (nodeToVisit->isLeaf()) {
// Notify the broad-phase about a new potential overlapping pair
callback.notifyOverlappingNode(nodeIDToVisit);
}
else { // If the node is not a leaf
_callback(nodeIDToVisit);
} else { // If the node is not a leaf
// We need to visit its children
stack.push(nodeToVisit->children[0]);
stack.push(nodeToVisit->children[1]);
@ -607,60 +602,51 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb,
}
// Ray casting method
void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &callback) const {
void DynamicAABBTree::raycast(const ephysics::Ray& _ray, etk::Function<float(int32_t _nodeId, const ephysics::Ray& _ray)> _callback) const {
PROFILE("DynamicAABBTree::raycast()");
float maxFraction = ray.maxFraction;
if (_callback == nullptr) {
EPHY_ERROR("call with nullptr callback");
return;
}
float maxFraction = _ray.maxFraction;
Stack<int32_t, 128> stack;
stack.push(m_rootNodeID);
// Walk through the tree from the root looking for proxy shapes
// that overlap with the ray AABB
while (stack.getNbElements() > 0) {
// Get the next node in the stack
int32_t nodeID = stack.pop();
// If it is a null node, skip it
if (nodeID == TreeNode::NULL_TREE_NODE) continue;
if (nodeID == TreeNode::NULL_TREE_NODE) {
continue;
}
// Get the corresponding node
const TreeNode* node = m_nodes + nodeID;
Ray rayTemp(ray.point1, ray.point2, maxFraction);
Ray rayTemp(_ray.point1, _ray.point2, maxFraction);
// Test if the ray int32_tersects with the current node AABB
if (!node->aabb.testRayIntersect(rayTemp)) continue;
if (node->aabb.testRayIntersect(rayTemp) == false) {
continue;
}
// If the node is a leaf of the tree
if (node->isLeaf()) {
// Call the callback that will raycast again the broad-phase shape
float hitFraction = callback.raycastBroadPhaseShape(nodeID, rayTemp);
float hitFraction = _callback(nodeID, rayTemp);
// If the user returned a hitFraction of zero, it means that
// the raycasting should stop here
if (hitFraction == 0.0f) {
return;
}
// If the user returned a positive fraction
if (hitFraction > 0.0f) {
// We update the maxFraction value and the ray
// AABB using the new maximum fraction
if (hitFraction < maxFraction) {
maxFraction = hitFraction;
}
}
// If the user returned a negative fraction, we continue
// the raycasting as if the proxy shape did not exist
}
else { // If the node has children
} else { // If the node has children
// Push its children in the stack of nodes to explore
stack.push(node->children[0]);
stack.push(node->children[1]);

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

@ -8,11 +8,11 @@
#include <ephysics/configuration.hpp>
#include <ephysics/collision/shapes/AABB.hpp>
#include <ephysics/body/CollisionBody.hpp>
#include <etk/Function.hpp>
namespace ephysics {
class BroadPhaseAlgorithm;
class BroadPhaseRaycastTestCallback;
class DynamicAABBTreeOverlapCallback;
struct RaycastTest;
/**
* @brief It represents a node of the dynamic AABB tree.
@ -43,29 +43,6 @@ namespace ephysics {
bool isLeaf() const;
};
/**
* @brief Overlapping callback method that has to be used as parameter of the
* reportAllShapesOverlappingWithNode() method.
*/
class DynamicAABBTreeOverlapCallback {
public :
virtual ~DynamicAABBTreeOverlapCallback() = default;
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int32_t nodeId)=0;
};
/**
* @brief Raycast callback in the Dynamic AABB Tree called when the AABB of a leaf
* node is hit by the ray.
*/
class DynamicAABBTreeRaycastCallback {
public:
virtual ~DynamicAABBTreeRaycastCallback() = default;
// Called when the AABB of a leaf node is hit by a ray
virtual float raycastBroadPhaseShape(int32_t nodeId, const Ray& ray)=0;
};
/**
* @brief It implements a dynamic AABB tree that is used for broad-phase
* collision detection. This data structure is inspired by Nathanael Presson's
@ -123,10 +100,9 @@ namespace ephysics {
/// Return the data pointer of a given leaf node of the tree
void* getNodeDataPointer(int32_t _nodeID) const;
/// Report all shapes overlapping with the AABB given in parameter.
void reportAllShapesOverlappingWithAABB(const AABB& _aabb,
DynamicAABBTreeOverlapCallback& _callback) const;
void reportAllShapesOverlappingWithAABB(const AABB& _aabb, etk::Function<void(int32_t _nodeId)> _callback) const;
/// Ray casting method
void raycast(const Ray& _ray, DynamicAABBTreeRaycastCallback& _callback) const;
void raycast(const Ray& _ray, etk::Function<float(int32_t _nodeId, const ephysics::Ray& _ray)> _callback) const;
/// Compute the height of the tree
int32_t computeHeight();
/// Return the root AABB of the tree

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

@ -18,64 +18,63 @@ EPAAlgorithm::~EPAAlgorithm() {
}
int32_t EPAAlgorithm::isOriginInTetrahedron(const vec3& p1, const vec3& p2, const vec3& p3, const vec3& p4) const {
int32_t EPAAlgorithm::isOriginInTetrahedron(const vec3& _p1, const vec3& _p2, const vec3& _p3, const vec3& _p4) const {
// Check vertex 1
vec3 normal1 = (p2-p1).cross(p3-p1);
if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) {
vec3 normal1 = (_p2-_p1).cross(_p3-_p1);
if ((normal1.dot(_p1) > 0.0) == (normal1.dot(_p4) > 0.0)) {
return 4;
}
// Check vertex 2
vec3 normal2 = (p4-p2).cross(p3-p2);
if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) {
vec3 normal2 = (_p4-_p2).cross(_p3-_p2);
if ((normal2.dot(_p2) > 0.0) == (normal2.dot(_p1) > 0.0)) {
return 1;
}
// Check vertex 3
vec3 normal3 = (p4-p3).cross(p1-p3);
if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) {
vec3 normal3 = (_p4-_p3).cross(_p1-_p3);
if ((normal3.dot(_p3) > 0.0) == (normal3.dot(_p2) > 0.0)) {
return 2;
}
// Check vertex 4
vec3 normal4 = (p2-p4).cross(p1-p4);
if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) {
vec3 normal4 = (_p2-_p4).cross(_p1-_p4);
if ((normal4.dot(_p4) > 0.0) == (normal4.dot(_p3) > 0.0)) {
return 3;
}
// The origin is in the tetrahedron, we return 0
return 0;
}
void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex,
CollisionShapeInfo shape1Info,
const etk::Transform3D& transform1,
CollisionShapeInfo shape2Info,
const etk::Transform3D& transform2,
vec3& v,
void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& _simplex,
CollisionShapeInfo _shape1Info,
const etk::Transform3D& _transform1,
CollisionShapeInfo _shape2Info,
const etk::Transform3D& _transform2,
vec3& _vector,
NarrowPhaseCallback* narrowPhaseCallback) {
PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()");
assert(shape1Info.collisionShape->isConvex());
assert(shape2Info.collisionShape->isConvex());
const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
assert(_shape1Info.collisionShape->isConvex());
assert(_shape2Info.collisionShape->isConvex());
const ConvexShape* shape1 = static_cast<const ConvexShape*>(_shape1Info.collisionShape);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(_shape2Info.collisionShape);
void** shape1CachedCollisionData = _shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = _shape2Info.cachedCollisionData;
vec3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
vec3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
vec3 points[MAX_SUPPORT_POINTS]; // Current points
TrianglesStore triangleStore; // Store the triangles
TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face
// candidate of the EPA algorithm
etk::Set<TriangleEPA*> triangleHeap; // list of face candidate of the EPA algorithm sorted lower square dist to upper square dist
triangleHeap.setComparator([](TriangleEPA * const & _face1, TriangleEPA * const & _face2) {
return (_face1->getDistSquare() < _face2->getDistSquare());
});
// etk::Transform3D a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1)
etk::Transform3D body2Tobody1 = transform1.getInverse() * transform2;
etk::Transform3D body2Tobody1 = _transform1.getInverse() * _transform2;
// Matrix that transform a direction from local
// space of body 1 int32_to local space of body 2
etk::Quaternion rotateToBody2 = transform2.getOrientation().getInverse() *
transform1.getOrientation();
etk::Quaternion rotateToBody2 = _transform2.getOrientation().getInverse() * _transform1.getOrientation();
// Get the simplex computed previously by the GJK algorithm
uint32_t nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
uint32_t nbVertices = _simplex.getSimplex(suppPointsA, suppPointsB, points);
// Compute the tolerance
float tolerance = FLT_EPSILON * simplex.getMaxLengthSquareOfAPoint();
// Number of triangles in the polytope
uint32_t nbTriangles = 0;
float tolerance = FLT_EPSILON * _simplex.getMaxLengthSquareOfAPoint();
// Clear the storing of triangles
triangleStore.clear();
// Select an action according to the number of points in the simplex
@ -177,10 +176,10 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
link(EdgeEPA(face1, 1), EdgeEPA(face3, 0));
link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
// Add the triangle faces in the candidate heap
addFaceCandidate(face0, triangleHeap, nbTriangles, FLT_MAX);
addFaceCandidate(face1, triangleHeap, nbTriangles, FLT_MAX);
addFaceCandidate(face2, triangleHeap, nbTriangles, FLT_MAX);
addFaceCandidate(face3, triangleHeap, nbTriangles, FLT_MAX);
addFaceCandidate(face0, triangleHeap, FLT_MAX);
addFaceCandidate(face1, triangleHeap, FLT_MAX);
addFaceCandidate(face2, triangleHeap, FLT_MAX);
addFaceCandidate(face3, triangleHeap, FLT_MAX);
break;
}
// The tetrahedron contains a wrong vertex (the origin is not inside the tetrahedron)
@ -213,10 +212,10 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
suppPointsB[4] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData);
points[4] = suppPointsA[4] - suppPointsB[4];
TriangleEPA* face0 = NULL;
TriangleEPA* face1 = NULL;
TriangleEPA* face2 = NULL;
TriangleEPA* face3 = NULL;
TriangleEPA* face0 = nullptr;
TriangleEPA* face1 = nullptr;
TriangleEPA* face2 = nullptr;
TriangleEPA* face3 = nullptr;
// If the origin is in the first tetrahedron
if (isOriginInTetrahedron(points[0], points[1],
points[2], points[3]) == 0) {
@ -242,9 +241,14 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
return;
}
// If the constructed tetrahedron is not correct
if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL)
&& face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
&& face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
if (!( face0 != nullptr
&& face1 != nullptr
&& face2 != nullptr
&& face3 != nullptr
&& face0->getDistSquare() > 0.0
&& face1->getDistSquare() > 0.0
&& face2->getDistSquare() > 0.0
&& face3->getDistSquare() > 0.0) ) {
return;
}
// Associate the edges of neighbouring triangle faces
@ -255,26 +259,28 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
link(EdgeEPA(face1, 1), EdgeEPA(face3, 0));
link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
// Add the triangle faces in the candidate heap
addFaceCandidate(face0, triangleHeap, nbTriangles, FLT_MAX);
addFaceCandidate(face1, triangleHeap, nbTriangles, FLT_MAX);
addFaceCandidate(face2, triangleHeap, nbTriangles, FLT_MAX);
addFaceCandidate(face3, triangleHeap, nbTriangles, FLT_MAX);
addFaceCandidate(face0, triangleHeap, FLT_MAX);
addFaceCandidate(face1, triangleHeap, FLT_MAX);
addFaceCandidate(face2, triangleHeap, FLT_MAX);
addFaceCandidate(face3, triangleHeap, FLT_MAX);
nbVertices = 4;
}
break;
}
// At this point, we have a polytope that contains the origin. Therefore, we
// can run the EPA algorithm.
if (nbTriangles == 0) {
if (triangleHeap.size() == 0) {
return;
}
TriangleEPA* triangle = 0;
float upperBoundSquarePenDepth = FLT_MAX;
do {
triangle = triangleHeap[0];
// Get the next candidate face (the face closest to the origin)
std::pop_heap(&triangleHeap[0], &triangleHeap[nbTriangles], m_triangleComparison);
nbTriangles--;
triangleHeap.popFront();
EPHY_INFO("rm from heap:");
for (size_t iii=0; iii<triangleHeap.size(); ++iii) {
EPHY_INFO(" [" << iii << "] " << triangleHeap[iii]->getDistSquare());
}
// If the candidate face in the heap is not obsolete
if (!triangle->getIsObsolete()) {
// If we have reached the maximum number of support points
@ -284,17 +290,14 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
}
// Compute the support point of the Minkowski
// difference (A-B) in the closest point direction
suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin(
triangle->getClosestPoint(), shape1CachedCollisionData);
suppPointsB[nbVertices] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 *
(-triangle->getClosestPoint()), shape2CachedCollisionData);
suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin(triangle->getClosestPoint(), shape1CachedCollisionData);
suppPointsB[nbVertices] = body2Tobody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-triangle->getClosestPoint()), shape2CachedCollisionData);
points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
int32_t indexNewVertex = nbVertices;
nbVertices++;
// Update the upper bound of the penetration depth
float wDotv = points[indexNewVertex].dot(triangle->getClosestPoint());
assert(wDotv > 0.0);
EPHY_ASSERT(wDotv > 0.0, "depth penetration error");
float wDotVSquare = wDotv * wDotv / triangle->getDistSquare();
if (wDotVSquare < upperBoundSquarePenDepth) {
upperBoundSquarePenDepth = wDotVSquare;
@ -310,7 +313,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// Now, we compute the silhouette cast by the new vertex. The current triangle
// face will not be in the convex hull. We start the local recursive silhouette
// algorithm from the current triangle face.
int32_t i = triangleStore.getNbTriangles();
size_t i = triangleStore.getNbTriangles();
if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) {
break;
}
@ -318,22 +321,23 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// to the candidates list of faces of the current polytope
while(i != triangleStore.getNbTriangles()) {
TriangleEPA* newTriangle = &triangleStore[i];
addFaceCandidate(newTriangle, triangleHeap, nbTriangles, upperBoundSquarePenDepth);
addFaceCandidate(newTriangle, triangleHeap, upperBoundSquarePenDepth);
i++;
}
}
} while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
} while( triangleHeap.size() > 0
&& triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
// Compute the contact info
v = transform1.getOrientation() * triangle->getClosestPoint();
_vector = _transform1.getOrientation() * triangle->getClosestPoint();
vec3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
vec3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
vec3 normal = v.safeNormalized();
float penetrationDepth = v.length();
vec3 normal = _vector.safeNormalized();
float penetrationDepth = _vector.length();
assert(penetrationDepth > 0.0);
if (normal.length2() < FLT_EPSILON) {
return;
}
// Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
ContactPointInfo contactInfo(_shape1Info.proxyShape, _shape2Info.proxyShape, _shape1Info.collisionShape, _shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal);
narrowPhaseCallback->notifyContact(_shape1Info.overlappingPair, contactInfo);
}

31
ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp
View File

@ -11,29 +11,14 @@
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp>
#include <ephysics/mathematics/mathematics.hpp>
#include <ephysics/collision/narrowphase/EPA/TriangleEPA.hpp>
#include <algorithm>
#include <ephysics/debug.hpp>
#include <etk/Set.hpp>
namespace ephysics {
/// Maximum number of support points of the polytope
const uint32_t MAX_SUPPORT_POINTS = 100;
/// Maximum number of facets of the polytope
const uint32_t MAX_FACETS = 200;
/**
* @brief Class TriangleComparison
* This class allows the comparison of two triangles in the heap
* The comparison between two triangles is made using their square distance to the closest
* point to the origin. The goal is that in the heap, the first triangle is the one with the
* smallest square distance.
*/
class TriangleComparison {
public:
/**
* @brief Comparison operator
*/
bool operator()(const TriangleEPA* face1, const TriangleEPA* face2) {
return (face1->getDistSquare() > face2->getDistSquare());
}
};
/**
* @brief Class EPAAlgorithm
* This class is the implementation of the Expanding Polytope Algorithm (EPA).
@ -50,15 +35,13 @@ namespace ephysics {
*/
class EPAAlgorithm {
private:
TriangleComparison m_triangleComparison; //!< Triangle comparison operator
/// Private copy-constructor
EPAAlgorithm(const EPAAlgorithm& _algorithm);
/// Private assignment operator
EPAAlgorithm& operator=(const EPAAlgorithm& _algorithm);
/// Add a triangle face in the candidate triangle heap
void addFaceCandidate(TriangleEPA* _triangle,
TriangleEPA** _heap,
uint32_t& _nbTriangles,
etk::Set<TriangleEPA*>& _heap,
float _upperBoundSquarePenDepth) {
// If the closest point of the affine hull of triangle
// points is int32_ternal to the triangle and if the distance
@ -67,9 +50,11 @@ namespace ephysics {
if ( _triangle->isClosestPointInternalToTriangle()
&& _triangle->getDistSquare() <= _upperBoundSquarePenDepth) {
// Add the triangle face to the list of candidates
_heap[_nbTriangles] = _triangle;
_nbTriangles++;
std::push_heap(&_heap[0], &_heap[_nbTriangles], m_triangleComparison);
_heap.add(_triangle);
EPHY_INFO("add in heap:");
for (size_t iii=0; iii<_heap.size(); ++iii) {
EPHY_INFO(" [" << iii << "] " << _heap[iii]->getDistSquare());
}
}
}
// Decide if the origin is in the tetrahedron.

30
ephysics/collision/narrowphase/EPA/EdgeEPA.cpp
View File

@ -36,13 +36,13 @@ uint32_t EdgeEPA::getTargetVertexIndex() const {
return (*m_ownerTriangle)[indexOfNextCounterClockwiseEdge(m_index)];
}
bool EdgeEPA::computeSilhouette(const vec3* vertices, uint32_t indexNewVertex,
TrianglesStore& triangleStore) {
bool EdgeEPA::computeSilhouette(const vec3* _vertices, uint32_t _indexNewVertex,
TrianglesStore& _triangleStore) {
// If the edge has not already been visited
if (!m_ownerTriangle->getIsObsolete()) {
// If the triangle of this edge is not visible from the given point
if (!m_ownerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) {
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
if (!m_ownerTriangle->isVisibleFromVertex(_vertices, _indexNewVertex)) {
TriangleEPA* triangle = _triangleStore.newTriangle(_vertices, _indexNewVertex,
getTargetVertexIndex(),
getSourceVertexIndex());
// If the triangle has been created
@ -54,13 +54,12 @@ bool EdgeEPA::computeSilhouette(const vec3* vertices, uint32_t indexNewVertex,
} else {
// The current triangle is visible and therefore obsolete
m_ownerTriangle->setIsObsolete(true);
int32_t backup = triangleStore.getNbTriangles();
if(!m_ownerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(
this->m_index)).computeSilhouette(vertices,
indexNewVertex,
triangleStore)) {
int32_t backup = _triangleStore.getNbTriangles();
if(!m_ownerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(this->m_index)).computeSilhouette(_vertices,
_indexNewVertex,
_triangleStore)) {
m_ownerTriangle->setIsObsolete(false);
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
TriangleEPA* triangle = _triangleStore.newTriangle(_vertices, _indexNewVertex,
getTargetVertexIndex(),
getSourceVertexIndex());
// If the triangle has been created
@ -69,13 +68,12 @@ bool EdgeEPA::computeSilhouette(const vec3* vertices, uint32_t indexNewVertex,
return true;
}
return false;
} else if (!m_ownerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(
this->m_index)).computeSilhouette(vertices,
indexNewVertex,
triangleStore)) {
} else if (!m_ownerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(this->m_index)).computeSilhouette(_vertices,
_indexNewVertex,
_triangleStore)) {
m_ownerTriangle->setIsObsolete(false);
triangleStore.setNbTriangles(backup);
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
_triangleStore.resize(backup);
TriangleEPA* triangle = _triangleStore.newTriangle(_vertices, _indexNewVertex,
getTargetVertexIndex(),
getSourceVertexIndex());
if (triangle != NULL) {

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

@ -24,11 +24,34 @@ namespace ephysics {
float m_lambda1; //!< Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
float m_lambda2; //!< Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
float m_distSquare; //!< Square distance of the point closest point v to the origin
/// Private copy-constructor
TriangleEPA(const TriangleEPA& _triangle);
/// Private assignment operator
TriangleEPA& operator=(const TriangleEPA& _triangle);
public:
/// Private copy-constructor
TriangleEPA(const TriangleEPA& _triangle) {
memcpy(m_indicesVertices, _triangle.m_indicesVertices, sizeof(m_indicesVertices));
m_adjacentEdges[0] = _triangle.m_adjacentEdges[0];
m_adjacentEdges[1] = _triangle.m_adjacentEdges[1];
m_adjacentEdges[2] = _triangle.m_adjacentEdges[2];
m_isObsolete = _triangle.m_isObsolete;
m_determinant = _triangle.m_determinant;
m_closestPoint = _triangle.m_closestPoint;
m_lambda1 = _triangle.m_lambda1;
m_lambda2 = _triangle.m_lambda2;
m_distSquare = _triangle.m_distSquare;
}
/// Private assignment operator
TriangleEPA& operator=(const TriangleEPA& _triangle) {
memcpy(m_indicesVertices, _triangle.m_indicesVertices, sizeof(m_indicesVertices));
m_adjacentEdges[0] = _triangle.m_adjacentEdges[0];
m_adjacentEdges[1] = _triangle.m_adjacentEdges[1];
m_adjacentEdges[2] = _triangle.m_adjacentEdges[2];
m_isObsolete = _triangle.m_isObsolete;
m_determinant = _triangle.m_determinant;
m_closestPoint = _triangle.m_closestPoint;
m_lambda1 = _triangle.m_lambda1;
m_lambda2 = _triangle.m_lambda2;
m_distSquare = _triangle.m_distSquare;
return *this;
}
/// Constructor
TriangleEPA();
/// Constructor

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

@ -6,10 +6,3 @@
#include <ephysics/collision/narrowphase/EPA/TrianglesStore.hpp>
ephysics::TrianglesStore::TrianglesStore() : m_numberTriangles(0) {
}
ephysics::TrianglesStore::~TrianglesStore() {
}

41
ephysics/collision/narrowphase/EPA/TrianglesStore.hpp
View File

@ -12,48 +12,43 @@ namespace ephysics {
*/
class TrianglesStore {
private:
TriangleEPA m_triangles[MAX_TRIANGLES]; //!< Triangles
int32_t m_numberTriangles; //!< Number of triangles
etk::Vector<TriangleEPA> m_triangles; //!< Triangles
/// Private copy-constructor
TrianglesStore(const TrianglesStore& triangleStore);
TrianglesStore(const TrianglesStore& triangleStore) = delete;
/// Private assignment operator
TrianglesStore& operator=(const TrianglesStore& triangleStore);
TrianglesStore& operator=(const TrianglesStore& triangleStore) = delete;
public:
/// Constructor
TrianglesStore();
/// Destructor
~TrianglesStore();
TrianglesStore() = default;
/// Clear all the storage
void clear() {
m_numberTriangles = 0;
m_triangles.clear();
}
/// Return the number of triangles
int32_t getNbTriangles() const {
return m_numberTriangles;
size_t getNbTriangles() const {
return m_triangles.size();
}
/// Set the number of triangles
void setNbTriangles(int32_t _backup) {
m_numberTriangles = _backup;
void resize(int32_t _backup) {
m_triangles.resize(_backup);
}
/// Return the last triangle
TriangleEPA& last() {
assert(m_numberTriangles > 0);
return m_triangles[m_numberTriangles - 1];
return m_triangles.back();
}
/// Create a new triangle
TriangleEPA* newTriangle(const vec3* _vertices, uint32_t _v0, uint32_t _v1, uint32_t _v2) {
TriangleEPA* newTriangle = nullptr;
// If we have not reached the maximum number of triangles
if (m_numberTriangles != MAX_TRIANGLES) {
newTriangle = &m_triangles[m_numberTriangles++];
newTriangle->set(_v0, _v1, _v2);
if (!newTriangle->computeClosestPoint(_vertices)) {
m_numberTriangles--;
newTriangle = nullptr;
if (m_triangles.size() < MAX_TRIANGLES) {
TriangleEPA tmp(_v0, _v1, _v2);
if (!tmp.computeClosestPoint(_vertices)) {
return nullptr;
}
m_triangles.pushBack(etk::move(tmp));
return &m_triangles.back();
}
// Return the new triangle
return newTriangle;
// We are at the limit (internal)
return nullptr;
}
/// Access operator
TriangleEPA& operator[](int32_t _id) {

30
ephysics/collision/shapes/ConcaveMeshShape.cpp
View File

@ -53,10 +53,17 @@ void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32_t _subPart, int
}
void ConcaveMeshShape::testAllTriangles(TriangleCallback& _callback, const AABB& _localAABB) const {
ConvexTriangleAABBOverlapCallback overlapCallback(_callback, *this, m_dynamicAABBTree);
// Ask the Dynamic AABB Tree to report all the triangles that are overlapping
// with the AABB of the convex shape.
m_dynamicAABBTree.reportAllShapesOverlappingWithAABB(_localAABB, overlapCallback);
m_dynamicAABBTree.reportAllShapesOverlappingWithAABB(_localAABB, [&](int32_t _nodeId) {
// Get the node data (triangle index and mesh subpart index)
int32_t* data = m_dynamicAABBTree.getNodeDataInt(_nodeId);
// Get the triangle vertices for this node from the concave mesh shape
vec3 trianglePoints[3];
getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
// Call the callback to test narrow-phase collision with this triangle
_callback.testTriangle(trianglePoints);
});
}
bool ConcaveMeshShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const {
@ -66,12 +73,12 @@ bool ConcaveMeshShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, Proxy
// Ask the Dynamic AABB Tree to report all AABB nodes that are hit by the ray.
// The raycastCallback object will then compute ray casting against the triangles
// in the hit AABBs.
m_dynamicAABBTree.raycast(_ray, raycastCallback);
m_dynamicAABBTree.raycast(_ray, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
raycastCallback.raycastTriangles();
return raycastCallback.getIsHit();
}
float ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32_t _nodeId, const Ray& _ray) {
float ConcaveMeshRaycastCallback::operator()(int32_t _nodeId, const Ray& _ray) {
// Add the id of the hit AABB node int32_to
m_hitAABBNodes.pushBack(_nodeId);
return _ray.maxFraction;
@ -104,7 +111,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
m_raycastInfo.meshSubpart = data[0];
m_raycastInfo.triangleIndex = data[1];
smallestHitFraction = raycastInfo.hitFraction;
mIsHit = true;
m_isHit = true;
}
}
}
@ -136,17 +143,4 @@ void ConcaveMeshShape::computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float
0, 0, _mass);
}
void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int32_t _nodeId) {
// Get the node data (triangle index and mesh subpart index)
int32_t* data = m_dynamicAABBTree.getNodeDataInt(_nodeId);
// Get the triangle vertices for this node from the concave mesh shape
vec3 trianglePoints[3];
m_concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
// Call the callback to test narrow-phase collision with this triangle
m_triangleTestCallback.testTriangle(trianglePoints);
}

30
ephysics/collision/shapes/ConcaveMeshShape.hpp
View File

@ -13,27 +13,7 @@
namespace ephysics {
class ConcaveMeshShape;
class ConvexTriangleAABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
private:
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) {
}
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int32_t _nodeId);
};
class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
class ConcaveMeshRaycastCallback {
private :
etk::Vector<int32_t> m_hitAABBNodes;
const DynamicAABBTree& m_dynamicAABBTree;
@ -41,7 +21,7 @@ namespace ephysics {
ProxyShape* m_proxyShape;
RaycastInfo& m_raycastInfo;
const Ray& m_ray;
bool mIsHit;
bool m_isHit;
public:
// Constructor
ConcaveMeshRaycastCallback(const DynamicAABBTree& _dynamicAABBTree,
@ -54,16 +34,16 @@ namespace ephysics {
m_proxyShape(_proxyShape),
m_raycastInfo(_raycastInfo),
m_ray(_ray),
mIsHit(false) {
m_isHit(false) {
}
/// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
virtual float raycastBroadPhaseShape(int32_t _nodeId, const Ray& _ray);
float operator()(int32_t _nodeId, const ephysics::Ray& _ray);
/// Raycast all collision shapes that have been collected
void raycastTriangles();
/// Return true if a raycast hit has been found
bool getIsHit() const {
return mIsHit;
return m_isHit;
}
};
/**

2
ephysics/engine/CollisionWorld.cpp
View File

@ -37,7 +37,7 @@ CollisionBody* CollisionWorld::createCollisionBody(const etk::Transform3D& trans
// Get the next available body ID
bodyindex bodyID = computeNextAvailableBodyID();
// Largest index cannot be used (it is used for invalid index)
EPHY_ASSERT(bodyID < std::numeric_limits<ephysics::bodyindex>::max(), "index too big");
EPHY_ASSERT(bodyID < UINT64_MAX, "index too big");
// Create the collision body
CollisionBody* collisionBody = new CollisionBody(transform, *this, bodyID);
EPHY_ASSERT(collisionBody != nullptr, "empty Body collision");

2
ephysics/engine/ContactSolver.cpp
View File

@ -521,7 +521,7 @@ void ContactSolver::solve() {
float beta = m_isSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
float biasPenetrationDepth = 0.0;
if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/m_timeStep) *
max(0.0f, float(contactPoint.penetrationDepth - SLOP));
etk::max(0.0f, float(contactPoint.penetrationDepth - SLOP));
float b = biasPenetrationDepth + contactPoint.restitutionBias;
// Compute the Lagrange multiplier lambda

2
ephysics/engine/DynamicsWorld.cpp
View File

@ -329,7 +329,7 @@ ephysics::RigidBody* ephysics::DynamicsWorld::createRigidBody(const etk::Transfo
// Compute the body ID
ephysics::bodyindex bodyID = computeNextAvailableBodyID();
// Largest index cannot be used (it is used for invalid index)
assert(bodyID < std::numeric_limits<ephysics::bodyindex>::max());
assert(bodyID < UINT64_MAX);
// Create the rigid body
ephysics::RigidBody* rigidBody = new RigidBody(_transform, *this, bodyID);
assert(rigidBody != nullptr);

1
lutin_ephysics.py
View File

@ -150,7 +150,6 @@ def configure(target, my_module):
my_module.compile_version("c++", 2011)
# add dependency of the generic C++ library:
my_module.add_depend([
'cxx',
'm',
'elog',
'etk',

82
test/testDynamicAABBTree.cpp
View File

@ -10,35 +10,35 @@
#include <etk/Vector.hpp>
class OverlapCallback : public ephysics::DynamicAABBTreeOverlapCallback {
class OverlapCallback {
public :
etk::Vector<int32_t> m_overlapNodes;
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int32_t nodeId) {
m_overlapNodes.pushBack(nodeId);
void operator()(int32_t _nodeId) {
m_overlapNodes.pushBack(_nodeId);
}
void reset() {
m_overlapNodes.clear();
}
bool isOverlapping(int32_t nodeId) const {
return etk::isIn(nodeId, m_overlapNodes);
bool isOverlapping(int32_t _nodeId) const {
return etk::isIn(_nodeId, m_overlapNodes);
}
};
class DynamicTreeRaycastCallback : public ephysics::DynamicAABBTreeRaycastCallback {
class DynamicTreeRaycastCallback {
public:
etk::Vector<int32_t> m_hitNodes;
// Called when the AABB of a leaf node is hit by a ray
virtual float raycastBroadPhaseShape(int32_t nodeId, const ephysics::Ray& ray) {
m_hitNodes.pushBack(nodeId);
float operator()(int32_t _nodeId, const ephysics::Ray& _ray) {
m_hitNodes.pushBack(_nodeId);
return 1.0;
}
void reset() {
m_hitNodes.clear();
}
bool isHit(int32_t nodeId) const {
return etk::isIn(nodeId, m_hitNodes);
bool isHit(int32_t _nodeId) const {
return etk::isIn(_nodeId, m_hitNodes);
}
};
@ -115,7 +115,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping nothing
overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-10, 12, -4), vec3(10, 50, 4)), overlapCallback);
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-10, 12, -4), vec3(10, 50, 4)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false);
@ -123,7 +123,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping everything
overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-15, -15, -4), vec3(15, 15, 4)), overlapCallback);
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-15, -15, -4), vec3(15, 15, 4)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -131,7 +131,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 1 and 3
overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-4, 2, -4), vec3(-1, 7, 4)), overlapCallback);
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-4, 2, -4), vec3(-1, 7, 4)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -139,7 +139,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 3 and 4
overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-6, -5, -2), vec3(2, 2, 0)), overlapCallback);
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-6, -5, -2), vec3(2, 2, 0)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -147,7 +147,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 2
overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(5, -10, -2), vec3(7, 10, 9)), overlapCallback);
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(5, -10, -2), vec3(7, 10, 9)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false);
@ -162,7 +162,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping nothing
overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-10, 12, -4), vec3(10, 50, 4)), overlapCallback);
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-10, 12, -4), vec3(10, 50, 4)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false);
@ -170,7 +170,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping everything
overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-15, -15, -4), vec3(15, 15, 4)), overlapCallback);
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-15, -15, -4), vec3(15, 15, 4)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -178,7 +178,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 1 and 3
overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-4, 2, -4), vec3(-1, 7, 4)), overlapCallback);
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-4, 2, -4), vec3(-1, 7, 4)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -186,7 +186,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 3 and 4
overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-6, -5, -2), vec3(2, 2, 0)), overlapCallback);
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-6, -5, -2), vec3(2, 2, 0)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -194,7 +194,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 2
overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(5, -10, -2), vec3(7, 10, 9)), overlapCallback);
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(5, -10, -2), vec3(7, 10, 9)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false);
@ -209,7 +209,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping nothing
overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-10, 12, -4), vec3(10, 50, 4)), overlapCallback);
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-10, 12, -4), vec3(10, 50, 4)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false);
@ -217,7 +217,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping everything
overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-15, -15, -4), vec3(15, 15, 4)), overlapCallback);
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-15, -15, -4), vec3(15, 15, 4)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -225,7 +225,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 1 and 3
overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-4, 2, -4), vec3(-1, 7, 4)), overlapCallback);
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-4, 2, -4), vec3(-1, 7, 4)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -233,7 +233,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 3 and 4
overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-6, -5, -2), vec3(2, 2, 0)), overlapCallback);
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-6, -5, -2), vec3(2, 2, 0)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -241,7 +241,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 2
overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(5, -10, -2), vec3(7, 10, 9)), overlapCallback);
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(5, -10, -2), vec3(7, 10, 9)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false);
@ -257,7 +257,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 3
overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(6, -10, -2), vec3(8, 5, 3)), overlapCallback);
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(6, -10, -2), vec3(8, 5, 3)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -265,7 +265,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping objects 1, 2
overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-8, 5, -3), vec3(-2, 11, 3)), overlapCallback);
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-8, 5, -3), vec3(-2, 11, 3)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false);
@ -303,7 +303,7 @@ TEST(TestAABBTree, raycast) {
// Ray with no hits
raycastCallback.reset();
ephysics::Ray ray1(vec3(4.5, -10, -5), vec3(4.5, 10, -5));
tree.raycast(ray1, raycastCallback);
tree.raycast(ray1, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), false);
EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -312,7 +312,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 1
raycastCallback.reset();
ephysics::Ray ray2(vec3(-1, -20, -2), vec3(-1, 20, -2));
tree.raycast(ray2, raycastCallback);
tree.raycast(ray2, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), true);
EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -321,7 +321,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 1 and 2
raycastCallback.reset();
ephysics::Ray ray3(vec3(-7, 6, -2), vec3(8, 6, -2));
tree.raycast(ray3, raycastCallback);
tree.raycast(ray3, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), true);
EXPECT_EQ(raycastCallback.isHit(object2Id), true);
EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -330,7 +330,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 3
raycastCallback.reset();
ephysics::Ray ray4(vec3(-7, 2, 0), vec3(-1, 2, 0));
tree.raycast(ray4, raycastCallback);
tree.raycast(ray4, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), false);
EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), true);
@ -345,7 +345,7 @@ TEST(TestAABBTree, raycast) {
// Ray with no hits
raycastCallback.reset();
tree.raycast(ray1, raycastCallback);
tree.raycast(ray1, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), false);
EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -353,7 +353,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 1
raycastCallback.reset();
tree.raycast(ray2, raycastCallback);
tree.raycast(ray2, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), true);
EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -361,7 +361,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 1 and 2
raycastCallback.reset();
tree.raycast(ray3, raycastCallback);
tree.raycast(ray3, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), true);
EXPECT_EQ(raycastCallback.isHit(object2Id), true);
EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -369,7 +369,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 3
raycastCallback.reset();
tree.raycast(ray4, raycastCallback);
tree.raycast(ray4, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), false);
EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), true);
@ -384,7 +384,7 @@ TEST(TestAABBTree, raycast) {
// Ray with no hits
raycastCallback.reset();
tree.raycast(ray1, raycastCallback);
tree.raycast(ray1, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), false);
EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -392,7 +392,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 1
raycastCallback.reset();
tree.raycast(ray2, raycastCallback);
tree.raycast(ray2, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), true);
EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -400,7 +400,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 1 and 2
raycastCallback.reset();
tree.raycast(ray3, raycastCallback);
tree.raycast(ray3, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), true);
EXPECT_EQ(raycastCallback.isHit(object2Id), true);
EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -408,7 +408,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 3
raycastCallback.reset();
tree.raycast(ray4, raycastCallback);
tree.raycast(ray4, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), false);
EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), true);
@ -425,7 +425,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 1, 2
ephysics::Ray ray5(vec3(-4, -5, 0), vec3(-4, 12, 0));
raycastCallback.reset();
tree.raycast(ray5, raycastCallback);
tree.raycast(ray5, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), true);
EXPECT_EQ(raycastCallback.isHit(object2Id), true);
EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -434,7 +434,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 3 and 4
ephysics::Ray ray6(vec3(11, -3, 1), vec3(-2, -3, 1));
raycastCallback.reset();
tree.raycast(ray6, raycastCallback);
tree.raycast(ray6, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), false);
EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), true);