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();
}
public void Init(Vector3D start, MySmartGoal goal)
{
ProfilerShort.Begin("MySmartPath.Init()");
Debug.Assert(m_valid == false);
m_lastInitTime = MySandboxGame.TotalGamePlayTimeInMilliseconds;
m_startPoint = start;
m_goal = goal;
ProfilerShort.Begin("Find start primitive");
ProfilerShort.Begin("FindClosestPrimitive");
m_currentPrimitive = m_pathfinding.FindClosestPrimitive(start, highLevel: false);
if (m_currentPrimitive != null)
{
ProfilerShort.BeginNextBlock("GetHighLevelPrimitive");
m_hlBegin = m_currentPrimitive.GetHighLevelPrimitive();
Debug.Assert(m_hlBegin != null, "Start primitive did not have a high-level primitive!");
if (m_hlBegin != null && !m_pathNodes.Contains(m_hlBegin))
{
ProfilerShort.BeginNextBlock("ObservePrimitive");
m_hlBegin.Parent.ObservePrimitive(m_hlBegin, this);
}
}
ProfilerShort.End();
ProfilerShort.End();
if (m_currentPrimitive == null)
{
// CH: TODO: Starting primitive was not found. What to do now?
m_currentPrimitive = null;
Invalidate();
ProfilerShort.End();
return;
}
m_pathNodePosition = 0;
m_expandedPathPosition = 0;
m_expandedPath.Clear();
m_pathNodes.Clear();
m_usedWholePath = false;
m_valid = true;
ProfilerShort.End();
}
private void RefineFoundPath(ref Vector3D begin, ref Vector3D end, MyPath <MyNavigationPrimitive> path)
{
Debug.Assert(MyPerGameSettings.EnablePathfinding, "Pathfinding is not enabled!");
if (!MyPerGameSettings.EnablePathfinding)
{
return;
}
if (path == null)
{
Debug.Assert(false, "Path to refine was null!");
return;
}
m_currentPrimitive = path[path.Count - 1].Vertex as MyNavigationPrimitive;
if (m_hlBegin != null && !m_pathNodes.Contains(m_hlBegin))
{
m_hlBegin.Parent.StopObservingPrimitive(m_hlBegin, this);
}
m_hlBegin = m_currentPrimitive.GetHighLevelPrimitive();
if (m_hlBegin != null && !m_pathNodes.Contains(m_hlBegin))
{
m_hlBegin.Parent.ObservePrimitive(m_hlBegin, this);
}
ProfilerShort.Begin("Path refining and post-processing");
IMyNavigationGroup prevGroup = null;
int groupStart = 0;
int groupEnd = 0;
Vector3 prevBegin = default(Vector3);
Vector3 prevEnd = default(Vector3);
for (int i = 0; i < path.Count; ++i)
{
var primitive = path[i].Vertex as MyNavigationPrimitive;
var group = primitive.Group;
if (prevGroup == null)
{
prevGroup = group;
prevBegin = prevGroup.GlobalToLocal(begin);
}
bool lastPrimitive = i == path.Count - 1;
if (group != prevGroup)
{
groupEnd = i - 1;
prevEnd = prevGroup.GlobalToLocal(primitive.WorldPosition);
}
else if (lastPrimitive)
{
groupEnd = i;
prevEnd = prevGroup.GlobalToLocal(end);
}
else
{
continue;
}
int refinedBegin = m_expandedPath.Count;
prevGroup.RefinePath(path, m_expandedPath, ref prevBegin, ref prevEnd, groupStart, groupEnd);
int refinedEnd = m_expandedPath.Count;
for (int j = refinedBegin; j < refinedEnd; ++j)
{
Vector3D position = new Vector3D(m_expandedPath[j]);
position = prevGroup.LocalToGlobal(position);
m_expandedPath[j] = new Vector4D(position, m_expandedPath[j].W);
}
if (lastPrimitive && group != prevGroup)
{
m_expandedPath.Add(new Vector4D(primitive.WorldPosition, m_expandedPath[refinedEnd - 1].W));
}
prevGroup = group;
groupStart = i;
if (m_expandedPath.Count != 0)
{
prevBegin = group.GlobalToLocal(new Vector3D(m_expandedPath[m_expandedPath.Count - 1]));
}
}
m_pathNodePosition++;
//m_expandedPath.RemoveAt(0);
m_expandedPathPosition = 0;
ProfilerShort.End();
}