Implement raycasting with cylinder shape
This commit is contained in:
Daniel Chappuis
parent
ebf13c3366
commit
c07a2dc9a2
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user