diff --git a/include/reactphysics3d/collision/ContactManifold.h b/include/reactphysics3d/collision/ContactManifold.h
index e3142612..a6dd577e 100644
--- a/include/reactphysics3d/collision/ContactManifold.h
+++ b/include/reactphysics3d/collision/ContactManifold.h
@@ -34,7 +34,7 @@ namespace reactphysics3d {
 
 // Class declarations
 class ContactManifold;
-class ContactManifoldInfo;
+struct ContactManifoldInfo;
 struct ContactPointInfo;
 class CollisionBody;
 class ContactPoint;
diff --git a/include/reactphysics3d/collision/narrowphase/GJK/GJKAlgorithm.h b/include/reactphysics3d/collision/narrowphase/GJK/GJKAlgorithm.h
index 3916d54e..c2fc4abe 100644
--- a/include/reactphysics3d/collision/narrowphase/GJK/GJKAlgorithm.h
+++ b/include/reactphysics3d/collision/narrowphase/GJK/GJKAlgorithm.h
@@ -34,7 +34,7 @@
 namespace reactphysics3d {
 
 // Declarations
-class ContactManifoldInfo;
+struct ContactManifoldInfo;
 struct NarrowPhaseInfoBatch;
 class ConvexShape;
 class Profiler;
diff --git a/include/reactphysics3d/collision/narrowphase/NarrowPhaseAlgorithm.h b/include/reactphysics3d/collision/narrowphase/NarrowPhaseAlgorithm.h
index 444b1744..3cae437b 100644
--- a/include/reactphysics3d/collision/narrowphase/NarrowPhaseAlgorithm.h
+++ b/include/reactphysics3d/collision/narrowphase/NarrowPhaseAlgorithm.h
@@ -33,7 +33,7 @@
 namespace reactphysics3d {
 
 class CollisionDetectionSystem;
-class ContactManifoldInfo;
+struct ContactManifoldInfo;
 class DefaultPoolAllocator;
 class OverlappingPair;
 struct NarrowPhaseInfoBatch;
diff --git a/include/reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h b/include/reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h
index 520db55b..f81da537 100644
--- a/include/reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h
+++ b/include/reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h
@@ -37,7 +37,7 @@ namespace reactphysics3d {
 // Declarations
 class CollisionShape;
 struct LastFrameCollisionInfo;
-class ContactManifoldInfo;
+struct ContactManifoldInfo;
 struct ContactPointInfo;
 
 // Struct NarrowPhaseInfoBatch
diff --git a/include/reactphysics3d/collision/narrowphase/SAT/SATAlgorithm.h b/include/reactphysics3d/collision/narrowphase/SAT/SATAlgorithm.h
index 322898ea..20e86df4 100644
--- a/include/reactphysics3d/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/include/reactphysics3d/collision/narrowphase/SAT/SATAlgorithm.h
@@ -36,7 +36,7 @@ namespace reactphysics3d {
 // Declarations
 class CapsuleShape;
 class SphereShape;
-class ContactManifoldInfo;
+struct ContactManifoldInfo;
 struct NarrowPhaseInfoBatch;
 class ConvexPolyhedronShape;
 class MemoryAllocator;
diff --git a/include/reactphysics3d/collision/shapes/AABB.h b/include/reactphysics3d/collision/shapes/AABB.h
index 7ff486be..6a095278 100644
--- a/include/reactphysics3d/collision/shapes/AABB.h
+++ b/include/reactphysics3d/collision/shapes/AABB.h
@@ -302,7 +302,7 @@ RP3D_FORCE_INLINE bool AABB::raycast(const Ray& ray, Vector3& hitPoint) const {
     decimal tMin = decimal(0.0);
     decimal tMax = DECIMAL_LARGEST;
 
-    const decimal epsilon = 0.00001;
+    const decimal epsilon = decimal(0.00001);
 
     const Vector3 rayDirection = ray.point2 - ray.point1;
 
diff --git a/include/reactphysics3d/collision/shapes/HeightFieldShape.h b/include/reactphysics3d/collision/shapes/HeightFieldShape.h
index ebe6c926..061072c7 100644
--- a/include/reactphysics3d/collision/shapes/HeightFieldShape.h
+++ b/include/reactphysics3d/collision/shapes/HeightFieldShape.h
@@ -117,9 +117,6 @@ class HeightFieldShape : public ConcaveShape {
         void getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex,
                                                  Vector3* outTriangleVertices) const;
 
-        /// Return the closest inside integer grid value of a given floating grid value
-        int computeIntegerGridValue(decimal value) const;
-
         /// Compute the min/max grid coords corresponding to the intersection of the AABB of the height field and the AABB to collide
         void computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, const AABB& aabbToCollide) const;
 
@@ -200,18 +197,13 @@ RP3D_FORCE_INLINE decimal HeightFieldShape::getHeightAt(int x, int y) const {
     assert(y >= 0 && y < mNbRows);
 
     switch(mHeightDataType) {
-        case HeightDataType::HEIGHT_FLOAT_TYPE : return ((float*)mHeightFieldData)[y * mNbColumns + x];
-        case HeightDataType::HEIGHT_DOUBLE_TYPE : return ((double*)mHeightFieldData)[y * mNbColumns + x];
-        case HeightDataType::HEIGHT_INT_TYPE : return ((int*)mHeightFieldData)[y * mNbColumns + x] * mIntegerHeightScale;
+        case HeightDataType::HEIGHT_FLOAT_TYPE : return decimal(((float*)mHeightFieldData)[y * mNbColumns + x]);
+        case HeightDataType::HEIGHT_DOUBLE_TYPE : return decimal(((double*)mHeightFieldData)[y * mNbColumns + x]);
+        case HeightDataType::HEIGHT_INT_TYPE : return decimal(((int*)mHeightFieldData)[y * mNbColumns + x] * mIntegerHeightScale);
         default: assert(false); return 0;
     }
 }
 
-// Return the closest inside integer grid value of a given floating grid value
-RP3D_FORCE_INLINE int HeightFieldShape::computeIntegerGridValue(decimal value) const {
-    return (value < decimal(0.0)) ? value - decimal(0.5) : value + decimal(0.5);
-}
-
 // Compute the shape Id for a given triangle
 RP3D_FORCE_INLINE uint32 HeightFieldShape::computeTriangleShapeId(uint32 iIndex, uint32 jIndex, uint32 secondTriangleIncrement) const {
 
diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp
index 24474b9c..06984abf 100644
--- a/src/collision/shapes/HeightFieldShape.cpp
+++ b/src/collision/shapes/HeightFieldShape.cpp
@@ -211,16 +211,23 @@ void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoor
     minPoint += translateVec;
     maxPoint += translateVec;
 
+    assert(minPoint.x >= 0);
+    assert(minPoint.y >= 0);
+    assert(minPoint.z >= 0);
+    assert(maxPoint.x >= 0);
+    assert(maxPoint.y >= 0);
+    assert(maxPoint.z >= 0);
+
     // Convert the floating min/max coords of the AABB into closest integer
     // grid values (note that we use the closest grid coordinate that is out
     // of the AABB)
-    minCoords[0] = computeIntegerGridValue(minPoint.x) - 1;
-    minCoords[1] = computeIntegerGridValue(minPoint.y) - 1;
-    minCoords[2] = computeIntegerGridValue(minPoint.z) - 1;
+    minCoords[0] = static_cast<int>(minPoint.x + 0.5) - 1;
+    minCoords[1] = static_cast<int>(minPoint.y + 0.5) - 1;
+    minCoords[2] = static_cast<int>(minPoint.z + 0.5) - 1;
 
-    maxCoords[0] = computeIntegerGridValue(maxPoint.x) + 1;
-    maxCoords[1] = computeIntegerGridValue(maxPoint.y) + 1;
-    maxCoords[2] = computeIntegerGridValue(maxPoint.z) + 1;
+    maxCoords[0] = static_cast<int>(maxPoint.x + 0.5) + 1;
+    maxCoords[1] = static_cast<int>(maxPoint.y + 0.5) + 1;
+    maxCoords[2] = static_cast<int>(maxPoint.z + 0.5) + 1;
 }
 
 // Raycast method with feedback information
diff --git a/src/systems/CollisionDetectionSystem.cpp b/src/systems/CollisionDetectionSystem.cpp
index 13b9d1c3..07494385 100644
--- a/src/systems/CollisionDetectionSystem.cpp
+++ b/src/systems/CollisionDetectionSystem.cpp
@@ -450,7 +450,7 @@ void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(OverlappingPair
     assert(!concaveShape->isConvex());
     assert(overlappingPair.narrowPhaseAlgorithmType != NarrowPhaseAlgorithmType::None);
 
-    // Compute the convex shape AABB in the local-space of the convex shape
+    // Compute the convex shape AABB in the local-space of the concave shape
     AABB aabb;
     convexShape->computeAABB(aabb, convexToConcaveTransform);