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

Change raycasting so that a ray is given by two points instead of a point and a direction

Browse Source
This commit is contained in:
Daniel Chappuis 2014-10-21 22:26:40 +02:00
parent 08e286d27c
commit e9257ec56f
27 changed files with 463 additions and 1212 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/body/Body.h
View File

@ -34,6 +34,7 @@
/// Namespace reactphysics3d
namespace reactphysics3d {
// TODO : Make this class abstract
// Class Body
/**
* This class is an abstract class to represent a body of the physics engine.
@ -53,6 +54,7 @@ class Body {
/// True if the body is allowed to go to sleep for better efficiency
bool mIsAllowedToSleep;
// TODO : Use this variable to make bodies active or not
/// True if the body is active
bool mIsActive;

20
src/body/CollisionBody.cpp
View File

@ -212,31 +212,19 @@ bool CollisionBody::testPointInside(const Vector3& worldPoint) const {
return false;
}
// Raycast method
bool CollisionBody::raycast(const Ray& ray) {
// For each collision shape of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Test if the ray hits the collision shape
if (shape->raycast(ray)) return true;
}
return false;
}
// Raycast method with feedback information
/// The method returns the closest hit among all the collision shapes of the body
bool CollisionBody::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) {
bool CollisionBody::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
bool isHit = false;
Ray rayTemp(ray);
// For each collision shape of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Test if the ray hits the collision shape
if (shape->raycast(ray, raycastInfo, distance)) {
distance = raycastInfo.distance;
if (shape->raycast(rayTemp, raycastInfo)) {
rayTemp.maxFraction = raycastInfo.hitFraction;
isHit = true;
}
}

6
src/body/CollisionBody.h
View File

@ -165,12 +165,8 @@ class CollisionBody : public Body {
/// Return true if a point is inside the collision body
bool testPointInside(const Vector3& worldPoint) const;
/// Raycast method
bool raycast(const Ray& ray);
/// Raycast method with feedback information
bool raycast(const Ray& ray, RaycastInfo& raycastInfo,
decimal distance = RAYCAST_INFINITY_DISTANCE);
bool raycast(const Ray& ray, RaycastInfo& raycastInfo);
// -------------------- Friendship -------------------- //

15
src/collision/ProxyShape.h
View File

@ -99,12 +99,8 @@ class ProxyShape {
/// Return true if a point is inside the collision shape
bool testPointInside(const Vector3& worldPoint);
/// Raycast method
bool raycast(const Ray& ray);
/// Raycast method with feedback information
bool raycast(const Ray& ray, RaycastInfo& raycastInfo,
decimal distance = RAYCAST_INFINITY_DISTANCE);
bool raycast(const Ray& ray, RaycastInfo& raycastInfo);
// -------------------- Friendship -------------------- //
@ -169,14 +165,9 @@ inline decimal ProxyShape::getMargin() const {
return mCollisionShape->getMargin();
}
// Raycast method
inline bool ProxyShape::raycast(const Ray& ray) {
return mCollisionShape->raycast(ray, this);
}
// Raycast method with feedback information
inline bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) {
return mCollisionShape->raycast(ray, raycastInfo, this, distance);
inline bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
return mCollisionShape->raycast(ray, raycastInfo, this);
}
}

5
src/collision/RaycastInfo.h
View File

@ -63,8 +63,9 @@ struct RaycastInfo {
/// Surface normal at hit point in world-space coordinates
Vector3 worldNormal;
/// Distance from the ray origin to the hit point
decimal distance;
/// Fraction distance of the hit point between point1 and point2 of the ray
/// The hit point "p" is such that p = point1 + hitFraction * (point2 - point1)
decimal hitFraction;
/// Pointer to the hit collision body
CollisionBody* body;

22
src/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

@ -397,8 +397,7 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* collis
// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
/// This method implements the GJK ray casting algorithm described by Gino Van Den Bergen in
/// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection".
bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* collisionShape, RaycastInfo& raycastInfo,
decimal maxDistance) {
bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* collisionShape, RaycastInfo& raycastInfo) {
Vector3 suppA; // Current lower bound point on the ray (starting at ray's origin)
Vector3 suppB; // Support point on the collision shape
@ -408,8 +407,13 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* collisionShape, RaycastIn
// Convert the ray origin and direction into the local-space of the collision shape
const Transform localToWorldTransform = collisionShape->getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
Vector3 origin = worldToLocalTransform * ray.origin;
Vector3 rayDirection = worldToLocalTransform.getOrientation() * ray.direction.getUnit();
Vector3 point1 = worldToLocalTransform * ray.point1;
Vector3 point2 = worldToLocalTransform * ray.point2;
Vector3 rayDirection = point2 - point1;
// If the points of the segment are two close, return no hit
if (rayDirection.lengthSquare() < machineEpsilonSquare) return false;
Vector3 w;
// Create a simplex set
@ -417,7 +421,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* collisionShape, RaycastIn
Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0));
decimal lambda = decimal(0.0);
suppA = origin; // Current lower bound point on the ray (starting at ray's origin)
suppA = point1; // Current lower bound point on the ray (starting at ray's origin)
suppB = collisionShape->getLocalSupportPointWithoutMargin(rayDirection);
Vector3 v = suppA - suppB;
decimal vDotW, vDotR;
@ -444,7 +448,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* collisionShape, RaycastIn
// We have found a better lower bound for the hit point along the ray
lambda = lambda - vDotW / vDotR;
suppA = origin + lambda * rayDirection;
suppA = point1 + lambda * rayDirection;
w = suppA - suppB;
n = v;
}
@ -465,7 +469,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* collisionShape, RaycastIn
}
// If the current lower bound distance is larger than the maximum raycasting distance
if (lambda > maxDistance) return false;
if (lambda > ray.maxFraction) return false;
nbIterations++;
}
@ -478,8 +482,8 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* collisionShape, RaycastIn
Vector3 pointB;
simplex.computeClosestPointsOfAandB(pointA, pointB);
// A raycast hit has been found, we fill in the raycast info object
raycastInfo.distance = lambda;
// A raycast hit has been found, we fill in the raycast info
raycastInfo.hitFraction = lambda;
raycastInfo.worldPoint = localToWorldTransform * pointB;
raycastInfo.body = collisionShape->getBody();
raycastInfo.proxyShape = collisionShape;

3
src/collision/narrowphase/GJK/GJKAlgorithm.h
View File

@ -99,8 +99,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
bool testPointInside(const Vector3& localPoint, ProxyShape* collisionShape);
/// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
bool raycast(const Ray& ray, ProxyShape* collisionShape, RaycastInfo& raycastInfo,
decimal maxDistance);
bool raycast(const Ray& ray, ProxyShape* collisionShape, RaycastInfo& raycastInfo);
};
}

77
src/collision/shapes/BoxShape.cpp
View File

@ -45,6 +45,7 @@ BoxShape::BoxShape(const BoxShape& shape) : CollisionShape(shape), mExtent(shape
}
// Destructor
BoxShape::~BoxShape() {
@ -62,61 +63,15 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const
0.0, 0.0, factor * (xSquare + ySquare));
}
// Raycast method
bool BoxShape::raycast(const Ray& ray, ProxyShape* proxyShape) const {
const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
Vector3 origin = worldToLocalTransform * ray.origin;
Vector3 rayDirection = worldToLocalTransform.getOrientation() * ray.direction.getUnit();
decimal tMin = decimal(0.0);
decimal tMax = DECIMAL_LARGEST;
// For each of the three slabs
for (int i=0; i<3; i++) {
// If ray is parallel to the slab
if (std::abs(rayDirection[i]) < MACHINE_EPSILON) {
// If the ray's origin is not inside the slab, there is no hit
if (origin[i] > mExtent[i] || origin[i] < -mExtent[i]) return false;
}
else {
// Compute the intersection of the ray with the near and far plane of the slab
decimal oneOverD = decimal(1.0) / rayDirection[i];
decimal t1 = (-mExtent[i] - origin[i]) * oneOverD;
decimal t2 = (mExtent[i] - origin [i]) * oneOverD;
// Swap t1 and t2 if need so that t1 is intersection with near plane and
// t2 with far plane
if (t1 > t2) std::swap(t1, t2);
// If t1 is negative, the origin is inside the box and therefore, there is no hit
if (t1 < decimal(0.0)) return false;
// Compute the intersection of the of slab intersection interval with previous slabs
tMin = std::max(tMin, t1);
tMax = std::min(tMax, t2);
// If the slabs intersection is empty, there is no hit
if (tMin > tMax) return false;
}
}
// A hit has been found
return true;
}
// Raycast method with feedback information
bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance) const {
bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
Vector3 origin = worldToLocalTransform * ray.origin;
Vector3 rayDirection = worldToLocalTransform.getOrientation() * ray.direction.getUnit();
decimal tMin = decimal(0.0);
const Vector3 point1 = worldToLocalTransform * ray.point1;
const Vector3 point2 = worldToLocalTransform * ray.point2;
Vector3 rayDirection = point2 - point1;
decimal tMin = DECIMAL_SMALLEST;
decimal tMax = DECIMAL_LARGEST;
Vector3 normalDirection(decimal(0), decimal(0), decimal(0));
Vector3 currentNormal;
@ -128,14 +83,14 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro
if (std::abs(rayDirection[i]) < MACHINE_EPSILON) {
// If the ray's origin is not inside the slab, there is no hit
if (origin[i] > mExtent[i] || origin[i] < -mExtent[i]) return false;
if (point1[i] > mExtent[i] || point1[i] < -mExtent[i]) return false;
}
else {
// Compute the intersection of the ray with the near and far plane of the slab
decimal oneOverD = decimal(1.0) / rayDirection[i];
decimal t1 = (-mExtent[i] - origin[i]) * oneOverD;
decimal t2 = (mExtent[i] - origin [i]) * oneOverD;
decimal t1 = (-mExtent[i] - point1[i]) * oneOverD;
decimal t2 = (mExtent[i] - point1 [i]) * oneOverD;
currentNormal = -mExtent;
// Swap t1 and t2 if need so that t1 is intersection with near plane and
@ -145,9 +100,6 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro
currentNormal = -currentNormal;
}
// If t1 is negative, the origin is inside the box and therefore, there is no hit
if (t1 < decimal(0.0)) return false;
// Compute the intersection of the of slab intersection interval with previous slabs
if (t1 > tMin) {
tMin = t1;
@ -155,20 +107,23 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro
}
tMax = std::min(tMax, t2);
// If tMin is larger than the maximum raycasting distance, we return no hit
if (tMin > distance) return false;
// If tMin is larger than the maximum raycasting fraction, we return no hit
if (tMin > ray.maxFraction) return false;
// If the slabs intersection is empty, there is no hit
if (tMin > tMax) return false;
}
}
// If tMin is negative, we return no hit
if (tMin < decimal(0.0)) return false;
// The ray intersects the three slabs, we compute the hit point
Vector3 localHitPoint = origin + tMin * rayDirection;
Vector3 localHitPoint = point1 + tMin * rayDirection;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.distance = tMin;
raycastInfo.hitFraction = tMin;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;

6
src/collision/shapes/BoxShape.h
View File

@ -78,12 +78,8 @@ class BoxShape : public CollisionShape {
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method
virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
public :

186
src/collision/shapes/CapsuleShape.cpp
View File

@ -143,103 +143,37 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
(xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius;
}
// Raycast method
bool CapsuleShape::raycast(const Ray& ray, ProxyShape* proxyShape) const {
// 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();
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 {
bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
// 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();
const Vector3 point1 = worldToLocalTransform * ray.point1;
const Vector3 point2 = worldToLocalTransform * ray.point2;
const Vector3 n = point2 - point1;
const decimal epsilon = decimal(0.00001);
const decimal epsilon = decimal(0.01);
Vector3 p(decimal(0), -mHalfHeight, decimal(0));
Vector3 q(decimal(0), mHalfHeight, decimal(0));
Vector3 d = q - p;
Vector3 m = origin - p;
Vector3 m = point1 - p;
decimal t;
decimal mDotD = m.dot(d);
decimal nDotD = n.dot(d);
decimal dDotD = d.dot(d);
// Test if the segment is outside the cylinder
decimal vec1DotD = (point1 - Vector3(decimal(0.0), -mHalfHeight - mRadius, decimal(0.0))).dot(d);
if (vec1DotD < decimal(0.0) && vec1DotD + nDotD < decimal(0.0)) return false;
decimal ddotDExtraCaps = decimal(2.0) * mRadius * d.y;
if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) return false;
decimal nDotN = n.dot(n);
decimal mDotN = m.dot(n);
decimal a = dDotD - nDotD * nDotD;
decimal a = dDotD * nDotN - nDotD * nDotD;
decimal k = m.dot(m) - mRadius * mRadius;
decimal c = dDotD * k - mDotD * mDotD;
@ -256,11 +190,11 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
// 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)) {
decimal hitFraction;
if (raycastWithSphereEndCap(point1, point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.distance = hitDistance;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
Vector3 normalDirection = (hitLocalPoint - p).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
@ -274,11 +208,11 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
// 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)) {
decimal hitFraction;
if (raycastWithSphereEndCap(point1, point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.distance = hitDistance;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
Vector3 normalDirection = (hitLocalPoint - q).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
@ -307,11 +241,11 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
// 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)) {
decimal hitFraction;
if (raycastWithSphereEndCap(point1, point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.distance = hitDistance;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
Vector3 normalDirection = (hitLocalPoint - p).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
@ -325,11 +259,11 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
// 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)) {
decimal hitFraction;
if (raycastWithSphereEndCap(point1, point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.distance = hitDistance;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
Vector3 normalDirection = (hitLocalPoint - q).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
@ -344,13 +278,13 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
// 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;
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = origin + t * n;
Vector3 localHitPoint = point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.distance = t;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
Vector3 v = localHitPoint - p;
Vector3 w = (v.dot(d) / d.lengthSquare()) * d;
@ -361,64 +295,46 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
}
// 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 {
bool CapsuleShape::raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,
const Vector3& sphereCenter, decimal maxFraction,
Vector3& hitLocalPoint, decimal& hitFraction) const {
Vector3 m = rayOrigin - sphereCenter;
const Vector3 m = point1 - 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;
const Vector3 rayDirection = point2 - point1;
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;
// is pointing away from the sphere, there is no intersection
if (b > decimal(0.0)) return false;
decimal raySquareLength = rayDirection.lengthSquare();
// Compute the discriminant of the quadratic equation
decimal discriminant = b * b - c;
decimal discriminant = b * b - raySquareLength * c;
// If the discriminant is negative, there is no intersection
if (discriminant < decimal(0.0)) return false;
// If the discriminant is negative or the ray length is very small, there is no intersection
if (discriminant < decimal(0.0) || raySquareLength < MACHINE_EPSILON) 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;
// If the hit point is withing the segment ray fraction
if (t < maxFraction * raySquareLength) {
// Compute the hit point and distance
hitLocalPoint = rayOrigin + t * rayDirection;
hitDistance = t;
// Compute the intersection information
t /= raySquareLength;
hitFraction = t;
hitLocalPoint = point1 + t * rayDirection;
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));
}
return false;
}

16
src/collision/shapes/CapsuleShape.h
View File

@ -75,21 +75,13 @@ class CapsuleShape : public CollisionShape {
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method
virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) 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;
bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,
const Vector3& sphereCenter, decimal maxFraction,
Vector3& hitLocalPoint, decimal& hitFraction) const;
public :

6
src/collision/shapes/CollisionShape.h
View File

@ -85,12 +85,8 @@ class CollisionShape {
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0;
/// Raycast method
virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const=0;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance = RAYCAST_INFINITY_DISTANCE) const=0;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const=0;
public :

126
src/collision/shapes/ConeShape.cpp
View File

@ -95,120 +95,18 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
return supportPoint;
}
// Raycast method
// This implementation is based on the technique described by David Eberly in the article
// "Intersection of a Line and a Cone" that can be found at
// http://www.geometrictools.com/Documentation/IntersectionLineCone.pdf
bool ConeShape::raycast(const Ray& ray, ProxyShape* proxyShape) const {
// 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 r = worldToLocalTransform.getOrientation() * ray.direction.getUnit();
const decimal epsilon = decimal(0.00001);
Vector3 V(0, mHalfHeight, 0);
Vector3 centerBase(0, -mHalfHeight, 0);
Vector3 axis(0, decimal(-1.0), 0);
decimal heightSquare = decimal(4.0) * mHalfHeight * mHalfHeight;
decimal cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius);
decimal factor = decimal(1.0) - cosThetaSquare;
Vector3 delta = origin - V;
decimal c0 = -cosThetaSquare * delta.x * delta.x + factor * delta.y * delta.y -
cosThetaSquare * delta.z * delta.z;
decimal c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z;
decimal c2 = -cosThetaSquare * r.x * r.x + factor * r.y * r.y - cosThetaSquare * r.z * r.z;
decimal tHit[] = {decimal(-1.0), decimal(-1.0), decimal(-1.0)};
Vector3 localHitPoint[3];
// If c2 is different from zero
if (std::abs(c2) > MACHINE_EPSILON) {
decimal gamma = c1 * c1 - c0 * c2;
// If there is no real roots in the quadratic equation
if (gamma < decimal(0.0)) {
return false;
}
else if (gamma > decimal(0.0)) { // The equation has two real roots
// Compute two intersections
decimal sqrRoot = std::sqrt(gamma);
tHit[0] = (-c1 - sqrRoot) / c2;
tHit[1] = (-c1 + sqrRoot) / c2;
}
else { // If the equation has a single real root
// Compute the intersection
tHit[0] = -c1 / c2;
}
}
else { // If c2 == 0
// If c2 = 0 and c1 != 0
if (std::abs(c1) > MACHINE_EPSILON) {
tHit[0] = -c0 / (decimal(2.0) * c1);
}
else { // If c2 = c1 = 0
// If c0 is different from zero, no solution and if c0 = 0, we have a
// degenerate case, the whole ray is contained in the cone side
// but we return no hit in this case
return false;
}
}
// If the origin of the ray is inside the cone, we return no hit
if (testPointInside(origin, NULL)) return false;
localHitPoint[0] = origin + tHit[0] * r;
localHitPoint[1] = origin + tHit[1] * r;
// Only keep hit points in one side of the double cone (the cone we are interested in)
if (axis.dot(localHitPoint[0] - V) < decimal(0.0)) {
tHit[0] = decimal(-1.0);
}
if (axis.dot(localHitPoint[1] - V) < decimal(0.0)) {
tHit[1] = decimal(-1.0);
}
// Only keep hit points that are within the correct height of the cone
if (localHitPoint[0].y < decimal(-mHalfHeight)) {
tHit[0] = decimal(-1.0);
}
if (localHitPoint[1].y < decimal(-mHalfHeight)) {
tHit[1] = decimal(-1.0);
}
if (tHit[0] >= decimal(0.0) || tHit[1] >= decimal(0.0)) return true;
// If the ray is in direction of the base plane of the cone
if (r.y > epsilon) {
// Compute the intersection with the base plane of the cone
tHit[2] = (-mHalfHeight + origin.y) / (-r.y);
// Only keep this intersection if it is inside the cone radius
localHitPoint[2] = origin + tHit[2] * r;
return ((localHitPoint[2] - centerBase).lengthSquare() <= mRadius * mRadius);
}
return false;
}
// Raycast method with feedback information
// This implementation is based on the technique described by David Eberly in the article
// "Intersection of a Line and a Cone" that can be found at
// http://www.geometrictools.com/Documentation/IntersectionLineCone.pdf
bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal maxDistance) const {
bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
// 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 r = worldToLocalTransform.getOrientation() * ray.direction.getUnit();
const Vector3 point1 = worldToLocalTransform * ray.point1;
const Vector3 point2 = worldToLocalTransform * ray.point2;
const Vector3 r = point2 - point1;
const decimal epsilon = decimal(0.00001);
Vector3 V(0, mHalfHeight, 0);
@ -217,7 +115,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
decimal heightSquare = decimal(4.0) * mHalfHeight * mHalfHeight;
decimal cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius);
decimal factor = decimal(1.0) - cosThetaSquare;
Vector3 delta = origin - V;
Vector3 delta = point1 - V;
decimal c0 = -cosThetaSquare * delta.x * delta.x + factor * delta.y * delta.y -
cosThetaSquare * delta.z * delta.z;
decimal c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z;
@ -263,10 +161,10 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
}
// If the origin of the ray is inside the cone, we return no hit
if (testPointInside(origin, NULL)) return false;
if (testPointInside(point1, NULL)) return false;
localHitPoint[0] = origin + tHit[0] * r;
localHitPoint[1] = origin + tHit[1] * r;
localHitPoint[0] = point1 + tHit[0] * r;
localHitPoint[1] = point1 + tHit[1] * r;
// Only keep hit points in one side of the double cone (the cone we are interested in)
if (axis.dot(localHitPoint[0] - V) < decimal(0.0)) {
@ -288,10 +186,10 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
if (r.y > epsilon) {
// Compute the intersection with the base plane of the cone
tHit[2] = (-origin.y - mHalfHeight) / (r.y);
tHit[2] = (-point1.y - mHalfHeight) / (r.y);
// Only keep this intersection if it is inside the cone radius
localHitPoint[2] = origin + tHit[2] * r;
localHitPoint[2] = point1 + tHit[2] * r;
if ((localHitPoint[2] - centerBase).lengthSquare() > mRadius * mRadius) {
tHit[2] = decimal(-1.0);
@ -313,7 +211,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
}
if (hitIndex < 0) return false;
if (t > maxDistance) return false;
if (t > ray.maxFraction) return false;
// Compute the normal direction for hit against side of the cone
if (hitIndex != 2) {
@ -329,7 +227,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.distance = t;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint[hitIndex];
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * localNormal[hitIndex];

6
src/collision/shapes/ConeShape.h
View File

@ -83,12 +83,8 @@ class ConeShape : public CollisionShape {
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method
virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal maxDistance = RAYCAST_INFINITY_DISTANCE) const;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
public :

12
src/collision/shapes/ConvexMeshShape.cpp
View File

@ -230,16 +230,8 @@ bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const
return true;
}
// Raycast method
bool ConvexMeshShape::raycast(const Ray& ray, ProxyShape* proxyShape) const {
RaycastInfo raycastInfo;
return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast(
ray, proxyShape, raycastInfo, RAYCAST_INFINITY_DISTANCE);
}
// Raycast method with feedback information
bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance) const {
bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast(
ray, proxyShape, raycastInfo, distance);
ray, proxyShape, raycastInfo);
}

6
src/collision/shapes/ConvexMeshShape.h
View File

@ -104,12 +104,8 @@ class ConvexMeshShape : public CollisionShape {
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method
virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
public :

145
src/collision/shapes/CylinderShape.cpp
View File

@ -88,122 +88,37 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio
return supportPoint;
}
// 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 {
// 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();
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;
return (t >= decimal(0.0));
}
else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder
t = (nDotD - mDotN);
return (t >= decimal(0.0));
}
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 (t >= decimal(0.0) && 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 (t >= decimal(0.0) && k + dDotD - decimal(2.0) * mDotD + t * (decimal(2.0) *
(mDotN - nDotD) + t) <= decimal(0.0));
}
// If the intersection is behind the origin of the ray, we return no hit
return (t0 >= 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 {
bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
// 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();
const Vector3 pointA = worldToLocalTransform * ray.point1;
const Vector3 pointB = worldToLocalTransform * ray.point2;
const Vector3 n = pointB - pointA;
const decimal epsilon = decimal(0.00001);
const decimal epsilon = decimal(0.01);
Vector3 p(decimal(0), -mHalfHeight, decimal(0));
Vector3 q(decimal(0), mHalfHeight, decimal(0));
Vector3 d = q - p;
Vector3 m = origin - p;
Vector3 m = pointA - p;
decimal t;
decimal mDotD = m.dot(d);
decimal nDotD = n.dot(d);
decimal dDotD = d.dot(d);
// Test if the segment is outside the cylinder
if (mDotD < decimal(0.0) && mDotD + nDotD < decimal(0.0)) return false;
if (mDotD > dDotD && mDotD + nDotD > dDotD) return false;
decimal nDotN = n.dot(n);
decimal mDotN = m.dot(n);
decimal a = dDotD - nDotD * nDotD;
decimal a = dDotD * nDotN - nDotD * nDotD;
decimal k = m.dot(m) - mRadius * mRadius;
decimal c = dDotD * k - mDotD * mDotD;
@ -218,17 +133,17 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
// If the ray intersects with the "p" endcap of the cylinder
if (mDotD < decimal(0.0)) {
t = -mDotN;
t = -mDotN / nDotN;
// 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;
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = origin + t * n;
Vector3 localHitPoint = pointA + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.distance = t;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
Vector3 normalDirection(0, decimal(-1), 0);
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
@ -237,17 +152,17 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
}
else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder
t = (nDotD - mDotN);
t = (nDotD - mDotN) / nDotN;
// 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;
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = origin + t * n;
Vector3 localHitPoint = pointA + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.distance = t;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
Vector3 normalDirection(0, decimal(1.0), 0);
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
@ -282,13 +197,13 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
// 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;
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = origin + t * n;
Vector3 localHitPoint = pointA + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.distance = t;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
Vector3 normalDirection(0, decimal(-1.0), 0);
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
@ -309,13 +224,13 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
// 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;
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = origin + t * n;
Vector3 localHitPoint = pointA + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.distance = t;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
Vector3 normalDirection(0, decimal(1.0), 0);
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
@ -327,13 +242,13 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
// 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;
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = origin + t * n;
Vector3 localHitPoint = pointA + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.distance = t;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
Vector3 v = localHitPoint - p;
Vector3 w = (v.dot(d) / d.lengthSquare()) * d;

6
src/collision/shapes/CylinderShape.h
View File

@ -80,12 +80,8 @@ class CylinderShape : public CollisionShape {
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method
virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
public :

69
src/collision/shapes/SphereShape.cpp
View File

@ -47,72 +47,51 @@ SphereShape::~SphereShape() {
}
// Raycast method
bool SphereShape::raycast(const Ray& ray, ProxyShape* proxyShape) const {
const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
Vector3 origin = worldToLocalTransform * ray.origin;
decimal c = origin.dot(origin) - mRadius * mRadius;
// If the origin of the ray is inside the sphere, we return no intersection
if (c < decimal(0.0)) return false;
Vector3 rayDirection = worldToLocalTransform.getOrientation() * ray.direction.getUnit();
decimal b = origin.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));
}
// Raycast method with feedback information
bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance) const {
bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
Vector3 origin = worldToLocalTransform * ray.origin;
decimal c = origin.dot(origin) - mRadius * mRadius;
// We perform the intersection test in world-space
const Vector3 sphereCenter = proxyShape->getLocalToWorldTransform().getPosition();
const Vector3 m = ray.point1 - 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;
Vector3 rayDirection = worldToLocalTransform.getOrientation() * ray.direction.getUnit();
decimal b = origin.dot(rayDirection);
const Vector3 rayDirection = ray.point2 - ray.point1;
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;
// is pointing away from the sphere, there is no intersection
if (b > decimal(0.0)) return false;
decimal raySquareLength = rayDirection.lengthSquare();
// Compute the discriminant of the quadratic equation
decimal discriminant = b*b - c;
decimal discriminant = b * b - raySquareLength * c;
// If the discriminant is negative, there is no intersection
if (discriminant < decimal(0.0)) return false;
// If the discriminant is negative or the ray length is very small, there is no intersection
if (discriminant < decimal(0.0) || raySquareLength < MACHINE_EPSILON) 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 > distance) return false;
// If the hit point is withing the segment ray fraction
if (t < ray.maxFraction * raySquareLength) {
// Compute the intersection information
Vector3 localPoint = origin + t * rayDirection;
t /= raySquareLength;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.distance = t;
raycastInfo.worldPoint = localToWorldTransform * localPoint;
raycastInfo.worldNormal = (raycastInfo.worldPoint -
localToWorldTransform.getPosition()).getUnit();
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = ray.point1 + t * rayDirection;
raycastInfo.worldNormal = (raycastInfo.worldPoint - sphereCenter).getUnit();
return true;
}
return false;
}

6
src/collision/shapes/SphereShape.h
View File

@ -70,12 +70,8 @@ class SphereShape : public CollisionShape {
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method
virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
public :

3
src/configuration.h
View File

@ -131,9 +131,6 @@ const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1);
/// followin constant with the linear velocity and the elapsed time between two frames.
const decimal DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = decimal(1.7);
/// Raycasting infinity distance constant
const decimal RAYCAST_INFINITY_DISTANCE = std::numeric_limits<decimal>::infinity();
}
#endif

8
src/engine/CollisionWorld.cpp
View File

@ -166,14 +166,8 @@ void CollisionWorld::removeCollisionShape(CollisionShape* collisionShape) {
}
}
/// Raycast method
bool CollisionWorld::raycast(const Ray& ray, decimal distance) {
// TODO : Implement this method
return false;
}
/// Raycast method with feedback information
bool CollisionWorld::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) {
bool CollisionWorld::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
// TODO : Implement this method
return false;
}

6
src/engine/CollisionWorld.h
View File

@ -120,12 +120,8 @@ class CollisionWorld {
/// Destroy a collision body
void destroyCollisionBody(CollisionBody* collisionBody);
/// Raycast method
bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE);
/// Raycast method with feedback information
bool raycast(const Ray& ray, RaycastInfo& raycastInfo,
decimal distance = RAYCAST_INFINITY_DISTANCE);
bool raycast(const Ray& ray, RaycastInfo& raycastInfo);
// -------------------- Friendship -------------------- //

26
src/mathematics/Ray.h
View File

@ -34,7 +34,9 @@ namespace reactphysics3d {
// Class Ray
/**
* This structure represents a 3D ray with a origin point and a direction.
* This structure represents a 3D ray represented by two points.
* The ray goes from point1 to point1 + maxFraction * (point2 - point1).
* The points are specified in world-space coordinates.
*/
struct Ray {
@ -42,22 +44,25 @@ struct Ray {
// -------------------- Attributes -------------------- //
/// Origin point of the ray
Vector3 origin;
/// First point of the ray (origin)
Vector3 point1;
/// Direction vector of the ray
Vector3 direction;
/// Second point of the ray
Vector3 point2;
/// Maximum fraction value
decimal maxFraction;
// -------------------- Methods -------------------- //
/// Constructor with arguments
Ray(const Vector3& originPoint, const Vector3& directionVector)
: origin(originPoint), direction(directionVector) {
Ray(const Vector3& p1, const Vector3& p2, decimal maxFrac = decimal(1.0))
: point1(p1), point2(p2), maxFraction(maxFrac) {
}
/// Copy-constructor
Ray(const Ray& ray) : origin(ray.origin), direction(ray.direction) {
Ray(const Ray& ray) : point1(ray.point1), point2(ray.point2), maxFraction(ray.maxFraction) {
}
@ -69,8 +74,9 @@ struct Ray {
/// Overloaded assignment operator
Ray& operator=(const Ray& ray) {
if (&ray != this) {
origin = ray.origin;
direction = ray.direction;
point1 = ray.point1;
point2 = ray.point2;
maxFraction = ray.maxFraction;
}
return *this;
}

1
src/mathematics/mathematics.h
View File

@ -33,6 +33,7 @@
#include "Vector3.h"
#include "Vector2.h"
#include "Transform.h"
#include "Ray.h"
#include "configuration.h"
#include "mathematics_functions.h"
#include <vector>

2
src/reactphysics3d.h
View File

@ -51,6 +51,8 @@
#include "collision/shapes/CapsuleShape.h"
#include "collision/shapes/ConvexMeshShape.h"
#include "collision/shapes/AABB.h"
#include "collision/ProxyShape.h"
#include "collision/RaycastInfo.h"
#include "constraint/BallAndSocketJoint.h"
#include "constraint/SliderJoint.h"
#include "constraint/HingeJoint.h"

879
test/tests/collision/TestRaycast.h
View File

File diff suppressed because it is too large Load Diff