// Update is called once per frame
void Update()
{
if (Input.GetKeyDown(KeyCode.A))
{
Debug.Log("Planning");
DStartState = new BestFirstState(startObject.transform.position) as DefaultState;
DGoalState = new BestFirstState(goalObject.transform.position) as DefaultState;
planner.oneStep = false;
planner.computePlan(ref DStartState, ref DGoalState, ref outputPlan, 10.0f);
}
if (Input.GetKeyDown(KeyCode.S))
{
Debug.Log("Planning");
DStartState = new BestFirstState(startObject.transform.position) as DefaultState;
DGoalState = new BestFirstState(goalObject.transform.position) as DefaultState;
planner.oneStep = true;
planner.computePlan(ref DStartState, ref DGoalState, ref outputPlan, 10.0f);
}
if (Input.GetKeyDown(KeyCode.Z))
{
showOpen = !showOpen;
}
if (Input.GetKeyDown(KeyCode.X))
{
showVisited = !showVisited;
}
}
private void ComputeTunnelSearch()
{
gridTimeDomain.UseTunnelSearch(tunnel);
DefaultState defStartState = startState as DefaultState;
DefaultState defLastState = lastState as DefaultState;
newPlan = new Stack <DefaultAction>();
goalReached = tunnelPlanner.computePlan(ref defStartState, ref defLastState, ref newPlan, maxTime);
gridTimeDomain.DisableTunnelSearch();
}
// Use this for initialization
void Start()
{
planner = new ARAstarPlanner();
BestFirstPlanner = new BestFirstSearchPlanner();
GetComponent<NeighbourObstacles>().Init ();
GridTimeDomain domain = new GridTimeDomain(GetComponent<NeighbourObstacles>(), GetComponent<NeighbourAgents>());
List<PlanningDomainBase> dl = new List<PlanningDomainBase>();
dl.Add(domain);
DefaultState startState = new GridTimeState(transform.position) as DefaultState;
DefaultState goalState = new GridTimeState(new Vector3(5,0.5f,0),5.0f) as DefaultState;
plan = new Dictionary<DefaultState, ARAstarNode>();
outputPlan = new Stack<DefaultState>();
if(BestFirstSearchPlanner) {
BestFirstPlanner.init(ref dl, 200);
bool completed = BestFirstPlanner.computePlan(ref startState, ref goalState, ref outputPlan, 10.0f);
Debug.Log("BestFirst Plan " + outputPlan.Count);
Debug.Log("Plan found: " + completed);
} else {
planner.init(ref dl,200);
float inflationFactor = 2.5f;
PathStatus status = planner.computePlan(ref startState ,ref goalState ,ref plan,ref inflationFactor, 10.0f);
planner.FillPlan();
Debug.Log ("ARA Plan " + plan.Count);
Debug.Log("Nodes Expanded: " + planner.Close.Elements().Count);
Debug.Log("Status: " + status);
}
}
// Use this for initialization
void Start()
{
planner = new ARAstarPlanner();
BestFirstPlanner = new BestFirstSearchPlanner();
GetComponent <NeighbourObstacles>().Init();
GridTimeDomain domain = new GridTimeDomain(GetComponent <NeighbourObstacles>(), GetComponent <NeighbourAgents>());
List <PlanningDomainBase> dl = new List <PlanningDomainBase>();
dl.Add(domain);
DefaultState startState = new GridTimeState(transform.position) as DefaultState;
DefaultState goalState = new GridTimeState(new Vector3(5, 0.5f, 0), 5.0f) as DefaultState;
plan = new Dictionary <DefaultState, ARAstarNode>();
outputPlan = new Stack <DefaultState>();
if (BestFirstSearchPlanner)
{
BestFirstPlanner.init(ref dl, 200);
bool completed = BestFirstPlanner.computePlan(ref startState, ref goalState, ref outputPlan, 10.0f);
Debug.Log("BestFirst Plan " + outputPlan.Count);
Debug.Log("Plan found: " + completed);
}
else
{
planner.init(ref dl, 200);
float inflationFactor = 2.5f;
PathStatus status = planner.computePlan(ref startState, ref goalState, ref plan, ref inflationFactor, 10.0f);
planner.FillPlan();
Debug.Log("ARA Plan " + plan.Count);
Debug.Log("Nodes Expanded: " + planner.Close.Elements().Count);
Debug.Log("Status: " + status);
}
}
public override bool ComputePlan()
{
bool stateWasNull = false;
if (currentState == null)
{
currentState = InitState();
stateWasNull = true;
}
goalState = new FootstepPlanningState(currentGoal, Quaternion.identity, goalTime);
outputPlan = new Stack <DefaultAction>();
if (stateWasNull || goalReached && !stateWasNull)
{
if (currentState.previousState != null)
{
float actionLength = currentState.time - currentState.previousState.time;
currentState.previousState.time = Time.time - actionLength;
}
currentState.time = Time.time;
}
DefaultState DcurrentState = currentState as DefaultState;
DefaultState DgoalState = goalState as DefaultState;
if (useGridDomain)
{
GridPlanningState currentGridState = new GridPlanningState(root.position);
DcurrentState = currentGridState as DefaultState;
GridPlanningState goalGridState = new GridPlanningState(currentGoal);
DgoalState = goalGridState as DefaultState;
}
if (useGridTimeDomain)
{
GridTimeState currentTimeState = new GridTimeState(root.position, Time.time, currentSpeed);
DcurrentState = currentTimeState as DefaultState;
GridTimeState goalTimeState = new GridTimeState(currentGoal, goalTime);
DgoalState = goalTimeState as DefaultState;
//Debug.Log("current time = " + currentTimeState.time);
}
//float startTime = Time.realtimeSinceStartup;
bool planComputed = planner.computePlan(ref DcurrentState, ref DgoalState, ref outputPlan, maxTime);
/*************************/
/* TUNNEL SEARCH TESTING */
//List<Vector3> movDirections = (domainList[0] as GridPlanningDomain).getMovDirections();
//TunnelSearch tunnelSearch = new TunnelSearch(outputPlan, 3, analyzer, movDirections);
if (TunnelSearch)
{
if (planComputed)
{
GridTunnelSearch gridTunnelSearch = new GridTunnelSearch(outputPlan, root.position,
Time.time, goalState.time, gtd,
MaxNumberOfNodes, maxTime,
//tunnelThresholdX, tunnelThresholdZ, tunnelThresholdT);
tunnelThresholdD, tunnelThresholdT,
currentSpeed);
outputPlanBeforeTunnel = outputPlan;
outputPlan = gridTunnelSearch.newPlan;
planComputed = gridTunnelSearch.goalReached;
//Debug.Log("At time " + Time.time + " tunnel plan computed? " + planComputed);
}
}
/*************************/
/*
* float endTime = Time.realtimeSinceStartup;
*
* timeSum += (endTime - startTime);
* numCalls++;
*
* float meanTime = 0.0f;
* if (numCalls == 100)
* {
* meanTime = timeSum / numCalls;
* Debug.Log("At time " + Time.time + " Mean Computing Plan Time = " + meanTime);
* numCalls = 0;
* timeSum = 0;
* }
*/
if (goalMeshRenderer != null && goalStateTransform.position == currentGoal)
{
if (planComputed && goalCompletedMaterial != null)
{
goalMeshRenderer.material = goalCompletedMaterial;
}
else
{
goalMeshRenderer.material = goalMaterial;
}
}
//Debug.Log("new plan! at time: " + Time.time + " with " + outputPlan.Count + " actions");
goalReached = false;
/*
* if (planComputed)
* {
* AnimationCurve[] curves = GetPlanAnimationCurve();
* curveX = curves[0];
* curveY = curves[1];
* curveZ = curves[2];
* }
*/
return(planComputed && outputPlan.Count > 0);
}
