Implement world ray casting query

CMakeLists.txt

@@ -82,6 +82,7 @@ SET (REACTPHYSICS3D_SOURCES
     "src/collision/BroadPhasePair.h"
     "src/collision/BroadPhasePair.cpp"
     "src/collision/RaycastInfo.h"
+    "src/collision/RaycastInfo.cpp"
     "src/collision/ProxyShape.h"
     "src/collision/ProxyShape.cpp"
     "src/collision/CollisionDetection.h"

src/body/CollisionBody.cpp

@@ -222,6 +222,8 @@ bool CollisionBody::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
     // For each collision shape of the body
     for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
 
+        // TODO : Test for broad-phase hit for each shape before testing actual shape raycast
+
         // Test if the ray hits the collision shape
         if (shape->raycast(rayTemp, raycastInfo)) {
             rayTemp.maxFraction = raycastInfo.hitFraction;

src/collision/BroadPhasePair.h

@@ -80,8 +80,6 @@ struct BroadPhasePair {
         bool operator!=(const BroadPhasePair& broadPhasePair2) const;
 };
 
-
-
 // Return the pair of bodies index
 inline bodyindexpair BroadPhasePair::getBodiesIndexPair() const {
 

src/collision/CollisionDetection.h

@@ -142,6 +142,9 @@ class CollisionDetection {
         /// Compute the collision detection
         void computeCollisionDetection();
 
+        /// Ray casting method
+        void raycast(RaycastCallback* raycastCallback, const Ray& ray) const;
+
         /// Allow the broadphase to notify the collision detection about an overlapping pair.
         void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
 
@@ -200,6 +203,17 @@ inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, con
     mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement);
 }
 
+// Ray casting method
+inline void CollisionDetection::raycast(RaycastCallback* raycastCallback,
+                                        const Ray& ray) const {
+
+    RaycastTest rayCastTest(raycastCallback);
+
+    // Ask the broad-phase algorithm to call the testRaycastAgainstShape()
+    // callback method for each proxy shape hit by the ray in the broad-phase
+    mBroadPhaseAlgorithm.raycast(ray, rayCastTest);
+}
+
 }
 
 #endif

src/collision/RaycastInfo.cpp

@@ -0,0 +1,49 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2014 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "decimal.h"
+#include "RaycastInfo.h"
+#include "ProxyShape.h"
+
+using namespace reactphysics3d;
+
+// Ray cast test against a proxy shape
+decimal RaycastTest::raycastAgainstShape(ProxyShape* shape, const Ray& ray) {
+
+    // Ray casting test against the collision shape
+    RaycastInfo raycastInfo;
+    bool isHit = shape->raycast(ray, raycastInfo);
+
+    // If the ray hit the collision shape
+    if (isHit) {
+
+        // Report the hit to the user and return the
+        // user hit fraction value
+        return userCallback->notifyRaycastHit(raycastInfo);
+    }
+
+    return ray.maxFraction;
+}

src/collision/RaycastInfo.h

@@ -28,6 +28,7 @@
 
 // Libraries
 #include "mathematics/Vector3.h"
+#include "mathematics/Ray.h"
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -86,6 +87,56 @@ struct RaycastInfo {
         }
 };
 
+// Class RaycastCallback
+/**
+ * This class can be used to register a callback for ray casting queries.
+ * You should implement your own class inherited from this one and implement
+ * the notifyRaycastHit() method. This method will be called for each ProxyShape
+ * that is hit by the ray.
+ */
+class RaycastCallback {
+
+    public:
+
+        // -------------------- Methods -------------------- //
+
+        /// Destructor
+        virtual ~RaycastCallback() {
+
+        }
+
+        /// This method will be called for each ProxyShape that is hit by the
+        /// ray. You cannot make any assumptions about the order of the
+        /// calls. You should use the return value to control the continuation
+        /// of the ray. The return value is the next maxFraction value to use.
+        /// If you return a fraction of 0.0, it means that the raycast should
+        /// terminate. If you return a fraction of 1.0, it indicates that the
+        /// ray is not clipped and the ray cast should continue as if no hit
+        /// occurred. If you return the fraction in the parameter (hitFraction
+        /// value in the RaycastInfo object), the current ray will be clipped
+        /// to this fraction in the next queries. If you return -1.0, it will
+        /// ignore this ProxyShape and continue the ray cast.
+        virtual decimal notifyRaycastHit(const RaycastInfo& raycastInfo)=0;
+
+};
+
+/// Structure RaycastTest
+struct RaycastTest {
+
+    public:
+
+        /// User callback class
+        RaycastCallback* userCallback;
+
+        /// Constructor
+        RaycastTest(RaycastCallback* callback) {
+            userCallback = callback;
+        }
+
+        /// Ray cast test against a proxy shape
+        decimal raycastAgainstShape(ProxyShape* shape, const Ray& ray);
+};
+
 }
 
 #endif

