private void CreateConverter()
{
Gps geo1 = new Gps(anchor1.lat, anchor1.lon);
Gps geo2 = new Gps(anchor2.lat, anchor2.lon);
Vector2 point1 = new Vector2(anchor1.transform.localPosition.x, anchor1.transform.localPosition.z);
Vector2 point2 = new Vector2(anchor2.transform.localPosition.x, anchor2.transform.localPosition.z);
flatGeo = new FlatGeo(geo1, geo2, point1, point2);
}
PointD PossiblyCombinePoint(PointD point, Double distance)
{
PointD ret = point;
foreach (KeyValuePair <String, TreePathVertice> kvp in _vertices)
{
TreePathVertice vertice = kvp.Value;
if (point.Equals(vertice.Position) == false && FlatGeo.Distance(point, vertice.Position) < distance)
{
ret = vertice.Position;
DebugLogText(LogLevel.DEBUG, "Combining {0} into {1}", point, ret);
break;
}
}
return(ret);
}
SortedList <Double, TreePathVertice> GetVerticesInProximalOrder(PointD to)
{
SortedList <Double, TreePathVertice> list = new SortedList <Double, TreePathVertice>();
foreach (TreePathVertice vertice in _vertices.Values)
{
Double distance = FlatGeo.GetDistance(vertice.Position, to);
while (list.ContainsKey(distance))
{
distance += .001;
}
list.Add(distance, vertice);
}
return(list);
}
public bool Cycle()
{
/** get the vertice with the lowest distance from the origin */
TreePathVertice current = null;
foreach (TreePathVertice vertice in m_UnvisitedVertices)
{
if (current == null || vertice.Distance <= current.Distance)
{
current = vertice;
}
}
DebugLogText(LogLevel.DEBUG, "Cycle set current vertice to {0}", current);
if (current.Distance != Double.PositiveInfinity)
{
/** go through each neighbor of the current */
foreach (KeyValuePair <String, TreePathVertice> kvp in current.Neighbors)
{
TreePathVertice neighbor = kvp.Value;
if (neighbor.State == TreePathVertice.VerticeState.Unvisited)
{
Double nDistance = FlatGeo.Distance(current.Position, neighbor.Position);
if (current.Distance + nDistance < neighbor.Distance)
{
neighbor.Distance = current.Distance + nDistance;
neighbor.Source = current;
DebugLogText(LogLevel.DEBUG, "--- set neighbor {0} to distance of {1:0.00}", neighbor, GetNativeDistance(neighbor.Distance));
}
}
}
/** move this node from visited to unvisited */
current.State = TreePathVertice.VerticeState.Visited;
DebugLogText(LogLevel.DEBUG, "Set vertice {0} to Visited", current);
m_UnvisitedVertices.Remove(current);
m_VisitedVertices.Add(current);
}
else
{
m_UnvisitedVertices.Remove(current);
DebugLogText(LogLevel.ERROR, "VERTICE {0} has no solution!", current);
}
return(m_UnvisitedVertices.Count > 0);
}
PointD FindClosestPointOnOriginalPath(PointD to)
{
PointD proximal = null;
Double shortest = Double.MaxValue;
foreach (Line line in m_Lines)
{
PointD point = line.ClosestPointTo(to);
Double distance = FlatGeo.GetDistance(point, to);
if (distance < shortest)
{
shortest = distance;
proximal = point;
}
}
return(proximal);
}
protected virtual Double GetNativeDistance(PointD p1, PointD p2)
{
return(FlatGeo.GetDistance(p1 as PointD, p2 as PointD));
}
