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)); }