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