public virtual FootstepPlanningAction[] GetOutputPlan()
{
FootstepPlanningAction[] a = new FootstepPlanningAction[1] {
default(FootstepPlanningAction)
};
return(a);
}
// 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);
}
// 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;
}
public override void generatePredecesors(ref DefaultState DcurrentState, ref DefaultState DpreviousState,
ref DefaultState DidealGoalState, ref List <DefaultAction> transitions)
{
FootstepPlanningState currentState = DcurrentState as FootstepPlanningState;
FootstepPlanningState idealGoalState = DidealGoalState as FootstepPlanningState;
FootstepPlanningState previousState = DpreviousState as FootstepPlanningState;
float timeLeft = idealGoalState.time - currentState.time;
float timeWindow = window * analyzer.maxActionDuration;
// If there is no time left
if (timeLeft + timeWindow < 0)
{
//We don't generate transitions
return;
}
AnotatedAnimation preconditions = currentState.preconditions;
float meanStepSize = analyzer.meanStepSize;
foreach (AnotatedAnimation anim in analyzer.analyzedAnimations.Values)
{
float minAnimSpeed = 0.0f;
float maxAnimSpeed = 1.0f;
float animSpeedIncr = 0.1f;
minAnimSpeed = 1.0f; maxAnimSpeed = 1.0f;
FootstepPlanningAction newAction = new FootstepPlanningAction(previousState, anim, minAnimSpeed, meanStepSize, 0.0f);
if (newAction.state != null)
{
if (newAction.SatisfiesPreconditions(preconditions))
{
if (!CheckStateCollisions(newAction.state))
{
transitions.Add(newAction);
}
// and we generate the other actions using the same animation but at different speeds
for (float animSpeed = minAnimSpeed + animSpeedIncr; animSpeed <= maxAnimSpeed; animSpeed += animSpeedIncr)
{
newAction = new FootstepPlanningAction(previousState, anim, animSpeed, meanStepSize, 0.0f);
if (!CheckStateCollisions(newAction.state))
{
transitions.Add(newAction);
}
}
}
}
}
}
// 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);
}
//public void Init(AnimationAnalyzer animAnalyzer, FootstepPlanningTest planner, RootMotionComputer computer, NeighbourAgents agents, NeighbourObstacles obstacles){
public void Init(AnimationAnalyzer animAnalyzer, Planner planner, NeighbourAgents agents, NeighbourObstacles obstacles)
{
analyzer = animAnalyzer;
if (analyzer != null && !analyzer.initialized)
{
analyzer.Init();
}
planning = planner;
if (planning != null && !planning.initialized)
{
planning.Init(analyzer, agents, obstacles);
}
foreach (AnimationState state in animation)
{
state.enabled = true;
state.speed = 1.0f;
state.wrapMode = WrapMode.Loop;
state.weight = 0;
}
currentAnimation = null;
action = null;
animation.Stop();
initialized = true;
actionNum = 0;
changed = false;
errorT = new Vector3(0, 0, 0);
errorR = 0;
currentBlendingTime = 0;
currentActionTime = 0;
previousBlendingTime = 0;
currentActionEndTime = 0;
actionsSinceLastPlan = 0;
insertedAction = false;
initGameObjectPos = transform.position;
initGameObjectRotY = transform.eulerAngles.y;
initLocalRootPos = root.localPosition;
lastRootPos = root.position;
lastRootRot = root.rotation;
}
public void React()
{
FootstepPlanningState frameState = CreateFrameState();
animation.Stop();
AnotatedAnimation stopAnim = analyzer.GetAnotatedAnimation("Idle");
FootstepPlanningAction stopAction = new FootstepPlanningAction(frameState, stopAnim, 1, analyzer.meanStepSize, analyzer.mass);
engine.InsertAction(stopAction);
reacting = true;
}
public void PlaceFootStep()
{
if (engine == null || engine.actionNum < 1)
{
return;
}
FootstepPlanningAction currentAction = engine.action;
if (currentAction != null)
{
AnotatedAnimation animInfo = currentAction.animInfo;
if (animInfo != null)
{
Vector3 translation = new Vector3(0, 0, 0);
//Quaternion quat = transform.rotation;
Quaternion quat = Quaternion.Euler(new Vector3(0, transform.eulerAngles.y, 0));
int endSample = animInfo.LeftFoot.Length - 1;
if (animInfo.swing == Joint.LeftFoot)
{
translation = animInfo.LeftFoot[endSample].position;
}
else
{
translation = animInfo.RightFoot[endSample].position;
};
Vector3 newFootStepPos = /*engine.lastRootPos*/ transform.position + quat * translation;
newFootStepPos[1] = auxHeight;
counter = (counter) % numberOfFootSteps;
GameObject.Destroy(footSteps[counter]);
footSteps[counter] = GameObject.Instantiate((Object)redFootStep, newFootStepPos, quat);
FootstepPlanningState state = currentAction.state as FootstepPlanningState;
lastPos = state.currentPosition;
lastObstacle = state.obstaclePos;
counter++;
}
}
}
// 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 FootstepPlanningAction getFirstAction()
{
FootstepPlanningAction firstAction = null;
int i = 0;
Debug.Log("OUTPUT PLAN: " + outputPlan.Count);
while (i < outputPlan.Count && firstAction == null)
{
firstAction = outputPlan.Pop() as FootstepPlanningAction;
i++;
}
if (firstAction != null)
{
currentState = firstAction.state as FootstepPlanningState;
}
return(firstAction);
}
public void InsertAction(FootstepPlanningAction newAction)
{
currentAnimation.time = currentActionTime;
action = newAction;
insertedAction = true;
float lastHeight = initGameObjectPos.y;
initGameObjectPos = transform.position;
initGameObjectPos.y = lastHeight;
initGameObjectRotY = transform.eulerAngles.y;
//Debug.Log("Before = " + planning.currentState.currentPosition);
planning.currentState = action.state as FootstepPlanningState;
planning.currentState.currentPosition.y = lastHeight;
//Debug.Log("After = " + planning.currentState.currentPosition);
}
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 FootstepPlanningAction[] GetOutputPlan()
{
FootstepPlanningAction[] actionList = new FootstepPlanningAction[outputPlan.Count];
int i = 0;
int count = 0;
while (i < outputPlan.Count)
{
FootstepPlanningAction fsAction = outputPlan.ElementAt(i) as FootstepPlanningAction;
if (fsAction != null)
{
actionList[count] = fsAction;
count++;
if (REPRODUCTION_MODE.REPRODUCTION_MODE.Equals(Reproduction.MODE.RECORD))
{
storedPlan.Add(fsAction);
//Debug.Log("Recording");
}
}
i++;
}
return(actionList);
}
// 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++;
}
}
}
// 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;
}
// Collisions check with other agents
public bool CheckAgentsCollisions(FootstepPlanningState state)
{
if (neighborhood == null)
{
return(false);
}
if (neighborhood.neighbors == null)
{
return(false);
}
Dictionary <int, Neighbor> neighbors = neighborhood.neighbors;
if (neighbors.Count <= 0)
{
return(false);
}
//if (neighbors.Count > 0)
// Debug.Log("There are neighbors");
//else
// Debug.Log("There are no neighbors");
foreach (KeyValuePair <int, Neighbor> pair in neighbors)
{
if (pair.Value.planning != null && pair.Value.triggers == 2)
{
FootstepPlanningAction[] plan = pair.Value.planning.GetOutputPlan();
if (plan.Length > 0 && plan[0] != null)
{
int i = 0;
FootstepPlanningAction action = plan[i];
FootstepPlanningState prevNeighborState = action.state as FootstepPlanningState;
while (prevNeighborState == null && i < plan.Length)
{
action = plan[i];
prevNeighborState = action.state as FootstepPlanningState;
i++;
}
bool thereIsTime = true;
while (i < plan.Length && plan[i] != null && action.state != null && thereIsTime)
{
FootstepPlanningState footstepState = action.state as FootstepPlanningState;
if (footstepState != null && footstepState.time < state.time)
{
action = plan[i];
i++;
}
else
{
thereIsTime = false;
}
}
FootstepPlanningState neighborState = prevNeighborState;
if (action.state != null)
{
neighborState = action.state as FootstepPlanningState;
}
if (state != null && prevNeighborState != null && neighborState != null)
{
return(CheckAgentRootCollisions(state, prevNeighborState, neighborState));
}
}
else
{
FootstepPlanningState neighborState = pair.Value.planning.currentState;
return(CheckAgentRootCollisions(state, null, neighborState));
}
}
}
return(false);
}
public void AnimationEngineUpdate()
{
//void Update(){
if (!initialized)
{
return;
}
//if (currentAnimation != null)
// Debug.Log("currentAnimationTime: " + currentAnimation.time);
if (
currentAnimation == null ||
Mathf.Abs(currentAnimation.time) >= currentActionTime
//|| Time.time >= currentActionEndTime
|| !currentAnimation.enabled
)
{
// Get new animation
if (!insertedAction)
{
action = planning.getFirstAction();
}
if (action != null)
{
string animationName = action.animInfo.name;
/*
* if (blending)
* {
* // We blend out the previous animation
* // if it exists and it is not the first one
* if (currentAnimation != null)
* {
* if (actionNum > 0)
* animation.Blend(currentAnimation.name,0.0f,currentBlendingTime);
* else
* {
* animation.Stop();
* animation.Sample();
* }
* }
* }
*/
// We set up the speed of the new animation
AnimationState newAnimationState = animation[animationName];
newAnimationState.speed = action.speed;
// We set the animation at the beginning
if (currentAnimation == null || planning.goalReached)
{
newAnimationState.time = 0;
}
else
{
//newAnimationState.time = currentAnimation.time - currentActionTime;
newAnimationState.time = Time.time - currentActionEndTime;
}
newAnimationState.enabled = true;
// We save the new animation
currentAnimation = newAnimationState;
// We get the info of the new animation
AnotatedAnimation animInfo = action.animInfo;
// We compute the time of the action and the time of the blending
float newActionTime = animInfo.time * animInfo.totalLength;
float newBlendingTime = animInfo.totalLength * animInfo.footPlantLenght * action.speed;
if (animInfo.type != LocomotionMode.Walk)
{
newBlendingTime = 0.5f;
}
currentActionTime = newActionTime;
if (previousBlendingTime < newBlendingTime)
{
currentBlendingTime = previousBlendingTime;
}
else
{
currentBlendingTime = newBlendingTime;
}
previousBlendingTime = newBlendingTime;
if (blending)
{
/*
* //We play/blend in the new animation if it's not the first one
* if (actionNum > 0)
* animation.Blend(currentAnimation.name,1.0f,currentBlendingTime);
* else
* {
* animation.Play(currentAnimation.name);
* animation.Sample();
* }
*/
//Debug.Log("blending time = " + currentBlendingTime);
if (insertedAction)
{
currentBlendingTime = 1.5f;
}
//animation.CrossFade(currentAnimation.name,currentBlendingTime);
animation.CrossFade(currentAnimation.name, 0.5f);
}
else
{
animation.Play(currentAnimation.name);
}
changed = true;
insertedAction = false;
if (action != null && action.state != null)
{
currentActionEndTime = (action.state as FootstepPlanningState).time;
}
float startTime = Time.time;
/*
* Debug.Log("anim starts at time: " + startTime);
* Debug.Log("offset: " + newAnimationState.time);
* float length = animInfo.time * animInfo.totalLength * action.speed - newAnimationState.time;
* float endTime = startTime + length;
* Debug.Log("action length: " + length);
* Debug.Log("predicted end time: " + endTime);
* Debug.Log("state time: " + currentActionEndTime);
*/
}
else
{
//if (!blending)
animation.Stop();
//else if (currentAnimation != null)
// animation.Blend(currentAnimation.name,0.0f,currentBlendingTime);
changed = false;
planning.goalReached = true;
//Debug.Log("goalReached at time " + Time.time);
}
}
else
{
changed = false;
}
}
override public void generateTransitions(ref DefaultState DcurrentState, ref DefaultState DpreviousState,
ref DefaultState DidealGoalState, ref List <DefaultAction> transitions)
{
FootstepPlanningState currentState = DcurrentState as FootstepPlanningState;
// FootstepPlanningState previousState = DpreviousState as FootstepPlanningState;
FootstepPlanningState idealGoalState = DidealGoalState as FootstepPlanningState;
float timeLeft = idealGoalState.time - currentState.time;
float timeWindow = window * analyzer.maxActionDuration;
// If there is no time left
if (timeLeft + timeWindow < 0)
{
//We don't generate transitions
return;
}
//Debug.Log("Calling my generation");
// The next action preconditions
AnotatedAnimation preconditions = currentState.preconditions;
float meanStepSize = analyzer.meanStepSize;
float mass = analyzer.mass;
// The added transitions
//int count = 0;
/*
* //string[] names = {"WalkRightSlow","WalkRightSlow2","WalkNormal","WalkNormal2","WalkFast","WalkFast2","WalkLeft","WalkLeft2","WalkRight","WalkRight2","LeftStep","RightStep"};
* string[] names = {
* "WalkFast","WalkFast2","WalkNormal","WalkNormal2","WalkLeft","WalkLeft2","WalkRight","WalkRight2","LeftStep"
* ,"RightStep","WalkLeftSlow","WalkLeftSlow2","WalkRightSlow","WalkRightSlow2"
* ,"RightStepSlow","LeftStepSlow","WalkSlow","WalkSlow2"
* ,"TurnRight360","TurnLeft360"
* ,"WalkTurnLeft","WalkTurnLeft2"
* "WalkTurnRight","WalkTurnRight2"
* ,"WalkSlowest","WalkSlowest2"
* ,"Idle"
* };
*
* AnotatedAnimation[] anims = new AnotatedAnimation[names.Length];
* int aux=0;
* foreach (string name in names)
* {
* anims[aux] = analyzer.GetAnotatedAnimation(name);
* aux++;
* }
*/
// For each possible animation
foreach (AnotatedAnimation anim in analyzer.analyzedAnimations.Values)
//foreach (AnotatedAnimation anim in anims)
{
// We compute the min and max possible speed of the animation
// (depending on the previous state, the current state and the velocity of the new animation)
float minAnimSpeed = 0.0f;
float maxAnimSpeed = 1.0f;
//ComputeMinAndMaxAnimSpeeds(currentState,previousState,anim.speed,ref minAnimSpeed,ref maxAnimSpeed);
float animSpeedIncr = 0.1f;
//minAnimSpeed = 0.2f; maxAnimSpeed = 1.0f; animSpeedIncr = 0.2f;
minAnimSpeed = 1.0f; maxAnimSpeed = 1.0f;
//minAnimSpeed = 0.5f; maxAnimSpeed = 0.5f;
//int animCount = 0;
//Debug.Log("Transitions: currentState.time = "+currentState.time+" realTime = "+Time.realtimeSinceStartup);
// we check the preconditions of that action
FootstepPlanningAction newAction = new FootstepPlanningAction(currentState, anim, minAnimSpeed, meanStepSize, mass);
if (newAction.state != null)
{
if (newAction.SatisfiesPreconditions(preconditions))
{
if (!CheckStateCollisions(newAction.state))
{
transitions.Add(newAction);
}
//count++;
//animCount++;
// and we generate the other actions using the same animation but at different speeds
for (float animSpeed = minAnimSpeed + animSpeedIncr; animSpeed <= maxAnimSpeed; animSpeed += animSpeedIncr)
{
newAction = new FootstepPlanningAction(currentState, anim, animSpeed, meanStepSize, mass);
//transitions.Insert(count,newAction);
if (!CheckStateCollisions(newAction.state))
{
transitions.Add(newAction);
}
//count++;
//animCount++;
}
}
}
//Debug.Log("Animation " + anim.name + " transitions = " + animCount);
//if (transitions.Count == 0)
// Debug.Log("No transitions!");
}
}
public void React()
{
FootstepPlanningState frameState = CreateFrameState();
animation.Stop();
AnotatedAnimation stopAnim = analyzer.GetAnotatedAnimation("Idle");
FootstepPlanningAction stopAction = new FootstepPlanningAction(frameState,stopAnim,1,analyzer.meanStepSize, analyzer.mass);
engine.InsertAction(stopAction);
reacting = true;
}
////////////////////////////////////////////////////////////////////////////////////
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)
{
FootstepPlanningState currentState = DcurrentState as FootstepPlanningState;
FootstepPlanningState idealGoalState = DidealGoalState as FootstepPlanningState;
FootstepPlanningState previousState = DpreviousState as FootstepPlanningState;
float timeLeft = idealGoalState.time - currentState.time;
float timeWindow = window*analyzer.maxActionDuration;
// If there is no time left
if ( timeLeft + timeWindow < 0 )
{
//We don't generate transitions
return;
}
AnotatedAnimation preconditions = currentState.preconditions;
float meanStepSize = analyzer.meanStepSize;
foreach( AnotatedAnimation anim in analyzer.analyzedAnimations.Values)
{
float minAnimSpeed = 0.0f;
float maxAnimSpeed = 1.0f;
float animSpeedIncr = 0.1f;
minAnimSpeed = 1.0f; maxAnimSpeed = 1.0f;
FootstepPlanningAction newAction = new FootstepPlanningAction(previousState,anim,minAnimSpeed,meanStepSize, 0.0f);
if (newAction.state != null)
{
if ( newAction.SatisfiesPreconditions(preconditions) )
{
if (!CheckStateCollisions(newAction.state))
transitions.Add(newAction);
// and we generate the other actions using the same animation but at different speeds
for (float animSpeed = minAnimSpeed + animSpeedIncr; animSpeed <= maxAnimSpeed; animSpeed += animSpeedIncr)
{
newAction = new FootstepPlanningAction(previousState,anim,animSpeed,meanStepSize, 0.0f);
if (!CheckStateCollisions(newAction.state))
transitions.Add(newAction);
}
}
}
}
}
////////////////////////////////////////////////////////////////////////////////////
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 generateTransitions(ref DefaultState DcurrentState, ref DefaultState DpreviousState,
ref DefaultState DidealGoalState, ref List<DefaultAction> transitions)
{
FootstepPlanningState currentState = DcurrentState as FootstepPlanningState;
// FootstepPlanningState previousState = DpreviousState as FootstepPlanningState;
FootstepPlanningState idealGoalState = DidealGoalState as FootstepPlanningState;
float timeLeft = idealGoalState.time - currentState.time;
float timeWindow = window*analyzer.maxActionDuration;
// If there is no time left
if ( timeLeft + timeWindow < 0 )
{
//We don't generate transitions
return;
}
//Debug.Log("Calling my generation");
// The next action preconditions
AnotatedAnimation preconditions = currentState.preconditions;
float meanStepSize = analyzer.meanStepSize;
float mass = analyzer.mass;
// The added transitions
//int count = 0;
/*
//string[] names = {"WalkRightSlow","WalkRightSlow2","WalkNormal","WalkNormal2","WalkFast","WalkFast2","WalkLeft","WalkLeft2","WalkRight","WalkRight2","LeftStep","RightStep"};
string[] names = {
"WalkFast","WalkFast2","WalkNormal","WalkNormal2","WalkLeft","WalkLeft2","WalkRight","WalkRight2","LeftStep"
,"RightStep","WalkLeftSlow","WalkLeftSlow2","WalkRightSlow","WalkRightSlow2"
,"RightStepSlow","LeftStepSlow","WalkSlow","WalkSlow2"
,"TurnRight360","TurnLeft360"
,"WalkTurnLeft","WalkTurnLeft2"
"WalkTurnRight","WalkTurnRight2"
,"WalkSlowest","WalkSlowest2"
,"Idle"
};
AnotatedAnimation[] anims = new AnotatedAnimation[names.Length];
int aux=0;
foreach (string name in names)
{
anims[aux] = analyzer.GetAnotatedAnimation(name);
aux++;
}
*/
// For each possible animation
foreach( AnotatedAnimation anim in analyzer.analyzedAnimations.Values)
//foreach (AnotatedAnimation anim in anims)
{
// We compute the min and max possible speed of the animation
// (depending on the previous state, the current state and the velocity of the new animation)
float minAnimSpeed = 0.0f;
float maxAnimSpeed = 1.0f;
//ComputeMinAndMaxAnimSpeeds(currentState,previousState,anim.speed,ref minAnimSpeed,ref maxAnimSpeed);
float animSpeedIncr = 0.1f;
//minAnimSpeed = 0.2f; maxAnimSpeed = 1.0f; animSpeedIncr = 0.2f;
minAnimSpeed = 1.0f; maxAnimSpeed = 1.0f;
//minAnimSpeed = 0.5f; maxAnimSpeed = 0.5f;
//int animCount = 0;
//Debug.Log("Transitions: currentState.time = "+currentState.time+" realTime = "+Time.realtimeSinceStartup);
// we check the preconditions of that action
FootstepPlanningAction newAction = new FootstepPlanningAction(currentState,anim,minAnimSpeed,meanStepSize,mass);
if (newAction.state != null)
{
if ( newAction.SatisfiesPreconditions(preconditions) )
{
if (!CheckStateCollisions(newAction.state))
transitions.Add(newAction);
//count++;
//animCount++;
// and we generate the other actions using the same animation but at different speeds
for (float animSpeed = minAnimSpeed + animSpeedIncr; animSpeed <= maxAnimSpeed; animSpeed += animSpeedIncr)
{
newAction = new FootstepPlanningAction(currentState,anim,animSpeed,meanStepSize,mass);
//transitions.Insert(count,newAction);
if (!CheckStateCollisions(newAction.state))
transitions.Add(newAction);
//count++;
//animCount++;
}
}
}
//Debug.Log("Animation " + anim.name + " transitions = " + animCount);
//if (transitions.Count == 0)
// Debug.Log("No transitions!");
}
}
public void AnimationEngineUpdate()
{
//void Update(){
if (!initialized)
return;
//if (currentAnimation != null)
// Debug.Log("currentAnimationTime: " + currentAnimation.time);
if (
currentAnimation == null
|| Mathf.Abs(currentAnimation.time) >= currentActionTime
//|| Time.time >= currentActionEndTime
|| !currentAnimation.enabled
)
{
// Get new animation
if (!insertedAction)
action = planning.getFirstAction();
if (action != null)
{
string animationName = action.animInfo.name;
/*
if (blending)
{
// We blend out the previous animation
// if it exists and it is not the first one
if (currentAnimation != null)
{
if (actionNum > 0)
animation.Blend(currentAnimation.name,0.0f,currentBlendingTime);
else
{
animation.Stop();
animation.Sample();
}
}
}
*/
// We set up the speed of the new animation
AnimationState newAnimationState = animation[animationName];
newAnimationState.speed = action.speed;
// We set the animation at the beginning
if (currentAnimation == null || planning.goalReached)
newAnimationState.time = 0;
else
//newAnimationState.time = currentAnimation.time - currentActionTime;
newAnimationState.time = Time.time - currentActionEndTime;
newAnimationState.enabled = true;
// We save the new animation
currentAnimation = newAnimationState;
// We get the info of the new animation
AnotatedAnimation animInfo = action.animInfo;
// We compute the time of the action and the time of the blending
float newActionTime = animInfo.time * animInfo.totalLength;
float newBlendingTime = animInfo.totalLength * animInfo.footPlantLenght * action.speed;
if (animInfo.type != LocomotionMode.Walk)
newBlendingTime = 0.5f;
currentActionTime = newActionTime;
if (previousBlendingTime < newBlendingTime)
currentBlendingTime = previousBlendingTime;
else
currentBlendingTime = newBlendingTime;
previousBlendingTime = newBlendingTime;
if (blending)
{
/*
//We play/blend in the new animation if it's not the first one
if (actionNum > 0)
animation.Blend(currentAnimation.name,1.0f,currentBlendingTime);
else
{
animation.Play(currentAnimation.name);
animation.Sample();
}
*/
//Debug.Log("blending time = " + currentBlendingTime);
if (insertedAction)
currentBlendingTime = 1.5f;
//animation.CrossFade(currentAnimation.name,currentBlendingTime);
animation.CrossFade(currentAnimation.name,0.5f);
}
else
animation.Play(currentAnimation.name);
changed = true;
insertedAction = false;
if (action != null && action.state != null)
currentActionEndTime = (action.state as FootstepPlanningState).time;
float startTime = Time.time;
/*
Debug.Log("anim starts at time: " + startTime);
Debug.Log("offset: " + newAnimationState.time);
float length = animInfo.time * animInfo.totalLength * action.speed - newAnimationState.time;
float endTime = startTime + length;
Debug.Log("action length: " + length);
Debug.Log("predicted end time: " + endTime);
Debug.Log("state time: " + currentActionEndTime);
*/
}
else
{
//if (!blending)
animation.Stop();
//else if (currentAnimation != null)
// animation.Blend(currentAnimation.name,0.0f,currentBlendingTime);
changed = false;
planning.goalReached = true;
//Debug.Log("goalReached at time " + Time.time);
}
}
else
changed = false;
}
//public void Init(AnimationAnalyzer animAnalyzer, FootstepPlanningTest planner, RootMotionComputer computer, NeighbourAgents agents, NeighbourObstacles obstacles){
public void Init(AnimationAnalyzer animAnalyzer, Planner planner, NeighbourAgents agents, NeighbourObstacles obstacles)
{
analyzer = animAnalyzer;
if (analyzer != null && !analyzer.initialized)
analyzer.Init();
planning = planner;
if (planning != null && !planning.initialized)
planning.Init(analyzer,agents,obstacles);
foreach (AnimationState state in animation)
{
state.enabled = true;
state.speed = 1.0f;
state.wrapMode = WrapMode.Loop;
state.weight = 0;
}
currentAnimation = null;
action = null;
animation.Stop();
initialized = true;
actionNum = 0;
changed = false;
errorT = new Vector3(0,0,0);
errorR = 0;
currentBlendingTime = 0;
currentActionTime = 0;
previousBlendingTime = 0;
currentActionEndTime = 0;
actionsSinceLastPlan = 0;
insertedAction = false;
initGameObjectPos = transform.position;
initGameObjectRotY = transform.eulerAngles.y;
initLocalRootPos = root.localPosition;
lastRootPos = root.position;
lastRootRot = root.rotation;
}
// 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 void InsertAction(FootstepPlanningAction newAction)
{
currentAnimation.time = currentActionTime;
action = newAction;
insertedAction = true;
float lastHeight = initGameObjectPos.y;
initGameObjectPos = transform.position;
initGameObjectPos.y = lastHeight;
initGameObjectRotY = transform.eulerAngles.y;
//Debug.Log("Before = " + planning.currentState.currentPosition);
planning.currentState = action.state as FootstepPlanningState;
planning.currentState.currentPosition.y = lastHeight;
//Debug.Log("After = " + planning.currentState.currentPosition);
}
public override FootstepPlanningAction[] GetOutputPlan()
{
FootstepPlanningAction[] actionList = new FootstepPlanningAction[outputPlan.Count];
int i=0;
int count=0;
while(i < outputPlan.Count)
{
FootstepPlanningAction fsAction = outputPlan.ElementAt(i) as FootstepPlanningAction;
if (fsAction != null)
{
actionList[count] = fsAction;
count++;
if(REPRODUCTION_MODE.REPRODUCTION_MODE.Equals(Reproduction.MODE.RECORD))
{
storedPlan.Add(fsAction);
//Debug.Log("Recording");
}
}
i++;
}
return actionList;
}
// 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);
}