// Update is called once per frame
void Update()
{
if (Input.GetKeyDown(KeyCode.A))
{
Debug.Log("Planning");
DStartState = new BestFirstState(startObject.transform.position) as DefaultState;
DGoalState = new BestFirstState(goalObject.transform.position) as DefaultState;
planner.oneStep = false;
planner.computePlan(ref DStartState, ref DGoalState, ref outputPlan, 10.0f);
}
if (Input.GetKeyDown(KeyCode.S))
{
Debug.Log("Planning");
DStartState = new BestFirstState(startObject.transform.position) as DefaultState;
DGoalState = new BestFirstState(goalObject.transform.position) as DefaultState;
planner.oneStep = true;
planner.computePlan(ref DStartState, ref DGoalState, ref outputPlan, 10.0f);
}
if (Input.GetKeyDown(KeyCode.Z))
{
showOpen = !showOpen;
}
if (Input.GetKeyDown(KeyCode.X))
{
showVisited = !showVisited;
}
}
public override bool isAGoalState(ref DefaultState state, ref DefaultState idealGoalState)
{
BestFirstState currentState = state as BestFirstState;
BestFirstState goalState = idealGoalState as BestFirstState;
return(Vector3.Distance(currentState.state, goalState.state) < 1);
}
public BestFirstAction(DefaultState _from, DefaultState _to)
{
BestFirstState Afrom = _from as BestFirstState;
BestFirstState Ato = _to as BestFirstState;
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)
{
BestFirstState ARAstate1 = s1 as BestFirstState;
BestFirstState ARAstate2 = s2 as BestFirstState;
if (ARAstate1.Equals(ARAstate2))
{
return(true);
}
else
{
return(false);
}
}
// Use this for initialization
void Start()
{
actions = new Stack <BestFirstAction>();
outputPlan = new Stack <DefaultState>();
domainList = new List <PlanningDomainBase>();
domainList.Add(new BestFirstDomain());
planner = new BestFirstSearchPlanner();
planner.init(ref domainList, 1000);
startState = new BestFirstState(startObject.transform.position);
goalState = new BestFirstState(goalObject.transform.position);
prevGoalState = goalState;
DStartState = startState as DefaultState;
DGoalState = goalState as DefaultState;
}
public override void generateTransitions(ref DefaultState currentState, ref DefaultState previousState, ref DefaultState idealGoalState, ref List <DefaultAction> transitions)
{
transitions.Clear();
BestFirstState ACurrentState = currentState as BestFirstState;
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;
BestFirstAction action = new BestFirstAction();
action.cost = Vector3.Distance(ACurrentState.state, ACurrentState.state + transitionsList[i]);
action.direction = transitionsList[i];
BestFirstState st = new BestFirstState(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.Log("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;
BestFirstAction action = new BestFirstAction();
action.cost = Vector3.Distance(ACurrentState.state, ACurrentState.state + transitionsList[i]);
action.direction = transitionsList[i];
BestFirstState st = new BestFirstState(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);
}
}
}
}
}
}
