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

Add double sided raycast test for TriangleShape, add unit tests

Browse Source
This commit is contained in:
Daniel Chappuis 2015-12-05 17:51:07 +01:00
parent 6ebad66acf
commit 9c7f70b9b7
6 changed files with 269 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -25,13 +25,13 @@
// Libraries
#include "ConcaveMeshShape.h"
#include "collision/shapes/TriangleShape.h"
using namespace reactphysics3d;
// Constructor
ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh) : ConcaveShape(CONCAVE_MESH) {
mTriangleMesh = triangleMesh;
mRaycastTestType = FRONT;
// Insert all the triangles into the dynamic AABB tree
initBVHTree();
@ -199,6 +199,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
// Create a triangle collision shape
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2]);
triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType());
// Ray casting test against the collision shape
RaycastInfo raycastInfo;

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

@ -30,6 +30,7 @@
#include "ConcaveShape.h"
#include "collision/broadphase/DynamicAABBTree.h"
#include "collision/TriangleMesh.h"
#include "collision/shapes/TriangleShape.h"
namespace reactphysics3d {
@ -120,6 +121,9 @@ class ConcaveMeshShape : public ConcaveShape {
/// Dynamic AABB tree to accelerate collision with the triangles
DynamicAABBTree mDynamicAABBTree;
/// Raycast test type for the triangle (front, back, front-back)
TriangleRaycastSide mRaycastTestType;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -170,6 +174,12 @@ class ConcaveMeshShape : public ConcaveShape {
/// Use a callback method on all triangles of the concave shape inside a given AABB
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const;
/// Return the raycast test type (front, back, front-back)
TriangleRaycastSide getRaycastTestType() const;
// Set the raycast test type (front, back, front-back)
void setRaycastTestType(TriangleRaycastSide testType);
// ---------- Friendship ----------- //
friend class ConvexTriangleAABBOverlapCallback;
@ -257,6 +267,19 @@ inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId)
mTriangleTestCallback.testTriangle(trianglePoints);
}
// Return the raycast test type (front, back, front-back)
inline TriangleRaycastSide ConcaveMeshShape::getRaycastTestType() const {
return mRaycastTestType;
}
// Set the raycast test type (front, back, front-back)
/**
* @param testType Raycast test type for the triangle (front, back, front-back)
*/
inline void ConcaveMeshShape::setRaycastTestType(TriangleRaycastSide testType) {
mRaycastTestType = testType;
}
}
#endif

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

@ -43,6 +43,7 @@ TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const
mPoints[0] = point1;
mPoints[1] = point2;
mPoints[2] = point3;
mRaycastTestType = FRONT;
}
// Destructor
@ -72,16 +73,21 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
// product for this test.
const Vector3 m = pq.cross(pc);
decimal u = pb.dot(m);
if (u < decimal(0.0)) return false;
if (mRaycastTestType == FRONT && u < decimal(0.0)) return false;
if (mRaycastTestType == BACK && u > decimal(0.0)) return false;
decimal v = -pa.dot(m);
if (v < decimal(0.0)) return false;
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;
decimal w = pa.dot(pq.cross(pb));
if (w < decimal(0.0)) return false;
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 the line PQ is in the triangle plane (case where u=v=w=0)
if (u < MACHINE_EPSILON && u < MACHINE_EPSILON && v < MACHINE_EPSILON) return false;
if (approxEqual(u, 0) && approxEqual(v, 0) && approxEqual(w, 0)) return false;
// Compute the barycentric coordinates (u, v, w) to determine the
// intersection point R, R = u * a + v * b + w * c
@ -93,7 +99,8 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
// Compute the local hit point using the barycentric coordinates
const Vector3 localHitPoint = u * mPoints[0] + v * mPoints[1] + w * mPoints[2];
const Vector3 localHitNormal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]);
Vector3 localHitNormal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]);
if (localHitNormal.dot(pq) > decimal(0.0)) localHitNormal = -localHitNormal;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;

35
src/collision/shapes/TriangleShape.h
View File

