/// <summary>
/// Arc constructor.
/// </summary>
/// <exception cref="ArgumentNullException">Extremity nodes cannot be null.</exception>
/// <exception cref="ArgumentException">StartNode and EndNode must be different.</exception>
/// <param name="Start">The node from which the arc starts.</param>
/// <param name="End">The node to which the arc ends.</param>
public SimRoute(SimWaypoint Start, SimWaypoint End)
{
StartNode = Start;
EndNode = End;
Weight = 1;
LengthUpdated = false;
_Passable = true;
}
public virtual SimRoute AppendPoint(SimWaypoint vector3, double fudge)
{
if (EndNode.Distance(vector3) < fudge)
{
return(new SimRoute(StartNode, vector3));
}
return(Append(new SimRoute(EndNode, vector3)));
}
/// <summary>
/// Arc constructor.
/// </summary>
/// <exception cref="ArgumentNullException">Extremity nodes cannot be null.</exception>
/// <exception cref="ArgumentException">StartNode and EndNode must be different.</exception>
/// <param name="Start">The node from which the arc starts.</param>
/// <param name="End">The node to which the arc ends.</param>
public SimRoute(SimWaypoint Start, SimWaypoint End)
{
StartNode = Start;
EndNode = End;
Weight = 1;
LengthUpdated = false;
_Passable = true;
}
/// <summary>
/// Directly Adds a node to the graph.
/// </summary>
/// <param name="NewNode">The node to add.</param>
/// <returns>'true' if it has actually been added / 'false' if the node is null or if it is already in the graph.</returns>
public bool AddNode(SimWaypoint NewNode)
{
if (NewNode == null || SimWaypoints.Contains(NewNode))
{
return(false);
}
SimWaypointsAdd(NewNode);
return(true);
}
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);
}
private SimWaypoint[] GoBackUpNodes(Track T)
{
int Nb = T.NbArcsVisited;
SimWaypoint[] Path = new SimWaypoint[Nb + 1];
for (int i = Nb; i >= 0; i--, T = T.Queue)
{
Path[i] = T.EndNode;
}
return(Path);
}
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;
}
/// <summary>
/// Searches for the best path to reach the specified EndNode from the specified StartNode.
/// </summary>
/// <exception cref="ArgumentNullException">StartNode and EndNode cannot be null.</exception>
/// <param name="StartNode">The node from which the path must start.</param>
/// <param name="EndNode">The node to which the path must end.</param>
/// <returns>'true' if succeeded / 'false' if failed.</returns>
public bool SearchPath(SimWaypoint StartNode, SimWaypoint EndNode)
{
lock (_Graph)
{
Initialize(StartNode, EndNode);
while (NextStep())
{
}
return(PathFound);
}
}
public virtual SimRoute GetSegment(double start, double distlen, SimPathStore PathStore)
{
IList <SimRoute> newmoves = new List <SimRoute>();
double len = Length;
if (distlen <= 0.0 || start + distlen > len)
{
distlen = len - start;
}
SimWaypoint First = StartNode;
SimWaypoint Last = EndNode;
foreach (SimRoute move in GetSegments())
{
double mlen = move.Length;
if (mlen > start)
{
First = move.GetPointAt(start, PathStore);
if (distlen + start < mlen)
{
Last = move.GetPointAt(distlen + start, PathStore);
newmoves.Add(new SimRoute(First, EndNode));
break; // start and end in a single segment
}
Last = move.EndNode;
newmoves.Add(new SimRoute(First, EndNode));
distlen -= (mlen - start);
start = 0.0f;
continue; // have a start but need distlen more
}
if (start > 0)
{
start -= mlen;
continue; // still scanning for start
}
else
{
if (distlen > mlen)
{
distlen -= mlen;
newmoves.Add(move);
continue; // add whole segment and get distlen more
}
else
{
First = move.StartNode;
EndNode = move.GetPointAt(mlen, PathStore);
newmoves.Add(new SimRoute(First, EndNode));
break; // this completed it
}
}
}
return(new SimRouteMulti(newmoves));
}
public Track(SimWaypoint GraphNode)
{
if (_Target == null)
{
throw new InvalidOperationException("You must specify a target Node for the Track class.");
}
_Cost = 0;
_NbArcsVisited = 0;
Queue = null;
EndNode = GraphNode;
}
/// <summary>
/// Returns the manhattan distance between two nodes : |Dx|+|Dy|+|Dz|
/// </summary>
/// <exception cref="ArgumentNullException">Argument nodes must not be null.</exception>
/// <param name="N1">First node.</param>
/// <param name="N2">Second node.</param>
/// <returns>Distance value.</returns>
public static double ManhattanDistance(SimWaypoint N1, SimWaypoint N2)
{
if (N1 == null || N2 == null)
{
throw new ArgumentNullException();
}
double DX = N1.Position.X - N2.Position.X;
double DY = N1.Position.Y - N2.Position.Y;
double DZ = N1.Position.Z - N2.Position.Z;
return((double)(Math.Abs(DX) + Math.Abs(DY) + Math.Abs(DZ)));
}
/// <summary>
/// Returns the square euclidian distance between two nodes : Dx²+Dy²+Dz²
/// </summary>
/// <exception cref="ArgumentNullException">Argument nodes must not be null.</exception>
/// <param name="N1">First node.</param>
/// <param name="N2">Second node.</param>
/// <returns>Distance value.</returns>
public static double SquareEuclidianDistance(SimWaypoint N1, SimWaypoint N2)
{
if (N1 == null || N2 == null)
{
throw new ArgumentNullException();
}
double DX = N1.Position.X - N2.Position.X;
double DY = N1.Position.Y - N2.Position.Y;
double DZ = N1.Position.Z - N2.Position.Z;
return((double)(DX * DX + DY * DY + DZ * DZ));
}
//public void FromFileString(String s)
//{
// string[] args = s.Split(null);
// StartNode = SimWaypoint.Create(new Vector3d(double.Parse(args[0]), double.Parse(args[1]), double.Parse(args[2])));
// EndNode = SimWaypoint.Create(new Vector3d(double.Parse(args[4]), double.Parse(args[5]), double.Parse(args[6])));
// // if (s.Contains("MustAutoPilot")) MustAutoPilot = true;
// if (s.Contains("MustFly")) MustFly = true;
// if (s.Contains("MustCrouch")) MustCrouch = true;
// if (s.Contains("IsBlocked")) IsBlocked = true;
// // if (s.Contains("IsOneDirrection")) IsOneDirrection = true;
//}
public virtual bool NearPoint(SimWaypoint e, double maxDist)
{
if (StartNode.Distance(e) < maxDist)
{
return(true);
}
if (EndNode.Distance(e) < maxDist)
{
return(true);
}
return(false);
}
/// <summary>
/// Returns the maximum distance between two nodes : Max(|Dx|, |Dy|, |Dz|)
/// </summary>
/// <exception cref="ArgumentNullException">Argument nodes must not be null.</exception>
/// <param name="N1">First node.</param>
/// <param name="N2">Second node.</param>
/// <returns>Distance value.</returns>
public static double MaxDistanceAlongAxis(SimWaypoint N1, SimWaypoint N2)
{
if (N1 == null || N2 == null)
{
throw new ArgumentNullException();
}
double DX = Math.Abs(N1.Position.X - N2.Position.X);
double DY = Math.Abs(N1.Position.Y - N2.Position.Y);
double DZ = Math.Abs(N1.Position.Z - N2.Position.Z);
return((double)Math.Max(DX, Math.Max(DY, DZ)));
}
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);
}
/// <summary>
/// Use for a 'step by step' search only. This method is alternate to SearchPath.
/// Initializes AStar before performing search steps manually with NextStep.
/// </summary>
/// <exception cref="ArgumentNullException">StartNode and EndNode cannot be null.</exception>
/// <param name="StartNode">The node from which the path must start.</param>
/// <param name="EndNode">The node to which the path must end.</param>
public void Initialize(SimWaypoint StartNode, SimWaypoint EndNode)
{
if (StartNode == null || EndNode == null)
{
throw new ArgumentNullException();
}
_Closed.Clear();
_Open.Clear();
Track.Target = EndNode;
_Open.Add(new Track(StartNode));
_NbIterations = 0;
_LeafToGoBackUp = null;
}
private SimWaypoint CreateXYZ(int x, int y)
{
int ix = IndexX(x);
int iy = IndexY(y);
if (saved[ix, iy] == null)
{
saved[ix, iy] = SimWaypointImpl.CreateGlobal(x, y, SimZ);
}
SimWaypoint wp = saved[ix, iy];
AddNode(wp);
return(wp);
}
public bool GoesTo(SimWaypoint e)
{
if (_OutgoingArcs != null)
{
foreach (SimRoute r in _OutgoingArcs)
{
if (r.EndNode == e)
{
return(true);
}
}
}
return(false);
}
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 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)));
}
/// <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 override CmdResult ExecuteRequest(CmdRequest args)
{
SimPosition pos;
if (!args.TryGetValue("pos", out pos))
{
pos = TheSimAvatar;
}
SimPathStore R = pos.PathStore;
Vector3 v3 = pos.SimPosition;
WriteLine("SimZInfo: " + pos + " " + R.GetGroundLevel(v3.X, v3.Y));
SimWaypoint WP = R.GetWaypointOf(v3);
WriteLine("WaypointInfo: {0}", WP.OccupiedString(R.GetCollisionPlane(v3.Z)));
return(Success("Ran " + Name));
}
/// <summary>
///
/// </summary>
/// <param name="v3"></param>
/// <returns></returns>
public SimWaypoint CreateClosestWaypoint(Vector3d v3)
{
double Dist;
SimWaypoint Closest = ClosestNode(v3.X, v3.Y, v3.Z, out Dist, false);
SimWaypoint V3Waypoint = SimWaypointImpl.CreateGlobal(v3);
if (Closest != V3Waypoint)
{
IList <SimWaypoint> more = ClosestNodes(V3Waypoint, Dist, Dist * 2, false);
AddNode(V3Waypoint);
Intern2Arc(Closest, V3Waypoint, 1f);
foreach (SimWaypoint P in more)
{
Intern2Arc(P, V3Waypoint, 1f);
}
}
return(V3Waypoint);
}
/// <summary>
/// Returns the arc that arc that comes to this from the specified node if it exists.
/// </summary>
/// <exception cref="ArgumentNullException">Argument node must not be null.</exception>
/// <param name="N">A node that could reach this one.</param>
/// <returns>The arc coming to this from N / null if there is no solution.</returns>
public SimRoute ArcComingFrom(SimWaypoint N)
{
if (N == null)
{
throw new ArgumentNullException();
}
if (_IncomingArcs != null)
{
foreach (SimRoute A in _IncomingArcs)
{
if (A.StartNode == N)
{
return(A);
}
}
}
return(null);
}
/// <summary>
/// Returns the arc that leads to the specified node if it exists.
/// </summary>
/// <exception cref="ArgumentNullException">Argument node must not be null.</exception>
/// <param name="N">A node that could be reached from this one.</param>
/// <returns>The arc leading to N from this / null if there is no solution.</returns>
public SimRoute ArcGoingTo(SimWaypoint N)
{
if (N == null)
{
throw new ArgumentNullException();
}
if (_OutgoingArcs != null)
{
foreach (SimRoute A in _OutgoingArcs)
{
if (A.EndNode == N)
{
return(A);
}
}
}
return(null);
}
/// <summary>
/// This function will find the nodes from a geographical position in space.
/// </summary>
/// <param name="P">Waypoint node.</param>
/// <param name="MinDistance">The min distance to the node.</param>
/// <param name="MaxDistance">The max distance to the node.</param>
/// <param name="IgnorePassableProperty">if 'false', then nodes whose property Passable is set to false will not be taken into account.</param>
/// <returns>The nodes that has been found.</returns>
public IList <SimWaypoint> ClosestNodes(SimWaypoint P, double DistanceMin, double DistanceMax, bool IgnorePassableProperty)
{
List <SimWaypoint> waypoints = new List <SimWaypoint>();
lock (SimWaypoints) foreach (SimWaypoint N in SimWaypoints)
{
if (IgnorePassableProperty && N.IsPassable == false)
{
continue;
}
double Distance = N.Distance(P);
if (Distance < DistanceMin || DistanceMax < Distance)
{
continue;
}
waypoints.Add(N);
}
return(waypoints);
}
public override bool NearPoint(SimWaypoint e, double maxDist)
{
if (StartNode.Distance(e) < maxDist)
{
return(true);
}
if (EndNode.Distance(e) < maxDist)
{
return(true);
}
foreach (SimRoute move in MoveList)
{
if (move.NearPoint(e, maxDist))
{
return(true);
}
}
return(false);
}
public IList <SimRoute> GetRouteList(SimWaypoint to, out bool IsFake)
{
SimWaypoint from = GetWaypoint();
//IList<SimRoute> route = PathStore.GetRoute(from, to, out IsFake);
//if (false)
//{
// ////pathByNodes
// //if (GraphFormer.DEBUGGER != null)
// //{
// // new Thread(new ThreadStart(delegate()
// // {
// // //GraphFormer.DEBUGGER.Invalidate();
// // // GraphFormer.DEBUGGER.SetTryPathNow(from, to, pathByNodes);
// // })).Start();
// //}
//}
// TODO return PathStore.GetRoute(from, to, out IsFake);
throw new Exception("The method or operation is not implemented.");
}
/// <summary>
/// This function will find the closest node from a geographical position in space.
/// </summary>
/// <param name="PtX">X coordinate of the point from which you want the closest node.</param>
/// <param name="PtY">Y coordinate of the point from which you want the closest node.</param>
/// <param name="PtZ">Z coordinate of the point from which you want the closest node.</param>
/// <param name="Distance">The distance to the closest node.</param>
/// <param name="IgnorePassableProperty">if 'false', then nodes whose property Passable is set to false will not be taken into account.</param>
/// <returns>The closest node that has been found.</returns>
public SimWaypoint ClosestNode(double PtX, double PtY, double PtZ, out double Distance, bool IgnorePassableProperty)
{
SimWaypoint NodeMin = null;
double DistanceMin = -1;
Vector3d P = new Vector3d(PtX, PtY, PtZ);
lock (SimWaypoints) foreach (SimWaypoint N in SimWaypoints)
{
if (IgnorePassableProperty && N.IsPassable == false)
{
continue;
}
double DistanceTemp = Vector3d.Distance(N.GlobalPosition, P);
if (DistanceMin == -1 || DistanceMin > DistanceTemp)
{
DistanceMin = DistanceTemp;
NodeMin = N;
}
}
Distance = DistanceMin;
return(NodeMin);
}
public override SimRoute FillIn(double maxDist)
{
IList <SimRoute> moves = new List <SimRoute>();
SimWaypoint at = StartNode;
bool filled = false;
foreach (SimRoute move in GetSegments())
{
if (at.Distance(move.StartNode) > maxDist)
{
moves.Add(new SimRoute(at, move.StartNode));
filled = true;
}
moves.Add(move);
at = move.EndNode;
}
if (filled)
{
return(new SimRouteMulti(moves));
}
return(this);
}
/// <summary>
/// Returns the bounding box that wraps the specified list of nodes.
/// </summary>
/// <exception cref="ArgumentException">The list must only contain elements of type Node.</exception>
/// <exception cref="ArgumentException">The list of nodes is empty.</exception>
/// <param name="NodesGroup">The list of nodes to wrap.</param>
/// <param name="MinPoint">The point of minimal coordinates for the box.</param>
/// <param name="MaxPoint">The point of maximal coordinates for the box.</param>
static public void BoundingBox(ICollection <SimWaypoint> NodesGroup, out double[] MinPoint, out double[] MaxPoint)
{
IEnumerator E = NodesGroup.GetEnumerator();
E.MoveNext();
SimWaypoint N1 = E.Current as SimWaypoint;
if (N1 == null)
{
throw new ArgumentException("The list must only contain elements of type Node.");
}
if (NodesGroup.Count == 0)
{
throw new ArgumentException("The list of nodes is empty.");
}
int Dim = 3;
MinPoint = new double[Dim];
MaxPoint = new double[Dim];
for (int i = 0; i < Dim; i++)
{
MinPoint[i] = MaxPoint[i] = N1.GlobalXYZ(i);
}
foreach (SimWaypoint N in NodesGroup)
{
for (int i = 0; i < Dim; i++)
{
if (MinPoint[i] > N.GlobalXYZ(i))
{
MinPoint[i] = N.GlobalXYZ(i);
}
if (MaxPoint[i] < N.GlobalXYZ(i))
{
MaxPoint[i] = N.GlobalXYZ(i);
}
}
}
}
static private void DessinerNumero(Graphics Grfx, SimWaypoint N1, SimWaypoint N2, int i)
{
StringFormat F = new StringFormat();
F.Alignment = StringAlignment.Center;
F.LineAlignment = StringAlignment.Center;
Rectangle R = RectangleCentres(N1, N2);
Font Police = DefaultFont;
int LargeurMin = (int)Police.GetHeight();
R.Inflate(LargeurMin, LargeurMin);
Grfx.DrawString(i.ToString(), Police, Brushes.Black, R, F);
}
public Track(SimWaypoint GraphNode)
{
if (_Target == null) throw new InvalidOperationException("You must specify a target Node for the Track class.");
_Cost = 0;
_NbArcsVisited = 0;
Queue = null;
EndNode = GraphNode;
}
/// <summary>
/// Use for a 'step by step' search only. This method is alternate to SearchPath.
/// Initializes AStar before performing search steps manually with NextStep.
/// </summary>
/// <exception cref="ArgumentNullException">StartNode and EndNode cannot be null.</exception>
/// <param name="StartNode">The node from which the path must start.</param>
/// <param name="EndNode">The node to which the path must end.</param>
public void Initialize(SimWaypoint StartNode, SimWaypoint EndNode)
{
if (StartNode == null || EndNode == null) throw new ArgumentNullException();
_Closed.Clear();
_Open.Clear();
Track.Target = EndNode;
_Open.Add(new Track(StartNode));
_NbIterations = 0;
_LeafToGoBackUp = null;
}
/// <summary>
/// This function will find the nodes from a geographical position in space.
/// </summary>
/// <param name="P">Waypoint node.</param>
/// <param name="MinDistance">The min distance to the node.</param>
/// <param name="MaxDistance">The max distance to the node.</param>
/// <param name="IgnorePassableProperty">if 'false', then nodes whose property Passable is set to false will not be taken into account.</param>
/// <returns>The nodes that has been found.</returns>
public IList<SimWaypoint> ClosestNodes(SimWaypoint P, double DistanceMin, double DistanceMax, bool IgnorePassableProperty)
{
List<SimWaypoint> waypoints = new List<SimWaypoint>();
lock (SimWaypoints) foreach (SimWaypoint N in SimWaypoints)
{
if (IgnorePassableProperty && N.IsPassable == false) continue;
double Distance = N.Distance(P);
if (Distance < DistanceMin || DistanceMax < Distance) continue;
waypoints.Add(N);
}
return waypoints;
}
/// <summary>
/// Adds the two opposite arcs between both specified nodes 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="Node1"></param>
/// <param name="Node2"></param>
/// <param name="Weight"></param>
public void Add2Arcs(SimWaypoint Node1, SimWaypoint Node2, double Weight)
{
AddArc(Node1, Node2, Weight);
AddArc(Node2, Node1, Weight);
}
private void SimWaypointsAdd(SimWaypoint NewNode)
{
lock (SimWaypoints) SimWaypoints.Add(NewNode);
}
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;
}
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 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 void SetTryPathNow(SimWaypoint start, SimWaypoint end)
{
NDepart = start;
NArrivee = end;
// CalculPossible = true;
// AEtoile_Debut();
// AEtoile_Fin();
Chemin = AE.SearchPath(NDepart, NArrivee) ? AE.PathByNodes : null;
GraphPanel.Invalidate();
}
static private void DessinerNoeudMoitie(Graphics Grfx, Brush B, SimWaypoint N)
{
if (N == null) return;
Rectangle R = new Rectangle((int)N.DX - Rayon, (int)N.DY - Rayon, 2 * Rayon + 1, 2 * Rayon + 1);
Grfx.FillPie(B, R, 0, 180);
}
private void AddNewArcs(SimWaypoint s, SimWaypoint e, double W)
{
InternArc(s, e, W);
InternArc(e, s, W);
}
static private void DessinerDrapeau(Graphics Grfx, SimWaypoint N, int Numero)
{
if (N == null) return;
Point[] Pts = new Point[5];
double AnglePortion = (double)(2 * Math.PI) / Pts.Length;
for (int i = 0; i < Pts.Length; i++)
{
double Angle = 2 * i * AnglePortion;
if (Numero == 1) Angle += AnglePortion / 2;
Pts[i] = new Point(1 + (int)(N.DX + (Rayon + 1) * Math.Cos(Angle)), 1 + (int)(N.DY + (Rayon + 1) * Math.Sin(Angle)));
}
GraphicsPath GP = new GraphicsPath();
GP.AddLines(Pts);
GP.FillMode = FillMode.Winding;
Grfx.FillPath(Numero == 1 ? Brushes.DarkTurquoise : Brushes.Blue, GP);
}
public SimRoute Intern2Arc(SimWaypoint s, SimWaypoint e, double W)
{
InternArc(e, s, W);
CollisionPlane.Debug("Intern2Arc: " + s + " <-> " + e);
return InternArc(s, e, W);
}
static private void DessinerChemin(Graphics Grfx, Pen P, SimWaypoint[] C)
{
Point[] Pnts = new Point[C.Length];
if (Pnts.Length > 1)
{
for (int i = 0; i < Pnts.Length; i++)
{
Pnts[i].X = (int)C[i].DX;
Pnts[i].Y = (int)C[i].DY;
}
Grfx.DrawCurve(P, Pnts);
}
}
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();
}
static bool Collision(SimWaypoint N1, SimWaypoint N2)
{
return SimWaypointImpl.SquareEuclidianDistance(N1, N2) <= Rayon * Rayon;
}
/// <summary>
/// Directly Adds a node to the graph.
/// </summary>
/// <param name="NewNode">The node to add.</param>
/// <returns>'true' if it has actually been added / 'false' if the node is null or if it is already in the graph.</returns>
public bool AddNode(SimWaypoint NewNode)
{
if (NewNode == null || SimWaypoints.Contains(NewNode)) return false;
SimWaypointsAdd(NewNode);
return true;
}
bool NoeudSelonPosition(int X, int Y, ref SimWaypoint N)
{
SimWaypoint NSJ = NoeudSousJacent(X, Y);
if (NSJ != null)
{
N = NSJ;
return false;
}
else
{
if (N == null) N = SimWaypointImpl.CreateGlobal(X / DSCALE + StartX, Y / DSCALE + StartY, 0);
else N.ChangeXYZDebug(X / DSCALE + StartX, Y / DSCALE + StartY, 0);
return true;
}
}
/// <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);
}
private void GraphPanel_MouseDown(object sender, System.Windows.Forms.MouseEventArgs e)
{
if (e.Button == MouseButtons.Left) BoutonGEnfonce = true;
else if (e.Button == MouseButtons.Right) BoutonDEnfonce = true;
else return;
if (BoutonGEnfonce && BoutonDEnfonce) return;
TempN1 = TempN2 = null;
switch (Mode)
{
case Action.Effacer:
case Action.Dessiner:
{
AjouterN1 = NoeudSelonPosition(e.X, e.Y, ref TempN1);
TempN2 = SimWaypointImpl.CreateGlobal((double)((TempN1.DX) / DSCALE) + StartX, (double)((TempN1.DY) / DSCALE) + StartY, (double)0);
GraphPanel.Invalidate(Boite(TempN1, TempN2));
break;
}
case Action.Deplacer:
{
TempP = new Point(e.X, e.Y);
TempN1 = NoeudSousJacent(e.X, e.Y);
break;
}
default: break;
}
}
/// <summary>
/// Removes a node from the graph as well as the linked arcs.
/// </summary>
/// <param name="NodeToRemove">The node to remove.</param>
/// <returns>'true' if succeeded / 'false' otherwise.</returns>
public bool RemoveNode(SimWaypoint NodeToRemove)
{
if (NodeToRemove == null) return false;
try
{
foreach (SimRoute A in NodeToRemove.IncomingArcs)
{
lock (A.StartNode.OutgoingArcs) A.StartNode.OutgoingArcs.Remove(A);
lock (SimRoutes) SimRoutes.Remove(A);
}
foreach (SimRoute A in NodeToRemove.OutgoingArcs)
{
lock (A.EndNode.IncomingArcs) A.EndNode.IncomingArcs.Remove(A);
lock (SimRoutes) SimRoutes.Remove(A);
}
lock (SimWaypoints) SimWaypoints.Remove(NodeToRemove);
}
catch { return false; }
return true;
}
private void GraphPanel_MouseMove(object sender, System.Windows.Forms.MouseEventArgs e)
{
if (BoutonGEnfonce && BoutonDEnfonce) return;
switch (Mode)
{
case Action.Effacer:
case Action.Dessiner:
{
if (e.Button == MouseButtons.Left || e.Button == MouseButtons.Right)
{
if (TempN1 == null || TempN2 == null) return;
Rectangle AncienRect = Boite(TempN1, TempN2);
TempN2.ChangeXYZDebug(e.X / DSCALE + StartX, e.Y / DSCALE + StartY, 0);
Rectangle NouveauRect = Boite(TempN1, TempN2);
GraphPanel.Invalidate(Rectangle.Union(AncienRect, NouveauRect));
}
break;
}
case Action.Deplacer:
{
if (e.Button == MouseButtons.Left)
{
if (TempN1 == null) break;
Rectangle AncienRect = Boite(TempN1.Molecule);
SimWaypoint[] AncienChemin = null;
if (Chemin != null)
{
AncienChemin = new SimWaypoint[Chemin.Length];
for (int i = 0; i < AncienChemin.Length; i++) AncienChemin[i] = (SimWaypoint)Chemin[i].Clone();
}
TempN1.ChangeXYZDebug(e.X / DSCALE + StartX, e.Y / DSCALE + StartY, 0);
Rectangle NouveauRect = Boite(TempN1.Molecule);
if (AdapterAEtoile() && CheminsDifferents(AncienChemin, Chemin)) GraphPanel.Invalidate();
else GraphPanel.Invalidate(Rectangle.Union(AncienRect, NouveauRect));
}
else if (e.Button == MouseButtons.Right)
{
int DX = (int)(e.X - TempP.X);
int DY = (int)(e.Y - TempP.Y);
TempP.X = e.X;
TempP.Y = e.Y;
foreach (SimWaypoint N in G.Nodes) N.ChangeXYZDebug((double)(N.DX + DX) / DSCALE + StartX, (double)(N.DY + DY) / DSCALE + StartY, 0);
GraphPanel.Invalidate();
}
break;
}
default: break;
}
if (!CalculPossible) StatusBarMouseMove(e);
}
/// <summary>
/// Searches for the best path to reach the specified EndNode from the specified StartNode.
/// </summary>
/// <exception cref="ArgumentNullException">StartNode and EndNode cannot be null.</exception>
/// <param name="StartNode">The node from which the path must start.</param>
/// <param name="EndNode">The node to which the path must end.</param>
/// <returns>'true' if succeeded / 'false' if failed.</returns>
public bool SearchPath(SimWaypoint StartNode, SimWaypoint EndNode)
{
lock (_Graph)
{
Initialize(StartNode, EndNode);
while (NextStep()) { }
return PathFound;
}
}
private void GraphPanel_MouseUp(object sender, System.Windows.Forms.MouseEventArgs e)
{
bool Retour = BoutonGEnfonce && BoutonDEnfonce;
if (e.Button == MouseButtons.Left) BoutonGEnfonce = false;
else if (e.Button == MouseButtons.Right) BoutonDEnfonce = false;
else return;
if (Retour) return;
switch (Mode)
{
case Action.Dessiner:
{
if (TempN1 == null || TempN2 == null) return;
bool AjouterArc = false;
Rectangle AncienRect = Boite(TempN1, TempN2);
AjouterN2 = NoeudSelonPosition(e.X, e.Y, ref TempN2);
if (AjouterN1) { TempN1.Isolate(); G.AddNode(TempN1); }
if (!Collision(TempN1, TempN2))
{
if (AjouterN2) { TempN2.Isolate(); G.AddNode(TempN2); }
if (e.Button == MouseButtons.Left)
G.AddArc(TempN1, TempN2, 1);
else if (e.Button == MouseButtons.Right) G.Add2Arcs(TempN1, TempN2, 1);
NbArcsPanel.Text = G.Arcs.Count.ToString();
AjouterArc = true;
}
NbNodesPanel.Text = G.Nodes.Count.ToString();
if (AjouterArc && (!AjouterN1 || !AjouterN2) && AdapterAEtoile()) GraphPanel.Invalidate();
else
{
Rectangle NouveauRect = Boite(TempN1, TempN2);
GraphPanel.Invalidate(Rectangle.Union(AncienRect, NouveauRect));
}
break;
}
case Action.Effacer:
{
if (TempN1 == null || TempN2 == null) return;
bool Selection = false;
Rectangle Zone = RectangleCentres(TempN1, TempN2);
Zone.Inflate(1, 1);
Region Invalide = new Region(Zone);
if (Zone.Size.Width < 2 * Rayon && Zone.Size.Height < 2 * Rayon)
{
SimWaypoint N = NoeudSousJacent(e.X, e.Y);
if (N != null)
{
TestAEtoile(N, ref Invalide);
Invalide.Union(Boite(N.Molecule));
G.RemoveNode(N);
NbNodesPanel.Text = G.Nodes.Count.ToString();
NbArcsPanel.Text = G.Arcs.Count.ToString();
Selection = true;
}
else
{
SimRoute A = ArcSousJacent(e.X, e.Y);
if (A != null)
{
Invalide.Union(Boite(A.StartNode, A.EndNode));
G.RemoveArc(A);
NbArcsPanel.Text = G.Arcs.Count.ToString();
Selection = true;
}
}
}
else
{
ArrayList ListeNoeuds = new ArrayList();
foreach (SimWaypoint N in G.Nodes)
{
if (Zone.Contains(new Point((int)N.DX, (int)N.DY)))
{
TestAEtoile(N, ref Invalide);
Invalide.Union(Boite(N.Molecule));
ListeNoeuds.Add(N);
Selection = true;
}
}
foreach (SimWaypoint N in ListeNoeuds) G.RemoveNode(N);
NbNodesPanel.Text = G.Nodes.Count.ToString();
NbArcsPanel.Text = G.Arcs.Count.ToString();
}
if (Selection && AdapterAEtoile()) GraphPanel.Invalidate();
else GraphPanel.Invalidate(Invalide);
break;
}
case Action.ChangerEtat:
{
SimWaypoint N = NoeudSousJacent(e.X, e.Y);
Region Invalide = null;
if (N != null)
{
N.IsPassable = !N.IsPassable;
Invalide = new Region(Boite(N.Molecule));
}
else
{
SimRoute A = ArcSousJacent(e.X, e.Y);
if (A != null)
{
A.Passable = !A.Passable;
Invalide = new Region(Boite(A.StartNode, A.EndNode));
}
}
if (Invalide != null)
{
if (AdapterAEtoile()) GraphPanel.Invalidate();
else GraphPanel.Invalidate(Invalide);
}
break;
}
case Action.AEtoile:
{
double Distance;
SimWaypoint NoeudPlusProche = G.ClosestNode(e.X / DSCALE + StartX, e.Y / DSCALE + StartY, 0, out Distance, true);
if (NoeudPlusProche == null) break;
Rectangle Invalide = Boite(NoeudPlusProche);
if (NDepart != null) Invalide = Rectangle.Union(Invalide, Boite(NDepart));
if (NArrivee != null) Invalide = Rectangle.Union(Invalide, Boite(NArrivee));
if (e.Button == MouseButtons.Left) NDepart = NDepart == NoeudPlusProche ? null : NoeudPlusProche;
else if (e.Button == MouseButtons.Right) NArrivee = NArrivee == NoeudPlusProche ? null : NoeudPlusProche;
if (AdapterAEtoile()) GraphPanel.Invalidate();
else
{
if (Chemin != null)
{
Chemin = null;
GraphPanel.Invalidate();
}
else GraphPanel.Invalidate(Invalide);
}
break;
}
default: break;
}
TempN1 = TempN2 = null;
}
private SimWaypoint[] GoBackUpNodes(Track T)
{
int Nb = T.NbArcsVisited;
SimWaypoint[] Path = new SimWaypoint[Nb + 1];
for (int i = Nb; i >= 0; i--, T = T.Queue)
Path[i] = T.EndNode;
return Path;
}
bool CheminsDifferents(SimWaypoint[] C1, SimWaypoint[] C2)
{
if (C1 == null || C2 == null || C1.Length != C2.Length) return true;
for (int i = 0; i < C1.Length; i++) if (!C1[i].Equals(C2[i])) return true;
return false;
}
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;
}
void TestAEtoile(SimWaypoint N, ref Region ZoneInvalide)
{
if (N != null)
{
if (N == NDepart) NDepart = null;
if (N == NArrivee) NArrivee = null;
if (Chemin != null && Array.IndexOf(Chemin, N) >= 0)
{
Chemin = null;
ZoneInvalide.Union(new Rectangle(new Point(0, 0), GraphPanel.Size));
}
}
}
