Fix issues in HeightFieldShape

src/collision/shapes/AABB.cpp

@@ -76,7 +76,7 @@ void AABB::mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2) {
 }
 
 // Return true if the current AABB contains the AABB given in parameter
-bool AABB::contains(const AABB& aabb) {
+bool AABB::contains(const AABB& aabb) const {
 
     bool isInside = true;
     isInside = isInside && mMinCoordinates.x <= aabb.mMinCoordinates.x;

src/collision/shapes/AABB.h

@@ -101,7 +101,10 @@ class AABB {
         void mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2);
 
         /// Return true if the current AABB contains the AABB given in parameter
-        bool contains(const AABB& aabb);
+        bool contains(const AABB& aabb) const;
+
+        /// Return true if a point is inside the AABB
+        bool contains(const Vector3& point) const;
 
         /// Return true if the AABB of a triangle intersects the AABB
         bool testCollisionTriangleAABB(const Vector3* trianglePoints) const;
@@ -188,6 +191,14 @@ inline bool AABB::testCollisionTriangleAABB(const Vector3* trianglePoints) const
     return true;
 }
 
+// Return true if a point is inside the AABB
+inline bool AABB::contains(const Vector3& point) const {
+
+    return (point.x >= mMinCoordinates.x - MACHINE_EPSILON && point.x <= mMaxCoordinates.x + MACHINE_EPSILON &&
+            point.y >= mMinCoordinates.y - MACHINE_EPSILON && point.y <= mMaxCoordinates.y + MACHINE_EPSILON &&
+            point.z >= mMinCoordinates.z - MACHINE_EPSILON && point.z <= mMaxCoordinates.z + MACHINE_EPSILON);
+}
+
 // Assignment operator
 inline AABB& AABB::operator=(const AABB& aabb) {
     if (this != &aabb) {

src/collision/shapes/ConcaveMeshShape.h

@@ -118,9 +118,6 @@ 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
@@ -163,12 +160,6 @@ 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;
@@ -239,19 +230,6 @@ 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
 

src/collision/shapes/ConcaveShape.cpp

@@ -33,7 +33,7 @@ using namespace reactphysics3d;
 // Constructor
 ConcaveShape::ConcaveShape(CollisionShapeType type)
              : CollisionShape(type), mIsSmoothMeshCollisionEnabled(false),
-               mTriangleMargin(0) {
+               mTriangleMargin(0), mRaycastTestType(FRONT) {
 
 }
 

src/collision/shapes/ConcaveShape.h

@@ -28,6 +28,7 @@
 
 // Libraries
 #include "CollisionShape.h"
+#include "TriangleShape.h"
 
 // ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -64,6 +65,9 @@ class ConcaveShape : public CollisionShape {
         // Margin use for collision detection for each triangle
         decimal mTriangleMargin;
 
+        /// Raycast test type for the triangle (front, back, front-back)
+        TriangleRaycastSide mRaycastTestType;
+
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -88,6 +92,12 @@ class ConcaveShape : public CollisionShape {
         /// Return the triangle margin
         decimal getTriangleMargin() 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);
+
         /// Return true if the collision shape is convex, false if it is concave
         virtual bool isConvex() const;
 
@@ -129,6 +139,19 @@ inline void ConcaveShape::setIsSmoothMeshCollisionEnabled(bool isEnabled) {
     mIsSmoothMeshCollisionEnabled = isEnabled;
 }
 
+// Return the raycast test type (front, back, front-back)
+inline TriangleRaycastSide ConcaveShape::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 ConcaveShape::setRaycastTestType(TriangleRaycastSide testType) {
+    mRaycastTestType = testType;
+}
+
 }
 
 #endif

src/collision/shapes/HeightFieldShape.cpp

@@ -192,7 +192,48 @@ void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoor
 /// the ray hits many triangles.
 bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
 
+    // TODO : Implement raycasting without using an AABB for the ray
+    //        but using a dynamic AABB tree or octree instead
+
     PROFILE("HeightFieldShape::raycast()");
 
-    // TODO : Implement this
+    TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this);
+
+    // Compute the AABB for the ray
+    const Vector3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
+    const AABB rayAABB(Vector3::min(ray.point1, rayEnd), Vector3::max(ray.point1, rayEnd));
+
+    testAllTriangles(triangleCallback, rayAABB);
+
+    return triangleCallback.getIsHit();
+}
+
+// Raycast test between a ray and a triangle of the heightfield
+void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) {
+
+    // Create a triangle collision shape
+    decimal margin = mHeightFieldShape.getTriangleMargin();
+    TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
+    triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType());
+
+    // Ray casting test against the collision shape
+    RaycastInfo raycastInfo;
+    bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape);
+
+    // If the ray hit the collision shape
+    if (isTriangleHit && raycastInfo.hitFraction <= mSmallestHitFraction) {
+
+        assert(raycastInfo.hitFraction >= decimal(0.0));
+
+        mRaycastInfo.body = raycastInfo.body;
+        mRaycastInfo.proxyShape = raycastInfo.proxyShape;
+        mRaycastInfo.hitFraction = raycastInfo.hitFraction;
+        mRaycastInfo.worldPoint = raycastInfo.worldPoint;
+        mRaycastInfo.worldNormal = raycastInfo.worldNormal;
+        mRaycastInfo.meshSubpart = -1;
+        mRaycastInfo.triangleIndex = -1;
+
+        mSmallestHitFraction = raycastInfo.hitFraction;
+        mIsHit = true;
+    }
 }

