public double TimeThroughAdjacent(NavigableEdge edge)
{
return edge.TimeCost();
}
public double TimeThroughAdjacent(NavigableEdge edge)
{
return exit.TimeThroughAdjacent(edge);
}
public void Reset(bool resetPrevious)
{
this.WrappingHelpers = new List<Coordinates>();
this.PreviousNode = null;
if (resetPrevious)
this.zt.Mode = ZoneToolboxMode.None;
this.rightClickNode = null;
this.rightClickEdge = null;
if (this.moveNode != null)
this.moveNode.Position = this.moveOriginalCoords;
this.moveNode = null;
}
public void RightClick(Coordinates c)
{
if (this.Mode == ZoneToolboxMode.NavNodes && this.zt.current != null)
{
// nullify
this.Reset(false);
// determine what we selected
NavigableEdge ne;
INavigableNode nn;
this.NavigationHitTest(c, out nn, out ne);
if (nn != null)
{
this.rightClickNode = nn;
}
else if (ne != null)
{
this.rightClickEdge = ne;
}
}
}
public void Click(Coordinates c)
{
if (this.zt.Mode == ZoneToolboxMode.Selection)
{
this.zt.SelectZone(c);
}
else if (this.Mode == ZoneToolboxMode.NavNodes && this.zt.current != null)
{
// save undo point
this.ed.SaveUndoPoint();
// check if we hit any of hte edges or nodes part of the zone
ArbiterZone az = this.zt.current;
// check if hit node or edge
NavigableEdge ne;
INavigableNode nn;
this.NavigationHitTest(c, out nn, out ne);
if (nn != null)
{
// create new node
INavigableNode aznn = nn;
if (this.PreviousNode != null)
{
// create new edges
NavigableEdge newE1 = new NavigableEdge(true, this.zt.current, false, null, new List<IConnectAreaWaypoints>(), this.PreviousNode, aznn);
this.PreviousNode.OutgoingConnections.Add(newE1);
this.zt.current.NavigableEdges.Add(newE1);
}
this.PreviousNode = aznn;
}
else if (ne != null)
{
// remove old
this.zt.current.NavigableEdges.Remove(ne);
// remove all references
ne.Start.OutgoingConnections.Remove(ne);
// create new node
ArbiterZoneNavigableNode aznn = new ArbiterZoneNavigableNode(c);
// create new edges
NavigableEdge newE1 = new NavigableEdge(true, this.zt.current, false, null, new List<IConnectAreaWaypoints>(), ne.Start, aznn);
NavigableEdge newE2 = new NavigableEdge(true, this.zt.current, false, null, new List<IConnectAreaWaypoints>(), aznn, ne.End);
// add edges
ne.Start.OutgoingConnections.Add(newE1);
aznn.OutgoingConnections.Add(newE2);
// add all to lists
this.zt.current.NavigableEdges.Add(newE1);
this.zt.current.NavigableEdges.Add(newE2);
this.zt.current.NavigationNodes.Add(aznn);
if (this.PreviousNode != null)
{
NavigableEdge newE3 = new NavigableEdge(true, this.zt.current, false, null, new List<IConnectAreaWaypoints>(), this.PreviousNode, aznn);
this.PreviousNode.OutgoingConnections.Add(newE3);
this.zt.current.NavigableEdges.Add(newE3);
}
this.PreviousNode = aznn;
}
else
{
// create new node
ArbiterZoneNavigableNode aznn = new ArbiterZoneNavigableNode(c);
if (this.PreviousNode != null)
{
// create new edges
NavigableEdge newE1 = new NavigableEdge(true, this.zt.current, false, null, new List<IConnectAreaWaypoints>(), this.PreviousNode, aznn);
this.PreviousNode.OutgoingConnections.Add(newE1);
this.zt.current.NavigableEdges.Add(newE1);
}
this.PreviousNode = aznn;
this.zt.current.NavigationNodes.Add(aznn);
}
}
else if (this.zt.Mode == ZoneToolboxMode.StayOut && this.zt.current != null)
{
if (this.WrappingHelpers.Count == 0)
{
this.WrappingHelpers.Add(c);
}
else
{
// Determine size of bounding box
float scaled_offset = 1 / this.rd.WorldTransform.Scale;
// invert the scale
float scaled_size = DrawingUtility.cp_large_size;
// assume that the world transform is currently applied correctly to the graphics
RectangleF rect = new RectangleF((float)c.X - scaled_size / 2, (float)c.Y - scaled_size / 2, scaled_size, scaled_size);
if (rect.Contains(DrawingUtility.ToPointF(this.WrappingHelpers[0])) &&
c.DistanceTo(this.WrappingHelpers[0]) < 1)
{
ed.SaveUndoPoint();
Polygon p = new Polygon(this.WrappingHelpers);
this.zt.current.StayOutAreas.Add(p);
this.WrappingHelpers = new List<Coordinates>();
}
else
this.WrappingHelpers.Add(c);
}
this.rd.Invalidate();
}
}
public void NavigationHitTest(Coordinates c, out INavigableNode node, out NavigableEdge edge)
{
if (this.zt.current != null)
{
// Determine size of bounding box
float scaled_offset = 1 / this.rd.WorldTransform.Scale;
// invert the scale
float scaled_size = DrawingUtility.cp_large_size;
// assume that the world transform is currently applied correctly to the graphics
RectangleF rect = new RectangleF((float)c.X - scaled_size / 2, (float)c.Y - scaled_size / 2, scaled_size, scaled_size);
foreach (ArbiterParkingSpotWaypoint apsw in this.zt.current.ParkingSpotWaypoints.Values)
{
if (apsw.Position.DistanceTo(c) < 1.0)
{
node = apsw;
edge = null;
return;
}
}
foreach (ArbiterPerimeterWaypoint apw in this.zt.current.Perimeter.PerimeterPoints.Values)
{
if ((apw.IsExit || apw.IsEntry) && rect.Contains(DrawingUtility.ToPointF(apw.Position)) &&
(this.PreviousNode == null || !this.PreviousNode.Equals(apw)) &&
apw.Position.DistanceTo(c) < 1.0)
{
node = apw;
edge = null;
return;
}
}
foreach (INavigableNode inn in this.zt.current.NavigationNodes)
{
if (rect.Contains(DrawingUtility.ToPointF(inn.Position)) &&
(this.PreviousNode == null || !this.PreviousNode.Equals(inn)) &&
inn.Position.DistanceTo(c) < 1.0)
{
node = inn;
edge = null;
return;
}
}
NavigableEdge closest = null;
double distance = double.MaxValue;
foreach (NavigableEdge ne in this.zt.current.NavigableEdges)
{
LinePath lp = new LinePath(new Coordinates[] { ne.Start.Position, ne.End.Position });
double dist = lp.GetClosestPoint(c).Location.DistanceTo(c);
if (dist < rect.Width && (dist < distance || closest == null) && dist < 1.0)
{
closest = ne;
distance = dist;
}
}
if (closest != null)
{
node = null;
edge = closest;
return;
}
}
node = null;
edge = null;
return;
}
public ZoneNavigationEdgeSort(double distance, NavigableEdge edge, LinePath lp)
{
this.distance = distance;
this.edge = edge;
this.lp = lp;
this.zp = null;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zone"></param>
public ZoneOrientationState(ArbiterZone zone, NavigableEdge final)
: base(zone)
{
this.final = final;
}
/// <summary>
/// Plans over the zone
/// </summary>
/// <param name="planningState"></param>
/// <param name="navigationalPlan"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicles"></param>
/// <param name="obstacles"></param>
/// <param name="blockages"></param>
/// <returns></returns>
public Maneuver Plan(IState planningState, INavigationalPlan navigationalPlan,
VehicleState vehicleState, SceneEstimatorTrackedClusterCollection vehicles,
SceneEstimatorUntrackedClusterCollection obstacles, List <ITacticalBlockage> blockages)
{
#region Zone Travelling State
if (planningState is ZoneTravelingState)
{
// check blockages
/*if (blockages != null && blockages.Count > 0 && blockages[0] is ZoneBlockage)
* {
* // create the blockage state
* EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);
*
* // go to a blockage handling tactical
* return new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
* }*/
// cast state
ZoneState zs = (ZoneState)planningState;
// plan over state and zone
ZonePlan zp = (ZonePlan)navigationalPlan;
// check zone path does not exist
if (zp.RecommendedPath.Count < 2)
{
// zone startup again
ZoneStartupState zss = new ZoneStartupState(zs.Zone, true);
return(new Maneuver(new HoldBrakeBehavior(), zss, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
// final path seg
LinePath.PointOnPath endBack = zp.RecommendedPath.AdvancePoint(zp.RecommendedPath.EndPoint, -TahoeParams.VL);
LinePath lp = zp.RecommendedPath.SubPath(endBack, zp.RecommendedPath.EndPoint);
LinePath lB = lp.ShiftLateral(TahoeParams.T);
LinePath rB = lp.ShiftLateral(-TahoeParams.T);
// add to info
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound));
// get speed command
ScalarSpeedCommand sc = new ScalarSpeedCommand(2.24);
// Behavior
Behavior b = new ZoneTravelingBehavior(zp.Zone.ZoneId, zp.Zone.Perimeter.PerimeterPolygon, zp.Zone.StayOutAreas.ToArray(),
sc, zp.RecommendedPath, lB, rB);
// maneuver
return(new Maneuver(b, CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
#endregion
#region Parking State
else if (planningState is ParkingState)
{
// get state
ParkingState ps = (ParkingState)planningState;
// determine stay out areas to use
List <Polygon> stayOuts = new List <Polygon>();
foreach (Polygon p in ps.Zone.StayOutAreas)
{
if (!p.IsInside(ps.ParkingSpot.NormalWaypoint.Position) && !p.IsInside(ps.ParkingSpot.Checkpoint.Position))
{
stayOuts.Add(p);
}
}
LinePath rB = ps.ParkingSpot.GetRightBound();
LinePath lB = ps.ParkingSpot.GetLeftBound();
// add to info
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound));
// create behavior
ZoneParkingBehavior zpb = new ZoneParkingBehavior(ps.Zone.ZoneId, ps.Zone.Perimeter.PerimeterPolygon, stayOuts.ToArray(), new ScalarSpeedCommand(2.24),
ps.ParkingSpot.GetSpotPath(), lB, rB, ps.ParkingSpot.SpotId, 1.0);
// maneuver
return(new Maneuver(zpb, ps, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
#endregion
#region Pulling Out State
else if (planningState is PullingOutState)
{
// get state
PullingOutState pos = (PullingOutState)planningState;
// plan over state and zone
ZonePlan zp = (ZonePlan)navigationalPlan;
// final path seg
Coordinates endVec = zp.RecommendedPath[0] - zp.RecommendedPath[1];
Coordinates endBack = zp.RecommendedPath[0] + endVec.Normalize(TahoeParams.VL * 2.0);
LinePath rp = new LinePath(new Coordinates[] { pos.ParkingSpot.Checkpoint.Position, pos.ParkingSpot.NormalWaypoint.Position,
zp.RecommendedPath[0], endBack });
LinePath lp = new LinePath(new Coordinates[] { zp.RecommendedPath[0], endBack });
LinePath lB = lp.ShiftLateral(TahoeParams.T * 2.0);
LinePath rB = lp.ShiftLateral(-TahoeParams.T * 2.0);
// add to info
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rp, ArbiterInformationDisplayObjectType.leftBound));
// determine stay out areas to use
List <Polygon> stayOuts = new List <Polygon>();
foreach (Polygon p in pos.Zone.StayOutAreas)
{
if (!p.IsInside(pos.ParkingSpot.NormalWaypoint.Position) && !p.IsInside(pos.ParkingSpot.Checkpoint.Position))
{
stayOuts.Add(p);
}
}
// get speed command
ScalarSpeedCommand sc = new ScalarSpeedCommand(2.24);
// Behavior
Behavior b = new ZoneParkingPullOutBehavior(zp.Zone.ZoneId, zp.Zone.Perimeter.PerimeterPolygon, stayOuts.ToArray(),
sc, pos.ParkingSpot.GetSpotPath(), pos.ParkingSpot.GetLeftBound(), pos.ParkingSpot.GetRightBound(), pos.ParkingSpot.SpotId,
rp, lB, rB);
// maneuver
return(new Maneuver(b, pos, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
#endregion
#region Zone Startup State
else if (planningState is ZoneStartupState)
{
// state
ZoneStartupState zss = (ZoneStartupState)planningState;
// get the zone
ArbiterZone az = zss.Zone;
// navigational edge
INavigableNode inn = CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId];
// check over all the parking spaces
foreach (ArbiterParkingSpot aps in az.ParkingSpots)
{
// inside both parts of spot
if ((vehicleState.VehiclePolygon.IsInside(aps.NormalWaypoint.Position) && vehicleState.VehiclePolygon.IsInside(aps.Checkpoint.Position)) ||
(vehicleState.VehiclePolygon.IsInside(aps.NormalWaypoint.Position)))
{
// want to just park in it again
return(new Maneuver(new HoldBrakeBehavior(), new ParkingState(az, aps), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
}
Polygon forwardPolygon = vehicleState.ForwardPolygon;
Polygon rearPolygon = vehicleState.RearPolygon;
Navigator nav = CoreCommon.Navigation;
List <ZoneNavigationEdgeSort> forwardForward = new List <ZoneNavigationEdgeSort>();
List <ZoneNavigationEdgeSort> reverseReverse = new List <ZoneNavigationEdgeSort>();
List <ZoneNavigationEdgeSort> perpendicularPerpendicular = new List <ZoneNavigationEdgeSort>();
List <ZoneNavigationEdgeSort> allEdges = new List <ZoneNavigationEdgeSort>();
List <ZoneNavigationEdgeSort> allEdgesNoLimits = new List <ZoneNavigationEdgeSort>();
foreach (NavigableEdge ne in az.NavigableEdges)
{
if (!(ne.End is ArbiterParkingSpotWaypoint) && !(ne.Start is ArbiterParkingSpotWaypoint))
{
// get distance to edge
LinePath lp = new LinePath(new Coordinates[] { ne.Start.Position, ne.End.Position });
double distTmp = lp.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front);
// get direction along segment
DirectionAlong da = vehicleState.DirectionAlongSegment(lp);
// check dist reasonable
if (distTmp > TahoeParams.VL)
{
// zone navigation edge sort item
ZoneNavigationEdgeSort znes = new ZoneNavigationEdgeSort(distTmp, ne, lp);
// add to lists
if (da == DirectionAlong.Forwards &&
(forwardPolygon.IsInside(ne.Start.Position) || forwardPolygon.IsInside(ne.End.Position)))
{
forwardForward.Add(znes);
}
/*else if (da == DirectionAlong.Perpendicular &&
* !(forwardPolygon.IsInside(ne.Start.Position) || forwardPolygon.IsInside(ne.End.Position)) &&
* !(rearPolygon.IsInside(ne.Start.Position) || rearPolygon.IsInside(ne.End.Position)))
* perpendicularPerpendicular.Add(znes);
* else if (rearPolygon.IsInside(ne.Start.Position) || rearPolygon.IsInside(ne.End.Position))
* reverseReverse.Add(znes);*/
// add to all edges
allEdges.Add(znes);
}
}
}
// sort by distance
forwardForward.Sort();
reverseReverse.Sort();
perpendicularPerpendicular.Sort();
allEdges.Sort();
ZoneNavigationEdgeSort bestZnes = null;
for (int i = 0; i < allEdges.Count; i++)
{
// get line to initial
Line toInitial = new Line(vehicleState.Front, allEdges[i].edge.Start.Position);
Line toFinal = new Line(vehicleState.Front, allEdges[i].edge.End.Position);
bool intersects = false;
Coordinates[] interPts;
foreach (Polygon sop in az.StayOutAreas)
{
if (!intersects &&
(sop.Intersect(toInitial, out interPts) && sop.Intersect(toFinal, out interPts)))
{
intersects = true;
}
}
if (!intersects)
{
allEdges[i].zp = nav.PlanZone(az, allEdges[i].edge.End, inn);
allEdges[i].zp.Time += vehicleState.Front.DistanceTo(allEdges[i].lp.GetClosestPoint(vehicleState.Front).Location) / 2.24;
ZoneNavigationEdgeSort tmpZnes = allEdges[i];
if ((bestZnes == null && tmpZnes.zp.RecommendedPath.Count > 1) ||
(bestZnes != null && tmpZnes.zp.RecommendedPath.Count > 1 && tmpZnes.zp.Time < bestZnes.zp.Time))
{
bestZnes = tmpZnes;
}
}
if (i > allEdges.Count / 2 && bestZnes != null)
{
break;
}
}
if (bestZnes != null)
{
ArbiterOutput.Output("Found good edge to start in zone");
return(new Maneuver(new HoldBrakeBehavior(), new ZoneOrientationState(az, bestZnes.edge), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
else
{
ArbiterOutput.Output("Could not find good edge to start, choosing random not blocked");
List <ZoneNavigationEdgeSort> okZnes = new List <ZoneNavigationEdgeSort>();
foreach (NavigableEdge tmpOk in az.NavigableEdges)
{
// get line to initial
LinePath edgePath = new LinePath(new Coordinates[] { tmpOk.Start.Position, tmpOk.End.Position });
double dist = edgePath.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front);
ZoneNavigationEdgeSort tmpZnes = new ZoneNavigationEdgeSort(dist, tmpOk, edgePath);
tmpZnes.zp = nav.PlanZone(az, tmpZnes.edge.End, inn);
tmpZnes.zp.Time += vehicleState.Front.DistanceTo(tmpZnes.lp.GetClosestPoint(vehicleState.Front).Location) / 2.24;
if (tmpZnes.zp.RecommendedPath.Count >= 2)
{
okZnes.Add(tmpZnes);
}
}
if (okZnes.Count == 0)
{
okZnes = allEdges;
}
else
{
okZnes.Sort();
}
// get random close edge
Random rand = new Random();
int chosen = rand.Next(Math.Min(5, okZnes.Count));
// get closest edge not part of a parking spot, get on it
NavigableEdge closest = okZnes[chosen].edge; //null;
//double distance = Double.MaxValue;
/*foreach (NavigableEdge ne in az.NavigableEdges)
* {
* if (!(ne.End is ArbiterParkingSpotWaypoint) && !(ne.Start is ArbiterParkingSpotWaypoint))
* {
* // get distance to edge
* LinePath lp = new LinePath(new Coordinates[] { ne.Start.Position, ne.End.Position });
* double distTmp = lp.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front);
* if (closest == null || distTmp < distance)
* {
* closest = ne;
* distance = distTmp;
* }
* }
* }*/
return(new Maneuver(new HoldBrakeBehavior(), new ZoneOrientationState(az, closest), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
}
#endregion
#region Unknown
else
{
// non-handled state
throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
}
#endregion
}
public void NavigationHitTest(Coordinates c, out INavigableNode node, out NavigableEdge edge)
{
if (this.zt.current != null)
{
// Determine size of bounding box
float scaled_offset = 1 / this.rd.WorldTransform.Scale;
// invert the scale
float scaled_size = DrawingUtility.cp_large_size;
// assume that the world transform is currently applied correctly to the graphics
RectangleF rect = new RectangleF((float)c.X - scaled_size / 2, (float)c.Y - scaled_size / 2, scaled_size, scaled_size);
foreach (ArbiterParkingSpotWaypoint apsw in this.zt.current.ParkingSpotWaypoints.Values)
{
if (apsw.Position.DistanceTo(c) < 1.0)
{
node = apsw;
edge = null;
return;
}
}
foreach (ArbiterPerimeterWaypoint apw in this.zt.current.Perimeter.PerimeterPoints.Values)
{
if ((apw.IsExit || apw.IsEntry) && rect.Contains(DrawingUtility.ToPointF(apw.Position)) &&
(this.PreviousNode == null || !this.PreviousNode.Equals(apw)) &&
apw.Position.DistanceTo(c) < 1.0)
{
node = apw;
edge = null;
return;
}
}
foreach (INavigableNode inn in this.zt.current.NavigationNodes)
{
if (rect.Contains(DrawingUtility.ToPointF(inn.Position)) &&
(this.PreviousNode == null || !this.PreviousNode.Equals(inn)) &&
inn.Position.DistanceTo(c) < 1.0)
{
node = inn;
edge = null;
return;
}
}
NavigableEdge closest = null;
double distance = double.MaxValue;
foreach (NavigableEdge ne in this.zt.current.NavigableEdges)
{
LinePath lp = new LinePath(new Coordinates[] { ne.Start.Position, ne.End.Position });
double dist = lp.GetClosestPoint(c).Location.DistanceTo(c);
if (dist < rect.Width && (dist < distance || closest == null) && dist < 1.0)
{
closest = ne;
distance = dist;
}
}
if (closest != null)
{
node = null;
edge = closest;
return;
}
}
node = null;
edge = null;
return;
}
public void Click(Coordinates c)
{
if (this.zt.Mode == ZoneToolboxMode.Selection)
{
this.zt.SelectZone(c);
}
else if (this.Mode == ZoneToolboxMode.NavNodes && this.zt.current != null)
{
// save undo point
this.ed.SaveUndoPoint();
// check if we hit any of hte edges or nodes part of the zone
ArbiterZone az = this.zt.current;
// check if hit node or edge
NavigableEdge ne;
INavigableNode nn;
this.NavigationHitTest(c, out nn, out ne);
if (nn != null)
{
// create new node
INavigableNode aznn = nn;
if (this.PreviousNode != null)
{
// create new edges
NavigableEdge newE1 = new NavigableEdge(true, this.zt.current, false, null, new List <IConnectAreaWaypoints>(), this.PreviousNode, aznn);
this.PreviousNode.OutgoingConnections.Add(newE1);
this.zt.current.NavigableEdges.Add(newE1);
}
this.PreviousNode = aznn;
}
else if (ne != null)
{
// remove old
this.zt.current.NavigableEdges.Remove(ne);
// remove all references
ne.Start.OutgoingConnections.Remove(ne);
// create new node
ArbiterZoneNavigableNode aznn = new ArbiterZoneNavigableNode(c);
// create new edges
NavigableEdge newE1 = new NavigableEdge(true, this.zt.current, false, null, new List <IConnectAreaWaypoints>(), ne.Start, aznn);
NavigableEdge newE2 = new NavigableEdge(true, this.zt.current, false, null, new List <IConnectAreaWaypoints>(), aznn, ne.End);
// add edges
ne.Start.OutgoingConnections.Add(newE1);
aznn.OutgoingConnections.Add(newE2);
// add all to lists
this.zt.current.NavigableEdges.Add(newE1);
this.zt.current.NavigableEdges.Add(newE2);
this.zt.current.NavigationNodes.Add(aznn);
if (this.PreviousNode != null)
{
NavigableEdge newE3 = new NavigableEdge(true, this.zt.current, false, null, new List <IConnectAreaWaypoints>(), this.PreviousNode, aznn);
this.PreviousNode.OutgoingConnections.Add(newE3);
this.zt.current.NavigableEdges.Add(newE3);
}
this.PreviousNode = aznn;
}
else
{
// create new node
ArbiterZoneNavigableNode aznn = new ArbiterZoneNavigableNode(c);
if (this.PreviousNode != null)
{
// create new edges
NavigableEdge newE1 = new NavigableEdge(true, this.zt.current, false, null, new List <IConnectAreaWaypoints>(), this.PreviousNode, aznn);
this.PreviousNode.OutgoingConnections.Add(newE1);
this.zt.current.NavigableEdges.Add(newE1);
}
this.PreviousNode = aznn;
this.zt.current.NavigationNodes.Add(aznn);
}
}
else if (this.zt.Mode == ZoneToolboxMode.StayOut && this.zt.current != null)
{
if (this.WrappingHelpers.Count == 0)
{
this.WrappingHelpers.Add(c);
}
else
{
// Determine size of bounding box
float scaled_offset = 1 / this.rd.WorldTransform.Scale;
// invert the scale
float scaled_size = DrawingUtility.cp_large_size;
// assume that the world transform is currently applied correctly to the graphics
RectangleF rect = new RectangleF((float)c.X - scaled_size / 2, (float)c.Y - scaled_size / 2, scaled_size, scaled_size);
if (rect.Contains(DrawingUtility.ToPointF(this.WrappingHelpers[0])) &&
c.DistanceTo(this.WrappingHelpers[0]) < 1)
{
ed.SaveUndoPoint();
Polygon p = new Polygon(this.WrappingHelpers);
this.zt.current.StayOutAreas.Add(p);
this.WrappingHelpers = new List <Coordinates>();
}
else
{
this.WrappingHelpers.Add(c);
}
}
this.rd.Invalidate();
}
}
