diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
index 72ba2e06..d6de739a 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
@@ -86,14 +86,15 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
 
                 const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
                 const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
-                const Vector3 capsuleInnerSegmentWorld = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA);
+                Vector3 capsuleInnerSegmentDirection = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA);
+                capsuleInnerSegmentDirection.normalize();
 
                 bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint->normal) > decimal(0.0);
                 bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal);
 
                 // If the polyhedron face normal is orthogonal to the capsule inner segment and parallel to the contact point normal and the face normal
                 // is in direction of the contact normal (from the polyhedron point of view).
-                if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentWorld)
+                if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentDirection)
                     && areParallelVectors(faceNormalWorld, contactPoint->normal)) {
 
                     // Remove the previous contact point computed by GJK
diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp
index fd813cdc..37335abe 100755
--- a/src/mathematics/mathematics_functions.cpp
+++ b/src/mathematics/mathematics_functions.cpp
@@ -80,7 +80,7 @@ bool reactphysics3d::areParallelVectors(const Vector3& vector1, const Vector3& v
 
 // Return true if two vectors are orthogonal
 bool reactphysics3d::areOrthogonalVectors(const Vector3& vector1, const Vector3& vector2) {
-	return std::abs(vector1.dot(vector2)) < decimal(0.00001);
+    return std::abs(vector1.dot(vector2)) < decimal(0.001);
 }
 
 // Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC"