// 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);
}
}
}
}
}
}
