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