// Computes an action that contains the preconditions of the selectable next actions
public AnotatedAnimation NextActionPreconditions()
{
AnotatedAnimation newAnimInfo = new AnotatedAnimation();
if (animInfo.type != LocomotionMode.SideStep)
{
if (animInfo.swing == Joint.LeftFoot)
{
newAnimInfo.swing = Joint.RightFoot;
newAnimInfo.supporting = Joint.LeftFoot;
}
else
{
newAnimInfo.swing = Joint.LeftFoot;
newAnimInfo.supporting = Joint.RightFoot;
}
}
else
{
newAnimInfo.swing = animInfo.swing;
newAnimInfo.supporting = animInfo.supporting;
}
return(newAnimInfo);
}
public FootstepPlanningAction(GridTimeAction gridAction, AnotatedAnimation anim, float sp)
{
state = gridAction.state;
cost = gridAction.cost;
animInfo = anim;
speed = sp;
}
//public static float timeSum = 0.0f;
//public static int numCalls = 0;
// Construction function for a state that comes from a previous state with an action
public FootstepPlanningState(FootstepPlanningState prevState, AnotatedAnimation anim, float speed, AnotatedAnimation pre)
{
//float startTime = Time.realtimeSinceStartup;
previousState = prevState;
actionName = anim.name;
actionSpeed = speed;
time = prevState.time;
ComputeActualState(anim);
preconditions = pre;
/*
* float endTime = Time.realtimeSinceStartup;
*
* timeSum += (endTime - startTime);
* numCalls++;
*
* float meanTime = 0.0f;
* if (numCalls == 100)
* {
* meanTime = timeSum / numCalls;
* Debug.Log("At time " + Time.time + " MeanTime = " + meanTime);
* numCalls = 0;
* timeSum = 0;
* }
*/
}
// Use this for initialization
void Start()
{
List<AnotatedAnimation> aList = new List<AnotatedAnimation>();
AnotatedAnimation anotatedAnimation = new AnotatedAnimation();
anotatedAnimation.angle = 0.1F;
anotatedAnimation.distance = 1.0F;
anotatedAnimation.LeftFoot = new JointState[2];
JointState js = new JointState();
js.angle = 0.1F;
js.orientation = new Vector3(1.0F,1.0F,1.0F);
anotatedAnimation.LeftFoot[0] = js;
anotatedAnimation.LeftFoot[1] = js;
aList.Add(anotatedAnimation);
aList.Add(anotatedAnimation);
// writing an object to file
ObjectSerializer os = new ObjectSerializer();
os.serializedObject = aList;
os.writeObjectToFile("test.xml");
//reading an object from file
ObjectSerializer os1 = new ObjectSerializer();
os1.serializedObject = new List<AnotatedAnimation>();
os1.readObjectFromFile("test.xml");
List<AnotatedAnimation> objectThatNeedsToBeReadFromFile = (List<AnotatedAnimation>) os1.serializedObject;
Debug.Log("after read " + objectThatNeedsToBeReadFromFile[1].LeftFoot[1].orientation);
}
//Constructor for actions using dummytype for planningstate
public FootstepPlanningAction(FootstepPlanningState previousState, AnotatedAnimation anim, float s, float meanStepSize, float mass, float dummyType)
{
animInfo = anim;
speed = s;
cost = ComputeCost(meanStepSize, mass);
state = new FootstepPlanningState(previousState, anim, s, NextActionPreconditions(), dummyType);
}
// Use this for initialization
void Start()
{
List <AnotatedAnimation> aList = new List <AnotatedAnimation>();
AnotatedAnimation anotatedAnimation = new AnotatedAnimation();
anotatedAnimation.angle = 0.1F;
anotatedAnimation.distance = 1.0F;
anotatedAnimation.LeftFoot = new JointState[2];
JointState js = new JointState();
js.angle = 0.1F;
js.orientation = new Vector3(1.0F, 1.0F, 1.0F);
anotatedAnimation.LeftFoot[0] = js;
anotatedAnimation.LeftFoot[1] = js;
aList.Add(anotatedAnimation);
aList.Add(anotatedAnimation);
// writing an object to file
ObjectSerializer os = new ObjectSerializer();
os.serializedObject = aList;
os.writeObjectToFile("test.xml");
//reading an object from file
ObjectSerializer os1 = new ObjectSerializer();
os1.serializedObject = new List <AnotatedAnimation>();
os1.readObjectFromFile("test.xml");
List <AnotatedAnimation> objectThatNeedsToBeReadFromFile = (List <AnotatedAnimation>)os1.serializedObject;
Debug.Log("after read " + objectThatNeedsToBeReadFromFile[1].LeftFoot[1].orientation);
}
public FootstepPlanningAction(GridTimeAction gridAction, AnotatedAnimation anim, float sp)
{
state = gridAction.state;
cost = gridAction.cost;
animInfo = anim;
speed = sp;
}
//public static float timeSum = 0.0f;
//public static int numCalls = 0;
// Construction function for a state that comes from a previous state with an action
public FootstepPlanningState(FootstepPlanningState prevState, AnotatedAnimation anim, float speed, AnotatedAnimation pre)
{
//float startTime = Time.realtimeSinceStartup;
previousState = prevState;
actionName = anim.name;
actionSpeed = speed;
time = prevState.time;
ComputeActualState(anim);
preconditions = pre;
/*
float endTime = Time.realtimeSinceStartup;
timeSum += (endTime - startTime);
numCalls++;
float meanTime = 0.0f;
if (numCalls == 100)
{
meanTime = timeSum / numCalls;
Debug.Log("At time " + Time.time + " MeanTime = " + meanTime);
numCalls = 0;
timeSum = 0;
}
*/
}
//Constructor for actions using dummytype for planningstate
public FootstepPlanningAction(FootstepPlanningState previousState, AnotatedAnimation anim, float s, float meanStepSize, float mass, float dummyType)
{
animInfo = anim;
speed = s;
cost = ComputeCost(meanStepSize,mass);
state = new FootstepPlanningState(previousState, anim, s, NextActionPreconditions(), dummyType);
}
// Collision check for the lower resolutions model
public bool CheckRootCollisions(FootstepPlanningState state)
{
Vector3 start = state.currentPosition - new Vector3(0, 10 * analyzer.GetHeight() / 2, 0);
Vector3 end = start + new Vector3(0, 10 * analyzer.GetHeight() / 2, 0);
float radius = analyzer.GetRadius();
if (Physics.CheckCapsule(start, end, radius, layer))
{
//if (Physics.CheckSphere(state.currentPosition,radius,layer))
return(true);
}
if (state.previousState != null)
{
int samples = analyzer.samples;
AnotatedAnimation anim = analyzer.GetAnotatedAnimation(state.actionName);
for (int i = 1; i < samples - 1; i++)
{
start = state.previousState.currentPosition + anim.Root[i].position;
end = start + new Vector3(0, analyzer.GetHeight() / 2, 0);
if (Physics.CheckCapsule(start, end, radius, layer))
{
//if (Physics.CheckSphere(state.previousState.currentPosition,radius,layer))
return(true);
}
}
}
return(false);
}
public void analyzeInitState(AnotatedAnimation anotatedAnim)
{
anotatedAnim.RootCurve.initRootPos = root.position;
anotatedAnim.RootCurve.initRotY = computeForwardRotY();
anotatedAnim.RootCurve.initHeight = root.position.y;
analyzeState(anotatedAnim, 0, 0);
}
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);
}
}
}
}
}
}
//This constructor creates a previous state given a current state and an action.
//The dummyType is to differentiate it from the constructor used to create successors states.
public FootstepPlanningState(FootstepPlanningState currentState, AnotatedAnimation anim,
float speed, AnotatedAnimation pre, float dummyType)
{
previousState = currentState;
actionName = anim.name;
actionSpeed = speed;
time = currentState.time;
ComputePreviousState(anim);
preconditions = pre;
}
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;
}
// Function that evaluates if the current action satisfies some preconditions
public bool SatisfiesPreconditions(AnotatedAnimation precondition)
{
if (precondition == null)
{
return(true);
}
// TODO: evaluate if preconditions are satisfied
if (animInfo.swing != precondition.swing)
{
return(false);
}
// if it passes all the precondition tests
return(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++;
}
}
}
public void copyRootCurve(AnotatedAnimation anotatedAnim, int sample, float normalizedTime)
{
Vector3 rootDisplacement = root.position - anotatedAnim.RootCurve.initRootPos;
// if it is the last sample we want to set back the height
// TODO: not do this if we want animations where we want the end position higher.
if (sample == rootSamples - 1)
{
rootDisplacement.y = 0;
}
anotatedAnim.RootCurve.xPos.AddKey(normalizedTime, rootDisplacement.x);
anotatedAnim.RootCurve.yPos.AddKey(normalizedTime, rootDisplacement.y);
anotatedAnim.RootCurve.zPos.AddKey(normalizedTime, rootDisplacement.z);
float rotY = computeForwardRotY() - anotatedAnim.RootCurve.initRotY;
anotatedAnim.RootCurve.yRot.AddKey(normalizedTime, rotY);
}
public float time; // added time since the computation of the plan for the character to get to that state
#endregion Fields
#region Constructors
// Construction function for an initial state
public FootstepPlanningState(Vector3 gameObjectPosition, Quaternion gameObjectRotation, float t)
{
previousState = null;
actionName = null;
actionSpeed = 0;
currentPosition = gameObjectPosition;
currentSpeed = 0.0f;
currentAcceleration = 0.0f;
float initAngle = gameObjectRotation.eulerAngles.y;
currentRotation = Quaternion.AngleAxis(initAngle,new Vector3(0,1,0));
preconditions = null;
time = t;
}
public void analyzeState(AnotatedAnimation anotatedAnim, int sample, float normalizedTime)
{
Vector3 rootDisplacement = root.position - anotatedAnim.RootCurve.initRootPos;
// if it is the last sample we want to set back the height
// TODO: not do this if we want animations where we want the end position higher.
if (sample == samples - 1)
{
rootDisplacement.y = 0;
}
//anotatedAnim.RootCurve.xPos.AddKey(normalizedTime,rootDisplacement.x);
//anotatedAnim.RootCurve.yPos.AddKey(normalizedTime,rootDisplacement.y);
//anotatedAnim.RootCurve.zPos.AddKey(normalizedTime,rootDisplacement.z);
float rotY = computeForwardRotY() - anotatedAnim.RootCurve.initRotY;
//anotatedAnim.RootCurve.yRot.AddKey(normalizedTime,rotY);
anotatedAnim.Root[sample].position = root.position;
//anotatedAnim.Root[sample].angle = root.localRotation.eulerAngles.y;
//anotatedAnim.Root[sample].orientation = new Vector3(Mathf.Cos(anotatedAnim.Root[sample].angle*Mathf.Deg2Rad),0,Mathf.Sin(anotatedAnim.Root[sample].angle*Mathf.Deg2Rad));
anotatedAnim.Root[sample].orientation = -root.right;
anotatedAnim.Root[sample].orientation.Normalize();
anotatedAnim.Root[sample].angle = CalculateAngle(anotatedAnim.Root[sample].orientation.x, anotatedAnim.Root[sample].orientation.z);
anotatedAnim.LeftFoot[sample].position = leftFoot.position;
anotatedAnim.RightFoot[sample].position = rightFoot.position;
anotatedAnim.LeftFoot[sample].orientation = leftToe.position - leftFoot.position;
anotatedAnim.LeftFoot[sample].orientation[1] = 0;
anotatedAnim.RightFoot[sample].orientation = rightToe.position - rightFoot.position;
anotatedAnim.RightFoot[sample].orientation[1] = 0;
anotatedAnim.LeftFoot[sample].angle = Vector3.Angle(new Vector3(1, 0, 0), anotatedAnim.LeftFoot[sample].orientation);
anotatedAnim.RightFoot[sample].angle = Vector3.Angle(new Vector3(1, 0, 0), anotatedAnim.RightFoot[sample].orientation);
anotatedAnim.LeftHand[sample] = leftHand.position;
anotatedAnim.RightHand[sample] = rightHand.position;
}
// Computes the actual state based on the effect of the lastAction over the previous state
// PRE: lastAction != null && previousState != null
private void ComputeActualState(AnotatedAnimation anim)
{
if (previousState != null)
{
int endSample = anim.Root.Length - 1;
/*
* Vector3 forward = Vector3.zero;
*
* float currentTime = anim.time/anim.totalLength;
*
* CompleteAnimationCurve animCurve = anim.RootCurve;
*
* forward.x = animCurve.xRot.Evaluate(currentTime);
* forward.y = animCurve.yRot.Evaluate(currentTime);
* forward.z = animCurve.zRot.Evaluate(currentTime);
*
* Quaternion rotation = Quaternion.FromToRotation(animCurve.initForward,forward);
* currentRotation = previousState.currentRotation * rotation;
*/
//currentRotation = previousState.currentRotation * Quaternion.AngleAxis(anim.rotationAngle,new Vector3(0,1,0));
float yRot = anim.rotationAngle; //anim.RootCurve.yRot.Evaluate(anim.time);
currentRotation = previousState.currentRotation * Quaternion.Euler(new Vector3(0, yRot, 0));
leftFoot = previousState.currentPosition + previousState.currentRotation * (anim.LeftFoot[endSample].position);
rightFoot = previousState.currentPosition + previousState.currentRotation * (anim.RightFoot[endSample].position);
leftHand = previousState.currentPosition + previousState.currentRotation * (anim.LeftHand[endSample]);
rightHand = previousState.currentPosition + previousState.currentRotation * (anim.RightHand[endSample]);
currentPosition = previousState.currentPosition + previousState.currentRotation * anim.rootDisplacement;
currentSpeed = anim.speed * actionSpeed;
currentAcceleration = currentSpeed - previousState.currentSpeed;
time += anim.time * anim.totalLength / actionSpeed;
}
}
//Compute previous state based on the effect of the action over the current state
//Meant to be used with backtracking algorithms (ADA*)
private void ComputePreviousState(AnotatedAnimation anim)
{
if (previousState != null)
{
int endSample = anim.Root.Length - 1;
//currentRotation = previousState.currentRotation * Quaternion.AngleAxis(anim.rotationAngle, new Vector3(0,1,0));
float yRot = anim.rotationAngle; //anim.RootCurve.yRot.Evaluate(anim.time);
currentRotation = previousState.currentRotation * Quaternion.Euler(new Vector3(0, yRot, 0));
leftFoot = previousState.currentPosition - previousState.currentRotation * (anim.LeftFoot[endSample].position);
rightFoot = previousState.currentPosition - previousState.currentRotation * (anim.RightFoot[endSample].position);
leftHand = previousState.currentPosition - previousState.currentRotation * (anim.LeftHand[endSample]);
rightHand = previousState.currentPosition - previousState.currentRotation * (anim.RightHand[endSample]);
currentPosition = previousState.currentPosition - previousState.currentRotation * anim.rootDisplacement;
currentSpeed = anim.speed * actionSpeed;
currentAcceleration = currentSpeed - previousState.currentSpeed;
time -= anim.time * anim.totalLength / actionSpeed;
}
}
public void updateGlobalInfo(AnotatedAnimation analyzedAnim)
{
// We update some global information
if (analyzedAnim.type == LocomotionMode.Walk || analyzedAnim.type == LocomotionMode.WalkTurn ||
analyzedAnim.type == LocomotionMode.SideStep)
{
walkingAnimations++;
float displ = analyzedAnim.rootDisplacement.magnitude;
if (displ > maxStepSize)
{
maxStepSize = displ;
}
sumStepsSizes += displ;
}
float duration = analyzedAnim.time * analyzedAnim.totalLength;
//Debug.Log(analyzedAnim.name + " duration: " + duration);
if (analyzedAnim.type != LocomotionMode.Idle)
{
if (duration > maxActionDuration)
{
maxActionDuration = duration;
}
sumActionDuration += duration;
}
// For debugging we print the resulting analysis
if (log)
{
analyzedAnim.LogAnotations();
}
}
void OnDrawGizmos()
{
if (!debugDraw)
{
return;
}
if (planning == null)
{
return;
}
/*
* //if(firstTime)
* //{
* foreach(ADAstarNode node in ADAstarPlanner.Closed.Values)
* {
* if(node.action.GetType() == typeof(FootstepPlanningAction)){
* Vector3 center = ((node.action as FootstepPlanningAction).state as FootstepPlanningState).currentPosition;
* Gizmos.color = Color.blue;
* Gizmos.DrawSphere(center, .25f);
* }
* }
* // firstTime = false;
* //}
*/
Gizmos.color = Color.red;
Gizmos.DrawWireSphere(lastPos, analyzer.GetRadius());
Gizmos.DrawWireSphere(lastObstacle, 1.0f);
//if (debugOnlyCurrent)
// return;
FootstepPlanningAction[] plan = planning.GetOutputPlan();
numberOfActions = plan.Length;
for (int i = 0; i < numberOfActions; i++)
{
if (plan[i] != null)
{
FootstepPlanningState state = plan[i].state as FootstepPlanningState;
if (state != null)
{
AnotatedAnimation animInfo = analyzer.GetAnotatedAnimation(state.actionName);
//Quaternion rotation = state.currentRotation;
Vector3 position;
if (animInfo.swing == Joint.LeftFoot)
{
position = state.leftFoot;
position[1] = auxHeight;
Gizmos.color = Color.green;
}
else
{
position = state.rightFoot;
position[1] = auxHeight;
Gizmos.color = Color.blue;
}
Gizmos.DrawWireSphere(state.currentPosition, analyzer.GetRadius());
Gizmos.DrawWireSphere(state.obstaclePos, 1.0f);
}
else
{
GridTimeState gtState = plan[i].state as GridTimeState;
if (gtState != null)
{
Vector3 position = gtState.currentPosition;
if (i % 2 == 0)
{
Gizmos.color = Color.green;
}
else
{
Gizmos.color = Color.blue;
}
if (!debugOnlyCurrent || (debugOnlyCurrent && Mathf.Abs(Time.time - gtState.time) < 0.5f))
{
Gizmos.DrawWireSphere(gtState.currentPosition, analyzer.GetRadius());
Gizmos.color = Color.red;
Gizmos.DrawWireSphere(gtState.obstaclePos, 1.0f);
}
}
}
}
}
}
// The function that analyzes an animation and returns the AnotatedAnimation
private AnotatedAnimation AnalyzeAnimation(AnimationState anim, int id)
{
AnotatedAnimation analyzedAnim = new AnotatedAnimation();
////////
analyzedAnim.Init(samples);
analyzedAnim.id = id;
analyzedAnim.name = anim.name;
analyzedAnim.totalLength = anim.length;
if (analyzedAnim.name.Contains("WalkTurn")
|| ( analyzedAnim.name.Contains("Walk") && analyzedAnim.name.Contains("Turn"))
|| ( analyzedAnim.name.Contains("Turn") && analyzedAnim.name.Contains("Turn"))
)
analyzedAnim.type = LocomotionMode.WalkTurn;
else if (analyzedAnim.name.Contains("Walk"))
analyzedAnim.type = LocomotionMode.Walk;
else if (analyzedAnim.name.Contains("Run"))
analyzedAnim.type = LocomotionMode.Run;
else if (analyzedAnim.name.Contains("Idle"))
analyzedAnim.type = LocomotionMode.Idle;
else if (analyzedAnim.name.Contains("Jump"))
analyzedAnim.type = LocomotionMode.Jump;
else if (analyzedAnim.name.Contains("Turn"))
analyzedAnim.type = LocomotionMode.Turn;
else if (analyzedAnim.name.Contains("Side"))
analyzedAnim.type = LocomotionMode.SideStep;
else //if (analyzedAnim.name.Contains("Action"))
analyzedAnim.type = LocomotionMode.Action;
// Load Animation
anim.normalizedTime = 0.0f;
anim.enabled = true;
anim.wrapMode = WrapMode.Loop;
animation.Play(analyzedAnim.name);
animation.Sample();
// Detect supporting and swing foot
analyzedAnim.SetSupportingFoot( DetectPlantedFoot() );
if (
analyzedAnim.type == LocomotionMode.Walk
|| analyzedAnim.type == LocomotionMode.Run
|| analyzedAnim.type == LocomotionMode.WalkTurn
|| analyzedAnim.type == LocomotionMode.Jump
)
{
// We look the time where the action is completed
// e.g. the swing foot is planted
SeekFootplantTime(anim, analyzedAnim.supporting, ref analyzedAnim.time, ref analyzedAnim.footPlantLenght);
}
else
{
analyzedAnim.time = 1.0f-0.001f;
analyzedAnim.footPlantLenght = analyzedAnim.time/2;
}
//////////
Vector3 initPos = gameObject.transform.position;
Quaternion initRot = gameObject.transform.rotation;
// Load Animation
anim.normalizedTime = 0.0f;
animation.Play(analyzedAnim.name);
animation.Sample();
// Analyze Initial State
analyzeInitState(analyzedAnim);
// For every sample we want
for (int i=1; i<samples; i++)
{
// Sample the Animation until the end of our action
anim.normalizedTime = i*analyzedAnim.time/(samples-1);
animation.Play(analyzedAnim.name);
animation.Sample();
// Analyze State
analyzeState(analyzedAnim,i,anim.normalizedTime);
}
for (int i=1; i<rootSamples; i++)
{
// Sample the Animation until the end of our action
anim.normalizedTime = i*analyzedAnim.time/(rootSamples-1);
animation.Play(analyzedAnim.name);
animation.Sample();
// Analyze State
copyRootCurve(analyzedAnim,i,anim.normalizedTime);
}
// Compute Action Movement Attributes
analyzeMovement(analyzedAnim);
////////
// We remove the animation
anim.normalizedTime = 0.0f;
anim.enabled = false;
animation.Stop();
animation.Sample();
animation["TPose"].enabled = true;
animation["TPose"].wrapMode = WrapMode.Loop;
animation.Play("TPose");
animation.Sample();
animation.Stop();
animation.Sample();
gameObject.transform.position = initPos;
gameObject.transform.rotation = initRot;
updateGlobalInfo(analyzedAnim);
return analyzedAnim;
}
public void analyzeInitState( AnotatedAnimation anotatedAnim)
{
anotatedAnim.RootCurve.initRootPos = root.position;
anotatedAnim.RootCurve.initRotY = computeForwardRotY();
anotatedAnim.RootCurve.initHeight = root.position.y;
analyzeState(anotatedAnim,0,0);
}
public void analyzeMovement( AnotatedAnimation anotatedAnim )
{
int endSample = samples-1;
anotatedAnim.RootCurve.xPos.postWrapMode = WrapMode.Clamp;
anotatedAnim.RootCurve.xPos.preWrapMode = WrapMode.Clamp;
anotatedAnim.RootCurve.yPos.postWrapMode = WrapMode.Clamp;
anotatedAnim.RootCurve.yPos.preWrapMode = WrapMode.Clamp;
anotatedAnim.RootCurve.zPos.postWrapMode = WrapMode.Clamp;
anotatedAnim.RootCurve.zPos.preWrapMode = WrapMode.Clamp;
anotatedAnim.RootCurve.yRot.postWrapMode = WrapMode.Clamp;
anotatedAnim.RootCurve.yRot.preWrapMode = WrapMode.Clamp;
Vector3 origin = anotatedAnim.Root[0].position;
//
float rootRefAngle = anotatedAnim.Root[0].angle;
//anotatedAnim.rotationAngle = -(anotatedAnim.Root[endSample].angle - anotatedAnim.Root[0].angle);
anotatedAnim.rotationAngle = anotatedAnim.RootCurve.yRot.Evaluate(anotatedAnim.time);
anotatedAnim.rotationSpeed = anotatedAnim.rotationAngle / (anotatedAnim.time*anotatedAnim.totalLength);
for (int i=0; i<samples; i++)
{
anotatedAnim.Root[i].position -= origin;
anotatedAnim.RightFoot[i].position -= origin;
anotatedAnim.LeftFoot[i].position -= origin;
anotatedAnim.LeftHand[i] -= origin;
anotatedAnim.RightHand[i] -= origin;
//anotatedAnim.Root[i].angle = 0;
anotatedAnim.Root[i].angle -= rootRefAngle;
anotatedAnim.RightFoot[i].angle -= rootRefAngle;
anotatedAnim.LeftFoot[i].angle -= rootRefAngle;
}
anotatedAnim.movement_RightFoot.position = anotatedAnim.RightFoot[endSample].position - anotatedAnim.RightFoot[0].position;
//anotatedAnim.movement_RightFoot.position[1] = 0;
anotatedAnim.movement_LeftFoot.position = anotatedAnim.LeftFoot[endSample].position - anotatedAnim.LeftFoot[0].position;
//anotatedAnim.movement_LeftFoot.position[1] = 0;
anotatedAnim.movement_LeftHand = anotatedAnim.LeftHand[endSample] - anotatedAnim.LeftHand[0];
anotatedAnim.movement_RightHand = anotatedAnim.RightHand[endSample] - anotatedAnim.RightHand[0];
anotatedAnim.rootDisplacement = anotatedAnim.Root[endSample].position - anotatedAnim.Root[0].position;
anotatedAnim.rootDisplacement[1] = 0;
anotatedAnim.distance = anotatedAnim.rootDisplacement.magnitude;
anotatedAnim.speed = anotatedAnim.distance / (anotatedAnim.time*anotatedAnim.totalLength);
//anotatedAnim.angle = Vector3.Angle(new Vector3(1,0,0), anotatedAnim.rootDisplacement) - rootRefAngle;
anotatedAnim.angle = -CalculateAngleOf2Vectors(anotatedAnim.Root[0].orientation,anotatedAnim.rootDisplacement);
if (anotatedAnim.type == LocomotionMode.WalkTurn || anotatedAnim.type == LocomotionMode.Turn )
anotatedAnim.angleCost = Mathf.Abs( Mathf.Abs( anotatedAnim.angle ) - Mathf.Abs( anotatedAnim.rotationAngle ) ) / 180;
else
anotatedAnim.angleCost = Mathf.Abs( anotatedAnim.angle );// / 90;
}
//Compute previous state based on the effect of the action over the current state
//Meant to be used with backtracking algorithms (ADA*)
private void ComputePreviousState(AnotatedAnimation anim)
{
if(previousState != null)
{
int endSample = anim.Root.Length-1;
//currentRotation = previousState.currentRotation * Quaternion.AngleAxis(anim.rotationAngle, new Vector3(0,1,0));
float yRot = anim.rotationAngle;//anim.RootCurve.yRot.Evaluate(anim.time);
currentRotation = previousState.currentRotation * Quaternion.Euler(new Vector3(0,yRot,0));
leftFoot = previousState.currentPosition - previousState.currentRotation * (anim.LeftFoot[endSample].position);
rightFoot = previousState.currentPosition - previousState.currentRotation * (anim.RightFoot[endSample].position);
leftHand = previousState.currentPosition - previousState.currentRotation * (anim.LeftHand[endSample]);
rightHand = previousState.currentPosition - previousState.currentRotation * (anim.RightHand[endSample]);
currentPosition = previousState.currentPosition - previousState.currentRotation * anim.rootDisplacement;
currentSpeed = anim.speed * actionSpeed;
currentAcceleration = currentSpeed - previousState.currentSpeed;
time -= anim.time * anim.totalLength / actionSpeed;
}
}
public FootstepPlanningState(FootstepPlanningState original)
{
this.actionName = original.actionName;
this.actionSpeed = original.actionSpeed;
this.currentAcceleration = original.currentAcceleration;
this.currentPosition = original.currentPosition;
this.currentRotation = original.currentRotation;
this.currentSpeed = original.currentSpeed;
this.leftFoot = original.leftFoot;
this.leftHand = original.leftHand;
this.obstaclePos = original.obstaclePos;
this.preconditions = original.preconditions;
this.previousState = original.previousState;
this.rightFoot = original.rightFoot;
this.rightHand = original.rightHand;
this.time = original.time;
}
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!");
}
}
void LateUpdate()
{
if (stored)
{
return;
}
// If analysis done remove all animations
if (currentAnimationIndex == animationNames.Length)
{
if (analyzer != null)
{
// Stop and remove animations from the character
analyzer.RemoveAnimations(animation);
// Update global information
analyzer.UpdateGlobalInfo();
// Store analysis
// writing an object to file
ObjectSerializer os = new ObjectSerializer();
os.serializedObject = analyzer.analyzedAnimations;
os.writeObjectToFile(fileName);
stored = true;
if (analyzer.log)
{
analyzer.ReadAnalysisFromFile(fileName);
}
}
// End execution
return;
}
//if (currentAnimation != null
// && Mathf.Abs(currentAnimation.time) < currentActionTime
//)
// We compute the root displacement
//rootMotion.ComputeRootMotion();
if (changed && currentAnimationName != null)
{
//currentAnimation.time = 0;
//agent.animation.Sample();
//rootMotion.ComputeRootMotion();
AnotatedAnimation animInfo = analyzer.GetAnotatedAnimation(currentAnimationName);
// analyze initial state
analyzer.analyzeInitState(animInfo);
currentSample = 1;
}
// If its the first animation or the current animation ended,
if (currentAnimation == null ||
Mathf.Abs(currentAnimation.time) >= currentActionTime
)
{
if (currentAnimationName != null)
{
Debug.Log("Time difference = " + (currentAnimation.time - currentActionTime));
// We correct the pose to the one we really want to analyze
//currentAnimation.time = currentActionTime;
//agent.animation.Sample();
//rootMotion.ComputeRootMotion();
AnotatedAnimation endedAnimInfo = analyzer.GetAnotatedAnimation(currentAnimationName);
// Analyze end state
analyzer.analyzeState(endedAnimInfo, analyzer.samples - 1, currentAnimation.normalizedTime);
// Analyze the movement
analyzer.analyzeMovement(endedAnimInfo);
// Update global values
analyzer.updateGlobalInfo(endedAnimInfo);
}
// Get new animation
currentAnimationIndex++;
// If there is no more animations we stop
if (currentAnimationIndex == animationNames.Length)
{
return;
}
string animationName = animationNames[currentAnimationIndex];
if (blending)
{
// We blend out the previous animation
// if it exists and it is not the first one
if (currentAnimation != null)
{
if (currentAnimationIndex > 0)
{
agent.animation.Blend(currentAnimation.name, 0.0f, currentBlendingTime);
}
else
{
agent.animation.Stop();
agent.animation.Sample();
}
}
}
// We set up the speed of the new animation
AnimationState newAnimationState = agent.animation[animationName];
newAnimationState.speed = 1.0f;
// We set the animation at the beginning
newAnimationState.time = 0;
newAnimationState.enabled = true;
// We save the new animation
currentAnimation = newAnimationState;
currentAnimationName = animationName;
// We get the info of the new animation
AnotatedAnimation animInfo = analyzer.GetAnotatedAnimation(animationName);
// 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;
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 (currentAnimationIndex > 0)
{
agent.animation.Blend(currentAnimation.name, 1.0f, currentBlendingTime);
}
else
{
agent.animation.Play(currentAnimation.name);
agent.animation.Sample();
}
}
else
{
agent.animation.Play(currentAnimation.name);
}
changed = true;
currentSample = 0;
}
else
{
changed = false;
if (currentAnimationName != null)
{
AnotatedAnimation animInfo = analyzer.GetAnotatedAnimation(currentAnimationName);
float previousSampleTime = (currentSample - 1) * animInfo.time / (analyzer.samples - 1);
previousSampleTime *= animInfo.totalLength;
float timeBetweenSamples = animInfo.time / (analyzer.samples - 1);
float timeDifference = Mathf.Abs(currentAnimation.time) - previousSampleTime;
if (timeDifference >= timeBetweenSamples)
{
// We correct the pose to the one we really want to analyze
//currentAnimation.time = currentSample*animInfo.time/(analyzer.samples-1);
//agent.animation.Sample();
analyzer.analyzeState(animInfo, currentSample, currentAnimation.normalizedTime);
currentSample++;
}
}
}
}
// The function that analyzes an animation and returns the AnotatedAnimation
private AnotatedAnimation AnalyzeAnimation(AnimationState anim, int id)
{
AnotatedAnimation analyzedAnim = new AnotatedAnimation();
////////
analyzedAnim.Init(samples);
analyzedAnim.id = id;
analyzedAnim.name = anim.name;
analyzedAnim.totalLength = anim.length;
if (analyzedAnim.name.Contains("WalkTurn") ||
(analyzedAnim.name.Contains("Walk") && analyzedAnim.name.Contains("Turn")) ||
(analyzedAnim.name.Contains("Turn") && analyzedAnim.name.Contains("Turn"))
)
{
analyzedAnim.type = LocomotionMode.WalkTurn;
}
else if (analyzedAnim.name.Contains("Walk"))
{
analyzedAnim.type = LocomotionMode.Walk;
}
else if (analyzedAnim.name.Contains("Run"))
{
analyzedAnim.type = LocomotionMode.Run;
}
else if (analyzedAnim.name.Contains("Idle"))
{
analyzedAnim.type = LocomotionMode.Idle;
}
else if (analyzedAnim.name.Contains("Jump"))
{
analyzedAnim.type = LocomotionMode.Jump;
}
else if (analyzedAnim.name.Contains("Turn"))
{
analyzedAnim.type = LocomotionMode.Turn;
}
else if (analyzedAnim.name.Contains("Side"))
{
analyzedAnim.type = LocomotionMode.SideStep;
}
else //if (analyzedAnim.name.Contains("Action"))
{
analyzedAnim.type = LocomotionMode.Action;
}
// Load Animation
anim.normalizedTime = 0.0f;
anim.enabled = true;
anim.wrapMode = WrapMode.Loop;
animation.Play(analyzedAnim.name);
animation.Sample();
// Detect supporting and swing foot
analyzedAnim.SetSupportingFoot(DetectPlantedFoot());
if (
analyzedAnim.type == LocomotionMode.Walk ||
analyzedAnim.type == LocomotionMode.Run ||
analyzedAnim.type == LocomotionMode.WalkTurn ||
analyzedAnim.type == LocomotionMode.Jump
)
{
// We look the time where the action is completed
// e.g. the swing foot is planted
SeekFootplantTime(anim, analyzedAnim.supporting, ref analyzedAnim.time, ref analyzedAnim.footPlantLenght);
}
else
{
analyzedAnim.time = 1.0f - 0.001f;
analyzedAnim.footPlantLenght = analyzedAnim.time / 2;
}
//////////
Vector3 initPos = gameObject.transform.position;
Quaternion initRot = gameObject.transform.rotation;
// Load Animation
anim.normalizedTime = 0.0f;
animation.Play(analyzedAnim.name);
animation.Sample();
// Analyze Initial State
analyzeInitState(analyzedAnim);
// For every sample we want
for (int i = 1; i < samples; i++)
{
// Sample the Animation until the end of our action
anim.normalizedTime = i * analyzedAnim.time / (samples - 1);
animation.Play(analyzedAnim.name);
animation.Sample();
// Analyze State
analyzeState(analyzedAnim, i, anim.normalizedTime);
}
for (int i = 1; i < rootSamples; i++)
{
// Sample the Animation until the end of our action
anim.normalizedTime = i * analyzedAnim.time / (rootSamples - 1);
animation.Play(analyzedAnim.name);
animation.Sample();
// Analyze State
copyRootCurve(analyzedAnim, i, anim.normalizedTime);
}
// Compute Action Movement Attributes
analyzeMovement(analyzedAnim);
////////
// We remove the animation
anim.normalizedTime = 0.0f;
anim.enabled = false;
animation.Stop();
animation.Sample();
animation["TPose"].enabled = true;
animation["TPose"].wrapMode = WrapMode.Loop;
animation.Play("TPose");
animation.Sample();
animation.Stop();
animation.Sample();
gameObject.transform.position = initPos;
gameObject.transform.rotation = initRot;
updateGlobalInfo(analyzedAnim);
return(analyzedAnim);
}
// Function that evaluates if the current action satisfies some preconditions
public bool SatisfiesPreconditions(AnotatedAnimation precondition)
{
if (precondition == null)
return true;
// TODO: evaluate if preconditions are satisfied
if (animInfo.swing != precondition.swing)
return false;
// if it passes all the precondition tests
return true;
}
public void copyRootCurve( AnotatedAnimation anotatedAnim , int sample, float normalizedTime)
{
Vector3 rootDisplacement = root.position - anotatedAnim.RootCurve.initRootPos;
// if it is the last sample we want to set back the height
// TODO: not do this if we want animations where we want the end position higher.
if (sample == rootSamples-1)
rootDisplacement.y = 0;
anotatedAnim.RootCurve.xPos.AddKey(normalizedTime,rootDisplacement.x);
anotatedAnim.RootCurve.yPos.AddKey(normalizedTime,rootDisplacement.y);
anotatedAnim.RootCurve.zPos.AddKey(normalizedTime,rootDisplacement.z);
float rotY = computeForwardRotY() - anotatedAnim.RootCurve.initRotY;
anotatedAnim.RootCurve.yRot.AddKey(normalizedTime,rotY);
}
public void PlaceSequenceFootSteps(bool tunnel = false)
{
FootstepPlanningAction[] plan = planning.GetOutputPlan();
if (plan == null)
{
return;
}
numberOfActions = plan.Length;
//Debug.Log("Number of actions = " + numberOfActions);
// Debug.Log("Footsteps " + this.gameObject.name + " " + numberOfActions);
if (!tunnel)
{
steps = new Object[numberOfActions];
}
else
{
steps2 = new Object[numberOfActions];
steps2Pos = new Vector3[numberOfActions];
}
if (drawTimes)
{
if (!tunnel)
{
texts = new Object[numberOfActions];
}
else
{
texts2 = new Object[numberOfActions];
}
}
for (int i = 0; i < numberOfActions; i++)
{
if (plan[i] != null)
{
FootstepPlanningState state = plan[i].state as FootstepPlanningState;
if (state != null)
{
AnotatedAnimation animInfo = analyzer.GetAnotatedAnimation(state.actionName);
Quaternion rotation = state.currentRotation;
Vector3 position;
if (animInfo.swing == Joint.LeftFoot)
{
position = state.leftFoot;
position[1] = auxHeight;
if (!tunnel)
{
steps[i] = GameObject.Instantiate((Object)leftFootStep, position, rotation);
}
else
{
steps2[i] = GameObject.Instantiate((Object)leftFootStep, position, rotation);
}
debugText.alignment = TextAlignment.Left;
}
else
{
position = state.rightFoot;
position[1] = auxHeight;
if (!tunnel)
{
steps[i] = GameObject.Instantiate((Object)rightFootStep, position, rotation);
}
else
{
steps2[i] = GameObject.Instantiate((Object)leftFootStep, position, rotation);
}
debugText.alignment = TextAlignment.Left;
}
if (drawTimes)
{
debugText.text = "" + state.time;
if (!tunnel)
{
texts[i] = GameObject.Instantiate((Object)debugText, position, rotation);
}
else
{
texts2[i] = GameObject.Instantiate((Object)debugText, position, rotation);
}
}
}
else // if state is not a footstep state (it is a grid state or a gridTime state)
{
// we instantiate a gridstep
GridTimeState gridTimeState = plan[i].state as GridTimeState;
if (gridTimeState != null)
{
//Debug.Log("we place a gridTimeStep");
if (!tunnel)
{
steps[i] = GameObject.Instantiate((Object)gridTimeSphere, gridTimeState.currentPosition, Quaternion.identity);
}
else
{
steps2[i] = GameObject.Instantiate((Object)gridTimeSphere, gridTimeState.currentPosition, Quaternion.identity);
steps2Pos[i] = gridTimeState.currentPosition;
}
if (drawTimes)
{
debugText.text = "" + gridTimeState.time;
if (!tunnel)
{
texts[i] = GameObject.Instantiate((Object)debugText, gridTimeState.currentPosition, Quaternion.identity);
}
else
{
texts2[i] = GameObject.Instantiate((Object)debugText, gridTimeState.currentPosition, Quaternion.identity);
}
}
}
else
{
//Debug.Log("We Should have a gridPlanningStep");
GridPlanningState gridState = plan[i].state as GridPlanningState;
if (gridState != null)
{
if (!tunnel)
{
steps[i] = GameObject.Instantiate((Object)sphere, gridState.currentPosition, Quaternion.identity);
}
else
{
steps2[i] = GameObject.Instantiate((Object)sphere, gridState.currentPosition, Quaternion.identity);
steps2Pos[i] = gridState.currentPosition;
}
}
}
}
}
}
footstepsPlaced = true;
}
public void updateGlobalInfo(AnotatedAnimation analyzedAnim)
{
// We update some global information
if ( analyzedAnim.type == LocomotionMode.Walk || analyzedAnim.type == LocomotionMode.WalkTurn
|| analyzedAnim.type == LocomotionMode.SideStep )
{
walkingAnimations++;
float displ = analyzedAnim.rootDisplacement.magnitude;
if ( displ > maxStepSize)
maxStepSize = displ;
sumStepsSizes += displ;
}
float duration = analyzedAnim.time*analyzedAnim.totalLength;
//Debug.Log(analyzedAnim.name + " duration: " + duration);
if ( analyzedAnim.type != LocomotionMode.Idle )
{
if (duration > maxActionDuration)
maxActionDuration = duration;
sumActionDuration += duration;
}
// For debugging we print the resulting analysis
if (log)
analyzedAnim.LogAnotations();
}
// Collision check for the coarser resolution model
public bool CheckJointsCollisions(FootstepPlanningState state)
{
// Check right foot collisions
//Debug.Log("RightFoot.y = " + state.rightFoot.y);
float radius = analyzer.footRadius;
Vector3 start = state.rightFoot - new Vector3(0, 2 * radius, 0);
Vector3 end = start + new Vector3(0, analyzer.GetHeight(), 0);
if (Physics.CheckCapsule(start, end, radius, layer))
//if (Physics.CheckCapsule(start,end,radius))
//if (Physics.CheckSphere(start,radius))
{
/*
* Vector3 direction = end - start;
* RaycastHit[] hits = Physics.CapsuleCastAll(start,end,radius,direction);
*
* for (int i=0; i<hits.Length; i++)
* {
* Debug.Log("Hit " +i+ ".x: " + hits[i].point.x);
* Debug.Log("Hit " +i+ ".y: " + hits[i].point.y);
* Debug.Log("Hit " +i+ ".z: " + hits[i].point.z);
* }
*/
return(true);
}
// Check left foot collisions
start = state.leftFoot - new Vector3(0, 2 * radius, 0);
end = start + new Vector3(0, analyzer.GetHeight(), 0);
if (Physics.CheckCapsule(start, end, radius, layer))
{
return(true);
}
// Check head collisions
start = state.currentPosition; // - new Vector3(0,analyzer.GetHeight()/2,0);
end = start + new Vector3(0, analyzer.GetHeight(), 0); ///2,0);
radius = analyzer.headRadius;
if (Physics.CheckCapsule(start, end, radius, layer))
{
return(true);
}
// Check left hand collision
start = state.leftHand;
radius = analyzer.handRadius;
if (Physics.CheckSphere(start, radius, layer))
{
return(true);
}
// Check right hand collision
start = state.rightHand;
if (Physics.CheckSphere(start, radius, layer))
{
return(true);
}
if (state.previousState != null)
{
int samples = analyzer.samples;
AnotatedAnimation anim = analyzer.GetAnotatedAnimation(state.actionName);
for (int i = 1; i < samples - 1; i++)
{
// Check right foot collisions
//Debug.Log("RightFoot.y = " + state.rightFoot.y);
radius = analyzer.footRadius;
start = state.previousState.rightFoot + anim.RightFoot[i].position - new Vector3(0, 2 * radius, 0);
end = start + new Vector3(0, analyzer.GetHeight(), 0);
if (Physics.CheckCapsule(start, end, radius, layer))
{
return(true);
}
// Check left foot collisions
start = state.previousState.leftFoot + anim.LeftFoot[i].position - new Vector3(0, 2 * radius, 0);
end = start + new Vector3(0, analyzer.GetHeight(), 0);
if (Physics.CheckCapsule(start, end, radius, layer))
{
return(true);
}
// Check head collisions
start = state.previousState.currentPosition + anim.Root[i].position; // - new Vector3(0,analyzer.GetHeight()/2,0);
end = start + new Vector3(0, analyzer.GetHeight(), 0); ///2,0);
radius = analyzer.headRadius;
if (Physics.CheckCapsule(start, end, radius, layer))
{
return(true);
}
// Check left hand collision
start = state.previousState.leftHand + anim.LeftHand[i];
radius = analyzer.handRadius;
if (Physics.CheckSphere(start, radius, layer))
{
return(true);
}
// Check right hand collision
start = state.previousState.rightHand + anim.RightHand[i];
if (Physics.CheckSphere(start, radius, layer))
{
return(true);
}
}
}
return(false);
}
// Computes the actual state based on the effect of the lastAction over the previous state
// PRE: lastAction != null && previousState != null
private void ComputeActualState(AnotatedAnimation anim)
{
if (previousState != null)
{
int endSample = anim.Root.Length-1;
/*
Vector3 forward = Vector3.zero;
float currentTime = anim.time/anim.totalLength;
CompleteAnimationCurve animCurve = anim.RootCurve;
forward.x = animCurve.xRot.Evaluate(currentTime);
forward.y = animCurve.yRot.Evaluate(currentTime);
forward.z = animCurve.zRot.Evaluate(currentTime);
Quaternion rotation = Quaternion.FromToRotation(animCurve.initForward,forward);
currentRotation = previousState.currentRotation * rotation;
*/
//currentRotation = previousState.currentRotation * Quaternion.AngleAxis(anim.rotationAngle,new Vector3(0,1,0));
float yRot = anim.rotationAngle;//anim.RootCurve.yRot.Evaluate(anim.time);
currentRotation = previousState.currentRotation * Quaternion.Euler(new Vector3(0,yRot,0));
leftFoot = previousState.currentPosition + previousState.currentRotation * ( anim.LeftFoot[endSample].position );
rightFoot = previousState.currentPosition + previousState.currentRotation * ( anim.RightFoot[endSample].position );
leftHand = previousState.currentPosition + previousState.currentRotation * ( anim.LeftHand[endSample] );
rightHand = previousState.currentPosition + previousState.currentRotation * ( anim.RightHand[endSample] );
currentPosition = previousState.currentPosition + previousState.currentRotation * anim.rootDisplacement;
currentSpeed = anim.speed * actionSpeed;
currentAcceleration = currentSpeed - previousState.currentSpeed;
time += anim.time * anim.totalLength / actionSpeed;
}
}
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;
}
}
// Computes an action that contains the preconditions of the selectable next actions
public AnotatedAnimation NextActionPreconditions()
{
AnotatedAnimation newAnimInfo = new AnotatedAnimation();
if (animInfo.type != LocomotionMode.SideStep)
{
if (animInfo.swing == Joint.LeftFoot)
{
newAnimInfo.swing = Joint.RightFoot;
newAnimInfo.supporting = Joint.LeftFoot;
}
else
{
newAnimInfo.swing = Joint.LeftFoot;
newAnimInfo.supporting = Joint.RightFoot;
}
}
else
{
newAnimInfo.swing = animInfo.swing;
newAnimInfo.supporting = animInfo.supporting;
}
return newAnimInfo;
}
public void analyzeMovement(AnotatedAnimation anotatedAnim)
{
int endSample = samples - 1;
anotatedAnim.RootCurve.xPos.postWrapMode = WrapMode.Clamp;
anotatedAnim.RootCurve.xPos.preWrapMode = WrapMode.Clamp;
anotatedAnim.RootCurve.yPos.postWrapMode = WrapMode.Clamp;
anotatedAnim.RootCurve.yPos.preWrapMode = WrapMode.Clamp;
anotatedAnim.RootCurve.zPos.postWrapMode = WrapMode.Clamp;
anotatedAnim.RootCurve.zPos.preWrapMode = WrapMode.Clamp;
anotatedAnim.RootCurve.yRot.postWrapMode = WrapMode.Clamp;
anotatedAnim.RootCurve.yRot.preWrapMode = WrapMode.Clamp;
Vector3 origin = anotatedAnim.Root[0].position;
//
float rootRefAngle = anotatedAnim.Root[0].angle;
//anotatedAnim.rotationAngle = -(anotatedAnim.Root[endSample].angle - anotatedAnim.Root[0].angle);
anotatedAnim.rotationAngle = anotatedAnim.RootCurve.yRot.Evaluate(anotatedAnim.time);
anotatedAnim.rotationSpeed = anotatedAnim.rotationAngle / (anotatedAnim.time * anotatedAnim.totalLength);
for (int i = 0; i < samples; i++)
{
anotatedAnim.Root[i].position -= origin;
anotatedAnim.RightFoot[i].position -= origin;
anotatedAnim.LeftFoot[i].position -= origin;
anotatedAnim.LeftHand[i] -= origin;
anotatedAnim.RightHand[i] -= origin;
//anotatedAnim.Root[i].angle = 0;
anotatedAnim.Root[i].angle -= rootRefAngle;
anotatedAnim.RightFoot[i].angle -= rootRefAngle;
anotatedAnim.LeftFoot[i].angle -= rootRefAngle;
}
anotatedAnim.movement_RightFoot.position = anotatedAnim.RightFoot[endSample].position - anotatedAnim.RightFoot[0].position;
//anotatedAnim.movement_RightFoot.position[1] = 0;
anotatedAnim.movement_LeftFoot.position = anotatedAnim.LeftFoot[endSample].position - anotatedAnim.LeftFoot[0].position;
//anotatedAnim.movement_LeftFoot.position[1] = 0;
anotatedAnim.movement_LeftHand = anotatedAnim.LeftHand[endSample] - anotatedAnim.LeftHand[0];
anotatedAnim.movement_RightHand = anotatedAnim.RightHand[endSample] - anotatedAnim.RightHand[0];
anotatedAnim.rootDisplacement = anotatedAnim.Root[endSample].position - anotatedAnim.Root[0].position;
anotatedAnim.rootDisplacement[1] = 0;
anotatedAnim.distance = anotatedAnim.rootDisplacement.magnitude;
anotatedAnim.speed = anotatedAnim.distance / (anotatedAnim.time * anotatedAnim.totalLength);
//anotatedAnim.angle = Vector3.Angle(new Vector3(1,0,0), anotatedAnim.rootDisplacement) - rootRefAngle;
anotatedAnim.angle = -CalculateAngleOf2Vectors(anotatedAnim.Root[0].orientation, anotatedAnim.rootDisplacement);
if (anotatedAnim.type == LocomotionMode.WalkTurn || anotatedAnim.type == LocomotionMode.Turn)
{
anotatedAnim.angleCost = Mathf.Abs(Mathf.Abs(anotatedAnim.angle) - Mathf.Abs(anotatedAnim.rotationAngle)) / 180;
}
else
{
anotatedAnim.angleCost = Mathf.Abs(anotatedAnim.angle); // / 90;
}
}
//This constructor creates a previous state given a current state and an action.
//The dummyType is to differentiate it from the constructor used to create successors states.
public FootstepPlanningState(FootstepPlanningState currentState, AnotatedAnimation anim,
float speed, AnotatedAnimation pre, float dummyType)
{
previousState = currentState;
actionName = anim.name;
actionSpeed = speed;
time = currentState.time;
ComputePreviousState(anim);
preconditions = pre;
}
public void analyzeState( AnotatedAnimation anotatedAnim , int sample, float normalizedTime)
{
Vector3 rootDisplacement = root.position - anotatedAnim.RootCurve.initRootPos;
// if it is the last sample we want to set back the height
// TODO: not do this if we want animations where we want the end position higher.
if (sample == samples-1)
rootDisplacement.y = 0;
//anotatedAnim.RootCurve.xPos.AddKey(normalizedTime,rootDisplacement.x);
//anotatedAnim.RootCurve.yPos.AddKey(normalizedTime,rootDisplacement.y);
//anotatedAnim.RootCurve.zPos.AddKey(normalizedTime,rootDisplacement.z);
float rotY = computeForwardRotY() - anotatedAnim.RootCurve.initRotY;
//anotatedAnim.RootCurve.yRot.AddKey(normalizedTime,rotY);
anotatedAnim.Root[sample].position = root.position;
//anotatedAnim.Root[sample].angle = root.localRotation.eulerAngles.y;
//anotatedAnim.Root[sample].orientation = new Vector3(Mathf.Cos(anotatedAnim.Root[sample].angle*Mathf.Deg2Rad),0,Mathf.Sin(anotatedAnim.Root[sample].angle*Mathf.Deg2Rad));
anotatedAnim.Root[sample].orientation = -root.right;
anotatedAnim.Root[sample].orientation.Normalize();
anotatedAnim.Root[sample].angle = CalculateAngle(anotatedAnim.Root[sample].orientation.x,anotatedAnim.Root[sample].orientation.z);
anotatedAnim.LeftFoot[sample].position = leftFoot.position;
anotatedAnim.RightFoot[sample].position = rightFoot.position;
anotatedAnim.LeftFoot[sample].orientation = leftToe.position - leftFoot.position;
anotatedAnim.LeftFoot[sample].orientation[1] = 0;
anotatedAnim.RightFoot[sample].orientation = rightToe.position - rightFoot.position;
anotatedAnim.RightFoot[sample].orientation[1] = 0;
anotatedAnim.LeftFoot[sample].angle = Vector3.Angle(new Vector3(1,0,0), anotatedAnim.LeftFoot[sample].orientation);
anotatedAnim.RightFoot[sample].angle = Vector3.Angle(new Vector3(1,0,0), anotatedAnim.RightFoot[sample].orientation);
anotatedAnim.LeftHand[sample] = leftHand.position;
anotatedAnim.RightHand[sample] = rightHand.position;
}
