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

Continue the implementation of convex shape raycasting

Browse Source
This commit is contained in:
Daniel Chappuis 2014-09-02 22:54:19 +02:00
parent 3c1b819fda
commit 677c694109
24 changed files with 308 additions and 118 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/body/CollisionBody.cpp
View File

@ -203,7 +203,7 @@ void CollisionBody::askForBroadPhaseCollisionCheck() const {
bool CollisionBody::testPointInside(const Vector3& worldPoint) const {
// For each collision shape of the body
for(ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Test if the point is inside the collision shape
if (shape->testPointInside(worldPoint)) return true;
@ -214,12 +214,32 @@ bool CollisionBody::testPointInside(const Vector3& worldPoint) const {
// Raycast method
bool CollisionBody::raycast(const Ray& ray, decimal distance) {
// TODO : Implement this method
// 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, distance)) 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) {
// TODO : Implement this method
return false;
bool isHit = false;
// 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;
isHit = true;
}
}
return isHit;
}

1
src/body/CollisionBody.h
View File

@ -179,6 +179,7 @@ class CollisionBody : public Body {
friend class CollisionDetection;
friend class BroadPhaseAlgorithm;
friend class ConvexMeshShape;
friend class ProxyShape;
};
// Return the type of the body

12
src/collision/ProxyShape.h
View File

@ -93,6 +93,9 @@ class ProxyShape {
/// Return the local to parent body transform
const Transform& getLocalToBodyTransform() const;
/// Return the local to world transform
const Transform getLocalToWorldTransform() const;
/// Return true if a point is inside the collision shape
bool testPointInside(const Vector3& worldPoint);
@ -146,6 +149,11 @@ inline const Transform& ProxyShape::getLocalToBodyTransform() const {
return mLocalToBodyTransform;
}
// Return the local to world transform
inline const Transform ProxyShape::getLocalToWorldTransform() const {
return mBody->mTransform * mLocalToBodyTransform;
}
// Return a local support point in a given direction with the object margin
inline Vector3 ProxyShape::getLocalSupportPointWithMargin(const Vector3& direction) {
return mCollisionShape->getLocalSupportPointWithMargin(direction, &mCachedCollisionData);
@ -163,12 +171,12 @@ inline decimal ProxyShape::getMargin() const {
// Raycast method
inline bool ProxyShape::raycast(const Ray& ray, decimal distance) {
return mCollisionShape->raycast(ray, distance);
return mCollisionShape->raycast(ray, this, distance);
}
// Raycast method with feedback information
inline bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) {
return mCollisionShape->raycast(ray, raycastInfo, distance);
return mCollisionShape->raycast(ray, raycastInfo, this, distance);
}
}

3
src/collision/RaycastInfo.h
View File

@ -60,6 +60,9 @@ struct RaycastInfo {
/// Hit point in world-space coordinates
Vector3 worldPoint;
/// Surface normal at hit point in world-space coordinates
Vector3 worldNormal;
/// Distance from the ray origin to the hit point
decimal distance;

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

@ -392,3 +392,104 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* collis
// The point is inside the collision shape
return true;
}
// 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) {
Vector3 suppA; // Current lower bound point on the ray (starting at ray's origin)
Vector3 suppB; // Support point on the collision shape
const decimal machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON;
const decimal epsilon = decimal(0.0001);
// 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 w;
// Create a simplex set
Simplex simplex;
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)
suppB = collisionShape->getLocalSupportPointWithoutMargin(rayDirection);
Vector3 v = suppA - suppB;
decimal vDotW, vDotR;
decimal distSquare = v.lengthSquare();
int nbIterations = 0;
// GJK Algorithm loop
while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) {
// Compute the support points
suppB = collisionShape->getLocalSupportPointWithoutMargin(v);
w = suppA - suppB;
vDotW = v.dot(w);
if (vDotW > decimal(0)) {
vDotR = v.dot(rayDirection);
if (vDotR >= -machineEpsilonSquare) {
return false;
}
else {
// We have found a better lower bound for the hit point along the ray
lambda = lambda - vDotW / vDotR;
suppA = origin + lambda * rayDirection;
w = suppA - suppB;
n = v;
}
}
// Add the new support point to the simplex
if (!simplex.isPointInSimplex(w)) {
simplex.addPoint(w, suppA, suppB);
}
// Compute the closest point
if (simplex.computeClosestPoint(v)) {
distSquare = v.lengthSquare();
}
else {
distSquare = decimal(0.0);
}
// If the current lower bound distance is larger than the maximum raycasting distance
if (lambda > maxDistance) return false;
nbIterations++;
}
// If the origin was inside the shape, we return no hit
if (lambda < MACHINE_EPSILON) return false;
// Compute the closet points of both objects (without the margins)
Vector3 pointA;
Vector3 pointB;
simplex.computeClosestPointsOfAandB(pointA, pointB);
// A raycast hit has been found, we fill in the raycast info object
raycastInfo.distance = lambda;
raycastInfo.worldPoint = localToWorldTransform * pointB;
raycastInfo.body = collisionShape->getBody();
raycastInfo.proxyShape = collisionShape;
if (n.lengthSquare() >= machineEpsilonSquare) { // The normal vector is valid
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * n.getUnit();
}
else { // Degenerated normal vector, we return a zero normal vector
raycastInfo.worldNormal = Vector3(decimal(0), decimal(0), decimal(0));
}
return true;
}

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