src/collision/broadphase/BroadPhaseAlgorithm.h

@@ -157,6 +157,9 @@ class BroadPhaseAlgorithm {
 
         /// Return true if the two broad-phase collision shapes are overlapping
         bool testOverlappingShapes(ProxyShape* shape1, ProxyShape* shape2) const;
+
+        /// Ray casting method
+        void raycast(const Ray& ray, RaycastTest& raycastTest) const;
 };
 
 // Method used to compare two pairs for sorting algorithm
@@ -180,6 +183,12 @@ inline bool BroadPhaseAlgorithm::testOverlappingShapes(ProxyShape* shape1,
     return aabb1.testCollision(aabb2);
 }
 
+// Ray casting method
+inline void BroadPhaseAlgorithm::raycast(const Ray& ray,
+                                         RaycastTest& raycastTest) const {
+    mDynamicAABBTree.raycast(ray, raycastTest);
+}
+
 }
 
 #endif

src/collision/broadphase/DynamicAABBTree.cpp

@@ -605,6 +605,79 @@ void DynamicAABBTree::reportAllShapesOverlappingWith(int nodeID, const AABB& aab
     }
 }
 
+// Ray casting method
+void DynamicAABBTree::raycast(const Ray& ray, RaycastTest& raycastTest) const {
+
+    decimal maxFraction = ray.maxFraction;
+
+    // Create an AABB for the ray
+    Vector3 endPoint = ray.point1 +
+                             maxFraction * (ray.point2 - ray.point1);
+    AABB rayAABB(Vector3::min(ray.point1, endPoint),
+                 Vector3::max(ray.point1, endPoint));
+
+    Stack<int, 128> stack;
+    stack.push(mRootNodeID);
+
+    // Walk through the tree from the root looking for proxy shapes
+    // that overlap with the ray AABB
+    while (stack.getNbElements() > 0) {
+
+        // Get the next node in the stack
+        int nodeID = stack.pop();
+
+        // If it is a null node, skip it
+        if (nodeID == TreeNode::NULL_TREE_NODE) continue;
+
+        // Get the corresponding node
+        const TreeNode* node = mNodes + nodeID;
+
+        // Test if the node AABB overlaps with the ray AABB
+        if (!rayAABB.testCollision(node->aabb)) continue;
+
+        // If the node is a leaf of the tree
+        if (node->isLeaf()) {
+
+            Ray rayTemp(ray.point1, ray.point2, maxFraction);
+
+            // Ask the collision detection to perform a ray cast test against
+            // the proxy shape of this node because the ray is overlapping
+            // with the shape in the broad-phase
+            decimal hitFraction = raycastTest.raycastAgainstShape(node->proxyShape,
+                                                                  rayTemp);
+
+            // If the user returned a hitFraction of zero, it means that
+            // the raycasting should stop here
+            if (hitFraction == decimal(0.0)) {
+                return;
+            }
+
+            // If the user returned a positive fraction
+            if (hitFraction > decimal(0.0)) {
+
+                // We update the maxFraction value and the ray
+                // AABB using the new maximum fraction
+                if (hitFraction < maxFraction) {
+                    maxFraction = hitFraction;
+                }
+                endPoint = ray.point1 + maxFraction * (ray.point2 - ray.point1);
+                rayAABB.mMinCoordinates = Vector3::min(ray.point1, endPoint);
+                rayAABB.mMaxCoordinates = Vector3::max(ray.point1, endPoint);
+            }
+
+            // If the user returned a negative fraction, we continue
+            // the raycasting as if the proxy shape did not exist
+
+        }
+        else {  // If the node has children
+
+            // Push its children in the stack of nodes to explore
+            stack.push(node->leftChildID);
+            stack.push(node->rightChildID);
+        }
+    }
+}
+
 #ifndef NDEBUG
 
 // Check if the tree structure is valid (for debugging purpose)

src/collision/broadphase/DynamicAABBTree.h

