public Node[] SolveFirst(Vector2 vqinit, Vector2 vqend)
{
QuadTreeArea currentArea = new QuadTreeArea(Math.Min(vqinit.X, vqend.X),
Math.Max(vqinit.Y, vqend.Y),
Math.Max(vqinit.X, vqend.X),
Math.Min(vqinit.Y, vqend.Y));
Node qend = new Node {
Position = vqend
};
Node qinit = new Node {
Position = vqinit
};
qinit.Heuristic = (vqend - vqinit).Module();
currentArea.Add(qinit);
ProblemContext.onNewNode(qinit);
int CurrentIteration = 0;
while (CurrentIteration < ProblemContext.MaxGenerations)
{
Node bestNode = currentArea.GetBestNodeFromMostUndenseArea();
Node newNode = bestNode.TryExpand(qend);
if (currentArea.Parent != null)
{
currentArea = currentArea.Antecesor;
}
if (newNode == qend)
{
return(newNode.Predecesors.ToArray());
}
CurrentIteration++;
}
return(null);
}
public Node TryExpand(Node qend)
{
bool found = false;
Node n = new Node();
bool solution = false;
Random r = new Random(Environment.TickCount);
if (candidateDisplacement == null)
{
candidateDisplacementGenerator = new randomExpansionIterator {
q = this.Position, qend = qend.Position, maxVectorModule = 50
};
candidateDisplacement = candidateDisplacementGenerator
.GetVectors()
.GetEnumerator();
}
int fails = 0;
candidateDisplacement.MoveNext();
while (!found && fails < ProblemContext.MaxAttemptsPerNodeExpansion)
{
Vector2 targetPoint;
Vector2?collisionPoint;
if (r.Next() % 3 == 0)
{
targetPoint = qend.Position;
}
else
{
QuadTreeArea CandidateArea;
do
{
targetPoint = this.Position + candidateDisplacement.Current;
CandidateArea = this.Area.FindArea(targetPoint);
this.candidateDisplacement.MoveNext();
}while (CandidateArea != null && CandidateArea.Density > this.Area.Density);
}
ProblemContext.onCandidate(this, candidateDisplacement.Current);
if (!ProblemContext.checkObstacle(targetPoint.X, targetPoint.Y))
{
collisionPoint = ProblemContext.LocalPlanner(Position, targetPoint);
//no collision
if (collisionPoint == null)
{
Node parentForNewChild = this;
//OK
n.Position = targetPoint;
n.Heuristic = 0;
n.Cost = parentForNewChild.Cost + targetPoint.Module();
n.Parent = parentForNewChild;
parentForNewChild.ChildrenCount++;
parentForNewChild.Area.Add(n);
ProblemContext.onNewNode(n);
found = true;
if (qend.Position.X == n.Position.X && qend.Position.Y == n.Position.Y)
{
solution = true;
}
}
}
if (!found)
{
fails++;
//candidateDisplacementGenerator.maxVectorModule /= 2.0;
}
}
if (solution)
{
qend.Parent = n;
return(qend);
}
else
{
return(n);
}
}