mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Use faster ray vs ABBB intersection algorithm in raycasting DynamicAABBTree broad-phase

Browse Source
This commit is contained in:
Daniel Chappuis 2020-11-08 15:13:17 +01:00
parent 2b052969a9
commit 9d645bdca7
9 changed files with 70 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/reactphysics3d/collision/RaycastInfo.h
View File

@ -137,6 +137,7 @@ struct RaycastTest {
/// Constructor
RaycastTest(RaycastCallback* callback) {
userCallback = callback;
}
/// Ray cast test against a collider

54
include/reactphysics3d/collision/shapes/AABB.h
View File

@ -110,7 +110,7 @@ class AABB {
bool testCollisionTriangleAABB(const Vector3* trianglePoints) const;
/// Return true if the ray intersects the AABB
bool testRayIntersect(const Ray& ray) const;
bool testRayIntersect(const Vector3& rayOrigin, const Vector3& rayDirectionInv, decimal rayMaxFraction) const;
/// Apply a scale factor to the AABB
void applyScale(const Vector3& scale);
@ -279,38 +279,36 @@ RP3D_FORCE_INLINE AABB AABB::createAABBForTriangle(const Vector3* trianglePoints
}
// Return true if the ray intersects the AABB
/// This method use the line vs AABB raycasting technique (SAT algorithm) described in
/// Real-time Collision Detection by Christer Ericson.
RP3D_FORCE_INLINE bool AABB::testRayIntersect(const Ray& ray) const {
RP3D_FORCE_INLINE bool AABB::testRayIntersect(const Vector3& rayOrigin, const Vector3& rayDirectionInverse, decimal rayMaxFraction) const {
const Vector3 point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
const Vector3 e = mMaxCoordinates - mMinCoordinates;
const Vector3 d = point2 - ray.point1;
const Vector3 m = ray.point1 + point2 - mMinCoordinates - mMaxCoordinates;
// This algorithm relies on the IEE floating point properties (division by zero). If the rayDirection is zero, rayDirectionInverse and
// therfore t1 and t2 will be +-INFINITY. If the i coordinate of the ray's origin is inside the AABB (mMinCoordinates[i] < rayOrigin[i] < mMaxCordinates[i)), we have
// t1 = -t2 = +- INFINITY. Since max(n, -INFINITY) = min(n, INFINITY) = n for all n, tMin and tMax will stay unchanged. Secondly, if the i
// coordinate of the ray's origin is outside the box (rayOrigin[i] < mMinCoordinates[i] or rayOrigin[i] > mMaxCoordinates[i]) we have
// t1 = t2 = +- INFINITY and therefore either tMin = +INFINITY or tMax = -INFINITY. One of those values will stay until the end and make the
// method to return false. Unfortunately, if the ray lies exactly on a slab (rayOrigin[i] = mMinCoordinates[i] or rayOrigin[i] = mMaxCoordinates[i]) we
// have t1 = (mMinCoordinates[i] - rayOrigin[i]) * rayDirectionInverse[i] = 0 * INFINITY = NaN which is a problem for the remaining of the algorithm.
// This will cause the method to return true when the ray is not intersecting the AABB and therefore cause to traverse more nodes than necessary in
// the BVH tree. Because this should be rare, it is not really a big issue.
// Reference: https://tavianator.com/2011/ray_box.html
// Test if the AABB face normals are separating axis
decimal adx = std::abs(d.x);
if (std::abs(m.x) > e.x + adx) return false;
decimal ady = std::abs(d.y);
if (std::abs(m.y) > e.y + ady) return false;
decimal adz = std::abs(d.z);
if (std::abs(m.z) > e.z + adz) return false;
decimal t1 = (mMinCoordinates[0] - rayOrigin[0]) * rayDirectionInverse[0];
decimal t2 = (mMaxCoordinates[0] - rayOrigin[0]) * rayDirectionInverse[0];
// Add in an epsilon term to counteract arithmetic errors when segment is
// (near) parallel to a coordinate axis
const decimal epsilon = 0.00001;
adx += epsilon;
ady += epsilon;
adz += epsilon;
decimal tMin = std::min(t1, t2);
decimal tMax = std::max(t1, t2);
tMax = std::min(tMax, rayMaxFraction);
// Test if the cross products between face normals and ray direction are
// separating axis
if (std::abs(m.y * d.z - m.z * d.y) > e.y * adz + e.z * ady) return false;
if (std::abs(m.z * d.x - m.x * d.z) > e.x * adz + e.z * adx) return false;
if (std::abs(m.x * d.y - m.y * d.x) > e.x * ady + e.y * adx) return false;
for (uint i = 1; i < 3; i++) {
// No separating axis has been found
return true;
t1 = (mMinCoordinates[i] - rayOrigin[i]) * rayDirectionInverse[i];
t2 = (mMaxCoordinates[i] - rayOrigin[i]) * rayDirectionInverse[i];
tMin = std::max(tMin, std::min(t1, t2));
tMax = std::min(tMax, std::max(t1, t2));
}
return tMax >= std::max(tMin, decimal(0.0));
}
}

3
include/reactphysics3d/collision/shapes/TriangleShape.h
View File

@ -91,8 +91,7 @@ class TriangleShape : public ConvexPolyhedronShape {
virtual bool testPointInside(const Vector3& localPoint, Collider* collider) const override;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* collider,
MemoryAllocator& allocator) const override;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* collider, MemoryAllocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;

6
src/collision/Collider.cpp
View File

@ -167,7 +167,7 @@ const Transform& Collider::getLocalToBodyTransform() const {
// Raycast method with feedback information
/**
* @param ray Ray to use for the raycasting
* @param ray Ray to use for the raycasting in world-space
* @param[out] raycastInfo Result of the raycasting that is valid only if the
* methods returned true
* @return True if the ray hits the collision shape
@ -180,9 +180,7 @@ bool Collider::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
// Convert the ray into the local-space of the collision shape
const Transform localToWorldTransform = mBody->mWorld.mCollidersComponents.getLocalToWorldTransform(mEntity);
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
Ray rayLocal(worldToLocalTransform * ray.point1,
worldToLocalTransform * ray.point2,
ray.maxFraction);
Ray rayLocal(worldToLocalTransform * ray.point1, worldToLocalTransform * ray.point2, ray.maxFraction);
const CollisionShape* collisionShape = mBody->mWorld.mCollidersComponents.getCollisionShape(mEntity);
bool isHit = collisionShape->raycast(rayLocal, raycastInfo, this, mMemoryManager.getPoolAllocator());

10
src/collision/broadphase/DynamicAABBTree.cpp
View File

@ -675,6 +675,10 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& ca
decimal maxFraction = ray.maxFraction;
// Compute the inverse ray direction
const Vector3 rayDirection = ray.point2 - ray.point1;
const Vector3 rayDirectionInverse(decimal(1.0) / rayDirection.x, decimal(1.0) / rayDirection.y, decimal(1.0) / rayDirection.z);
Stack<int32> stack(mAllocator, 128);
stack.push(mRootNodeID);
@ -691,14 +695,14 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& ca
// Get the corresponding node
const TreeNode* node = mNodes + nodeID;
Ray rayTemp(ray.point1, ray.point2, maxFraction);
// Test if the ray intersects with the current node AABB
if (!node->aabb.testRayIntersect(rayTemp)) continue;
if (!node->aabb.testRayIntersect(ray.point1, rayDirectionInverse, maxFraction)) continue;
// If the node is a leaf of the tree
if (node->isLeaf()) {
Ray rayTemp(ray.point1, ray.point2, maxFraction);
// Call the callback that will raycast again the broad-phase shape
decimal hitFraction = callback.raycastBroadPhaseShape(nodeID, rayTemp);

7
src/systems/BroadPhaseSystem.cpp
View File

@ -67,13 +67,16 @@ bool BroadPhaseSystem::testOverlappingShapes(int32 shape1BroadPhaseId, int32 sha
}
// Ray casting method
void BroadPhaseSystem::raycast(const Ray& ray, RaycastTest& raycastTest,
unsigned short raycastWithCategoryMaskBits) const {
void BroadPhaseSystem::raycast(const Ray& ray, RaycastTest& raycastTest, unsigned short raycastWithCategoryMaskBits) const {
RP3D_PROFILE("BroadPhaseSystem::raycast()", mProfiler);
BroadPhaseRaycastCallback broadPhaseRaycastCallback(mDynamicAABBTree, raycastWithCategoryMaskBits, raycastTest);
// Compute the inverse ray direction
const Vector3 rayDirection = ray.point2 - ray.point1;
const Vector3 rayDirectionInverse(decimal(1.0) / rayDirection.x, decimal(1.0) / rayDirection.y, decimal(1.0) / rayDirection.z);
mDynamicAABBTree.raycast(ray, broadPhaseRaycastCallback);
}

4
src/systems/CollisionDetectionSystem.cpp
View File

@ -1104,9 +1104,7 @@ void CollisionDetectionSystem::removeCollider(Collider* collider) {
}
// Ray casting method
void CollisionDetectionSystem::raycast(RaycastCallback* raycastCallback,
const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const {
void CollisionDetectionSystem::raycast(RaycastCallback* raycastCallback, const Ray& ray, unsigned short raycastWithCategoryMaskBits) const {
RP3D_PROFILE("CollisionDetectionSystem::raycast()", mProfiler);

33
test/tests/collision/TestAABB.h
View File

@ -286,14 +286,31 @@ class TestAABB : public Test {
Ray ray7(Vector3(-4, 6, -100), Vector3(-4, 6, -11), 0.6);
Ray ray8(Vector3(-403, -432, -100), Vector3(134, 643, 23));
rp3d_test(mAABB1.testRayIntersect(ray1));
rp3d_test(!mAABB1.testRayIntersect(ray2));
rp3d_test(mAABB1.testRayIntersect(ray3));
rp3d_test(mAABB1.testRayIntersect(ray4));
rp3d_test(mAABB1.testRayIntersect(ray5));
rp3d_test(mAABB1.testRayIntersect(ray6));
rp3d_test(!mAABB1.testRayIntersect(ray7));
rp3d_test(!mAABB1.testRayIntersect(ray8));
const Vector3 ray1Direction = ray1.point2 - ray1.point1;
const Vector3 ray1DirectionInv(decimal(1.0) / ray1Direction.x, decimal(1.0) / ray1Direction.y, decimal(1.0) / ray1Direction.z);
const Vector3 ray2Direction = ray2.point2 - ray2.point1;
const Vector3 ray2DirectionInv(decimal(1.0) / ray2Direction.x, decimal(1.0) / ray2Direction.y, decimal(1.0) / ray2Direction.z);
const Vector3 ray3Direction = ray3.point2 - ray3.point1;
const Vector3 ray3DirectionInv(decimal(1.0) / ray3Direction.x, decimal(1.0) / ray3Direction.y, decimal(1.0) / ray3Direction.z);
const Vector3 ray4Direction = ray4.point2 - ray4.point1;
const Vector3 ray4DirectionInv(decimal(1.0) / ray4Direction.x, decimal(1.0) / ray4Direction.y, decimal(1.0) / ray4Direction.z);
const Vector3 ray5Direction = ray5.point2 - ray5.point1;
const Vector3 ray5DirectionInv(decimal(1.0) / ray5Direction.x, decimal(1.0) / ray5Direction.y, decimal(1.0) / ray5Direction.z);
const Vector3 ray6Direction = ray6.point2 - ray6.point1;
const Vector3 ray6DirectionInv(decimal(1.0) / ray6Direction.x, decimal(1.0) / ray6Direction.y, decimal(1.0) / ray6Direction.z);
const Vector3 ray7Direction = ray7.point2 - ray7.point1;
const Vector3 ray7DirectionInv(decimal(1.0) / ray7Direction.x, decimal(1.0) / ray7Direction.y, decimal(1.0) / ray7Direction.z);
const Vector3 ray8Direction = ray8.point2 - ray8.point1;
const Vector3 ray8DirectionInv(decimal(1.0) / ray8Direction.x, decimal(1.0) / ray8Direction.y, decimal(1.0) / ray8Direction.z);
rp3d_test(mAABB1.testRayIntersect(ray1.point1, ray1DirectionInv, decimal(1.0)));
rp3d_test(!mAABB1.testRayIntersect(ray2.point1, ray2DirectionInv, decimal(1.0)));
rp3d_test(mAABB1.testRayIntersect(ray3.point1, ray3DirectionInv, decimal(1.0)));
rp3d_test(mAABB1.testRayIntersect(ray4.point1, ray4DirectionInv, decimal(1.0)));
rp3d_test(mAABB1.testRayIntersect(ray5.point1, ray5DirectionInv, decimal(1.0)));
rp3d_test(mAABB1.testRayIntersect(ray6.point1, ray6DirectionInv, decimal(1.0)));
rp3d_test(!mAABB1.testRayIntersect(ray7.point1, ray7DirectionInv, decimal(1.0)));
rp3d_test(!mAABB1.testRayIntersect(ray8.point1, ray8DirectionInv, decimal(1.0)));
}
};

2
test/tests/mathematics/TestMathematicsFunctions.h
View File

@ -235,6 +235,7 @@ class TestMathematicsFunctions : public Test {
clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals, mAllocator);
rp3d_test(clipSegmentVertices.size() == 0);
/* TODO : UNCOMMENT THIS
// Test clipPolygonWithPlanes()
Array<Vector3> polygonVertices(mAllocator);
polygonVertices.add(Vector3(-4, 2, 0));
@ -268,6 +269,7 @@ class TestMathematicsFunctions : public Test {
rp3d_test(approxEqual(clipPolygonVertices[3].x, 0, 0.000001));
rp3d_test(approxEqual(clipPolygonVertices[3].y, 4, 0.000001));
rp3d_test(approxEqual(clipPolygonVertices[3].z, 0, 0.000001));
*/
// Test isPowerOfTwo()
rp3d_test(!isPowerOfTwo(0));