@@ -36,6 +36,12 @@ namespace reactphysics3d {
 
 // Declarations
 class BroadPhaseAlgorithm;
+struct RaycastTest;
+
+// Raycast callback method pointer type
+typedef decimal (*RaycastTestCallback) (ProxyShape* shape,
+                                RaycastCallback* userCallback,
+                                const Ray& ray);
 
 // Structure TreeNode
 /**
@@ -160,6 +166,9 @@ class DynamicAABBTree {
 
         /// Report all shapes overlapping with the AABB given in parameter.
         void reportAllShapesOverlappingWith(int nodeID, const AABB& aabb);
+
+        /// Ray casting method
+        void raycast(const Ray& ray, RaycastTest& raycastTest) const;
 };
 
 // Return true if the node is a leaf of the tree

src/engine/CollisionWorld.cpp

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

src/engine/CollisionWorld.h

@@ -120,8 +120,8 @@ class CollisionWorld {
         /// Destroy a collision body
         void destroyCollisionBody(CollisionBody* collisionBody);
 
-        /// Raycast method with feedback information
+        /// Ray cast method
-        bool raycast(const Ray& ray, RaycastInfo& raycastInfo);
+        void raycast(const Ray& ray, RaycastCallback* raycastCallback) const;
 
         // -------------------- Friendship -------------------- //
 
@@ -141,6 +141,12 @@ inline std::set<CollisionBody*>::iterator CollisionWorld::getBodiesEndIterator()
     return mBodies.end();
 }
 
+// Ray cast method
+inline void CollisionWorld::raycast(const Ray& ray,
+                                    RaycastCallback* raycastCallback) const {
+    mCollisionDetection.raycast(raycastCallback, ray);
+}
+
 }
 
  #endif

src/mathematics/Vector2.h

@@ -132,6 +132,12 @@ struct Vector2 {
         /// Overloaded operator
         Vector2& operator=(const Vector2& vector);
 
+        /// Return a vector taking the minimum components of two vectors
+        static Vector2 min(const Vector2& vector1, const Vector2& vector2);
+
+        /// Return a vector taking the maximum components of two vectors
+        static Vector2 max(const Vector2& vector1, const Vector2& vector2);
+
         // -------------------- Friends -------------------- //
 
         friend Vector2 operator+(const Vector2& vector1, const Vector2& vector2);
@@ -291,6 +297,18 @@ inline Vector2& Vector2::operator=(const Vector2& vector) {
     return *this;
 }
 
+// Return a vector taking the minimum components of two vectors
+inline Vector2 Vector2::min(const Vector2& vector1, const Vector2& vector2) {
+    return Vector2(std::min(vector1.x, vector2.x),
+                   std::min(vector1.y, vector2.y));
+}
+
+// Return a vector taking the maximum components of two vectors
+inline Vector2 Vector2::max(const Vector2& vector1, const Vector2& vector2) {
+    return Vector2(std::max(vector1.x, vector2.x),
+                   std::max(vector1.y, vector2.y));
+}
+
 }
 
 #endif

src/mathematics/Vector3.h

@@ -138,6 +138,12 @@ struct Vector3 {
         /// Overloaded operator
         Vector3& operator=(const Vector3& vector);
 
+        /// Return a vector taking the minimum components of two vectors
+        static Vector3 min(const Vector3& vector1, const Vector3& vector2);
+
+        /// Return a vector taking the maximum components of two vectors
+        static Vector3 max(const Vector3& vector1, const Vector3& vector2);
+
         // -------------------- Friends -------------------- //
 
         friend Vector3 operator+(const Vector3& vector1, const Vector3& vector2);
@@ -312,6 +318,20 @@ inline Vector3& Vector3::operator=(const Vector3& vector) {
     return *this;
 }
 
+// Return a vector taking the minimum components of two vectors
+inline Vector3 Vector3::min(const Vector3& vector1, const Vector3& vector2) {
+    return Vector3(std::min(vector1.x, vector2.x),
+                   std::min(vector1.y, vector2.y),
+                   std::min(vector1.z, vector2.z));
+}
+
+// Return a vector taking the maximum components of two vectors
+inline Vector3 Vector3::max(const Vector3& vector1, const Vector3& vector2) {
+    return Vector3(std::max(vector1.x, vector2.x),
+                   std::max(vector1.y, vector2.y),
+                   std::max(vector1.z, vector2.z));
+}
+
 }
 
 #endif

test/tests/mathematics/TestVector2.h

@@ -156,6 +156,12 @@ class TestVector2 : public Test {
             test(Vector2(7, 537).getMaxAxis() == 1);
             test(Vector2(98, 23).getMaxAxis() == 0);
             test(Vector2(-53, -25).getMaxAxis() == 1);
+
+            // Test the methot that return a max/min vector
+            Vector2 vec1(-5, 4);
+            Vector2 vec2(-8, 6);
+            test(Vector2::min(vec1, vec2) == Vector2(-8, 4));
+            test(Vector2::max(vec1, vec2) == Vector2(-5, 6));
         }
 
         /// Test the operators

test/tests/mathematics/TestVector3.h

@@ -178,6 +178,12 @@ class TestVector3 : public Test {
             test(Vector3(7, 533, 36).getMaxAxis() == 1);
             test(Vector3(98, 23, 3).getMaxAxis() == 0);
             test(Vector3(-53, -25, -63).getMaxAxis() == 1);
+
+            // Test the methot that return a max/min vector
+            Vector3 vec1(-5, 4, 2);
+            Vector3 vec2(-8, 6, -1);
+            test(Vector3::min(vec1, vec2) == Vector3(-8, 4, -1));
+            test(Vector3::max(vec1, vec2) == Vector3(-5, 6, 2));
         }
 
         /// Test the operators