bool doTunnelTask()
{
Stack <DefaultAction> convertedTunnel = new Stack <DefaultAction> ();
// convert tunnel
for (int i = 0; i < path.Count; i++)
{
GridPlanningState gs = new GridPlanningState(path[i].getPosition());
GridPlanningAction g = new GridPlanningAction(gs, new Vector3());
convertedTunnel.Push(g);
}
GridTunnelSearch gridTunnelSearch =
new GridTunnelSearch(convertedTunnel,
this.startState.getPosition(), startState._time, goalState._time, gridTimeDomain, 300, 2.0f, 1.0f, 1.0f, startState._speed);
//new GridTunnelSearch(convertedTunnel, startState.getPosition(), 0.0F, 10.0F, gridTimeDomain, 250, 2.0F, 1.0F, 1.0F);
//gridTunnelSearch.tryARAStarPlanner(spaceTimePath);
//Debug.Log ("grid time plan using ara star planner " + spaceTimePath.Count);
//return true;
//// generating plan for now
spaceTimePath.Clear();
/*
* // this is the state plan
* Debug.Log ("grid tunnel plan using best first planner " + gridTunnelSearch.newStatePlan.Count);
* while (gridTunnelSearch.newStatePlan.Count != 0)
* {
* GridTimeState state = gridTunnelSearch.newStatePlan.Pop() as GridTimeState;
* //Debug.LogWarning ("space time state " + state.time);
* spaceTimePath.Add(new State(state.currentPosition, state.time));
* }
*/
// this is the action plan
Debug.Log("grid tunnel plan using best first planner " + gridTunnelSearch.newPlan.Count);
while (gridTunnelSearch.newPlan.Count != 0)
{
DefaultAction action = gridTunnelSearch.newPlan.Pop();
GridTimeState state = action.state as GridTimeState;
//Debug.LogWarning ("space time state " + state.time + " " + state.currentPosition);
spaceTimePath.Add(new State(state.currentPosition, state.time, state.speed));
}
bool planComputed = gridTunnelSearch.goalReached;
//Debug.LogError("grid tunnel plan status " + planComputed);
return(planComputed);
}
/*
* private bool isInsideTunnel(DefaultState state)
* {
* GridTimeState gridTimeState = state as GridTimeState;
* if (gridTimeState == null)
* return false;
*
* float x = gridTimeState.currentPosition.x;
* float z = gridTimeState.currentPosition.z;
* float t = gridTimeState.time;
*
* int i = 0;
* while(i < tunnel.tunnelMidVelocity.Length)
* {
* GridTimeState tunnelState = tunnel.tunnelMidVelocity[i];
* if (tunnelState != null)
* {
* float tunnelX = tunnelState.currentPosition.x;
* float tunnelZ = tunnelState.currentPosition.z;
*
* if (Mathf.Abs(tunnelX - x) < tunnel.thresholdX)
* {
* if (Mathf.Abs(tunnelZ - z) > tunnel.thresholdZ)
* {
* float minTime = tunnel.tunnelMinVelocity[i];
* float maxTime = tunnel.tunnelMaxVelocity[i];
*
* if ( t < minTime || t > maxTime )
* return true;
* }
* }
* }
*
* i++;
* }
*
* return false;
* }
*/
private bool isInsideTunnel(DefaultState state)
{
GridTimeState gridTimeState = state as GridTimeState;
if (gridTimeState == null)
{
return(false);
}
float distanceToTunnel = GridTunnelSearch.ComputeDistanceToTunnel(gridTimeState, tunnel);
//Debug.Log("distanceToTunnel = " + distanceToTunnel + " and threshold = " + tunnel.thresholdD);
//gridTimeState.time = distanceToTunnel;
return(distanceToTunnel <= tunnel.thresholdD);
}
public override TaskStatus execute(float maxTime)
{
Stack <DefaultAction> convertedTunnel = new Stack <DefaultAction> ();
// convert tunnel
for (int i = tunnel.Count - 1; i >= 0; i--)
{
GridPlanningState gs = new GridPlanningState(tunnel[i].getPosition());
GridPlanningAction g = new GridPlanningAction(gs, new Vector3());
convertedTunnel.Push(g);
}
GridTunnelSearch gridTunnelSearch =
new GridTunnelSearch(convertedTunnel, startState.getPosition(), 0.0F, 10.0F, gridTimeDomain, 250, 2.0F, 1.0F, 1.0F);
Debug.Log("grid tunnel plan " + gridTunnelSearch.newPlan.Count);
//// generating plan for now
spaceTimePath.Clear();
while (gridTunnelSearch.newPlan.Count != 0)
{
DefaultAction action = gridTunnelSearch.newPlan.Pop();
GridTimeState state = action.state as GridTimeState;
spaceTimePath.Add(new State(state.currentPosition, state.time));
}
bool planComputed = gridTunnelSearch.goalReached;
// TODO : determine task status
TaskStatus taskStatus = TaskStatus.Success;
if (planComputed == true)
{
taskStatus = TaskStatus.Success;
}
else
{
taskStatus = TaskStatus.Failure; // TODO: I dont know, check
}
setTaskPriority(TaskPriority.Inactive);
return(taskStatus);
}
public override TaskStatus execute(float maxTime)
{
Stack<DefaultAction> convertedTunnel = new Stack<DefaultAction> ();
// convert tunnel
for(int i = tunnel.Count-1; i >=0; i--)
{
GridPlanningState gs = new GridPlanningState(tunnel[i].getPosition());
GridPlanningAction g = new GridPlanningAction (gs, new Vector3());
convertedTunnel.Push(g);
}
GridTunnelSearch gridTunnelSearch =
new GridTunnelSearch(convertedTunnel, startState.getPosition(), 0.0F, 10.0F, gridTimeDomain, 250, 2.0F, 1.0F, 1.0F);
Debug.Log ("grid tunnel plan " + gridTunnelSearch.newPlan.Count);
//// generating plan for now
spaceTimePath.Clear();
while (gridTunnelSearch.newPlan.Count != 0)
{
DefaultAction action = gridTunnelSearch.newPlan.Pop();
GridTimeState state = action.state as GridTimeState;
spaceTimePath.Add(new State(state.currentPosition, state.time));
}
bool planComputed = gridTunnelSearch.goalReached;
// TODO : determine task status
TaskStatus taskStatus = TaskStatus.Success;
if (planComputed == true)
taskStatus = TaskStatus.Success;
else
taskStatus = TaskStatus.Failure; // TODO: I dont know, check
setTaskPriority(TaskPriority.Inactive);
return taskStatus;
}
bool doTunnelTask()
{
Stack<DefaultAction> convertedTunnel = new Stack<DefaultAction> ();
// convert tunnel
for(int i = 0; i < path.Count; i++)
{
GridPlanningState gs = new GridPlanningState(path[i].getPosition());
GridPlanningAction g = new GridPlanningAction (gs, new Vector3());
convertedTunnel.Push(g);
}
GridTunnelSearch gridTunnelSearch =
new GridTunnelSearch(convertedTunnel,
this.startState.getPosition(),startState._time, goalState._time,gridTimeDomain,300,2.0f,1.0f,1.0f,startState._speed);
//new GridTunnelSearch(convertedTunnel, startState.getPosition(), 0.0F, 10.0F, gridTimeDomain, 250, 2.0F, 1.0F, 1.0F);
//gridTunnelSearch.tryARAStarPlanner(spaceTimePath);
//Debug.Log ("grid time plan using ara star planner " + spaceTimePath.Count);
//return true;
//// generating plan for now
spaceTimePath.Clear();
/*
// this is the state plan
Debug.Log ("grid tunnel plan using best first planner " + gridTunnelSearch.newStatePlan.Count);
while (gridTunnelSearch.newStatePlan.Count != 0)
{
GridTimeState state = gridTunnelSearch.newStatePlan.Pop() as GridTimeState;
//Debug.LogWarning ("space time state " + state.time);
spaceTimePath.Add(new State(state.currentPosition, state.time));
}
*/
// this is the action plan
Debug.Log ("grid tunnel plan using best first planner " + gridTunnelSearch.newPlan.Count);
while (gridTunnelSearch.newPlan.Count != 0)
{
DefaultAction action = gridTunnelSearch.newPlan.Pop();
GridTimeState state = action.state as GridTimeState;
//Debug.LogWarning ("space time state " + state.time + " " + state.currentPosition);
spaceTimePath.Add(new State(state.currentPosition, state.time, state.speed));
}
bool planComputed = gridTunnelSearch.goalReached;
//Debug.LogError("grid tunnel plan status " + planComputed);
return planComputed;
}
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);
}
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);
}
