public override bool isAGoalState(ref DefaultState state, ref DefaultState idealGoalState)
{
ARAstarState currentState = state as ARAstarState;
ARAstarState goalState = idealGoalState as ARAstarState;
return(Vector3.Distance(currentState.state, goalState.state) < 1);
}
void fillActionStack(DefaultState reachedState)
{
ARAstarState state = reachedState as ARAstarState;
while (!state.Equals(DStartState as ARAstarState))
{
actions.Push(outputPlan[state].action as ARAstarAction);
state = outputPlan[state].previousState as ARAstarState;
}
}
public ARAstarAction(DefaultState _from, DefaultState _to)
{
ARAstarState Afrom = _from as ARAstarState;
ARAstarState Ato = _to as ARAstarState;
Vector3 dir = new Vector3(Ato.state.x - Afrom.state.x, Ato.state.y - Afrom.state.y, Ato.state.z - Afrom.state.z);
direction = dir;
cost = Vector3.Distance(Afrom.state, Ato.state);
state = Ato;
}
public override bool equals(DefaultState s1, DefaultState s2, bool isStart)
{
ARAstarState ARAstate1 = s1 as ARAstarState;
ARAstarState ARAstate2 = s2 as ARAstarState;
if (ARAstate1.Equals(ARAstate2))
{
return(true);
}
else
{
return(false);
}
}
public GridNeighbors()
{
neighborMap = new Dictionary <DefaultState, List <DefaultState> >();
for (float i = 0; i < 20; ++i)
{
for (float j = 0; j < 20; ++j)
{
float x = -10.0f + j; float z = -10.0f + i;
Vector3 newState = new Vector3(x, 0.5f, z);
List <DefaultState> neighborStates = new List <DefaultState>();
if (newState.x + 1 < 10.0f)
{
neighborStates.Add(new ARAstarState(new Vector3(x + 1, 0.5f, z)));
}
if (newState.x - 1 >= -10.0f)
{
neighborStates.Add(new ARAstarState(new Vector3(x - 1, 0.5f, z)));
}
if (newState.z + 1 < 10.0f)
{
neighborStates.Add(new ARAstarState(new Vector3(x, 0.5f, z + 1)));
}
if (newState.z - 1 >= -10.0f)
{
neighborStates.Add(new ARAstarState(new Vector3(x, 0.5f, z - 1)));
}
if (newState.x + 1 < 10.0f && newState.z + 1 < 10.0f)
{
neighborStates.Add(new ARAstarState(new Vector3(x + 1, 0.5f, z + 1)));
}
if (newState.x + 1 < 10.0f && newState.z - 1 >= -10.0f)
{
neighborStates.Add(new ARAstarState(new Vector3(x + 1, 0.5f, z - 1)));
}
if (newState.x - 1 >= -10.0f && newState.z + 1 < 10.0f)
{
neighborStates.Add(new ARAstarState(new Vector3(x - 1, 0.5f, z + 1)));
}
if (newState.x - 1 >= -10.0f && newState.z - 1 >= -10.0f)
{
neighborStates.Add(new ARAstarState(new Vector3(x - 1, 0.5f, z - 1)));
}
DefaultState DState = new ARAstarState(newState) as DefaultState;
neighborMap[DState] = neighborStates;
}
}
}
// Use this for initialization
void Start()
{
actions = new Stack <ARAstarAction>();
outputPlan = new Dictionary <DefaultState, ARAstarNode>();
domainList = new List <PlanningDomainBase>();
domainList.Add(new ARAstarDomain());
planner = new ARAstarPlanner();
planner.init(ref domainList, 100);
planner.usingHeap = usingHeap;
startState = new ARAstarState(startObject.transform.position);
goalState = new ARAstarState(goalObject.transform.position);
prevGoalState = goalState;
DStartState = startState as DefaultState;
DGoalState = goalState as DefaultState;
neighborMap = new GridNeighbors();
}
void moveSelectedObjectWithDirection(float x, float y, float z)
{
previousState = new ARAstarState(selectedGameObject.transform.position) as DefaultState;
Vector3 prevPos = selectedGameObject.transform.position;
selectedGameObject.transform.position = new Vector3(prevPos.x + x, prevPos.y + y, prevPos.z + z);
currentState = new ARAstarState(selectedGameObject.transform.position) as DefaultState;
//inflationFactor = 2.5f;
if (selectedGameObject == goalObject)
{
goalState = new ARAstarState(goalObject.transform.position);
prevGoalState = new ARAstarState(prevPos);
ARAstarPlanner.goalMoved = true;
planner.plannerFinished = false;
}
else if (selectedGameObject == startObject)
{
ARAstarPlanner.moved = true;
}
}
void generatePlanList(DefaultState stateReached)
{
// here we are clearing the plan list
path.Clear();
// TODO : what if we want someone to monitor the states in this plan
// TODO : this is unnecessary -- make planner use State
ARAstarState currentState = stateReached as ARAstarState;
ARAstarState starttState = new ARAstarState(startState.getPosition());
Debug.Log("generating plan to state " + currentState.state);
while (!currentState.Equals(starttState))
{
path.Add(new State(currentState.state));
currentState = outputPlan[currentState].previousState as ARAstarState;
}
// making sure start state enters as well
path.Add(new State(currentState.state));
//notifyObservers(Event.GRID_PATH_CHANGED,path);
}
// Update is called once per frame
void Update()
{
if (Input.GetKeyDown(KeyCode.A))
{
Debug.Log("Planning");
DStartState = new ARAstarState(startObject.transform.position) as DefaultState;
DGoalState = new ARAstarState(goalObject.transform.position) as DefaultState;
DPrevGoalState = prevGoalState as DefaultState;
actions.Clear();
planner.OneStep = false;
//planner.computePlan(ref DStartState, ref DGoalState, DPrevGoalState, ref outputPlan, ref inflationFactor, 1.0f);
PlanStatus = planner.computePlan(ref DStartState, ref DGoalState, ref outputPlan, ref inflationFactor, 10.0f);
Debug.Log("Status: " + PlanStatus);
if (PlanStatus == PathStatus.NoPath)
{
Debug.LogWarning("NO PLAN. PLANNING AGAIN");
//inflationFactor = 2.5f;
PlanStatus = planner.computePlan(ref DStartState, ref DGoalState, ref outputPlan, ref inflationFactor, 10.0f);
}
new WaitForEndOfFrame();
}
if (Input.GetKeyDown(KeyCode.Z))
{
showOpen = !showOpen;
}
if (Input.GetKeyDown(KeyCode.C))
{
showClose = !showClose;
}
if (Input.GetKeyDown(KeyCode.X))
{
showVisited = !showVisited;
}
if (Input.GetKeyDown(KeyCode.V))
{
showIncons = !showIncons;
}
if (Input.GetKeyDown(KeyCode.Q))
{
DStartState = new ARAstarState(startObject.transform.position) as DefaultState;
PlanStatus = PathStatus.NoPath;
planner.restartPlanner();
}
if (Input.GetKeyDown(KeyCode.S))
{
Debug.Log("Planning");
DStartState = new ARAstarState(startObject.transform.position) as DefaultState;
DGoalState = new ARAstarState(goalObject.transform.position) as DefaultState;
DPrevGoalState = prevGoalState as DefaultState;
actions.Clear();
//planner.computePlan(ref DStartState, ref DGoalState, DPrevGoalState, ref outputPlan, ref inflationFactor, 1.0f);
planner.OneStep = true;
PlanStatus = planner.computePlan(ref DStartState, ref DGoalState, ref outputPlan, ref inflationFactor, 10.0f);
Debug.Log("Status: " + PlanStatus);
new WaitForEndOfFrame();
}
if (Input.GetKeyDown(KeyCode.Return))
{
if (actions.Count > 0)
{
ARAstarAction action = actions.Pop();
Debug.Log("Direction: " + action.direction);
Vector3 prevPost = startObject.transform.position;
startObject.transform.position = new Vector3(prevPost.x + action.direction.x, prevPost.y + action.direction.y, prevPost.z + action.direction.z);
}
ARAstarPlanner.moved = true;
}
if (selectedGameObject != null)
{
if (Input.GetKeyDown(KeyCode.RightArrow))
{
moveSelectedObjectWithDirection(1.0f, 0.0f, 0.0f);
}
else if (Input.GetKeyDown(KeyCode.LeftArrow))
{
moveSelectedObjectWithDirection(-1.0f, 0.0f, 0.0f);
}
else if (Input.GetKeyDown(KeyCode.UpArrow))
{
moveSelectedObjectWithDirection(0.0f, 0.0f, 1.0f);
}
else if (Input.GetKeyDown(KeyCode.DownArrow))
{
moveSelectedObjectWithDirection(0.0f, 0.0f, -1.0f);
}
}
if (Input.GetMouseButtonDown(0))
{
RaycastHit hit;
Debug.Log("Mouse Down");
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
if (Physics.Raycast(ray, out hit, 100.0f))
{
Debug.Log("Selected");
selectedGameObject = hit.transform.gameObject;
Debug.Log("Position: " + selectedGameObject.transform.position);
}
else
{
Debug.Log("Nothing selected");
}
}
if (Input.GetKeyDown(KeyCode.Space))
{
Debug.Log("Obstacle Moved Update");
Debug.Log("Prev: " + (previousState as ARAstarState).state);
Debug.Log("Curret: " + (currentState as ARAstarState).state);
planner.UpdateAfterObstacleMoved(previousState, currentState);
}
if (Input.GetKeyDown(KeyCode.P))
{
DefaultState stateReached = planner.FillPlan();
fillActionStack(stateReached);
}
}
public override void generateTransitions(ref DefaultState currentState, ref DefaultState previousState, ref DefaultState idealGoalState, ref List <DefaultAction> transitions)
{
transitions.Clear();
ARAstarState ACurrentState = currentState as ARAstarState;
bool[] transitionsPossible = new bool[8];
// doing non-diagonals first
for (int i = 0; i < transitionsList.Count; i += 2)
{
Collider [] colliders = Physics.OverlapSphere(ACurrentState.state + transitionsList[i], 0.25F, layer);
//if (! Physics.CheckSphere(ACurrentState.state + transitionsList[i],0.5F,layer))
if (colliders.Count() == 0)
{
transitionsPossible[i] = true;
ARAstarAction action = new ARAstarAction();
action.cost = Vector3.Distance(ACurrentState.state, ACurrentState.state + transitionsList[i]);
action.direction = transitionsList[i];
ARAstarState st = new ARAstarState(ACurrentState.state + transitionsList[i]);
action.state = st;
transitions.Add(action);
}
else
{
transitionsPossible[i] = false;
// TODO: register the non deterministic obstacle here
foreach (Collider collider in colliders)
{
NonDeterministicObstacle nonDeterministicObstacle = collider.GetComponent <NonDeterministicObstacle>();
if (nonDeterministicObstacle == null)
{
} // Debug.LogWarning("planner collided with something that is not a non deterministic obtacle " + collider.name) ;
else
{
if (planningTask != null &&
_currentObservedNonDeterministicObstacles.Contains(nonDeterministicObstacle) == false) // not using a planning task, no need to register
{
Debug.LogError("NON DET OBSTACLE " + collider.name);
//nonDeterministicObstacle.observable.registerObserver(Event.NON_DETERMINISTIC_OBSTACLE_CHANGED,planningTask);
_currentObservedNonDeterministicObstacles.Add(nonDeterministicObstacle);
}
}
}
}
}
// diagonals
for (int i = 1; i < transitionsList.Count; i += 2)
{
Collider [] colliders = Physics.OverlapSphere(ACurrentState.state + transitionsList[i], 0.25F, layer);
//if (! Physics.CheckSphere(ACurrentState.state + transitionsList[i],0.5F,layer))
if (colliders.Count() == 0)
{
if (transitionsPossible[i - 1] == true || transitionsPossible[(i + 1) % transitionsList.Count] == true)
{
transitionsPossible[i] = true;
ARAstarAction action = new ARAstarAction();
action.cost = Vector3.Distance(ACurrentState.state, ACurrentState.state + transitionsList[i]);
action.direction = transitionsList[i];
ARAstarState st = new ARAstarState(ACurrentState.state + transitionsList[i]);
action.state = st;
transitions.Add(action);
}
else
{
transitionsPossible[i] = false;
}
}
else
{
transitionsPossible[i] = false;
// TODO: register the non deterministic obstacle here
foreach (Collider collider in colliders)
{
NonDeterministicObstacle nonDeterministicObstacle = collider.GetComponent <NonDeterministicObstacle>();
if (nonDeterministicObstacle == null)
{
} // Debug.LogWarning("planner collided with something that is not a non deterministic obtacle " + collider.name);
else
{
if (planningTask != null &&
_currentObservedNonDeterministicObstacles.Contains(nonDeterministicObstacle) == false) // not using a planning task, no need to register
{
Debug.Log("NON DET OBSTACLE " + collider.name);
//nonDeterministicObstacle.observable.registerObserver(Event.NON_DETERMINISTIC_OBSTACLE_CHANGED,planningTask);
_currentObservedNonDeterministicObstacles.Add(nonDeterministicObstacle);
}
}
}
}
}
}
public override void notifyEvent(Event ev, System.Object data)
{
if (initialized == false)
{
return;
}
// dont handle any events other than goal validation when task is asleep
if (taskPriority == TaskPriority.Asleep && ev != Event.GOAL_VALID)
{
return;
}
switch (ev)
{
case Event.GOAL_INVALID:
Debug.LogWarning("goal is INVALID for " + taskName);
setTaskPriority(TaskPriority.Asleep);
break;
case Event.GOAL_VALID:
Debug.LogWarning("goal is VALID for " + taskName);
setTaskPriority(TaskPriority.Inactive);
break;
case Event.STATE_POSITION_CHANGED:
// in this case, either the start must have changed or the goal must have changed
Vector3[] posChange = (Vector3[])data; // {oldPos, newPos}
Debug.Log(startState.getPosition() + " " + posChange[0] + " " + posChange[1] + " " + goalState.getPosition());
DefaultState prevState = new ARAstarState(posChange[0]) as DefaultState;
DefaultState newState = new ARAstarState(posChange[1]) as DefaultState;
if (Vector3.Distance(startState.getPosition(), posChange[1]) < 0.1F) // this should always be exactly the same if hit
{
//Debug.Log ("start state has changed in grid task " + taskName);
// TODO : this comes either from the global nav task OR the by executing the tunnel task
// second case will return a float value -- problem
if (startState.ApproximatelyEquals(goalState, 1.0f)) // this is being compared to the grid task goal -- not the tunnel task goal
{
Debug.LogWarning("REACHED GOAL in " + taskName);
path.Clear();
spaceTimePath.Clear();
startState.unregisterObserver(Event.STATE_POSITION_CHANGED, this);
goalState.unregisterObserver(Event.GOAL_INVALID, this);
goalState.unregisterObserver(Event.GOAL_VALID, this);
goalState.unregisterObserver(Event.STATE_POSITION_CHANGED, this);
// TODO : unregister the non determinstic obstacles -- we really just need to keep a UNIQUE list (set) here
// task is complete
taskCompleted();
break;
}
else if (Vector3.Distance(posChange[0], posChange[1]) > 1.0F) // NOT A MAJOR CHANGE IN START POSITION (ASSUMING ALWAYS ALONG THE PATH
{
setTaskPriority(TaskPriority.RealTime);
}
// always set this -- make sure you do call UpdateAfterStartMoved if this task is executed
startChanged = true;
}
else if (Vector3.Distance(goalState.getPosition(), posChange[1]) < 0.1F)
{
Debug.LogError("goal state has changed in grid task");
goalChanged = true;
setTaskPriority(TaskPriority.RealTime);
}
break;
case Event.NON_DETERMINISTIC_OBSTACLE_CHANGED:
if (currentlyExecutingTask)
{
obstacleChanged = true;
// TODO : what if the same obstacle changes ? we need to check for that
obstacleChangedData.Add(data);
Debug.LogError("grid task has recieved non deterministic obstacle change event ");
setTaskPriority(TaskPriority.RealTime);
}
break;
case Event.NEIGBHOR_AGENTS_CHANGED:
Debug.LogError("NEIGHBOR AGENTS HAVE CHANGED WE NEED TO REPLAN SPACE TIME");
dynamicChange = true;
setTaskPriority(TaskPriority.RealTime);
return;
case Event.NEIGBHOR_DYNAMIC_OBSTACLES_CHANGED:
Debug.LogError("NEIGHBOR OBSTACLES HAVE CHANGED WE NEED TO REPLAN SPACE TIME");
dynamicChange = true;
setTaskPriority(TaskPriority.RealTime);
return;
case Event.CURRENT_EXECUTING_TASK:
Debug.LogError("I AM NOW THE CURRENT EXECUTING TASK");
setTaskPriority(TaskPriority.RealTime);
return;
}
}
public override TaskStatus execute(float maxTime)
{
// TODO : make sure start and goal are set to integers -- else this planner will crash, never return
// TODO : make sure the 'y' value is consistent throughout -- causes big problems
// as soon as u see the planner expanding max nodes -- u know its prob y value .
bool doWeNeedToGridPlan = false;
if (firstPlan == false)
{
doWeNeedToGridPlan = true;
}
if (startChanged == true && firstPlan == true)
{
Debug.LogError("HANDLING START CHANGE ");
DefaultState newStartState = new ARAstarState(startState.getPosition()) as DefaultState;
gridPlanner.UpdateAfterStartMoved(newStartState);
startChanged = false;
doWeNeedToGridPlan = true;
}
// handle obstacle changes here
if (obstacleChanged == true && firstPlan == true)
{
Debug.LogError("HANDLING OBSTACLE CHANGE");
foreach (object data in obstacleChangedData)
{
Vector3[] pc = (Vector3[])data; // {oldPos, newPos}
DefaultState ps = new ARAstarState(pc[0]) as DefaultState;
DefaultState ns = new ARAstarState(pc[1]) as DefaultState;
gridPlanner.UpdateAfterObstacleMoved(ps, ns);
}
obstacleChanged = false;
obstacleChangedData.Clear();
doWeNeedToGridPlan = true;
}
if (goalChanged == true && firstPlan == true) // update goal is called only if we have already planned once
{
Debug.LogError("HANDLING GOAL CHANGE");
DefaultState newGoalState = new ARAstarState(goalState.getPosition()) as DefaultState;
gridPlanner.UpdateAfterGoalMoved(newGoalState);
doWeNeedToGridPlan = true;
}
// SINISTER BUG THAT GOALCHANGED WAS NOT BEING SET TO FALSE IF firstPlan was false -- always set it when we come here
goalChanged = false;
// grid planning only if start, goal, or obstracle changed OR first time
if (doWeNeedToGridPlan)
{
Debug.Log("grid Planning from " + startState.getPosition() + "to " + goalState.getPosition() + " " + doWeNeedToGridPlan + " " + firstPlan);
DefaultState DStartState = new ARAstarState(startState.getPosition()) as DefaultState;
DefaultState DGoalState = new ARAstarState(goalState.getPosition()) as DefaultState;
gridPathStatus = gridPlanner.computePlan(ref DStartState, ref DGoalState, ref outputPlan, ref inflationFactor, 1.0f);
Debug.Log("grid Plan result : " + gridPathStatus);
// filling plan -- made separate operations now
DefaultState stateReached = gridPlanner.FillPlan();
generatePlanList(stateReached);
if (firstPlan == false)
{
firstPlan = true;
}
}
// space time planning only if grid plan chagned or DYNAMIC CHANGE
if (doWeNeedToGridPlan || dynamicChange || firstPlan)
{
Debug.LogError("SPACE TIME PLANNING");
spaceTimePathStatus = false;
if (gridPathStatus == PathStatus.NoPath || gridPathStatus == PathStatus.Incomplete)
{
Debug.LogError("GRID PLAN IS INCOMPLETE OR NO PATH EXISTS -- CANNOT DO TUNNEL PLANNING");
}
else
{
spaceTimePathStatus = doTunnelTask();
}
dynamicChange = false; // just assuming it was handled here for now
}
// TODO : accomodate tunnel status in this
//if ( finished != PathStatus.Optimal)
if (gridPathStatus == PathStatus.Incomplete || spaceTimePathStatus == false)
{
setTaskPriority(TaskPriority.RealTime);
}
else
{
setTaskPriority(TaskPriority.Inactive);
}
return(TaskStatus.Success);
}
public override TaskStatus execute(float maxTime)
{
// TODO : make sure start and goal are set to integers -- else this planner will crash, never return
// TODO : make sure the 'y' value is consistent throughout -- causes big problems
// as soon as u see the planner expanding max nodes -- u know its prob y value .
bool doWeNeedToGridPlan = false;
if (firstPlan == false) doWeNeedToGridPlan = true;
if (startChanged == true && firstPlan == true )
{
Debug.LogError("HANDLING START CHANGE " );
DefaultState newStartState = new ARAstarState( startState.getPosition() ) as DefaultState;
gridPlanner.UpdateAfterStartMoved(newStartState);
startChanged = false;
doWeNeedToGridPlan = true;
}
// handle obstacle changes here
if (obstacleChanged == true && firstPlan == true)
{
Debug.LogError("HANDLING OBSTACLE CHANGE");
foreach (object data in obstacleChangedData)
{
Vector3[] pc = (Vector3[]) data; // {oldPos, newPos}
DefaultState ps = new ARAstarState( pc[0] ) as DefaultState;
DefaultState ns = new ARAstarState( pc[1] ) as DefaultState;
gridPlanner.UpdateAfterObstacleMoved(ps, ns);
}
obstacleChanged = false;
obstacleChangedData.Clear ();
doWeNeedToGridPlan = true;
}
if (goalChanged == true && firstPlan == true) // update goal is called only if we have already planned once
{
Debug.LogError("HANDLING GOAL CHANGE");
DefaultState newGoalState = new ARAstarState( goalState.getPosition() ) as DefaultState;
gridPlanner.UpdateAfterGoalMoved(newGoalState);
doWeNeedToGridPlan = true;
}
// SINISTER BUG THAT GOALCHANGED WAS NOT BEING SET TO FALSE IF firstPlan was false -- always set it when we come here
goalChanged = false;
// grid planning only if start, goal, or obstracle changed OR first time
if (doWeNeedToGridPlan)
{
Debug.Log("grid Planning from " + startState.getPosition() + "to " + goalState.getPosition() + " " + doWeNeedToGridPlan + " " + firstPlan);
DefaultState DStartState = new ARAstarState(startState.getPosition()) as DefaultState;
DefaultState DGoalState = new ARAstarState(goalState.getPosition()) as DefaultState;
gridPathStatus = gridPlanner.computePlan(ref DStartState, ref DGoalState, ref outputPlan, ref inflationFactor, 1.0f);
Debug.Log("grid Plan result : " + gridPathStatus);
// filling plan -- made separate operations now
DefaultState stateReached = gridPlanner.FillPlan();
generatePlanList (stateReached);
if ( firstPlan == false ) firstPlan = true;
}
// space time planning only if grid plan chagned or DYNAMIC CHANGE
if ( doWeNeedToGridPlan || dynamicChange || firstPlan)
{
Debug.LogError("SPACE TIME PLANNING");
spaceTimePathStatus = false;
if (gridPathStatus == PathStatus.NoPath || gridPathStatus == PathStatus.Incomplete )
{
Debug.LogError("GRID PLAN IS INCOMPLETE OR NO PATH EXISTS -- CANNOT DO TUNNEL PLANNING");
}
else
spaceTimePathStatus = doTunnelTask ();
dynamicChange = false; // just assuming it was handled here for now
}
// TODO : accomodate tunnel status in this
//if ( finished != PathStatus.Optimal)
if ( gridPathStatus == PathStatus.Incomplete || spaceTimePathStatus == false)
setTaskPriority(TaskPriority.RealTime);
else
setTaskPriority(TaskPriority.Inactive);
return TaskStatus.Success;
}
void generatePlanList(DefaultState stateReached)
{
// here we are clearing the plan list
path.Clear();
// TODO : what if we want someone to monitor the states in this plan
// TODO : this is unnecessary -- make planner use State
ARAstarState currentState = stateReached as ARAstarState;
ARAstarState starttState = new ARAstarState(startState.getPosition());
Debug.Log ("generating plan to state " + currentState.state);
while(!currentState.Equals(starttState)){
path.Add(new State(currentState.state));
currentState = outputPlan[currentState].previousState as ARAstarState;
}
// making sure start state enters as well
path.Add(new State(currentState.state));
//notifyObservers(Event.GRID_PATH_CHANGED,path);
}
public override void notifyEvent(Event ev, System.Object data)
{
if (initialized == false) return;
// dont handle any events other than goal validation when task is asleep
if (taskPriority == TaskPriority.Asleep && ev != Event.GOAL_VALID)
return;
switch (ev)
{
case Event.GOAL_INVALID :
Debug.LogWarning ("goal is INVALID for " + taskName);
setTaskPriority(TaskPriority.Asleep);
break;
case Event.GOAL_VALID :
Debug.LogWarning ("goal is VALID for " + taskName);
setTaskPriority(TaskPriority.Inactive);
break;
case Event.STATE_POSITION_CHANGED:
// in this case, either the start must have changed or the goal must have changed
Vector3[] posChange = (Vector3[]) data; // {oldPos, newPos}
Debug.Log (startState.getPosition() + " " + posChange[0] + " " + posChange[1] + " " + goalState.getPosition()) ;
DefaultState prevState = new ARAstarState( posChange[0] ) as DefaultState;
DefaultState newState = new ARAstarState( posChange[1] ) as DefaultState;
if (Vector3.Distance(startState.getPosition(),posChange[1]) < 0.1F) // this should always be exactly the same if hit
{
//Debug.Log ("start state has changed in grid task " + taskName);
// TODO : this comes either from the global nav task OR the by executing the tunnel task
// second case will return a float value -- problem
if ( startState.ApproximatelyEquals(goalState,1.0f) ) // this is being compared to the grid task goal -- not the tunnel task goal
{
Debug.LogWarning("REACHED GOAL in " + taskName);
path.Clear ();
spaceTimePath.Clear ();
startState.unregisterObserver(Event.STATE_POSITION_CHANGED,this);
goalState.unregisterObserver(Event.GOAL_INVALID, this);
goalState.unregisterObserver(Event.GOAL_VALID, this);
goalState.unregisterObserver(Event.STATE_POSITION_CHANGED, this);
// TODO : unregister the non determinstic obstacles -- we really just need to keep a UNIQUE list (set) here
// task is complete
taskCompleted ();
break;
}
else if (Vector3.Distance(posChange[0],posChange[1]) > 1.0F) // NOT A MAJOR CHANGE IN START POSITION (ASSUMING ALWAYS ALONG THE PATH
{
setTaskPriority(TaskPriority.RealTime);
}
// always set this -- make sure you do call UpdateAfterStartMoved if this task is executed
startChanged = true;
}
else if (Vector3.Distance(goalState.getPosition(),posChange[1]) < 0.1F)
{
Debug.LogError ("goal state has changed in grid task");
goalChanged = true;
setTaskPriority(TaskPriority.RealTime);
}
break;
case Event.NON_DETERMINISTIC_OBSTACLE_CHANGED:
if ( currentlyExecutingTask )
{
obstacleChanged = true;
// TODO : what if the same obstacle changes ? we need to check for that
obstacleChangedData.Add(data);
Debug.LogError("grid task has recieved non deterministic obstacle change event ");
setTaskPriority(TaskPriority.RealTime);
}
break;
case Event.NEIGBHOR_AGENTS_CHANGED :
Debug.LogError("NEIGHBOR AGENTS HAVE CHANGED WE NEED TO REPLAN SPACE TIME");
dynamicChange = true;
setTaskPriority(TaskPriority.RealTime);
return;
case Event.NEIGBHOR_DYNAMIC_OBSTACLES_CHANGED :
Debug.LogError("NEIGHBOR OBSTACLES HAVE CHANGED WE NEED TO REPLAN SPACE TIME");
dynamicChange = true;
setTaskPriority(TaskPriority.RealTime);
return;
case Event.CURRENT_EXECUTING_TASK :
Debug.LogError("I AM NOW THE CURRENT EXECUTING TASK");
setTaskPriority(TaskPriority.RealTime);
return;
}
}
