From 25c11c6d6a2b7eda08324782d6287dc9c78af27b Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 20 Sep 2014 17:00:32 +0200 Subject: [PATCH] Implement raycasting for capsule shape --- src/collision/shapes/CapsuleShape.cpp | 274 +++++++++++++++++++++++++- src/collision/shapes/CapsuleShape.h | 9 + test/tests/collision/TestRaycast.h | 61 ++++-- 3 files changed, 322 insertions(+), 22 deletions(-) diff --git a/src/collision/shapes/CapsuleShape.cpp b/src/collision/shapes/CapsuleShape.cpp index 40433475..d4fa711a 100644 --- a/src/collision/shapes/CapsuleShape.cpp +++ b/src/collision/shapes/CapsuleShape.cpp @@ -25,6 +25,7 @@ // Libraries #include "CapsuleShape.h" +#include "collision/ProxyShape.h" #include "configuration.h" #include @@ -145,18 +146,279 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS // Raycast method bool CapsuleShape::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 capsule + if (mDotD < decimal(0.0)) { + + // Check intersection with the sphere "p" endcap of the capsule + return raycastWithSphereEndCap(origin, n, p); + } + else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder + + // Check intersection with the sphere "q" endcap of the capsule + return raycastWithSphereEndCap(origin, n, q); + } + 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)) { + + // Check intersection with the sphere "p" endcap of the capsule + return raycastWithSphereEndCap(origin, n, p); + } + else if (value > dDotD) { // If the intersection is outside the cylinder on the "q" side + + // Check intersection with the sphere "q" endcap of the capsule + return raycastWithSphereEndCap(origin, n, q); + } + + // If the intersection is behind the origin of the ray, we return no hit + return (t0 >= decimal(0.0)); } // Raycast method with feedback information bool CapsuleShape::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 capsule axis + if (std::abs(a) < epsilon) { + + // If the origin is outside the surface of the capusle's cylinder, we return no hit + if (c > decimal(0.0)) return false; + + // Here we know that the segment intersect an endcap of the capsule + + // If the ray intersects with the "p" endcap of the capsule + if (mDotD < decimal(0.0)) { + + // Check intersection between the ray and the "p" sphere endcap of the capsule + Vector3 hitLocalPoint; + decimal hitDistance; + if (raycastWithSphereEndCap(origin, n, p, distance, hitLocalPoint, hitDistance)) { + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.distance = hitDistance; + raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint; + Vector3 normalDirection = (hitLocalPoint - p).getUnit(); + raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + + return true; + } + + return false; + } + else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder + + // Check intersection between the ray and the "q" sphere endcap of the capsule + Vector3 hitLocalPoint; + decimal hitDistance; + if (raycastWithSphereEndCap(origin, n, q, distance, hitLocalPoint, hitDistance)) { + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.distance = hitDistance; + raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint; + Vector3 normalDirection = (hitLocalPoint - q).getUnit(); + raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + + return true; + } + + return false; + } + 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 finite cylinder of the capsule on "p" endcap side + decimal value = mDotD + t * nDotD; + if (value < decimal(0.0)) { + + // Check intersection between the ray and the "p" sphere endcap of the capsule + Vector3 hitLocalPoint; + decimal hitDistance; + if (raycastWithSphereEndCap(origin, n, p, distance, hitLocalPoint, hitDistance)) { + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.distance = hitDistance; + raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint; + Vector3 normalDirection = (hitLocalPoint - p).getUnit(); + raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + + return true; + } + + return false; + } + else if (value > dDotD) { // If the intersection is outside the finite cylinder on the "q" side + + // Check intersection between the ray and the "q" sphere endcap of the capsule + Vector3 hitLocalPoint; + decimal hitDistance; + if (raycastWithSphereEndCap(origin, n, q, distance, hitLocalPoint, hitDistance)) { + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.distance = hitDistance; + raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint; + Vector3 normalDirection = (hitLocalPoint - q).getUnit(); + raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + + return true; + } + + return false; + } + + 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.lengthSquare()) * d; + Vector3 normalDirection = (localHitPoint - (p + w)).getUnit(); + raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + + return true; +} + +// Raycasting method between a ray one of the two spheres end cap of the capsule +bool CapsuleShape::raycastWithSphereEndCap(const Vector3& rayOrigin, const Vector3& rayDirection, + const Vector3& sphereCenter, decimal maxDistance, + Vector3& hitLocalPoint, decimal& hitDistance) const { + + Vector3 m = rayOrigin - sphereCenter; + decimal c = m.dot(m) - mRadius * mRadius; + + // If the origin of the ray is inside the sphere, we return no intersection + if (c < decimal(0.0)) return false; + + decimal b = m.dot(rayDirection); + + // If the origin of the ray is outside the sphere and the ray + // is pointing away from the sphere and there is no intersection + if (c >= decimal(0.0) && b > decimal(0.0)) return false; + + // Compute the discriminant of the quadratic equation + decimal discriminant = b * b - c; + + // If the discriminant is negative, there is no intersection + if (discriminant < decimal(0.0)) return false; + + // Compute the solution "t" closest to the origin + decimal t = -b - std::sqrt(discriminant); + + assert(t >= decimal(0.0)); + + // If the intersection distance is larger than the allowed distance, return no intersection + if (t > maxDistance) return false; + + // Compute the hit point and distance + hitLocalPoint = rayOrigin + t * rayDirection; + hitDistance = t; + + return true; +} + +// Raycasting method between a ray one of the two spheres end cap of the capsule +/// This method returns true if there is an intersection and false otherwise but does not +/// compute the intersection point. +bool CapsuleShape::raycastWithSphereEndCap(const Vector3& rayOrigin, const Vector3& rayDirection, + const Vector3& sphereCenter) const { + + Vector3 m = rayOrigin - sphereCenter; + decimal c = m.dot(m) - mRadius * mRadius; + + // If the origin of the ray is inside the sphere, we return no intersection + if (c < decimal(0.0)) return false; + + decimal b = m.dot(rayDirection); + + // If the origin of the ray is outside the sphere and the ray + // is pointing away from the sphere and there is no intersection + if (c >= decimal(0.0) && b > decimal(0.0)) return false; + + // Compute the discriminant of the quadratic equation + decimal discriminant = b * b - c; + + // If the discriminant is negative, there is no intersection + return (discriminant >= decimal(0.0)); } diff --git a/src/collision/shapes/CapsuleShape.h b/src/collision/shapes/CapsuleShape.h index 97184a77..15d3f101 100644 --- a/src/collision/shapes/CapsuleShape.h +++ b/src/collision/shapes/CapsuleShape.h @@ -82,6 +82,15 @@ class CapsuleShape : public CollisionShape { virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, decimal distance = RAYCAST_INFINITY_DISTANCE) const; + /// Raycasting method between a ray one of the two spheres end cap of the capsule + bool raycastWithSphereEndCap(const Vector3& rayOrigin, const Vector3& rayDirection, + const Vector3& sphereCenter, decimal maxDistance, + Vector3& hitLocalPoint, decimal& hitDistance) const; + + // Raycasting method between a ray one of the two spheres end cap of the capsule + bool raycastWithSphereEndCap(const Vector3& rayOrigin, const Vector3& rayDirection, + const Vector3& sphereCenter) const; + public : // -------------------- Methods -------------------- // diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index 2f85fba8..a11118b9 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -506,41 +506,70 @@ class TestRaycast : public Test { void testCapsule() { // ----- 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(4.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(-4.5), 0); // CollisionWorld::raycast() RaycastInfo raycastInfo; test(mWorld->raycast(ray, raycastInfo)); test(raycastInfo.body == mCapsuleBody); test(raycastInfo.proxyShape == mCapsuleShape); - 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(mCapsuleBody->raycast(ray, raycastInfo2)); test(raycastInfo2.body == mCapsuleBody); test(raycastInfo2.proxyShape == mCapsuleShape); - 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(mCapsuleShape->raycast(ray, raycastInfo3)); test(raycastInfo3.body == mCapsuleBody); test(raycastInfo3.proxyShape == mCapsuleShape); - 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)); + + RaycastInfo raycastInfo4; + test(mCapsuleShape->raycast(rayTop, raycastInfo4)); + test(raycastInfo4.body == mCapsuleBody); + test(raycastInfo4.proxyShape == mCapsuleShape); + test(approxEqual(raycastInfo4.distance, decimal(5.5), epsilon)); + test(approxEqual(raycastInfo4.worldPoint.x, hitPointTop.x, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.y, hitPointTop.y, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.z, hitPointTop.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo5; + test(mCapsuleShape->raycast(rayBottom, raycastInfo5)); + test(raycastInfo5.body == mCapsuleBody); + test(raycastInfo5.proxyShape == mCapsuleShape); + test(approxEqual(raycastInfo5.distance, decimal(5.5), epsilon)); + test(approxEqual(raycastInfo5.worldPoint.x, hitPointBottom.x, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.y, hitPointBottom.y, epsilon)); + test(approxEqual(raycastInfo5.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)); @@ -552,7 +581,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));