void reparent(DubinNode root, DubinNode parent, DubinNode goal)
{
var nodes = new List <Pair <DubinNode, float> >();
Utils.TopoSort(root, nodes, 0F);
for (int i = 0; i < nodes.Count; i++)
{
DubinNode p = nodes[i].first;
float dt = nodes[i].second;
var input = dubinInput(parent, p);
if (p.parent != null && p.parent.time + dt < p.time)
{
p.time = p.parent.time + dt;
}
if (input.time + parent.time < p.time && pathFeasible(parent, p, input))
{
p.time = input.time + parent.time;
p.SetParent(parent);
reparentCnt++;
}
}
}
List <Node> PlanRrtStar()
{
// RRT*
startNode = new DubinNode(new Vector2(startPos[0], startPos[1]), new Vector2(startVel[0], startVel[1]).magnitude,
startTheta, MAX_OMEGA, 0);
addNode(nodes, startNode);
goalNode = new DubinNode(new Vector2(goalPos[0], goalPos[1]), new Vector2(goalVel[0], goalVel[1]).magnitude, goalTheta, MAX_OMEGA, 0);
int cnt = iter, i = 0;
float r = Mathf.Infinity; // TODO: Dynamic compute this r
var solnCnt = 0;
while (i++ < cnt)
{
DubinNode target = i == cnt ? goalNode : randomNode();
var cheapest = findCheapestPoint(nodes, target);
var cheapestNode = cheapest.first;
var cheapestCost = cheapest.second;
if (cheapestCost == Mathf.Infinity)
{
continue;
}
// TODO: Change the magic number
if ((target.pos - goalNode.pos).magnitude <= 0.5)
{
Debug.Log("found a soln");
solnCnt++;
informedLength = Mathf.Min(informedLength, cheapestCost * MAX_SPEED);
curBestTime = Mathf.Min(curBestTime, cheapestCost);
Debug.Log(target.pos);
Debug.Log(cheapestCost);
}
if (cheapestCost + (goalNode.pos - target.pos).magnitude / MAX_SPEED > curBestTime + 0.5)
{
usefulCnt++;
// i--;
// continue;
}
cheapestNode.children.Add(target);
target.SetParent(cheapestNode);
target.time = cheapestCost;
addNode(nodes, target);
reparent(startNode, target, goalNode);
}
Debug.Log("reparent cnt = " + reparentCnt);
Debug.Log("useful count = " + usefulCnt);
Debug.Log("goalNode parent count " + (goalNode.parent != null));
List <DubinNode> path = new List <DubinNode>();
Utils.backtrackPath(nodes, startNode, goalNode, path);
for (i = 0; i < path.Count - 1; i++)
{
var f = path[i];
Debug.Log("vel " + i);
Debug.Log(f.vel);
Debug.Log("pos");
Debug.Log(f.pos);
}
var filledPath = GetFilledPath(path);
Utils.SaveFilledPath(filledPath);
Debug.Log("path " + path.Count);
Debug.Log("path[cnt] " + path[path.Count - 1].pos.x + " " + path[path.Count - 1].pos.y + " " +
path[path.Count - 1].time);
return(filledPath);
}