src/collision/shapes/HeightFieldShape.h

@@ -33,6 +33,40 @@
 
 namespace reactphysics3d {
 
+class HeightFieldShape;
+
+// Class TriangleOverlapCallback
+/**
+ * This class is used for testing AABB and triangle overlap for raycasting
+ */
+class TriangleOverlapCallback : public TriangleCallback {
+
+    protected:
+
+        const Ray& mRay;
+        ProxyShape* mProxyShape;
+        RaycastInfo& mRaycastInfo;
+        bool mIsHit;
+        decimal mSmallestHitFraction;
+        const HeightFieldShape& mHeightFieldShape;
+
+    public:
+
+        // Constructor
+        TriangleOverlapCallback(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo,
+                                const HeightFieldShape& heightFieldShape)
+                               : mRay(ray), mProxyShape(proxyShape), mRaycastInfo(raycastInfo),
+                                 mHeightFieldShape (heightFieldShape) {
+            mIsHit = false;
+            mSmallestHitFraction = mRay.maxFraction;
+        }
+
+        bool getIsHit() const {return mIsHit;}
+
+        /// Raycast test between a ray and a triangle of the heightfield
+        virtual void testTriangle(const Vector3* trianglePoints);
+};
+
 // TODO : Implement raycasting for this shape
 
 // TODO : Implement smooth collision mesh for this shape
@@ -167,13 +201,19 @@ inline Vector3 HeightFieldShape::getVertexAt(int x, int y) const {
     // Get the difference between the center of AABB and zero level of the height field
     const decimal originToZeroHeight = (mMaxHeight - mMinHeight) * decimal(0.5);
 
+    Vector3 vertex;
     switch (mUpAxis) {
-        case 0: return Vector3(height - originToZeroHeight, -mWidth * decimal(0.5) + x, -mLength * decimal(0.5) + y) * mScaling;
-        case 1: return Vector3(-mWidth * decimal(0.5) + x, height - originToZeroHeight, -mLength * decimal(0.5) + y) * mScaling;
-        case 2: return Vector3(-mWidth * decimal(0.5) + x, -mLength * decimal(0.5) + y, height - originToZeroHeight) * mScaling;
+        case 0: vertex = Vector3(height - originToZeroHeight, -mWidth * decimal(0.5) + x, -mLength * decimal(0.5) + y) * mScaling;
+                break;
+        case 1: vertex = Vector3(-mWidth * decimal(0.5) + x, height - originToZeroHeight, -mLength * decimal(0.5) + y) * mScaling;
+                break;
+        case 2: vertex = Vector3(-mWidth * decimal(0.5) + x, -mLength * decimal(0.5) + y, height - originToZeroHeight) * mScaling;
+                break;
         default: assert(false);
     }
 
+    assert(mAABB.contains(vertex));
+    return vertex;
 }
 
 // Return the height of a given (x,y) point in the height field

src/collision/shapes/TriangleShape.h

@@ -119,6 +119,7 @@ class TriangleShape : public ConvexShape {
         // ---------- Friendship ---------- //
 
         friend class ConcaveMeshRaycastCallback;
+        friend class TriangleOverlapCallback;
 };
 
 // Return the number of bytes used by the collision shape

test/tests/collision/TestAABB.h

@@ -229,13 +229,23 @@ class TestAABB : public Test {
 
         void testIntersection() {
 
-            // -------- Test contains() -------- //
+            // -------- Test contains(AABB) -------- //
             test(!mAABB1.contains(mAABB2));
             test(mAABB3.contains(mAABB1));
             test(!mAABB1.contains(mAABB3));
             test(!mAABB1.contains(mAABB4));
             test(!mAABB4.contains(mAABB1));
 
+            // -------- Test contains(Vector3) -------- //
+            test(mAABB1.contains(Vector3(0, 0, 0)));
+            test(mAABB1.contains(Vector3(-5, 6, 9)));
+            test(mAABB1.contains(Vector3(-9, -4, -9)));
+            test(mAABB1.contains(Vector3(9, 4, 7)));
+            test(!mAABB1.contains(Vector3(-11, -4, -9)));
+            test(!mAABB1.contains(Vector3(1, 12, -9)));
+            test(!mAABB1.contains(Vector3(1, 8, -13)));
+            test(!mAABB1.contains(Vector3(-14, 82, -13)));
+
             // -------- Test testCollision() -------- //
             test(mAABB1.testCollision(mAABB2));
             test(mAABB2.testCollision(mAABB1));