public void TryAddVoxelNavmeshLinks2(MyVoxelPathfinding.CellId cellId, Dictionary <MyGridPathfinding.CubeId, List <MyNavigationPrimitive> > linkCandidates)
{
ProfilerShort.Begin("TryAddVoxelNavmeshLinks");
foreach (var entry in linkCandidates)
{
double closestDistSq = double.MaxValue;
MyNavigationTriangle closestGridTri = null;
MyNavigationPrimitive closestLinkedPrim = null;
m_tmpNavTris.Clear();
m_gridPathfinding.GetCubeTriangles(entry.Key, m_tmpNavTris);
foreach (var tri in m_tmpNavTris)
{
Vector3 a, b, c;
tri.GetVertices(out a, out b, out c);
a = tri.Parent.LocalToGlobal(a);
b = tri.Parent.LocalToGlobal(b);
c = tri.Parent.LocalToGlobal(c);
Vector3D normal = (c - a).Cross(b - a);
Vector3D center = (a + b + c) / 3.0f;
double lowerY = Math.Min(a.Y, Math.Min(b.Y, c.Y));
double upperY = Math.Max(a.Y, Math.Max(b.Y, c.Y));
lowerY -= 0.25f;
upperY += 0.25f;
foreach (var primitive in entry.Value)
{
Vector3D primPos = primitive.WorldPosition;
Vector3D offset = primPos - center;
double offsetLen = offset.Length();
offset = offset / offsetLen;
double dot; Vector3D.Dot(ref offset, ref normal, out dot);
if (dot > -0.2f && primPos.Y < upperY && primPos.Y > lowerY)
{
double dist = offsetLen / (dot + 0.3f);
if (dist < closestDistSq)
{
closestDistSq = dist;
closestGridTri = tri;
closestLinkedPrim = primitive;
}
}
}
}
m_tmpNavTris.Clear();
if (closestGridTri != null)
{
Debug.Assert(closestLinkedPrim.GetHighLevelPrimitive() != null);
Debug.Assert(closestGridTri.GetHighLevelPrimitive() != null);
m_links.AddLink(closestLinkedPrim, closestGridTri);
SaveVoxelLinkToDictionary(cellId, closestLinkedPrim);
IncreaseGridLinkCounter(entry.Key);
}
}
ProfilerShort.End();
}
