public override bool TryGetReached(long key, out PathNode reached)
{
return(m_reachedNodes.TryGetValue(key, out reached));
}
public abstract bool TryGetReached(long key, out PathNode reached);
/// <summary>
/// Create a path node from a parent.
/// </summary>
/// <param name="parentKey">The key of the parent's postion.</param>
/// <param name="parent">The parent of the new node.</param>
/// <param name="neighbour">The cell direction of the new node.</param>
/// <param name="distMulti">Multiplied by NodeDistance to get the distance the new node will be from its parent.</param>
private void CreatePathNode(long parentKey, ref PathNode parent, Vector3I neighbour, float distMulti)
{
Profiler.StartProfileBlock();
Vector3D position = parent.Position + neighbour * NodeDistance * distMulti;
// round position so that is a discrete number of steps from m_referencePosition
Vector3D finishToPosition; Vector3D.Subtract(ref position, ref m_referencePosition, out finishToPosition);
VectorExtensions.RoundTo(ref finishToPosition, NodeDistance);
Vector3D.Add(ref m_referencePosition, ref finishToPosition, out position);
//Log.DebugLog("m_reachedNodes == null", Logger.severity.FATAL, condition: m_reachedNodes == null);
if (m_reachedNodes.ContainsKey(position.GetHash()))
{
Profiler.EndProfileBlock();
return;
}
PathNode result = new PathNode(ref parent, ref position);
float turn; Vector3.Dot(ref parent.DirectionFromParent, ref result.DirectionFromParent, out turn);
if (turn < 0f)
{
Profiler.EndProfileBlock();
return;
}
double distToDest = double.MaxValue;
foreach (PathNodeSet target in m_targets)
{
double dist = MinPathDistance(ref target.m_startPosition, ref position);
if (dist < distToDest)
{
distToDest = dist;
}
}
float resultKey = result.DistToCur + (float)distToDest;
if (turn > 0.99f && parent.ParentKey != 0f)
{
parentKey = parent.ParentKey;
}
else
{
if (turn < 0f)
{
Profiler.EndProfileBlock();
return;
}
resultKey += TurnPenalty * (1f - turn);
}
//Log.DebugLog("DirectionFromParent is incorrect. DirectionFromParent: " + result.DirectionFromParent + ", parent: " + parent.Position + ", current: " + result.Position + ", direction: " +
// Vector3.Normalize(result.Position - parent.Position), Logger.severity.ERROR, condition: !Vector3.Normalize(result.Position - parent.Position).Equals(result.DirectionFromParent, 0.01f));
//Log.DebugLog("Length is incorrect. Length: " + (result.DistToCur - parent.DistToCur) + ", distance: " + Vector3D.Distance(result.Position, parent.Position), Logger.severity.ERROR,
// condition: Math.Abs((result.DistToCur - parent.DistToCur) - (Vector3D.Distance(result.Position, parent.Position))) > 0.01f);
//Log.DebugLog("resultKey <= 0", Logger.severity.ERROR, condition: resultKey <= 0f);
AddOpenNode(ref result, resultKey);
Profiler.EndProfileBlock();
}
