double find_current_nearest(Vector2d pt, double distThresh, out Vector2d nearPt) { nearPt = Vector2d.MaxValue; var result = current_grid.FindNearestInSquaredRadius(pt, distThresh * distThresh, (seg) => { return(seg.DistanceSquared(pt)); }, null); if (result.Value != double.MaxValue) { nearPt = result.Key.NearestPoint(pt); } return(Math.Sqrt(result.Value)); }
public double Distance(Vector2d v, double max_query_dist) { var found = grid.FindNearestInSquaredRadius(v, max_query_dist * max_query_dist, (idx) => { return(segments[idx].DistanceSquared(v)); }); if (found.Key != -1) { return(Math.Sqrt(found.Value)); } else { return(max_query_dist); } }
/// <summary> /// find nearest point to vertex in collision graph /// </summary> double MinCollisionConstraintDistance(int vid, double collision_radius) { if (collision_edge_hash == null) { return(double.MaxValue); } Vector2d pos = Graph.GetVertex(vid); Vector2d a = Vector2d.Zero, b = Vector2d.Zero; var result = collision_edge_hash.FindNearestInSquaredRadius(pos, collision_radius * collision_radius, (eid) => { CollisionGraph.GetEdgeV(eid, ref a, ref b); return(Segment2d.FastDistanceSquared(ref a, ref b, ref pos)); } ); return((result.Key == -1) ? double.MaxValue : Math.Sqrt(result.Value)); }
/// <summary> /// Find nearest point to vertex vid in graph, but filter out **connected** neighbourhood within self_radius /// </summary> double MinSelfSegDistance(int vid, double self_radius) { Vector2d pos = Graph.GetVertex(vid); List <int> ignore_edges = new List <int>(16); FindConnectedEdgesInRadius(vid, self_radius * self_radius, ignore_edges); Vector2d a = Vector2d.Zero, b = Vector2d.Zero; var result = edge_hash.FindNearestInSquaredRadius(pos, self_radius * self_radius, (eid) => { Graph.GetEdgeV(eid, ref a, ref b); return(Segment2d.FastDistanceSquared(ref a, ref b, ref pos)); }, (eid) => { return(ignore_edges.Contains(eid)); } ); return((result.Key == -1) ? double.MaxValue : Math.Sqrt(result.Value)); }