// Return true if the given waypoints are connected by a path. O(n) (or O(1) if connected components were unchanged)
/// <summary>This function needs to be called with EntityCloseLock already acquired, or not an issue.</summary>
public static bool Connected(MyWayPoint v1, MyWayPoint v2)
{
int c1 = MyWayPointGraph.GetConnectedComponentId(v1);
int c2 = MyWayPointGraph.GetConnectedComponentId(v2);
return(c1 == c2 && c1 != -1);
}
// Add a symmetric edge between two vertices. O(1)
public static void Connect(MyWayPoint v1, MyWayPoint v2)
{
if (v1 == v2)
{
return;
}
using (NeighborsLock.AcquireExclusiveUsing())
{
MyWayPointGraph.MakeConnectedComponentsDirty();
v1.m_neighbors.Add(v2);
v2.m_neighbors.Add(v1);
}
}
// Reconstruct the path that leads to the given vertex using cameFrom links.
private static List <MyWayPoint> ReconstructPath(MyWayPoint v)
{
var result = new List <MyWayPoint>();
result.Add(v);
while (v.m_cameFrom != v)
{
v = v.m_cameFrom;
result.Add(v);
}
result.Reverse();
return(result);
}
public bool ContainsEdge(MyWayPoint v, MyWayPoint w)
{
using (MyEntities.EntityCloseLock.AcquireSharedUsing())
{
for (int i = 0; i < WayPoints.Count - 1; i++)
{
var subpath = WayPoints[i].GetShortestPathTo(WayPoints[i + 1]);
for (int j = 0; j < subpath.Count - 1; j++)
{
if ((subpath[j] == v && subpath[j + 1] == w) || (subpath[j] == w && subpath[j + 1] == v))
{
return(true);
}
}
}
}
return(false);
}
// Add a symmetric edge between two vertices if there's nothing blocking the way.
// Returns whether the connection exists after the raycast.
public static bool ConnectIfVisible(MyWayPoint v1, MyWayPoint v2)
{
if (v1 == v2)
{
return(true);
}
if (v1.Neighbors.Contains(v2))
{
return(true);
}
if (v1.Position == v2.Position)
{
Connect(v1, v2);
return(true);
}
var line = new MyLine(v1.Position, v2.Position, true);
if (MyEntities.GetAnyIntersectionWithLine(ref line, null, null, true, true, true, false) == null)
{
Connect(v1, v2);
return(true);
}
return(false);
}
// Add a symmetric edge between two vertices if there are no AABBs blocking the way.
// Returns whether the connection exists after the raycast.
public static bool ConnectIfNoAABBBlockers(MyWayPoint v1, MyWayPoint v2, MyEntity ignore1 = null, MyEntity ignore2 = null)
{
if (v1 == v2)
{
return(true);
}
if (v1.Neighbors.Contains(v2))
{
return(true);
}
if (v1.Position == v2.Position)
{
Connect(v1, v2);
return(true);
}
var line = new MyLine(v1.Position, v2.Position, true);
if (!MyEntities.IsAnyIntersectionWithLineAABBOnly(ref line, ignore1, ignore2))
{
Connect(v1, v2);
return(true);
}
return(false);
}
public override bool DebugDraw()
{ /*
* int i = MyWayPointGraph.GetConnectedComponentId(this);
* var vertexColor = HsvToRgb((0.36f + i * 0.618034f) % 1.0f, 0.8f, 0.75f);
*
* DrawWaypointVertex(WorldMatrix.Translation, vertexColor); // draw only edges for generated waypoints
*
*
* // draw edges
* foreach (var neighbor in Neighbors)
* {
* //DrawWaypointEdge(wp.WorldMatrix.Translation, neighbor.WorldMatrix.Translation, Color.Red, Color.Green); // selected path: red-green edges
*
* if (neighbor.WorldMatrix.Translation != WorldMatrix.Translation)
* {
* Vector3 direction = neighbor.WorldMatrix.Translation - WorldMatrix.Translation;
* float lineLength = direction.Length();
* direction.Normalize();
* MyTransparentGeometry.AddLineBillboard(MyTransparentMaterialEnum.ProjectileTrailLine, Color.Yellow.ToVector4(), WorldMatrix.Translation, direction, lineLength, 0.25f);
* }
*
* }
*/
if (((MyHud.ShowDebugWaypoints) || (MyGuiScreenGamePlay.Static.IsEditorActive() && !MyGuiScreenGamePlay.Static.IsIngameEditorActive())) && (MyFakes.ENABLE_GENERATED_WAYPOINTS_IN_EDITOR || MyHud.ShowDebugGeneratedWaypoints || Save))
{
// color by connected components
int i = MyWayPointGraph.GetConnectedComponentId(this);
var vertexColor = HsvToRgb((0.36f + i * 0.618034f) % 1.0f, 0.8f, 0.75f);
if (MyWayPointGraph.SelectedPath != null && MyWayPointGraph.SelectedPath.WayPoints.Contains(this))
{
vertexColor = Color.Orange.ToVector3(); // selected path: orange vertices
}
if (IsSecret)
{
vertexColor *= 0.25f;
}
// draw vertices
if (MyEditorGizmo.SelectedEntities.Contains(this))
{
DrawWaypointVertex(WorldMatrix.Translation, vertexColor + (IsSecret ? 1 : 3) * GetHighlightColor());
var name = new StringBuilder();
if (MyWayPointGraph.SelectedPath != null && MyWayPointGraph.SelectedPath.WayPoints.Contains(this))
{
name.Append(MyWayPointGraph.SelectedPath.Name).Append(": ").Append(MyWayPointGraph.SelectedPath.WayPoints.IndexOf(this) + 1);
}
else
{
name.Append(MyWayPoint.FilterWayPoints(MyEditorGizmo.SelectedEntities).IndexOf(this) + 1);
}
MyDebugDraw.DrawText(WorldMatrix.Translation, name, Color.White, 1);
}
else
{
if (Save)
{
DrawWaypointVertex(WorldMatrix.Translation, vertexColor); // for generated waypoints, draw only edges
}
}
// draw edges
if (Save || MyHud.ShowDebugGeneratedWaypoints)
{
using (MyWayPoint.NeighborsLock.AcquireSharedUsing())
{
foreach (var neighbor in Neighbors)
{
if (MyWayPointGraph.SelectedPath != null && MyWayPointGraph.SelectedPath.ContainsEdge(this, neighbor))
{
DrawWaypointEdge(WorldMatrix.Translation, neighbor.WorldMatrix.Translation, Color.Yellow, Color.White); // on selected path: yellow-white
continue;
}
if (neighbor.Save || MyHud.ShowDebugGeneratedWaypoints)
{
using (MyWayPoint.BlockedEdgesLock.AcquireSharedUsing())
{
// blocked for player (by a locked indestructible door: white-gray)
if (BlockedEdgesForPlayer.Contains(Tuple.Create(this, neighbor)) || BlockedEdgesForPlayer.Contains(Tuple.Create(neighbor, this)))
{
DrawWaypointEdge(WorldMatrix.Translation, neighbor.WorldMatrix.Translation, Color.White, Color.Gray);
continue;
}
// blocked for bots by a locked door: black-gray
if (BlockedEdgesForBots.Contains(Tuple.Create(this, neighbor)) || BlockedEdgesForBots.Contains(Tuple.Create(neighbor, this)))
{
DrawWaypointEdge(WorldMatrix.Translation, neighbor.WorldMatrix.Translation, Color.Black, Color.Gray);
continue;
}
}
// obstructed: violet-white
if (MyHud.ShowDebugWaypointsCollisions && Position != neighbor.Position)
{
var line = new MyLine(Position, neighbor.Position, true);
if (MyEntities.GetAnyIntersectionWithLine(ref line, null, null, true, true, true, false) != null)
{
DrawWaypointEdge(WorldMatrix.Translation, neighbor.WorldMatrix.Translation, Color.Violet, Color.White);
continue;
}
}
// normal-normal: red-green
// generated-normal: orange-green (normally invisible)
// generated-generated: yellow-green (normally invisible)
bool generated = !(Save && neighbor.Save);
bool fullyGenerated = !Save && !neighbor.Save;
DrawWaypointEdge(WorldMatrix.Translation, neighbor.WorldMatrix.Translation, generated ? fullyGenerated ? Color.Yellow : Color.Orange : Color.Red, Color.Green);
continue;
}
}
}
}
}
return(base.DebugDraw());
}
// Get the distance between two vertices, no matter their parents.
public static float Distance(MyWayPoint v1, MyWayPoint v2)
{
return(Vector3.Distance(v1.Position, v2.Position));
}
// Get the shortest path to the goal vertex using the A* algorithm. Return an empty list if the goal is unreachable.
/// <summary>This function needs to be called with EntityCloseLock already acquired, or not an issue.</summary>
public List <MyWayPoint> GetShortestPathTo(MyWayPoint goal, HashSet <Tuple <MyWayPoint, MyWayPoint> > blockedEdges = null, bool useGeneratedWaypoints = true, bool useSecretWaypoints = true)
{
lock (m_shortestPathLock)
{
if (Connected(this, goal) && (useGeneratedWaypoints || (Save && goal.Save)) && (useSecretWaypoints || (!IsSecret && !goal.IsSecret)))
{
// use next search id, handle overflow
m_searchId++;
if (m_searchId == int.MaxValue)
{
MyWayPointGraph.ResetAllVisitedSearchIds();
m_searchId = 1;
}
// seen but yet unvisited vertices; add start vertex
m_openSet.Clear();
m_fScore = Distance(this, goal); // estimate distance to goal as Euclidean (= consistent)
m_gScore = 0;
m_cameFrom = this;
m_openSet.Add(this);
int num = 0;
while (m_openSet.Count != 0)
{
if (num++ > MyWayPointGraph.WaypointCount() + 10)
{
int maxSearchId = 0;
foreach (var v in MyWayPointGraph.GetCopyOfAllWaypoints())
{
maxSearchId = Math.Max(maxSearchId, v.m_visitedSearchId);
}
Debug.Fail(string.Format(
"Infinite path search... contact JanK! \nDebug info: set_count={0} start_end_connected={1} current_search_id={2} max_search_id={3}",
m_openSet.Count,
Connected(this, goal),
m_searchId,
maxSearchId
));
break;
}
var current = m_openSet.Min; // get vertex with the smallest f (ties: highest g)
m_openSet.Remove(current);
if (current == goal)
{
return(ReconstructPath(goal)); // found the shortest path, return it
}
current.m_visitedSearchId = m_searchId; // mark current as visited
using (NeighborsLock.AcquireSharedUsing())
{
// look at all neighbors
foreach (var neighbor in current.m_neighbors)
{
if (!useGeneratedWaypoints && !neighbor.Save)
{
continue; // generated
}
if (!useSecretWaypoints && neighbor.IsSecret)
{
continue; // secret
}
if (neighbor.m_visitedSearchId == m_searchId)
{
continue; // already visited
}
if (blockedEdges != null && blockedEdges.Contains(Tuple.Create(current, neighbor)))
{
continue; // blocked
}
var gScoreThroughCurrent = current.m_gScore + Distance(current, neighbor);
if (m_openSet.Contains(neighbor)) // already seen
{
if (gScoreThroughCurrent < neighbor.m_gScore)
{
m_openSet.Remove(neighbor); // path through current is better: remove neighbor and put it back with updated score
}
else
{
continue;
}
}
neighbor.m_fScore = gScoreThroughCurrent + Distance(neighbor, goal);
neighbor.m_gScore = gScoreThroughCurrent;
neighbor.m_cameFrom = current;
m_openSet.Add(neighbor);
}
}
}
}
}
return(new List <MyWayPoint>()); // unreachable
}
// Return the complete shortest path between vertices in a list.
public List <MyWayPoint> CompletePath(HashSet <Tuple <MyWayPoint, MyWayPoint> > blockedEdges, MyWayPoint currentWayPoint, bool useGeneratedWaypoints = true, bool cycle = false, bool cachedIsOk = true)
{
if (!cachedIsOk || m_cachedCompletePath == null)
{
using (MyEntities.EntityCloseLock.AcquireSharedUsing())
{
m_cachedCompletePath = new List <MyWayPoint>();
if (WayPoints == null || WayPoints.Count == 0)
{
return(m_cachedCompletePath);
}
int currentIndex = currentWayPoint != null?WayPoints.IndexOf(currentWayPoint) : 0;
m_cachedCompletePath.Add(WayPoints[0]);
//for (int i = 0; i < WayPoints.Count - 1; i++)
//{
// var subpath = WayPoints[i].GetShortestPathTo(WayPoints[i + 1], blockedEdges, useGeneratedWaypoints);
// if (subpath.Count == 0)
// m_cachedCompletePath.Add(WayPoints[i + 1]); // no path: add straight line to next waypoint
// else
// m_cachedCompletePath.AddRange(subpath.GetRange(1, subpath.Count - 1)); // the first point was the last point of the previous segment
//}
//if (cycle)
//{
// var subpath = WayPoints[WayPoints.Count - 1].GetShortestPathTo(WayPoints[0], blockedEdges, useGeneratedWaypoints);
// if (subpath.Count > 2)
// m_cachedCompletePath.AddRange(subpath.GetRange(1, subpath.Count - 2)); // the first point was the last point of the previous segment, the last point was point 0
//}
for (int i = 0; i < WayPoints.Count - 1; i++)
{
var subpath = WayPoints[i].GetShortestPathTo(WayPoints[i + 1], blockedEdges, useGeneratedWaypoints);
if (subpath.Count == 0)
{
if (blockedEdges != null && blockedEdges.Contains(Tuple.Create(WayPoints[i], WayPoints[i + 1])))
{
// this part of the path is unreachable
if (currentIndex > i)
{
m_cachedCompletePath.Clear();
m_cachedCompletePath.Add(WayPoints[i + 1]);
}
else
{
break;
}
}
else
{
m_cachedCompletePath.Add(WayPoints[i + 1]); // no path: add straight line to next waypoint
}
}
else
{
m_cachedCompletePath.AddRange(subpath.GetRange(1, subpath.Count - 1)); // the first point was the last point of the previous segment
}
}
if (cycle)
{
var subpath = WayPoints[WayPoints.Count - 1].GetShortestPathTo(WayPoints[0], blockedEdges, useGeneratedWaypoints);
if (subpath.Count > 2)
{
m_cachedCompletePath.AddRange(subpath.GetRange(1, subpath.Count - 2)); // the first point was the last point of the previous segment, the last point was point 0
}
}
}
}
return(m_cachedCompletePath);
}
