From c07a2dc9a27b86b5a5f6393380951d81eaa092c7 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 19 Sep 2014 22:53:40 +0200 Subject: [PATCH] Implement raycasting with cylinder shape --- src/collision/shapes/CylinderShape.cpp | 253 ++++++++++++++++++++++++- test/main.cpp | 2 +- test/tests/collision/TestRaycast.h | 62 ++++-- 3 files changed, 294 insertions(+), 23 deletions(-) diff --git a/src/collision/shapes/CylinderShape.cpp b/src/collision/shapes/CylinderShape.cpp index 6ac43e87..46932c2f 100644 --- a/src/collision/shapes/CylinderShape.cpp +++ b/src/collision/shapes/CylinderShape.cpp @@ -25,6 +25,7 @@ // Libraries #include "CylinderShape.h" +#include "collision/ProxyShape.h" #include "configuration.h" using namespace reactphysics3d; @@ -88,20 +89,260 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio } // Raycast method +/// Algorithm based on the one described at page 194 in Real-ime Collision Detection by +/// Morgan Kaufmann. bool CylinderShape::raycast(const Ray& ray, ProxyShape* proxyShape) const { - // TODO : Normalize the ray direction + // Transform the ray direction and origin in local-space coordinates + const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform(); + const Transform worldToLocalTransform = localToWorldTransform.getInverse(); + Vector3 origin = worldToLocalTransform * ray.origin; + Vector3 n = worldToLocalTransform.getOrientation() * ray.direction.getUnit(); - // TODO : Implement this method - return false; + const decimal epsilon = decimal(0.00001); + Vector3 p(decimal(0), -mHalfHeight, decimal(0)); + Vector3 q(decimal(0), mHalfHeight, decimal(0)); + Vector3 d = q - p; + Vector3 m = origin - p; + decimal t; + + decimal mDotD = m.dot(d); + decimal nDotD = n.dot(d); + decimal dDotD = d.dot(d); + decimal mDotN = m.dot(n); + + decimal a = dDotD - nDotD * nDotD; + decimal k = m.dot(m) - mRadius * mRadius; + decimal c = dDotD * k - mDotD * mDotD; + + // If the ray is parallel to the cylinder axis + if (std::abs(a) < epsilon) { + + // If the origin is outside the surface of the cylinder, we return no hit + if (c > decimal(0.0)) return false; + + // Here we know that the segment intersect an endcap of the cylinder + + // If the ray intersects with the "p" endcap of the cylinder + if (mDotD < decimal(0.0)) { + return true; + } + else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder + return true; + } + else { // If the origin is inside the cylinder, we return no hit + return false; + } + } + decimal b = dDotD * mDotN - nDotD * mDotD; + decimal discriminant = b * b - a * c; + + // If the discriminant is negative, no real roots and therfore, no hit + if (discriminant < decimal(0.0)) return false; + + // Compute the smallest root (first intersection along the ray) + decimal t0 = t = (-b - std::sqrt(discriminant)) / a; + + // If the intersection is outside the cylinder on "p" endcap side + decimal value = mDotD + t * nDotD; + if (value < decimal(0.0)) { + + // If the ray is pointing away from the "p" endcap, we return no hit + if (nDotD <= decimal(0.0)) return false; + + // Compute the intersection against the "p" endcap (intersection agains whole plane) + t = -mDotD / nDotD; + + // Keep the intersection if the it is inside the cylinder radius + return (k + t * (decimal(2.0) * mDotN + t) <= decimal(0.0)); + } + else if (value > dDotD) { // If the intersection is outside the cylinder on the "q" side + + // If the ray is pointing away from the "q" endcap, we return no hit + if (nDotD >= decimal(0.0)) return false; + + // Compute the intersection against the "q" endcap (intersection against whole plane) + t = (dDotD - mDotD) / nDotD; + + // Keep the intersection if it is inside the cylinder radius + return (k + dDotD - decimal(2.0) * mDotD + t * (decimal(2.0) * (mDotN - nDotD) + t) + <= decimal(0.0)); + } + + t = t0; + + // If the intersection is behind the origin of the ray, we return no hit + return (t >= decimal(0.0)); } // Raycast method with feedback information +/// Algorithm based on the one described at page 194 in Real-ime Collision Detection by +/// Morgan Kaufmann. bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, decimal distance) const { - // TODO : Normalize the ray direction + // Transform the ray direction and origin in local-space coordinates + const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform(); + const Transform worldToLocalTransform = localToWorldTransform.getInverse(); + Vector3 origin = worldToLocalTransform * ray.origin; + Vector3 n = worldToLocalTransform.getOrientation() * ray.direction.getUnit(); - // TODO : Implement this method - return false; + const decimal epsilon = decimal(0.00001); + Vector3 p(decimal(0), -mHalfHeight, decimal(0)); + Vector3 q(decimal(0), mHalfHeight, decimal(0)); + Vector3 d = q - p; + Vector3 m = origin - p; + decimal t; + + decimal mDotD = m.dot(d); + decimal nDotD = n.dot(d); + decimal dDotD = d.dot(d); + decimal mDotN = m.dot(n); + + decimal a = dDotD - nDotD * nDotD; + decimal k = m.dot(m) - mRadius * mRadius; + decimal c = dDotD * k - mDotD * mDotD; + + // If the ray is parallel to the cylinder axis + if (std::abs(a) < epsilon) { + + // If the origin is outside the surface of the cylinder, we return no hit + if (c > decimal(0.0)) return false; + + // Here we know that the segment intersect an endcap of the cylinder + + // If the ray intersects with the "p" endcap of the cylinder + if (mDotD < decimal(0.0)) { + + t = -mDotN; + + // If the intersection is behind the origin of the ray or beyond the maximum + // raycasting distance, we return no hit + if (t < decimal(0.0) || t > distance) return false; + + // Compute the hit information + Vector3 localHitPoint = origin + t * n; + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.distance = t; + raycastInfo.worldPoint = localToWorldTransform * localHitPoint; + Vector3 v = localHitPoint - p; + Vector3 w = v.dot(d) * d.getUnit(); + Vector3 normalDirection = (localHitPoint - (p + w)).getUnit(); + raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + + return true; + } + else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder + + t = (nDotD - mDotN); + + // If the intersection is behind the origin of the ray or beyond the maximum + // raycasting distance, we return no hit + if (t < decimal(0.0) || t > distance) return false; + + // Compute the hit information + Vector3 localHitPoint = origin + t * n; + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.distance = t; + raycastInfo.worldPoint = localToWorldTransform * localHitPoint; + Vector3 v = localHitPoint - p; + Vector3 w = v.dot(d) * d.getUnit(); + Vector3 normalDirection = (localHitPoint - (p + w)).getUnit(); + raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + + return true; + } + else { // If the origin is inside the cylinder, we return no hit + return false; + } + } + decimal b = dDotD * mDotN - nDotD * mDotD; + decimal discriminant = b * b - a * c; + + // If the discriminant is negative, no real roots and therfore, no hit + if (discriminant < decimal(0.0)) return false; + + // Compute the smallest root (first intersection along the ray) + decimal t0 = t = (-b - std::sqrt(discriminant)) / a; + + // If the intersection is outside the cylinder on "p" endcap side + decimal value = mDotD + t * nDotD; + if (value < decimal(0.0)) { + + // If the ray is pointing away from the "p" endcap, we return no hit + if (nDotD <= decimal(0.0)) return false; + + // Compute the intersection against the "p" endcap (intersection agains whole plane) + t = -mDotD / nDotD; + + // Keep the intersection if the it is inside the cylinder radius + if (k + t * (decimal(2.0) * mDotN + t) > decimal(0.0)) return false; + + // If the intersection is behind the origin of the ray or beyond the maximum + // raycasting distance, we return no hit + if (t < decimal(0.0) || t > distance) return false; + + // Compute the hit information + Vector3 localHitPoint = origin + t * n; + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.distance = t; + raycastInfo.worldPoint = localToWorldTransform * localHitPoint; + Vector3 v = localHitPoint - p; + Vector3 w = v.dot(d) * d.getUnit(); + Vector3 normalDirection = (localHitPoint - (p + w)).getUnit(); + raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + + return true; + } + else if (value > dDotD) { // If the intersection is outside the cylinder on the "q" side + + // If the ray is pointing away from the "q" endcap, we return no hit + if (nDotD >= decimal(0.0)) return false; + + // Compute the intersection against the "q" endcap (intersection against whole plane) + t = (dDotD - mDotD) / nDotD; + + // Keep the intersection if it is inside the cylinder radius + if (k + dDotD - decimal(2.0) * mDotD + t * (decimal(2.0) * (mDotN - nDotD) + t) > + decimal(0.0)) return false; + + // If the intersection is behind the origin of the ray or beyond the maximum + // raycasting distance, we return no hit + if (t < decimal(0.0) || t > distance) return false; + + // Compute the hit information + Vector3 localHitPoint = origin + t * n; + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.distance = t; + raycastInfo.worldPoint = localToWorldTransform * localHitPoint; + Vector3 v = localHitPoint - p; + Vector3 w = v.dot(d) * d.getUnit(); + Vector3 normalDirection = (localHitPoint - (p + w)).getUnit(); + raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + + return true; + } + + t = t0; + + // If the intersection is behind the origin of the ray or beyond the maximum + // raycasting distance, we return no hit + if (t < decimal(0.0) || t > distance) return false; + + // Compute the hit information + Vector3 localHitPoint = origin + t * n; + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.distance = t; + raycastInfo.worldPoint = localToWorldTransform * localHitPoint; + Vector3 v = localHitPoint - p; + Vector3 w = v.dot(d) * d.getUnit(); + Vector3 normalDirection = (localHitPoint - (p + w)).getUnit(); + raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + + return true; } diff --git a/test/main.cpp b/test/main.cpp index d8934b2a..78564a99 100644 --- a/test/main.cpp +++ b/test/main.cpp @@ -51,7 +51,7 @@ int main() { // ---------- Collision Detection tests ---------- // - testSuite.addTest(new TestPointInside("Is Point Inside")); + testSuite.addTest(new TestPointInside("IsPointInside")); testSuite.addTest(new TestRaycast("Raycasting")); // Run the tests diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index 09eeffee..2f85fba8 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -1026,41 +1026,71 @@ class TestRaycast : public Test { void testCylinder() { // ----- Test feedback data ----- // - Vector3 origin = mLocalShapeToWorld * Vector3(0 , 10, 0); + Vector3 origin = mLocalShapeToWorld * Vector3(6 , 1, 0); const Matrix3x3 mLocalToWorldMatrix = mLocalShapeToWorld.getOrientation().getMatrix(); - Vector3 direction = mLocalToWorldMatrix * Vector3(0, -3, 0); + Vector3 direction = mLocalToWorldMatrix * Vector3(-2, 0, 0); Ray ray(origin, direction); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(0, 7, 0); + Vector3 hitPoint = mLocalShapeToWorld * Vector3(2, 1, 0); + + Vector3 origin2 = mLocalShapeToWorld * Vector3(0 , 10, 0); + Vector3 direction2 = mLocalToWorldMatrix * Vector3(0, -3, 0); + Ray rayTop(origin2, direction2); + Vector3 hitPointTop = mLocalShapeToWorld * Vector3(0, decimal(2.5), 0); + + Vector3 origin3 = mLocalShapeToWorld * Vector3(0 , -10, 0); + Vector3 direction3 = mLocalToWorldMatrix * Vector3(0, 3, 0); + Ray rayBottom(origin3, direction3); + Vector3 hitPointBottom = mLocalShapeToWorld * Vector3(0, decimal(-2.5), 0); // CollisionWorld::raycast() RaycastInfo raycastInfo; test(mWorld->raycast(ray, raycastInfo)); test(raycastInfo.body == mCylinderBody); test(raycastInfo.proxyShape == mCylinderShape); - test(approxEqual(raycastInfo.distance, 6)); - test(approxEqual(raycastInfo.worldPoint.x, hitPoint.x)); - test(approxEqual(raycastInfo.worldPoint.y, hitPoint.y)); - test(approxEqual(raycastInfo.worldPoint.z, hitPoint.z)); + test(approxEqual(raycastInfo.distance, 4, epsilon)); + test(approxEqual(raycastInfo.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo.worldPoint.z, hitPoint.z, epsilon)); // CollisionBody::raycast() RaycastInfo raycastInfo2; test(mCylinderBody->raycast(ray, raycastInfo2)); test(raycastInfo2.body == mCylinderBody); test(raycastInfo2.proxyShape == mCylinderShape); - test(approxEqual(raycastInfo2.distance, 6)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z)); + test(approxEqual(raycastInfo2.distance, 4, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo3; test(mCylinderShape->raycast(ray, raycastInfo3)); test(raycastInfo3.body == mCylinderBody); test(raycastInfo3.proxyShape == mCylinderShape); - test(approxEqual(raycastInfo3.distance, 6)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z)); + test(approxEqual(raycastInfo3.distance, 4, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo5; + test(mCylinderShape->raycast(rayTop, raycastInfo5)); + test(raycastInfo5.body == mCylinderBody); + test(raycastInfo5.proxyShape == mCylinderShape); + test(approxEqual(raycastInfo5.distance, decimal(7.5), epsilon)); + test(approxEqual(raycastInfo5.worldPoint.x, hitPointTop.x, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.y, hitPointTop.y, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.z, hitPointTop.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo6; + test(mCylinderShape->raycast(rayBottom, raycastInfo6)); + test(raycastInfo6.body == mCylinderBody); + test(raycastInfo6.proxyShape == mCylinderShape); + test(approxEqual(raycastInfo6.distance, decimal(7.5), epsilon)); + test(approxEqual(raycastInfo6.worldPoint.x, hitPointBottom.x, epsilon)); + test(approxEqual(raycastInfo6.worldPoint.y, hitPointBottom.y, epsilon)); + test(approxEqual(raycastInfo6.worldPoint.z, hitPointBottom.z, epsilon)); Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalToWorldMatrix * Vector3(5, 7, -1)); Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalToWorldMatrix * Vector3(4, 6, 7)); @@ -1072,7 +1102,7 @@ class TestRaycast : public Test { Ray ray8(mLocalShapeToWorld * Vector3(-4, 9, 0), mLocalToWorldMatrix * Vector3(1, 0, 0)); Ray ray9(mLocalShapeToWorld * Vector3(0, -9, -4), mLocalToWorldMatrix * Vector3(0, 5, 0)); Ray ray10(mLocalShapeToWorld * Vector3(-4, 0, -6), mLocalToWorldMatrix * Vector3(0, 0, 8)); - Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 2), mLocalToWorldMatrix * Vector3(-4, 0, 0)); + Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 1.5), mLocalToWorldMatrix * Vector3(-4, 0, 0)); Ray ray12(mLocalShapeToWorld * Vector3(1, 9, -1), mLocalToWorldMatrix * Vector3(0, -3, 0)); Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 3), mLocalToWorldMatrix * Vector3(0, 0, -8)); Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -2), mLocalToWorldMatrix * Vector3(4, 0, 0));