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

Transform ray from world-space to local-space earlier during raycasting

Browse Source
This commit is contained in:
Daniel Chappuis 2015-12-07 17:32:18 +01:00
parent 9c7f70b9b7
commit df88e59017
14 changed files with 137 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/collision/CollisionDetection.h
View File

@ -291,6 +291,8 @@ inline void CollisionDetection::raycast(RaycastCallback* raycastCallback,
const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const {
PROFILE("CollisionDetection::raycast()");
RaycastTest rayCastTest(raycastCallback);
// Ask the broad-phase algorithm to call the testRaycastAgainstShape()

28
src/collision/ProxyShape.cpp
View File

@ -62,3 +62,31 @@ bool ProxyShape::testPointInside(const Vector3& worldPoint) {
return mCollisionShape->testPointInside(localPoint, this);
}
// Raycast method with feedback information
/**
* @param ray Ray to use for the raycasting
* @param[out] raycastInfo Result of the raycasting that is valid only if the
* methods returned true
* @return True if the ray hit the collision shape
*/
bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
// If the corresponding body is not active, it cannot be hit by rays
if (!mBody->isActive()) return false;
// Convert the ray into the local-space of the collision shape
const Transform localToWorldTransform = getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
Ray rayLocal(worldToLocalTransform * ray.point1,
worldToLocalTransform * ray.point2,
ray.maxFraction);
bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this);
// Convert the raycast info into world-space
raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint;
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * raycastInfo.worldNormal;
raycastInfo.worldNormal.normalize();
return isHit;
}

15
src/collision/ProxyShape.h
View File

