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