public override SimRouteMulti Prepend(SimRoute extra)
{
IList <SimRoute> MS = new List <SimRoute>();
MS.Add(extra);
MS.Add(this);
return(new SimRouteMulti(MS));
}
private SimRoute[] FakeRoute(SimWaypoint StartNode, SimWaypoint EndNode)
{
SimRoute[] route = new SimRoute[1];//
SimRoute fr = Intern2Arc(StartNode, EndNode, 1.2f);
//fr.Passable = true;
route[0] = fr;
return(route);
}
/// <summary>
/// Object.Equals override.
/// Tells if two arcs are equal by comparing StartNode and EndNode.
/// </summary>
/// <exception cref="ArgumentException">Cannot compare an arc with another type.</exception>
/// <param name="O">The arc to compare with.</param>
/// <returns>'true' if both arcs are equal.</returns>
public override bool Equals(object O)
{
SimRoute A = (SimRoute)O;
if (A == null)
{
throw new ArgumentException("Cannot compare type " + GetType() + " with type " + O.GetType() + " !");
}
return(_StartNode.Equals(A._StartNode) && _EndNode.Equals(A._EndNode));
}
public Track(Track PreviousTrack, SimRoute Transition)
{
if (_Target == null)
{
throw new InvalidOperationException("You must specify a target Node for the Track class.");
}
Queue = PreviousTrack;
_Cost = Queue.Cost + Transition.Cost;
_NbArcsVisited = Queue._NbArcsVisited + 1;
EndNode = Transition.EndNode;
}
private SimRoute FindArc(SimWaypoint s, SimWaypoint e)
{
lock (SimRoutes) for (int i = SimRoutes.Count; i != 0;)
{
SimRoute sr = SimRoutes[--i];
if (sr.IsSame(s, e))
{
return(sr);
}
}
return(null);
}
public virtual SimRouteMulti Prepend(SimRoute extra)
{
if (extra is SimRouteMulti)
{
return(extra.Append(this));
}
IList <SimRoute> MS = new List <SimRoute>();
MS.Add(extra);
MS.Add(this);
return(new SimRouteMulti(MS));
}
public override SimRoute WhichRoute(Vector3d point)
{
foreach (SimRoute R in MoveList)
{
SimRoute W = R.WhichRoute(point);
if (W != null)
{
return(W);
}
}
return(null);
}
/// <summary>
/// Directly Adds an arc to the graph.
/// </summary>
/// <exception cref="ArgumentException">Cannot add an arc if one of its extremity nodes does not belong to the graph.</exception>
/// <param name="NewArc">The arc to add.</param>
/// <returns>'true' if it has actually been added / 'false' if the arc is null or if it is already in the graph.</returns>
public bool AddArc(SimRoute NewArc)
{
if (NewArc == null || SimRoutes.Contains(NewArc))
{
return(false);
}
if (!SimWaypoints.Contains(NewArc.StartNode) || !SimWaypoints.Contains(NewArc.EndNode))
{
throw new ArgumentException("Cannot add an arc if one of its extremity nodes does not belong to the graph.");
}
SimRoutesAdd(NewArc);
return(true);
}
/// <summary>
/// Removes a node from the graph as well as the linked arcs.
/// </summary>
/// <param name="ArcToRemove">The arc to remove.</param>
/// <returns>'true' if succeeded / 'false' otherwise.</returns>
public bool RemoveArc(SimRoute ArcToRemove)
{
if (ArcToRemove == null)
{
return(false);
}
try
{
lock (SimRoutes) SimRoutes.Remove(ArcToRemove);
lock (ArcToRemove.StartNode.OutgoingArcs) ArcToRemove.StartNode.OutgoingArcs.Remove(ArcToRemove);
lock (ArcToRemove.StartNode.IncomingArcs) ArcToRemove.EndNode.IncomingArcs.Remove(ArcToRemove);
}
catch { return(false); }
return(true);
}
public SimRoute InternArc(SimWaypoint s, SimWaypoint e, double W)
{
if (s == e)
{
throw new ArgumentException("s and e the same!" + s);
}
SimRoute fr = FindArc(s, e);
if (fr == null)
{
fr = new SimRoute(s, e);
SimRoutesAdd(fr);
}
fr.Weight = W;
return(fr);
}
/// <summary>
/// Creates an arc between two nodes that are already registered in the graph, adds it to the graph and returns its reference.
/// </summary>
/// <exception cref="ArgumentException">Cannot add an arc if one of its extremity nodes does not belong to the graph.</exception>
/// <param name="StartNode">Start node for the arc.</param>
/// <param name="EndNode">End node for the arc.</param>
/// <param name="Weight">Weight for the arc.</param>
/// <returns>The reference of the new arc / null if the arc is already in the graph.</returns>
public SimRoute AddArc(SimWaypoint StartNode, SimWaypoint EndNode, double Weight)
{
SimRoute NewArc = FindArc(StartNode, EndNode);
if (NewArc == null)
{
NewArc = new SimRoute(StartNode, EndNode);
NewArc.Weight = Weight;
return(AddArc(NewArc) ? NewArc : null);
}
else
{
NewArc.Weight = Weight;
return(AddArc(NewArc) ? NewArc : null);
}
///SimRoute NewArc = new SimRoute(StartNode, EndNode);
}
public virtual SimRouteMulti Divide(int by, SimPathStore PathStore)
{
IList <SimRoute> moves = new List <SimRoute>();
double len = Length;
double seglen = len / by;
SimWaypoint beg = StartNode;
int current = 1;
while (current < by)
{
SimWaypoint end = GetPointAt(seglen * current, PathStore);
SimRoute move = new SimRoute(beg, end);
moves.Add(move);
beg = end;
}
return(CopyProperties(this, new SimRouteMulti(moves)));
}
public static SimRoute CopyProperties(SimRoute simMovement, SimRoute movement)
{
//if (simMovement.MustAutoPilot) movement.MustAutoPilot = simMovement.MustAutoPilot;
if (simMovement.MustCrouch)
{
movement.MustCrouch = simMovement.MustCrouch;
}
if (simMovement.MustFly)
{
movement.MustFly = simMovement.MustFly;
}
//if (simMovement.IsBlocked) movement.IsBlocked = simMovement.IsBlocked;
//if (simMovement.IsOneDirrection) movement.IsOneDirrection = simMovement.IsOneDirrection;
movement.Weight = simMovement.Weight;
movement._Length = simMovement._Length;
movement.LengthUpdated = simMovement.LengthUpdated;
return(movement);
}
public SimRoute[] GetRoute(SimWaypoint StartNode, SimWaypoint EndNode, out bool IsFake)
{
SimMovement AS = new SimMovement(this);
AS.Initialize(StartNode, EndNode);
while (AS.NextStep())
{
}
if (AS.PathFound)
{
// Full Path
IsFake = false;
return(AS.PathByArcs);
}
// Partial Path
IsFake = true;
//int Nb = AS._LeafToGoBackUp.NbArcsVisited;
//SimRoute[] Path = new SimRoute[Nb];
//Track Cur = _LeafToGoBackUp;
//for (int i = Nb - 1; i >= 0; i--, Cur = Cur.Queue)
// Path[i] = Cur.Queue.EndNode.ArcGoingTo(Cur.EndNode);
//return Path;
//AS.Open.Length, AS.Closed.Length, AS.StepCounter
SimRoute[] PathByArcs = AS.PathByArcs;
if (PathByArcs == null || PathByArcs.Length == 0)
{
return(FakeRoute(StartNode, EndNode));
}
List <SimRoute> list = new List <SimRoute>();
list.AddRange(PathByArcs);
SimRoute LastArc = PathByArcs[PathByArcs.Length - 1];
list.AddRange(FakeRoute(LastArc.EndNode, EndNode));
return(list.ToArray());
}
/// <summary>
/// This function will find the closest arc from a geographical position in space using projection.
/// </summary>
/// <param name="PtX">X coordinate of the point from which you want the closest arc.</param>
/// <param name="PtY">Y coordinate of the point from which you want the closest arc.</param>
/// <param name="PtZ">Z coordinate of the point from which you want the closest arc.</param>
/// <param name="Distance">The distance to the closest arc.</param>
/// <param name="IgnorePassableProperty">if 'false', then arcs whose property Passable is set to false will not be taken into account.</param>
/// <returns>The closest arc that has been found.</returns>
public SimRoute ClosestArc(double PtX, double PtY, double PtZ, out double Distance, bool IgnorePassableProperty)
{
SimRoute ArcMin = null;
double DistanceMin = -1;
Vector3d P = new Vector3d(PtX, PtY, PtZ);
lock (SimRoutes) foreach (SimRoute A in SimRoutes)
{
if (IgnorePassableProperty && A.Passable == false)
{
continue;
}
Vector3d Projection = ProjectOnLine(P, A.StartNode.Position, A.EndNode.Position);
double DistanceTemp = Vector3d.Distance(P, Projection);
if (DistanceMin == -1 || DistanceMin > DistanceTemp)
{
DistanceMin = DistanceTemp;
ArcMin = A;
}
}
Distance = DistanceMin;
return(ArcMin);
}
public SimMoverState FollowRoute(SimRoute route)
{
Vector3d vectStart = route.StartNode.GlobalPosition;
Vector3d vectMover = Mover.GlobalPosition;
double currentDistFromStart = DistanceNoZ(vectMover, vectStart);
if (currentDistFromStart > CloseDistance)
{
Debug("FollowRoute: TRYING for Start " + vectMover + " -> " + vectStart);
if (!MoveTo(vectStart))
{
Debug("FollowRoute: FAILED Start " + vectMover + " -> " + vectStart);
return(SimMoverState.TRYAGAIN);
}
}
Vector3d endVect = route.EndNode.GlobalPosition;
bool MadeIt = MoveTo(endVect);
if (!MadeIt)
{
Debug("FollowRoute: BLOCKED ROUTE " + vectMover + "-> " + endVect);
//route.Passable = false;
return(SimMoverState.BLOCKED);
}
Vector3d endVectMover = Mover.GlobalPosition;
double currentDistFromfinish = DistanceNoZ(endVectMover, endVect);
if (currentDistFromfinish > CloseDistance)
{
Debug("FollowRoute: CANNOT FINISH " + endVectMover + " -> " + endVect);
return(SimMoverState.PAUSED);
}
Debug("FollowRoute: SUCCEED " + vectStart + " -> " + endVectMover);
return(SimMoverState.COMPLETE);
}
public void SetBlocked(SimRoute StuckAt)
{
BlockTowardsVector(StuckAt._EndNode.SimPosition);
Vector3d pos = Mover.GlobalPosition;
if (StuckAt.BlockedPoint(GetGlobal(pos)))
{
StuckAt.ReWeight(1.1f);
Debug("BLOCKED: " + StuckAt);
}
else
{
SimRoute StuckAt2 = OuterRoute.WhichRoute(GetGlobal(pos));
if (StuckAt2 == null)
{
StuckAt.ReWeight(1.3f);
//OuterRoute.ReWeight(1.2f);
Debug("INACESSABLE: " + StuckAt);
StuckAt.Passable = false;
}
else
{
StuckAt2.ReWeight(1.1f);
Debug("ROUTE BLOCKED: " + StuckAt2);
StuckAt2.BlockedPoint(GetGlobal(pos));
StuckAt2.Passable = false;
StuckAt2.Reverse.Passable = false;
}
}
// SimPathStore.Instance.RemoveArc(StuckAt);
///SimPathStore.Instance.RemoveArc(StuckAt.Reverse);
STATE = SimMoverState.BLOCKED;
}
public void SetBlocked(SimRoute StuckAt)
{
BlockTowardsVector(StuckAt._EndNode.SimPosition);
Vector3d pos = Mover.GlobalPosition;
if (StuckAt.BlockedPoint(GetGlobal(pos)))
{
StuckAt.ReWeight(1.1f);
Debug("BLOCKED: " + StuckAt);
}
else
{
SimRoute StuckAt2 = OuterRoute.WhichRoute(GetGlobal(pos));
if (StuckAt2 == null)
{
StuckAt.ReWeight(1.3f);
//OuterRoute.ReWeight(1.2f);
Debug("INACESSABLE: " + StuckAt);
StuckAt.Passable = false;
}
else
{
StuckAt2.ReWeight(1.1f);
Debug("ROUTE BLOCKED: " + StuckAt2);
StuckAt2.BlockedPoint(GetGlobal(pos));
StuckAt2.Passable = false;
StuckAt2.Reverse.Passable = false;
}
}
// SimPathStore.Instance.RemoveArc(StuckAt);
///SimPathStore.Instance.RemoveArc(StuckAt.Reverse);
STATE = SimMoverState.BLOCKED;
}
public virtual SimRouteMulti Divide(int by, SimPathStore PathStore)
{
IList<SimRoute> moves = new List<SimRoute>();
double len = Length;
double seglen = len / by;
SimWaypoint beg = StartNode;
int current = 1;
while (current < by)
{
SimWaypoint end = GetPointAt(seglen * current, PathStore);
SimRoute move = new SimRoute(beg, end);
moves.Add(move);
beg = end;
}
return CopyProperties(this, new SimRouteMulti(moves));
}
public virtual SimRouteMulti Prepend(SimRoute extra)
{
if (extra is SimRouteMulti)
{
return extra.Append(this);
}
IList<SimRoute> MS = new List<SimRoute>();
MS.Add(extra);
MS.Add(this);
return new SimRouteMulti(MS);
}
private void SimRoutesAdd(SimRoute NewArc)
{
lock (SimRoutes) SimRoutes.Add(NewArc);
}
private void SimRoutesAdd(SimRoute NewArc)
{
lock (SimRoutes) SimRoutes.Add(NewArc);
}
private SimRoute[] FakeRoute(SimWaypoint StartNode, SimWaypoint EndNode)
{
SimRoute[] route = new SimRoute[1];//
SimRoute fr = Intern2Arc(StartNode, EndNode, 1.2f);
//fr.Passable = true;
route[0] = fr;
return route;
}
static private void DessinerArc(Graphics Grfx, Pen P, SimRoute A)
{
DessinerArc(Grfx, P, A.StartNode, A.EndNode);
}
public SimRouteMover(SimMover mover, IList <SimRoute> routes, Vector3d finalGoal, double finalDistance)
: base(mover, mover.PathStore.CreateClosestWaypoint(finalGoal, finalDistance), finalDistance)
{
Routes = routes;
OuterRoute = new SimRouteMulti(routes);
}
//public bool IsOneDirrection = true;
public static SimRouteMulti CopyProperties(SimRoute simMovement, SimRouteMulti movement)
{
return((SimRouteMulti)CopyProperties(simMovement, (SimRoute)movement));
}
public static SimRoute CopyProperties(SimRoute simMovement, SimRoute movement)
{
//if (simMovement.MustAutoPilot) movement.MustAutoPilot = simMovement.MustAutoPilot;
if (simMovement.MustCrouch) movement.MustCrouch = simMovement.MustCrouch;
if (simMovement.MustFly) movement.MustFly = simMovement.MustFly;
//if (simMovement.IsBlocked) movement.IsBlocked = simMovement.IsBlocked;
//if (simMovement.IsOneDirrection) movement.IsOneDirrection = simMovement.IsOneDirrection;
movement.Weight = simMovement.Weight;
movement._Length = simMovement._Length;
movement.LengthUpdated = simMovement.LengthUpdated;
return movement;
}
/// <summary>
/// Removes a node from the graph as well as the linked arcs.
/// </summary>
/// <param name="ArcToRemove">The arc to remove.</param>
/// <returns>'true' if succeeded / 'false' otherwise.</returns>
public bool RemoveArc(SimRoute ArcToRemove)
{
if (ArcToRemove == null) return false;
try
{
lock (SimRoutes) SimRoutes.Remove(ArcToRemove);
lock (ArcToRemove.StartNode.OutgoingArcs) ArcToRemove.StartNode.OutgoingArcs.Remove(ArcToRemove);
lock (ArcToRemove.StartNode.IncomingArcs) ArcToRemove.EndNode.IncomingArcs.Remove(ArcToRemove);
}
catch { return false; }
return true;
}
public override SimMoverState Goto()
{
STATE = SimMoverState.MOVING;
int CanSkip = 0;
int Skipped = 0;
int tried = 0;
SimRoute prev = null;
for (int cI = CurrentRouteIndex; cI < Routes.Count; cI++)
{
CurrentRouteIndex = cI;
if (cI > 0)
{
prev = Routes[cI - 1];
}
SimRoute route = Routes[cI];
if (route.IsBlocked)
{
STATE = SimMoverState.BLOCKED;
continue;
}
tried++;
// TRY
STATE = FollowRoute(route);
double distance = Mover.Distance(FinalPosition);
if (STATE == SimMoverState.BLOCKED)
{
Mover.StopMoving();
// SetBlocked(route);
if (distance < FinalDistance)
{
return(SimMoverState.COMPLETE);
}
//CreateSurroundWaypoints();
route.ReWeight(1.1f);
route.BumpyCount++;
if (CanSkip > 0)
{
CanSkip--;
Skipped++;
continue;
}
if (route.BumpyCount > 0 && Skipped == 0)
{
SetBlocked(route);
// if (prev!=null) if (FollowRoute(prev.Reverse) == SimMoverState.COMPLETE)
// {
// return SimMoverState.TRYAGAIN;
// }
}
return(STATE);
}
if (STATE == SimMoverState.PAUSED)
{
if (distance < FinalDistance)
{
return(SimMoverState.COMPLETE);
}
return(SimMoverState.TRYAGAIN);
}
if (STATE == SimMoverState.COMPLETE)
{
// if made it here then the prev was very good
if (prev != null)
{
prev.ReWeight(0.8f);
}
}
if (distance < FinalDistance)
{
return(SimMoverState.COMPLETE);
}
}
if (STATE != SimMoverState.COMPLETE)
{
if (tried == 0)
{
return(SimMoverState.TRYAGAIN);
}
return(STATE);
}
OuterRoute.ReWeight(0.7f); // Reward
//TODO Mover.PathStore.AddArc(OuterRoute);
STATE = SimMoverState.COMPLETE;
return(STATE);
}
public SimMoverState FollowRoute(SimRoute route)
{
Vector3d vectStart = route.StartNode.GlobalPosition;
Vector3d vectMover = Mover.GlobalPosition;
double currentDistFromStart = DistanceNoZ(vectMover, vectStart);
if (currentDistFromStart > CloseDistance)
{
Debug("FollowRoute: TRYING for Start " + vectMover + " -> " + vectStart);
if (!MoveTo(vectStart))
{
Debug("FollowRoute: FAILED Start " + vectMover + " -> " + vectStart);
return SimMoverState.TRYAGAIN;
}
}
Vector3d endVect = route.EndNode.GlobalPosition;
bool MadeIt = MoveTo(endVect);
if (!MadeIt)
{
Debug("FollowRoute: BLOCKED ROUTE " + vectMover + "-> " + endVect);
//route.Passable = false;
return SimMoverState.BLOCKED;
}
Vector3d endVectMover = Mover.GlobalPosition;
double currentDistFromfinish = DistanceNoZ(endVectMover, endVect);
if (currentDistFromfinish > CloseDistance)
{
Debug("FollowRoute: CANNOT FINISH " + endVectMover + " -> " + endVect);
return SimMoverState.PAUSED;
}
Debug("FollowRoute: SUCCEED " + vectStart + " -> " + endVectMover);
return SimMoverState.COMPLETE;
}
public SimRouteMover(SimMover mover, IList<SimRoute> routes, Vector3d finalGoal, double finalDistance)
: base(mover, mover.PathStore.CreateClosestWaypoint(finalGoal, finalDistance), finalDistance)
{
Routes = routes;
OuterRoute = new SimRouteMulti(routes);
}
private void AddArc(SimRoute R)
{
SimGlobalRoutes.Instance.AddArc(R);
}
public SimRoute InternArc(SimWaypoint s, SimWaypoint e, double W)
{
if (s == e) throw new ArgumentException("s and e the same!" + s);
SimRoute fr = FindArc(s, e);
if (fr == null)
{
fr = new SimRoute(s, e);
SimRoutesAdd(fr);
}
fr.Weight = W;
return fr;
}
public override SimRouteMulti Prepend(SimRoute extra)
{
IList<SimRoute> MS = new List<SimRoute>();
MS.Add(extra);
MS.Add(this);
return new SimRouteMulti(MS);
}
/// <summary>
/// Directly Adds an arc to the graph.
/// </summary>
/// <exception cref="ArgumentException">Cannot add an arc if one of its extremity nodes does not belong to the graph.</exception>
/// <param name="NewArc">The arc to add.</param>
/// <returns>'true' if it has actually been added / 'false' if the arc is null or if it is already in the graph.</returns>
public bool AddArc(SimRoute NewArc)
{
if (NewArc == null || SimRoutes.Contains(NewArc)) return false;
if (!SimWaypoints.Contains(NewArc.StartNode) || !SimWaypoints.Contains(NewArc.EndNode))
throw new ArgumentException("Cannot add an arc if one of its extremity nodes does not belong to the graph.");
SimRoutesAdd(NewArc);
return true;
}
/// <summary>
/// When something is changed the simRouteMovement that must be updated
/// </summary>
/// <param name="simRouteMovement"></param>
public virtual void AddDependant(SimRoute simRouteMovement)
{
Dependants.Add(simRouteMovement);
}
/// <summary>
/// Creates an arc between two nodes that are already registered in the graph, adds it to the graph and returns its reference.
/// </summary>
/// <exception cref="ArgumentException">Cannot add an arc if one of its extremity nodes does not belong to the graph.</exception>
/// <param name="StartNode">Start node for the arc.</param>
/// <param name="EndNode">End node for the arc.</param>
/// <param name="Weight">Weight for the arc.</param>
/// <returns>The reference of the new arc / null if the arc is already in the graph.</returns>
public SimRoute AddArc(SimWaypoint StartNode, SimWaypoint EndNode, double Weight)
{
SimRoute NewArc = FindArc(StartNode, EndNode);
if (NewArc == null)
{
NewArc = new SimRoute(StartNode, EndNode);
NewArc.Weight = Weight;
return AddArc(NewArc) ? NewArc : null;
}
else
{
NewArc.Weight = Weight;
return AddArc(NewArc) ? NewArc : null;
}
///SimRoute NewArc = new SimRoute(StartNode, EndNode);
}
/// <summary>
/// When something is changed the simRouteMovement that must be updated
/// </summary>
/// <param name="simRouteMovement"></param>
public virtual void AddDependant(SimRoute simRouteMovement)
{
Dependants.Add(simRouteMovement);
}
public Track(Track PreviousTrack, SimRoute Transition)
{
if (_Target == null) throw new InvalidOperationException("You must specify a target Node for the Track class.");
Queue = PreviousTrack;
_Cost = Queue.Cost + Transition.Cost;
_NbArcsVisited = Queue._NbArcsVisited + 1;
EndNode = Transition.EndNode;
}
//public bool IsOneDirrection = true;
public static SimRouteMulti CopyProperties(SimRoute simMovement, SimRouteMulti movement)
{
return (SimRouteMulti)CopyProperties(simMovement, (SimRoute)movement);
}
