// Returns the square distance between the two states of two actions of different domains
public float SqrDistance(GridPlanningAction gridAction, FootstepPlanningAction footstepAction)
{
Vector3 gridPos = (gridAction.state as GridPlanningState).currentPosition;
Vector3 footstepPos = (footstepAction.state as FootstepPlanningState).currentPosition;
return((footstepPos - gridPos).sqrMagnitude);
}
private float[] ComputeNodeDistances(DefaultAction[] plan)
{
float[] distances = new float[plan.Length];
int i = 0;
GridPlanningAction action = null;
GridPlanningState state = null;
while (action == null && i < plan.Length)
{
action = plan[i] as GridPlanningAction;
if (action != null)
{
state = action.state as GridPlanningState;
}
distances[i] = 0;
i++;
}
GridPlanningState nextState;
while (i + 1 < plan.Length)
{
nextState = (plan[i + 1] as GridPlanningAction).state as GridPlanningState;
distances[i + 1] = (state.currentPosition - nextState.currentPosition).magnitude;
state = nextState;
i++;
}
return(distances);
}
public FootstepPlanningAction(GridPlanningAction gridAction, AnotatedAnimation anim, float sp)
{
state = gridAction.state;
cost = gridAction.cost;
animInfo = anim;
speed = sp;
}
public FootstepPlanningAction(GridPlanningAction gridAction, AnotatedAnimation anim, float sp)
{
state = gridAction.state;
cost = gridAction.cost;
animInfo = anim;
speed = sp;
}
// Returns all the FootstepPlanningActions that can be mapped to the GridPlanningAction girdAction
public FootstepPlanningAction[] MappingFromGridAction(GridPlanningAction gridAction,
FootstepPlanningAction prevFootstepAction)
{
FootstepPlanningAction[] mappingActions;
// TODO: all the mapping function using a predefined table that returns the mapping actions
mappingActions = null;
return mappingActions;
}
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);
}
// Returns all the FootstepPlanningActions that can be mapped to the GridPlanningAction girdAction
public FootstepPlanningAction[] MappingFromGridAction(GridPlanningAction gridAction,
FootstepPlanningAction prevFootstepAction)
{
FootstepPlanningAction[] mappingActions;
// TODO: all the mapping function using a predefined table that returns the mapping actions
mappingActions = null;
return(mappingActions);
}
override public void generateTransitions(ref DefaultState DcurrentState, ref DefaultState DpreviousState,
ref DefaultState DidealGoalState, ref List <DefaultAction> transitions)
{
GridPlanningState currentState = DcurrentState as GridPlanningState;
GridPlanningState idealGoalState = DidealGoalState as GridPlanningState;
if (currentState == null)
{
FootstepPlanningState fsState = DcurrentState as FootstepPlanningState;
if (fsState != null)
{
currentState = new GridPlanningState(fsState);
}
else
{
currentState = new GridPlanningState(DcurrentState as GridTimeState);
}
}
if (idealGoalState == null)
{
FootstepPlanningState fsIdealGoalState = DidealGoalState as FootstepPlanningState;
if (fsIdealGoalState != null)
{
idealGoalState = new GridPlanningState(fsIdealGoalState);
}
else
{
idealGoalState = new GridPlanningState(DidealGoalState as GridTimeState);
}
}
foreach (Vector3 mov in movDirections)
{
GridPlanningAction newAction = new GridPlanningAction(currentState, mov);
//if (!CheckTransitionCollisions(newAction.state,DcurrentState))
if (!CheckStateCollisions(newAction.state))
{
//Debug.Log(Time.time + ": grid successor generated");
transitions.Add(newAction);
}
}
//Debug.Log(transitions.Count + " grid transitions generated at time " + Time.time);
}
public override void generatePredecesors(ref DefaultState DcurrentState, ref DefaultState DpreviousState,
ref DefaultState DidealGoalState, ref List <DefaultAction> transitions)
{
GridPlanningState currentState = DcurrentState as GridPlanningState;
GridPlanningState idealGoalState = DidealGoalState as GridPlanningState;
GridPlanningState previousState = DpreviousState as GridPlanningState;
foreach (Vector3 mov in movDirections)
{
GridPlanningAction newAction = new GridPlanningAction(previousState, mov, 0.0f);
if (!CheckStateCollisions(newAction.state))
{
transitions.Add(newAction);
}
}
}
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);
}
// This function searches and extracts the tunnel zone, letting it in tunnelZone
private void ExtractTunnelZone()
{
if (currentPlan == null)
{
return;
}
// We look for the first GridPlanningAction
GridPlanningAction action = currentPlan[0] as GridPlanningAction;;
int i = 0;
while (action == null && i < currentPlan.Length)
{
i++;
action = currentPlan[i] as GridPlanningAction;
}
// if we have found a GridPlanningAction
if (action != null && i < currentPlan.Length)
{
// we copy the first part of the plan (the footsteps)
newPlan = new Stack <DefaultAction>();
for (int fs = 0; fs < i; fs++)
{
newPlan.Push(currentPlan[fs] as FootstepPlanningAction);
}
// we save the lastFootstepAction;
lastFootstepAction = currentPlan[i - 1] as FootstepPlanningAction;
// we compute how many grid actions are in the plan
int numberOfGridActions = currentPlan.Length - (i - 1);
// we create and fill the tunnelZone
tunnelZone = new GridPlanningAction[numberOfGridActions];
int j = 0;
while (i < currentPlan.Length)
{
tunnelZone[j] = currentPlan[i] as GridPlanningAction;
j++;
i++;
}
}
}
public override FootstepPlanningAction getFirstAction()
{
FootstepPlanningAction firstAction = null;
int plannedActions = outputPlan.Count;
int i = 0;
bool isGridAction = false;
while (i < plannedActions && firstAction == null && !isGridAction)
{
DefaultAction defAction = outputPlan.Pop();
firstAction = defAction as FootstepPlanningAction;
if (firstAction == null)
{
GridPlanningAction gridAction = defAction as GridPlanningAction;
if (gridAction != null)
{
outputPlan.Push(defAction);
isGridAction = true;
}
else
{
GridTimeAction gridTimeAction = defAction as GridTimeAction;
if (gridTimeAction != null)
{
outputPlan.Push(defAction);
isGridAction = true;
}
}
}
i++;
}
if (firstAction != null)
{
currentState = firstAction.state as FootstepPlanningState;
}
return(firstAction);
}
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;
}
// Returns the square distance between the two states of two actions of different domains
public float SqrDistance(GridPlanningAction gridAction, FootstepPlanningAction footstepAction)
{
Vector3 gridPos = (gridAction.state as GridPlanningState).currentPosition;
Vector3 footstepPos = (footstepAction.state as FootstepPlanningState).currentPosition;
return (footstepPos - gridPos).sqrMagnitude;
}
// This function search for a new path inside the tunnel
// Hopefully it will return just a path of FootstepPlanningAction
public Stack <DefaultAction> SearchInTunnel()
{
int[] backTrackingIndexes = new int[tunnelZone.Length];
for (int bt = 0; bt < backTrackingIndexes.Length; bt++)
{
backTrackingIndexes[bt] = 0;
}
// For every GridPlanningAction in the tunnel zone
int i = 0;
while (i < tunnelZone.Length && i > 0) // if i < 0 it means we have backtracked and found no solution
{
GridPlanningAction gridAction = tunnelZone[i];
// We have all the mapping actions coming from the GridPlanningAction
FootstepPlanningAction[] mappingActions = MappingFromGridAction(gridAction, lastFootstepAction);
// We look for a mapping action that is valid for our tunnel
int j = backTrackingIndexes[i];
bool found = false;
FootstepPlanningAction footstepAction = null;
while (!found && j < mappingActions.Length)
{
footstepAction = mappingActions[j];
// If it falls inside the tunnel
if (SqrDistance(gridAction, footstepAction) <= SqrW)
{
// TODO: CHECK FOR COLLISIONS
found = true;
}
else
{
j++;
}
}
// If we have found a valid FootstepAction
if (found && footstepAction != null)
{
// we add id to our plan
newPlan.Push(footstepAction);
// we save its mapping index in case we do backtracking later
backTrackingIndexes[i] = j;
// we also save the FootstepAction we have added
lastFootstepAction = footstepAction;
i++;
}
else // if we haven't found a valid action
{
// we do backtrack
newPlan.Pop();
// we retrieve the last footstepAction
lastFootstepAction = newPlan.Pop() as FootstepPlanningAction;
newPlan.Push(lastFootstepAction);
i--;
}
}
return(newPlan);
}
////////////////////////////////////////////////////////////////////////////////////
public override FootstepPlanningAction[] GetOutputPlan()
{
if (outputPlan == null)
{
return(null);
}
FootstepPlanningAction[] actionList = new FootstepPlanningAction[outputPlan.Count];
int i = 0;
int count = 0;
// Debug.Log("OutputPlan: " + outputPlan.Count);
while (i < outputPlan.Count)
{
DefaultAction action = outputPlan.ElementAt(i);
//Debug.Log("we have an action " + i);
FootstepPlanningAction fsAction = action as FootstepPlanningAction;
if (fsAction != null)
{
actionList[count] = fsAction;
count++;
if (REPRODUCTION_MODE.REPRODUCTION_MODE.Equals(Reproduction.MODE.RECORD))
{
storedPlan.Add(fsAction);
//Debug.Log("Recording");
}
}
else
{
GridPlanningAction gridAction = action as GridPlanningAction;
if (gridAction != null)
{
fsAction = new FootstepPlanningAction(gridAction, analyzer.GetAnotatedAnimation("Idle"), 1.5f);
if (fsAction != null)
{
actionList[count] = fsAction;
count++;
if (REPRODUCTION_MODE.REPRODUCTION_MODE.Equals(Reproduction.MODE.RECORD))
{
storedPlan.Add(fsAction);
//Debug.Log("Recording");
}
}
//Debug.Log("OutputPlan at time " + Time.time + " has " + outputPlan.Count + " actions.");
//Debug.Log("Grid Action added at time: " + Time.time + " at pos " + count);
}
else
{
//Debug.Log("We might add a GridTimeAction");
GridTimeAction gridTimeAction = action as GridTimeAction;
if (gridTimeAction != null)
{
if (analyzer != null)
{
fsAction = new FootstepPlanningAction(gridTimeAction, analyzer.GetAnotatedAnimation("Idle"), 1.0f);
}
if (fsAction != null)
{
//Debug.Log("Adding a gridTimeAction");
actionList[count] = fsAction;
count++;
if (REPRODUCTION_MODE.REPRODUCTION_MODE.Equals(Reproduction.MODE.RECORD))
{
storedPlan.Add(fsAction);
//Debug.Log("Recording");
}
}
}
}
}
i++;
}
return(actionList);
}
public override void generatePredecesors(ref DefaultState DcurrentState, ref DefaultState DpreviousState,
ref DefaultState DidealGoalState, ref List<DefaultAction> transitions)
{
GridPlanningState currentState = DcurrentState as GridPlanningState;
GridPlanningState idealGoalState = DidealGoalState as GridPlanningState;
GridPlanningState previousState = DpreviousState as GridPlanningState;
foreach ( Vector3 mov in movDirections )
{
GridPlanningAction newAction = new GridPlanningAction(previousState,mov,0.0f);
if (!CheckStateCollisions(newAction.state))
transitions.Add(newAction);
}
}
public override void generateTransitions(ref DefaultState DcurrentState, ref DefaultState DpreviousState,
ref DefaultState DidealGoalState, ref List<DefaultAction> transitions)
{
GridPlanningState currentState = DcurrentState as GridPlanningState;
GridPlanningState idealGoalState = DidealGoalState as GridPlanningState;
if (currentState == null)
{
FootstepPlanningState fsState = DcurrentState as FootstepPlanningState;
if (fsState != null)
currentState = new GridPlanningState(fsState);
else
currentState = new GridPlanningState(DcurrentState as GridTimeState);
}
if (idealGoalState == null)
{
FootstepPlanningState fsIdealGoalState = DidealGoalState as FootstepPlanningState;
if (fsIdealGoalState != null)
idealGoalState = new GridPlanningState(fsIdealGoalState);
else
idealGoalState = new GridPlanningState(DidealGoalState as GridTimeState);
}
foreach ( Vector3 mov in movDirections )
{
GridPlanningAction newAction = new GridPlanningAction(currentState,mov);
//if (!CheckTransitionCollisions(newAction.state,DcurrentState))
if (!CheckStateCollisions(newAction.state))
{
//Debug.Log(Time.time + ": grid successor generated");
transitions.Add(newAction);
}
}
//Debug.Log(transitions.Count + " grid transitions generated at time " + Time.time);
}