@ -33,6 +33,19 @@
/// ReactPhysics3D namespace
namespace reactphysics3d {
/// Raycast test side for the triangle
enum TriangleRaycastSide {
/// Raycast against front triangle
FRONT,
/// Raycast against back triangle
BACK,
/// Raycast against front and back triangle
FRONT_AND_BACK
};
// Class TriangleShape
/**
* This class represents a triangle collision shape that is centered
@ -47,6 +60,9 @@ class TriangleShape : public ConvexShape {
/// Three points of the triangle
Vector3 mPoints[3];
/// Raycast test type for the triangle (front, back, front-back)
TriangleRaycastSide mRaycastTestType;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -94,6 +110,12 @@ class TriangleShape : public ConvexShape {
/// Update the AABB of a body using its collision shape
virtual void computeAABB(AABB& aabb, const Transform& transform) const;
/// Return the raycast test type (front, back, front-back)
TriangleRaycastSide getRaycastTestType() const;
// Set the raycast test type (front, back, front-back)
void setRaycastTestType(TriangleRaycastSide testType);
// ---------- Friendship ---------- //
friend class ConcaveMeshRaycastCallback;
@ -177,6 +199,19 @@ inline bool TriangleShape::testPointInside(const Vector3& localPoint, ProxyShape
return false;
}
// Return the raycast test type (front, back, front-back)
inline TriangleRaycastSide TriangleShape::getRaycastTestType() const {
return mRaycastTestType;
}
// Set the raycast test type (front, back, front-back)
/**
* @param testType Raycast test type for the triangle (front, back, front-back)
*/
inline void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) {
mRaycastTestType = testType;
}
}
#endif

8
src/mathematics/mathematics_functions.h
View File

@ -41,8 +41,7 @@ struct Vector3;
/// Function to test if two real numbers are (almost) equal
/// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON]
inline bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON) {
decimal difference = a - b;
return (difference < epsilon && difference > -epsilon);
return (fabs(a - b) < epsilon);
}
/// Function that returns the result of the "value" clamped by
@ -62,6 +61,11 @@ inline decimal max3(decimal a, decimal b, decimal c) {
return std::max(std::max(a, b), c);
}
/// Return true if two values have the same sign
inline bool sameSign(decimal a, decimal b) {
return a * b >= decimal(0.0);
}
}
#endif

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

