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