// 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
public override void Init(AnimationAnalyzer animAnalyzer, NeighbourAgents neighborhood, NeighbourObstacles obstacles)
{
analyzer = animAnalyzer;
if (analyzer != null && !analyzer.initialized)
{
analyzer.Init();
}
storedPlan = new List <FootstepPlanningAction>();
outputPlan = new Stack <DefaultAction>();
domainList = new List <PlanningDomainBase>();
gpd = new GridPlanningDomain(analyzer);
//gtd = new GridTimeDomain(analyzer, obstacles);
gtd = new GridTimeDomain(obstacles, neighborhood);
fpd = new FootstepPlanningDomain(analyzer, neighborhood, obstacles);
if (useGridDomain)
{
domainList.Add(gpd);
//domainList.Add(gpd);
}
if (useGridTimeDomain)
{
domainList.Add(gtd);
//domainList.Add(gtd);
}
if (useFootstepDomain)
{
domainList.Add(fpd);
//domainList.Add(fpd);
}
planner = new BestFirstSearchPlanner();
planner.init(ref domainList, MaxNumberOfNodes);
initialized = true;
currentState = null;
isPlanComputed = false;
planChanged = false;
goalMeshRenderer = goalStateTransform.gameObject.GetComponent("MeshRenderer") as MeshRenderer;
currentGoal = goalStateTransform.position;
goalReached = false;
}
// Use this for initialization
void Start()
{
actions = new Stack <BestFirstAction>();
outputPlan = new Stack <DefaultState>();
domainList = new List <PlanningDomainBase>();
domainList.Add(new BestFirstDomain());
planner = new BestFirstSearchPlanner();
planner.init(ref domainList, 1000);
startState = new BestFirstState(startObject.transform.position);
goalState = new BestFirstState(goalObject.transform.position);
prevGoalState = goalState;
DStartState = startState as DefaultState;
DGoalState = goalState as DefaultState;
}
// 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);
}
}
// Creation function
//public GridTunnelSearch(Stack<DefaultAction> plan, float startTime, float idealGoalTime, GridTimeDomain gtd, int MaxNumberOfNodes, float MaxTime, float T_x, float T_z, float T_t)
public GridTunnelSearch(Stack <DefaultAction> plan, Vector3 startPos, float startTime, float idealGoalTime, GridTimeDomain gtd, int MaxNumberOfNodes, float MaxTime, float T_d, float T_t, float startSpeed = 0)
{
currentPlan = new DefaultAction[plan.Count];
plan.CopyTo(currentPlan, 0);
this.T_d = T_d;
//this.T_x = T_x;
//this.T_z = T_z;
this.T_t = T_t;
this.maxTime = MaxTime;
int startIndex = 0;
//GridPlanningState startPlanningState = currentPlan[startIndex].state as GridPlanningState;
//while (startPlanningState == null)
//{
// startIndex++;
// startPlanningState = currentPlan[startIndex].state as GridPlanningState;
//}
//startState = new GridTimeState(startPlanningState.currentPosition, startTime, startSpeed);
startState = new GridTimeState(startPos, startTime, startSpeed);
lastState = new GridTimeState((currentPlan[currentPlan.Length - 1].state as GridPlanningState).currentPosition, idealGoalTime);
gridTimeDomain = gtd;
float dist = (startState.currentPosition - lastState.currentPosition).magnitude;
// TODO: what to do if there is no time???
float minSpeed = dist / (idealGoalTime - startTime);
if (minSpeed < 0)
{
minSpeed = GridTimeDomain.MIN_SPEED;
}
//tunnel = ConvertPlan(gtd.minSpeed, gtd.midSpeed, gtd.maxSpeed);
tunnel = ConvertPlan(startTime, minSpeed, GridTimeDomain.MID_SPEED, GridTimeDomain.MAX_SPEED);
//for(int aux = 0; aux < tunnel.tunnelMinVelocity.Length; aux++)
// Debug.Log("Tunnel "+aux+":"+tunnel.tunnelMinVelocity[aux]);
tunnelPlanner = new BestFirstSearchPlanner();
List <PlanningDomainBase> domainList = new List <PlanningDomainBase>();
domainList.Add(gridTimeDomain);
tunnelPlanner.init(ref domainList, MaxNumberOfNodes);
// MUBBASIR TESTING ARA STAR PLANNER HERE
araStarPlanner = new ARAstarPlanner();
araStarPlanner.init(ref domainList, MaxNumberOfNodes);
goalReached = false;
ComputeTunnelSearch();
//ComputeTunnelSearchStatePlan ();
}
// Creation function
//public GridTunnelSearch(Stack<DefaultAction> plan, float startTime, float idealGoalTime, GridTimeDomain gtd, int MaxNumberOfNodes, float MaxTime, float T_x, float T_z, float T_t)
public GridTunnelSearch(Stack<DefaultAction> plan, Vector3 startPos, float startTime, float idealGoalTime, GridTimeDomain gtd, int MaxNumberOfNodes, float MaxTime, float T_d, float T_t, float startSpeed = 0)
{
currentPlan = new DefaultAction[plan.Count];
plan.CopyTo(currentPlan,0);
this.T_d = T_d;
//this.T_x = T_x;
//this.T_z = T_z;
this.T_t = T_t;
this.maxTime = MaxTime;
int startIndex = 0;
//GridPlanningState startPlanningState = currentPlan[startIndex].state as GridPlanningState;
//while (startPlanningState == null)
//{
// startIndex++;
// startPlanningState = currentPlan[startIndex].state as GridPlanningState;
//}
//startState = new GridTimeState(startPlanningState.currentPosition, startTime, startSpeed);
startState = new GridTimeState(startPos, startTime, startSpeed);
lastState = new GridTimeState((currentPlan[currentPlan.Length-1].state as GridPlanningState).currentPosition, idealGoalTime);
gridTimeDomain = gtd;
float dist = (startState.currentPosition - lastState.currentPosition).magnitude;
// TODO: what to do if there is no time???
float minSpeed = dist / (idealGoalTime-startTime);
if (minSpeed < 0)
minSpeed = GridTimeDomain.MIN_SPEED;
//tunnel = ConvertPlan(gtd.minSpeed, gtd.midSpeed, gtd.maxSpeed);
tunnel = ConvertPlan(startTime, minSpeed, GridTimeDomain.MID_SPEED, GridTimeDomain.MAX_SPEED);
//for(int aux = 0; aux < tunnel.tunnelMinVelocity.Length; aux++)
// Debug.Log("Tunnel "+aux+":"+tunnel.tunnelMinVelocity[aux]);
tunnelPlanner = new BestFirstSearchPlanner();
List<PlanningDomainBase> domainList = new List<PlanningDomainBase>();
domainList.Add(gridTimeDomain);
tunnelPlanner.init(ref domainList, MaxNumberOfNodes);
// MUBBASIR TESTING ARA STAR PLANNER HERE
araStarPlanner = new ARAstarPlanner ();
araStarPlanner.init(ref domainList, MaxNumberOfNodes);
goalReached = false;
ComputeTunnelSearch();
//ComputeTunnelSearchStatePlan ();
}
// Use this for initialization
public override void Init(AnimationAnalyzer animAnalyzer, NeighbourAgents neighborhood, NeighbourObstacles obstacles)
{
analyzer = animAnalyzer;
if (analyzer != null && !analyzer.initialized)
analyzer.Init();
storedPlan = new List<FootstepPlanningAction>();
outputPlan = new Stack<DefaultAction>();
domainList = new List<PlanningDomainBase>();
gpd = new GridPlanningDomain(analyzer);
//gtd = new GridTimeDomain(analyzer, obstacles);
gtd = new GridTimeDomain(obstacles,neighborhood);
fpd = new FootstepPlanningDomain(analyzer, neighborhood, obstacles);
if (useGridDomain)
{
domainList.Add(gpd);
//domainList.Add(gpd);
}
if (useGridTimeDomain)
{
domainList.Add(gtd);
//domainList.Add(gtd);
}
if (useFootstepDomain)
{
domainList.Add(fpd);
//domainList.Add(fpd);
}
planner = new BestFirstSearchPlanner();
planner.init(ref domainList, MaxNumberOfNodes);
initialized = true;
currentState = null;
isPlanComputed = false;
planChanged = false;
goalMeshRenderer = goalStateTransform.gameObject.GetComponent("MeshRenderer") as MeshRenderer;
currentGoal = goalStateTransform.position;
goalReached = false;
}