From 0b6d9af18ebaedbfb74eb1ee00bd28f3e9156b24 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 7 Dec 2015 23:03:04 +0100 Subject: [PATCH] Add testRayIntersect() method to AABB and use in raycasting broadphase --- src/collision/broadphase/DynamicAABBTree.cpp | 16 +++------ src/collision/shapes/AABB.cpp | 35 ++++++++++++++++++++ src/collision/shapes/AABB.h | 3 ++ src/collision/shapes/BoxShape.cpp | 2 +- src/collision/shapes/TriangleShape.cpp | 5 ++- 5 files changed, 47 insertions(+), 14 deletions(-) diff --git a/src/collision/broadphase/DynamicAABBTree.cpp b/src/collision/broadphase/DynamicAABBTree.cpp index 92e4656e..8cd2e4cb 100644 --- a/src/collision/broadphase/DynamicAABBTree.cpp +++ b/src/collision/broadphase/DynamicAABBTree.cpp @@ -637,11 +637,6 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &ca decimal maxFraction = ray.maxFraction; - // Create an AABB for the ray - Vector3 endPoint = ray.point1 + maxFraction * (ray.point2 - ray.point1); - AABB rayAABB(Vector3::min(ray.point1, endPoint), - Vector3::max(ray.point1, endPoint)); - Stack stack; stack.push(mRootNodeID); @@ -658,14 +653,14 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &ca // Get the corresponding node const TreeNode* node = mNodes + nodeID; - // Test if the node AABB overlaps with the ray AABB - if (!rayAABB.testCollision(node->aabb)) continue; + Ray rayTemp(ray.point1, ray.point2, maxFraction); + + // Test if the ray intersects with the current node AABB + if (!node->aabb.testRayIntersect(rayTemp)) 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); @@ -683,9 +678,6 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &ca if (hitFraction < maxFraction) { maxFraction = hitFraction; } - endPoint = ray.point1 + maxFraction * (ray.point2 - ray.point1); - rayAABB.mMinCoordinates = Vector3::min(ray.point1, endPoint); - rayAABB.mMaxCoordinates = Vector3::max(ray.point1, endPoint); } // If the user returned a negative fraction, we continue diff --git a/src/collision/shapes/AABB.cpp b/src/collision/shapes/AABB.cpp index c69a6aba..6271316b 100644 --- a/src/collision/shapes/AABB.cpp +++ b/src/collision/shapes/AABB.cpp @@ -113,3 +113,38 @@ AABB AABB::createAABBForTriangle(const Vector3* trianglePoints) { return AABB(minCoords, maxCoords); } + +// Return true if the ray intersects the AABB +/// This method use the line vs AABB raycasting technique described in +/// Real-time Collision Detection by Christer Ericson. +bool AABB::testRayIntersect(const Ray& ray) 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; + + // 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; + + // Add in an epsilon term to counteract arithmetic errors when segment is + // (near) parallel to a coordinate axis (see text for detail) + const decimal epsilon = 0.00001; + adx += epsilon; + ady += epsilon; + adz += epsilon; + + // 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; + + // No separating axis has been found + return true; +} diff --git a/src/collision/shapes/AABB.h b/src/collision/shapes/AABB.h index d019f655..dfe9884c 100644 --- a/src/collision/shapes/AABB.h +++ b/src/collision/shapes/AABB.h @@ -100,6 +100,9 @@ class AABB { /// Return true if the AABB of a triangle intersects the AABB bool testCollisionTriangleAABB(const Vector3* trianglePoints) const; + /// Return true if the ray intersects the AABB + bool testRayIntersect(const Ray& ray) const; + /// Create and return an AABB for a triangle static AABB createAABBForTriangle(const Vector3* trianglePoints); diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index 4d8d488f..fe1f5693 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -117,7 +117,7 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro } // If tMin is negative, we return no hit - if (tMin < decimal(0.0)) return false; + if (tMin < decimal(0.0) || tMin > ray.maxFraction) return false; // The ray intersects the three slabs, we compute the hit point Vector3 localHitPoint = ray.point1 + tMin * rayDirection; diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 1731193b..f7f0b556 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -109,6 +109,9 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape // Compute the local hit point using the barycentric coordinates const Vector3 localHitPoint = u * mPoints[0] + v * mPoints[1] + w * mPoints[2]; + const decimal hitFraction = (localHitPoint - ray.point1).length() / pq.length(); + + if (hitFraction < decimal(0.0) || hitFraction > ray.maxFraction) return false; Vector3 localHitNormal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]); if (localHitNormal.dot(pq) > decimal(0.0)) localHitNormal = -localHitNormal; @@ -116,7 +119,7 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.worldPoint = localHitPoint; - raycastInfo.hitFraction = (localHitPoint - ray.point1).length() / pq.length(); + raycastInfo.hitFraction = hitFraction; raycastInfo.worldNormal = localHitNormal; return true;