/// <summary>
/// 计算点连接圆的两条切线（用来处理起点和终点）
/// </summary>
private static void HandlePointCircleEdges(CircleGraphNode p, Circle c, List <Circle> circles, List <CircleGraphNode> nodes)
{
float d = Vector2.Distance(c.center, p.Center);
float theta = Mathf.Acos(c.radius / d);
Vector2 toP = p.Center - c.center;
float alpha = Mathf.Atan2(toP.y, toP.x);
var c1 = c.center + new Vector2(Mathf.Cos(alpha + theta), Mathf.Sin(alpha + theta)) * c.radius;
var e1 = new Edge(p.Center, c1);
var c2 = c.center + new Vector2(Mathf.Cos(alpha - theta), Mathf.Sin(alpha - theta)) * c.radius;
var e2 = new Edge(p.Center, c2);
bool isValid1 = true, isValid2 = true;
for (int i = 0; i < circles.Count; i++)
{
if (ReferenceEquals(circles[i], c))
{
continue;
}
if (isValid1 && !circles[i].CheckLineOfSignt(e1))
{
isValid1 = false;
}
if (isValid2 && !circles[i].CheckLineOfSignt(e2))
{
isValid2 = false;
}
if (!isValid1 && !isValid2)
{
break;
}
}
if (isValid1)
{
CircleGraphNode gb = new CircleGraphNode(e1.b, c);
p.AddNeighbor(gb);
gb.AddNeighbor(p);
nodes.Add(gb);
c.surfingPoints.Add(gb);
}
if (isValid2)
{
CircleGraphNode gb = new CircleGraphNode(e2.b, c);
p.AddNeighbor(gb);
gb.AddNeighbor(p);
nodes.Add(gb);
c.surfingPoints.Add(gb);
}
}
private static List <CircleGraphNode> BruteForce(Vector2 start, Vector2 end, List <Circle> obstacles)
{
List <CircleGraphNode> result = new List <CircleGraphNode>();
for (int i = 0; i < obstacles.Count; i++)
{
obstacles[i].surfingPoints.Clear();
}
CircleGraphNode startNode = new CircleGraphNode(start, null);
CircleGraphNode endNode = new CircleGraphNode(end, null);
result.Add(startNode);
result.Add(endNode);
for (int i = 0; i < obstacles.Count; i++)
{
var a = obstacles[i];
//连接起点和终点
HandlePointCircleEdges(startNode, a, obstacles, result);
HandlePointCircleEdges(endNode, a, obstacles, result);
//到别的圆的链接
for (int j = i + 1; j < obstacles.Count; j++)
{
var b = obstacles[j];
var internalBitangents = InternalBitangents(a, b);
HandleEdges(a, b, obstacles, internalBitangents, result);
var externalBitangents = ExternalBitangents(a, b);
HandleEdges(a, b, obstacles, externalBitangents, result);
}
//本圆的链接
for (int j = 0; j < a.surfingPoints.Count - 1; j++)
{
for (int k = j + 1; k < a.surfingPoints.Count; k++)
{
var ga = a.surfingPoints[j];
var gb = a.surfingPoints[k];
ga.AddNeighbor(gb);
gb.AddNeighbor(ga);
}
}
}
return(result);
}
private static void HandleEdges(Circle a, Circle b, List <Circle> circles, List <Edge> edges, List <CircleGraphNode> nodes)
{
if (edges == null)
{
return;
}
for (int index = 0; index < circles.Count; index++)
{
var c = circles[index];
if (ReferenceEquals(a, c) || ReferenceEquals(b, c))
{
continue;
}
for (int i = edges.Count - 1; i >= 0; i--)
{
var e = edges[i];
if (!c.CheckLineOfSignt(e))
{
edges.RemoveAt(i);
}
}
if (edges.Count <= 0)
{
break;
}
}
for (int i = 0; i < edges.Count; i++)
{
var ga = new CircleGraphNode(edges[i].a, a);
a.surfingPoints.Add(ga);
nodes.Add(ga);
var gb = new CircleGraphNode(edges[i].b, b);
b.surfingPoints.Add(gb);
nodes.Add(gb);
ga.AddNeighbor(gb);
gb.AddNeighbor(ga);
}
}
public CircleGraphAStar(CircleGraphNode startNode, CircleGraphNode endNode) : base(startNode, endNode)
{
}