Fixes falling though map when using charge on a hill.

Source/Framework/RecastDetour/Detour/DetourCommon.cs

@@ -254,7 +254,7 @@ public static partial class Detour
 
         if (u >= 0.0f && v >= 0.0f && (u + v) <= denom)
         {
-            h = a[1] + (v0[1] * u + v1[1] * v) / denom;
+            h = a[aStart + 1] + (v0[1] * u + v1[1] * v) / denom;
             return true;
         }
 

Source/Framework/RecastDetour/Detour/DetourNavMesh.cs

@@ -782,14 +782,14 @@ public static partial class Detour
             dtVlerp(closest, 0, new float[] { v[pmin].X, v[pmin].Y, v[pmin].Z }, 0, new float[] { v[pmax].X, v[pmax].Y, v[pmax].Z }, 0, tmin);
         }
 
-        public bool getPolyHeight(dtMeshTile tile, dtPoly poly, float[] pos, float height)
+        public bool getPolyHeight(dtMeshTile tile, dtPoly poly, float[] pos, ref float height)
         {
             // Off-mesh connections do not have detail polys and getting height
             // over them does not make sense.
             if (poly.getType() == (byte)dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
                 return false;
 
-            uint ip = (uint)tile.polys.ToList().IndexOf(poly);
+            uint ip = (uint)Array.IndexOf(tile.polys, poly);
             dtPolyDetail pd = tile.detailMeshes[ip];
 
             float[] verts = new float[DT_VERTS_PER_POLYGON * 3];
@@ -855,7 +855,7 @@ public static partial class Detour
             getTileAndPolyByRefUnsafe(polyRef, ref tile, ref poly);
 
             dtVcopy(closest, pos);
-            if (getPolyHeight(tile, poly, pos, closest[1]))
+            if (getPolyHeight(tile, poly, pos, ref closest[1]))
             {
                 if (posOverPoly)
                     posOverPoly = true;

Source/Framework/RecastDetour/Detour/DetourNavMeshQuery.cs

@@ -743,7 +743,7 @@ public static partial class Detour
                 return DT_SUCCESS;
             }
 
-            return m_nav.getPolyHeight(tile, poly, pos, height) ? DT_SUCCESS : DT_FAILURE | DT_INVALID_PARAM;
+            return m_nav.getPolyHeight(tile, poly, pos, ref height) ? DT_SUCCESS : DT_FAILURE | DT_INVALID_PARAM;
         }
 
         // @}

Source/Framework/Util/Extensions.cs

@@ -241,9 +241,16 @@ namespace System
             return a;
         }
 
+        public static Vector3 direction(this Vector3 vector)
+        {
+            float lenSquared = vector.LengthSquared();
+            float invSqrt = 1.0f / MathF.Sqrt(lenSquared);
+            return new Vector3(vector.X * invSqrt, vector.Y * invSqrt, vector.Z * invSqrt);
+        }
+        
         public static Vector3 directionOrZero(this Vector3 vector)
         {
-            float mag = vector.Length();
+            float mag = vector.LengthSquared();
             if (mag < 0.0000001f)
             {
                 return Vector3.Zero;