@ -962,20 +962,71 @@ class TestRaycast : public Test {
Vector3 point1 = mLocalShapeToWorld * Vector3(101, 101, 400);
Vector3 point2 = mLocalShapeToWorld * Vector3(101, 101, -200);
Ray ray(point1, point2);
Vector3 hitPoint = mLocalShapeToWorld * Vector3(101, 101, 0);
Ray rayBackward(point2, point1);
Vector3 hitPoint = mLocalShapeToWorld * Vector3(101, 101, 0);
Vector3 hitNormal = mLocalShapeToWorld.getOrientation() * Vector3(0, 0, 1);
hitNormal.normalize();
mCallback.shapeToTest = mTriangleProxyShape;
// CollisionWorld::raycast()
mCallback.reset();
mTriangleShape->setRaycastTestType(FRONT);
mWorld->raycast(ray, &mCallback);
test(mCallback.isHit);
test(mCallback.raycastInfo.body == mTriangleBody);
test(mCallback.raycastInfo.proxyShape == mTriangleProxyShape);
test(approxEqual(mCallback.raycastInfo.hitFraction, 0.2, epsilon));
test(approxEqual(mCallback.raycastInfo.hitFraction, 0.6666, epsilon));
test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon));
test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon));
test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon));
test(approxEqual(mCallback.raycastInfo.worldNormal.x, hitNormal.x, epsilon));
test(approxEqual(mCallback.raycastInfo.worldNormal.y, hitNormal.y, epsilon));
test(approxEqual(mCallback.raycastInfo.worldNormal.z, hitNormal.z, epsilon));
mCallback.reset();
mTriangleShape->setRaycastTestType(BACK);
mWorld->raycast(rayBackward, &mCallback);
test(mCallback.isHit);
test(mCallback.raycastInfo.body == mTriangleBody);
test(mCallback.raycastInfo.proxyShape == mTriangleProxyShape);
test(approxEqual(mCallback.raycastInfo.hitFraction, 0.3333, epsilon));
test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon));
test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon));
test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon));
test(approxEqual(mCallback.raycastInfo.worldNormal.x, -hitNormal.x, epsilon));
test(approxEqual(mCallback.raycastInfo.worldNormal.y, -hitNormal.y, epsilon));
test(approxEqual(mCallback.raycastInfo.worldNormal.z, -hitNormal.z, epsilon));
mCallback.reset();
mTriangleShape->setRaycastTestType(FRONT_AND_BACK);
mWorld->raycast(ray, &mCallback);
test(mCallback.isHit);
test(mCallback.raycastInfo.body == mTriangleBody);
test(mCallback.raycastInfo.proxyShape == mTriangleProxyShape);
test(approxEqual(mCallback.raycastInfo.hitFraction, 0.6666, epsilon));
test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon));
test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon));
test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon));
test(approxEqual(mCallback.raycastInfo.worldNormal.x, hitNormal.x, epsilon));
test(approxEqual(mCallback.raycastInfo.worldNormal.y, hitNormal.y, epsilon));
test(approxEqual(mCallback.raycastInfo.worldNormal.z, hitNormal.z, epsilon));
mCallback.reset();
mTriangleShape->setRaycastTestType(FRONT_AND_BACK);
mWorld->raycast(rayBackward, &mCallback);
test(mCallback.isHit);
test(mCallback.raycastInfo.body == mTriangleBody);
test(mCallback.raycastInfo.proxyShape == mTriangleProxyShape);
test(approxEqual(mCallback.raycastInfo.hitFraction, 0.3333, epsilon));
test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon));
test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon));
test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon));
test(approxEqual(mCallback.raycastInfo.worldNormal.x, -hitNormal.x, epsilon));
test(approxEqual(mCallback.raycastInfo.worldNormal.y, -hitNormal.y, epsilon));
test(approxEqual(mCallback.raycastInfo.worldNormal.z, -hitNormal.z, epsilon));
mTriangleShape->setRaycastTestType(FRONT);
// Correct category filter mask
mCallback.reset();
@ -992,7 +1043,7 @@ class TestRaycast : public Test {
test(mTriangleBody->raycast(ray, raycastInfo2));
test(raycastInfo2.body == mTriangleBody);
test(raycastInfo2.proxyShape == mTriangleProxyShape);
test(approxEqual(raycastInfo2.hitFraction, 0.2, epsilon));
test(approxEqual(raycastInfo2.hitFraction, 0.6666, 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));
@ -1002,7 +1053,7 @@ class TestRaycast : public Test {
test(mTriangleProxyShape->raycast(ray, raycastInfo3));
test(raycastInfo3.body == mTriangleBody);
test(raycastInfo3.proxyShape == mTriangleProxyShape);
test(approxEqual(raycastInfo3.hitFraction, 0.2, epsilon));
test(approxEqual(raycastInfo3.hitFraction, 0.6666, 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));
@ -1015,6 +1066,10 @@ class TestRaycast : public Test {
Ray ray5(mLocalShapeToWorld * Vector3(100.5, 101.5, 4), mLocalShapeToWorld * Vector3(100.5, 101.5, -54));
Ray ray6(mLocalShapeToWorld * Vector3(102, 101, 1), mLocalShapeToWorld * Vector3(102, 102, -1));
Ray ray4Back(mLocalShapeToWorld * Vector3(100.2, 101, -5), mLocalShapeToWorld * Vector3(100.2, 101, 5));
Ray ray5Back(mLocalShapeToWorld * Vector3(100.5, 101.5, -54), mLocalShapeToWorld * Vector3(100.5, 101.5, 4));
Ray ray6Back(mLocalShapeToWorld * Vector3(102, 102, -1), mLocalShapeToWorld * Vector3(102, 101, 1));
// ----- Test raycast miss ----- //
test(!mTriangleBody->raycast(ray1, raycastInfo3));
test(!mTriangleProxyShape->raycast(ray1, raycastInfo3));
@ -1040,7 +1095,53 @@ class TestRaycast : public Test {
mWorld->raycast(ray3, &mCallback);
test(!mCallback.isHit);
// Test backward ray against front triangles (not hit should occur)
mTriangleShape->setRaycastTestType(FRONT);
test(!mTriangleBody->raycast(ray4Back, raycastInfo3));
test(!mTriangleProxyShape->raycast(ray4Back, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray4Back, &mCallback);
test(!mCallback.isHit);
test(!mTriangleBody->raycast(ray5Back, raycastInfo3));
test(!mTriangleProxyShape->raycast(ray5Back, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray5Back, &mCallback);
test(!mCallback.isHit);
test(!mTriangleBody->raycast(ray6Back, raycastInfo3));
test(!mTriangleProxyShape->raycast(ray6Back, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray6Back, &mCallback);
test(!mCallback.isHit);
// Test front ray against back triangles (not hit should occur)
mTriangleShape->setRaycastTestType(BACK);
test(!mTriangleBody->raycast(ray4, raycastInfo3));
test(!mTriangleProxyShape->raycast(ray4, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray4, &mCallback);
test(!mCallback.isHit);
test(!mTriangleBody->raycast(ray5, raycastInfo3));
test(!mTriangleProxyShape->raycast(ray5, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray5, &mCallback);
test(!mCallback.isHit);
test(!mTriangleBody->raycast(ray6, raycastInfo3));
test(!mTriangleProxyShape->raycast(ray6, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray6, &mCallback);
test(!mCallback.isHit);
// ----- Test raycast hits ----- //
// Test front ray against front triangles
mTriangleShape->setRaycastTestType(FRONT);
test(mTriangleBody->raycast(ray4, raycastInfo3));
test(mTriangleProxyShape->raycast(ray4, raycastInfo3));
mCallback.reset();
@ -1065,6 +1166,90 @@ class TestRaycast : public Test {
mWorld->raycast(ray6, &mCallback);
mCallback.reset();
mWorld->raycast(Ray(ray6.point1, ray6.point2, decimal(0.8)), &mCallback);
// Test back ray against back triangles
mTriangleShape->setRaycastTestType(BACK);
test(mTriangleBody->raycast(ray4Back, raycastInfo3));
test(mTriangleProxyShape->raycast(ray4Back, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray4Back, &mCallback);
test(mCallback.isHit);
mCallback.reset();
mWorld->raycast(Ray(ray4Back.point1, ray4Back.point2, decimal(0.8)), &mCallback);
test(mCallback.isHit);
test(mTriangleBody->raycast(ray5Back, raycastInfo3));
test(mTriangleProxyShape->raycast(ray5Back, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray5Back, &mCallback);
test(mCallback.isHit);
mCallback.reset();
mWorld->raycast(Ray(ray5Back.point1, ray5Back.point2, decimal(1.0)), &mCallback);
test(mCallback.isHit);
test(mTriangleBody->raycast(ray6Back, raycastInfo3));
test(mTriangleProxyShape->raycast(ray6Back, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray6Back, &mCallback);
mCallback.reset();
mWorld->raycast(Ray(ray6Back.point1, ray6Back.point2, decimal(0.8)), &mCallback);
// Test front ray against front-back triangles
mTriangleShape->setRaycastTestType(FRONT_AND_BACK);
test(mTriangleBody->raycast(ray4, raycastInfo3));
test(mTriangleProxyShape->raycast(ray4, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray4, &mCallback);
test(mCallback.isHit);
mCallback.reset();
mWorld->raycast(Ray(ray4.point1, ray4.point2, decimal(0.8)), &mCallback);
test(mCallback.isHit);
test(mTriangleBody->raycast(ray5, raycastInfo3));
test(mTriangleProxyShape->raycast(ray5, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray5, &mCallback);
test(mCallback.isHit);
mCallback.reset();
mWorld->raycast(Ray(ray5.point1, ray5.point2, decimal(0.8)), &mCallback);
test(mCallback.isHit);
test(mTriangleBody->raycast(ray6, raycastInfo3));
test(mTriangleProxyShape->raycast(ray6, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray6, &mCallback);
mCallback.reset();
mWorld->raycast(Ray(ray6.point1, ray6.point2, decimal(0.8)), &mCallback);
// Test back ray against front-back triangles
mTriangleShape->setRaycastTestType(FRONT_AND_BACK);
test(mTriangleBody->raycast(ray4Back, raycastInfo3));
test(mTriangleProxyShape->raycast(ray4Back, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray4Back, &mCallback);
test(mCallback.isHit);
mCallback.reset();
mWorld->raycast(Ray(ray4Back.point1, ray4Back.point2, decimal(0.8)), &mCallback);
test(mCallback.isHit);
test(mTriangleBody->raycast(ray5Back, raycastInfo3));
test(mTriangleProxyShape->raycast(ray5Back, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray5Back, &mCallback);
test(mCallback.isHit);
mCallback.reset();
mWorld->raycast(Ray(ray5Back.point1, ray5Back.point2, decimal(1.0)), &mCallback);
test(mCallback.isHit);
test(mTriangleBody->raycast(ray6Back, raycastInfo3));
test(mTriangleProxyShape->raycast(ray6Back, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray6Back, &mCallback);
mCallback.reset();
mWorld->raycast(Ray(ray6Back.point1, ray6Back.point2, decimal(0.8)), &mCallback);
}
/// Test the ProxyConeShape::raycast(), CollisionBody::raycast() and