public DubinState(DubinNode node)
{
this.pos = node.pos;
this.v = node.vel;
this.theta = node.theta;
this.omega = node.omega;
}
void addNode(List <DubinNode> nodes, DubinNode node)
{
nodes.Add(node);
kdTree.insert(new double[2] {
node.pos.x, node.pos.y
}, node);
}
public static void backtrackPath(List <DubinNode> nodes, DubinNode startNode, DubinNode current, List <DubinNode> path)
{
if (current.parent != null)
{
backtrackPath(nodes, startNode, current.parent, path);
}
path.Add(current);
}
public void SetParent(DubinNode p)
{
if (this.parent != null)
{
this.parent.children.Remove(this);
}
this.parent = p;
}
public DubinNode(Vector2 pos, float vel, float theta, float omega, float time)
{
this.pos = pos;
this.vel = vel;
this.theta = theta;
this.omega = omega;
this.time = time;
this.children = new List <DubinNode>();
this.parent = null;
}
public DubinNode()
{
this.pos = Vector2.zero;
this.vel = 0F;
this.theta = 0F;
this.omega = 0F;
this.time = 0F;
this.children = new List <DubinNode>();
this.parent = null;
}
public static void TopoSort(DubinNode root, List <Pair <DubinNode, float> > sorted, float dt)
{
foreach (var child in root.children)
{
TopoSort(child, sorted, child.time - root.time);
}
sorted.Add(new Pair <DubinNode, float>(root, dt));
if (root.parent == null)
{
sorted.Reverse();
}
}
/*
* dubins transition function, multiple uses
* 1. if collision param is true: check for collision, will return bool
* 2. else: put interpolated nodes into list
*/
public static bool dubinsTransition(bool collision, List <Node> list, DubinNode from, DubinNode to, DubinInput input)
{
DubinNode prev = from;
double tx = from.pos.x, ty = from.pos.y, ttheta = from.theta; // HACK: to improve precision
foreach (DubinInput.Steer steer in input.steer)
{
float sumT = 0;
double v = steer.v;
while (sumT < steer.time)
{
var dt = Mathf.Min(DELTA_T, steer.time);
tx = tx + dt * v * Math.Cos(prev.theta);
ty = ty + dt * v * Math.Sin(prev.theta);
var pos = new Vector2((float)tx, (float)ty);
v += dt * steer.a;
ttheta = ttheta + dt * steer.omega;
while (ttheta > 2 * Mathf.PI)
{
ttheta -= 2 * Mathf.PI;
}
while (ttheta < 0)
{
ttheta += 2 * Mathf.PI;
}
if (collision && !Cheetah.instance.IsValid(pos))
{
return(false);
}
DubinNode nextPoint = new DubinNode(pos, steer.v, (float)ttheta, steer.omega, prev.time + dt);
if (!collision)
{
list.Add(nextPoint);
}
// if ((nextPoint.pos - prev.pos).magnitude > 0.5) // TODO: debug
// {
// Debug.Log("WTF");
// dubinInput(from, to);
// return false;
// }
prev = nextPoint;
sumT += dt;
}
}
// // TODO: debug
// if ((prev.pos - to.pos).magnitude > 0.5 || Mathf.Abs(prev.theta - to.theta) > 0.3)
// {
// Debug.Log("WTF");
// dubinInput(from, to);
// return false;
// }
return(true);
}
bool pathFeasible(DubinNode from, DubinNode to, DubinInput input)
{
// TODO: check for max condition
foreach (DubinInput.Steer steer in input.steer)
{
if (Mathf.Abs(steer.a) > MAX_ACCEL)
{
return(false);
}
}
var dubinsCollision = DDrive.dubinsTransition(true, null, from, to, input);
return(dubinsCollision);
}
Pair <DubinNode, float> findCheapestPoint(List <DubinNode> nodes, DubinNode target)
{
float minCost = Mathf.Infinity;
DubinNode cheapest = new DubinNode();
//var knn = kdTree.nearest(new double[2] {target.pos.x, target.pos.y}, Math.Min(nodes.Count, 100));
var knn = nodes;
foreach (var nn in knn)
{
DubinNode p = (DubinNode)nn;
var input = dubinInput(p, target);
if (input.time + p.time < minCost && pathFeasible(p, target, input))
{
minCost = p.time + input.time;
cheapest = p;
}
}
return(new Pair <DubinNode, float>(cheapest, minCost));
}
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> GetFilledPath(List <TreeNode> path)
{
List <Node> filledPath = new List <Node>();
for (var i = 0; i < path.Count - 1; i++)
{
if (path[i + 1].isDubins)
{
DubinNode from = nodeToDubinNode(path[i]);
DubinNode to = nodeToDubinNode(path[i + 1]);
var input = dubinInput(from, to);
filledPath.Add(from);
DDrive.dubinsTransition(false, filledPath, from, to, input);
}
else
{
var input = dynamicPointInput(path[i], path[i + 1]);
//Debug.Log(path[i + 1].vel.magnitude + " " + (path[i].vel + input.first * input.second).magnitude);
float sumT = 0;
var prev = path[i];
filledPath.Add(prev);
while (sumT < input.time)
{
var dt = Mathf.Min(DELTA_T, input.time);
var pos = prev.pos + prev.vel * dt + 0.5F * input.accel * dt * dt;
var vel = prev.vel + input.accel * dt;
TreeNode nextPoint = new TreeNode(pos, vel, prev.time + dt);
filledPath.Add((Node)nextPoint);
prev = nextPoint;
sumT += dt;
}
}
}
return(filledPath);
}
private DubinInput dubinInput(DubinNode near, DubinNode target)
{
return(DubinUtils.DubinSP(new DubinState(near), new DubinState(target), true));
}
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);
}
