private bool TryGetSuccessor([NotNull] IDictionary <TEdge, int> visited, [NotNull] TVertex vertex, out TEdge successor)
{
IEnumerable <TEdge> outEdges = VisitedGraph.OutEdges(vertex);
IEnumerable <TEdge> edges = outEdges.Where(edge => !visited.ContainsKey(edge));
return(EdgeChain.TryGetSuccessor(edges, vertex, out successor));
}
static EdgeChain SearchChain(EdgeChain chainStart, int maxSearch = 800)
{
int counter = 0;
RemoveTurningPoints(chainStart.pos, rayLen * 1.5f);
//MarkSearchedMask(chainStart.pos, rayLen * 0.5f);
EdgeChain currentChainStart = chainStart;
Vector2 prevPos;
float L2 = rayLen * rayLen;
while (SearchPrevious(currentChainStart, out prevPos))
{
counter++;
if (counter > maxSearch)
{
Debug.LogError("search exited");
//if (!errorFrameSaved)
//{
// byte[] bytes = image.EncodeToPNG();
// var fs = System.IO.File.OpenWrite(@"D:\LZR\MyFiles\temp\errorImage.png");
// fs.Write(bytes, 0, bytes.Length);
// fs.Flush();
// fs.Close();
//}
break;
}
//成环
if (currentChainStart.next && currentChainStart.next.next && (currentChainStart.pos - chainStart.pos).sqrMagnitude < L2)
{
return(currentChainStart);
}
//合并
EdgeChain chainToMerge = null;
foreach (var chain in chains)
{
if ((chain.Last.pos - currentChainStart.pos).sqrMagnitude <= L2)
{
chainToMerge = chain;
break;
}
}
if (chainToMerge)
{
var chainEnd = chainToMerge.Last;
chainEnd.next = currentChainStart;
currentChainStart.previous = chainEnd;
return(null);
}
//此处标记为已搜索
RemoveTurningPoints(prevPos, rayLen * 1.5f);
//MarkSearchedMask((currentChainStart.pos + prevPos) / 2, rayLen * 0.5f);
MarkSearchedMask(prevPos, rayLen * 0.3f);
//新节点
var newNode = new EdgeChain(prevPos);
newNode.next = currentChainStart;
currentChainStart.previous = newNode;
currentChainStart = newNode;
}
return(currentChainStart);
}
static bool SearchPrevious(EdgeChain chainStart, out Vector2 res)
{
float prevAngle = 0;
if (chainStart.next)
{
var d = chainStart.pos - chainStart.next.pos;
prevAngle = Mathf.Atan2(d.y, d.x);
}
Vector2 prevPos = chainStart.pos + new Vector2(Mathf.Cos(prevAngle), Mathf.Sin(prevAngle)) * rayLen;
int prevSample = BilinearSample(prevPos);
int steps = Mathf.FloorToInt(Mathf.PI / angleStep);
int firstSample = prevSample;
int dir = firstSample > 0 ? -1 : 1;
for (float i = 0; i <= steps; i++)
{
float curAngle = prevAngle + angleStep * dir;
Vector2 curPos = chainStart.pos + new Vector2(Mathf.Cos(curAngle), Mathf.Sin(curAngle)) * rayLen;
int curSample = BilinearSample(curPos);
if (curSample != prevSample && curSample != firstSample && curSample != -1 && prevSample != -1 && !searchedMask[Mathf.FloorToInt(curPos.x), Mathf.FloorToInt(curPos.y)])
{
res = BinSearchBetween(curPos, prevPos, curSample, prevSample, 1);
return(true);
}
prevPos = curPos;
prevSample = curSample;
prevAngle = curAngle;
}
res = Vector2.zero;
return(false);
}
static EdgeChain NearestChain(List <EdgeChain> chains, Vector2 pos)
{
EdgeChain nearest = null;
float nearestD2 = float.PositiveInfinity;
foreach (var chain in chains)
{
var D2 = (chain.pos - pos).sqrMagnitude;
if (D2 < nearestD2)
{
nearestD2 = D2;
nearest = chain;
}
}
return(nearest);
}
/// <summary>
/// Generates a random walk with <paramref name="walkCount"/> steps.
/// </summary>
/// <param name="root">Root vertex.</param>
/// <param name="walkCount">Number of steps for the random walk.</param>
public void Generate([NotNull] TVertex root, int walkCount)
{
if (root == null)
{
throw new ArgumentNullException(nameof(root));
}
int count = 0;
TVertex current = root;
OnStartVertex(root);
while (count < walkCount && TryGetSuccessor(current, out TEdge edge))
{
// If dead end stop
if (edge == null)
{
break;
}
// If end predicate
if (EndPredicate != null && EndPredicate(edge))
{
break;
}
OnTreeEdge(edge);
current = edge.Target;
// Upgrade count
++count;
}
OnEndVertex(current);
#region Local function
bool TryGetSuccessor(TVertex vertex, out TEdge successor)
{
return(EdgeChain.TryGetSuccessor(VisitedGraph, vertex, out successor));
}
#endregion
}
public override void Keyboard(SFML.Window.KeyCode key)
{
switch (char.ToLower((char)key))
{
case 'a':
m_leftJoint.MaxMotorTorque = 800.0f;
m_leftJoint.MotorSpeed = 36.0f;
break;
case 's':
m_leftJoint.MaxMotorTorque = 100.0f;
m_leftJoint.MotorSpeed = 0.0f;
break;
case 'd':
m_leftJoint.MaxMotorTorque = 1200.0f;
m_leftJoint.MotorSpeed = -36.0f;
break;
case 'w':
m_vehicle.ApplyLinearImpulse(new Vec2(0.0f, 455.0f), m_vehicle.WorldCenter);
m_vehicle.ApplyAngularImpulse(66);
break;
case 't':
_points.Add(Program.MainForm.ConvertScreenToWorld(Main.GLWindow.Input.GetMouseX(), Main.GLWindow.Input.GetMouseY()));
break;
case 'y':
{
var chain = new EdgeChain(_points.ToArray());
chain.GenerateBodies(m_world, new Vec2(0, 0), new FixtureDef(null, 0, 0, 0.65f));
_points.Clear();
}
break;
}
}
public PickEvent(Vector2d point, double distance, EdgeChain chain) : base(point, distance)
{
Chain = chain;
}
