// used by ara star + used by evaluateTotalCost which is used by best first search
public override float ComputeHEstimate(DefaultState _from, DefaultState _to)
{
GridTimeState from = _from as GridTimeState;
GridTimeState to = _to as GridTimeState;
/*
* float distanceToGoal = Vector3.Distance(to.currentPosition, from.currentPosition);
* float timeToGoal = to.time - from.time;
* float acceleration = Mathf.Abs( 2 * ( distanceToGoal - from.speed * timeToGoal ) / (timeToGoal * timeToGoal)) ;
* float velocityRequiredToReachGoal = from.speed + acceleration * timeToGoal;
*
* float e1 = Mathf.Abs (from.speed * from.speed - velocityRequiredToReachGoal * velocityRequiredToReachGoal);
* if (velocityRequiredToReachGoal > GridTimeDomain.MAX_SPEED )
* {
* //Debug.Log ("e1 is infinity");
* e1 = Mathf.Infinity;
* }
*/
//float e2 = Mathf.Abs(GridTimeDomain.DESIRED_SPEED_SQUARE - velocityRequiredToReachGoal * velocityRequiredToReachGoal);
//float e3 = distanceToGoal;
Vector3 fromStart;
fromStart = to.currentPosition - from.currentPosition;
float spaceCost = fromStart.magnitude / meanStepSize;
//Debug.Log("old h = " + spaceCost + " HEURISTIC = " + (e1+e2+e3) + " e1 " + e1 + " e2 " + e2 + "e3 " + e3 );
return(spaceCost);
//return (spaceCost + e1);
//return e1+e2+e3;
}
//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;
//Debug.Log("hi, " +Time.time);
bool b = false;
GridTimeState state1 = s1 as GridTimeState;
GridTimeState state2 = s2 as GridTimeState;
// MUBBASIR TODO : WHY DONT U CONSIDER TIME HERE ??
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);
}
// Use this for initialization
void Start()
{
planner = new ARAstarPlanner();
BestFirstPlanner = new BestFirstSearchPlanner();
GetComponent<NeighbourObstacles>().Init ();
GridTimeDomain domain = new GridTimeDomain(GetComponent<NeighbourObstacles>(), GetComponent<NeighbourAgents>());
List<PlanningDomainBase> dl = new List<PlanningDomainBase>();
dl.Add(domain);
DefaultState startState = new GridTimeState(transform.position) as DefaultState;
DefaultState goalState = new GridTimeState(new Vector3(5,0.5f,0),5.0f) as DefaultState;
plan = new Dictionary<DefaultState, ARAstarNode>();
outputPlan = new Stack<DefaultState>();
if(BestFirstSearchPlanner) {
BestFirstPlanner.init(ref dl, 200);
bool completed = BestFirstPlanner.computePlan(ref startState, ref goalState, ref outputPlan, 10.0f);
Debug.Log("BestFirst Plan " + outputPlan.Count);
Debug.Log("Plan found: " + completed);
} else {
planner.init(ref dl,200);
float inflationFactor = 2.5f;
PathStatus status = planner.computePlan(ref startState ,ref goalState ,ref plan,ref inflationFactor, 10.0f);
planner.FillPlan();
Debug.Log ("ARA Plan " + plan.Count);
Debug.Log("Nodes Expanded: " + planner.Close.Elements().Count);
Debug.Log("Status: " + status);
}
}
// used by ara star only --
public override float ComputeGEstimate(DefaultState _from, DefaultState _to)
{
//Debug.Log ("compute g estimate");
GridTimeState from = _from as GridTimeState;
GridTimeState to = _to as GridTimeState;
return(Mathf.Abs(Vector3.Distance(to.currentPosition, from.currentPosition)));
}
public override void generatePredecessors(DefaultState DcurrentState, ref List <DefaultAction> actionList)
{
GridTimeState currentState = DcurrentState as GridTimeState;
GridTimeAction stopAction = new GridTimeAction(currentState, Vector3.zero, 0);
if (!CheckTransitionCollisions(currentState, stopAction.state, false))
{
if (!useTunnel || isInsideTunnel(stopAction.state))
{
actionList.Add(stopAction);
}
}
#if USE_ANGLE_MOV
foreach (float angle in movAngles)
{
Vector3 mov = currentState.actionMov;
mov = Matrix4x4.TRS(Vector3.zero, Quaternion.AngleAxis(angle, Vector3.up), new Vector3(1, 1, 1)) * mov;
mov.Normalize();
#else
foreach (Vector3 mov in movDirections)
{
#endif
foreach (float speedIncr in possibleSpeedIncrements)
{
float newSpeed = currentState.speed + speedIncr;
if (newSpeed > MAX_SPEED)
{
newSpeed = MAX_SPEED;
}
else if (newSpeed < MIN_SPEED)
{
newSpeed = MIN_SPEED;
}
GridTimeState prevState = new GridTimeState();
prevState.ComputePreviousState(currentState, mov, newSpeed);
GridTimeAction newAction = new GridTimeAction(currentState, prevState);
if (!CheckTransitionCollisions(currentState, newAction.state))
{
// If we are not using a tunnel
// or if we are using one and the stata falls inside it
if (!useTunnel || isInsideTunnel(newAction.state))
{
//Debug.Log(Time.time + ": grid successor generated");
actionList.Add(newAction); // we add the action as a transition
}
}
}
}
//Debug.Log(transitions.Count + " grid transitions generated at time " + Time.time);
}
public GridTimeAction(GridTimeState previousState, GridTimeState nextState)
{
state = nextState;
cost = (previousState.speed - nextState.speed)*(previousState.speed - nextState.speed) + (nextState.speed-GridTimeDomain.DESIRED_SPEED)*(nextState.speed-GridTimeDomain.DESIRED_SPEED);
//cost = Vector3.Distance(previousState.statePosition(), nextState.statePosition());
//cost = _calculateCost(previousState, state as GridTimeState);
//Debug.Log ("old cost " + cost + " new cost " + cost1);
}
//Constructor for actions using dummytype for planningstate
// dummy type is used to create predecessor instead of successor
public GridTimeAction(GridTimeState previousState, Vector3 mov, float s, float dummyType)
{
state = new GridTimeState(previousState, mov, s, dummyType);
cost = (previousState.speed - s) * (previousState.speed - s) + (s - GridTimeDomain.DESIRED_SPEED) * (s - GridTimeDomain.DESIRED_SPEED);
//cost = Vector3.Distance(previousState.statePosition(), state.statePosition());
//cost = _calculateCost(previousState, state as GridTimeState);
//Debug.Log ("old cost " + cost + " new cost " + cost1);
}
public GridTimeAction(GridTimeState previousState, GridTimeState nextState)
{
state = nextState;
cost = (previousState.speed - nextState.speed) * (previousState.speed - nextState.speed) + (nextState.speed - GridTimeDomain.DESIRED_SPEED) * (nextState.speed - GridTimeDomain.DESIRED_SPEED);
//cost = Vector3.Distance(previousState.statePosition(), nextState.statePosition());
//cost = _calculateCost(previousState, state as GridTimeState);
//Debug.Log ("old cost " + cost + " new cost " + cost1);
}
//Constructor for actions using dummytype for planningstate
// dummy type is used to create predecessor instead of successor
public GridTimeAction(GridTimeState previousState, Vector3 mov, float s, float dummyType)
{
state = new GridTimeState(previousState, mov, s, dummyType);
cost = (previousState.speed - s)*(previousState.speed - s) + (s-GridTimeDomain.DESIRED_SPEED)*(s-GridTimeDomain.DESIRED_SPEED);
//cost = Vector3.Distance(previousState.statePosition(), state.statePosition());
//cost = _calculateCost(previousState, state as GridTimeState);
//Debug.Log ("old cost " + cost + " new cost " + cost1);
}
bool doTunnelTask()
{
Stack <DefaultAction> convertedTunnel = new Stack <DefaultAction> ();
// convert tunnel
for (int i = 0; i < path.Count; i++)
{
GridPlanningState gs = new GridPlanningState(path[i].getPosition());
GridPlanningAction g = new GridPlanningAction(gs, new Vector3());
convertedTunnel.Push(g);
}
GridTunnelSearch gridTunnelSearch =
new GridTunnelSearch(convertedTunnel,
this.startState.getPosition(), startState._time, goalState._time, gridTimeDomain, 300, 2.0f, 1.0f, 1.0f, startState._speed);
//new GridTunnelSearch(convertedTunnel, startState.getPosition(), 0.0F, 10.0F, gridTimeDomain, 250, 2.0F, 1.0F, 1.0F);
//gridTunnelSearch.tryARAStarPlanner(spaceTimePath);
//Debug.Log ("grid time plan using ara star planner " + spaceTimePath.Count);
//return true;
//// generating plan for now
spaceTimePath.Clear();
/*
* // this is the state plan
* Debug.Log ("grid tunnel plan using best first planner " + gridTunnelSearch.newStatePlan.Count);
* while (gridTunnelSearch.newStatePlan.Count != 0)
* {
* GridTimeState state = gridTunnelSearch.newStatePlan.Pop() as GridTimeState;
* //Debug.LogWarning ("space time state " + state.time);
* spaceTimePath.Add(new State(state.currentPosition, state.time));
* }
*/
// this is the action plan
Debug.Log("grid tunnel plan using best first planner " + gridTunnelSearch.newPlan.Count);
while (gridTunnelSearch.newPlan.Count != 0)
{
DefaultAction action = gridTunnelSearch.newPlan.Pop();
GridTimeState state = action.state as GridTimeState;
//Debug.LogWarning ("space time state " + state.time + " " + state.currentPosition);
spaceTimePath.Add(new State(state.currentPosition, state.time, state.speed));
}
bool planComputed = gridTunnelSearch.goalReached;
//Debug.LogError("grid tunnel plan status " + planComputed);
return(planComputed);
}
// Creation function
//public GridTunnelSearch(Stack<DefaultAction> plan, float startTime, float idealGoalTime, GridTimeDomain gtd, int MaxNumberOfNodes, float MaxTime, float T_x, float T_z, float T_t)
public GridTunnelSearch(Stack<DefaultAction> plan, Vector3 startPos, float startTime, float idealGoalTime, GridTimeDomain gtd, int MaxNumberOfNodes, float MaxTime, float T_d, float T_t, float startSpeed = 0)
{
currentPlan = new DefaultAction[plan.Count];
plan.CopyTo(currentPlan,0);
this.T_d = T_d;
//this.T_x = T_x;
//this.T_z = T_z;
this.T_t = T_t;
this.maxTime = MaxTime;
int startIndex = 0;
//GridPlanningState startPlanningState = currentPlan[startIndex].state as GridPlanningState;
//while (startPlanningState == null)
//{
// startIndex++;
// startPlanningState = currentPlan[startIndex].state as GridPlanningState;
//}
//startState = new GridTimeState(startPlanningState.currentPosition, startTime, startSpeed);
startState = new GridTimeState(startPos, startTime, startSpeed);
lastState = new GridTimeState((currentPlan[currentPlan.Length-1].state as GridPlanningState).currentPosition, idealGoalTime);
gridTimeDomain = gtd;
float dist = (startState.currentPosition - lastState.currentPosition).magnitude;
// TODO: what to do if there is no time???
float minSpeed = dist / (idealGoalTime-startTime);
if (minSpeed < 0)
minSpeed = GridTimeDomain.MIN_SPEED;
//tunnel = ConvertPlan(gtd.minSpeed, gtd.midSpeed, gtd.maxSpeed);
tunnel = ConvertPlan(startTime, minSpeed, GridTimeDomain.MID_SPEED, GridTimeDomain.MAX_SPEED);
//for(int aux = 0; aux < tunnel.tunnelMinVelocity.Length; aux++)
// Debug.Log("Tunnel "+aux+":"+tunnel.tunnelMinVelocity[aux]);
tunnelPlanner = new BestFirstSearchPlanner();
List<PlanningDomainBase> domainList = new List<PlanningDomainBase>();
domainList.Add(gridTimeDomain);
tunnelPlanner.init(ref domainList, MaxNumberOfNodes);
// MUBBASIR TESTING ARA STAR PLANNER HERE
araStarPlanner = new ARAstarPlanner ();
araStarPlanner.init(ref domainList, MaxNumberOfNodes);
goalReached = false;
ComputeTunnelSearch();
//ComputeTunnelSearchStatePlan ();
}
private float _calculateCost(GridTimeState previousState, GridTimeState currentState)
{
// cost for acceleration
float e1 = Mathf.Abs (previousState.speed * previousState.speed - currentState.speed * currentState.speed);
// cost for deviation from desired speed
float e2 = Mathf.Abs(GridTimeDomain.DESIRED_SPEED_SQUARE - currentState.speed * currentState.speed);
// cost for traveling distance
float e3 = Vector3.SqrMagnitude(currentState.currentPosition - previousState.currentPosition);
//Debug.Log("e1 " + e1 + " e2 " + e2 + "e3 " + e3);
return e1+e2+e3;
//return _calculateKineticEnergy (previousState, currentState) + _calculatePotentialEnergy (currentState);
}
private float _calculateCost(GridTimeState previousState, GridTimeState currentState)
{
// cost for acceleration
float e1 = Mathf.Abs(previousState.speed * previousState.speed - currentState.speed * currentState.speed);
// cost for deviation from desired speed
float e2 = Mathf.Abs(GridTimeDomain.DESIRED_SPEED_SQUARE - currentState.speed * currentState.speed);
// cost for traveling distance
float e3 = Vector3.SqrMagnitude(currentState.currentPosition - previousState.currentPosition);
//Debug.Log("e1 " + e1 + " e2 " + e2 + "e3 " + e3);
return(e1 + e2 + e3);
//return _calculateKineticEnergy (previousState, currentState) + _calculatePotentialEnergy (currentState);
}
// Returns the perpendicular distance from the state "state" to the vector/path/edge
// formed by the states s1 and s2
private static float ComputeDistance(GridTimeState state, GridTimeState s1, GridTimeState s2)
{
Vector3 A = s1.currentPosition;
Vector3 B = s2.currentPosition;
Vector3 C = state.currentPosition;
Vector3 AB = B - A;
Vector3 AC = C - A;
//float ABdotAC = Vector3.Dot(AB,AC);
//float distance = Mathf.Sqrt(1 - (ABdotAC*ABdotAC)/(AC.sqrMagnitude * AB.sqrMagnitude)) * AC.magnitude;
// P is the projection of C over AB
Vector3 AP = Vector3.Project(AC, AB);
Vector3 P = A + AP;
Vector3 BP = P - B;
//Debug.DrawLine(A,(A+Vector3.up),Color.red);
//Debug.DrawLine(P,(P+Vector3.up),Color.blue);
//Debug.DrawLine(A,P,Color.green);
float APsqrMagnitude = AP.sqrMagnitude;
float BPsqrMagnitude = BP.sqrMagnitude;
float ABsqrMagnitude = AB.sqrMagnitude;
float distance;
if (APsqrMagnitude <= ABsqrMagnitude && BPsqrMagnitude <= ABsqrMagnitude)
{
// if the projection is between A and B (in the segment)
distance = (P - C).magnitude;
}
else
{
float ACmagnitude = AC.magnitude;
float BCmagnitude = (C - B).magnitude;
if (ACmagnitude < BCmagnitude)
{
distance = ACmagnitude;
}
else
{
distance = BCmagnitude;
}
}
return(distance);
}
/*
* private bool isInsideTunnel(DefaultState state)
* {
* GridTimeState gridTimeState = state as GridTimeState;
* if (gridTimeState == null)
* return false;
*
* float x = gridTimeState.currentPosition.x;
* float z = gridTimeState.currentPosition.z;
* float t = gridTimeState.time;
*
* int i = 0;
* while(i < tunnel.tunnelMidVelocity.Length)
* {
* GridTimeState tunnelState = tunnel.tunnelMidVelocity[i];
* if (tunnelState != null)
* {
* float tunnelX = tunnelState.currentPosition.x;
* float tunnelZ = tunnelState.currentPosition.z;
*
* if (Mathf.Abs(tunnelX - x) < tunnel.thresholdX)
* {
* if (Mathf.Abs(tunnelZ - z) > tunnel.thresholdZ)
* {
* float minTime = tunnel.tunnelMinVelocity[i];
* float maxTime = tunnel.tunnelMaxVelocity[i];
*
* if ( t < minTime || t > maxTime )
* return true;
* }
* }
* }
*
* i++;
* }
*
* return false;
* }
*/
private bool isInsideTunnel(DefaultState state)
{
GridTimeState gridTimeState = state as GridTimeState;
if (gridTimeState == null)
{
return(false);
}
float distanceToTunnel = GridTunnelSearch.ComputeDistanceToTunnel(gridTimeState, tunnel);
//Debug.Log("distanceToTunnel = " + distanceToTunnel + " and threshold = " + tunnel.thresholdD);
//gridTimeState.time = distanceToTunnel;
return(distanceToTunnel <= tunnel.thresholdD);
}
public AnimationCurve[] GetPlanAnimationCurve(Stack <DefaultAction> outputPlan)
{
int numKeys = outputPlan.Count + 1;
AnimationCurve curveX = new AnimationCurve();
AnimationCurve curveY = new AnimationCurve();
AnimationCurve curveZ = new AnimationCurve();
AnimationCurve curveSpeed = new AnimationCurve();
float time = Time.time;
Vector3 pos = currentStateTransform.position;
float speed = currentSpeed;
curveX.AddKey(time, pos.x);
curveY.AddKey(time, pos.y);
curveZ.AddKey(time, pos.z);
curveSpeed.AddKey(time, speed);
for (int i = 0; i < numKeys; i++)
{
DefaultAction action = outputPlan.ElementAt(i);
if (action != null && action.state != null)
{
GridTimeState gridTimeState = action.state as GridTimeState;
if (gridTimeState != null)
{
time = gridTimeState.time;
pos = gridTimeState.currentPosition;
speed = gridTimeState.speed;
}
curveX.AddKey(time, pos.x);
curveY.AddKey(time, pos.y);
curveZ.AddKey(time, pos.z);
curveSpeed.AddKey(time, speed);
}
}
AnimationCurve[] curves = new AnimationCurve[4];
curves[0] = curveX;
curves[1] = curveY;
curves[2] = curveZ;
curves[3] = curveSpeed;
return(curves);
}
public override TaskStatus execute(float maxTime)
{
Stack <DefaultAction> convertedTunnel = new Stack <DefaultAction> ();
// convert tunnel
for (int i = tunnel.Count - 1; i >= 0; i--)
{
GridPlanningState gs = new GridPlanningState(tunnel[i].getPosition());
GridPlanningAction g = new GridPlanningAction(gs, new Vector3());
convertedTunnel.Push(g);
}
GridTunnelSearch gridTunnelSearch =
new GridTunnelSearch(convertedTunnel, startState.getPosition(), 0.0F, 10.0F, gridTimeDomain, 250, 2.0F, 1.0F, 1.0F);
Debug.Log("grid tunnel plan " + gridTunnelSearch.newPlan.Count);
//// generating plan for now
spaceTimePath.Clear();
while (gridTunnelSearch.newPlan.Count != 0)
{
DefaultAction action = gridTunnelSearch.newPlan.Pop();
GridTimeState state = action.state as GridTimeState;
spaceTimePath.Add(new State(state.currentPosition, state.time));
}
bool planComputed = gridTunnelSearch.goalReached;
// TODO : determine task status
TaskStatus taskStatus = TaskStatus.Success;
if (planComputed == true)
{
taskStatus = TaskStatus.Success;
}
else
{
taskStatus = TaskStatus.Failure; // TODO: I dont know, check
}
setTaskPriority(TaskPriority.Inactive);
return(taskStatus);
}
void generateSpaceTimePlanStack(DefaultState startState, DefaultState stateReached, Dictionary <DefaultState, ARAstarNode> araStarPlan, List <State> spaceTimePath)
{
// here we are clearing the plan list
spaceTimePath.Clear();
GridTimeState currentState = stateReached as GridTimeState;
GridTimeState starttState = startState as GridTimeState;
while (!currentState.Equals(starttState))
{
spaceTimePath.Add(new State(currentState.currentPosition, currentState.time));
currentState = araStarPlan[currentState].previousState as GridTimeState;
}
spaceTimePath.Add(new State(currentState.currentPosition, currentState.time));
//notifyObservers(Event.GRID_PATH_CHANGED,path);
}
public override void generatePredecesors(ref DefaultState DcurrentState, ref DefaultState DpreviousState,
ref DefaultState DidealGoalState, ref List <DefaultAction> transitions)
{
GridTimeState currentState = DcurrentState as GridTimeState;
GridTimeState idealGoalState = DidealGoalState as GridTimeState;
GridTimeState previousState = DpreviousState as GridTimeState;
GridTimeAction stopAction = new GridTimeAction(currentState, Vector3.zero, 0);
transitions.Add(stopAction);
#if USE_ANGLE_MOV
foreach (float angle in movAngles)
{
Vector3 mov = currentState.actionMov;
mov = Matrix4x4.TRS(Vector3.zero, Quaternion.AngleAxis(angle, Vector3.up), new Vector3(1, 1, 1)) * mov;
mov.Normalize();
#else
foreach (Vector3 mov in movDirections)
{
#endif
foreach (float speedIncr in possibleSpeedIncrements)
{
float newSpeed = previousState.speed - speedIncr;
if (newSpeed > MAX_SPEED)
{
newSpeed = MAX_SPEED;
}
else if (newSpeed < MIN_SPEED)
{
newSpeed = MIN_SPEED;
}
GridTimeAction newAction = new GridTimeAction(previousState, mov, newSpeed, 0.0f);
if (!CheckTransitionCollisions(currentState, newAction.state))
{
transitions.Add(newAction);
}
}
}
}
// does not look like it is being used
public override float ComputeEstimate(ref DefaultState Dfrom, ref DefaultState Dto, string estimateType)
{
GridTimeState _from = Dfrom as GridTimeState;
GridTimeState _to = Dto as GridTimeState;
/*
* if (_from == null)
* {
* FootstepPlanningState fsState = Dfrom as FootstepPlanningState;
* _from = new GridTimeState(fsState);
* }
*/
Debug.Log("compute estimate " + estimateType);
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 / meanStepSize;
return(spaceCost);
}
else
{
return(0.0f);
}
}
// Collision check for the lower resolutions model
public bool CheckRootCollisions(GridTimeState state, GridTimeState previousState, bool sampling)
{
Vector3 start = state.currentPosition - new Vector3(0, 10 * agentHeight / 2, 0);
Vector3 end = start + new Vector3(0, 10 * agentHeight / 2, 0);
float radius = agentRadius;
if (Physics.CheckCapsule(start, end, radius, layer))
{
//if (Physics.CheckSphere(state.currentPosition,radius,layer))
return(true);
}
if (previousState != null)
{
int samples = numSamples;
if (!sampling)
{
samples = 2;
}
Vector3 mov = state.currentPosition - previousState.currentPosition;
for (int i = 1; i < samples - 1; i += 1) // we sample less in this domain the collision check
{
start = (previousState as GridTimeState).currentPosition + i / (samples - 1) * mov;
end = start + new Vector3(0, agentHeight / 2, 0);
if (Physics.CheckCapsule(start, end, radius, layer))
{
//if (Physics.CheckSphere(state.previousState.currentPosition,radius,layer))
return(true);
}
}
}
return(false);
}
// Use this for initialization
void Start()
{
planner = new ARAstarPlanner();
BestFirstPlanner = new BestFirstSearchPlanner();
GetComponent <NeighbourObstacles>().Init();
GridTimeDomain domain = new GridTimeDomain(GetComponent <NeighbourObstacles>(), GetComponent <NeighbourAgents>());
List <PlanningDomainBase> dl = new List <PlanningDomainBase>();
dl.Add(domain);
DefaultState startState = new GridTimeState(transform.position) as DefaultState;
DefaultState goalState = new GridTimeState(new Vector3(5, 0.5f, 0), 5.0f) as DefaultState;
plan = new Dictionary <DefaultState, ARAstarNode>();
outputPlan = new Stack <DefaultState>();
if (BestFirstSearchPlanner)
{
BestFirstPlanner.init(ref dl, 200);
bool completed = BestFirstPlanner.computePlan(ref startState, ref goalState, ref outputPlan, 10.0f);
Debug.Log("BestFirst Plan " + outputPlan.Count);
Debug.Log("Plan found: " + completed);
}
else
{
planner.init(ref dl, 200);
float inflationFactor = 2.5f;
PathStatus status = planner.computePlan(ref startState, ref goalState, ref plan, ref inflationFactor, 10.0f);
planner.FillPlan();
Debug.Log("ARA Plan " + plan.Count);
Debug.Log("Nodes Expanded: " + planner.Close.Elements().Count);
Debug.Log("Status: " + status);
}
}
override public bool CheckTransitionCollisions(DefaultState Dstate, DefaultState DprevState, bool sampling = true)
{
//float startTime = Time.realtimeSinceStartup;
GridTimeState state = Dstate as GridTimeState;
GridTimeState prevState = DprevState as GridTimeState;
bool b = false;
// If we are checking a state of this domain
if (state != null)
{
b = (
CheckRootCollisions(state, prevState, sampling) ||
CheckAgentsCollisions(state, prevState, sampling) ||
CheckDynamicObstaclesCollisions(state, prevState, sampling)
);
}
/*
* 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 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 deterministic dynamic obstacles
public bool CheckDynamicObstaclesCollisions(GridTimeState state, GridTimeState previousState, bool sampling)
{
if (obstacles == null)
{
return(false);
}
if (obstacles.closeObstacles == null)
{
return(false);
}
List <DDObstacle> visibleObstacles = obstacles.visibleObstacles;
if (visibleObstacles.Count <= 0)
{
return(false);
}
if (previousState != null)
{
int samples = numSamples;
if (!sampling)
{
samples = 2;
}
Vector3 mov = state.currentPosition - previousState.currentPosition;
float timeStep = (state.time - previousState.time) / (samples - 1);
float time = previousState.time;
for (int i = 1; i < samples; i += 1)
{
Vector3 pos = previousState.currentPosition + i / (samples - 1) * mov;
time += timeStep;
//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 = obstacleScript.gameObj.transform.position; //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(pos, 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);
}
/*
* private Tunnel ConvertPlan(float minVelocity, float midVelocity, float maxVelocity)
* {
* Tunnel tunnel;
* tunnel.tunnelMidVelocity = new GridTimeState[currentPlan.Length];
* tunnel.tunnelMinVelocity = new float[currentPlan.Length];
* tunnel.tunnelMaxVelocity = new float[currentPlan.Length];
*
* tunnel.thresholdD = T_d;
* //tunnel.thresholdX = T_x;
* //tunnel.thresholdZ = T_z;
* tunnel.thresholdT = T_t;
*
* int i = 0;
* foreach ( DefaultAction action in currentPlan )
* {
* GridPlanningState state = action.state as GridPlanningState;
*
* if (state != null)
* {
* float midTime = ComputeEstimatedTime(state, midVelocity);
*
* GridTimeState gridTimeState = new GridTimeState(state.currentPosition,midTime);
*
* tunnel.tunnelMidVelocity[i] = gridTimeState;
*
* tunnel.tunnelMinVelocity[i] = ComputeEstimatedTime(state, minVelocity);
* tunnel.tunnelMaxVelocity[i] = ComputeEstimatedTime(state, maxVelocity);
* }
*
* i++;
* }
*
* return tunnel;
* }
*/
private Tunnel ConvertPlan(float startTime, float minVelocity, float midVelocity, float maxVelocity)
{
Tunnel tunnel;
tunnel.tunnelMidVelocity = new GridTimeState[currentPlan.Length];
tunnel.tunnelMinVelocity = new float[currentPlan.Length];
tunnel.tunnelMaxVelocity = new float[currentPlan.Length];
tunnel.thresholdD = T_d;
//tunnel.thresholdX = T_x;
//tunnel.thresholdZ = T_z;
tunnel.thresholdT = T_t;
GridPlanningState lastGridPlanningState = currentPlan[currentPlan.Length - 1].state as GridPlanningState;
float maxGoalTime = ComputeEstimatedTime(lastGridPlanningState, minVelocity);
float midGoalTime = ComputeEstimatedTime(lastGridPlanningState, midVelocity);
float minGoalTime = ComputeEstimatedTime(lastGridPlanningState, maxVelocity);
//Debug.Log("minGoalTime = " + minGoalTime);
//Debug.Log("midGoalTime = " + midGoalTime);
//Debug.Log("maxGoalTime = " + maxGoalTime);
float[] distances = ComputeNodeDistances(currentPlan);
//for (int aux=0; aux<distances.Length; aux++)
// Debug.Log("distance "+aux+": "+distances[aux]);
float totalDistance = ComputeTotalDistance(distances);
int i = 0;
float minSum = startTime;
float midSum = startTime;
float maxSum = startTime;
while (i < currentPlan.Length)
{
GridPlanningState state = currentPlan[i].state as GridPlanningState;
if (state != null)
{
float midTime = midSum + midGoalTime * distances[i] / totalDistance;
midSum = midTime;
GridTimeState gridTimeState = new GridTimeState(state.currentPosition, midTime);
tunnel.tunnelMidVelocity[i] = gridTimeState;
}
else
{
tunnel.tunnelMidVelocity[i] = null;
}
tunnel.tunnelMaxVelocity[i] = minSum + minGoalTime * distances[i] / totalDistance;
tunnel.tunnelMinVelocity[i] = maxSum + maxGoalTime * distances[i] / totalDistance;
minSum = tunnel.tunnelMaxVelocity[i];
maxSum = tunnel.tunnelMinVelocity[i];
i++;
}
return(tunnel);
}
public static float ComputeDistanceToTunnel(GridTimeState state, Tunnel t)
{
int startIndexMaxV = -1;
int endIndexMaxV = -1;
int startIndexMinV = -1;
int endIndexMinV = -1;
SearchTimeRanges(t, state, ref startIndexMinV, ref endIndexMinV, ref startIndexMaxV, ref endIndexMaxV);
Vector3 statePos = state.currentPosition;
float minDistance = float.MaxValue;
if (
(startIndexMinV >= 0 && startIndexMinV < t.tunnelMidVelocity.Length) ||
(endIndexMinV >= 0 && endIndexMinV < t.tunnelMidVelocity.Length)
)
{
if (startIndexMinV >= 0 || startIndexMinV < t.tunnelMidVelocity.Length)
{
startIndexMinV = 0;
}
if (endIndexMinV >= 0 || endIndexMinV < t.tunnelMidVelocity.Length)
{
endIndexMaxV = t.tunnelMidVelocity.Length - 1;
}
for (int i = startIndexMinV; i < endIndexMinV; i++)
{
GridTimeState s1 = t.tunnelMidVelocity[i];
GridTimeState s2 = t.tunnelMidVelocity[i + 1];
float distance = float.MaxValue;
if (s1 != null && s2 != null)
{
distance = ComputeDistance(state, s1, s2);
}
if (distance < minDistance)
{
minDistance = distance;
}
}
}
if (
(startIndexMaxV >= 0 && startIndexMaxV < t.tunnelMidVelocity.Length) ||
(endIndexMaxV >= 0 && endIndexMaxV < t.tunnelMidVelocity.Length)
)
{
if (startIndexMaxV >= 0 || startIndexMaxV < t.tunnelMidVelocity.Length)
{
startIndexMaxV = 0;
}
if (endIndexMaxV >= 0 || endIndexMaxV < t.tunnelMidVelocity.Length)
{
endIndexMaxV = t.tunnelMidVelocity.Length - 1;
}
for (int i = startIndexMaxV; i < endIndexMaxV; i++)
{
GridTimeState s1 = t.tunnelMidVelocity[i];
GridTimeState s2 = t.tunnelMidVelocity[i + 1];
float distance = float.MaxValue;
if (s1 != null && s2 != null)
{
distance = ComputeDistance(state, s1, s2);
}
if (distance < minDistance)
{
minDistance = distance;
}
}
}
return(minDistance);
}
// Collisions check with other agents
public bool CheckAgentsCollisions(GridTimeState state, GridTimeState previousState, bool sampling)
{
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");
if (previousState != null)
{
int samples = numSamples;
if (!sampling)
{
samples = 2;
}
Vector3 mov = state.currentPosition - previousState.currentPosition;
float timeStep = (state.time - previousState.time) / (samples - 1);
float time = previousState.time;
for (int i = 1; i < samples; i += 1)
{
Vector3 pos = previousState.currentPosition + i / (samples - 1) * mov;
time += timeStep;
foreach (KeyValuePair <int, Neighbor> pair in neighbors)
{
if (pair.Value.animationInterface != null && pair.Value.triggers == 2)
{
AnimationCurve[] curves = pair.Value.animationInterface.GetPlanAnimationCurve();
if (curves != null)
{
float x = curves[0].Evaluate(time);
float y = curves[1].Evaluate(time);
float z = curves[2].Evaluate(time);
Vector3 npos = new Vector3(x, y, z);
//Debug.Log("neighbour pos = " + pos);
if ((npos - pos).magnitude < 2 * agentRadius)
{
return(true);
}
}
}
}
}
}
return(false);
}
public GridPlanningState(GridTimeState gridTimeState)
{
currentPosition = gridTimeState.currentPosition;
actionMov = gridTimeState.actionMov;
previousState = null; //gridTimeState.previousState;
}
// Performs a binary search for the indexes in the tunnel t corresponding to the portion of the path
// where the time values are in the valid range of the state we are looking at
private static void SearchTimeRanges(Tunnel t, GridTimeState state,
ref int startIndexMinV, ref int endIndexMinV,
ref int startIndexMaxV, ref int endIndexMaxV)
{
startIndexMinV = -1;
endIndexMinV = -1;
startIndexMaxV = -1;
endIndexMaxV = -1;
float minTime = state.time - t.thresholdT;
if (minTime < 0)
{
minTime = 0;
}
float maxTime = state.time + t.thresholdT;
int i = 0;
int j = t.tunnelMidVelocity.Length - 1;
int k = -1;
while (i < j)
{
k = i + (int)((j - i) / 2);
float minTimeK = t.tunnelMaxVelocity[k];
if (minTime == minTimeK)
{
i = k;
j = k;
}
else
{
if (minTime < minTimeK)
{
k--;
j = k;
}
else // minTim > minTimeK
{
k++;
i = k;
}
}
}
startIndexMaxV = k;
float aux;
if (k >= 0 && k < t.tunnelMidVelocity.Length)
{
aux = t.tunnelMaxVelocity[k];
if (aux < minTime)
{
startIndexMaxV++;
}
}
i = 0;
j = t.tunnelMidVelocity.Length - 1;
k = -1;
while (i < j)
{
k = i + (int)((j - i) / 2);
float minTimeK = t.tunnelMaxVelocity[k];
if (maxTime == minTimeK)
{
i = k;
j = k;
}
else
{
if (maxTime < minTimeK)
{
k--;
j = k;
}
else // maxTim > minTimeK
{
k++;
i = k;
}
}
}
endIndexMaxV = k;
if (k >= 0 && k < t.tunnelMidVelocity.Length)
{
aux = t.tunnelMaxVelocity[k];
if (aux > maxTime)
{
endIndexMaxV--;
}
}
i = 0;
j = t.tunnelMidVelocity.Length - 1;
k = -1;
while (i < j)
{
k = i + (int)((j - i) / 2);
float maxTimeK = t.tunnelMinVelocity[k];
if (minTime == maxTimeK)
{
i = k;
j = k;
}
else
{
if (minTime < maxTimeK)
{
k--;
j = k;
}
else // minTim > maxTimeK
{
k++;
i = k;
}
}
}
startIndexMinV = k;
if (k >= 0 && k < t.tunnelMidVelocity.Length)
{
aux = t.tunnelMaxVelocity[k];
if (aux < minTime)
{
startIndexMinV--;
}
}
i = 0;
j = t.tunnelMidVelocity.Length - 1;
k = -1;
while (i < j)
{
k = i + (int)((j - i) / 2);
float maxTimeK = t.tunnelMinVelocity[k];
if (maxTime == maxTimeK)
{
i = k;
j = k;
}
else
{
if (maxTime < maxTimeK)
{
k--;
j = k;
}
else // maxTim > maxTimeK
{
k++;
i = k;
}
}
}
endIndexMinV = k;
if (k >= 0 && k < t.tunnelMidVelocity.Length)
{
aux = t.tunnelMaxVelocity[k];
if (aux > maxTime)
{
endIndexMinV--;
}
}
/*
* for(int aux2 = 0; aux2 < t.tunnelMaxVelocity.Length; aux2++)
* Debug.Log("Tunnel "+aux2+":"+t.tunnelMaxVelocity[aux2]+ " y "+t.tunnelMinVelocity[aux2]);
* Debug.Log("minTime = "+minTime);
* Debug.Log("maxTime = "+maxTime);
* Debug.Log("startIndexMaxV = "+startIndexMaxV);
* Debug.Log("endIndexMaxV= "+endIndexMaxV);
* Debug.Log("startIndexMinV = "+startIndexMinV);
* Debug.Log("endIndexMinV= "+endIndexMinV);
*/
}
// Collisions check with deterministic dynamic obstacles
public bool CheckDynamicObstaclesCollisions(GridTimeState state, GridTimeState previousState, bool sampling)
{
if (obstacles == null)
return false;
if (obstacles.closeObstacles == null)
return false;
List<DDObstacle> visibleObstacles = obstacles.visibleObstacles;
if (visibleObstacles.Count <= 0)
return false;
if (previousState != null)
{
int samples = numSamples;
if (!sampling)
samples = 2;
Vector3 mov = state.currentPosition - previousState.currentPosition;
float timeStep = (state.time - previousState.time) / (samples-1);
float time = previousState.time;
for (int i=1; i<samples; i+=1)
{
Vector3 pos = previousState.currentPosition + i/(samples-1)*mov;
time += timeStep;
//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 = obstacleScript.gameObj.transform.position;//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(pos,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;
}
public GridPlanningState(GridTimeState gridTimeState)
{
currentPosition = gridTimeState.currentPosition;
actionMov = gridTimeState.actionMov;
previousState = null; //gridTimeState.previousState;
}
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);
}
}
}
}
}
}
override public void generateTransitions(ref DefaultState DcurrentState, ref DefaultState DpreviousState,
ref DefaultState DidealGoalState, ref List <DefaultAction> transitions)
{
GridTimeState currentState = DcurrentState as GridTimeState;
GridTimeState idealGoalState = DidealGoalState as GridTimeState;
/*
* if (currentState == null)
* currentState = new GridTimeState(DcurrentState as FootstepPlanningState);
*
* if (idealGoalState == null)
* idealGoalState = new GridTimeState(DidealGoalState as FootstepPlanningState);
*/
GridTimeAction stopAction = new GridTimeAction(currentState, Vector3.zero, 0);
if (useTimeConstraint)
{
GridTimeState stopState = stopAction.state as GridTimeState;
Vector3 toGoal = idealGoalState.currentPosition - stopState.currentPosition;
float availableTime = idealGoalState.time - stopState.time; // + timeWindow;
float necessarySpeed = toGoal.magnitude / availableTime;
float timeCost = necessarySpeed - stopState.speed;
bool negative = timeCost < 0;
if (negative)
{
timeCost = 0;
}
else
{
timeCost *= timeCost;
}
stopAction.cost += timeCost;
}
if (!CheckTransitionCollisions(currentState, stopAction.state, false))
{
if (!useTunnel || isInsideTunnel(stopAction.state))
{
transitions.Add(stopAction);
}
}
#if USE_ANGLE_MOV
foreach (float angle in movAngles)
{
Vector3 mov = currentState.actionMov;
mov = Matrix4x4.TRS(Vector3.zero, Quaternion.AngleAxis(angle, Vector3.up), new Vector3(1, 1, 1)) * mov;
mov.Normalize();
#else
foreach (Vector3 mov in movDirections)
{
#endif
foreach (float speedIncr in possibleSpeedIncrements)
{
float newSpeed = currentState.speed + speedIncr;
if (newSpeed > MAX_SPEED)
{
newSpeed = MAX_SPEED;
}
else if (newSpeed < MIN_SPEED)
{
newSpeed = MIN_SPEED;
}
GridTimeAction newAction = new GridTimeAction(currentState, mov, newSpeed);
if (useTimeConstraint)
{
GridTimeState newState = newAction.state as GridTimeState;
Vector3 toGoal = idealGoalState.currentPosition - newState.currentPosition;
float availableTime = idealGoalState.time - newState.time; // + timeWindow;
float necessarySpeed = toGoal.magnitude / availableTime;
float timeCost = necessarySpeed - newState.speed;
bool negative = timeCost < 0;
if (negative)
{
timeCost = 0;
}
else
{
timeCost *= timeCost;
}
newAction.cost += timeCost;
}
if (!CheckTransitionCollisions(currentState, newAction.state))
{
// If we are not using a tunnel
// or if we are using one and the stata falls inside it
if (!useTunnel || isInsideTunnel(newAction.state))
{
//Debug.Log(Time.time + ": grid successor generated");
transitions.Add(newAction); // we add the action as a transition
}
}
}
}
//Debug.Log(transitions.Count + " grid transitions generated at time " + Time.time);
}
/*
private Tunnel ConvertPlan(float minVelocity, float midVelocity, float maxVelocity)
{
Tunnel tunnel;
tunnel.tunnelMidVelocity = new GridTimeState[currentPlan.Length];
tunnel.tunnelMinVelocity = new float[currentPlan.Length];
tunnel.tunnelMaxVelocity = new float[currentPlan.Length];
tunnel.thresholdD = T_d;
//tunnel.thresholdX = T_x;
//tunnel.thresholdZ = T_z;
tunnel.thresholdT = T_t;
int i = 0;
foreach ( DefaultAction action in currentPlan )
{
GridPlanningState state = action.state as GridPlanningState;
if (state != null)
{
float midTime = ComputeEstimatedTime(state, midVelocity);
GridTimeState gridTimeState = new GridTimeState(state.currentPosition,midTime);
tunnel.tunnelMidVelocity[i] = gridTimeState;
tunnel.tunnelMinVelocity[i] = ComputeEstimatedTime(state, minVelocity);
tunnel.tunnelMaxVelocity[i] = ComputeEstimatedTime(state, maxVelocity);
}
i++;
}
return tunnel;
}
*/
private Tunnel ConvertPlan(float startTime, float minVelocity, float midVelocity, float maxVelocity)
{
Tunnel tunnel;
tunnel.tunnelMidVelocity = new GridTimeState[currentPlan.Length];
tunnel.tunnelMinVelocity = new float[currentPlan.Length];
tunnel.tunnelMaxVelocity = new float[currentPlan.Length];
tunnel.thresholdD = T_d;
//tunnel.thresholdX = T_x;
//tunnel.thresholdZ = T_z;
tunnel.thresholdT = T_t;
GridPlanningState lastGridPlanningState = currentPlan[currentPlan.Length-1].state as GridPlanningState;
float maxGoalTime = ComputeEstimatedTime(lastGridPlanningState,minVelocity);
float midGoalTime = ComputeEstimatedTime(lastGridPlanningState,midVelocity);
float minGoalTime = ComputeEstimatedTime(lastGridPlanningState,maxVelocity);
//Debug.Log("minGoalTime = " + minGoalTime);
//Debug.Log("midGoalTime = " + midGoalTime);
//Debug.Log("maxGoalTime = " + maxGoalTime);
float[] distances = ComputeNodeDistances(currentPlan);
//for (int aux=0; aux<distances.Length; aux++)
// Debug.Log("distance "+aux+": "+distances[aux]);
float totalDistance = ComputeTotalDistance(distances);
int i = 0;
float minSum = startTime;
float midSum = startTime;
float maxSum = startTime;
while( i < currentPlan.Length )
{
GridPlanningState state = currentPlan[i].state as GridPlanningState;
if (state != null)
{
float midTime = midSum + midGoalTime * distances[i] / totalDistance;
midSum = midTime;
GridTimeState gridTimeState = new GridTimeState(state.currentPosition,midTime);
tunnel.tunnelMidVelocity[i] = gridTimeState;
}
else
tunnel.tunnelMidVelocity[i] = null;
tunnel.tunnelMaxVelocity[i] = minSum + minGoalTime * distances[i] / totalDistance;
tunnel.tunnelMinVelocity[i] = maxSum + maxGoalTime * distances[i] / totalDistance;
minSum = tunnel.tunnelMaxVelocity[i];
maxSum = tunnel.tunnelMinVelocity[i];
i++;
}
return tunnel;
}
// Performs a binary search for the indexes in the tunnel t corresponding to the portion of the path
// where the time values are in the valid range of the state we are looking at
private static void SearchTimeRanges(Tunnel t, GridTimeState state,
ref int startIndexMinV, ref int endIndexMinV,
ref int startIndexMaxV, ref int endIndexMaxV)
{
startIndexMinV = -1;
endIndexMinV = -1;
startIndexMaxV = -1;
endIndexMaxV = -1;
float minTime = state.time - t.thresholdT;
if (minTime < 0 ) minTime = 0;
float maxTime = state.time + t.thresholdT;
int i = 0;
int j = t.tunnelMidVelocity.Length-1;
int k = -1;
while (i < j)
{
k = i + (int)(( j - i ) / 2);
float minTimeK = t.tunnelMaxVelocity[k];
if (minTime == minTimeK)
{
i = k;
j = k;
}
else
{
if (minTime < minTimeK)
{
k--;
j = k;
}
else // minTim > minTimeK
{
k++;
i = k;
}
}
}
startIndexMaxV = k;
float aux;
if (k >= 0 && k < t.tunnelMidVelocity.Length)
{
aux = t.tunnelMaxVelocity[k];
if (aux < minTime)
startIndexMaxV++;
}
i = 0;
j = t.tunnelMidVelocity.Length-1;
k = -1;
while (i < j)
{
k = i + (int)(( j - i ) / 2);
float minTimeK = t.tunnelMaxVelocity[k];
if (maxTime == minTimeK)
{
i = k;
j = k;
}
else
{
if (maxTime < minTimeK)
{
k--;
j = k;
}
else // maxTim > minTimeK
{
k++;
i = k;
}
}
}
endIndexMaxV = k;
if (k >= 0 && k < t.tunnelMidVelocity.Length)
{
aux = t.tunnelMaxVelocity[k];
if (aux > maxTime)
endIndexMaxV--;
}
i = 0;
j = t.tunnelMidVelocity.Length-1;
k = -1;
while (i < j)
{
k = i + (int)(( j - i ) / 2);
float maxTimeK = t.tunnelMinVelocity[k];
if (minTime == maxTimeK)
{
i = k;
j = k;
}
else
{
if (minTime < maxTimeK)
{
k--;
j = k;
}
else // minTim > maxTimeK
{
k++;
i = k;
}
}
}
startIndexMinV = k;
if (k >= 0 && k < t.tunnelMidVelocity.Length)
{
aux = t.tunnelMaxVelocity[k];
if (aux < minTime)
startIndexMinV--;
}
i = 0;
j = t.tunnelMidVelocity.Length-1;
k = -1;
while (i < j)
{
k = i + (int)(( j - i ) / 2);
float maxTimeK = t.tunnelMinVelocity[k];
if (maxTime == maxTimeK)
{
i = k;
j = k;
}
else
{
if (maxTime < maxTimeK)
{
k--;
j = k;
}
else // maxTim > maxTimeK
{
k++;
i = k;
}
}
}
endIndexMinV = k;
if (k >= 0 && k < t.tunnelMidVelocity.Length)
{
aux = t.tunnelMaxVelocity[k];
if (aux > maxTime)
endIndexMinV--;
}
/*
for(int aux2 = 0; aux2 < t.tunnelMaxVelocity.Length; aux2++)
Debug.Log("Tunnel "+aux2+":"+t.tunnelMaxVelocity[aux2]+ " y "+t.tunnelMinVelocity[aux2]);
Debug.Log("minTime = "+minTime);
Debug.Log("maxTime = "+maxTime);
Debug.Log("startIndexMaxV = "+startIndexMaxV);
Debug.Log("endIndexMaxV= "+endIndexMaxV);
Debug.Log("startIndexMinV = "+startIndexMinV);
Debug.Log("endIndexMinV= "+endIndexMinV);
*/
}
// Returns the perpendicular distance from the state "state" to the vector/path/edge
// formed by the states s1 and s2
private static float ComputeDistance(GridTimeState state, GridTimeState s1, GridTimeState s2)
{
Vector3 A = s1.currentPosition;
Vector3 B = s2.currentPosition;
Vector3 C = state.currentPosition;
Vector3 AB = B - A;
Vector3 AC = C - A;
//float ABdotAC = Vector3.Dot(AB,AC);
//float distance = Mathf.Sqrt(1 - (ABdotAC*ABdotAC)/(AC.sqrMagnitude * AB.sqrMagnitude)) * AC.magnitude;
// P is the projection of C over AB
Vector3 AP = Vector3.Project(AC,AB);
Vector3 P = A + AP;
Vector3 BP = P - B;
//Debug.DrawLine(A,(A+Vector3.up),Color.red);
//Debug.DrawLine(P,(P+Vector3.up),Color.blue);
//Debug.DrawLine(A,P,Color.green);
float APsqrMagnitude = AP.sqrMagnitude;
float BPsqrMagnitude = BP.sqrMagnitude;
float ABsqrMagnitude = AB.sqrMagnitude;
float distance;
if ( APsqrMagnitude <= ABsqrMagnitude && BPsqrMagnitude <= ABsqrMagnitude)
// if the projection is between A and B (in the segment)
distance = ( P - C ).magnitude;
else
{
float ACmagnitude = AC.magnitude;
float BCmagnitude = (C-B).magnitude;
if (ACmagnitude < BCmagnitude)
distance = ACmagnitude;
else
distance = BCmagnitude;
}
return distance;
}
public static float ComputeDistanceToTunnel(GridTimeState state, Tunnel t)
{
int startIndexMaxV = -1;
int endIndexMaxV = -1;
int startIndexMinV = -1;
int endIndexMinV = -1;
SearchTimeRanges(t,state,ref startIndexMinV, ref endIndexMinV, ref startIndexMaxV, ref endIndexMaxV);
Vector3 statePos = state.currentPosition;
float minDistance = float.MaxValue;
if (
(startIndexMinV >=0 && startIndexMinV < t.tunnelMidVelocity.Length) ||
(endIndexMinV >=0 && endIndexMinV < t.tunnelMidVelocity.Length)
)
{
if (startIndexMinV >=0 || startIndexMinV < t.tunnelMidVelocity.Length)
startIndexMinV = 0;
if (endIndexMinV >=0 || endIndexMinV < t.tunnelMidVelocity.Length)
endIndexMaxV = t.tunnelMidVelocity.Length-1;
for (int i = startIndexMinV; i < endIndexMinV; i++)
{
GridTimeState s1 = t.tunnelMidVelocity[i];
GridTimeState s2 = t.tunnelMidVelocity[i+1];
float distance = float.MaxValue;
if (s1 != null && s2 != null)
distance = ComputeDistance(state,s1,s2);
if (distance < minDistance)
minDistance = distance;
}
}
if (
(startIndexMaxV >=0 && startIndexMaxV < t.tunnelMidVelocity.Length) ||
(endIndexMaxV >=0 && endIndexMaxV < t.tunnelMidVelocity.Length)
)
{
if (startIndexMaxV >=0 || startIndexMaxV < t.tunnelMidVelocity.Length)
startIndexMaxV = 0;
if (endIndexMaxV >=0 || endIndexMaxV < t.tunnelMidVelocity.Length)
endIndexMaxV = t.tunnelMidVelocity.Length-1;
for (int i = startIndexMaxV; i < endIndexMaxV; i++)
{
GridTimeState s1 = t.tunnelMidVelocity[i];
GridTimeState s2 = t.tunnelMidVelocity[i+1];
float distance = float.MaxValue;
if (s1 != null && s2 != null)
distance = ComputeDistance(state,s1,s2);
if (distance < minDistance)
minDistance = distance;
}
}
return minDistance;
}
// Collisions check with other agents
public bool CheckAgentsCollisions(GridTimeState state, GridTimeState previousState, bool sampling)
{
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");
if (previousState != null)
{
int samples = numSamples;
if (!sampling)
samples = 2;
Vector3 mov = state.currentPosition - previousState.currentPosition;
float timeStep = (state.time - previousState.time) / (samples-1);
float time = previousState.time;
for (int i=1; i<samples; i+=1)
{
Vector3 pos = previousState.currentPosition + i/(samples-1)*mov;
time += timeStep;
foreach(KeyValuePair<int,Neighbor> pair in neighbors)
{
if (pair.Value.animationInterface != null && pair.Value.triggers == 2)
{
AnimationCurve[] curves = pair.Value.animationInterface.GetPlanAnimationCurve();
if (curves != null)
{
float x = curves[0].Evaluate(time);
float y = curves[1].Evaluate(time);
float z = curves[2].Evaluate(time);
Vector3 npos = new Vector3(x,y,z);
//Debug.Log("neighbour pos = " + pos);
if ( (npos - pos).magnitude < 2*agentRadius )
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);
}
// Creation function
//public GridTunnelSearch(Stack<DefaultAction> plan, float startTime, float idealGoalTime, GridTimeDomain gtd, int MaxNumberOfNodes, float MaxTime, float T_x, float T_z, float T_t)
public GridTunnelSearch(Stack <DefaultAction> plan, Vector3 startPos, float startTime, float idealGoalTime, GridTimeDomain gtd, int MaxNumberOfNodes, float MaxTime, float T_d, float T_t, float startSpeed = 0)
{
currentPlan = new DefaultAction[plan.Count];
plan.CopyTo(currentPlan, 0);
this.T_d = T_d;
//this.T_x = T_x;
//this.T_z = T_z;
this.T_t = T_t;
this.maxTime = MaxTime;
int startIndex = 0;
//GridPlanningState startPlanningState = currentPlan[startIndex].state as GridPlanningState;
//while (startPlanningState == null)
//{
// startIndex++;
// startPlanningState = currentPlan[startIndex].state as GridPlanningState;
//}
//startState = new GridTimeState(startPlanningState.currentPosition, startTime, startSpeed);
startState = new GridTimeState(startPos, startTime, startSpeed);
lastState = new GridTimeState((currentPlan[currentPlan.Length - 1].state as GridPlanningState).currentPosition, idealGoalTime);
gridTimeDomain = gtd;
float dist = (startState.currentPosition - lastState.currentPosition).magnitude;
// TODO: what to do if there is no time???
float minSpeed = dist / (idealGoalTime - startTime);
if (minSpeed < 0)
{
minSpeed = GridTimeDomain.MIN_SPEED;
}
//tunnel = ConvertPlan(gtd.minSpeed, gtd.midSpeed, gtd.maxSpeed);
tunnel = ConvertPlan(startTime, minSpeed, GridTimeDomain.MID_SPEED, GridTimeDomain.MAX_SPEED);
//for(int aux = 0; aux < tunnel.tunnelMinVelocity.Length; aux++)
// Debug.Log("Tunnel "+aux+":"+tunnel.tunnelMinVelocity[aux]);
tunnelPlanner = new BestFirstSearchPlanner();
List <PlanningDomainBase> domainList = new List <PlanningDomainBase>();
domainList.Add(gridTimeDomain);
tunnelPlanner.init(ref domainList, MaxNumberOfNodes);
// MUBBASIR TESTING ARA STAR PLANNER HERE
araStarPlanner = new ARAstarPlanner();
araStarPlanner.init(ref domainList, MaxNumberOfNodes);
goalReached = false;
ComputeTunnelSearch();
//ComputeTunnelSearchStatePlan ();
}
override public bool isAGoalState(ref DefaultState Dstate, ref DefaultState DidealGoalState)
{
//float startTime = Time.realtimeSinceStartup;
GridPlanningState state = Dstate as GridPlanningState;
GridPlanningState idealGoalState = DidealGoalState as GridPlanningState;
if (state == null)
{
FootstepPlanningState fsState = Dstate as FootstepPlanningState;
if (fsState != null)
{
state = new GridPlanningState(fsState);
}
else
{
GridTimeState gridTimeState = Dstate as GridTimeState;
state = new GridPlanningState(gridTimeState);
}
}
Vector3 toGoal;
if (idealGoalState != null)
{
// A state is a goal one if it's really close to the goal
toGoal = idealGoalState.currentPosition - state.currentPosition;
}
else
{
FootstepPlanningState fsIdealGoalState = DidealGoalState as FootstepPlanningState;
if (fsIdealGoalState != null)
{
toGoal = fsIdealGoalState.currentPosition - state.currentPosition;
}
else
{
GridTimeState gtIdealGoalState = DidealGoalState as GridTimeState;
toGoal = gtIdealGoalState.currentPosition - state.currentPosition;
}
}
//bool b = toGoal.magnitude/analyzer.maxStepSize < 0.5;
bool b = toGoal.magnitude / 2.0 < 0.5;
/*
* 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 override void generatePredecessors(DefaultState DcurrentState, ref List<DefaultAction> actionList)
{
GridTimeState currentState = DcurrentState as GridTimeState;
GridTimeAction stopAction = new GridTimeAction(currentState,Vector3.zero,0);
if (!CheckTransitionCollisions(currentState, stopAction.state, false))
{
if (!useTunnel || isInsideTunnel(stopAction.state) )
actionList.Add(stopAction);
}
#if USE_ANGLE_MOV
foreach( float angle in movAngles)
{
Vector3 mov = currentState.actionMov;
mov = Matrix4x4.TRS(Vector3.zero, Quaternion.AngleAxis(angle,Vector3.up),new Vector3(1,1,1)) * mov;
mov.Normalize();
#else
foreach ( Vector3 mov in movDirections )
{
#endif
foreach( float speedIncr in possibleSpeedIncrements )
{
float newSpeed = currentState.speed + speedIncr;
if (newSpeed > MAX_SPEED)
newSpeed = MAX_SPEED;
else if (newSpeed < MIN_SPEED)
newSpeed = MIN_SPEED;
GridTimeState prevState = new GridTimeState();
prevState.ComputePreviousState(currentState,mov,newSpeed);
GridTimeAction newAction = new GridTimeAction(currentState,prevState);
if (!CheckTransitionCollisions(currentState, newAction.state))
{
// If we are not using a tunnel
// or if we are using one and the stata falls inside it
if (!useTunnel || isInsideTunnel(newAction.state) )
{
//Debug.Log(Time.time + ": grid successor generated");
actionList.Add(newAction); // we add the action as a transition
}
}
}
}
//Debug.Log(transitions.Count + " grid transitions generated at time " + Time.time);
}
public 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;
}
// Collision check for the lower resolutions model
public bool CheckRootCollisions(GridTimeState state, GridTimeState previousState, bool sampling)
{
Vector3 start = state.currentPosition - new Vector3(0,10*agentHeight/2,0);
Vector3 end = start + new Vector3(0,10*agentHeight/2,0);
float radius = agentRadius;
if (Physics.CheckCapsule(start,end,radius,layer))
//if (Physics.CheckSphere(state.currentPosition,radius,layer))
return true;
if (previousState != null)
{
int samples = numSamples;
if (!sampling)
samples = 2;
Vector3 mov = state.currentPosition - previousState.currentPosition;
for (int i=1; i<samples-1; i+=1) // we sample less in this domain the collision check
{
start = (previousState as GridTimeState).currentPosition + i/(samples-1)*mov;
end = start + new Vector3(0,agentHeight/2,0);
if (Physics.CheckCapsule(start,end,radius,layer))
//if (Physics.CheckSphere(state.previousState.currentPosition,radius,layer))
return true;
}
}
return false;
}
