public FootstepPlanningAction(GridTimeAction gridAction, AnotatedAnimation anim, float sp)
{
state = gridAction.state;
cost = gridAction.cost;
animInfo = anim;
speed = sp;
}
public FootstepPlanningAction(GridTimeAction gridAction, AnotatedAnimation anim, float sp)
{
state = gridAction.state;
cost = gridAction.cost;
animInfo = anim;
speed = sp;
}
public override void generatePredecessors(DefaultState DcurrentState, ref List <DefaultAction> actionList)
{
GridTimeState currentState = DcurrentState as GridTimeState;
GridTimeAction stopAction = new GridTimeAction(currentState, Vector3.zero, 0);
if (!CheckTransitionCollisions(currentState, stopAction.state, false))
{
if (!useTunnel || isInsideTunnel(stopAction.state))
{
actionList.Add(stopAction);
}
}
#if USE_ANGLE_MOV
foreach (float angle in movAngles)
{
Vector3 mov = currentState.actionMov;
mov = Matrix4x4.TRS(Vector3.zero, Quaternion.AngleAxis(angle, Vector3.up), new Vector3(1, 1, 1)) * mov;
mov.Normalize();
#else
foreach (Vector3 mov in movDirections)
{
#endif
foreach (float speedIncr in possibleSpeedIncrements)
{
float newSpeed = currentState.speed + speedIncr;
if (newSpeed > MAX_SPEED)
{
newSpeed = MAX_SPEED;
}
else if (newSpeed < MIN_SPEED)
{
newSpeed = MIN_SPEED;
}
GridTimeState prevState = new GridTimeState();
prevState.ComputePreviousState(currentState, mov, newSpeed);
GridTimeAction newAction = new GridTimeAction(currentState, prevState);
if (!CheckTransitionCollisions(currentState, newAction.state))
{
// If we are not using a tunnel
// or if we are using one and the stata falls inside it
if (!useTunnel || isInsideTunnel(newAction.state))
{
//Debug.Log(Time.time + ": grid successor generated");
actionList.Add(newAction); // we add the action as a transition
}
}
}
}
//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)
{
GridTimeState currentState = DcurrentState as GridTimeState;
GridTimeState idealGoalState = DidealGoalState as GridTimeState;
GridTimeState previousState = DpreviousState as GridTimeState;
GridTimeAction stopAction = new GridTimeAction(currentState, Vector3.zero, 0);
transitions.Add(stopAction);
#if USE_ANGLE_MOV
foreach (float angle in movAngles)
{
Vector3 mov = currentState.actionMov;
mov = Matrix4x4.TRS(Vector3.zero, Quaternion.AngleAxis(angle, Vector3.up), new Vector3(1, 1, 1)) * mov;
mov.Normalize();
#else
foreach (Vector3 mov in movDirections)
{
#endif
foreach (float speedIncr in possibleSpeedIncrements)
{
float newSpeed = previousState.speed - speedIncr;
if (newSpeed > MAX_SPEED)
{
newSpeed = MAX_SPEED;
}
else if (newSpeed < MIN_SPEED)
{
newSpeed = MIN_SPEED;
}
GridTimeAction newAction = new GridTimeAction(previousState, mov, newSpeed, 0.0f);
if (!CheckTransitionCollisions(currentState, newAction.state))
{
transitions.Add(newAction);
}
}
}
}
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 bool HasExpired()
{
bool expired = false;
GridTimeAction lastAction = outputPlan.Last() as GridTimeAction;
if (lastAction != null)
{
GridTimeState lastState = lastAction.state as GridTimeState;
if (lastState != null)
{
float lastTime = lastState.time;
if (Time.time > lastTime)
{
expired = true;
}
}
}
else
{
FootstepPlanningAction lastFsAction = outputPlan.Last() as FootstepPlanningAction;
if (lastFsAction != null)
{
FootstepPlanningState lastFsState = lastFsAction.state as FootstepPlanningState;
if (lastFsState != null)
{
float lastTime = lastFsState.time;
if (Time.time > lastTime)
{
expired = true;
}
}
}
}
return(expired);
}
public override void generatePredecesors (ref DefaultState DcurrentState, ref DefaultState DpreviousState,
ref DefaultState DidealGoalState, ref List<DefaultAction> transitions)
{
GridTimeState currentState = DcurrentState as GridTimeState;
GridTimeState idealGoalState = DidealGoalState as GridTimeState;
GridTimeState previousState = DpreviousState as GridTimeState;
GridTimeAction stopAction = new GridTimeAction(currentState,Vector3.zero,0);
transitions.Add(stopAction);
#if USE_ANGLE_MOV
foreach( float angle in movAngles)
{
Vector3 mov = currentState.actionMov;
mov = Matrix4x4.TRS(Vector3.zero, Quaternion.AngleAxis(angle,Vector3.up),new Vector3(1,1,1)) * mov;
mov.Normalize();
#else
foreach ( Vector3 mov in movDirections )
{
#endif
foreach ( float speedIncr in possibleSpeedIncrements )
{
float newSpeed = previousState.speed - speedIncr;
if (newSpeed > MAX_SPEED)
newSpeed = MAX_SPEED;
else if (newSpeed < MIN_SPEED)
newSpeed = MIN_SPEED;
GridTimeAction newAction = new GridTimeAction(previousState,mov,newSpeed,0.0f);
if (!CheckTransitionCollisions(currentState, newAction.state))
transitions.Add(newAction);
}
}
}
override public void generateTransitions(ref DefaultState DcurrentState, ref DefaultState DpreviousState,
ref DefaultState DidealGoalState, ref List<DefaultAction> transitions)
{
GridTimeState currentState = DcurrentState as GridTimeState;
GridTimeState idealGoalState = DidealGoalState as GridTimeState;
/*
if (currentState == null)
currentState = new GridTimeState(DcurrentState as FootstepPlanningState);
if (idealGoalState == null)
idealGoalState = new GridTimeState(DidealGoalState as FootstepPlanningState);
*/
GridTimeAction stopAction = new GridTimeAction(currentState,Vector3.zero,0);
if (useTimeConstraint)
{
GridTimeState stopState = stopAction.state as GridTimeState;
Vector3 toGoal = idealGoalState.currentPosition - stopState.currentPosition;
float availableTime = idealGoalState.time - stopState.time;// + timeWindow;
float necessarySpeed = toGoal.magnitude / availableTime;
float timeCost = necessarySpeed - stopState.speed;
bool negative = timeCost < 0;
if (negative)
timeCost = 0;
else
timeCost *= timeCost;
stopAction.cost += timeCost;
}
if (!CheckTransitionCollisions(currentState, stopAction.state, false))
{
if (!useTunnel || isInsideTunnel(stopAction.state) )
transitions.Add(stopAction);
}
#if USE_ANGLE_MOV
foreach( float angle in movAngles)
{
Vector3 mov = currentState.actionMov;
mov = Matrix4x4.TRS(Vector3.zero, Quaternion.AngleAxis(angle,Vector3.up),new Vector3(1,1,1)) * mov;
mov.Normalize();
#else
foreach ( Vector3 mov in movDirections )
{
#endif
foreach( float speedIncr in possibleSpeedIncrements )
{
float newSpeed = currentState.speed + speedIncr;
if (newSpeed > MAX_SPEED)
newSpeed = MAX_SPEED;
else if (newSpeed < MIN_SPEED)
newSpeed = MIN_SPEED;
GridTimeAction newAction = new GridTimeAction(currentState,mov,newSpeed);
if (useTimeConstraint)
{
GridTimeState newState = newAction.state as GridTimeState;
Vector3 toGoal = idealGoalState.currentPosition - newState.currentPosition;
float availableTime = idealGoalState.time - newState.time; // + timeWindow;
float necessarySpeed = toGoal.magnitude / availableTime;
float timeCost = necessarySpeed - newState.speed;
bool negative = timeCost < 0;
if (negative)
timeCost = 0;
else
timeCost *= timeCost;
newAction.cost += timeCost;
}
if (!CheckTransitionCollisions(currentState, newAction.state))
{
// If we are not using a tunnel
// or if we are using one and the stata falls inside it
if (!useTunnel || isInsideTunnel(newAction.state) )
{
//Debug.Log(Time.time + ": grid successor generated");
transitions.Add(newAction); // we add the action as a transition
}
}
}
}
//Debug.Log(transitions.Count + " grid transitions generated at time " + Time.time);
}
public override void generatePredecessors(DefaultState DcurrentState, ref List<DefaultAction> actionList)
{
GridTimeState currentState = DcurrentState as GridTimeState;
GridTimeAction stopAction = new GridTimeAction(currentState,Vector3.zero,0);
if (!CheckTransitionCollisions(currentState, stopAction.state, false))
{
if (!useTunnel || isInsideTunnel(stopAction.state) )
actionList.Add(stopAction);
}
#if USE_ANGLE_MOV
foreach( float angle in movAngles)
{
Vector3 mov = currentState.actionMov;
mov = Matrix4x4.TRS(Vector3.zero, Quaternion.AngleAxis(angle,Vector3.up),new Vector3(1,1,1)) * mov;
mov.Normalize();
#else
foreach ( Vector3 mov in movDirections )
{
#endif
foreach( float speedIncr in possibleSpeedIncrements )
{
float newSpeed = currentState.speed + speedIncr;
if (newSpeed > MAX_SPEED)
newSpeed = MAX_SPEED;
else if (newSpeed < MIN_SPEED)
newSpeed = MIN_SPEED;
GridTimeState prevState = new GridTimeState();
prevState.ComputePreviousState(currentState,mov,newSpeed);
GridTimeAction newAction = new GridTimeAction(currentState,prevState);
if (!CheckTransitionCollisions(currentState, newAction.state))
{
// If we are not using a tunnel
// or if we are using one and the stata falls inside it
if (!useTunnel || isInsideTunnel(newAction.state) )
{
//Debug.Log(Time.time + ": grid successor generated");
actionList.Add(newAction); // we add the action as a transition
}
}
}
}
//Debug.Log(transitions.Count + " grid transitions generated at time " + Time.time);
}
override public void generateTransitions(ref DefaultState DcurrentState, ref DefaultState DpreviousState,
ref DefaultState DidealGoalState, ref List <DefaultAction> transitions)
{
GridTimeState currentState = DcurrentState as GridTimeState;
GridTimeState idealGoalState = DidealGoalState as GridTimeState;
/*
* if (currentState == null)
* currentState = new GridTimeState(DcurrentState as FootstepPlanningState);
*
* if (idealGoalState == null)
* idealGoalState = new GridTimeState(DidealGoalState as FootstepPlanningState);
*/
GridTimeAction stopAction = new GridTimeAction(currentState, Vector3.zero, 0);
if (useTimeConstraint)
{
GridTimeState stopState = stopAction.state as GridTimeState;
Vector3 toGoal = idealGoalState.currentPosition - stopState.currentPosition;
float availableTime = idealGoalState.time - stopState.time; // + timeWindow;
float necessarySpeed = toGoal.magnitude / availableTime;
float timeCost = necessarySpeed - stopState.speed;
bool negative = timeCost < 0;
if (negative)
{
timeCost = 0;
}
else
{
timeCost *= timeCost;
}
stopAction.cost += timeCost;
}
if (!CheckTransitionCollisions(currentState, stopAction.state, false))
{
if (!useTunnel || isInsideTunnel(stopAction.state))
{
transitions.Add(stopAction);
}
}
#if USE_ANGLE_MOV
foreach (float angle in movAngles)
{
Vector3 mov = currentState.actionMov;
mov = Matrix4x4.TRS(Vector3.zero, Quaternion.AngleAxis(angle, Vector3.up), new Vector3(1, 1, 1)) * mov;
mov.Normalize();
#else
foreach (Vector3 mov in movDirections)
{
#endif
foreach (float speedIncr in possibleSpeedIncrements)
{
float newSpeed = currentState.speed + speedIncr;
if (newSpeed > MAX_SPEED)
{
newSpeed = MAX_SPEED;
}
else if (newSpeed < MIN_SPEED)
{
newSpeed = MIN_SPEED;
}
GridTimeAction newAction = new GridTimeAction(currentState, mov, newSpeed);
if (useTimeConstraint)
{
GridTimeState newState = newAction.state as GridTimeState;
Vector3 toGoal = idealGoalState.currentPosition - newState.currentPosition;
float availableTime = idealGoalState.time - newState.time; // + timeWindow;
float necessarySpeed = toGoal.magnitude / availableTime;
float timeCost = necessarySpeed - newState.speed;
bool negative = timeCost < 0;
if (negative)
{
timeCost = 0;
}
else
{
timeCost *= timeCost;
}
newAction.cost += timeCost;
}
if (!CheckTransitionCollisions(currentState, newAction.state))
{
// If we are not using a tunnel
// or if we are using one and the stata falls inside it
if (!useTunnel || isInsideTunnel(newAction.state))
{
//Debug.Log(Time.time + ": grid successor generated");
transitions.Add(newAction); // we add the action as a transition
}
}
}
}
//Debug.Log(transitions.Count + " grid transitions generated at time " + Time.time);
}
////////////////////////////////////////////////////////////////////////////////////
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);
}