@ -39,6 +39,7 @@ namespace reactphysics3d {
// Constants
const decimal REL_ERROR = decimal(1.0e-3);
const decimal REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
const int MAX_ITERATIONS_GJK_RAYCAST = 32;
// Class GJKAlgorithm
/**
@ -95,7 +96,11 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
ContactPointInfo*& contactInfo);
/// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool testPointInside(const Vector3& localPoint, ProxyShape *collisionShape);
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);
};
}

2
src/collision/narrowphase/GJK/Simplex.h
View File

@ -130,7 +130,7 @@ class Simplex {
/// Return true if the simplex contains 4 points
bool isFull() const;
/// Return true if the simple is empty
/// Return true if the simplex is empty
bool isEmpty() const;
/// Return the points of the simplex

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

@ -62,13 +62,20 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const
}
// Raycast method
bool BoxShape::raycast(const Ray& ray, decimal distance) const {
bool BoxShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const {
// TODO : Normalize the ray direction
// TODO : Implement this method
return false;
}
// Raycast method with feedback information
bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const {
bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance) const {
// TODO : Normalize the ray direction
// TODO : Implement this method
return false;
}

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

@ -78,6 +78,14 @@ 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,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
public :
// -------------------- Methods -------------------- //
@ -105,13 +113,6 @@ class BoxShape : public CollisionShape {
/// Test equality between two box shapes
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
/// Raycast method
virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
};
// Allocate and return a copy of the object

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

@ -126,18 +126,6 @@ void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) co
0.0, 0.0, IxxAndzz);
}
// Raycast method
bool CapsuleShape::raycast(const Ray& ray, decimal distance) const {
// TODO : Implement this method
return false;
}
// Raycast method with feedback information
bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const {
// TODO : Implement this method
return false;
}
// Return true if a point is inside the collision shape
bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
@ -153,3 +141,22 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
(xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius ||
(xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius;
}
// Raycast method
bool CapsuleShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const {
// TODO : Normalize the ray direction
// TODO : Implement this method
return false;
}
// Raycast method with feedback information
bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance) const {
// TODO : Normalize the ray direction
// TODO : Implement this method
return false;
}

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

@ -75,6 +75,14 @@ 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,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
public :
// -------------------- Methods -------------------- //
@ -105,13 +113,6 @@ class CapsuleShape : public CollisionShape {
/// Test equality between two capsule shapes
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
/// Raycast method
virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
};
/// Allocate and return a copy of the object

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

@ -86,10 +86,11 @@ class CollisionShape {
virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0;
/// Raycast method
virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const=0;
virtual bool raycast(const Ray& ray, ProxyShape* proxyShape,
decimal distance = RAYCAST_INFINITY_DISTANCE) const=0;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo,
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance = RAYCAST_INFINITY_DISTANCE) const=0;
public :

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

@ -95,13 +95,20 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
}
// Raycast method
bool ConeShape::raycast(const Ray& ray, decimal distance) const {
bool ConeShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const {
// TODO : Normalize the ray direction
// TODO : Implement this method
return false;
}
// Raycast method with feedback information
bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const {
bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance) const {
// TODO : Normalize the ray direction
// TODO : Implement this method
return false;
}

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

@ -82,6 +82,14 @@ 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,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
public :
@ -113,13 +121,6 @@ class ConeShape : public CollisionShape {
/// Test equality between two cone shapes
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
/// Raycast method
virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
};
// Allocate and return a copy of the object

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

@ -156,14 +156,14 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct
}
else { // If the edges information is not used
decimal maxDotProduct = DECIMAL_SMALLEST;
double maxDotProduct = DECIMAL_SMALLEST;
uint indexMaxDotProduct = 0;
// For each vertex of the mesh
for (uint i=0; i<mNbVertices; i++) {
// Compute the dot product of the current vertex
decimal dotProduct = direction.dot(mVertices[i]);
double dotProduct = direction.dot(mVertices[i]);
// If the current dot product is larger than the maximum one
if (dotProduct > maxDotProduct) {
@ -231,13 +231,15 @@ bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const
}
// Raycast method
bool ConvexMeshShape::raycast(const Ray& ray, decimal distance) const {
// TODO : Implement this method
return false;
bool ConvexMeshShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const {
RaycastInfo raycastInfo;
return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast(
ray, proxyShape, raycastInfo, distance);
}
// Raycast method with feedback information
bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const {
// TODO : Implement this method
return false;
bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance) const {
return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast(
ray, proxyShape, raycastInfo, distance);
}

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

@ -104,6 +104,14 @@ 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,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
public :
// -------------------- Methods -------------------- //
@ -145,13 +153,6 @@ class ConvexMeshShape : public CollisionShape {
/// Set the variable to know if the edges information is used to speed up the
/// collision detection
void setIsEdgesInformationUsed(bool isEdgesUsed);
/// Raycast method
virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
};
// Allocate and return a copy of the object

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

@ -88,13 +88,20 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio
}
// Raycast method
bool CylinderShape::raycast(const Ray& ray, decimal distance) const {
bool CylinderShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const {
// TODO : Normalize the ray direction
// TODO : Implement this method
return false;
}
// Raycast method with feedback information
bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const {
bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance) const {
// TODO : Normalize the ray direction
// TODO : Implement this method
return false;
}

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

@ -80,6 +80,14 @@ 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,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
public :
// -------------------- Methods -------------------- //
@ -110,13 +118,6 @@ class CylinderShape : public CollisionShape {
/// Test equality between two cylinder shapes
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
/// Raycast method
virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
};
/// Allocate and return a copy of the object

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

@ -47,13 +47,21 @@ SphereShape::~SphereShape() {
}
// Raycast method
bool SphereShape::raycast(const Ray& ray, decimal distance) const {
bool SphereShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const {
// TODO : Normalize the ray direction
// TODO : Implement this method
return false;
}
// Raycast method with feedback information
bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const {
bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance) const {
// TODO : Normalize the ray direction
// TODO : Implement this method
return false;
}

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

@ -70,6 +70,14 @@ 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,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
public :
// -------------------- Methods -------------------- //
@ -100,13 +108,6 @@ class SphereShape : public CollisionShape {
/// Test equality between two sphere shapes
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
/// Raycast method
virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo,
decimal distance = RAYCAST_INFINITY_DISTANCE) const;
};
/// Allocate and return a copy of the object

2
src/configuration.h
View File

@ -132,7 +132,7 @@ const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1);
const decimal DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = decimal(1.7);
/// Raycasting infinity distance constant
const decimal RAYCAST_INFINITY_DISTANCE = decimal(-1.0);
const decimal RAYCAST_INFINITY_DISTANCE = std::numeric_limits<decimal>::infinity();
}

2
test/Test.cpp
View File

@ -66,7 +66,7 @@ void Test::applyFail(const std::string& testText, const char* filename, long lin
if (mOutputStream) {
// Display the failure message
*mOutputStream << mName << "failure : (" << testText << "), " <<
*mOutputStream << mName << " failure : (" << testText << "), " <<
filename << "(line " << lineNumber << ")" << std::endl;
}

7
test/tests/collision/TestPointInside.h
View File

@ -89,7 +89,7 @@ class TestPointInside : public Test {
// Body transform
Vector3 position(-3, 2, 7);
Quaternion orientation(PI / 5, PI / 6, PI / 7);
//mBodyTransform = Transform(position, orientation); // TODO : Uncomment this
mBodyTransform = Transform(position, orientation);
// Create the bodies
mBoxBody = mWorld->createCollisionBody(mBodyTransform);
@ -104,7 +104,7 @@ class TestPointInside : public Test {
// Collision shape transform
Vector3 shapePosition(1, -4, -3);
Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3);
//mShapeTransform = Transform(shapePosition, shapeOrientation); // TODO : Uncomment this
mShapeTransform = Transform(shapePosition, shapeOrientation);
// Compute the the transform from a local shape point to world-space
mLocalShapeToWorld = mBodyTransform * mShapeTransform;
@ -156,7 +156,8 @@ class TestPointInside : public Test {
convexMeshShapeEdgesInfo.addEdge(3, 7);
convexMeshShapeEdgesInfo.setIsEdgesInformationUsed(true);
mConvexMeshShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape(
convexMeshShapeEdgesInfo);
convexMeshShapeEdgesInfo,
mShapeTransform);
CylinderShape cylinderShape(3, 8, 0);
mCylinderShape = mCylinderBody->addCollisionShape(cylinderShape, mShapeTransform);

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

@ -50,6 +50,9 @@ class TestRaycast : public Test {
// ---------- Atributes ---------- //
// Epsilon
decimal epsilon;
// Physics world
CollisionWorld* mWorld;
@ -85,6 +88,8 @@ class TestRaycast : public Test {
/// Constructor
TestRaycast(const std::string& name) : Test(name) {
epsilon = 0.0001;
// Create the world
mWorld = new CollisionWorld();
@ -125,40 +130,41 @@ class TestRaycast : public Test {
mConeShape = mConeBody->addCollisionShape(coneShape, mShapeTransform);
ConvexMeshShape convexMeshShape(0); // Box of dimension (2, 3, 4)
convexMeshShape.addVertex(Vector3(-2, -3, 4));
convexMeshShape.addVertex(Vector3(-2, -3, -4));
convexMeshShape.addVertex(Vector3(2, -3, -4));
convexMeshShape.addVertex(Vector3(2, -3, 4));
convexMeshShape.addVertex(Vector3(-2, -3, 4));
convexMeshShape.addVertex(Vector3(2, -3, -4));
convexMeshShape.addVertex(Vector3(-2, 3, 4));
convexMeshShape.addVertex(Vector3(2, 3, 4));
convexMeshShape.addVertex(Vector3(-2, 3, -4));
convexMeshShape.addVertex(Vector3(2, 3, -4));
convexMeshShape.addVertex(Vector3(2, 3, 4));
convexMeshShape.addVertex(Vector3(-2, 3, 4));
mConvexMeshShape = mConvexMeshBody->addCollisionShape(convexMeshShape, mShapeTransform);
ConvexMeshShape convexMeshShapeEdgesInfo(0);
convexMeshShapeEdgesInfo.addVertex(Vector3(-2, -3, 4));
convexMeshShapeEdgesInfo.addVertex(Vector3(-2, -3, -4));
convexMeshShapeEdgesInfo.addVertex(Vector3(2, -3, -4));
convexMeshShapeEdgesInfo.addVertex(Vector3(2, -3, 4));
convexMeshShapeEdgesInfo.addVertex(Vector3(-2, -3, 4));
convexMeshShapeEdgesInfo.addVertex(Vector3(2, -3, -4));
convexMeshShapeEdgesInfo.addVertex(Vector3(-2, 3, 4));
convexMeshShapeEdgesInfo.addVertex(Vector3(2, 3, 4));
convexMeshShapeEdgesInfo.addVertex(Vector3(-2, 3, -4));
convexMeshShapeEdgesInfo.addVertex(Vector3(2, 3, -4));
convexMeshShapeEdgesInfo.addVertex(Vector3(2, 3, 4));
convexMeshShapeEdgesInfo.addVertex(Vector3(-2, 3, 4));
convexMeshShapeEdgesInfo.addEdge(0, 1);
convexMeshShapeEdgesInfo.addEdge(1, 3);
convexMeshShapeEdgesInfo.addEdge(1, 2);
convexMeshShapeEdgesInfo.addEdge(2, 3);
convexMeshShapeEdgesInfo.addEdge(0, 2);
convexMeshShapeEdgesInfo.addEdge(0, 3);
convexMeshShapeEdgesInfo.addEdge(4, 5);
convexMeshShapeEdgesInfo.addEdge(5, 7);
convexMeshShapeEdgesInfo.addEdge(5, 6);
convexMeshShapeEdgesInfo.addEdge(6, 7);
convexMeshShapeEdgesInfo.addEdge(4, 6);
convexMeshShapeEdgesInfo.addEdge(4, 7);
convexMeshShapeEdgesInfo.addEdge(0, 4);
convexMeshShapeEdgesInfo.addEdge(1, 5);
convexMeshShapeEdgesInfo.addEdge(2, 6);
convexMeshShapeEdgesInfo.addEdge(3, 7);
convexMeshShapeEdgesInfo.setIsEdgesInformationUsed(true);
mConvexMeshShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape(
convexMeshShapeEdgesInfo);
convexMeshShapeEdgesInfo,
mShapeTransform);
CylinderShape cylinderShape(2, 5, 0);
mCylinderShape = mCylinderBody->addCollisionShape(cylinderShape, mShapeTransform);
@ -823,50 +829,50 @@ class TestRaycast : public Test {
test(mWorld->raycast(ray, raycastInfo));
test(raycastInfo.body == mConvexMeshBody);
test(raycastInfo.proxyShape == mConvexMeshShape);
test(approxEqual(raycastInfo.distance, 6));
test(approxEqual(raycastInfo.worldPoint.x, hitPoint.x));
test(approxEqual(raycastInfo.worldPoint.y, hitPoint.y));
test(approxEqual(raycastInfo.worldPoint.z, hitPoint.z));
test(approxEqual(raycastInfo.distance, 6, epsilon));
test(approxEqual(raycastInfo.worldPoint.x, hitPoint.x, epsilon));
test(approxEqual(raycastInfo.worldPoint.y, hitPoint.y, epsilon));
test(approxEqual(raycastInfo.worldPoint.z, hitPoint.z, epsilon));
// CollisionBody::raycast()
RaycastInfo raycastInfo2;
test(mConvexMeshBody->raycast(ray, raycastInfo2));
test(raycastInfo2.body == mConvexMeshBody);
test(raycastInfo2.proxyShape == mConvexMeshShape);
test(approxEqual(raycastInfo2.distance, 6));
test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x));
test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y));
test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z));
test(approxEqual(raycastInfo2.distance, 6, epsilon));
test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon));
test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon));
test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon));
// ProxyCollisionShape::raycast()
RaycastInfo raycastInfo3;
test(mConvexMeshBodyEdgesInfo->raycast(ray, raycastInfo3));
test(raycastInfo3.body == mConvexMeshBodyEdgesInfo);
test(raycastInfo3.proxyShape == mConvexMeshShapeEdgesInfo);
test(approxEqual(raycastInfo3.distance, 6));
test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x));
test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y));
test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z));
test(approxEqual(raycastInfo3.distance, 6, epsilon));
test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon));
test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon));
test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon));
// ProxyCollisionShape::raycast()
RaycastInfo raycastInfo4;
test(mConvexMeshShape->raycast(ray, raycastInfo4));
test(raycastInfo4.body == mConvexMeshBody);
test(raycastInfo4.proxyShape == mConvexMeshShape);
test(approxEqual(raycastInfo4.distance, 6));
test(approxEqual(raycastInfo4.worldPoint.x, hitPoint.x));
test(approxEqual(raycastInfo4.worldPoint.y, hitPoint.y));
test(approxEqual(raycastInfo4.worldPoint.z, hitPoint.z));
test(approxEqual(raycastInfo4.distance, 6, epsilon));
test(approxEqual(raycastInfo4.worldPoint.x, hitPoint.x, epsilon));
test(approxEqual(raycastInfo4.worldPoint.y, hitPoint.y, epsilon));
test(approxEqual(raycastInfo4.worldPoint.z, hitPoint.z, epsilon));
// ProxyCollisionShape::raycast()
RaycastInfo raycastInfo5;
test(mConvexMeshShapeEdgesInfo->raycast(ray, raycastInfo5));
test(raycastInfo5.body == mConvexMeshBodyEdgesInfo);
test(raycastInfo5.proxyShape == mConvexMeshShapeEdgesInfo);
test(approxEqual(raycastInfo5.distance, 6));
test(approxEqual(raycastInfo5.worldPoint.x, hitPoint.x));
test(approxEqual(raycastInfo5.worldPoint.y, hitPoint.y));
test(approxEqual(raycastInfo5.worldPoint.z, hitPoint.z));
test(approxEqual(raycastInfo5.distance, 6, epsilon));
test(approxEqual(raycastInfo5.worldPoint.x, hitPoint.x, epsilon));
test(approxEqual(raycastInfo5.worldPoint.y, hitPoint.y, epsilon));
test(approxEqual(raycastInfo5.worldPoint.z, hitPoint.z, epsilon));
Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalToWorldMatrix * Vector3(5, 7, -1));
Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalToWorldMatrix * Vector3(4, 6, 7));