public float RemainingDistance()
{
Vector3 toGoal = goalState.getPosition() - currentState.getPosition();
float distanceToGoal = toGoal.magnitude;
if (goalReached == true)
{
distanceToGoal = 0.0f;
}
return(distanceToGoal);
}
public override TaskStatus execute(float maxTime)
{
GlobalNavigator.CalculateNavigationMeshPath(startState.getPosition(), goalState.getPosition(), passableMask);
TaskStatus taskStatus = TaskStatus.Success;
switch (GlobalNavigator.pathStatus)
{
case NavMeshPathStatus.PathComplete:
taskStatus = TaskStatus.Success; break;
case NavMeshPathStatus.PathInvalid:
taskStatus = TaskStatus.Failure; break;
case NavMeshPathStatus.PathPartial:
taskStatus = TaskStatus.Incomplete; break;
}
// TODO: change task priority after execution
setTaskPriority(TaskPriority.Inactive);
// notify agent that the global path has changed
notifyObservers(Event.GLOBAL_PATH_CHANGED, null);
return(taskStatus);
}
// Update is called once per frame
void Update()
{
float distance = Vector3.SqrMagnitude(transform.position - goalState.getPosition());
if (distance > 1.0F)
{
goalState.setPosition(transform.position);
}
}
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 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);
}
////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////
// Use this for initialization
public void InitializeAgentBrain()
{
// NAV MESH AGENT TEST
GetComponent <NavMeshAgent>().Stop();
GetComponent <NavMeshAgent>().updatePosition = false;
GetComponent <NavMeshAgent>().updateRotation = false;
locomotionSystem = GetComponent <SoldierLocomotion>();
// initializing neighbor lists
neighborObstacles = GetComponent <NeighbourObstacles>();
neighborObstacles.Init();
neighborAgents = GetComponent <NeighbourAgents>();
neighborAgents.Init();
currentState = new State(transform.position);
if (GlobalNavigator.usingDynamicNavigationMesh)
{
currentPolygonIndex = GlobalNavigator.recastSteeringManager.GetClosestPolygon(currentState.getPosition());
}
if (goal == null)
{
Debug.LogError("Goal has not been initialized for this agent");
}
goalState = goal.goalState;
passableMask = 1; // this could be part of the goal
globalPath = new List <State> ();
numGlobalPathWaypoints = 0;
globalPathPolygonIndices = new List <uint> ();
localPaths = new List <List <State> > ();
spaceTimePaths = new List <List <State> > ();
currentPlanExecutionIndex = 0;
offMeshLinkWayPoint = new List <bool>();
taskManager = new TaskManager();
// creating global navigation task
globalNavigationTask = new GlobalNavigationtask(currentState, goalState, passableMask,
TaskPriority.RealTime, taskManager);
globalNavigationTask.taskName = "globalNavigationTask";
// EVENT: AgentBrain is triggered by the globalNavigationTask when GLOBAL_PATH_CHANGED
globalNavigationTask.registerObserver(Event.GLOBAL_PATH_CHANGED, this);
gridNavigationTasks = new List <GridNavigationTask>();
// creating grid navigation and grid time tasks for each statically allocated global waypoint
// -- we can statically allocate how many ever we want
for (int i = 0; i < 1; i++)
{
_addNewEmptyGlobalPathWayPointAtIndex(i);
}
curves = new AnimationCurve[4];
curves[0] = new AnimationCurve(); // x
curves[1] = new AnimationCurve(); // y
curves[2] = new AnimationCurve(); // z
curves[3] = new AnimationCurve(); // speed
}
////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////
// Use this for initialization
public void InitializeAgentBrain()
{
// NAV MESH AGENT TEST
GetComponent<NavMeshAgent>().Stop ();
GetComponent<NavMeshAgent>().updatePosition = false;
GetComponent<NavMeshAgent>().updateRotation = false;
locomotionSystem = GetComponent<SoldierLocomotion>();
// initializing neighbor lists
neighborObstacles = GetComponent<NeighbourObstacles>();
neighborObstacles.Init ();
neighborAgents = GetComponent<NeighbourAgents>();
neighborAgents.Init();
currentState = new State(transform.position);
if (GlobalNavigator.usingDynamicNavigationMesh)
{
currentPolygonIndex = GlobalNavigator.recastSteeringManager.GetClosestPolygon(currentState.getPosition());
}
if ( goal == null ) Debug.LogError("Goal has not been initialized for this agent");
goalState = goal.goalState;
passableMask = 1; // this could be part of the goal
globalPath = new List<State> ();
numGlobalPathWaypoints = 0;
globalPathPolygonIndices = new List<uint> ();
localPaths = new List<List<State>> ();
spaceTimePaths = new List<List<State>> ();
currentPlanExecutionIndex = 0;
offMeshLinkWayPoint = new List<bool>();
taskManager = new TaskManager ();
// creating global navigation task
globalNavigationTask = new GlobalNavigationtask(currentState,goalState,passableMask,
TaskPriority.RealTime, taskManager);
globalNavigationTask.taskName = "globalNavigationTask";
// EVENT: AgentBrain is triggered by the globalNavigationTask when GLOBAL_PATH_CHANGED
globalNavigationTask.registerObserver(Event.GLOBAL_PATH_CHANGED, this);
gridNavigationTasks = new List<GridNavigationTask>();
// creating grid navigation and grid time tasks for each statically allocated global waypoint
// -- we can statically allocate how many ever we want
for (int i=0; i < 1; i ++ )
{
_addNewEmptyGlobalPathWayPointAtIndex(i);
}
curves = new AnimationCurve[4];
curves[0] = new AnimationCurve(); // x
curves[1] = new AnimationCurve(); // y
curves[2] = new AnimationCurve(); // z
curves[3] = new AnimationCurve(); // speed
}
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 bool ApproximatelyEquals( State otherState, float distanceThreshold = 0.25F, float timeThreshold = 1.0F)
{
return (Vector3.Distance(otherState.getPosition(),this.getPosition()) < distanceThreshold);
//&& Mathf.Abs(this._time - otherState._time) < timeThreshold);
}
public override bool Equals(object obj)
{
State objState = obj as State;
return(obj != null && (Vector3.Distance(objState.getPosition(), this.getPosition()) < 0.25F));
}
public bool ApproximatelyEquals(State otherState, float distanceThreshold = 0.25F, float timeThreshold = 1.0F)
{
return(Vector3.Distance(otherState.getPosition(), this.getPosition()) < distanceThreshold);
//&& Mathf.Abs(this._time - otherState._time) < timeThreshold);
}
