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