public bool CheckAgentRootCollisions(FootstepPlanningState state,
FootstepPlanningState prevNeighborState, FootstepPlanningState neighborState)
{
if (neighborState == null)
return false;
Vector3 neighborPosition = neighborState.currentPosition;
if (prevNeighborState != null)
{
float stateTime = state.time;
float interval = neighborState.time - prevNeighborState.time;
float a = 0;
if (interval > 0)
a = (stateTime - prevNeighborState.time)/interval;
//float b = (neighborState.time - stateTime)/interval;
Vector3 neighborTrajectory = neighborState.currentPosition - prevNeighborState.currentPosition;
neighborPosition = prevNeighborState.currentPosition + a*(neighborTrajectory);
}
float distance = (state.currentPosition - neighborPosition).magnitude;
//float distance = (state.currentPosition - prevNeighborState.currentPosition).magnitude;
//Debug.Log("Distance = " + (distance - 4*analyzer.GetRadius()));
//if (distance - 4*analyzer.GetRadius() < 0)
//Debug.Log("Possible collision detected");
return distance - 2*analyzer.GetRadius() < 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);
}
// We are going to check for collisions from a coarse resolution to a finer grain resolution
private bool CollisionCheck(FootstepPlanningState state)
{
bool isColliding = true;
// For every domain we have, we are going to make a collision check, from the coarser to the finer
int d = 0;
int numberOfDomains = planning.GetNumberOfDomains();
while (isColliding && d < numberOfDomains)
{
isColliding = planning.CheckDomainStateCollisions(d, state);
d++;
}
// if collisions are not found in some of the checks we return false
if (!isColliding)
{
return(false);
}
if (meshCollisions)
{
// if collisions are found for all our previous checks
// We are going a make a final check at the coarser possible resolution
// using the whole mesh
return(MeshCollisionCheck());
}
else
{
return(true);
}
}
// 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);
}
override public bool CheckStateCollisions(DefaultState Dstate)
{
//float startTime = Time.realtimeSinceStartup;
FootstepPlanningState state = Dstate as FootstepPlanningState;
//return CheckRootCollisions(state) || CheckAgentsCollisions(state);
//return CheckJointsCollisions(state) || CheckAgentsCollisions(state);
bool b = (CheckJointsCollisions(state) || CheckAgentsCollisions(state) || CheckDynamicObstaclesCollisions(state));
/*
* 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;
* }
*/
return(b);
}
//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);
}
public GridPlanningState(FootstepPlanningState footstepState)
{
//float startTime = Time.realtimeSinceStartup;
currentPosition = footstepState.currentPosition;
previousState = footstepState.previousState;
if (previousState != null)
actionMov = currentPosition - footstepState.previousState.currentPosition;
else
actionMov = new Vector3(0,0,0);
/*
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;
}
*/
}
//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;
}
*/
}
//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;
* }
*/
}
public GridPlanningState(FootstepPlanningState footstepState)
{
//float startTime = Time.realtimeSinceStartup;
currentPosition = footstepState.currentPosition;
previousState = footstepState.previousState;
if (previousState != null)
{
actionMov = currentPosition - footstepState.previousState.currentPosition;
}
else
{
actionMov = new Vector3(0, 0, 0);
}
/*
* 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;
* }
*/
}
public bool CurrentActionCollisionCheck()
{
if (engine == null)
{
return(false);
}
if (!initialized)
{
return(false);
}
if (engine.action == null)
{
return(false);
}
FootstepPlanningState actionEndState = engine.action.state as FootstepPlanningState;
if (actionEndState != null)
{
return(CollisionCheck(actionEndState));
}
else
{
return(false);
}
}
public override bool ComputePlan()
{
if (currentState == null)
currentState = InitState();
goalState = new FootstepPlanningState(goalStateTransform.position, Quaternion.identity,goalTime);
currentGoal = goalStateTransform.position;
if(!planInitialized){
outputPlan = new Stack<DefaultAction>();
planInitialized = true;
}
currentState.time = Time.time;
DefaultState DcurrentState = currentState as DefaultState;
DefaultState DgoalState = goalState as DefaultState;
//Debug.Log("START: " + currentState.currentPosition);
//Debug.Log("START TRANSFORM: " + currentStateTransform.position);
//Debug.Log("GOAL: " + goalState.currentPosition);
Debug.Log("GOAL TRANSFORM: " + goalStateTransform.position);
bool planComputed = planner.computePlan(ref DcurrentState, ref DgoalState, ref outputPlan, inflationFactor, maxTime);
if (goalMeshRenderer!= null && goalStateTransform.position == currentGoal)
{
if(planComputed && goalCompletedMaterial != null)
goalMeshRenderer.material = goalCompletedMaterial;
else
goalMeshRenderer.material = goalMaterial;
}
return (planComputed && outputPlan.Count > 0);
}
override public float estimateTotalCost(ref DefaultState DcurrentState, ref DefaultState DidealGoalState, float currentg)
{
//float startTime = Time.realtimeSinceStartup;
FootstepPlanningState currentState = DcurrentState as FootstepPlanningState;
FootstepPlanningState idealGoalState = DidealGoalState as FootstepPlanningState;
//Debug.Log("Estimating cost");
// For now we are just estimating the cost as the distance between states;
Vector3 toGoal = idealGoalState.currentPosition - currentState.currentPosition;
/*
* float spaceCost = toGoal.magnitude / analyzer.meanStepSize;
* float timeCost = Int32.MaxValue;
* if (idealGoalState.time - currentState.time > 0)
* timeCost = toGoal.magnitude / (idealGoalState.time - currentState.time);
*
* float angle = -currentState.currentRotation.eulerAngles.y;
* Vector3 orientation = new Vector3(Mathf.Cos(angle*Mathf.Deg2Rad),0,Mathf.Sin(angle*Mathf.Deg2Rad));
* toGoal.y = 0;
* float toGoalAngle = Vector3.Angle(orientation,toGoal);
*
* //Debug.Log("ToGoalAngle = " + toGoalAngle);
*
* float angleCost = Mathf.Abs(toGoalAngle) / 180;
*
* //float h = timeCost;
* //float h = spaceCost;
* float h = spaceCost + timeCost;
* //float h = spaceCost + timeCost + angleCost;
*/
float h = 2 * analyzer.mass * toGoal.magnitude * Mathf.Sqrt(FootstepPlanningAction.e_s * FootstepPlanningAction.e_w);
float f = currentg + W * h;
//Debug.Log("Estimated cost = " + f);
/*
* 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;
* }
*/
return(f);
}
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 static float timeSum = 0.0f;
//public static int numCalls = 0;
public override bool equals(DefaultState s1, DefaultState s2, bool isStart)
{
//float startTime = Time.realtimeSinceStartup;
bool b = false;
FootstepPlanningState state1 = s1 as FootstepPlanningState;
FootstepPlanningState state2 = s2 as FootstepPlanningState;
if (isStart)
{
if ((Mathf.Abs(state1.currentPosition.x - state2.currentPosition.x) < .5) &&
(Mathf.Abs(state1.currentPosition.y - state2.currentPosition.y) < .5) &&
(Mathf.Abs(state1.currentPosition.z - state2.currentPosition.z) < .5))
{
b = true; //return true;
}
else
{
b = false; //return false;
}
}
else
{
if ((Mathf.Abs(state1.currentPosition.x - state2.currentPosition.x) < .1) &&
(Mathf.Abs(state1.currentPosition.y - state2.currentPosition.y) < .1) &&
(Mathf.Abs(state1.currentPosition.z - state2.currentPosition.z) < .1))
{
b = true; //return true;
}
else
{
b = false; //return false;
}
}
/*
* 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;
* }
*/
return(b);
}
// computes the min and max animatino speeds of an action that gives the character a speed of actionSpeed
public void ComputeMinAndMaxAnimSpeeds(FootstepPlanningState currentState, FootstepPlanningState previousState,
float actionSpeed, ref float minAnimSpeed, ref float maxAnimSpeed)
{
if (actionSpeed == 0.0f)
{
minAnimSpeed = 1.0f;
maxAnimSpeed = 1.0f;
return;
}
else
{
float previousAcceleration = previousState.currentAcceleration;
float currentAcceleration = currentState.currentAcceleration;
float minAcceleration = 0.0f;
float maxAcceleration = 0.0f;
bool accelerating = currentAcceleration >= previousAcceleration;
// TODO: revise this part of the code
if (accelerating)
{
minAcceleration = currentAcceleration - 0.2f;
maxAcceleration = currentAcceleration + 0.4f;
}
else
{
minAcceleration = currentAcceleration - 0.4f;
maxAcceleration = currentAcceleration + 0.2f;
}
float currentSpeed = currentState.currentSpeed;
float minSpeed = currentSpeed + minAcceleration;
float maxSpeed = currentSpeed + maxAcceleration;
if (minSpeed < 0)
{
minSpeed = 0.01f;
}
if (maxSpeed > MAX_HUMAN_SPEED)
{
maxSpeed = MAX_HUMAN_SPEED;
}
minAnimSpeed = minSpeed / actionSpeed;
maxAnimSpeed = maxSpeed / actionSpeed;
if (maxAnimSpeed > 1)
{
maxAnimSpeed = 1;
}
}
}
override public bool isAGoalState(ref DefaultState Dstate, ref DefaultState DidealGoalState)
{
//float startTime = Time.realtimeSinceStartup;
FootstepPlanningState state = Dstate as FootstepPlanningState;
FootstepPlanningState idealGoalState = DidealGoalState as FootstepPlanningState;
// A state is a goal one if it's really close to the goal
Vector3 toGoal = idealGoalState.currentPosition - state.currentPosition;
//Debug.Log("distance to goal = " + toGoal.magnitude);
float timeWindow = window * analyzer.maxActionDuration;
bool inTime = (state.time <= (idealGoalState.time + timeWindow))
//&& (state.time >= (idealGoalState.time - timeWindow) )
;
//if (inTime)
// Debug.Log("Max action duration = " + analyzer.maxActionDuration + " and " + state.time + " -> In time!");
/*
* if
* (
* (toGoal.magnitude/analyzer.maxStepSize < 0.5)
* && inTime
* )
* Debug.Log("GoalState found");
*/
bool b = (
(toGoal.magnitude / analyzer.maxStepSize < 0.5) &&
inTime
);
/*
* 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;
* }
*/
return(b);
}
public bool FrameCollisionCheck()
{
if (!initialized)
{
return(false);
}
// we create a state determining the current frame
FootstepPlanningState frameState = CreateFrameState();
return(CollisionCheck(frameState));
}
override public float estimateTotalCost(ref DefaultState DcurrentState, ref DefaultState DidealGoalState, float currentg)
{
//float startTime = Time.realtimeSinceStartup;
GridPlanningState currentState = DcurrentState as GridPlanningState;
GridPlanningState idealGoalState = DidealGoalState as GridPlanningState;
if (currentState == null)
{
FootstepPlanningState fsState = DcurrentState as FootstepPlanningState;
currentState = new GridPlanningState(fsState);
}
//Debug.Log("Estimating cost");
Vector3 toGoal;
if (idealGoalState != null)
{
// A state is a goal one if it's really close to the goal
toGoal = idealGoalState.currentPosition - currentState.currentPosition;
}
else
{
FootstepPlanningState fsIdealGoalState = DidealGoalState as FootstepPlanningState;
toGoal = fsIdealGoalState.currentPosition - currentState.currentPosition;
}
float h = toGoal.magnitude / analyzer.meanStepSize;
float f = currentg + W * h;
//Debug.Log("Estimated cost = " + f);
/*
* 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;
* }
*/
return(f);
}
//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;
}
public FootstepPlanningState InitState()
{
analyzer.RemoveAnimations(animation);
//FootstepPlanningState initState = new FootstepPlanningState(root.position, currentStateTransform.rotation,0);
FootstepPlanningState initState = new FootstepPlanningState(root.position, root.rotation, Time.time);
initState.leftFoot = leftFoot.position;
initState.rightFoot = rightFoot.position;
initState.leftHand = leftHand.position;
initState.rightHand = rightHand.position;
initState.previousState = initState;
return(initState);
}
override public void generateTransitions(ref DefaultState DcurrentState, ref DefaultState DpreviousState,
ref DefaultState DidealGoalState, ref List <DefaultAction> transitions)
{
GridPlanningState currentState = DcurrentState as GridPlanningState;
GridPlanningState idealGoalState = DidealGoalState as GridPlanningState;
if (currentState == null)
{
FootstepPlanningState fsState = DcurrentState as FootstepPlanningState;
if (fsState != null)
{
currentState = new GridPlanningState(fsState);
}
else
{
currentState = new GridPlanningState(DcurrentState as GridTimeState);
}
}
if (idealGoalState == null)
{
FootstepPlanningState fsIdealGoalState = DidealGoalState as FootstepPlanningState;
if (fsIdealGoalState != null)
{
idealGoalState = new GridPlanningState(fsIdealGoalState);
}
else
{
idealGoalState = new GridPlanningState(DidealGoalState as GridTimeState);
}
}
foreach (Vector3 mov in movDirections)
{
GridPlanningAction newAction = new GridPlanningAction(currentState, mov);
//if (!CheckTransitionCollisions(newAction.state,DcurrentState))
if (!CheckStateCollisions(newAction.state))
{
//Debug.Log(Time.time + ": grid successor generated");
transitions.Add(newAction);
}
}
//Debug.Log(transitions.Count + " grid transitions generated at time " + Time.time);
}
public 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++;
}
}
}
override public bool CheckStateCollisions(DefaultState Dstate)
{
//float startTime = Time.realtimeSinceStartup;
GridPlanningState state = Dstate as GridPlanningState;
bool b = false;
// If we are checking a state of this domain
if (state != null)
{
b = CheckRootCollisions(state); //return CheckRootCollisions(state);
}
else // If not
{
// We need to transform the state of the coarser domain
// to our low resolution domain
FootstepPlanningState footstepState = Dstate as FootstepPlanningState;
if (footstepState != null)
{
state = new GridPlanningState(footstepState);
b = CheckRootCollisions(state); //return CheckRootCollisions(state);
}
else
{
b = false; //return false;
}
}
/*
* 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;
* }
*/
return(b);
}
//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(Vector3 mov)
{
GridPlanningState prevGridState = previousState as GridPlanningState;
if (prevGridState != null)
{
currentPosition = prevGridState.currentPosition - mov;
}
else
{
FootstepPlanningState prevFootstepState = previousState as FootstepPlanningState;
if (prevFootstepState != null)
{
currentPosition = prevFootstepState.currentPosition - mov;
}
}
}
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;
}
public override bool ComputePlan()
{
if (currentState == null)
{
currentState = InitState();
}
goalState = new FootstepPlanningState(goalStateTransform.position, Quaternion.identity, goalTime);
currentGoal = goalStateTransform.position;
if (!planInitialized)
{
outputPlan = new Stack <DefaultAction>();
planInitialized = true;
}
currentState.time = Time.time;
DefaultState DcurrentState = currentState as DefaultState;
DefaultState DgoalState = goalState as DefaultState;
//Debug.Log("START: " + currentState.currentPosition);
//Debug.Log("START TRANSFORM: " + currentStateTransform.position);
//Debug.Log("GOAL: " + goalState.currentPosition);
Debug.Log("GOAL TRANSFORM: " + goalStateTransform.position);
bool planComputed = planner.computePlan(ref DcurrentState, ref DgoalState, ref outputPlan, inflationFactor, maxTime);
if (goalMeshRenderer != null && goalStateTransform.position == currentGoal)
{
if (planComputed && goalCompletedMaterial != null)
{
goalMeshRenderer.material = goalCompletedMaterial;
}
else
{
goalMeshRenderer.material = goalMaterial;
}
}
return(planComputed && outputPlan.Count > 0);
}
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 FootstepPlanningState CreateFrameState()
{
if (!initialized)
{
return(null);
}
/*
* Quaternion rot;
* if (planning.currentState != null)
* rot = planning.currentState.currentRotation;
* else
* {
* float initAngle = this.gameObject.transform.eulerAngles.y;
* rot = Quaternion.AngleAxis(initAngle,new Vector3(0,1,0));
* }
*
* FootstepPlanningState frameState;
* if (engine.action != null && engine.action.state != null)
* {
* frameState = new FootstepPlanningState(engine.action.state as FootstepPlanningState);
*
* frameState.currentPosition = root.position;
* frameState.currentRotation = rot;
*
* frameState.time = Time.time;
* }
* else
* {
* frameState = new FootstepPlanningState(root.position,rot,Time.time);
* }
*/
//FootstepPlanningState frameState = new FootstepPlanningState(engine.lastRootPos,engine.lastRootRot,Time.time);
FootstepPlanningState frameState = new FootstepPlanningState(transform.position, transform.rotation, Time.time);
frameState.leftFoot = leftFoot.position;
frameState.rightFoot = rightFoot.position;
frameState.leftHand = leftHand.position;
frameState.rightHand = rightHand.position;
return(frameState);
}
public FootstepPlanningState CreateFrameState()
{
if (!initialized)
return null;
/*
Quaternion rot;
if (planning.currentState != null)
rot = planning.currentState.currentRotation;
else
{
float initAngle = this.gameObject.transform.eulerAngles.y;
rot = Quaternion.AngleAxis(initAngle,new Vector3(0,1,0));
}
FootstepPlanningState frameState;
if (engine.action != null && engine.action.state != null)
{
frameState = new FootstepPlanningState(engine.action.state as FootstepPlanningState);
frameState.currentPosition = root.position;
frameState.currentRotation = rot;
frameState.time = Time.time;
}
else
{
frameState = new FootstepPlanningState(root.position,rot,Time.time);
}
*/
//FootstepPlanningState frameState = new FootstepPlanningState(engine.lastRootPos,engine.lastRootRot,Time.time);
FootstepPlanningState frameState = new FootstepPlanningState(transform.position,transform.rotation,Time.time);
frameState.leftFoot = leftFoot.position;
frameState.rightFoot = rightFoot.position;
frameState.leftHand = leftHand.position;
frameState.rightHand = rightHand.position;
return frameState;
}
public override float ComputeEstimate(ref DefaultState Dfrom, ref DefaultState Dto, string estimateType)
{
GridPlanningState _from = Dfrom as GridPlanningState;
GridPlanningState _to = Dto as GridPlanningState;
if (_from == null)
{
FootstepPlanningState fsState = Dfrom as FootstepPlanningState;
_from = new GridPlanningState(fsState);
}
if (estimateType == "g")
{
return(Mathf.Abs(Vector3.Distance(_to.currentPosition, _from.currentPosition)));
}
else if (estimateType == "h")
{
Vector3 fromStart;
if (_to != null)
{
// A state is a goal one if it's really close to the goal
fromStart = _to.currentPosition - _from.currentPosition;
}
else
{
FootstepPlanningState fsIdealGoalState = Dto as FootstepPlanningState;
fromStart = fsIdealGoalState.currentPosition - _from.currentPosition;
}
float spaceCost = fromStart.magnitude / analyzer.meanStepSize;
return(spaceCost);
}
else
{
return(0.0f);
}
}
public override float ComputeEstimate(ref DefaultState Dfrom, ref DefaultState Dto, string estimateType)
{
FootstepPlanningState _from = Dfrom as FootstepPlanningState;
FootstepPlanningState _to = Dto as FootstepPlanningState;
if (estimateType == "g")
{
Vector3 toGoal = _to.currentPosition - _from.currentPosition;
float spaceCost = toGoal.magnitude / analyzer.meanStepSize;
float timeCost = Int32.MaxValue;
if (_to.time - _from.time > 0)
{
timeCost = toGoal.magnitude / (_to.time - _from.time);
}
return(Mathf.Abs(Vector3.Distance(_to.currentPosition, _from.currentPosition)));
//return (spaceCost + timeCost);
//return spaceCost;
}
else if (estimateType == "h")
{
Vector3 fromStart = _to.currentPosition - _from.currentPosition;
float spaceCost = fromStart.magnitude / analyzer.meanStepSize;
float timeCost = Int32.MaxValue;
if (_to.time - _from.time > 0)
{
timeCost = fromStart.magnitude / (_to.time - _from.time);
}
//return (Mathf.Abs(Vector3.Distance(_to.currentPosition, _from.currentPosition)));
return(spaceCost + timeCost);
//return spaceCost;
}
else
{
return(0.0f);
}
}
public bool CheckObstacleCollision(FootstepPlanningState state, ObstaclesScript script)
{
//if (state == null || script == null)
// return false;
if (script.ShapeChoice.Shape == Enum.Enumeration.Sphere)
{
Vector3 obstaclePos = script.gameObj.transform.position;
Vector3 aux = obstaclePos - state.currentPosition;
float sphereRadius = script.size.x;
return((aux.magnitude - sphereRadius - analyzer.GetRadius()) <= 0);
}
else if (script.ShapeChoice.Shape == Enum.Enumeration.Rectangle)
{
Bounds box = script.gameObj.collider.bounds;
return(box.SqrDistance(state.currentPosition) <= analyzer.GetRadius());
}
return(false);
}
public bool CheckAgentRootCollisions(FootstepPlanningState state,
FootstepPlanningState prevNeighborState, FootstepPlanningState neighborState)
{
if (neighborState == null)
{
return(false);
}
Vector3 neighborPosition = neighborState.currentPosition;
if (prevNeighborState != null)
{
float stateTime = state.time;
float interval = neighborState.time - prevNeighborState.time;
float a = 0;
if (interval > 0)
{
a = (stateTime - prevNeighborState.time) / interval;
}
//float b = (neighborState.time - stateTime)/interval;
Vector3 neighborTrajectory = neighborState.currentPosition - prevNeighborState.currentPosition;
neighborPosition = prevNeighborState.currentPosition + a * (neighborTrajectory);
}
float distance = (state.currentPosition - neighborPosition).magnitude;
//float distance = (state.currentPosition - prevNeighborState.currentPosition).magnitude;
//Debug.Log("Distance = " + (distance - 4*analyzer.GetRadius()));
//if (distance - 4*analyzer.GetRadius() < 0)
//Debug.Log("Possible collision detected");
return(distance - 2 * analyzer.GetRadius() < 0);
}
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);
}
// 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 FootstepPlanningState InitState()
{
analyzer.RemoveAnimations(animation);
//FootstepPlanningState initState = new FootstepPlanningState(root.position, currentStateTransform.rotation,0);
FootstepPlanningState initState = new FootstepPlanningState(root.position, root.rotation,Time.time);
initState.leftFoot = leftFoot.position;
initState.rightFoot = rightFoot.position;
initState.leftHand = leftHand.position;
initState.rightHand = rightHand.position;
initState.previousState = initState;
return initState;
}
public override bool CheckDomainStateCollisions(int d, FootstepPlanningState state)
{
return domainList.ElementAt(d).CheckStateCollisions(state);
}
// computes the min and max animatino speeds of an action that gives the character a speed of actionSpeed
public void ComputeMinAndMaxAnimSpeeds(FootstepPlanningState currentState, FootstepPlanningState previousState,
float actionSpeed, ref float minAnimSpeed, ref float maxAnimSpeed)
{
if (actionSpeed == 0.0f)
{
minAnimSpeed = 1.0f;
maxAnimSpeed = 1.0f;
return;
}
else
{
float previousAcceleration = previousState.currentAcceleration;
float currentAcceleration = currentState.currentAcceleration;
float minAcceleration = 0.0f;
float maxAcceleration = 0.0f;
bool accelerating = currentAcceleration >= previousAcceleration;
// TODO: revise this part of the code
if (accelerating)
{
minAcceleration = currentAcceleration - 0.2f;
maxAcceleration = currentAcceleration + 0.4f;
}
else
{
minAcceleration = currentAcceleration - 0.4f;
maxAcceleration = currentAcceleration + 0.2f;
}
float currentSpeed = currentState.currentSpeed;
float minSpeed = currentSpeed + minAcceleration;
float maxSpeed = currentSpeed + maxAcceleration;
if ( minSpeed < 0 ) minSpeed = 0.01f;
if ( maxSpeed > MAX_HUMAN_SPEED ) maxSpeed = MAX_HUMAN_SPEED;
minAnimSpeed = minSpeed / actionSpeed;
maxAnimSpeed = maxSpeed / actionSpeed;
if (maxAnimSpeed > 1) maxAnimSpeed = 1;
}
}
//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 FootstepPlanningState(GridPlanningState gridState)
{
//float startTime = Time.realtimeSinceStartup;
this.actionName = "GridAction";
//this.actionSpeed = 1.0f;
//this.currentAcceleration = 1.0f;
this.previousState = gridState.previousState as FootstepPlanningState;
this.currentPosition = gridState.currentPosition;
//this.currentRotation = gridState.previousState.currentRotation;
//this.currentSpeed = 1.0f;
//this.leftFoot = gridState.currentPosition;
//this.leftHand = gridState.currentPosition;
//this.obstaclePos = original.obstaclePos;
//this.preconditions = original.preconditions;
//this.rightFoot = gridState.currentPosition;
//this.rightHand = gridState.currentPosition;
//this.time = original.time;
/*
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;
}
*/
}
// 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 bool CheckObstacleCollision(FootstepPlanningState state, ObstaclesScript script)
{
//if (state == null || script == null)
// return false;
if (script.ShapeChoice.Shape == Enum.Enumeration.Sphere)
{
Vector3 obstaclePos = script.gameObj.transform.position;
Vector3 aux = obstaclePos - state.currentPosition;
float sphereRadius = script.size.x;
return (aux.magnitude - sphereRadius - analyzer.GetRadius()) <= 0;
}
else if (script.ShapeChoice.Shape == Enum.Enumeration.Rectangle)
{
Bounds box = script.gameObj.collider.bounds;
return box.SqrDistance(state.currentPosition) <= analyzer.GetRadius();
}
return false;
}
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;
}
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;
}
// We are going to check for collisions from a coarse resolution to a finer grain resolution
private bool CollisionCheck(FootstepPlanningState state)
{
bool isColliding = true;
// For every domain we have, we are going to make a collision check, from the coarser to the finer
int d = 0;
int numberOfDomains = planning.GetNumberOfDomains();
while ( isColliding && d < numberOfDomains)
{
isColliding = planning.CheckDomainStateCollisions(d,state);
d++;
}
// if collisions are not found in some of the checks we return false
if (!isColliding)
return false;
if (meshCollisions)
{
// if collisions are found for all our previous checks
// We are going a make a final check at the coarser possible resolution
// using the whole mesh
return MeshCollisionCheck();
}
else
return true;
}
// 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;
}
public override bool ComputePlan()
{
bool stateWasNull = false;
if (currentState == null)
{
currentState = InitState();
stateWasNull = true;
}
goalState = new FootstepPlanningState(currentGoal, Quaternion.identity,goalTime);
outputPlan = new Stack<DefaultAction>();
if (stateWasNull || goalReached && !stateWasNull)
{
if (currentState.previousState != null)
{
float actionLength = currentState.time - currentState.previousState.time;
currentState.previousState.time = Time.time - actionLength;
}
currentState.time = Time.time;
}
DefaultState DcurrentState = currentState as DefaultState;
DefaultState DgoalState = goalState as DefaultState;
if (useGridDomain)
{
GridPlanningState currentGridState = new GridPlanningState(root.position);
DcurrentState = currentGridState as DefaultState;
GridPlanningState goalGridState = new GridPlanningState(currentGoal);
DgoalState = goalGridState as DefaultState;
}
if (useGridTimeDomain)
{
GridTimeState currentTimeState = new GridTimeState(root.position, Time.time, currentSpeed);
DcurrentState = currentTimeState as DefaultState;
GridTimeState goalTimeState = new GridTimeState(currentGoal,goalTime);
DgoalState = goalTimeState as DefaultState;
//Debug.Log("current time = " + currentTimeState.time);
}
//float startTime = Time.realtimeSinceStartup;
bool planComputed = planner.computePlan(ref DcurrentState, ref DgoalState, ref outputPlan, maxTime);
/*************************/
/* TUNNEL SEARCH TESTING */
//List<Vector3> movDirections = (domainList[0] as GridPlanningDomain).getMovDirections();
//TunnelSearch tunnelSearch = new TunnelSearch(outputPlan, 3, analyzer, movDirections);
if (TunnelSearch)
{
if (planComputed)
{
GridTunnelSearch gridTunnelSearch = new GridTunnelSearch(outputPlan, root.position,
Time.time, goalState.time, gtd,
MaxNumberOfNodes, maxTime,
//tunnelThresholdX, tunnelThresholdZ, tunnelThresholdT);
tunnelThresholdD, tunnelThresholdT,
currentSpeed);
outputPlanBeforeTunnel = outputPlan;
outputPlan = gridTunnelSearch.newPlan;
planComputed = gridTunnelSearch.goalReached;
//Debug.Log("At time " + Time.time + " tunnel plan computed? " + planComputed);
}
}
/*************************/
/*
float endTime = Time.realtimeSinceStartup;
timeSum += (endTime - startTime);
numCalls++;
float meanTime = 0.0f;
if (numCalls == 100)
{
meanTime = timeSum / numCalls;
Debug.Log("At time " + Time.time + " Mean Computing Plan Time = " + meanTime);
numCalls = 0;
timeSum = 0;
}
*/
if (goalMeshRenderer!= null && goalStateTransform.position == currentGoal)
{
if(planComputed && goalCompletedMaterial != null)
goalMeshRenderer.material = goalCompletedMaterial;
else
goalMeshRenderer.material = goalMaterial;
}
//Debug.Log("new plan! at time: " + Time.time + " with " + outputPlan.Count + " actions");
goalReached = false;
/*
if (planComputed)
{
AnimationCurve[] curves = GetPlanAnimationCurve();
curveX = curves[0];
curveY = curves[1];
curveZ = curves[2];
}
*/
return (planComputed && outputPlan.Count > 0);
}
// Collisions check with deterministic dynamic obstacles
public bool CheckDynamicObstaclesCollisions(FootstepPlanningState state)
{
if (obstacles == null)
return false;
if (obstacles.closeObstacles == null)
return false;
List<DDObstacle> visibleObstacles = obstacles.visibleObstacles;
if (visibleObstacles.Count <= 0)
return false;
//float[] actualTime = new float[visibleObstacles.Count];
// Move obstacles
//int i = 0;
foreach(DDObstacle ddObstacle in visibleObstacles)
{
ObstaclesScript obstacleScript = ddObstacle.script;
float timeWindow = 0;
//for (float timeWindow = -0.5f; timeWindow <= 0.5f; timeWindow += 0.5f)
{
if (obstacleScript != null)
{
//actualTime[i] = obstacleScript.animation["test"].time;
float actualTime = obstacleScript.animation["test"].time;
float time = state.time + timeWindow;
obstacleScript.animation["test"].wrapMode = WrapMode.PingPong;
obstacleScript.animation["test"].enabled = true;
obstacleScript.animation["test"].time = time;
obstacleScript.animation.Play("test");
obstacleScript.animation.Sample();
//obstacleScript.animation["test"].enabled = false;
//Debug.DrawRay(pair.Value.obstacle.transform.position,4*Vector3.up,Color.blue);
state.obstaclePos = ddObstacle.obstacle.transform.position;
// Check collisions
//bool collision = CheckJointsCollisions(state);
//bool collision = CheckRootCollisions(state);
//Vector3 aux = state.obstaclePos - state.currentPosition;
//bool collision = (aux.magnitude - 1 - analyzer.GetRadius()) < 0;
bool collision = CheckObstacleCollision(state,obstacleScript);
//if (collision)
{
//Debug.DrawRay(state.currentPosition,4*Vector3.up,Color.yellow);
//Debug.Log("Possible collision detected at" + state.time);
//Debug.Break ();
}
obstacleScript.animation["test"].enabled = true;
//obstacleScript.animation["test"].time = actualTime[i];
obstacleScript.animation["test"].time = actualTime;
obstacleScript.animation.Play("test");
obstacleScript.animation.Sample();
if (collision)
return true;
}
}
}
return false;
}
