private void GetBoundingPoints(ArbiterZone az, out Coordinates bl, out Coordinates tr)
{
// get bL and tR
double n = Double.MinValue;
double s = Double.MaxValue;
double e = Double.MinValue;
double w = Double.MaxValue;
foreach (ArbiterPerimeterWaypoint apw in this.current.Perimeter.PerimeterPoints.Values)
{
if (apw.Position.X > e)
{
e = apw.Position.X;
}
if (apw.Position.X < w)
{
w = apw.Position.X;
}
if (apw.Position.Y > n)
{
n = apw.Position.Y;
}
if (apw.Position.Y < s)
{
s = apw.Position.Y;
}
}
bl = new Coordinates(w, s);
tr = new Coordinates(n, e);
}
private ArbiterZoneNavigableNode[][] NodeMatrix(ArbiterZone az)
{
// get bL and tR
Coordinates bl;
Coordinates tr;
this.GetBoundingPoints(az, out bl, out tr);
int n = (int)tr.Y;
int s = (int)bl.Y;
int e = (int)tr.X;
int w = (int)bl.X;
// create matrix
ArbiterZoneNavigableNode[][] nodeMatrix = new ArbiterZoneNavigableNode[e - w + 1][];
// loop through coordinates
for (int i = w; i <= e; i++)
{
for (int j = s; j <= n; j++)
{
// position
Coordinates c = new Coordinates((double)i, (double)j);
// check inside perimeter
if (az.Perimeter.PerimeterPolygon.IsInside(c))
{
// check interacts
bool clear = true;
foreach (Polygon o in az.StayOutAreas)
{
if (o.IsInside(c))
{
clear = false;
}
}
// not inside out of polys
if (clear)
{
nodeMatrix[i - w][j - s] = new ArbiterZoneNavigableNode(c);
}
}
}
}
// return
return(nodeMatrix);
}
public void SelectZone(Coordinates c)
{
this.current = null;
if (this.parentTool.arn != null)
{
foreach (ArbiterZone az in this.parentTool.arn.ArbiterZones.Values)
{
if (az.Perimeter.PerimeterPolygon.IsInside(c))
{
this.current = az;
this.ZoneToolboxSelectedZoneTextBox.Text = az.ToString();
this.Invalidate();
}
}
}
if (this.current == null)
{
this.ZoneToolboxSelectedZoneTextBox.Text = "None";
}
}
public SceneZonePartition(ArbiterZone az)
{
this.Zone = az;
apws = new ArbiterPerimeterWaypoint[Zone.Perimeter.PerimeterPoints.Count];
Zone.Perimeter.PerimeterPoints.Values.CopyTo(apws, 0);
}
/// <summary>
/// Plan in a zone
/// </summary>
/// <param name="az"></param>
/// <param name="goal"></param>
/// <returns></returns>
public ZonePlan PlanZone(ArbiterZone az, INavigableNode start, INavigableNode goal)
{
// zone plan
ZonePlan zp = new ZonePlan();
zp.Zone = az;
zp.Start = start;
// given start and goal, get plan
double time;
List <INavigableNode> nodes;
this.Plan(start, goal, out nodes, out time);
zp.Time = time;
// final path
LinePath recommendedPath = new LinePath();
List <INavigableNode> pathNodes = new List <INavigableNode>();
// start and end counts
int startCount = 0;
int endCount = 0;
// check start type
if (start is ArbiterParkingSpotWaypoint)
{
startCount = 2;
}
// check end type
if (goal is ArbiterParkingSpotWaypoint &&
((ArbiterParkingSpotWaypoint)goal).ParkingSpot.Zone.Equals(az))
{
endCount = -2;
}
// loop through nodes
for (int i = startCount; i < nodes.Count + endCount; i++)
{
// go to parking spot or endpoint
if (nodes[i] is ArbiterParkingSpotWaypoint)
{
// set zone goal
zp.ZoneGoal = ((ArbiterParkingSpotWaypoint)nodes[i]).ParkingSpot.Checkpoint;
// set path, return
zp.RecommendedPath = recommendedPath;
zp.PathNodes = pathNodes;
// set route info
RouteInformation ri = new RouteInformation(recommendedPath, time, goal.ToString());
CoreCommon.CurrentInformation.Route1 = ri;
// return the plan
return(zp);
}
// go to perimeter waypoint if this is one
else if (nodes[i] is ArbiterPerimeterWaypoint && ((ArbiterPerimeterWaypoint)nodes[i]).IsExit)
{
// add
recommendedPath.Add(nodes[i].Position);
// set zone goal
zp.ZoneGoal = nodes[i];
// set path, return
zp.RecommendedPath = recommendedPath;
zp.PathNodes = pathNodes;
// set route info
RouteInformation ri = new RouteInformation(recommendedPath, time, goal.ToString());
CoreCommon.CurrentInformation.Route1 = ri;
// return the plan
return(zp);
}
// otherwise just add
else
{
// add
recommendedPath.Add(nodes[i].Position);
pathNodes.Add(nodes[i]);
}
}
// set zone goal
zp.ZoneGoal = goal;
// set path, return
zp.RecommendedPath = recommendedPath;
// set route info
CoreCommon.CurrentInformation.Route1 = new RouteInformation(recommendedPath, time, goal.ToString());
// return the plan
return(zp);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zone"></param>
public ZoneStartupState(ArbiterZone zone, bool unreachable)
: base(zone)
{
this.unreachablePathExists = unreachable;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zone"></param>
public ZoneStartupState(ArbiterZone zone)
: base(zone)
{
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zone"></param>
/// <param name="parkingSpot"></param>
public PullingOutState(ArbiterZone zone, ArbiterParkingSpot parkingSpot)
: base(zone)
{
this.ParkingSpot = parkingSpot;
}
private void ZoneToolbox_Load(object sender, EventArgs e)
{
this.current = null;
}
/// <summary>
/// Startup the vehicle from a certain position, pass the next state back,
/// and initialize the lane agent if possible
/// </summary>
/// <param name="vehicleState"></param>
/// <returns></returns>
public IState Startup(VehicleState vehicleState, CarMode carMode)
{
// check car mode
if (carMode == CarMode.Run)
{
// check areas
ArbiterLane al = CoreCommon.RoadNetwork.ClosestLane(vehicleState.Front);
ArbiterZone az = CoreCommon.RoadNetwork.InZone(vehicleState.Front);
ArbiterIntersection ain = CoreCommon.RoadNetwork.InIntersection(vehicleState.Front);
ArbiterInterconnect ai = CoreCommon.RoadNetwork.ClosestInterconnect(vehicleState.Front, vehicleState.Heading);
if (az != null)
{
// special zone startup
return(new ZoneStartupState(az));
}
if (ain != null)
{
if (al != null)
{
// check lane stuff
PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front);
// get distance from front of car
double dist = lanePoint.pt.DistanceTo(vehicleState.Front);
// check dist to lane
if (dist < al.Width + 1.0)
{
// check orientation relative to lane
Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize();
// get our heading
Coordinates ourHeading = vehicleState.Heading.Normalize();
// forwards or backwards
bool forwards = true;
// check dist to each other
if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0))
{
// not going forwards
forwards = false;
}
// stay in lane if forwards
if (forwards)
{
ArbiterOutput.Output("Starting up in lane: " + al.ToString());
return(new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState));
}
else
{
// Opposing lane
return(new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState));
}
}
}
// startup intersection state
return(new IntersectionStartupState(ain));
}
if (al != null)
{
// get a startup chute
ArbiterLanePartition startupChute = this.GetStartupChute(vehicleState);
// check if in a startup chute
if (startupChute != null && !startupChute.IsInside(vehicleState.Front))
{
ArbiterOutput.Output("Starting up in chute: " + startupChute.ToString());
return(new StartupOffChuteState(startupChute));
}
// not in a startup chute
else
{
PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front);
// get distance from front of car
double dist = lanePoint.pt.DistanceTo(vehicleState.Front);
// check orientation relative to lane
Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize();
// get our heading
Coordinates ourHeading = vehicleState.Heading.Normalize();
// forwards or backwards
bool forwards = true;
// check dist to each other
if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0))
{
// not going forwards
forwards = false;
}
// stay in lane if forwards
if (forwards)
{
ArbiterOutput.Output("Starting up in lane: " + al.ToString());
return(new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState));
}
else
{
// opposing
return(new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState));
}
}
}
// fell out
ArbiterOutput.Output("Cannot find area to startup in");
return(CoreCommon.CorePlanningState);
}
else
{
return(CoreCommon.CorePlanningState);
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zone"></param>
public ZoneState(ArbiterZone zone)
{
this.Zone = zone;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zone"></param>
public ZoneOrientationState(ArbiterZone zone, NavigableEdge final)
: base(zone)
{
this.final = final;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zone"></param>
public ZoneTravelingState(ArbiterZone zone, INavigableNode start) : base(zone)
{
this.Start = start;
}
/// <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 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();
}
}
