private void Move()
        {
            // update mode for each move
            if (mode == ModeEnum.EXPLORING)
                npAgentGameMode = Stage.GameMode.NP_AGENT_EXPLORING;
            else if (mode == ModeEnum.TREASURE_HUNTING)
                npAgentGameMode = Stage.GameMode.NP_AGENT_TREASURE_HUNTING;

            if (!flagSignal) {
                AddPathToExploreQueue();
                flagSignal = true;
            }

            displayAgentInfo();

            // see if at or close to nextGoal, distance measured in the flat XZ plane
            float distance = Vector3.Distance(
               new Vector3(nextGoal.Translation.X, 0, nextGoal.Translation.Z),
               new Vector3(agentObject.Translation.X, 0, agentObject.Translation.Z));

            if (distance <= snapDistance) {
                stage.SetInfo(17, string.Format("distance to goal = {0,5:f2}", distance));

                // snap to nextGoal and orient toward the new nextGoal
                nextGoal = currentPath.NextNode;
                agentObject.TurnToFace(nextGoal.Translation);

                /*
                 * Here we have two options when the current path is done
                 *      if npAgent is in TREASURE_HUNTING mode
                 *      if npAgent is in EXPLORING mode
                 */
                if (currentPath.Done) {
                    Debug.WriteLine("Path traversal is done.");
                    if (mode == ModeEnum.TREASURE_HUNTING) {
                        HandleTreasureMode();
                    }
                    else if (mode == ModeEnum.EXPLORING) {
                        HandleExploringMode();
                    }
                }
                else {
                    turnCount++;
                    // stage.SetInfo(18, string.Format("turnToFace count = {0}", turnCount));
                }
            }
        }
        private void HandleTreasureMode()
        {
            if (treasurePathQueue.Count == 0) {
                // switch mode
                mode = ModeEnum.EXPLORING;
                npAgentGameMode = Stage.GameMode.NP_AGENT_EXPLORING;

                // resume to previous exploring path if any
                if (previousPath != null && !previousPath.Done) {
                    currentPath = previousPath;
                    // nextGoal = currentPath.NextNode;
                    // agentObject.TurnToFace(nextGoal.Translation);
                }
                else {
                    HandleExploringMode();
                }
            }
            else {
                // get another path from treasure path queue
                currentPath = treasurePathQueue.Dequeue();
                // add path to stage two show trace
                stage.Components.Add(currentPath);
                // get first path goal
                nextGoal = currentPath.NextNode;
                // orient towards the first path goal
                agentObject.TurnToFace(nextGoal.Translation);
            }
        }
        /// <summary>
        /// A very simple limited random walk.  Repeatedly moves skipSteps forward then
        /// randomly decides how to turn (left, right, or not to turn).  Does not move
        /// very well -- its just an example...
        /// </summary>
        public override void Update(GameTime gameTime)
        {
            // find a treasure withing detection range
            int idx = DetectTreasure();

            // there is still treasure
            if (idx != -1) {
                npAgentGameMode = Stage.GameMode.NP_AGENT_TREASURE_HUNTING;
                ChangeToTreasurePath(idx);
            }

            if (!done)
                Move();

            base.Update(gameTime);
        }
 private void HandleExploringMode()
 {
     if (explorePathQueue.Count == 0) {
         done = true;
         npAgentGameMode = Stage.GameMode.NP_AGENT_TREASURE_STOP;
     }
     else {
         // get another path from explore path queue
         currentPath = explorePathQueue.Dequeue();
         // get first path goal
         nextGoal = currentPath.NextNode;
         // orient towards the first path goal
         agentObject.TurnToFace(nextGoal.Translation);
     }
 }
        /// <summary>
        /// Create a NPC. 
        /// AGXNASK distribution has npAgent move following a Path.
        /// </summary>
        /// <param name="theStage"> the world</param>
        /// <param name="label"> name of </param>
        /// <param name="pos"> initial position </param>
        /// <param name="orientAxis"> initial rotation axis</param>
        /// <param name="radians"> initial rotation</param>
        /// <param name="meshFile"> Direct X *.x Model in Contents directory </param>
        public NPAgent(Stage theStage, string label, Vector3 pos, Vector3 orientAxis, float radians, string meshFile)
            : base(theStage, label, pos, orientAxis, radians, meshFile)
        {
            IsCollidable = true;

            first.Name = "npFirst";
            follow.Name = "npFollow";
            above.Name = "npAbove";

            // initialize all treasures
            InitializeMarkTreasures();

            // initialize treasure path queue
            treasurePathQueue = new Queue<Path>();
            // initialize exploring path queue
            explorePathQueue = new Queue<Path>();

            // add all predefined path to explore path queue
            // AddPathToExploreQueue();

            Path initialPath = new Path(stage, MakeExploringPaths(), Path.PathType.SINGLE, true);

             // set it to current path
            currentPath = initialPath;

            // set the mode
            mode = ModeEnum.EXPLORING;
            npAgentGameMode = Stage.GameMode.NP_AGENT_EXPLORING;

            stage.Components.Add(currentPath);
            nextGoal = currentPath.NextNode;
            agentObject.TurnToFace(nextGoal.Translation);
        }