@ -251,21 +251,6 @@ inline const Transform ProxyShape::getLocalToWorldTransform() const {
return mBody->mTransform * mLocalToBodyTransform;
}
// Raycast method with feedback information
/**
* @param ray Ray to use for the raycasting
* @param[out] raycastInfo Result of the raycasting that is valid only if the
* methods returned true
* @return True if the ray hit the collision shape
*/
inline bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
// If the corresponding body is not active, it cannot be hit by rays
if (!mBody->isActive()) return false;
return mCollisionShape->raycast(ray, raycastInfo, this);
}
// Return the next proxy shape in the linked list of proxy shapes
/**
* @return Pointer to the next proxy shape in the linked list of proxy shapes

3
src/collision/broadphase/BroadPhaseAlgorithm.h
View File

@ -31,6 +31,7 @@
#include "body/CollisionBody.h"
#include "collision/ProxyShape.h"
#include "DynamicAABBTree.h"
#include "engine/Profiler.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
@ -234,6 +235,8 @@ inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1,
inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest,
unsigned short raycastWithCategoryMaskBits) const {
PROFILE("BroadPhaseAlgorithm::raycast()");
BroadPhaseRaycastCallback broadPhaseRaycastCallback(mDynamicAABBTree, raycastWithCategoryMaskBits, raycastTest);
mDynamicAABBTree.raycast(ray, broadPhaseRaycastCallback);

5
src/collision/broadphase/DynamicAABBTree.cpp
View File

@ -633,11 +633,12 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb,
// Ray casting method
void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &callback) const {
PROFILE("DynamicAABBTree::raycast()");
decimal maxFraction = ray.maxFraction;
// Create an AABB for the ray
Vector3 endPoint = ray.point1 +
maxFraction * (ray.point2 - ray.point1);
Vector3 endPoint = ray.point1 + maxFraction * (ray.point2 - ray.point1);
AABB rayAABB(Vector3::min(ray.point1, endPoint),
Vector3::max(ray.point1, endPoint));

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

@ -433,11 +433,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
const decimal epsilon = decimal(0.0001);
// Convert the ray origin and direction into the local-space of the collision shape
const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
Vector3 point1 = worldToLocalTransform * ray.point1;
Vector3 point2 = worldToLocalTransform * ray.point2;
Vector3 rayDirection = point2 - point1;
Vector3 rayDirection = ray.point2 - ray.point1;
// If the points of the segment are two close, return no hit
if (rayDirection.lengthSquare() < machineEpsilonSquare) return false;
@ -449,7 +445,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0));
decimal lambda = decimal(0.0);
suppA = point1; // Current lower bound point on the ray (starting at ray's origin)
suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin)
suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData);
Vector3 v = suppA - suppB;
decimal vDotW, vDotR;
@ -476,7 +472,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
// We have found a better lower bound for the hit point along the ray
lambda = lambda - vDotW / vDotR;
suppA = point1 + lambda * rayDirection;
suppA = ray.point1 + lambda * rayDirection;
w = suppA - suppB;
n = v;
}
@ -512,12 +508,12 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
// A raycast hit has been found, we fill in the raycast info
raycastInfo.hitFraction = lambda;
raycastInfo.worldPoint = localToWorldTransform * pointB;
raycastInfo.worldPoint = pointB;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
if (n.lengthSquare() >= machineEpsilonSquare) { // The normal vector is valid
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * n.getUnit();
raycastInfo.worldNormal = n;
}
else { // Degenerated normal vector, we return a zero normal vector
raycastInfo.worldNormal = Vector3(decimal(0), decimal(0), decimal(0));

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

@ -69,11 +69,7 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const
// Raycast method with feedback information
bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
const Vector3 point1 = worldToLocalTransform * ray.point1;
const Vector3 point2 = worldToLocalTransform * ray.point2;
Vector3 rayDirection = point2 - point1;
Vector3 rayDirection = ray.point2 - ray.point1;
decimal tMin = DECIMAL_SMALLEST;
decimal tMax = DECIMAL_LARGEST;
Vector3 normalDirection(decimal(0), decimal(0), decimal(0));
@ -86,14 +82,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 (point1[i] > mExtent[i] || point1[i] < -mExtent[i]) return false;
if (ray.point1[i] > mExtent[i] || ray.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] - point1[i]) * oneOverD;
decimal t2 = (mExtent[i] - point1[i]) * oneOverD;
decimal t1 = (-mExtent[i] - ray.point1[i]) * oneOverD;
decimal t2 = (mExtent[i] - ray.point1[i]) * oneOverD;
currentNormal[0] = (i == 0) ? -mExtent[i] : decimal(0.0);
currentNormal[1] = (i == 1) ? -mExtent[i] : decimal(0.0);
currentNormal[2] = (i == 2) ? -mExtent[i] : decimal(0.0);
@ -124,14 +120,13 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro
if (tMin < decimal(0.0)) return false;
// The ray intersects the three slabs, we compute the hit point
Vector3 localHitPoint = point1 + tMin * rayDirection;
Vector3 localHitPoint = ray.point1 + tMin * rayDirection;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = tMin;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
normalDirection.normalize();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldPoint = localHitPoint;
raycastInfo.worldNormal = normalDirection;
return true;
}

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

@ -149,18 +149,13 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
// Raycast method with feedback information
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();
const Vector3 point1 = worldToLocalTransform * ray.point1;
const Vector3 point2 = worldToLocalTransform * ray.point2;
const Vector3 n = point2 - point1;
const Vector3 n = ray.point2 - ray.point1;
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 = point1 - p;
Vector3 m = ray.point1 - p;
decimal t;
decimal mDotD = m.dot(d);
@ -168,7 +163,7 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
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);
decimal vec1DotD = (ray.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;
@ -194,13 +189,13 @@ 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 hitFraction;
if (raycastWithSphereEndCap(point1, point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
Vector3 normalDirection = (hitLocalPoint - p).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldPoint = hitLocalPoint;
Vector3 normalDirection = hitLocalPoint - p;
raycastInfo.worldNormal = normalDirection;
return true;
}
@ -212,13 +207,13 @@ 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 hitFraction;
if (raycastWithSphereEndCap(point1, point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
Vector3 normalDirection = (hitLocalPoint - q).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldPoint = hitLocalPoint;
Vector3 normalDirection = hitLocalPoint - q;
raycastInfo.worldNormal = normalDirection;
return true;
}
@ -245,13 +240,13 @@ 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 hitFraction;
if (raycastWithSphereEndCap(point1, point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
Vector3 normalDirection = (hitLocalPoint - p).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldPoint = hitLocalPoint;
Vector3 normalDirection = hitLocalPoint - p;
raycastInfo.worldNormal = normalDirection;
return true;
}
@ -263,13 +258,13 @@ 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 hitFraction;
if (raycastWithSphereEndCap(point1, point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
Vector3 normalDirection = (hitLocalPoint - q).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldPoint = hitLocalPoint;
Vector3 normalDirection = hitLocalPoint - q;
raycastInfo.worldNormal = normalDirection;
return true;
}
@ -284,15 +279,15 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = point1 + t * n;
Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
raycastInfo.worldPoint = localHitPoint;
Vector3 v = localHitPoint - p;
Vector3 w = (v.dot(d) / d.lengthSquare()) * d;
Vector3 normalDirection = (localHitPoint - (p + w)).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldNormal = normalDirection;
return true;
}

4
src/collision/shapes/ConcaveMeshShape.cpp
View File

@ -162,6 +162,8 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB&
/// the ray hits many triangles.
bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
PROFILE("ConcaveMeshShape::raycast()");
// Create the callback object that will compute ray casting against triangles
ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray);
@ -170,6 +172,8 @@ bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh
// in the hit AABBs.
mDynamicAABBTree.raycast(ray, raycastCallback);
raycastCallback.raycastTriangles();
return raycastCallback.getIsHit();
}

1
src/collision/shapes/ConcaveMeshShape.h
View File

@ -31,6 +31,7 @@
#include "collision/broadphase/DynamicAABBTree.h"
#include "collision/TriangleMesh.h"
#include "collision/shapes/TriangleShape.h"
#include "engine/Profiler.h"
namespace reactphysics3d {

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

@ -99,12 +99,7 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
// http://www.geometrictools.com/Documentation/IntersectionLineCone.pdf
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();
const Vector3 point1 = worldToLocalTransform * ray.point1;
const Vector3 point2 = worldToLocalTransform * ray.point2;
const Vector3 r = point2 - point1;
const Vector3 r = ray.point2 - ray.point1;
const decimal epsilon = decimal(0.00001);
Vector3 V(0, mHalfHeight, 0);
@ -113,7 +108,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 = point1 - V;
Vector3 delta = ray.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;
@ -159,10 +154,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(point1, NULL)) return false;
if (testPointInside(ray.point1, NULL)) return false;
localHitPoint[0] = point1 + tHit[0] * r;
localHitPoint[1] = point1 + tHit[1] * r;
localHitPoint[0] = ray.point1 + tHit[0] * r;
localHitPoint[1] = ray.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)) {
@ -184,10 +179,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] = (-point1.y - mHalfHeight) / (r.y);
tHit[2] = (-ray.point1.y - mHalfHeight) / (r.y);
// Only keep this intersection if it is inside the cone radius
localHitPoint[2] = point1 + tHit[2] * r;
localHitPoint[2] = ray.point1 + tHit[2] * r;
if ((localHitPoint[2] - centerBase).lengthSquare() > mRadius * mRadius) {
tHit[2] = decimal(-1.0);
@ -229,8 +224,8 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint[hitIndex];
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * localNormal[hitIndex];
raycastInfo.worldPoint = localHitPoint[hitIndex];
raycastInfo.worldNormal = localNormal[hitIndex];
return true;
}

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

@ -92,18 +92,13 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio
/// Morgan Kaufmann.
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();
const Vector3 pointA = worldToLocalTransform * ray.point1;
const Vector3 pointB = worldToLocalTransform * ray.point2;
const Vector3 n = pointB - pointA;
const Vector3 n = ray.point2 - ray.point1;
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 = pointA - p;
Vector3 m = ray.point1 - p;
decimal t;
decimal mDotD = m.dot(d);
@ -139,13 +134,13 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = pointA + t * n;
Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(-1), 0);
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldNormal = normalDirection;
return true;
}
@ -158,13 +153,13 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = pointA + t * n;
Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(1.0), 0);
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldNormal = normalDirection;
return true;
}
@ -199,13 +194,13 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = pointA + t * n;
Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(-1.0), 0);
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldNormal = normalDirection;
return true;
}
@ -226,13 +221,13 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = pointA + t * n;
Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(1.0), 0);
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldNormal = normalDirection;
return true;
}
@ -244,15 +239,15 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = pointA + t * n;
Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
raycastInfo.worldPoint = localHitPoint;
Vector3 v = localHitPoint - p;
Vector3 w = (v.dot(d) / d.lengthSquare()) * d;
Vector3 normalDirection = (localHitPoint - (p + w)).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
Vector3 normalDirection = (localHitPoint - (p + w));
raycastInfo.worldNormal = normalDirection;
return true;
}

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

@ -47,10 +47,7 @@ SphereShape::~SphereShape() {
// Raycast method with feedback information
bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
// We perform the intersection test in world-space
const Vector3 sphereCenter = proxyShape->getLocalToWorldTransform().getPosition();
const Vector3 m = ray.point1 - sphereCenter;
const Vector3 m = ray.point1;
decimal c = m.dot(m) - mMargin * mMargin;
// If the origin of the ray is inside the sphere, we return no intersection
@ -85,7 +82,7 @@ bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = ray.point1 + t * rayDirection;
raycastInfo.worldNormal = (raycastInfo.worldPoint - sphereCenter).getUnit();
raycastInfo.worldNormal = raycastInfo.worldPoint;
return true;
}

56
src/collision/shapes/TriangleShape.cpp
View File

@ -26,6 +26,7 @@
// Libraries
#include "TriangleShape.h"
#include "collision/ProxyShape.h"
#include "engine/Profiler.h"
#include "configuration.h"
#include <cassert>
@ -56,35 +57,45 @@ TriangleShape::~TriangleShape() {
/// Real-time Collision Detection by Christer Ericson.
bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
// TODO : For all collision shape, try to perform raycasting in local-space and
// compute world-space hit point, normal in upper classes when local-space
// hit points are returned
const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
const Vector3 point1 = worldToLocalTransform * ray.point1;
const Vector3 point2 = worldToLocalTransform * ray.point2;
PROFILE("TriangleShape::raycast()");
const Vector3 pq = point2 - point1;
const Vector3 pa = mPoints[0] - point1;
const Vector3 pb = mPoints[1] - point1;
const Vector3 pc = mPoints[2] - point1;
const Vector3 pq = ray.point2 - ray.point1;
const Vector3 pa = mPoints[0] - ray.point1;
const Vector3 pb = mPoints[1] - ray.point1;
const Vector3 pc = mPoints[2] - ray.point1;
// Test if the line PQ is inside the eges BC, CA and AB. We use the triple
// product for this test.
const Vector3 m = pq.cross(pc);
decimal u = pb.dot(m);
if (mRaycastTestType == FRONT && u < decimal(0.0)) return false;
if (mRaycastTestType == BACK && u > decimal(0.0)) return false;
if (mRaycastTestType == FRONT) {
if (u < decimal(0.0)) return false;
}
else if (mRaycastTestType == BACK) {
if (u > decimal(0.0)) return false;
}
decimal v = -pa.dot(m);
if (mRaycastTestType == FRONT && v < decimal(0.0)) return false;
if (mRaycastTestType == BACK && v > decimal(0.0)) return false;
if (mRaycastTestType == FRONT_AND_BACK && !sameSign(u, v)) return false;
if (mRaycastTestType == FRONT) {
if (v < decimal(0.0)) return false;
}
else if (mRaycastTestType == BACK) {
if (v > decimal(0.0)) return false;
}
else if (mRaycastTestType == FRONT_AND_BACK) {
if (!sameSign(u, v)) return false;
}
decimal w = pa.dot(pq.cross(pb));
if (mRaycastTestType == FRONT && w < decimal(0.0)) return false;
if (mRaycastTestType == BACK && w > decimal(0.0)) return false;
if (mRaycastTestType == FRONT_AND_BACK && !sameSign(u, w)) return false;
if (mRaycastTestType == FRONT) {
if (w < decimal(0.0)) return false;
}
else if (mRaycastTestType == BACK) {
if (w > decimal(0.0)) return false;
}
else if (mRaycastTestType == FRONT_AND_BACK) {
if (!sameSign(u, w)) return false;
}
// If the line PQ is in the triangle plane (case where u=v=w=0)
if (approxEqual(u, 0) && approxEqual(v, 0) && approxEqual(w, 0)) return false;
@ -104,10 +115,9 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
raycastInfo.hitFraction = (localHitPoint - point1).length() / pq.length();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * localHitNormal;
raycastInfo.worldNormal.normalize();
raycastInfo.worldPoint = localHitPoint;
raycastInfo.hitFraction = (localHitPoint - ray.point1).length() / pq.length();
raycastInfo.worldNormal = localHitNormal;
return true;
}