The “planning” package

1. Package description

The “planning” package implements the planning module which serves as the brain of a human agent. The package contains classes as listed below:

• AbstractPathFinding
• AgentPath
• DoorSorter
• EvacuationHumanPathFinding
• EvacuationHumanPlanExecutor
• EvacuationHumanPlanGenerator
• EvacuationHumanPlanningModule
• EvacuationHumanReactionModule
• EvacuationPath
• EvacuationPlan
• HumanPlanningModule
• NodeSorter
• OrcaLine
• PathFinding
• PathFindingNode
• PlanEvaluator
• PlanStorageModule
• RVO_Utils
• RVOObstacle
• RVOPathFinder

2. Package implementation

To implement the “AbstractPathFinding” class:

• • Create a new class inside the “planning” package and name it “AbstractPathFinding”.

• Copy the code given below that describes the full implementation of a “AbstractPathFinding” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import java.io.Serializable;

import edu.utdallas.mavs.divas.core.sim.agent.knowledge.KnowledgeModule;

public abstract class AbstractPathFinding<KM extends KnowledgeModule<?>> implements PathFinding<KM>, Serializable
{
    private static final long serialVersionUID = 1L;
    
    protected KM knowledgeModule;

    public AbstractPathFinding(KM knowledgeModule)
    {
        super();
        this.knowledgeModule = knowledgeModule;
    }
}

To implement the “AgentPath” class:

• • Create a new class inside the “planning” package and name it “AgentPath”.

• Copy the code given below that describes the full implementation of a “AgentPath” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import java.io.Serializable;
import java.util.List;

import edu.utdallas.mavs.divas.core.sim.agent.knowledge.ontology.Thing;

/**
 * An evacuation path
 */
public class AgentPath implements Serializable
{
    private static final long serialVersionUID = 1L;

    List<Thing>               path;

    float                     value;
    Thing                     next;
    float                     distanceFactor;

    public AgentPath()
    {
        super();
        path = null;
    }

    public List<Thing> getPath()
    {
        return path;
    }

    public void setPath(List<Thing> path)
    {
        this.path = path;
    }

    public void removeLast()
    {
        path.remove(0);
    }

    public Thing getFirstStep()
    {
        System.out.println(path.size());


        if(path.size() > 1)
        {
            next = path.get(0);
            return next;
        }
        if(path.size() == 1)
        {
            Thing retVal = path.get(0);
            path = null;
            return retVal;
        }
        return null;
    }

    public Thing getFinalStep()
    {
        System.out.println(path.size());

        if(path.size() > 0)
        {
            next = path.get(path.size() - 1);
            return next;
        }
        return null;
    }

    public Thing getNextStep()
    {
        removeLast();
        if(path.size() > 0)
        {
            next = path.get(0);
            return next;
        }
        path = null;
        return null;
    }

    public Thing getNextStepNoRemove()
    {
        if(path.size() > 0)
        {
            next = path.get(0);
            return next;
        }
        path = null;
        return null;
    }

    public float getValue()
    {
        return value;
    }

    public void setValue(float value)
    {
        this.value = value;
    }

    public float getDistanceFactor()
    {
        return distanceFactor;
    }

    public void setDistanceFactor(float distanceFactor)
    {
        this.distanceFactor = distanceFactor;
    }

}

To implement the “DoorSorter” class:

• • Create a new class inside the “planning” package and name it “DoorSorter”.

• Copy the code given below that describes the full implementation of a “DoorSorter” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import java.io.Serializable;
import java.util.List;
import java.util.Map;

import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.ontology.Door;

/**
 * A quick sort implementation for path finding nodes
 */
public class DoorSorter implements Serializable
{
    private static final long  serialVersionUID = 1L;
    private List<Integer>      nodeIDs;
    private int                number;
    private Map<Integer, Door> nodeStore;
    private Door               goalNode;

    /**
     * Create a node sorter. (Using integer IDs and this storage map.)
     * 
     * @param nodeStore
     *        the node storage map
     * @param goalNode
     * @param startNode
     */
    public DoorSorter(Map<Integer, Door> nodeStore, Door goalNode)
    {
        this.nodeStore = nodeStore;
        this.goalNode = goalNode;
    }

    /**
     * Method to sort these path finding nodes (using quick sort)
     * 
     * @param values2
     *        nodes to be sorted
     */
    public void sort(List<Integer> values2)
    {
        // Check for empty or null array
        if(values2 == null || values2.size() == 0)
        {
            return;
        }
        this.nodeIDs = values2;
        number = values2.size();
        quicksort(0, number - 1);
    }

    private void quicksort(int low, int high)
    {
        int i = low, j = high;
        // Get the pivot element from the middle of the list
        int pivot = nodeIDs.get(low + (high - low) / 2);

        // Divide into two lists
        while(i <= j)
        {
            // If the current value from the left list is smaller then the pivot
            // element then get the next element from the left list
            while(getProperNode(nodeIDs.get(i)).getFscore() < getProperNode(pivot).getFscore())
            {
                i++;
            }
            // If the current value from the right list is larger then the pivot
            // element then get the next element from the right list
            while(getProperNode(nodeIDs.get(j)).getFscore() > getProperNode(pivot).getFscore())
            {
                j--;
            }

            // If we have found a values in the left list which is larger then
            // the pivot element and if we have found a value in the right list
            // which is smaller then the pivot element then we exchange the
            // values.
            // As we are done we can increase i and j
            if(i <= j)
            {
                exchange(i, j);
                i++;
                j--;
            }
        }
        // Recursion
        if(low < j)
            quicksort(low, j);
        if(i < high)
            quicksort(i, high);
    }

    protected Door getProperNode(Integer nodeID)
    {
        if(nodeID == goalNode.getId())
        {
            return goalNode;
        }
        return nodeStore.get(nodeID);
    }

    private void exchange(int i, int j)
    {
        int temp = nodeIDs.get(i);
        nodeIDs.set(i, nodeIDs.get(j));
        nodeIDs.set(j, temp);
    }
}

To implement the “EvacuationHumanPathFinding” class:

• • Create a new class inside the “planning” package and name it “EvacuationHumanPathFinding”.

• Copy the code given below that describes the full implementation of a “EvacuationHumanPathFinding” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import java.io.Serializable;
import java.util.ArrayDeque;
import java.util.ArrayList;
import java.util.List;
import java.util.Random;

import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import com.jme3.math.FastMath;
import com.jme3.math.Vector2f;
import com.jme3.math.Vector3f;

import edu.utdallas.mavs.divas.core.sim.agent.knowledge.external.Collision;
import edu.utdallas.mavs.divas.core.sim.agent.knowledge.external.Collision.CollisionType;
import edu.utdallas.mavs.divas.core.sim.agent.knowledge.ontology.Thing;
import edu.utdallas.mavs.divas.core.sim.agent.planning.Plan;
import edu.utdallas.mavs.divas.core.sim.common.state.EnvObjectState;
import edu.utdallas.mavs.divas.utils.collections.LightWeightBoundedMap;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.EvacuationHumanKnowledgeModule;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.EvacuationHumanKnowledgeModule.MentalStateOfAgent;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.external.AgentKnowledgeStorageObject;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.external.EnvObjectKnowledgeStorageObject;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.external.VirtualKnowledgeStorageObject;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.external.VisionModel;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.ontology.Door;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.task.FaceTask;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.task.HumanTaskModule;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.task.IdleTask;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.task.MoveTask;
import edu.utdallas.mavs.evacuation.simulation.sim.common.state.EHumanAgentState;
import edu.utdallas.mavs.evacuation.simulation.sim.common.state.Posture;

public class EvacuationHumanPathFinding extends AbstractPathFinding<EvacuationHumanKnowledgeModule> implements Serializable
{
    private static final int                                MAX_SUBGOAL_CYC   = 20;

    private static final int                                MAX_NODE          = 40;

    private static final long                               serialVersionUID  = 1L;

    private final static Logger                             logger            = LoggerFactory.getLogger(EvacuationHumanPathFinding.class);

    private static final int                                STARTGOALMAXDIST  = 95;
    private static final int                                MAXUPDATEPERCYCLE = 50;

    private static final String                             WALL              = "wall";
    private static final String                             ENV_OBJECT        = "envObject";
    private static final String                             FLOOR             = "floor";
    private static final String                             DOOR              = "door";
    private static final String                             HEADING           = "Heading";
    private static final String                             POSITION          = "Position";
    private static final String                             AGENT             = "agent";
    private static final String                             IDLE              = "Idle";

    private HumanTaskModule                                 taskModule;

    private static final float                              space             = 1.5f;
    private static final float                              maxAngleTurn      = FastMath.PI / 4;

    private List<PathFindingNode>                           agentPath         = null;
    private boolean                                         replan            = false;

    private LightWeightBoundedMap<Integer, PathFindingNode> nodeStore         = new LightWeightBoundedMap<Integer, PathFindingNode>(MAX_NODE);

    private ArrayDeque<PathFindingNode>                     nodesToProcess;

    private PathFindingNode                                 newNodeSave;

    private Vector3f                                        lastMove;

    private boolean                                         unableToFindPath;

    RVOPathFinder                                           RVO;

    Vector3f                                                savedpos          = new Vector3f();

    private Vector3f                                        tempSubGoal       = null;

    int                                                     turnTend          = 1;

    private long                                            subGoalStartCycle = 0;

    private HeadingType                                     heading           = HeadingType.GOING;

    public enum HeadingType
    {
        GOING, TURNING, PUSHED
    }

    public EvacuationHumanPathFinding(EvacuationHumanKnowledgeModule knowledgeModule, HumanTaskModule taskModule)
    {
        super(knowledgeModule);
        this.taskModule = taskModule;
        nodesToProcess = new ArrayDeque<PathFindingNode>();
        RVO = new RVOPathFinder(knowledgeModule);
    }

    @Override
    public void pathplan(Vector3f vecGoal, Plan generatedPlan)
    {
        if(knowledgeModule.isWaiting())
        {
            knowledgeModule.setWaitingTime(knowledgeModule.getWaitingTime() + 1);
            if(knowledgeModule.getWaitingTime() > 3)
            {
                knowledgeModule.setWaiting(false);
            }
        }

        List<Collision> collisions = getState().getCollisions();

        // System.out.println("Agent id: " + getState().getID());
        // System.out.println("I am at: " + getState().getPosition());
        // if(agentPath != null)
        // {
        //
        // printPath(agentPath, 1);
        // }
        // else
        // {
        // System.out.println("path null");
        // }
        // System.out.println("My final goal is: " + vecGoal);

        savedpos = getState().getPosition();
        Vector3f currentPos = getState().getPosition();
        Vector3f currentHeading = getState().getHeading();
        Vector3f nextPos = null;

        for(Collision collision : collisions)
        {
            if(collision.getCollidableType().equals(CollisionType.ENVOBJ))
            {
                // addCollidedObjectToKnowledge(collision);
                replan = true;
                 System.out.println("Colliding with Object");
            }
            else if(collision.getCollidableType().equals(CollisionType.AGENT))
            {
                // System.out.println("Colliding with agent");
            }
        }

        if(tempSubGoal == null || (knowledgeModule.getTime() - subGoalStartCycle) > MAX_SUBGOAL_CYC)
        {
            tempSubGoal = makeTemporarySubGoal(vecGoal);
        }

        if(!testifTempSubGoalStillNeeded(vecGoal))
        {
            tempSubGoal = null;
        }

        if(tempSubGoal == null)
        {
            nextPos = RVOPathFinding(vecGoal, currentPos, nextPos);
        }
        else
        {
            nextPos = RVOPathFinding(tempSubGoal, currentPos, nextPos);
        }

        // System.out.println("NextPos: " + nextPos);
        lastMove = nextPos;

        applyPositionalChange(generatedPlan, currentPos, currentHeading, nextPos, vecGoal);

        if(unableToFindPath)
        {
            int sign = 1;
            Vector3f nextHeading = new Vector3f(currentHeading.x * FastMath.cos(maxAngleTurn * sign) + currentHeading.z * FastMath.sin(maxAngleTurn * sign), 0, -currentHeading.x * FastMath.sin(maxAngleTurn * sign) + currentHeading.z
                    * FastMath.cos(maxAngleTurn * sign));

            addFaceTask(generatedPlan, nextHeading);
        }

        knowledgeModule.cleanNearbyAgents();
    }

    private boolean testifTempSubGoalStillNeeded(Vector3f vecGoal)
    {
        List<RVOObstacle> perceivedRVOObstacles = knowledgeModule.getPerceivedRVOObstacles();
        if(perceivedRVOObstacles.size() > 0)
        {
            RVOObstacle rvoObstacle = perceivedRVOObstacles.get(0);
            EnvObjectKnowledgeStorageObject obsj = knowledgeModule.findEnvObj(rvoObstacle.getID());
            if(obsj.agentPathIntersectsObj2D(getState().getPosition(), vecGoal, getState().getRadius()))
            {
                return true;
            }
        }
        return false;
    }

    private Vector3f makeTemporarySubGoal(Vector3f vecGoal)
    {
        // Vector3f newVec = null;
        // Vector3f retVec = null;
        // Vector3f vecTowardGoal = vecGoal.subtract(getState().getPosition());
        // int testAngle = 3;
        // if(vecTowardGoal.angleBetween(Vector3f.UNIT_X) < testAngle *
        // FastMath.DEG_TO_RAD ||
        // vecTowardGoal.angleBetween(Vector3f.UNIT_X.mult(-1)) < testAngle *
        // FastMath.DEG_TO_RAD)
        // {
        // Vector3f vecToObj = angleTestObj(vecGoal);
        // if(vecToObj != null)
        // {
        // // newVec = vecToObj;
        // newVec = new Vector3f(0, 0, vecToObj.z);
        // newVec.normalizeLocal();
        // }
        // }
        //
        // if(vecTowardGoal.angleBetween(Vector3f.UNIT_Z) < testAngle *
        // FastMath.DEG_TO_RAD ||
        // vecTowardGoal.angleBetween(Vector3f.UNIT_Z.mult(-1)) < testAngle *
        // FastMath.DEG_TO_RAD)
        // {
        // Vector3f vecToObj = angleTestObj(vecGoal);
        // if(vecToObj != null)
        // {
        // // newVec = vecToObj;
        // newVec = new Vector3f(vecToObj.x, 0, 0);
        // newVec.normalizeLocal();
        // }
        // }
        // if(newVec != null)
        // {
        // vecTowardGoal.normalizeLocal();
        // vecTowardGoal.addLocal(newVec);
        // vecTowardGoal.normalizeLocal();
        // retVec =
        // getState().getPosition().add(vecTowardGoal).mult(knowledgeModule.desiredSpeed);
        // }
        Vector3f retVec = null;

        List<RVOObstacle> perceivedRVOObstacles = knowledgeModule.getPerceivedRVOObstacles();
        if(perceivedRVOObstacles.size() > 0)
        {
            RVOObstacle rvoObstacle = perceivedRVOObstacles.get(0);
            EnvObjectKnowledgeStorageObject obsj = knowledgeModule.findEnvObj(rvoObstacle.getID());
            if(obsj.agentPathIntersectsObj2D(getState().getPosition(), vecGoal, getState().getRadius()))
            {
                List<Vector3f> points = new ArrayList<Vector3f>();
                List<Vector3f> safePoints = new ArrayList<Vector3f>();
                points.add(obsj.getPosition().add(obsj.getScale().getX() + space, 0, obsj.getScale().getZ() + space));
                points.add(obsj.getPosition().add(-1 * (obsj.getScale().getX() + space), 0, obsj.getScale().getZ() + space));
                points.add(obsj.getPosition().add(obsj.getScale().getX() + space, 0, -1 * (obsj.getScale().getZ() + space)));
                points.add(obsj.getPosition().add(-1 * (obsj.getScale().getX() + space), 0, -1 * (obsj.getScale().getZ() + space)));

                for(Vector3f point : points)
                {
                    if(!obsj.agentPathIntersectsObj2D(getState().getPosition(), point, getState().getRadius()))
                    {
                        safePoints.add(point);
                    }
                }

                float closestDist = Float.MAX_VALUE;
                Vector3f closestVec = null;
                for(Vector3f point : safePoints)
                {
                    float distance = point.distance(vecGoal);
                    if(distance < closestDist)
                    {
                        closestDist = distance;
                        closestVec = point;
                    }
                }

                if(closestVec != null)
                {
                    subGoalStartCycle = knowledgeModule.getTime();
                    retVec = closestVec;
                }
            }
        }

        return retVec;
    }

    protected boolean testObs(Vector3f vecGoal)
    {
        for(EnvObjectKnowledgeStorageObject obj : knowledgeModule.getEnvObjects())
        {
            if(obj.isCollidable() && !obj.getType().equals(DOOR))
            {
                if(obj.agentPathIntersectsObj2D(getState().getPosition(), vecGoal, getState().getRadius()))
                {
                    return true;
                }
            }
        }
        return false;
    }

    protected boolean testHeading(Vector3f vecGoal)
    {
        for(EnvObjectKnowledgeStorageObject obj : knowledgeModule.getEnvObjects())
        {
            if(obj.isCollidable() && !obj.getType().equals(WALL) && !obj.getType().equals(DOOR))
            {
                if(obj.agentPathIntersectsObj2D(getState().getPosition(), vecGoal, getState().getRadius()))
                {
                    return true;
                }
            }
        }
        return false;
    }

    protected boolean testGoal(Vector3f vecGoal)
    {
        for(EnvObjectKnowledgeStorageObject obj : knowledgeModule.getEnvObjects())
        {
            if(obj.isCollidable())
            {
                if(obj.intersects2D(vecGoal, getState().getScale()))
                {
                    return true;
                }
            }
        }
        return false;
    }

    protected void applyPositionalChange(Plan generatedPlan, Vector3f currentPos, Vector3f currentHeading, Vector3f nextPos, Vector3f vecGoal)
    {

        if((nextPos != null) && nextPos.distance(currentPos) > .01)
        {

            if(heading != HeadingType.PUSHED)
            {
                // logger.debug("I WANT TO MOVE! "+ getState().getID());
                knowledgeModule.setSavedPos(knowledgeModule.getSelf().getPosition());

                // set heading to match new path.
                heading = HeadingType.GOING;
                heading = setNewHeading(generatedPlan, currentPos, currentHeading, nextPos);

                if(heading == HeadingType.GOING)
                {
                    MoveTask moveTask = (MoveTask) taskModule.createTask(POSITION, knowledgeModule.getTime());
                    moveTask.setPosition(new Vector3f(nextPos));
                    generatedPlan.addTask(moveTask);
                    knowledgeModule.setWantToMove(true);
                }
            }
            else if(heading == HeadingType.PUSHED)
            {
                MoveTask moveTask = (MoveTask) taskModule.createTask(POSITION, knowledgeModule.getTime());
                moveTask.setPosition(new Vector3f(nextPos));
                generatedPlan.addTask(moveTask);
                knowledgeModule.setWantToMove(true);
                knowledgeModule.getSelf().setPosture(Posture.Walk_back);

                setNewHeadingPushed(generatedPlan, currentPos, currentHeading, nextPos, vecGoal);
            }
        }
        else
        {
            knowledgeModule.setWantToMove(false);
            // knowledgeModule.getSelf().setPosture(Posture.STANDING);
            knowledgeModule.getSelf().setPosture(Posture.Idle1);

            IdleTask idleTask = (IdleTask) taskModule.createTask(IDLE, knowledgeModule.getTime());
            generatedPlan.addTask(idleTask);
        }

        if((nextPos != null) && nextPos.distance(currentPos) < .0f)
        {
            Random r = new Random();
            if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.GOING_DOOR))
            {
                float dist = getState().getPosition().distance(knowledgeModule.findEnvObj(knowledgeModule.getGoalDoor().getId()).getPosition());
                if(dist < 8 && dist > 2)
                {
                    if(!knowledgeModule.isWaiting())
                    {
                        knowledgeModule.setWaiting(true);
                        knowledgeModule.setWaitingTime(0);
                    }
                }
            }
        }
        else if(nextPos != null)
        {
            knowledgeModule.setWaiting(false);
        }
//
//         MoveTask moveTask = (MoveTask) taskModule.createTask(POSITION, knowledgeModule.getTime());
//         moveTask.setPosition(new Vector3f(currentPos.add(1, 1, 1)));
//         generatedPlan.addTask(moveTask);
        
    }

    protected Vector3f RVOPathFinding(Vector3f vecGoal, Vector3f currentPos, Vector3f nextPos)
    {
        if(heading == HeadingType.PUSHED)
        {
            heading = HeadingType.GOING;
        }

        Vector3f expectedHeading = vecGoal.subtract(currentPos).normalize();

        Vector3f RVOHeading = null;
        if(heading != HeadingType.TURNING)
        {
            float distanceToDestination = vecGoal.subtract(currentPos).length();
            if(getState().getDesiredSpeed() >= distanceToDestination)
            {
                // move to my destination
                if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.GOING_DOOR) && tempSubGoal == null && knowledgeModule.isSirenHeard() && knowledgeModule.getGoalDoor().isExit())
                {
                    knowledgeModule.setCurrentMentalState(MentalStateOfAgent.ARRIVED_AT_EXIT);
                }
                else if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.GOING_DOOR) && tempSubGoal == null)
                {
                    knowledgeModule.setCurrentMentalState(MentalStateOfAgent.ARRIVED_IN_ROOM);
                }
                else if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.EXITING_BUILDING) && tempSubGoal == null)
                {
                    knowledgeModule.setCurrentMentalState(MentalStateOfAgent.EXITED_BUILDING);
                }
                else if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.GOING_TO_SAFETY) && tempSubGoal == null)
                {
                    knowledgeModule.setCurrentMentalState(MentalStateOfAgent.SAFE);
                }
                // else if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.FLEEING) && tempSubGoal == null)
                // {
                // knowledgeModule.setCurrentMentalState(knowledgeModule.getPreviousMentalState());
                // }
                else if(tempSubGoal == null)
                {
                    knowledgeModule.setCurrentMentalState(MentalStateOfAgent.ARRIVED);
                }
                else
                {
                    // arrived at subgoal
                }
                RVOHeading = RVO.getGoodHeading(new Vector2f(vecGoal.x, vecGoal.z));
                nextPos = getState().getPosition().add(RVOHeading);
                tempSubGoal = null;
            }
            else
            {

                // if(knowledgeModule.isWaiting())
                // {
                // vecGoal = getState().getPosition();
                // }

                // System.out.println("**********************************************START RVO PATH FINDING HERE**********************************************");
                RVOHeading = RVO.getGoodHeading(new Vector2f(vecGoal.x, vecGoal.z));

                // System.out.println("**********************************************END   RVO PATH FINDING HERE**********************************************");

                float angleBetween = expectedHeading.angleBetween(RVOHeading);
                if(angleBetween > FastMath.PI / 2)
                {

                    heading = HeadingType.PUSHED;
                    // nextPos = getState().getPosition().add(RVOHeading.mult(.3f / RVOHeading.length()));
                    // nextPos = getState().getPosition().add(RVOHeading.mult(.3f));
                    nextPos = getState().getPosition().add(RVOHeading);
                    // nextPos = getState().getPosition();
                    // System.out.println("####@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@AGEMT{ISJJEDE{ISJPUSHED");
                    // knowledgeModule.setWaiting(true);
                    // knowledgeModule.setWaitingTime(0);
                }
                else
                {
                    nextPos = getState().getPosition().add(RVOHeading);
                }

            }
        }
        else
        {
            nextPos = lastMove;
        }

        return nextPos;
    }

    protected Vector3f oldPathFinding(Vector3f vecGoal, Vector3f currentPos, Vector3f nextPos, Vector3f goalPoint, Vector3f finalPos)
    {
        List<PathFindingNode> newNodes = calculateNewTravelNodes(knowledgeModule.getNewEnvObjs());

        removeBadNodes(newNodes);
        for(PathFindingNode pathFindingNode : newNodes)
        {
            nodeStore.put(pathFindingNode.getNodeID(), pathFindingNode);
        }
        updateNeighbors(newNodes);

        // System.out.println("Time to update neighbors: " +
        // (System.currentTimeMillis() - before));

        if(agentPath != null)
        {
            if(agentPath.size() > 0)
            {
                goalPoint = agentPath.get(0).getPoint();
            }
        }

        if(agentPath == null || (agentPath.size() == 0) || replan == true || testObstacled(goalPoint))
        {
            unableToFindPath = false;
            deepPathFinder(vecGoal);
            replan = false;
        }

        if(agentPath != null)
        {
            if(agentPath.size() > 0)
            {
                goalPoint = agentPath.get(0).getPoint();
            }
        }

        knowledgeModule.getSelf().setPlanningDetails(knowledgeModule.getSelf().getPlanningDetails().concat(", GoalPoint: " + goalPoint));
        knowledgeModule.getSelf().setPlanningDetails(knowledgeModule.getSelf().getPlanningDetails().concat(", EnvObjID: " + agentPath.get(0).getParentEnvObjID()));
        if(goalPoint != null)
        {

            Vector3f directionVector = goalPoint.subtract(currentPos);

            // calculate the distance to the destination
            float distanceToDestination = directionVector.length();

            // calculate the unit vector of the edge
            // currently setting agent head position to where it's going

            if(distanceToDestination == 0)
            {
                // I have arrived at a destination
                agentPath.remove(0);
                if(agentPath.size() == 0)
                {
                	System.out.println("reached");
                    knowledgeModule.setCurrentMentalState(MentalStateOfAgent.IDLE);
                }
                else
                {
                    goalPoint = agentPath.get(0).getPoint();
                    directionVector = goalPoint.subtract(currentPos);
                    distanceToDestination = directionVector.length();
                }
            }
            else if((getState().getDesiredSpeed() >= distanceToDestination))
            {
                // move to my destination
                nextPos = goalPoint;
            }
            else
            {
                Vector3f unitVector = directionVector.clone();
                unitVector.normalizeLocal();
                // unitVector.addLocal(avoidAgents());
                // unitVector.normalizeLocal();
                nextPos = getState().getPosition().add(unitVector.mult(getState().getDesiredSpeed()));
                if(checkMoveAgainstWalls(nextPos))
                {
                    // System.out.println("Agent " + getState().getID() +
                    // "  Getting in here");
                    nextPos = getState().getPosition().add(directionVector.normalize().mult(getState().getDesiredSpeed()));
                }
            }

        }

        if(finalPos != null)
        {
            nextPos = finalPos;
        }

        // don't move thru walls!!! -_-
        // nextPos = checkMoveAgainstWalls(nextPos, vecGoal);

        if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.FLEEING) && nextPos.subtract(currentPos).dot(knowledgeModule.getThreatDirection()) > 0)
        {
            Vector3f directionVector = vecGoal.subtract(currentPos);
            directionVector.normalizeLocal();
            Vector3f u_Vnew = directionVector.clone();
            for(VisionModel vis : knowledgeModule.getVisibleList())
            {
                if(vis.getType().equals("envObject"))
                {
                    EnvObjectKnowledgeStorageObject envObj = knowledgeModule.findEnvObj(vis.getId());
                    if(envObj.getType().equals("box"))
                    {

                        Vector3f d_V = envObj.getPosition().subtract(getState().getPosition());

                        float dist = FastMath.sqrt(FastMath.pow(envObj.getScale().getX(), 2) + FastMath.pow(envObj.getScale().getZ(), 2)) + getState().getDesiredSpeed() + 5;
                        if(d_V.length() <= dist)
                        {

                            Vector3f temp = d_V.cross(directionVector);
                            u_Vnew.addLocal((temp.cross(d_V)));

                            if((u_Vnew.x == 0) && (u_Vnew.y == 0) && (u_Vnew.z == 0))
                            {
                                u_Vnew = directionVector.cross(new Vector3f(0, 1, 0));
                            }
                        }
                    }
                }
            }
            u_Vnew.setY(0);
            u_Vnew.normalizeLocal();
            // u_Vnew = directionVector;
            nextPos = getState().getPosition().add(u_Vnew.mult(getState().getDesiredSpeed()));
        }
        return nextPos;
    }

    protected void addFaceTask(Plan generatedPlan, Vector3f nextHeading)
    {
        FaceTask faceTask = (FaceTask) taskModule.createTask(HEADING, knowledgeModule.getTime());
        faceTask.setHeading(new Vector3f(nextHeading));

        generatedPlan.addTask(faceTask);
    }

    protected HeadingType setNewHeading(Plan generatedPlan, Vector3f currentPos, Vector3f currentHeading, Vector3f nextPos)
    {

        HeadingType result = HeadingType.GOING;
        Vector3f nextHeading = (nextPos.subtract(currentPos)).normalizeLocal();

        if((nextHeading.getX() == 0) && (nextHeading.getY() == 0) && (nextHeading.getZ() == 0))
        {
            nextHeading = currentHeading; // must look at SOMEthing.
        }

        float angleBetween = nextHeading.angleBetween(currentHeading);
        Vector3f cp = currentHeading.cross(nextHeading);
        if((angleBetween > maxAngleTurn))
        {
            result = HeadingType.TURNING;
            float sign = FastMath.sign(cp.y);
            if(sign == 0)
            {
                sign = 1;
            }
            nextHeading.set(currentHeading.x * FastMath.cos(maxAngleTurn * sign) + currentHeading.z * FastMath.sin(maxAngleTurn * sign), 0, -currentHeading.x * FastMath.sin(maxAngleTurn * sign) + currentHeading.z * FastMath.cos(maxAngleTurn * sign));
        }
        addFaceTask(generatedPlan, nextHeading);

        return result;
    }

    protected void setNewHeadingPushed(Plan generatedPlan, Vector3f currentPos, Vector3f currentHeading, Vector3f nextPos, Vector3f vecGoal)
    {
        Vector3f nextHeading;
        if(tempSubGoal == null)
        {
            nextHeading = (vecGoal.subtract(currentPos)).normalizeLocal();
        }
        else
        {
            nextHeading = (tempSubGoal.subtract(currentPos)).normalizeLocal();
        }
        if((nextHeading.getX() == 0) && (nextHeading.getY() == 0) && (nextHeading.getZ() == 0))
        {
            nextHeading = currentHeading; // must look at SOMEthing.
        }

        float angleBetween = nextHeading.angleBetween(currentHeading);
        Vector3f cp = currentHeading.cross(nextHeading);
        float pushMaxTurn = maxAngleTurn / 8;
        if((angleBetween > pushMaxTurn))
        {
            float sign = FastMath.sign(cp.y);
            if(sign == 0)
            {
                sign = 1;
            }
            nextHeading.set(currentHeading.x * FastMath.cos(pushMaxTurn * sign) + currentHeading.z * FastMath.sin(pushMaxTurn * sign), 0, -currentHeading.x * FastMath.sin(pushMaxTurn * sign) + currentHeading.z * FastMath.cos(pushMaxTurn * sign));
        }
        addFaceTask(generatedPlan, nextHeading);

    }

    private void addCollidedObjectToKnowledge(Collision collision)
    {
        EnvObjectState obj = new EnvObjectState();
        obj.setType("box");
        obj.setDescription("box");
        obj.setScale(new Vector3f(1, 1, 1));
        obj.setPosition(lastMove);
        obj.setID(10000 + collision.getCollidableId());
        knowledgeModule.addEnvObj(obj);
    }

    private Vector3f avoidAgents()
    {
        Vector3f totalForce = new Vector3f(0, 0, 0);
        knowledgeModule.getSelf().setPlanningDetails(knowledgeModule.getSelf().getPlanningDetails().concat(", Avoiding: "));
        for(AgentKnowledgeStorageObject agent : knowledgeModule.getNearbyAgents().values())
        {
            knowledgeModule.getSelf().setPlanningDetails(knowledgeModule.getSelf().getPlanningDetails().concat(agent.getID() + ", "));
            Vector3f d = agent.getPosition().subtract(getState().getPosition());

            Vector3f tForce = d.cross(getState().getVelocity()).cross(d);
            if(d.dot(getState().getVelocity()) < .001)
            {
                tForce.set(-getState().getHeading().z, 0, getState().getHeading().x);
            }
            tForce.normalizeLocal();

            float distanceWeight = (d.length() - knowledgeModule.getNearDistance()) * (d.length() - knowledgeModule.getNearDistance());
            float opposingWeight;
            if(getState().getVelocity().dot(agent.getVelocity()) > 0)
            {
                opposingWeight = 1.2f;
            }
            else
            {
                opposingWeight = 2.4f;
            }
            tForce.multLocal(distanceWeight);
            tForce.multLocal(opposingWeight);

            totalForce.addLocal(tForce);
        }

        // totalForce.normalizeLocal();
        float forceMultiplier = .01f;
        return totalForce.mult(forceMultiplier);
    }

    /**
     * Make sure the agent is not moving from one side of a wall to another.
     * 
     * @param nextPos
     * @return
     */
    private boolean checkMoveAgainstWalls(Vector3f nextPos)
    {
        List<VisionModel> visibleList = knowledgeModule.getVisibleList();
        for(int val = 0; val < visibleList.size(); val++)
        {
            if(visibleList.get(val).getType().compareTo(ENV_OBJECT) == 0)
            {
                EnvObjectKnowledgeStorageObject seenObj = knowledgeModule.findEnvObj(visibleList.get(val).getId());
                if(seenObj != null)
                {
                    if(seenObj.isCollidable())
                    {
                        if(seenObj.intersects2D(nextPos, getState().getScale()))
                        {
                            return true;
                        }
                    }
                }
            }

        }
        return false;
    }

    /**
     * The new DEEP path finder obstacle avoidance algorithm.
     * 
     * @param currentPositionalGoal
     *        The Current goal location.
     */
    private void deepPathFinder(Vector3f goal)
    {

        List<PathFindingNode> path = findGoodSearchPath(goal, knowledgeModule.getSelf().getPosition());

        if(path != null)
        {
            // grr
        }
        else
        {
            logger.debug("PATH IS NULL");
            logger.debug("My goal is: " + goal);
        }

        agentPath = path;

    }

    public void printPath(List<PathFindingNode> path)
    {
        int count = 0;
        System.out.println("My PATH is: ");
        for(PathFindingNode node : path)
        {
            System.out.println(count + ": " + node.getPoint());
            count++;
        }
    }

    public void printDoorPath(List<Thing> list)
    {
        int count = 0;
        if(list != null)
        {
            System.out.println("My PATH is: ");
            for(Thing node : list)
            {
                if(node instanceof Door)
                {
                    System.out.println(count + ": " + ((Door) node).getId());
                    count++;

                }
            }
        }
    }

    private List<PathFindingNode> findGoodSearchPath(Vector3f goal, Vector3f start)
    {
        cleanUpStoredNodes();

        ArrayList<Integer> closedNodeIDS = new ArrayList<>();
        PathFindingNode startNode = new PathFindingNode(getState().getAgentSize(), start, -2);
        PathFindingNode goalNode = new PathFindingNode(getState().getAgentSize(), goal, -3);

        List<Integer> openNodeIDs = prepareNodes(goal, start, startNode, goalNode);

        NodeSorter sorter = new NodeSorter(nodeStore, startNode, goalNode);
        boolean needsort = false;

        while(openNodeIDs.size() > 0)
        {
            PathFindingNode currentNode;

            if(needsort)
            {
                sorter.sort(openNodeIDs);
                needsort = false;
            }

            int currentNodeID = openNodeIDs.get(0);
            if(currentNodeID == startNode.getNodeID())
            {
                currentNode = startNode;
            }
            else if(currentNodeID == goalNode.getNodeID())
            {
                currentNode = goalNode;
                return makepath(currentNode);
            }
            else
            {
                currentNode = nodeStore.get(currentNodeID);
            }
            openNodeIDs.remove(0);
            currentNode.setClosed(true);
            closedNodeIDS.add(currentNode.getNodeID());
            for(int neighborID : currentNode.getNeighbors())
            {
                PathFindingNode neighbor = nodeStore.get(neighborID);
                if(neighbor != null)
                {
                    needsort = processNode(goal, neighbor, openNodeIDs, needsort, currentNode);
                }
            }
            if(currentNode.isConnectedToGoal())
            {
                needsort = processNode(goal, goalNode, openNodeIDs, needsort, currentNode);
            }

            // FIXME: temporary bug fix. The number of loops is exploding!
            // if(loops > 10)
            // {
            // break;
            // }
        }
        // if(loops > 20)
        // System.out.println("fail: "+loops);
        Random r = new Random();
        // return makepath(maxdepthnode); // if can't find any path to goal,
        // return a long path.
        int randomint = r.nextInt(closedNodeIDS.size());

        if(closedNodeIDS.get(randomint) == startNode.getNodeID())
        {
            // System.out.println("AgentID: " + getState().getID());
            // System.out.println("returning path to self? closed nodes size: "
            // + closedNodeIDS.size());
            // System.out.println("random was: " + randomint);
            // System.out.println("start node # of neighbors was: " +
            // startNode.getNeighbors().size());
            unableToFindPath = true;
            return makepath(startNode);
        }
        else if(nodeStore.get(closedNodeIDS.get(randomint)) != null)
        {
            return makepath(nodeStore.get(closedNodeIDS.get(randomint)));
        }
        else
            return null;
    }

    private boolean processNode(Vector3f goal, PathFindingNode node, List<Integer> openNodeIDs, boolean needsort, PathFindingNode currentNode)
    {
        if(!node.isClosed())
        {
            float tempGScore = currentNode.getGscore() + currentNode.getPoint().distance(node.getPoint());
            if(node.isNewNode() || (tempGScore < node.getGscore()))
            {
                if(node.isNewNode())
                {
                    openNodeIDs.add(node.getNodeID());
                    needsort = true;
                    node.setNewNode(false);
                }
                node.setCameFrom(currentNode);
                node.setDepth(currentNode.getDepth() + 1);
                node.setGscore(tempGScore);
                node.setFscore(node.getGscore() + estimateCost(node.getPoint(), goal));
            }
        }
        return needsort;
    }

    private List<Integer> prepareNodes(Vector3f goal, Vector3f start, PathFindingNode startNode, PathFindingNode goalNode)
    {
        startNode.calculateStartNeighbors(nodeStore.values(), knowledgeModule.getEnvObjects(), STARTGOALMAXDIST, goalNode);
        // startNode.calculateNeighborsGoal(goalNode,
        // knowledgeModule.getEnvObjects());

        testIfNodesCanSeeGoal(goal, start, goalNode);

        List<Integer> openNodeIDs = new ArrayList<Integer>();
        openNodeIDs.add(startNode.getNodeID());
        return openNodeIDs;
    }

    protected void testIfNodesCanSeeGoal(Vector3f goal, Vector3f start, PathFindingNode goalNode)
    {
        for(PathFindingNode node : nodeStore.values())
        {
            node.setFscore(estimateCost(start, goal));
            if(node.getPoint().distance(goal) < STARTGOALMAXDIST)
            {
                node.calculateNeighborsGoal(goalNode, knowledgeModule.getEnvObjects());
            }
        }
    }

    protected void cleanUpStoredNodes()
    {
        for(PathFindingNode pathFindingNode : nodeStore.values())
        {
            pathFindingNode.setDepth(0);
            pathFindingNode.setFscore(0);
            pathFindingNode.setGscore(0);
            pathFindingNode.setClosed(false);
            pathFindingNode.setNewNode(true);
            pathFindingNode.setConnectedToGoal(false);
        }
    }

    protected void cleanUpStoredDoors()
    {
        for(Door d : knowledgeModule.getDoors())
        {
            d.setDepth(0);
            d.setFscore(0);
            d.setGscore(0);
            d.setClosed(false);
            d.setNewNode(true);
        }
    }

    private List<PathFindingNode> makepath(PathFindingNode currentNode)
    {
        List<PathFindingNode> path = new ArrayList<PathFindingNode>();

        while(currentNode.getCameFrom() != null)
        {
            path.add(currentNode);
            currentNode = currentNode.getCameFrom();
        }
        if(path.size() == 0)
        {
            path.add(currentNode);
        }

        List<PathFindingNode> reversepath = new ArrayList<PathFindingNode>();

        for(int i = path.size() - 1; i >= 0; i--)
        {
            reversepath.add(path.get(i));
        }

        return reversepath;
    }

    private List<Thing> makepathDoor(Door currentNode)
    {
        List<Thing> path = new ArrayList<Thing>();

        while(currentNode.getCameFrom() != null)
        {
            path.add(currentNode);
            currentNode = currentNode.getCameFrom();
        }
        if(path.size() == 0)
        {
            path.add(currentNode);
        }

        List<Thing> reversepath = new ArrayList<Thing>();

        for(int i = path.size() - 1; i >= 0; i--)
        {
            reversepath.add(path.get(i));
        }

        return reversepath;
    }

    public List<Thing> findPathToDoor(Door goal, Vector3f start)
    {
        cleanUpStoredDoors();

        EnvObjectKnowledgeStorageObject goalObj;
        if(goal.isExit())
        {
            goalObj = knowledgeModule.getExitDoor(goal.getId());
        }
        else
        {
            goalObj = knowledgeModule.findEnvObj(goal.getId());
        }

        if(goalObj == null)
        {
            goalObj = knowledgeModule.getImportantObject(goal.getId());
        }

        // for(Door d : knowledgeModule.getDoors())
        // {
        // d.setFscore(estimateCost(start, goalObj.getPosition()));
        // }
        ArrayList<Integer> closedNodeIDS = new ArrayList<>();

        List<Integer> openNodeIDs = new ArrayList<Integer>();
        Door startNode = createStartNode();
        addStartNeighbors(startNode);
        startNode.setFscore(estimateCost(start, goalObj.getPosition()));
        openNodeIDs.add(startNode.getId());

        DoorSorter sorter = new DoorSorter(knowledgeModule.getDoorsMap(), goal);
        boolean needsort = false;

        while(openNodeIDs.size() > 0)
        {
            Door currentNode;

            if(needsort)
            {
                sorter.sort(openNodeIDs);
                needsort = false;
            }

            // System.out.println("Fscores:");
            // for(int i : openNodeIDs)
            // {
            // Door d = knowledgeModule.getDoor(i);
            // if(d != null)
            // System.out.println("NodeId: " + d.getId() + ", Fscore: " + d.getFscore());
            // else
            // System.out.println("door is null");
            //
            // }

            int currentNodeID = openNodeIDs.get(0);
            if(currentNodeID == goal.getId())
            {
                currentNode = goal;
                return makepathDoor(currentNode);
            }
            else if(currentNodeID == startNode.getId())
            {
                currentNode = startNode;
            }
            else
            {
                currentNode = knowledgeModule.getDoor(currentNodeID);
            }
            openNodeIDs.remove(0);
            currentNode.setClosed(true);
            closedNodeIDS.add(currentNode.getId());
            for(int neighborID : currentNode.getNeighbors())
            {
                Door neighbor = knowledgeModule.getDoor(neighborID);
                if(neighbor != null)
                {
                    needsort = processDoor(goal, neighbor, openNodeIDs, needsort, currentNode);
                }
            }
        }
        return null;
    }

    protected void addStartNeighbors(Door startNode)
    {
        for(Door d : knowledgeModule.getDoors())
        {
            EnvObjectKnowledgeStorageObject dObj = knowledgeModule.findEnvObj(d.getId());
            if(dObj != null)
            {
                if(getState().getPosition().distance(dObj.getPosition()) < 180)
                {
                    boolean intersects = false;
                    for(EnvObjectKnowledgeStorageObject testObj : knowledgeModule.getEnvObjects())
                    {
                        if(testObj.getID() != dObj.getID())
                        {
                            if(testObj.getType().equals(WALL) || testObj.getType().equals(DOOR))
                            {
                                if(testObj.miniBAIntersects2DLine(getState().getPosition(), dObj.getPosition()))
                                {
                                    intersects = true;
                                    break;
                                }
                            }
                        }
                    }
                    if(!intersects)
                    {
                        startNode.addNeighbor(d.getId());
                    }

                }
            }
        }

    }

    private boolean processDoor(Door goal, Door node, List<Integer> openNodeIDs, boolean needsort, Door currentNode)
    {
        if(!node.isClosed())
        {
            VirtualKnowledgeStorageObject currentNodeObj;
            if(currentNode.getId() == -1)
            {
                currentNodeObj = new VirtualKnowledgeStorageObject(-1, getState().getPosition());
            }
            else
            {
                currentNodeObj = knowledgeModule.findEnvObj(currentNode.getId());
            }

            EnvObjectKnowledgeStorageObject nodeObj;
            if(node.isExit())
            {
                nodeObj = knowledgeModule.getExitDoor(node.getId());
            }
            else
            {
                nodeObj = knowledgeModule.findEnvObj(node.getId());
            }

            float tempGScore = currentNode.getGscore() + estimateCost(currentNodeObj, nodeObj);
            tempGScore = addCongestionFactor(node, currentNode, tempGScore);

            if(node.getDoorCongestion() > 8 && node.getDoorflow() < 0 && !node.isExit())
            {
                return false;
            }

            if(node.isNewNode() || (tempGScore < node.getGscore()))
            {
                if(node.isNewNode())
                {
                    openNodeIDs.add(node.getId());
                    node.setNewNode(false);
                }
                needsort = true;
                node.setCameFrom(currentNode);
                node.setDepth(currentNode.getDepth() + 1);
                node.setGscore(tempGScore);
                EnvObjectKnowledgeStorageObject goalObj;
                if(goal.isExit())
                {
                    goalObj = knowledgeModule.getExitDoor(goal.getId());
                }
                else
                {
                    goalObj = knowledgeModule.findEnvObj(goal.getId());
                }

                node.setFscore(node.getGscore() + estimateCost(nodeObj, goalObj));
                // addCongestionFactorF(goal, node);

            }
        }
        return needsort;
    }

    protected void addCongestionFactorF(Door goal, Door node)
    {
        if(node.getDoorCongestion() > 4)
        {
            node.setFscore(node.getFscore() + 25 * node.getDoorCongestion());
        }
        if(goal.getDoorCongestion() > 4)
        {
            node.setFscore(node.getFscore() + 25 * goal.getDoorCongestion());
        }

        if(node.getDoorCongestion() > 10)
        {
            node.setFscore(node.getFscore() + 50 * node.getDoorCongestion());
        }
        if(goal.getDoorCongestion() > 10)
        {
            node.setFscore(node.getFscore() + 50 * goal.getDoorCongestion());
        }
    }

    protected float addCongestionFactor(Door node, Door currentNode, float tempGScore)
    {

        if(node.getDoorCongestion() > 4)
        {
            tempGScore = tempGScore + 25 * node.getDoorCongestion();
        }

        if(node.getDoorCongestion() > 10)
        {
            tempGScore = tempGScore + 50 * node.getDoorCongestion();
        }
        return tempGScore;
    }

    protected Door createStartNode()
    {
        Door startNode = new Door(-1);
        startNode.setDepth(0);
        startNode.setFscore(0);
        startNode.setGscore(0);
        startNode.setClosed(false);
        startNode.setNewNode(true);
        return startNode;
    }

    private float estimateCost(Vector3f start, Vector3f goal)
    {
        return start.distance(goal);
    }

    private float estimateCost(VirtualKnowledgeStorageObject start, VirtualKnowledgeStorageObject goal)
    {
        if(start != null && goal != null)
        {
            return start.getPosition().distance(goal.getPosition());
        }
        return 2000;
    }

    // @SuppressWarnings("unused")
    // private List<EnvObjectKnowledgeStorageObject> calculateAvoidList(Vector3f
    // goal)
    // {
    // List<EnvObjectKnowledgeStorageObject> avoidList = new
    // ArrayList<EnvObjectKnowledgeStorageObject>();
    //
    // for(EnvObjectKnowledgeStorageObject obj :
    // knowledgeModule.getEnvObjects())
    // {
    // if(obj.isCollidable())
    // {
    // boolean inWay = obj.intersects(knowledgeModule.getSelf().getPosition(),
    // goal);
    //
    // if(!inWay)
    // {
    // inWay = obj.isInBetween(knowledgeModule.getSelf().getPosition(), goal,
    // space);
    // }
    //
    // if(inWay)
    // {
    // avoidList.add(obj);
    // }
    // }
    // }
    // return avoidList;
    // }

    private List<PathFindingNode> calculateNewTravelNodes(List<EnvObjectKnowledgeStorageObject> newObjsList)
    {
        List<PathFindingNode> nodes = new ArrayList<PathFindingNode>();

        for(EnvObjectKnowledgeStorageObject obj : newObjsList)
        {
            if(!obj.getType().equals(WALL))
            {
                if(obj.getType().equals(DOOR))
                {
                    nodes.addAll(obj.calcNodesForDoor(space, getState().getAgentSize()));
                }
                else if(obj.getType().equals(FLOOR))
                {
                    nodes.addAll(obj.calcNodesNoCenter(-3, getState().getAgentSize()));
                }
                else
                {
                    nodes.addAll(obj.calcNodesNoCenter(space, getState().getAgentSize()));
                }
            }
        }

        return nodes;

    }

    @Override
    public void notifyReact()
    {
        replan = true;
    }

    private void updateNeighbors(List<PathFindingNode> newNodes)
    {
        long endTime = System.currentTimeMillis() + MAXUPDATEPERCYCLE;

        nodesToProcess.addAll(newNodes);

        // process new nodes
        updateNewNodes(newNodes);
        // update for new envobjs
        updateNewEnvObjs();

        // System.out.println("Nodes to proess SIZEEEEE " +
        // nodesToProcess.size());
        if(newNodeSave != null)
        {
            newNodeSave.calculateNeighbors(nodeStore.values(), knowledgeModule.getEnvObjects(), endTime, STARTGOALMAXDIST);
        }

        PathFindingNode newNode = null;
        while(System.currentTimeMillis() <= endTime && (newNode = nodesToProcess.poll()) != null)
        {
            newNode.calculateNeighbors(nodeStore.values(), knowledgeModule.getEnvObjects(), endTime, STARTGOALMAXDIST);
        }
        if(System.currentTimeMillis() >= endTime)
        {
            newNodeSave = newNode;
        }
        else
        {
            newNodeSave = null;
        }

    }

    protected void updateNewNodes(List<PathFindingNode> newNodes)
    {
        for(PathFindingNode newNode : newNodes)
        {
            for(PathFindingNode storedNode : nodeStore.values())
            {
                if(newNode.getPoint().distance(storedNode.getPoint()) < STARTGOALMAXDIST)
                {
                    if(!nodesToProcess.contains(storedNode))
                    {
                        nodesToProcess.add(storedNode);
                        storedNode.clearNeighbors();
                    }
                }
            }
        }
    }

    protected void updateNewEnvObjs()
    {
        for(EnvObjectKnowledgeStorageObject obj : knowledgeModule.getNewEnvObjs())
        {
            for(PathFindingNode storedNode : nodeStore.values())
            {
                if(obj.getPosition().distance(storedNode.getPoint()) < STARTGOALMAXDIST)
                {
                    if(!nodesToProcess.contains(storedNode))
                    {
                        nodesToProcess.add(storedNode);
                        storedNode.clearNeighbors();
                    }
                }
            }
        }
    }

    private void removeBadNodes(List<PathFindingNode> newNodes)
    {
        List<PathFindingNode> toRemove = new ArrayList<PathFindingNode>();
        for(PathFindingNode pathFindingNode : newNodes)
        {
            for(EnvObjectKnowledgeStorageObject obj : knowledgeModule.getEnvObjects())
            {
                if(obj.isCollidable() && obj.intersects2D(pathFindingNode.getPoint(), getState().getScale()))
                {

                    toRemove.add(pathFindingNode);
                    break;
                }
            }
        }

        for(PathFindingNode pathFindingNode : toRemove)
        {
            newNodes.remove(pathFindingNode);
        }

    }

    private boolean testObstacled(Vector3f goalPoint)
    {
        for(EnvObjectKnowledgeStorageObject obj : knowledgeModule.getEnvObjects())
        {
            if(obj.isCollidable() && obj.agentPathIntersectsObj2D(getState().getPosition(), goalPoint, knowledgeModule.getSelf().getAgentSize()))
            {
                return true;
            }
        }
        return false;
    }

    private EHumanAgentState getState()
    {
        return knowledgeModule.getSelf();
    }

    public PathFindingNode getNodeByID(int id)
    {
        return nodeStore.get(id);
    }

    public void removeTempSubGoal()
    {
        tempSubGoal = null;
    }

    @Override
    public void notifyUserGoalChange()
    {
        tempSubGoal = null;
    }
}

To implement the “EvacuationHumanPlanExecutor” class:

• • Create a new class inside the “planning” package and name it “EvacuationHumanPlanExecutor”.

• Copy the code given below that describes the full implementation of a “EvacuationHumanPlanExecutor” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import java.io.Serializable;

import edu.utdallas.mavs.divas.core.sim.agent.planning.AbstractPlanExecutor;
import edu.utdallas.mavs.divas.core.sim.agent.planning.Plan;
import edu.utdallas.mavs.divas.core.sim.agent.task.Task;
import edu.utdallas.mavs.divas.core.sim.common.stimulus.Stimuli;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.EvacuationHumanKnowledgeModule;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.task.HumanTaskModule;
import edu.utdallas.mavs.evacuation.simulation.sim.common.stimulus.ChangeDesiredSpeedStimulus;
import edu.utdallas.mavs.evacuation.simulation.sim.common.stimulus.ChangePostureStimulus;

public class EvacuationHumanPlanExecutor extends AbstractPlanExecutor<EvacuationHumanKnowledgeModule, HumanTaskModule> implements Serializable
{
    private static final long   serialVersionUID = 1L;

    public EvacuationHumanPlanExecutor(EvacuationHumanKnowledgeModule knowledgeModule)
    {
        super(knowledgeModule);
    }

    @Override
    public Stimuli executePlan(Plan plan)
    {
        Stimuli sm = new Stimuli();

        //System.out.println(plan.getTasks().size());
        for(Task task : plan.getTasks())
        {
            //System.out.println(task);
            if(task.isEnabled())
            {
                sm.addAll(task.execute(knowledgeModule.getId()));
            }
        }

        sm.add(new ChangeDesiredSpeedStimulus(knowledgeModule.getId(), knowledgeModule.desiredSpeed));

        sm.add(new ChangePostureStimulus(knowledgeModule.getId(), knowledgeModule.getSelf().getPosture()));

        writeDebugInfo(plan);
        
        return sm;

    }

    /**
     * Creates debug info to be displayed on agent details on the GUI. To be removed.
     */
    private void writeDebugInfo(Plan plan)
    {
        if(plan != null)
        {
            // System.out.println("Plan: " + plan.toString());
            knowledgeModule.getSelf().setPlanningDetails("State: " + knowledgeModule.getCurrentMentalState() + ", Plan: " + plan.toString());
        }
    }
}

To implement the “EvacuationHumanPlanGenerator” class:

• • Create a new class inside the “planning” package and name it “EvacuationHumanPlanGenerator”.

• Copy the code given below that describes the full implementation of a “EvacuationHumanPlanGenerator” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;
import java.util.Random;

import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import com.jme3.math.FastMath;
import com.jme3.math.Vector3f;

import edu.utdallas.mavs.divas.core.sim.agent.interaction.communication.AgentMessage;
import edu.utdallas.mavs.divas.core.sim.agent.interaction.perception.data.CombinedReasonedData;
import edu.utdallas.mavs.divas.core.sim.agent.knowledge.internal.Goal;
import edu.utdallas.mavs.divas.core.sim.agent.knowledge.ontology.Thing;
import edu.utdallas.mavs.divas.core.sim.agent.planning.AbstractPlanGenerator;
import edu.utdallas.mavs.divas.core.sim.agent.planning.Plan;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.interaction.HumanInteractionModule;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.EvacuationHumanKnowledgeModule;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.EvacuationHumanKnowledgeModule.MentalStateOfAgent;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.external.AgentKnowledgeStorageObject;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.external.EnvObjectKnowledgeStorageObject;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.external.VisionModel;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.internal.LocationGoal;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.ontology.Door;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.task.FaceTask;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.task.HumanTaskModule;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.task.IdleTask;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.task.OpenDoorTask;
import edu.utdallas.mavs.evacuation.simulation.sim.common.state.EHumanAgentState;
import edu.utdallas.mavs.evacuation.simulation.sim.common.state.Posture;

public class EvacuationHumanPlanGenerator extends AbstractPlanGenerator<EvacuationHumanKnowledgeModule, HumanTaskModule>
{
    private static final long                  serialVersionUID = 1L;

    private final static Logger                logger           = LoggerFactory.getLogger(EvacuationHumanPlanGenerator.class);

    private static final String                WALL             = "wall";
    private static final String                FLOOR            = "floor";
    private static final String                DOOR             = "door";
    private static final String                GRILLING_FOOD    = "GrillingFood";
    private static final String                DRUMS            = "Drums";
    private static final String                FIREWORK         = "Firework";
    private static final String                ENV_OBJECT       = "envObject";
    private static final String                VECTOR           = "Vector";
    private static final String                HEADING          = "Heading";
    private static final String                POSITION         = "Position";
    private static final String                OPENDOOR         = "OpenDoor";
    private static final String                EXITDOOR         = "outsidedoor";
    private static final String                IDLE             = "Idle";

    private PlanEvaluator<EvacuationPlan>      planEvaluator;
    private PathFinding<EvacuationHumanKnowledgeModule> pathFinding;
    private HumanInteractionModule             interactionModule;

    private Vector3f                           currentVecGoal;

    /**
     * Create a evacuation human plan generator.
     * 
     * @param knowledgeModule
     * @param taskModule
     * @param humanInteractionModule
     * @param planStorageModule
     * @param pathFinding
     */
    public EvacuationHumanPlanGenerator(EvacuationHumanKnowledgeModule knowledgeModule, HumanTaskModule taskModule, HumanInteractionModule humanInteractionModule,

    EvacuationHumanPathFinding pathFinding)
    {
        super(knowledgeModule, taskModule);
        this.interactionModule = humanInteractionModule;
        this.pathFinding = pathFinding;
        this.planEvaluator = new PlanEvaluator<EvacuationPlan>(knowledgeModule);
    }

    @Override
    public Plan plan()
    {
        // System.out.println("EXIT DOR SIIIIIIIIIIIIIIIIIIIZ" + knowledgeModule.getExitDoors().size());
        // printState();

        // System.out.println(knowledgeModule.getDoors().size());
        // System.out.println(getState().getReachDistance());

        // System.out.println(knowledgeModule.getEnvObjects().size());

        EvacuationPlan generatedPlan = new EvacuationPlan();

        updateAgentKnowledge();

        respondToEvents();

        processActionGoals(generatedPlan);

        pathPlanning(generatedPlan);

        planMovement(generatedPlan);

        planEvaluator.evaluatePlan(generatedPlan);
        // planStorageModule.addPlan(generatedPlan);
        generatedPlan.setAgentPath(knowledgeModule.getAgentPath());
        knowledgeModule.getSelf().setAgentPath(makeVectorPath());
        return generatedPlan;
    }

    protected void pathPlanning(Plan generatedPlan)
    {
        if(!knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.SAFE))
        {
            if(!knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.GOING_TO_SAFETY))
            {
                reevaluateCurrentPlan();
            }
            if(!knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.PERSUING_USER_GOAL))
            {
                processNavigationGoals(); // don't process navigation if goal specified by user.
            }
        }
        else
        {

            knowledgeModule.setWaiting(true);
            knowledgeModule.getGoals().remove(knowledgeModule.getCurrentGoal());
            knowledgeModule.getSelf().setPosture(Posture.Idle1);

            IdleTask idleTask = (IdleTask) taskModule.createTask(IDLE, knowledgeModule.getTime());
            generatedPlan.addTask(idleTask);

        }

    }

    private void reevaluateCurrentPlan()
    {
        boolean stuck = checkIfStuck();

        // if(knowledgeModule.getAgentPath().getDistanceFactor() > 3)
        // {
        // createPath();
        // }

        if(!stuck)
        {
            if(havePath() && knowledgeModule.getTime() % 25 == 0)
            {
                Vector3f goalPos = null;
                Door finalStep = (Door) knowledgeModule.getAgentPath().getFinalStep();
                if(finalStep.getDoorCongestion() < 8 && knowledgeModule.getGoalDoor().getDoorCongestion() > 5)
                {
                    knowledgeModule.setCurrentMentalState(MentalStateOfAgent.IDLE);
                    knowledgeModule.getGoals().remove(knowledgeModule.getCurrentGoal());
                    goalPos = calculatePath(goalPos, finalStep);
                    testPossibleGoal(goalPos);
                }
            }

            // if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.GOING_DOOR) && havePath())
            // {
            // EnvObjectKnowledgeStorageObject goalDoor = knowledgeModule.findEnvObj(knowledgeModule.getGoalDoor().getId());
            // if(goalDoor != null)
            // {
            // checkCongestionDoor(knowledgeModule.getGoalDoor());
            // if(knowledgeModule.getGoalDoor().getDoorCongestion() > 4 && getState().getPosition().distance(goalDoor.getPosition()) > 2)
            // {
            // knowledgeModule.setStuckPos(getState().getPosition());
            // knowledgeModule.resetStuckCount();
            // knowledgeModule.resetTimeOnCurrentLegOfPath();
            // knowledgeModule.setCurrentMentalState(MentalStateOfAgent.NEED_NEW_PATH);
            // // knowledgeModule.setWaiting(true);
            // // knowledgeModule.setWaitingTime(0);
            // }
            //
            // }
            // }
        }
        
        if(stuck) {
        	knowledgeModule.setCurrentMentalState(MentalStateOfAgent.NEED_NEW_PATH);
        }

    }

    private void checkCongestionDoor(Door goalDoor)
    {
        int agentsNearDoor = 0;
        int doorNearnessThreshold = 8;
        List<Integer> ANDlist = new ArrayList<>();
        for(AgentKnowledgeStorageObject a : knowledgeModule.getAgents())
        {
            EnvObjectKnowledgeStorageObject goalDoorObj = knowledgeModule.findEnvObj(goalDoor.getId());
            if(goalDoorObj != null)
            {
                if(a.getPosition().distance(goalDoorObj.getPosition()) < doorNearnessThreshold)
                {
                    agentsNearDoor++;
                    ANDlist.add(a.getID());
                }
            }
        }

        if(agentsNearDoor > 0)
        {
            float sum = 0;
            Vector3f myPosVec = knowledgeModule.findEnvObj(goalDoor.getId()).getPosition().subtract(getState().getPosition()).normalize();
            for(Integer agentID : ANDlist)
            {
                // sum = sum + myPosVec.dot(knowledgeModule.findAgent(agentID).getVelocity().normalize());
                sum = sum + myPosVec.dot(knowledgeModule.findEnvObj(goalDoor.getId()).getPosition().subtract(knowledgeModule.findAgent(agentID).getPosition()).normalize());
            }
            goalDoor.setDoorflow(sum);
        }
        else
        {
            goalDoor.setDoorflow(0);
        }

        goalDoor.setDoorCongestion(agentsNearDoor);
        goalDoor.setCycleSet(knowledgeModule.getTime());
        // System.out.println("####################################################CONGS FOR" + goalDoor.getId() + " ==== " + agentsNearDoor);
    }

    private List<Vector3f> makeVectorPath()
    {
        List<Vector3f> ret = new ArrayList<>();
        if(knowledgeModule.getReactGoal() != null)
        {
            ret.add((Vector3f) knowledgeModule.getReactGoal().getValue());
        }
        if(knowledgeModule.getExplorePos() != null)
        {
            ret.add(knowledgeModule.getExplorePos());
        }
        if(knowledgeModule.getAgentPath().getPath() != null)
        {
            for(Thing a : knowledgeModule.getAgentPath().getPath())
            {
                if(a instanceof Door)
                {
                    EnvObjectKnowledgeStorageObject obj = knowledgeModule.findEnvObj(((Door) a).getId());

                    if(obj == null && ((Door) a).isExit())
                    {
                        obj = knowledgeModule.getExitDoor(((Door) a).getId());
                    }

                    if(obj != null)
                    {
                        ret.add(obj.getPosition());
                    }
                    else
                        ret.add(null);
                }
            }
        }
        else
        {
            ret.add(currentVecGoal);
        }

        if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.SAFE))
        {
            ret.clear();
            ret.add(getState().getPosition());
        }

        return ret;

    }

    protected boolean checkIfStuck()
    {
        boolean ret = false;
        int stuckTime = 20;
        knowledgeModule.incrementTimeOnCurrentLegOfPath();
        if(getState().getPosition().distance(knowledgeModule.getStuckPos()) < stuckTime / 5)
        {
            knowledgeModule.incrementStuckCount();
            if(knowledgeModule.getStuckCount() > stuckTime)
            {
                // knowledgeModule.setCurrentState(StateOfAgent.STUCK);
                // System.out.println("tempsub:" + tempSubGoal);
                //
                // System.out.println("STUCKKKKK");
                knowledgeModule.setCurrentMentalState(MentalStateOfAgent.NEED_NEW_PATH);
                ret = true;

                ((EvacuationHumanPathFinding) pathFinding).removeTempSubGoal();
                // knowledgeModule.setLocalizedSelf(false);
                // knowledgeModule.setCurrentMentalState(MentalStateOfAgent.IDLE);
                // knowledgeModule.setLocalizationTime(50);
                // knowledgeModule.clearNearbyDoor();
                knowledgeModule.setStuckPos(getState().getPosition());
                knowledgeModule.resetStuckCount();
                knowledgeModule.resetTimeOnCurrentLegOfPath();
            }
        }
        else
        {
            knowledgeModule.resetStuckCount();
            knowledgeModule.setStuckPos(getState().getPosition());
        }
        if(knowledgeModule.getTimeOnCurrentLegOfPath() > knowledgeModule.getPathLegThreshold())
        {
            knowledgeModule.setStuckPos(getState().getPosition());
            knowledgeModule.resetStuckCount();
            knowledgeModule.resetTimeOnCurrentLegOfPath();
            knowledgeModule.setCurrentMentalState(MentalStateOfAgent.NEED_NEW_PATH);
            ret = true;
        }
        return ret;
    }

    protected void updateExploringData()
    {
        if(!knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.EXPLORING_AREA))
        {
            knowledgeModule.setExplorePos(null);
        }
    }

    protected void processActionGoals(Plan generatedPlan)
    {
        openDoor(generatedPlan);
    }

    protected void openDoor(Plan generatedPlan)
    {
        for(EnvObjectKnowledgeStorageObject obj : knowledgeModule.getEnvObjects())
        {
            if(obj.getType().equals(DOOR) && !obj.isOpen() && getState().getPosition().distance(obj.getPosition()) < getState().getReachDistance() * 2.5f)
            {
                OpenDoorTask openDoorTask = (OpenDoorTask) taskModule.createTask(OPENDOOR, knowledgeModule.getTime());
                openDoorTask.setId(obj.getID());
                generatedPlan.addTask(openDoorTask);
            }
        }
    }

    protected void processNavigationGoals()
    {
        if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.NEED_NEW_PATH))
        {
            knowledgeModule.setCurrentMentalState(MentalStateOfAgent.IDLE);
            knowledgeModule.getGoals().remove(knowledgeModule.getCurrentGoal());
            if(havePath())
            {
                Vector3f goalPos = null;
                Door finalStep = (Door) knowledgeModule.getAgentPath().getFinalStep();
                if(finalStep.getDoorCongestion() < 8)
                {
                    goalPos = calculatePath(goalPos, finalStep);
                }
                else
                {
                    createPath();
                }

                testPossibleGoal(goalPos);
            }
            else
            {
                createPath();
            }
        }
        else
        {

            if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.ARRIVED))
            {
                knowledgeModule.getGoals().remove(knowledgeModule.getCurrentGoal());
                knowledgeModule.setCurrentMentalState(MentalStateOfAgent.IDLE);
            }
            if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.ARRIVED_IN_ROOM))
            {
                knowledgeModule.getGoals().remove(knowledgeModule.getCurrentGoal());
                enterNewRoom();
            }
            if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.ARRIVED_AT_EXIT))
            {
                knowledgeModule.getGoals().remove(knowledgeModule.getCurrentGoal());
                findSafeSpotOutside();
            }
            if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.EXITED_BUILDING))
            {
                knowledgeModule.getGoals().remove(knowledgeModule.getCurrentGoal());
                testPossibleGoal(knowledgeModule.getFinalGoalPos());
                knowledgeModule.setCurrentMentalState(MentalStateOfAgent.GOING_TO_SAFETY);
            }

            localizeSelf();

            if(amIdle())
            {
                if(!havePath())
                {
                    createPath();
                }
                else
                {
                    knowledgeModule.getGoals().remove(knowledgeModule.getCurrentGoal());

                    knowledgeModule.getGoals().remove(knowledgeModule.getCurrentGoal());
                    // System.out.println("continuing on path");
                    Door currentDoor;
                    if(knowledgeModule.isGetSafe())
                    {
                        currentDoor = (Door) knowledgeModule.getAgentPath().getNextStepNoRemove();
                        knowledgeModule.setGetSafe(false);
                    }
                    else
                    {
                        currentDoor = (Door) knowledgeModule.getAgentPath().getNextStep();
                    }

                    if(currentDoor != null)
                    {
                        knowledgeModule.resetTimeOnCurrentLegOfPath();
                        // ((EvacuationHumanPathFinding) pathFinding).printDoorPath(knowledgeModule.getAgentPath().getPath());
                        Vector3f goalPos = null;
                        goalPos = setGoalDoor(goalPos, currentDoor);
                        testPossibleGoal(goalPos);
                    }

                }
            }
        }
    }

    private void findSafeSpotOutside()
    {
        Vector3f goalPos = null;

        EnvObjectKnowledgeStorageObject closestWall = null;
        for(int i = 0; i < knowledgeModule.getPerceivedRVOObstacles().size(); i++)
        {
            EnvObjectKnowledgeStorageObject foundObj = knowledgeModule.findEnvObj(knowledgeModule.getPerceivedRVOObstacles().get(i).getID());
            System.out.println("foundObj");
            if(foundObj.getType().equals(WALL))
            {
            	System.out.println("WALL");
                closestWall = foundObj;
                break;
            }
        }
        Random r = new Random();
        int forwardDist = 15 + r.nextInt(50);
        int horizDist = r.nextInt(400) - 200;
        Vector3f dir = null;
        if(closestWall != null)
        {
            float xDist = FastMath.abs(getState().getPosition().x - closestWall.getPosition().x);
            float zDist = FastMath.abs(getState().getPosition().z - closestWall.getPosition().z);

            if(xDist > zDist)
            {
                dir = new Vector3f(horizDist, 0, FastMath.sign(getState().getHeading().z) * forwardDist);
            }
            else
            {
                dir = new Vector3f(FastMath.sign(getState().getHeading().x) * forwardDist, 0, horizDist);
            }
        }
        if(dir != null)
        {
            goalPos = getState().getPosition().add(dir);
        }

        EnvObjectKnowledgeStorageObject floor = knowledgeModule.findEnvObj(FLOOR);
        goalPos = reverseGoal(goalPos, dir, floor);

        // int count = 0;
        // while(testBadPoint(goalPos))
        // {
        // count++;
        // goalPos = getState().getPosition().add(getState().getHeading().mult(10)).add(r.nextInt(11) - 5, 0, r.nextInt(11) - 5);
        // if(count > 5)
        // {
        // goalPos = null;
        // break;
        // }
        // }

        Vector3f firstGoalPos = exitBuilding();
        if(goalPos != null)
        {
            knowledgeModule.setCurrentMentalState(MentalStateOfAgent.EXITING_BUILDING);
        }
        knowledgeModule.setFinalGoalPos(goalPos);

        testPossibleGoal(firstGoalPos);
    }

    protected boolean amIdle()
    {
        return knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.IDLE);
    }

    protected boolean havePath()
    {
        return knowledgeModule.getAgentPath().getPath() != null;
    }

    protected void updateAgentKnowledge()
    {
        processMessages();

        updateEmotionalState();

        updateKnowledgeGraph();

        updateExploringData();

        processNewKnowledge();

        // printKnowledgeGraph();
    }

    private void processNewKnowledge()
    {
        for(EnvObjectKnowledgeStorageObject obj : knowledgeModule.getNewEnvObjs())
        {
            if(obj.getType().equals(DOOR))
            {
                knowledgeModule.addDoorObj(obj);
            }
            if(obj.getType().equals(WALL))
            {
                knowledgeModule.addWallObj(obj);
            }
        }
    }

    private void printKnowledgeGraph()
    {
        System.out.println("################################KNOWLEDGE GRAPH START################################");
        if(knowledgeModule.getDoors().size() > 0)
        {
            List<Integer> visitedList = new ArrayList<>();
            List<Integer> openList = new ArrayList<>();
            Door firstDoor = knowledgeModule.getDoorsMap().values().iterator().next();

            openList.add(firstDoor.getId());
            visitedList.add(firstDoor.getId());
            while(openList.size() > 0)
            {
                int currentNodeID = openList.get(0);
                openList.remove(0);
                Door currentDoor = knowledgeModule.getDoor(currentNodeID);
                System.out.println(currentNodeID + " connects to: ");
                for(int i : currentDoor.getNeighbors())
                {
                    System.out.print(knowledgeModule.getDoor(i).getId() + ", ");
                }
                System.out.println();
                for(int i : currentDoor.getNeighbors())
                {
                    if(!visitedList.contains(i))
                    {
                        openList.add(i);
                        visitedList.add(i);
                    }
                }
            }

        }
        System.out.println("################################KNOWLEDGE GRAPH END##################################");
    }

    protected void printState()
    {
        System.out.println("CurState: " + knowledgeModule.getCurrentMentalState());
        // System.out.println("DoorID: " + knowledgeModule.getGoalDoor().getId());
        // System.out.println("Nearbydoors size: " + knowledgeModule.getNearbyDoors().size());
    }

    private void createPath()
    {
        if(knowledgeModule.isLocalizedSelf() && !knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.GOING_DOOR))
        {
            Random r = new Random();
            Vector3f goalPos = null;

            Door doorToVisit = null;

            if(knowledgeModule.getDoors().size() > 0)
            {
                if(knowledgeModule.isSirenHeard() && knowledgeModule.getExitDoors().size() > 0)
                {
                    Door closestExit = null;
                    float closest = Float.MAX_VALUE;
                    List<Thing> bestPath = null;
                    for(EnvObjectKnowledgeStorageObject d : knowledgeModule.getExitDoors().values())
                    {
                        Door tempDoor = knowledgeModule.findDoor(d.getID());
                        List<Thing> tempPath = ((EvacuationHumanPathFinding) pathFinding).findPathToDoor(tempDoor, getState().getPosition());

                        if(tempDoor.getFscore() < closest)
                        {
                            closestExit = tempDoor;
                            closest = tempDoor.getFscore();
                            bestPath = tempPath;
                        }

                    }

                    if(closestExit != null)
                    {
                        doorToVisit = closestExit;
                        if(bestPath != null && bestPath.size() > 0)
                        {
                            float f = doorToVisit.getFscore() / getState().getPosition().distance(knowledgeModule.findEnvObj(doorToVisit.getId()).getPosition());
                            // if(f > 3)
                            // System.out.println("distance factor Value: " + f);
                            knowledgeModule.resetTimeOnCurrentLegOfPath();
                            knowledgeModule.getAgentPath().setPath(bestPath);
                            knowledgeModule.getAgentPath().setValue(doorToVisit.getFscore());
                            knowledgeModule.getAgentPath().setDistanceFactor(f);
                            Door nearbyDoor = (Door) knowledgeModule.getAgentPath().getFirstStep();
                            goalPos = setGoalDoor(goalPos, nearbyDoor);
                            EnvObjectKnowledgeStorageObject objModel = knowledgeModule.findEnvObj(doorToVisit.getId());
                            if(objModel != null)
                            {
                                knowledgeModule.clearImportantObjects();
                                knowledgeModule.addImportantObject(objModel);
                            }
                        }
                    }
                }
                else
                {

                    if(knowledgeModule.getDoors().size() > 0) // find unvisited door anywhere
                    {
                        List<Door> visitWorthyFrontDoors = new ArrayList<>();
                        List<Door> visitWorthyBackDoors = new ArrayList<>();
                        Iterator<Door> doorIter = knowledgeModule.getDoors().iterator();
                        while(doorIter.hasNext())
                        {
                            Door d = doorIter.next();
                            if(!d.isVisited() && !d.isExit())
                            {
                                if(verifyGoalExists(d))
                                {
                                    EnvObjectKnowledgeStorageObject dObj = knowledgeModule.findEnvObj(d.getId());
                                    if(dObj != null)
                                    {
                                        if(getState().getHeading().dot(dObj.getPosition().subtract(getState().getPosition())) > 0)
                                        {
                                            visitWorthyFrontDoors.add(d);
                                        }
                                        else
                                        {
                                            visitWorthyBackDoors.add(d);
                                        }
                                    }

                                }
                            }
                        }

                        if(visitWorthyFrontDoors.size() > 0)
                        {
                            doorToVisit = visitWorthyFrontDoors.get(r.nextInt(visitWorthyFrontDoors.size()));
                            // System.out.println("Going Unvisited (front) DOOR: " + doorToVisit.getId());
                        }
                        else if(visitWorthyBackDoors.size() > 0)
                        {
                            doorToVisit = visitWorthyBackDoors.get(r.nextInt(visitWorthyBackDoors.size()));
                            // System.out.println("Going Unvisited (rear) DOOR: " + doorToVisit.getId());
                        }
                    }

                    if(doorToVisit != null && !verifyGoalExists(doorToVisit))
                    {
                        // knowledgeModule.getDoorsMap().remove(doorToVisit.getId());
                        doorToVisit = null;
                    }
                    if(doorToVisit != null)
                    {
                        goalPos = calculatePath(goalPos, doorToVisit);
                    }
                }
            }
            testPossibleGoal(goalPos);
        }
    }

    protected Vector3f calculatePath(Vector3f goalPos, Door doorToVisit)
    {
        List<Thing> path = ((EvacuationHumanPathFinding) pathFinding).findPathToDoor(doorToVisit, getState().getPosition());
        // System.out.println("Start Point: " + getState().getPosition());
        // ((EvacuationHumanPathFinding) pathFinding).printDoorPath(path);
        if(path != null && path.size() > 0)
        {
            float f = doorToVisit.getFscore() / getState().getPosition().distance(knowledgeModule.findEnvObj(doorToVisit.getId()).getPosition());
            // if(f > 3)
            // System.out.println("distance factor Value: " + f);
            knowledgeModule.resetTimeOnCurrentLegOfPath();
            knowledgeModule.getAgentPath().setPath(path);
            knowledgeModule.getAgentPath().setValue(doorToVisit.getFscore());
            knowledgeModule.getAgentPath().setDistanceFactor(f);
            Door nearbyDoor = (Door) knowledgeModule.getAgentPath().getFirstStep();
            goalPos = setGoalDoor(goalPos, nearbyDoor);
            EnvObjectKnowledgeStorageObject objModel = knowledgeModule.findEnvObj(doorToVisit.getId());
            if(objModel != null)
            {
                knowledgeModule.clearImportantObjects();
                knowledgeModule.addImportantObject(objModel);
            }
        }
        return goalPos;
    }

    protected boolean verifyGoalExists(Door doorToVisit)
    {
        if(knowledgeModule.findEnvObj(doorToVisit.getId()) == null)
        {
            if(!doorToVisit.isExit())
            {
                return false;
            }
        }
        return true;
    }

    protected Vector3f setGoalDoor(Vector3f goalPos, Door nearbyDoor)
    {
        EnvObjectKnowledgeStorageObject obj = knowledgeModule.findEnvObj(nearbyDoor.getId());
        if(obj != null)
        {
            goalPos = getDoorTravelPos(obj);
            knowledgeModule.setGoalDoor(nearbyDoor);
            knowledgeModule.setCurrentMentalState(MentalStateOfAgent.GOING_DOOR);
        }
        else
        {
            knowledgeModule.setLocalizedSelf(false);
        }
        return goalPos;
    }

    private Vector3f getDoorTravelPos(EnvObjectKnowledgeStorageObject obj)
    {
        Vector3f agentVec = obj.getPosition().subtract(getState().getPosition());

        Vector3f returnVal;
        if(obj.getScale().x < obj.getScale().z)
        {

            if(agentVec.dot(Vector3f.UNIT_X) > 0)
            {
                returnVal = obj.getPosition().add(Vector3f.UNIT_Z.mult(1.5f));
            }
            else
            {
                returnVal = obj.getPosition().add(Vector3f.UNIT_Z.mult(-1.5f));
            }

        }
        else
        {

            if(agentVec.dot(Vector3f.UNIT_Z) < 0)
            {
                returnVal = obj.getPosition().add(Vector3f.UNIT_X.mult(1.5f));
            }
            else
            {
                returnVal = obj.getPosition().add(Vector3f.UNIT_X.mult(-1.5f));
            }

        }

        return returnVal;
    }

    protected void testPossibleGoal(Vector3f goalPos)
    {
        // System.out.println("hi4");
        if(goalPos != null)
        {
            setNewGoal(goalPos);
        }
        else
        {
            knowledgeModule.setLocalizedSelf(false);
            knowledgeModule.setGetSafe(true);
        }
    }

    private void enterNewRoom()
    {
        Vector3f goalPos = null;

        EnvObjectKnowledgeStorageObject closestWall = null;
        for(int i = 0; i < knowledgeModule.getPerceivedRVOObstacles().size(); i++)
        {
            EnvObjectKnowledgeStorageObject foundObj = knowledgeModule.findEnvObj(knowledgeModule.getPerceivedRVOObstacles().get(i).getID());
            if(foundObj.getType().equals(WALL))
            {
                closestWall = foundObj;
                break;
            }
        }

        Vector3f dir = null;
       if(closestWall != null)
        {
            float xDist = FastMath.abs(getState().getPosition().x - closestWall.getPosition().x);
            float zDist = FastMath.abs(getState().getPosition().z - closestWall.getPosition().z);

            if(xDist > zDist)
            {
                dir = new Vector3f(0, 0, FastMath.sign(getState().getHeading().z) * 2);
                System.out.println("wall");
            }
            else
            {
                dir = new Vector3f(FastMath.sign(getState().getHeading().x) * 2, 0, 0);
               // dir = new Vector3f(0, 0, FastMath.sign(getState().getHeading().z) * 2);
                System.out.println("wall1");

            }
        }
        if(dir != null)
        {
            goalPos = getState().getPosition().add(dir);
            System.out.println("wall2");

        }

        // int count = 0;
        // while(testBadPoint(goalPos))
        // {
        // count++;
        // goalPos = getState().getPosition().add(getState().getHeading().mult(10)).add(r.nextInt(11) - 5, 0, r.nextInt(11) - 5);
        // if(count > 5)
        // {
        // goalPos = null;
        // break;
        // }
        // }
        if(goalPos != null)
        {
            knowledgeModule.setCurrentMentalState(MentalStateOfAgent.ENTERING_NEW_ROOM);
        }

        testPossibleGoal(goalPos);
    }

    private Vector3f exitBuilding()
    {
        Vector3f goalPos = null;

        EnvObjectKnowledgeStorageObject closestWall = null;
        for(int i = 0; i < knowledgeModule.getPerceivedRVOObstacles().size(); i++)
        {
            EnvObjectKnowledgeStorageObject foundObj = knowledgeModule.findEnvObj(knowledgeModule.getPerceivedRVOObstacles().get(i).getID());
            if(foundObj.getType().equals(WALL))
            {
                closestWall = foundObj;
                break;
            }
        }

        Vector3f dir = null;
        if(closestWall != null)
        {
            float xDist = FastMath.abs(getState().getPosition().x - closestWall.getPosition().x);
            float zDist = FastMath.abs(getState().getPosition().z - closestWall.getPosition().z);

            if(xDist > zDist)
            {
                dir = new Vector3f(0, 0, FastMath.sign(getState().getHeading().z) * 5);
            //    System.out.println("I am here");
               // dir = new Vector3f(FastMath.sign(getState().getHeading().x) * 5, 0, 0);
            }
            else
            {
                dir = new Vector3f(FastMath.sign(getState().getHeading().x) * 5, 0, 0);
              //  System.out.println("I am here2");
                
            }
        }
        if(dir != null)
        {
            goalPos = getState().getPosition().add(dir);
        }

        EnvObjectKnowledgeStorageObject floor = knowledgeModule.findEnvObj(FLOOR);
        goalPos = reverseGoal(goalPos, dir, floor);

        // int count = 0;
        // while(testBadPoint(goalPos))
        // {
        // count++;
        // goalPos = getState().getPosition().add(getState().getHeading().mult(10)).add(r.nextInt(11) - 5, 0, r.nextInt(11) - 5);
        // if(count > 5)
        // {
        // goalPos = null;
        // break;
        // }
        // }
        return goalPos;
    }

    protected Vector3f reverseGoal(Vector3f goalPos, Vector3f dir, EnvObjectKnowledgeStorageObject floor)
    {
        if(floor != null)
        {
            if(floor.contains2D(goalPos))
            {
                goalPos = getState().getPosition().subtract(dir);
            }
        }
        return goalPos;
    }

    protected void setNewGoal(Vector3f goalPos)
    {
        knowledgeModule.getGoals().add(new Goal(VECTOR, POSITION, goalPos, 10));
    }

    protected void localizeSelf()
    {

        if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.EXPLORING_AREA))
        {
            knowledgeModule.incrementExploringTime();
            if(knowledgeModule.getExploringTime() > knowledgeModule.getLocalizationTime())
            {
                knowledgeModule.setLocalizedSelf(true);
                knowledgeModule.resetExploringTime();
            }
        }

        if(!knowledgeModule.isLocalizedSelf() && amIdle())
        {
            exploreArea();
        }

    }

    protected void processMessages()
    {
        readInbox();
        AgentMessage am;

        for(int i = 0; i < 2; i++)
        {
            am = new AgentMessage(getState().getID(), getState().getID(), knowledgeModule.getTime(), "HI!!!");
            interactionModule.sendMessage(am);
        }
    }

    protected void updateEmotionalState()
    {
        if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.FLEEING))
        {
            knowledgeModule.setPanicTime(knowledgeModule.getPanicTime() - 1);
            if(knowledgeModule.getPanicTime() <= 0)
            {
                knowledgeModule.setCurrentMentalState(MentalStateOfAgent.IDLE);
                knowledgeModule.setReactGoal(null);
                pathFinding.notifyReact();
            }
        }
        if(!knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.FLEEING) && knowledgeModule.getReactGoal() != null)
        {
            knowledgeModule.getGoals().remove(knowledgeModule.getReactGoal());
            knowledgeModule.setReactGoal(null);
        }

    }

    private void readInbox()
    {
        AgentMessage am;
        while((am = interactionModule.getMessageFromInbox()) != null)
        {
            // logger.debug("Source: " + am.getSourceID() + ", Destin: " + am.getDestinationID() + ", Message: " + am.getMessage());
        }
    }

    private void exploreArea()
    {
        Vector3f goalPos = null;

        goalPos = generateRandomNearbyPos();

        if(goalPos != null)
        {
            knowledgeModule.setExplorePos(goalPos);
            setNewGoal(goalPos);
        }

    }

    private Vector3f generateRandomNearbyPos()
    {
        Vector3f goalPos = new Vector3f();
        Random r = new Random();

        goalPos = getState().getPosition().add(r.nextInt(31) - 15, 0, r.nextInt(31) - 15);

        // goalPos = new Vector3f(r.nextInt(50) - 25, 0, r.nextInt(50) - 25);
        int count = 0;
        while(testBadPointAndPath(goalPos))
        {
            count++;
            goalPos = getState().getPosition().add(r.nextInt(31) - 15, 0, r.nextInt(31) - 15);
            // goalPos = new Vector3f(r.nextInt(50) - 25, 0, r.nextInt(50) - 25);
            if(count > 5)
                return null;
        }
        knowledgeModule.setCurrentMentalState(MentalStateOfAgent.EXPLORING_AREA);
        return goalPos;
    }

    private boolean testBadPointAndPath(Vector3f vec)
    {
        // System.out.println("here1: ");
        for(EnvObjectKnowledgeStorageObject obj : knowledgeModule.getEnvObjects())
        {
            if(obj.isCollidable())
            {
                if(obj.intersects2D(vec, getState().getScale()))
                {
                    return true;
                }
                if(obj.agentPathIntersectsObj2D(getState().getPosition(), vec, getState().getRadius()))
                {
                    return true;
                }
            }
        }
        // System.out.println("here2: ");
        EnvObjectKnowledgeStorageObject floor = knowledgeModule.findEnvObj(FLOOR);
        if(floor != null)
        {
            if(!floor.contains2D(vec))
            {
                // System.out.println("here4: ");
                return true;
            }
        }
        // System.out.println("here3: ");
        return false;
    }

    /**
     * Agent updates its knowledge graph of rooms and doors.
     */

    private void updateKnowledgeGraph()
    {
        updatePostition();
        updateOldObjects();
        addSeenDoors();

        // if(!knowledgeModule.isInsideDoor())
        // {
        // addSeenDoors();
        // }
    }

    private void updateOldObjects()
    {
        for(EnvObjectKnowledgeStorageObject newObj : knowledgeModule.getNewEnvObjs())
        {
            if(newObj.getType().equals(WALL) || newObj.getType().equals(DOOR))
            {
                for(EnvObjectKnowledgeStorageObject doorObj : knowledgeModule.getEnvObjects())
                {
                    if(doorObj.getType().equals(DOOR) && newObj.getPosition().distance(doorObj.getPosition()) < 180 && doorObj.getID() != newObj.getID())
                    {
                        Door d = knowledgeModule.getDoor(doorObj.getID());
                        if(d != null)
                        {
                            Iterator<Integer> iter = d.getNeighbors().iterator();
                            while(iter.hasNext())
                            {
                                int nID = iter.next();
                                EnvObjectKnowledgeStorageObject nObj = knowledgeModule.findEnvObj(nID);
                                if(nObj != null && newObj.getID() != nObj.getID())
                                {
                                    if(newObj.miniBAIntersects2DLine(doorObj.getPosition(), nObj.getPosition()))
                                    {
                                        // System.out.println("Object " + newObj.getType() + " with ID " + newObj.getID() + "intersects between : (" + d.getId() + "," + nID + ")");
                                        iter.remove();
                                        Door n = knowledgeModule.getDoor(nID);
                                        if(n != null)
                                        {
                                            n.removeNeighbor(d.getId());
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    }

    private void updatePostition()
    {
        boolean inDoor = false;
        for(EnvObjectKnowledgeStorageObject obj : knowledgeModule.getEnvObjects())
        {
            if(obj.getType().equals(DOOR))
            {
                if(obj.intersects2D(getState().getPosition(), getState().getScale()))
                {
                    inDoor = true;
                    Door door = knowledgeModule.getDoor(obj.getID());
                    if(door != null)
                    {
                        door.setVisited(true);
                        if(door.isExit() && !knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.GOING_TO_SAFETY))
                        {
                            knowledgeModule.setCurrentMentalState(MentalStateOfAgent.ARRIVED_AT_EXIT);
                        }
                    }
                }
            }
        }
        if(inDoor)
        {
            knowledgeModule.setInsideDoor(true);
        }
        else
        {
            knowledgeModule.setInsideDoor(false);
        }
    }

    /**
     * Agent updates its knowledge by adding newly seen doors to its door knowledgebase.
     */
    private void addSeenDoors()
    {

        List<VisionModel> visList = knowledgeModule.getVisibleList();

        for(int i = 0; i < visList.size(); i++)
        {
            if(visList.get(i).getType().equals(ENV_OBJECT))
            {
                EnvObjectKnowledgeStorageObject obj = knowledgeModule.findEnvObj(visList.get(i).getId());
                if(obj != null)
                {
                    if(obj.getType().equals(DOOR))
                    {
                        Door newDoor = knowledgeModule.addDoor(obj.getID());
                        if(newDoor != null)
                        {
                            if(obj.getDescription().equals(EXITDOOR))
                            {
                                newDoor.setExit(true);
                                knowledgeModule.addExitDoor(obj);
                            }
                            checkCongestionDoor(newDoor);
                            findNearbyDoors(obj, newDoor);
                        }
                        else
                        {
                            Door d = knowledgeModule.getDoor(obj.getID());
                            checkCongestionDoor(d);
                        }
                    }
                }
            }
        }

        for(Door d : knowledgeModule.getDoors())
        {
            if(d.getCycleSet() > 0 && knowledgeModule.getTime() - d.getCycleSet() > 100)
            {
                // System.out.println("RESETTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTRESETTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT");
                d.setDoorCongestion(0);
                d.setCycleSet(-1);
            }
        }

    }

    protected void findNearbyDoors(EnvObjectKnowledgeStorageObject obj, Door newDoor)
    {
        for(Door d : knowledgeModule.getDoors())
        {
            EnvObjectKnowledgeStorageObject dObj = knowledgeModule.findEnvObj(d.getId());
            if(dObj != null)
            {
                if(obj.getPosition().distance(dObj.getPosition()) < 180)
                {
                    if(d.getId() != newDoor.getId() && !d.getNeighbors().contains(newDoor.getId()))
                    {
                        boolean intersects = false;
                        for(EnvObjectKnowledgeStorageObject testObj : knowledgeModule.getEnvObjects())
                        {
                            if(testObj.getID() != obj.getID() && testObj.getID() != dObj.getID())
                            {
                                if(testObj.getType().equals(WALL) || testObj.getType().equals(DOOR))
                                {
                                    if(testObj.miniBAIntersects2DLine(obj.getPosition(), dObj.getPosition()))
                                    {
                                        intersects = true;
                                        break;
                                    }
                                }
                            }
                        }
                        if(!intersects)
                        {
                            newDoor.addNeighbor(d.getId());
                            d.addNeighbor(newDoor.getId());
                        }
                    }
                }
            }
        }
    }

    /**
     * Agent Path Finding.
     * 
     * @return TODO
     */
    private Plan planMovement(Plan generatedPlan)
    {

        if(knowledgeModule.getReactGoal() != null)
        {
            if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.FLEEING))
            {
                knowledgeModule.desiredSpeed = knowledgeModule.getSelf().getMaxSpeed() / 2.5f;
                knowledgeModule.getSelf().setPosture(Posture.Run);
            }
            else
            {
                knowledgeModule.desiredSpeed = knowledgeModule.getSelf().getMaxSpeed() / 5;
                knowledgeModule.getSelf().setPosture(Posture.Walk);
            }
            pathFinding.pathplan((Vector3f) knowledgeModule.getReactGoal().getValue(), generatedPlan);
        }
        else if(taskModule.containsTask(POSITION) && (taskModule.containsTask(HEADING)))
        {
            Vector3f vec = null;
            List<Goal> goal = knowledgeModule.getGoals();
            float maxUtil = 0;

            for(int i = 0; i < goal.size(); i++)
            {
                if(goal.get(i).getName().equals(POSITION))
                {
                    Object value = goal.get(i).getValue();

                    if(value instanceof Vector3f)
                    {
                        if(goal.get(i).getUtilityValue() > maxUtil)
                        {
                            vec = (Vector3f) value;

                            knowledgeModule.setCurrentGoal(goal.get(i));
                            maxUtil = goal.get(i).getUtilityValue();
                            if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.FLEEING))
                            {
                                knowledgeModule.desiredSpeed = knowledgeModule.getSelf().getMaxSpeed() / 2.5f;
                                knowledgeModule.getSelf().setPosture(Posture.Run);
                            }
                            else
                            {
                                knowledgeModule.desiredSpeed = knowledgeModule.getSelf().getMaxSpeed() / 5;
                                knowledgeModule.getSelf().setPosture(Posture.Walk);
                            }
                        }
                    }

                }
                else if(goal.get(i).getName().equals(HEADING))
                {
                    FaceTask faceTask = (FaceTask) taskModule.createTask(HEADING, knowledgeModule.getTime());
                    faceTask.setHeading(new Vector3f((Vector3f) goal.get(i).getValue()));
                    generatedPlan.addTask(faceTask);
                }

            }
            if(vec != null)
            {
                if(knowledgeModule.isSirenHeard())
                {
                    knowledgeModule.desiredSpeed = knowledgeModule.getSelf().getMaxSpeed() / 3.5f;
                    knowledgeModule.getSelf().setPosture(Posture.Run);
                }
                if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.SAFE))
                {
                    knowledgeModule.desiredSpeed = knowledgeModule.getSelf().getMaxSpeed() / 3.5f;
                    knowledgeModule.getSelf().setPosture(Posture.Idle1);
                }

                pathFinding.pathplan(vec, generatedPlan);
                currentVecGoal = vec;

            }
        }
        return generatedPlan;
    }

    /**
     * Agent reaction. (React to immediate events that do not require planning.)
     */

    private void respondToEvents()
    {

        List<CombinedReasonedData> events = knowledgeModule.getEventsThisTick();

        boolean sirenOn = false;
        for(int i = 0; i < events.size(); i++)
        {
            CombinedReasonedData thisEvent = events.get(i);
            Vector3f pos = knowledgeModule.getSelf().getPosition();

            // System.out.println(thisEvent.getPredicatedName() + " " + thisEvent.getCertaintyPercent());

            if(thisEvent.getPredicatedName().equals(FIREWORK) && (thisEvent.getCertaintyPercent() > 100))
            {
                if(thisEvent.hasOrigin())
                {
                    Vector3f runDir = new Vector3f();
                    long time = knowledgeModule.getTime() + 20;
                    runDir = pos.subtract(thisEvent.getOrigin()).mult(-1);
                    float distance = pos.distance(thisEvent.getOrigin());
                    runDir.normalizeLocal();
                    runDir.multLocal(distance * .3f);
                    Vector3f newpos = pos.add(runDir);
                    newpos.setY(0);

                    knowledgeModule.getGoals().add(new Goal(VECTOR, POSITION, newpos, 20, time));
                    pathFinding.notifyReact();
                }
            }
           /* else if(thisEvent.getPredicatedName().equals(WALL) && (thisEvent.getCertaintyPercent() > 100))
            {
                if(thisEvent.hasOrigin())
                {
                    Vector3f runDir = new Vector3f();
                    long time = knowledgeModule.getTime() + 20;
                    runDir = pos.subtract(thisEvent.getOrigin()).mult(0);
                    float distance = pos.distance(thisEvent.getOrigin());
                    runDir.normalizeLocal();
                    runDir.multLocal(distance * .3f);
                    Vector3f newpos = pos.add(runDir);
                    newpos.setY(0);

                    knowledgeModule.getGoals().add(new Goal(VECTOR, POSITION, newpos, 20, time));
                    pathFinding.notifyReact();
                    //removeOldEventReactions();
                    
                }
            }*/
            
            else if(thisEvent.getPredicatedName().equals(WALL) && (thisEvent.getCertaintyPercent() > 80))
            {

                long time = knowledgeModule.getTime() + 1;
                Vector3f runDir = new Vector3f();
                boolean change = false;
                if(thisEvent.hasDirection())
                {
                    runDir = thisEvent.getDirection();
                }
                else if(thisEvent.hasOrigin())
                {
                    runDir = pos.subtract(thisEvent.getOrigin()).mult(-1);
                    float distance = pos.distance(thisEvent.getOrigin());
                    runDir.normalizeLocal();
                    runDir.multLocal(distance);
                    if(distance < 4)
                    {
                        change = true;
                    }
                }

                Vector3f newpos = pos.add(runDir);

                if(change)
                {
                    newpos = pos;
                }

                knowledgeModule.getGoals().add(new Goal(VECTOR, POSITION, newpos, 12, time));
               // removeOldEventReactions();
                pathFinding.notifyReact();
                removeOldEventReactions();
            }
            
            else if(thisEvent.getPredicatedName().equals(DRUMS) && (thisEvent.getCertaintyPercent() > 100))
            {

                long time = knowledgeModule.getTime() + 20;
                Vector3f runDir = new Vector3f();
                if(thisEvent.hasDirection())
                {
                    runDir = thisEvent.getDirection().mult(-1);
                }
                else if(thisEvent.hasOrigin())
                {
                    runDir = pos.subtract(thisEvent.getOrigin());
                }
                runDir.multLocal(10);

                knowledgeModule.getGoals().add(new Goal(VECTOR, POSITION, pos.add(runDir), 10, time));
                pathFinding.notifyReact();
            }
            else if(thisEvent.getPredicatedName().equals(GRILLING_FOOD) && (thisEvent.getCertaintyPercent() > 100))
            {

                long time = knowledgeModule.getTime() + 1;
                Vector3f runDir = new Vector3f();
                boolean change = false;
                if(thisEvent.hasDirection())
                {
                    runDir = thisEvent.getDirection();
                }
                else if(thisEvent.hasOrigin())
                {
                    runDir = pos.subtract(thisEvent.getOrigin()).mult(-1);
                    float distance = pos.distance(thisEvent.getOrigin());
                    runDir.normalizeLocal();
                    runDir.multLocal(distance);
                    if(distance < 4)
                    {
                        change = true;
                    }
                }

                Vector3f newpos = pos.add(runDir);

                if(change)
                {
                    newpos = pos;
                }

                knowledgeModule.getGoals().add(new Goal(VECTOR, POSITION, newpos, 12, time));
                pathFinding.notifyReact();
            }
            else if(thisEvent.getPredicatedName().equals("Siren") && (thisEvent.getCertaintyPercent() > 50))
            {
                sirenOn = true;
                // System.out.println("AGENT" + getState().getID() + " HEARD SIREN: " + knowledgeModule.getTime());
                if(!knowledgeModule.isSirenHeard())
                {
                    knowledgeModule.setStuckPos(getState().getPosition());
                    knowledgeModule.resetStuckCount();
                    knowledgeModule.resetTimeOnCurrentLegOfPath();
                    knowledgeModule.setCurrentMentalState(MentalStateOfAgent.NEED_NEW_PATH);
                    knowledgeModule.setSirenHeard(true);
                    knowledgeModule.getAgentPath().setPath(null);
                }
            }
        }

        if(sirenOn)
        {
            knowledgeModule.setSirenOn(true);
        }

    }

    private void removeOldEventReactions()
    {
        ArrayList<Goal> toBeRemoved = new ArrayList<Goal>();
        for(int i = 0; i < knowledgeModule.getGoals().size(); i++)
        {
            if(knowledgeModule.getGoals().get(i).getRemoveTime() == knowledgeModule.getTime())
            {
                toBeRemoved.add(knowledgeModule.getGoals().get(i));
            }
        }

        for(int i = 0; i < toBeRemoved.size(); i++)
        {
            knowledgeModule.getGoals().remove(toBeRemoved.get(i));
        }
    }

    /**
     * Get the agent self state.
     * 
     * @return
     */
    public EHumanAgentState getState()
    {
        return knowledgeModule.getSelf();
    }

    /**
     * Get the knowledge module.
     * 
     * @return KM
     */
    public EvacuationHumanKnowledgeModule getKnowledgeModule()
    {
        return knowledgeModule;
    }

    @Override
    public void addGoal(Goal newGoal)
    {
        if(newGoal instanceof LocationGoal)
        {

            Vector3f goalPos = ((LocationGoal) newGoal).getLocation();
            if(goalPos != null)
            {
                knowledgeModule.getGoals().remove(knowledgeModule.getCurrentGoal());
                knowledgeModule.getGoals().clear();
                knowledgeModule.getGoals().add(new Goal(VECTOR, POSITION, goalPos, 50));
                knowledgeModule.setCurrentMentalState(MentalStateOfAgent.PERSUING_USER_GOAL);
                knowledgeModule.getAgentPath().setPath(null);
                pathFinding.notifyUserGoalChange();
                knowledgeModule.clearImportantObjects();
            }
        }
    }

}

To implement the “EvacuationHumanPlanningModule” class:

• • Create a new class inside the “planning” package and name it “EvacuationHumanPlanningModule”.

• Copy the code given below that describes the full implementation of a “EvacuationHumanPlanningModule” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import java.io.Serializable;
import java.util.List;

import edu.utdallas.mavs.divas.core.sim.agent.interaction.perception.data.CombinedReasonedData;
import edu.utdallas.mavs.divas.core.sim.agent.knowledge.internal.Goal;
import edu.utdallas.mavs.divas.core.sim.agent.planning.ReactiveProactivePlanningModule;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.EvacuationHumanKnowledgeModule;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.task.HumanTaskModule;

public class EvacuationHumanPlanningModule extends ReactiveProactivePlanningModule<EvacuationHumanKnowledgeModule, HumanTaskModule, EvacuationHumanPlanExecutor, EvacuationHumanPlanGenerator, EvacuationHumanReactionModule> implements Serializable
{
    private static final long serialVersionUID = 1L;

    public EvacuationHumanPlanningModule(EvacuationHumanPlanGenerator planGenerator, EvacuationHumanPlanExecutor planExecutor, EvacuationHumanKnowledgeModule knowledgeModule, EvacuationHumanReactionModule reactionModule)
    {
        super(planGenerator, planExecutor, knowledgeModule, reactionModule);
    }

    @Override
    public void addGoal(Goal newGoal)
    {
        planGenerator.addGoal(newGoal);
    }

    @Override
    public boolean isImmediateReactionRequired()
    {
        boolean ret = false;
        List<CombinedReasonedData> events = knowledgeModule.getEventsThisTick();

        for(int i = 0; i < events.size(); i++)
        {
            CombinedReasonedData thisEvent = events.get(i);

            // System.out.println(thisEvent.getPredicatedName() + " " + thisEvent.getCertaintyPercent());

            if(knowledgeModule.isEventDangerous(thisEvent.getPredicatedName()) && (thisEvent.getCertaintyPercent() > 15))
            {
                ret = true;
            }

        }

        return ret;
    }

}

To implement the “EvacuationHumanReactionModule” class:

• • Create a new class inside the “planning” package and name it “EvacuationHumanReactionModule”.

• Copy the code given below that describes the full implementation of a “EvacuationHumanReactionModule” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import java.util.ArrayList;
import java.util.List;

import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import com.jme3.math.Vector3f;

import edu.utdallas.mavs.divas.core.sim.agent.interaction.perception.data.CombinedReasonedData;
import edu.utdallas.mavs.divas.core.sim.agent.knowledge.internal.Goal;
import edu.utdallas.mavs.divas.core.sim.agent.planning.AbstractReactionModule;
import edu.utdallas.mavs.divas.core.sim.agent.planning.Plan;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.interaction.HumanInteractionModule;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.EvacuationHumanKnowledgeModule;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.EvacuationHumanKnowledgeModule.MentalStateOfAgent;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.task.HumanTaskModule;
import edu.utdallas.mavs.evacuation.simulation.sim.common.state.Posture;

public class EvacuationHumanReactionModule extends AbstractReactionModule<EvacuationHumanKnowledgeModule, HumanTaskModule>
{
    private static final long                           serialVersionUID = 1L;

    private final static Logger                         logger           = LoggerFactory.getLogger(EvacuationHumanReactionModule.class);

    private PathFinding<EvacuationHumanKnowledgeModule> pathFinding;
    private HumanInteractionModule                      interactionModule;

    private static final String                         BOMB             = "Bomb";
    private static final String                         VECTOR           = "Vector";

    private static final String                         POSITION         = "Position";

    /**
     * Where plans are stored
     */
    // protected PlanStorageModule planStorageModule;

    public EvacuationHumanReactionModule(EvacuationHumanKnowledgeModule knowledgeModule, HumanTaskModule taskModule, HumanInteractionModule humanInteractionModule, EvacuationHumanPathFinding pathFinding)
    {
        super(knowledgeModule, taskModule);
        this.interactionModule = humanInteractionModule;
        this.pathFinding = pathFinding;
    }

    @Override
    public Plan react()
    {
        reactToEvents();
        return planMovement();
    }

    protected Plan planMovement()
    {
        EvacuationPlan generatedPlan = new EvacuationPlan();

        if(knowledgeModule.getReactGoal() != null)
        {
            if(knowledgeModule.getCurrentMentalState().equals(MentalStateOfAgent.FLEEING))
            {
                knowledgeModule.desiredSpeed = knowledgeModule.getSelf().getMaxSpeed() / 2.5f;
                knowledgeModule.getSelf().setPosture(Posture.Run);
            }
            else
            {
                knowledgeModule.desiredSpeed = knowledgeModule.getSelf().getMaxSpeed() / 5;
                knowledgeModule.getSelf().setPosture(Posture.Walk);
            }
            pathFinding.pathplan((Vector3f) knowledgeModule.getReactGoal().getValue(), generatedPlan);
        }

        generatedPlan.setPlanUtility(knowledgeModule.getTime());
        // planStorageModule.addPlan(generatedPlan);
        knowledgeModule.getSelf().setAgentPath(makeVectorPath());
        return generatedPlan;
    }

    private List<Vector3f> makeVectorPath()
    {
        List<Vector3f> ret = new ArrayList<>();
        ret.add((Vector3f) knowledgeModule.getReactGoal().getValue());

        return ret;

    }

    protected void reactToEvents()
    {
        List<CombinedReasonedData> events = knowledgeModule.getEventsThisTick();

        for(int i = 0; i < events.size(); i++)
        {
            CombinedReasonedData thisEvent = events.get(i);
            Vector3f pos = knowledgeModule.getSelf().getPosition();

            // System.out.println(thisEvent.getPredicatedName() + " " + thisEvent.getCertaintyPercent());

            if(thisEvent.getPredicatedName().equals(BOMB) && (thisEvent.getCertaintyPercent() > 15))
            {
                int bombTime = 8;
                Vector3f runDir = new Vector3f();
                if(thisEvent.hasDirection())
                {
                    runDir = thisEvent.getDirection().mult(-1);
                    runDir.normalizeLocal().multLocal(bombTime * 2 + 5);
                    knowledgeModule.setThreatDirection(thisEvent.getDirection());
                }
                else if(thisEvent.hasOrigin())
                {
                    runDir = pos.subtract(thisEvent.getOrigin());
                    runDir.normalizeLocal().multLocal(bombTime * 2 + 5);
                    knowledgeModule.setThreatDirection(thisEvent.getOrigin().subtract(pos));
                }
                Vector3f finGoalDir = pos.add(runDir);
                finGoalDir.setY(0);
                long time = knowledgeModule.getTime() + 20;
                // knowledgeModule.getGoal().addGoals(new GoalModel(VECTOR, POSITION, finGoalDir, 100, time));
                knowledgeModule.setReactGoal(new Goal(VECTOR, POSITION, finGoalDir, 100, time));
                pathFinding.notifyReact();
                knowledgeModule.getGoals().remove(knowledgeModule.getCurrentGoal());
                knowledgeModule.setCurrentMentalState(MentalStateOfAgent.FLEEING);
                knowledgeModule.setPanicTime(bombTime - 1);
            }
        }
    }

}

To implement the “EvacuationPath” class:

• • Create a new class inside the “planning” package and name it “EvacuationPath”.

• Copy the code given below that describes the full implementation of a “EvacuationPath” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import java.io.Serializable;
import java.util.List;

import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.ontology.Door;

/**
 * An evacuation path
 */
public class EvacuationPath implements Serializable
{
    List<Door> path;

    public EvacuationPath()
    {
        super();
        path = null;
    }

    public List<Door> getPath()
    {
        return path;
    }

    public void setPath(List<Door> path)
    {
        this.path = path;
    }

    public void addToPath(Door d)
    {
        path.add(d);
    }

    public Door getNextStep()
    {
        if(path.size() > 0)
        {
            return path.remove(0);
        }
        return null;
    }

}

To implement the “EvacuationPlan” class:

• • Create a new class inside the “planning” package and name it “EvacuationPlan”.

• Copy the code given below that describes the full implementation of a “EvacuationPlan” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import edu.utdallas.mavs.divas.core.sim.agent.planning.Plan;

public class EvacuationPlan extends Plan
{
    /**
     * The utility of the plan.
     */
    protected float planUtility;
    /**
     * The most recently selected task from the plan
     */
    protected int   lastUsedIndex;

    AgentPath       agentPath;

    public EvacuationPlan()
    {
        super();
        planUtility = Float.NEGATIVE_INFINITY;
        agentPath = new AgentPath();
    }

    /**
     * Get the plans utility.
     * 
     * @return utility
     */
    public float getPlanUtility()
    {
        return planUtility;
    }

    /**
     * Set the plan's utility.
     * 
     * @param planUtility
     *        the utility
     */
    public void setPlanUtility(float planUtility)
    {
        this.planUtility = planUtility;
    }

    public AgentPath getAgentPath()
    {
        return agentPath;
    }

    public void setAgentPath(AgentPath agentPath)
    {
        this.agentPath = agentPath;
    }

}

To implement the “HumanPlanningModule” class:

• • Create a new class inside the “planning” package and name it “HumanPlanningModule”.

• Copy the code given below that describes the full implementation of a “HumanPlanningModule” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import java.io.Serializable;
import java.util.List;

import edu.utdallas.mavs.divas.core.sim.agent.interaction.perception.data.CombinedReasonedData;
import edu.utdallas.mavs.divas.core.sim.agent.knowledge.internal.Goal;
import edu.utdallas.mavs.divas.core.sim.agent.planning.ReactiveProactivePlanningModule;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.EvacuationHumanKnowledgeModule;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.task.HumanTaskModule;

public class HumanPlanningModule extends ReactiveProactivePlanningModule<EvacuationHumanKnowledgeModule, HumanTaskModule, EvacuationHumanPlanExecutor, EvacuationHumanPlanGenerator, EvacuationHumanReactionModule> implements Serializable
{
    private static final long serialVersionUID = 1L;

    public HumanPlanningModule(EvacuationHumanPlanGenerator planGenerator, EvacuationHumanPlanExecutor planExecutor, EvacuationHumanKnowledgeModule knowledgeModule, EvacuationHumanReactionModule reactionModule)
    {
        super(planGenerator, planExecutor, knowledgeModule, reactionModule);
    }

    @Override
    public void addGoal(Goal newGoal)
    {
        planGenerator.addGoal(newGoal);
    }

    @Override
    public boolean isImmediateReactionRequired()
    {
        boolean ret = false;
        List<CombinedReasonedData> events = knowledgeModule.getEventsThisTick();

        for(int i = 0; i < events.size(); i++)
        {
            CombinedReasonedData thisEvent = events.get(i);

            // System.out.println(thisEvent.getPredicatedName() + " " + thisEvent.getCertaintyPercent());

            if(knowledgeModule.isEventDangerous(thisEvent.getPredicatedName()) && (thisEvent.getCertaintyPercent() > 15))
            {
                ret = true;
            }

        }

        return ret;
    }

}

To implement the “NodeSorter” class:

• • Create a new class inside the “planning” package and name it “NodeSorter”.

• Copy the code given below that describes the full implementation of a “NodeSorter” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import java.io.Serializable;
import java.util.HashMap;
import java.util.List;

/**
 * A quick sort implementation for path finding nodes
 */
public class NodeSorter implements Serializable
{
    private static final long                 serialVersionUID = 1L;
    private List<Integer>                     nodeIDs;
    private int                               number;
    private HashMap<Integer, PathFindingNode> nodeStore;
    private PathFindingNode                   startNode;
    private PathFindingNode                   goalNode;

    /**
     * Create a node sorter. (Using integer IDs and this storage map.)
     * 
     * @param nodeStore
     *        the node storage map
     * @param goalNode
     * @param startNode
     */
    public NodeSorter(HashMap<Integer, PathFindingNode> nodeStore, PathFindingNode startNode, PathFindingNode goalNode)
    {
        this.nodeStore = nodeStore;
        this.startNode = startNode;
        this.goalNode = goalNode;
    }

    /**
     * Method to sort these path finding nodes (using quick sort)
     * 
     * @param values2
     *        nodes to be sorted
     */
    public void sort(List<Integer> values2)
    {
        // Check for empty or null array
        if(values2 == null || values2.size() == 0)
        {
            return;
        }
        this.nodeIDs = values2;
        number = values2.size();
        quicksort(0, number - 1);
    }

    private void quicksort(int low, int high)
    {
        int i = low, j = high;
        // Get the pivot element from the middle of the list
        int pivot = nodeIDs.get(low + (high - low) / 2);

        // Divide into two lists
        while(i <= j)
        {
            // If the current value from the left list is smaller then the pivot
            // element then get the next element from the left list
            while(getProperNode(nodeIDs.get(i)).getFscore() < getProperNode(pivot).getFscore())
            {
                i++;
            }
            // If the current value from the right list is larger then the pivot
            // element then get the next element from the right list
            while(getProperNode(nodeIDs.get(j)).getFscore() > getProperNode(pivot).getFscore())
            {
                j--;
            }

            // If we have found a values in the left list which is larger then
            // the pivot element and if we have found a value in the right list
            // which is smaller then the pivot element then we exchange the
            // values.
            // As we are done we can increase i and j
            if(i <= j)
            {
                exchange(i, j);
                i++;
                j--;
            }
        }
        // Recursion
        if(low < j)
            quicksort(low, j);
        if(i < high)
            quicksort(i, high);
    }

    protected PathFindingNode getProperNode(Integer nodeID)
    {
        if(nodeID == startNode.getNodeID())
        {
            return startNode;
        }
        else if(nodeID == goalNode.getNodeID())
        {
            return goalNode;
        }
        else
        {
            return nodeStore.get(nodeID);
        }
    }

    private void exchange(int i, int j)
    {
        int temp = nodeIDs.get(i);
        nodeIDs.set(i, nodeIDs.get(j));
        nodeIDs.set(j, temp);
    }
}

To implement the “OrcaLine” class:

• • Create a new class inside the “planning” package and name it “OrcaLine”.

• Copy the code given below that describes the full implementation of a “OrcaLine” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import java.io.Serializable;

import com.jme3.math.Vector2f;

public class OrcaLine implements Serializable
{
    private static final long serialVersionUID = 1L;
    
    public Vector2f point;
    public Vector2f direction;

    public OrcaLine()
    {
        super();
        this.point = new Vector2f();
        this.direction = new Vector2f();
    }

    public OrcaLine(OrcaLine orcaLine)
    {
        super();
        this.point = new Vector2f(orcaLine.point);
        this.direction = new Vector2f(orcaLine.direction);
    }
}

To implement the “PathFinding” class:

• • Create a new class inside the “planning” package and name it “PathFinding”.

• Copy the code given below that describes the full implementation of a “PathFinding” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import com.jme3.math.Vector3f;

import edu.utdallas.mavs.divas.core.sim.agent.knowledge.KnowledgeModule;
import edu.utdallas.mavs.divas.core.sim.agent.planning.Plan;

/**
 * The Agent's Path Finding Module
 * 
 * @param <KM>
 *        the agent's knowledge module
 */
public interface PathFinding<KM extends KnowledgeModule>
{
    /**
     * Find a path to the specified goal
     * 
     * @param vecGoal
     *        the vector point to the goal
     * @param generatedPlan
     *        the plan to add path planning to
     */
    public void pathplan(Vector3f vecGoal, Plan generatedPlan);

    /**
     * A method to notify the path finding that the agent has reacted to an event without planning.
     */
    public void notifyReact();

    public void notifyUserGoalChange();
}

To implement the “PathFindingNode” class:

• • Create a new class inside the “planning” package and name it “PathFindingNode”.

• Copy the code given below that describes the full implementation of a “PathFindingNode” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import java.io.Serializable;
import java.util.ArrayDeque;
import java.util.Collection;
import java.util.List;

import com.jme3.math.Vector3f;

import edu.utdallas.mavs.divas.utils.collections.LightWeightBoundedQueue;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.external.EnvObjectKnowledgeStorageObject;

/**
 * A generic path finding node (For use with Astar path finding algorithm)
 */
public class PathFindingNode implements Serializable
{
    /**
     * 
     */
    private static final int                 MAX_NEIGHBOR     = 15;
    private static final long                serialVersionUID = 1L;
    private int                              nodeID;
    private Vector3f                         point;
    private float                            gscore, fscore;
    private PathFindingNode                  cameFrom;
    private LightWeightBoundedQueue<Integer> neighbors;
    private int                              depth            = 0;
    private boolean                          closed           = false;
    private boolean                          newNode          = true;
    private boolean                          connectedToGoal  = false;
    private int                              parentEnvObjID   = -1;
    private float                            agentSize;

    ArrayDeque<PathFindingNode>              nodesToProcess;

    private static int                       nextNodeID       = 0;

    /**
     * Create a path finding node from a point
     * 
     * @param point
     *        the 3d point
     */
    public PathFindingNode(float agentSize, Vector3f point)
    {
        this.point = point;
        gscore = 0;
        fscore = 0;
        depth = 0;
        cameFrom = null;
        neighbors = new LightWeightBoundedQueue<Integer>(MAX_NEIGHBOR);
        nodesToProcess = new ArrayDeque<PathFindingNode>();
        this.nodeID = nextNodeID++;
        this.agentSize = agentSize;
    }

    /**
     * Create a path finding node from a point and pass in the parent env-obj
     * 
     * @param point
     *        the 3d point
     * @param parentEnvObjID
     *        parent object ID
     */
    public PathFindingNode(float agentSize, Vector3f point, int parentEnvObjID)
    {
        this.point = point;
        gscore = 0;
        fscore = 0;
        cameFrom = null;
        neighbors = new LightWeightBoundedQueue<Integer>(MAX_NEIGHBOR);
        nodesToProcess = new ArrayDeque<PathFindingNode>();
        this.parentEnvObjID = parentEnvObjID;
        this.nodeID = nextNodeID++;
        this.agentSize = agentSize;
    }

    /**
     * Get the node's point
     * 
     * @return the 3d point
     */
    public Vector3f getPoint()
    {
        return point;
    }

    /**
     * Set the nodes point.
     * 
     * @param point
     *        the 3d point
     */
    public void setPoint(Vector3f point)
    {
        this.point = point;
    }

    /**
     * Get the G-score of the node (Astar)
     * 
     * @return the G-score float
     */
    public float getGscore()
    {
        return gscore;
    }

    /**
     * Set the G-score of the node (Astar)
     * 
     * @param gscore
     *        the G-score float
     */
    public void setGscore(float gscore)
    {
        this.gscore = gscore;
    }

    /**
     * Get the F-score of the node (Astar)
     * 
     * @return the F-score float
     */
    public float getFscore()
    {
        return fscore;
    }

    /**
     * Set the F-score of the node (Astar)
     * 
     * @param fscore
     *        the F-score float
     */
    public void setFscore(float fscore)
    {
        this.fscore = fscore;
    }

    /**
     * Get the parent node (Astar)
     * 
     * @return parent node
     */
    public PathFindingNode getCameFrom()
    {
        return cameFrom;
    }

    /**
     * Set the parent node (Astar)
     * 
     * @param cameFrom
     *        parent node
     */
    public void setCameFrom(PathFindingNode cameFrom)
    {
        this.cameFrom = cameFrom;
    }

    /**
     * Calculate all the neighbors that this node has. Uses all other nodes to check if neighbor. Has a max distance. And has a build in queue of nodes to process
     * so that too much time each tick is not spent processing. (If many nodes are added at once this can cripple the simulation, so the queue and the maximum time processing prevents this
     * and makes sure the simulation runs smoothly.
     * 
     * @param allNodes
     *        all the nodes to check against
     * @param avoidList
     *        the env obs to check for obstruction
     * @param endTime
     *        The cut off time for testing
     * @param distance
     *        the max distance to check for nodes
     */
    public void calculateNeighbors(Collection<PathFindingNode> allNodes, List<EnvObjectKnowledgeStorageObject> avoidList, long endTime, int distance)
    {
        if(nodesToProcess.isEmpty())
        {
            nodesToProcess.addAll(allNodes);
        }
        PathFindingNode node;
        while(System.currentTimeMillis() <= endTime && (node = nodesToProcess.poll()) != null)
        {
            if(node.getNodeID() != this.getNodeID())
            {
                if(!neighbors.contains(node))
                {
                    if(node.getPoint().distance(point) < distance)
                    {
                        boolean neighbor = true;
                        for(EnvObjectKnowledgeStorageObject obj : avoidList)
                        {
                            if(obj.isCollidable() && testIntersectsObject(node, obj))
                            {
                                neighbor = false;
                                break;
                            }
                        }
                        if(neighbor == true)
                        {
                            neighbors.add(node.getNodeID());
                            node.addNeighbor(this.nodeID);
                        }
                    }
                }
            }
        }
    }

    /**
     * Calculate all the neighbors that this node has. Same as generic method but this one is made specificly for checking ONLY the goal node. (not normal nodes)
     * Has no queue since only one node.
     * 
     * @param node
     *        all the nodes to check against
     * @param avoidList
     *        the env obs to check for obstruction
     */
    public void calculateNeighborsGoal(PathFindingNode node, List<EnvObjectKnowledgeStorageObject> avoidList)
    {

        if(node != this)
        {
            boolean neighbor = true;
            for(EnvObjectKnowledgeStorageObject obj : avoidList)
            {
                if(obj.isCollidable() && testIntersectsObject(node, obj))
                {
                    neighbor = false;
                    break;
                }
            }
            if(neighbor == true)
            {
                connectedToGoal = true;
            }
        }
    }

    /**
     * Get all neighbors to this node
     * 
     * @return list of neighbors
     */
    public LightWeightBoundedQueue<Integer> getNeighbors()
    {
        return neighbors;
    }

    /**
     * clear the neighbor list
     */
    public void clearNeighbors()
    {
        neighbors.clear();
    }

    /**
     * Add a new neighbor to this node
     * 
     * @param newNeighbor
     *        the neighbor to be added
     */
    public void addNeighbor(int newNeighbor)
    {
        neighbors.add(newNeighbor);
    }

    /**
     * Calculate all the neighbors that this node has. Same as generic method but this one is made specificly for checking ONLY the start node. (not normal nodes)
     * Has no queue since only one node.
     * 
     * @param allNodes
     *        all the nodes to check against
     * @param envObjects
     *        the env obs to check for obstruction
     * @param distance
     *        the max distance to check for nodes
     */
    public void calculateStartNeighbors(Collection<PathFindingNode> allNodes, List<EnvObjectKnowledgeStorageObject> envObjects, int distance, PathFindingNode goalNode)
    {
        for(PathFindingNode node : allNodes)
        {
            if(node.getPoint().distance(point) < distance)
            {
                boolean neighbor = true;
                for(EnvObjectKnowledgeStorageObject obj : envObjects)
                {
                    if(obj.isCollidable() && testIntersectsObject(node, obj))
                    {
                        neighbor = false;
                        break;
                    }
                }

                if(neighbor == true)
                {
                    neighbors.add(node.getNodeID());
                }
            }
        }
        PathFindingNode node = goalNode;
        if(node.getPoint().distance(point) < distance)
        {
            boolean neighbor = true;
            for(EnvObjectKnowledgeStorageObject obj : envObjects)
            {
                if(obj.isCollidable() && testIntersectsObject(node, obj))
                {
                    neighbor = false;
                    break;
                }
            }

            if(neighbor == true)
            {
                connectedToGoal = true;
            }
        }
    }

    private boolean testIntersectsObject(PathFindingNode node, EnvObjectKnowledgeStorageObject obj)
    {
        return obj.agentPathIntersectsObj2D(node.getPoint(), point, agentSize);
    }

    /**
     * Get the depth if this node (Astar)
     * 
     * @return integer depth
     */
    public int getDepth()
    {
        return depth;
    }

    /**
     * Set the depth of this node (Astar)
     * 
     * @param depth
     *        the integer depth
     */
    public void setDepth(int depth)
    {
        this.depth = depth;
    }

    /**
     * Whether the node is closed or not. (Astar)
     * 
     * @return true if closed, false otherwise
     */
    public boolean isClosed()
    {
        return closed;
    }

    /**
     * Set whether this node is closed or not. (Astar)
     * 
     * @param closed
     *        true if closed, false if not
     */
    public void setClosed(boolean closed)
    {
        this.closed = closed;
    }

    /**
     * Check whether this is a new node or not. (Astar)
     * 
     * @return true if new, false otherwise
     */
    public boolean isNewNode()
    {
        return newNode;
    }

    /**
     * Set whether this is a new node or not. (Astar)
     * 
     * @param newNode
     *        true if new, false otherwise
     */
    public void setNewNode(boolean newNode)
    {
        this.newNode = newNode;
    }

    /**
     * Get the parent objects ID
     * 
     * @return the parent object's ID (integer)
     */
    public int getParentEnvObjID()
    {
        return parentEnvObjID;
    }

    /**
     * Set the parent object's iD
     * 
     * @param parentEnvObjID
     *        the parent object's ID (integer)
     */
    public void setParentEnvObjID(int parentEnvObjID)
    {
        this.parentEnvObjID = parentEnvObjID;
    }

    public int getNodeID()
    {
        return nodeID;
    }

    public void setNodeID(int nodeID)
    {
        this.nodeID = nodeID;
    }

    public boolean isConnectedToGoal()
    {
        return connectedToGoal;
    }

    public void setConnectedToGoal(boolean connectedToGoal)
    {
        this.connectedToGoal = connectedToGoal;
    }

}

To implement the “PlanEvaluator” class:

• • Create a new class inside the “planning” package and name it “PlanEvaluator”.

• Copy the code given below that describes the full implementation of a “PlanEvaluator” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import java.io.Serializable;

import edu.utdallas.mavs.divas.core.sim.agent.knowledge.HumanKnowledgeModule;

/**
 * An Agent's Plan Evaluator
 */
public class PlanEvaluator<P extends EvacuationPlan> implements Serializable
{
    private static final long serialVersionUID = -8249907719082180498L;

    /**
     * The agent's knowledge module
     */
    HumanKnowledgeModule      knowledgeModule;

    /**
     * Create the plan evaluator
     * 
     * @param km
     *        the agent's knowledge module
     */
    public PlanEvaluator(HumanKnowledgeModule km)
    {
        knowledgeModule = km;
    }

    /**
     * Evaluate and assign a utility to the plan
     * 
     * @param plan
     *        the plan
     */
    public void evaluatePlan(P plan)
    {
        float utility = 0;

        utility = knowledgeModule.getTime();
        plan.setPlanUtility(utility);
    }
}

To implement the “PlanStorageModule” class:

• • Create a new class inside the “planning” package and name it “PlanStorageModule”.

• Copy the code given below that describes the full implementation of a “PlanStorageModule” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import java.io.Serializable;
import java.util.ArrayList;
import java.util.List;

/**
 * The module that stores and manages an agent's plan.
 */
public class PlanStorageModule<P extends EvacuationPlan> implements Serializable
{
    private static final long serialVersionUID = 1L;

    /**
     * A list of plans.
     */
    protected List<P>         plans;
    /**
     * The best plan
     */
    protected P               bestPlan;

    /**
     * Create a new plan storage module
     */
    @SuppressWarnings("unchecked")
    public PlanStorageModule()
    {
        plans = new ArrayList<P>();
        bestPlan = (P) new EvacuationPlan();
    }

    /**
     * Get the best plan.
     * 
     * @return the best plan
     */
    public P getBestPlan()
    {
        return bestPlan;
    }

    /**
     * Set the best plan
     * 
     * @param bestPlan
     *        the best plan
     */
    public void setBestPlan(P bestPlan)
    {
        this.bestPlan = bestPlan;
    }

    /**
     * Reset the plan module.
     */
    public void clear()
    {
        plans.clear();
    }

    /**
     * Add a plan to the planning module and redefine the best plan if needed.
     * 
     * @param plan
     *        the plan to be added
     */
    public void addPlan(P plan)
    {
        plans.add(plan);
        if(plan.getPlanUtility() > bestPlan.getPlanUtility())
        {
            bestPlan = plan;
        }
    }
}

To implement the “RVO_Utils” class:

• • Create a new class inside the “planning” package and name it “RVO_Utils”.

• Copy the code given below that describes the full implementation of a “RVO_Utils” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import java.io.Serializable;

import com.jme3.math.FastMath;
import com.jme3.math.Vector2f;

public class RVO_Utils implements Serializable
{
    private static final long serialVersionUID = 1L;

    public static final float EPSILON          = 0.0001f;

    public static boolean doLinesegmentsIntersect(float[] line1, float[] line2)
    {
        final float EPS = 0.00001f;

        float x1 = line1[0];
        float x2 = line1[2];
        float x3 = line2[0];
        float x4 = line2[2];

        float y1 = line1[1];
        float y2 = line1[3];
        float y3 = line2[1];
        float y4 = line2[3];

        float denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
        float numera = (x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3);
        float numerb = (x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3);

        // are the line coincident?
        if(Math.abs(numera) < EPS && Math.abs(numerb) < EPS && Math.abs(denom) < EPS)
        {
            return true;
        }

        // are the lines parallel?
        if(Math.abs(denom) < EPS)
        {
            return false;
        }

        // is the intersection along the segment?
        float mua = numera / denom;
        float mub = numerb / denom;

        if(mua < 0 || mua > 1 || mub < 0 || mub > 1)
        {
            return false;
        }

        return true;
    }

    // returns the cos(theta) of the two vectors
    public static float sameDirection(Vector2f v1, Vector2f v2)
    {
        return v1.dot(v2) / (v1.length() * v2.length()); // 0 - pi
    }

    /**
     * returns angle from v1 to v2
     * 
     * @param v1
     * @param v2
     * @return angle between v1 and v2 in 0-pi
     */
    public static float angleBetween(Vector2f v1, Vector2f v2)
    {
        return FastMath.acos(sameDirection(v1, v2)); // 0-pi
    }

    /**
     * @param v1
     * @param v2
     * @return an angle from v1 to v2 in -pi to pi
     */
    public static float angleBetweenWSign(Vector2f v1, Vector2f v2)
    {
        return FastMath.atan2(v2.y, v2.x) - FastMath.atan2(v1.y, v1.x); // -pi to pi
    }

    public static float det(Vector2f a, Vector2f b)
    {
        return a.getX() * b.getY() - a.getY() * b.getX();
    }

    public static boolean lineSegmentIntersectionTest(Vector2f p1, Vector2f p2, Vector2f p3, Vector2f p4)
    {
        float x1 = p1.getX();
        float x2 = p2.getX();
        float x3 = p3.getX();
        float x4 = p4.getX();
        float y1 = p1.getY();
        float y2 = p2.getY();
        float y3 = p3.getY();
        float y4 = p4.getY();

        float denom = ((y4 - y3) * (x2 - x1)) - ((x4 - x3) * (y2 - y1));
        if(denom == 0)
        {
            return false;
        }
        else
        {
            float num1 = ((x4 - x3) * (y1 - y3)) - ((y4 - y3) * (x1 - x3));
            float num2 = ((x2 - x1) * (y1 - y3)) - ((y2 - y1) * (x1 - x3));
            float ua = num1 / denom;
            float ub = num2 / denom;
            if(Double.compare(ua, -EPSILON) > 0 && Double.compare(ua - 1, EPSILON) < 0 && Double.compare(ub, -EPSILON) > 0 && Double.compare(ub - 1, EPSILON) < 0)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

    }

    public static float calcDistanceToLineSegment(Vector2f p1, Vector2f p2, Vector2f p3)
    {

        final float xDelta = p2.getX() - p1.getX();
        final float yDelta = p2.getY() - p1.getY();

        if((xDelta == 0) && (yDelta == 0))
        {
            throw new IllegalArgumentException("p1 and p2 cannot be the same point");
        }

        final float u = ((p3.getX() - p1.getX()) * xDelta + (p3.getY() - p1.getY()) * yDelta) / (xDelta * xDelta + yDelta * yDelta);

        final Vector2f closestPoint;
        if(u < 0)
        {
            closestPoint = p1;
        }
        else if(u > 1)
        {
            closestPoint = p2;
        }
        else
        {
            closestPoint = new Vector2f(p1.getX() + u * xDelta, p1.getY() + u * yDelta);
        }

        return closestPoint.distance(p3);
    }

    public static float absSq(Vector2f bMinusA)
    {
        return bMinusA.dot(bMinusA);
    }

    public static boolean leftOf(Vector2f a, Vector2f b, Vector2f c)
    {
        Vector2f aMinusC = new Vector2f(a);
        aMinusC.subtractLocal(c);

        Vector2f bMinusA = new Vector2f(b);
        bMinusA.subtractLocal(a);

        if(Double.compare(RVO_Utils.det(aMinusC, bMinusA), 0.0f) > 0)
        {

            return true;
        }
        else
        {

            return false;
        }
    }
}

To implement the “RVOObstacle” class:

• • Create a new class inside the “planning” package and name it “RVOObstacle”.

• Copy the code given below that describes the full implementation of a “RVOObstacle” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import java.io.Serializable;

import com.jme3.math.Vector2f;

public class RVOObstacle implements Serializable, Comparable<RVOObstacle>
{
    private static final long serialVersionUID = 1L;

    public RVOObstacle(Vector2f point)
    {
        super();
        this.point = point;
    }

    public RVOObstacle(Vector2f point, Vector2f unitdir)
    {
        super();
        this.point = point;
        this.unitDir = unitdir;
    }

    public RVOObstacle(Vector2f point, RVOObstacle prevObstacle, RVOObstacle nextObstacle)
    {
        super();
        this.point = point;
        this.prevObstacle = prevObstacle;
        this.nextObstacle = nextObstacle;
    }

    public RVOObstacle(Vector2f point, Vector2f unitdir, RVOObstacle prevObstacle, RVOObstacle nextObstacle)
    {
        super();
        this.point = point;
        this.unitDir = unitdir;
        this.prevObstacle = prevObstacle;
        this.nextObstacle = nextObstacle;
    }

    private float       distance;
    private Vector2f    point;
    private Vector2f    unitDir;
    private RVOObstacle prevObstacle;
    private RVOObstacle nextObstacle;
    private int         ID;

    public int getID()
    {
        return ID;
    }

    public void setID(int iD)
    {
        ID = iD;
    }

    public Vector2f getPoint()
    {
        return point;
    }

    public void setPoint(Vector2f point)
    {
        this.point = point;
    }

    public Vector2f getUnitdir()
    {
        return unitDir;
    }

    public void setUnitdir(Vector2f unitdir)
    {
        this.unitDir = unitdir;
    }

    public RVOObstacle getPrevObstacle()
    {
        return prevObstacle;
    }

    public void setPrevObstacle(RVOObstacle prevObstacle)
    {
        this.prevObstacle = prevObstacle;
    }

    public RVOObstacle getNextObstacle()
    {
        return nextObstacle;
    }

    public void setNextObstacle(RVOObstacle nextObstacle)
    {
        this.nextObstacle = nextObstacle;
    }

    public void updateUnitVector()
    {
        unitDir = new Vector2f(nextObstacle.getPoint());
        unitDir.subtractLocal(point);
        unitDir.normalizeLocal();
    }

    public float getDistance()
    {
        return distance;
    }

    public void setDistance(float distance)
    {
        this.distance = distance;
    }

    @Override
    public int compareTo(RVOObstacle o)
    {
        Float f0 = this.distance;
        Float f1 = o.distance;
        return f0.compareTo(f1);
    }

}

To implement the “RVOPathFinder” class:

• • Create a new class inside the “planning” package and name it “RVOPathFinder”.

• Copy the code given below that describes the full implementation of a “RVOPathFinder” class.

package edu.utdallas.mavs.evacuation.simulation.sim.agent.planning;

import java.io.Serializable;
import java.util.ArrayList;
import java.util.Collection;
import java.util.List;

import com.jme3.math.FastMath;
import com.jme3.math.Vector2f;
import com.jme3.math.Vector3f;

import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.EvacuationHumanKnowledgeModule;
import edu.utdallas.mavs.evacuation.simulation.sim.agent.knowledge.external.NeighborKnowledgeStorageObject;

public class RVOPathFinder implements Serializable
{
	private static final long serialVersionUID = 1L;
	
	static float                   RVO_EPSILON           = 0.00001f;
    EvacuationHumanKnowledgeModule knowledgeModule;
    /**
     * Stores the orcalines for calculation
     */
    List<OrcaLine>                 orcaLines;

    private static final float     TIMESTEP              = 1.0f;
    public static float            TIME_HORIZON          = 8.0f;
    public static float            TIME_HORIZON_OBSTACLE = 5.0f;

    public RVOPathFinder(EvacuationHumanKnowledgeModule knowledgeModule)
    {
        orcaLines = new ArrayList<>();
        this.knowledgeModule = knowledgeModule;
    }

    public Vector3f getGoodHeading(Vector2f waypoint)
    {

        // prepare env
        final float invTimeHorizonObst = 1.0f / TIME_HORIZON_OBSTACLE;
        final float invTimeHorizon = 1.0f / TIME_HORIZON;

        Vector2f preferredVelocity = new Vector2f(waypoint.x, waypoint.y);
        preferredVelocity.subtractLocal(knowledgeModule.getSelf().getPosition2D());
        Vector2f newVelocity = new Vector2f(preferredVelocity);
        preferredVelocity.normalizeLocal();
        preferredVelocity.multLocal(knowledgeModule.getSelf().getDesiredSpeed());

        Collection<NeighborKnowledgeStorageObject> neighbors = knowledgeModule.getNearbyAgents().values();
        List<RVOObstacle> obstacles = knowledgeModule.getPerceivedRVOObstacles();
        orcaLines.clear();

        // printAllObstacles(obstacles);

        /* Create obstacle ORCA lines. */
        processObstacles(obstacles, invTimeHorizonObst);

        final int numObstLines = orcaLines.size();

        // System.out.println("obsLines: " + numObstLines);

        /* Create agent ORCA lines. */
        processAgents(neighbors, invTimeHorizon);
        // These function should return the new velocity based on linear programming solution

        // System.out.println("AgentID: " + knowledgeModule.getSelf().getID() + " orig vel: " + newVelocity);

        return runLinearPrograms(preferredVelocity, newVelocity, numObstLines);

    }

    protected Vector3f runLinearPrograms(Vector2f preferredVelocity, Vector2f newVelocity, final int numObstLines)
    {
        int lineFail = linearProgram2(orcaLines, knowledgeModule.getSelf().getDesiredSpeed(), preferredVelocity, false, newVelocity);
        // System.out.println("AgentID: " + knowledgeModule.getSelf().getID() + " new vel: " + newVelocity + " #fail: " + lineFail + " orca lines: " + orcaLines.size());

        if(lineFail < orcaLines.size())
        {

            linearProgram3(orcaLines, numObstLines, lineFail, knowledgeModule.getSelf().getDesiredSpeed(), newVelocity);
        }
        // System.out.println("AgentID: " + knowledgeModule.getSelf().getID() + " final vel: " + newVelocity);
        // if(!newVelocity.equals(preferredVelocity))
        // {
        // System.out.println("AgentID: " + knowledgeModule.getSelf().getID() + " VELOCTY CHANGED");
        // }
        Vector3f vel = new Vector3f(newVelocity.x, 0, newVelocity.y);
        return vel;
    }

    protected void processAgents(Collection<NeighborKnowledgeStorageObject> neighbors, final float invTimeHorizon)
    {
        for(NeighborKnowledgeStorageObject otherAgent : neighbors)
        {

            if(otherAgent.getID() == knowledgeModule.getSelf().getID())
            {
                continue;
            }

            Vector2f relativePosition = new Vector2f(otherAgent.getPosition2D());
            relativePosition.subtractLocal(knowledgeModule.getSelf().getPosition2D());

            Vector2f relativeVelocity = new Vector2f(knowledgeModule.getSelf().getVelocity2D());
            relativeVelocity.subtractLocal(otherAgent.getVelocity2D());

            float distSq = relativePosition.dot(relativePosition);
            float combinedRadius = knowledgeModule.getSelf().getRadius() + otherAgent.getRadius();

            float combinedRadiusSq = FastMath.pow(combinedRadius, 2.0f);

            OrcaLine line = new OrcaLine();
            Vector2f u;

            if(distSq > combinedRadiusSq)
            {
                /* No collision. */
                Vector2f w = new Vector2f(relativePosition);
                w.multLocal(invTimeHorizon);
                w.subtractLocal(relativeVelocity);
                w.negateLocal();

                /* Vector from cutoff center to relative velocity. */
                final float wLengthSq = w.dot(w);

                final float dotProduct1 = w.dot(relativePosition);

                if(dotProduct1 < 0.0f && FastMath.pow(dotProduct1, 2.0f) > combinedRadiusSq * wLengthSq)
                {
                    /* Project on cut-off circle. */
                    final float wLength = FastMath.sqrt(wLengthSq);
                    Vector2f unitW = new Vector2f(w);
                    unitW.multLocal(1.0f / wLength);

                    line.direction = new Vector2f(unitW.getY(), -unitW.getX());
                    u = new Vector2f(unitW);
                    u.multLocal((combinedRadius * invTimeHorizon) - wLength);
                }
                else
                {
                    /* Project on legs. */

                    // final float LEG = ((distSq - combinedRadiusSq) > 0) ? Math.sqrt(distSq - combinedRadiusSq) : 0;
                    final float LEG = FastMath.sqrt(Math.abs(distSq - combinedRadiusSq));

                    if(RVO_Utils.det(relativePosition, w) > 0.0f)
                    {
                        /* Project on left LEG. */

                        line.direction = new Vector2f(relativePosition.getX() * LEG - relativePosition.getY() * combinedRadius, relativePosition.getX() * combinedRadius + relativePosition.getY() * LEG);
                        line.direction.multLocal(1.0f / distSq);
                    }
                    else
                    {
                        /* Project on right LEG. */

                        line.direction = new Vector2f(relativePosition.getX() * LEG + relativePosition.getY() * combinedRadius, -relativePosition.getX() * combinedRadius + relativePosition.getY() * LEG);
                        line.direction.multLocal(-1.0f / distSq);
                    }

                    final float dotProduct2 = relativeVelocity.dot(line.direction);
                    u = new Vector2f(line.direction);
                    u.multLocal(dotProduct2);
                    u.subtractLocal(relativeVelocity);

                }
            }
            else
            {
                /* Collision. */
                // //System.out.println("Collision!!!");

                final float invTimeStep = 1.0f / TIMESTEP;

                Vector2f w = new Vector2f(relativePosition);
                w.multLocal(invTimeStep);
                w.subtractLocal(relativeVelocity);

                w.negateLocal();

                float wLength = w.length();

                Vector2f unitW = new Vector2f(w);
                unitW.multLocal(1.0f / wLength);

                line.direction = new Vector2f(unitW.getY(), -unitW.getX());
                u = new Vector2f(unitW);
                u.multLocal((combinedRadius * invTimeStep) - wLength);

            }
            Vector2f newU = new Vector2f(u);
            newU.multLocal(0.5f);
            newU.addLocal(knowledgeModule.getSelf().getVelocity2D());

            line.point = new Vector2f(newU);
            assert Math.abs(line.direction.length() - 1.0) < RVO_EPSILON;
            orcaLines.add(line);

        }
    }

    protected void processObstacles(List<RVOObstacle> obstacles, final float invTimeHorizonObst)
    {
        for(RVOObstacle obstacleFromList : obstacles)
        {

            RVOObstacle obstacle1 = obstacleFromList;
            RVOObstacle obstacle2 = obstacleFromList.getNextObstacle();

            // System.out.println("obs1dist: " + obstacle1.getPoint().distance(knowledgeModule.getSelf().getPosition2D()));
            // System.out.println("obs2dist: " + obstacle2.getPoint().distance(knowledgeModule.getSelf().getPosition2D()));

            // System.out.println("for agent at " + knowledgeModule.getSelf().getPosition2D() + " Avoiding obstacle from " + obstacle1 + " to " + obstacle2);

            Vector2f relativePosition1 = new Vector2f(obstacle1.getPoint());
            relativePosition1.subtractLocal(knowledgeModule.getSelf().getPosition2D());

            Vector2f relativePosition2 = new Vector2f(obstacle2.getPoint());
            relativePosition2.subtractLocal(knowledgeModule.getSelf().getPosition2D());

            Vector2f obstacleVector = new Vector2f(obstacle2.getPoint());
            obstacleVector.subtractLocal(obstacle1.getPoint());

            /*
             * Check if velocity obstacle of obstacle is already taken care of by
             * previously constructed obstacle ORCA lines.
             */
            boolean alreadyCovered = false;

            for(int j = 0; j < orcaLines.size(); ++j)
            {
                if(checkIfSafe(invTimeHorizonObst, relativePosition1, relativePosition2, orcaLines.get(j), knowledgeModule.getSelf().getRadius()))
                {
                    alreadyCovered = true;
                    // System.out.println("Covered");
                    break;
                }
            }

            if(alreadyCovered)
            {
                continue;
            }

            /* Not yet covered. Check for collisions. */

            float distSq1 = relativePosition1.dot(relativePosition1);

            float distSq2 = relativePosition2.dot(relativePosition2);

            float radiusSq = knowledgeModule.getSelf().getRadius() * knowledgeModule.getSelf().getRadius();

            Vector2f leftLegDirection, rightLegDirection;

            Vector2f negRelativePosition1 = new Vector2f(relativePosition1);
            negRelativePosition1.negateLocal();
            float s = (negRelativePosition1.dot(obstacleVector) / obstacleVector.dot(obstacleVector));
            Vector2f distSq = new Vector2f(obstacleVector);
            distSq.multLocal(-s);
            distSq.addLocal(negRelativePosition1);
            float distSqLine = distSq.dot(distSq);

            Vector2f negRelativePosition2 = new Vector2f(relativePosition2);
            negRelativePosition2.negateLocal();

            OrcaLine line = new OrcaLine();

            // System.out.println("s: " + s);
            // System.out.println("distSq1: " + distSq1);
            // System.out.println("distSq2: " + distSq2);
            // System.out.println("distSqLine: " + distSqLine);

            if(s < 0 && distSq1 <= radiusSq)
            {
                /* Collision with left vertex. Ignore if non-convex. */
                // System.out.println("Left Vertex Collision");

                line.point = new Vector2f(0, 0);

                line.direction = new Vector2f(-relativePosition1.y, relativePosition1.x);
                line.direction.normalizeLocal();
                addOrcaLine(obstacle1, obstacle2, line, "Adding from first");

                continue;

            }
            else if(s > 1 && distSq2 <= radiusSq)
            {
                /* Collision with right vertex. Ignore if non-convex */

                if(RVO_Utils.det(relativePosition2, obstacle2.getUnitdir()) >= 0)
                {

                    line.point = new Vector2f(0, 0);

                    line.direction = new Vector2f(-relativePosition2.y, relativePosition2.x);
                    line.direction.normalizeLocal();
                    addOrcaLine(obstacle1, obstacle2, line, "Adding from second");

                }
                continue;
            }
            else if(s >= 0 && s < 1 && distSqLine <= radiusSq)
            {
                /* Collision with obstacle segment. */

                // System.out.println("Collision with a segment");
                line.point = new Vector2f(0, 0);

                line.direction = new Vector2f(obstacle1.getUnitdir());
                line.direction.negateLocal();
                addOrcaLine(obstacle1, obstacle2, line, "Adding from third");
                continue;
            }
            /*
             * No collision
             * Compute legs. When obliquely viewed, both legs can come from a single
             * vertex. Legs extend cut-off line when nonconvex vertex.
             */
            // System.out.println("No collision");
            if(s < 0 && distSqLine <= radiusSq)
            {
                // System.out.println("oblique view from left");

                obstacle2 = obstacle1;

                // final float LEG1 = ((distSq1 - radiusSq) < 0) ? 0 : Math.sqrt(distSq1 - radiusSq);
                final float LEG1 = FastMath.sqrt(Math.abs(distSq1 - radiusSq));

                leftLegDirection = new Vector2f(relativePosition1.getX() * LEG1 - relativePosition1.getY() * knowledgeModule.getSelf().getRadius(), relativePosition1.getX() * knowledgeModule.getSelf().getRadius() + relativePosition1.getY() * LEG1);
                rightLegDirection = new Vector2f(relativePosition1.getX() * LEG1 + relativePosition1.getY() * knowledgeModule.getSelf().getRadius(), negRelativePosition1.getX() * knowledgeModule.getSelf().getRadius() + relativePosition1.getY()
                        * LEG1);
                leftLegDirection.multLocal(1.0f / distSq1);
                rightLegDirection.multLocal(1.0f / distSq1);
            }
            else if(s > 1 && distSqLine <= radiusSq)
            {
                /*
                 * RVO2Obstacle viewed obliquely so that
                 * right vertex defines velocity obstacle.
                 */
                // System.out.println("oblique view from right");

                obstacle1 = obstacle2;

                // final float LEG2 = ((distSq2 - radiusSq) < 0) ? 0 : Math.sqrt(distSq2 - radiusSq);
                final float LEG2 = FastMath.sqrt(Math.abs(distSq2 - radiusSq));
                leftLegDirection = new Vector2f(relativePosition2.getX() * LEG2 - relativePosition2.getY() * knowledgeModule.getSelf().getRadius(), relativePosition2.getX() * knowledgeModule.getSelf().getRadius() + relativePosition2.getY() * LEG2);
                rightLegDirection = new Vector2f(relativePosition2.getX() * LEG2 + relativePosition2.getY() * knowledgeModule.getSelf().getRadius(), negRelativePosition2.getX() * knowledgeModule.getSelf().getRadius() + relativePosition2.getY()
                        * LEG2);
                leftLegDirection.multLocal(1.0f / distSq2);
                rightLegDirection.multLocal(1.0f / distSq2);
            }
            else
            {
                /* Usual situation. */

                // System.out.println("The usual");

                final float LEG1 = FastMath.sqrt(FastMath.abs(distSq1 - radiusSq));
                leftLegDirection = new Vector2f(relativePosition1.getX() * LEG1 - relativePosition1.getY() * knowledgeModule.getSelf().getRadius(), relativePosition1.getX() * knowledgeModule.getSelf().getRadius() + relativePosition1.getY() * LEG1);
                leftLegDirection.multLocal(1.0f / distSq1);

                final float LEG2 = FastMath.sqrt(Math.abs(distSq2 - radiusSq));
                rightLegDirection = new Vector2f(relativePosition2.getX() * LEG2 + relativePosition2.getY() * knowledgeModule.getSelf().getRadius(), negRelativePosition2.getX() * knowledgeModule.getSelf().getRadius() + relativePosition2.getY()
                        * LEG2);
                rightLegDirection.multLocal(1.0f / distSq2);

            }

            /*
             * Legs can never point into neighboring edge when convex vertex,
             * take cutoff-line of neighboring edge instead. If velocity projected on
             * "foreign" LEG, no constraint is added.
             */

            final RVOObstacle leftNeighbor = obstacle1.getPrevObstacle();

            boolean isLeftLegForeign = false;
            boolean isRightLegForeign = false;

            if(RVO_Utils.det(leftLegDirection, leftNeighbor.getUnitdir().negate()) >= 0.0f)
            {
                /* Left LEG points into obstacle. */
                // System.out.println("left leg into obstacle");

                leftLegDirection = new Vector2f(leftNeighbor.getUnitdir().negate());
                isLeftLegForeign = true;
            }

            if(RVO_Utils.det(rightLegDirection, obstacle2.getUnitdir()) <= 0.0f)
            {
                /* Right LEG points into obstacle. */
                // System.out.println("right leg into obstacle");
                rightLegDirection = new Vector2f(obstacle2.getUnitdir());
                isRightLegForeign = true;
            }

            /* Compute cut-off centers. */
            Vector2f vectorToObstacle1 = new Vector2f(obstacle1.getPoint());
            vectorToObstacle1.subtractLocal(knowledgeModule.getSelf().getPosition2D());
            vectorToObstacle1.multLocal(invTimeHorizonObst);
            final Vector2f LEFTCUTOFF = new Vector2f(vectorToObstacle1);

            Vector2f vectorToObstacle2 = new Vector2f(obstacle2.getPoint());
            vectorToObstacle2.subtractLocal(knowledgeModule.getSelf().getPosition2D());
            vectorToObstacle2.multLocal(invTimeHorizonObst);
            final Vector2f RIGHTCUTOFF = new Vector2f(vectorToObstacle2);

            Vector2f cutOffVecTemp = new Vector2f(RIGHTCUTOFF);
            cutOffVecTemp.subtractLocal(LEFTCUTOFF);
            final Vector2f CUTOFFVEC = new Vector2f(cutOffVecTemp);

            /* Project current velocity on velocity obstacle. */

            /* Check if current velocity is projected on cutoff circles. */

            Vector2f velocityMinusLeft = new Vector2f(knowledgeModule.getSelf().getVelocity2D());
            velocityMinusLeft.subtractLocal(LEFTCUTOFF);

            Vector2f velocityMinusRight = new Vector2f(knowledgeModule.getSelf().getVelocity2D());
            velocityMinusRight.subtractLocal(RIGHTCUTOFF);

            final float T = ((obstacle1.equals(obstacle2)) ? 0.5f : (velocityMinusLeft.dot(CUTOFFVEC) / CUTOFFVEC.dot(CUTOFFVEC)));
            final float TLEFT = (velocityMinusLeft.dot(leftLegDirection));
            final float TRIGHT = (velocityMinusRight.dot(rightLegDirection));

            // System.out.println("T: " + T);
            // System.out.println("TLEFT: " + TLEFT);
            // System.out.println("TRIGHT: " + TRIGHT);
            if((T < 0.0f && TLEFT < 0.0f) || (obstacle1.equals(obstacle2) && TLEFT < 0.0f && TRIGHT < 0.0f))
            {
                /* Project on left cut-off circle. */

                // System.out.println("Project on left cut off");

                Vector2f unitW = new Vector2f(velocityMinusLeft);
                unitW.normalizeLocal();

                line.direction = new Vector2f(unitW.getY(), -unitW.getX());
                unitW.multLocal(invTimeHorizonObst);
                unitW.multLocal(knowledgeModule.getSelf().getRadius());
                unitW.addLocal(LEFTCUTOFF);
                line.point = new Vector2f(unitW);
                addOrcaLine(obstacle1, obstacle2, line, "Adding from fourth");
                continue;
            }
            else if(T > 1.0f && TRIGHT < 0.0f)
            {
                /* Project on right cut-off circle. */
                // System.out.println("Project on righ cut off");
                Vector2f unitW = new Vector2f(velocityMinusRight);
                unitW.normalizeLocal();

                line.direction = new Vector2f(unitW.getY(), -unitW.getX());
                unitW.multLocal(invTimeHorizonObst);
                unitW.multLocal(knowledgeModule.getSelf().getRadius());
                unitW.addLocal(RIGHTCUTOFF);
                line.point = new Vector2f(unitW);
                addOrcaLine(obstacle1, obstacle2, line, "Adding from fifth");
                continue;

            }

            /*
             * Project on left LEG, right LEG, or cut-off line, whichever is closest
             * to velocity.
             */
            Vector2f vectorForCutOff = new Vector2f(CUTOFFVEC);
            vectorForCutOff.multLocal(T);
            vectorForCutOff.addLocal(LEFTCUTOFF);
            vectorForCutOff.negateLocal();
            vectorForCutOff.addLocal(knowledgeModule.getSelf().getVelocity2D());

            final float DISTSQCUTOFF = ((T < 0.0f || T > 1.0f || obstacle1.equals(obstacle2)) ? Float.MAX_VALUE : vectorForCutOff.dot(vectorForCutOff));

            Vector2f vectorForLeftCutOff = new Vector2f(leftLegDirection);
            vectorForLeftCutOff.multLocal(TLEFT);
            vectorForLeftCutOff.addLocal(LEFTCUTOFF);
            vectorForLeftCutOff.negateLocal();
            vectorForLeftCutOff.addLocal(knowledgeModule.getSelf().getVelocity2D());

            final float DISTSQLEFT = ((TLEFT < 0.0f) ? Float.MAX_VALUE : vectorForLeftCutOff.dot(vectorForLeftCutOff));

            Vector2f vectorForRightCutOff = new Vector2f(rightLegDirection);
            vectorForRightCutOff.multLocal(TRIGHT);
            vectorForRightCutOff.addLocal(RIGHTCUTOFF);
            vectorForRightCutOff.negateLocal();
            vectorForRightCutOff.addLocal(knowledgeModule.getSelf().getVelocity2D());

            final float DISTSQRIGHT = ((TRIGHT < 0.0f) ? Float.MAX_VALUE : vectorForRightCutOff.dot(vectorForRightCutOff));

            if(DISTSQCUTOFF <= DISTSQLEFT && DISTSQCUTOFF <= DISTSQRIGHT)
            {
                /* Project on cut-off line. */
                // System.out.println("Project on cut off");

                line.direction = new Vector2f(obstacle1.getUnitdir());

                line.direction.negateLocal();

                Vector2f vectorForPoint = new Vector2f(-line.direction.getY(), line.direction.getX());
                vectorForPoint.multLocal(invTimeHorizonObst);
                vectorForPoint.multLocal(knowledgeModule.getSelf().getRadius());
                vectorForPoint.addLocal(LEFTCUTOFF);
                line.point = new Vector2f(vectorForPoint);
                assert Math.abs(line.direction.length() - 1.0) < RVO_EPSILON;
                addOrcaLine(obstacle1, obstacle2, line, "Adding from sixth");
                continue;

            }
            else if(DISTSQLEFT <= DISTSQRIGHT)
            { /* Project on left LEG. */

                // System.out.println("Project on left leg");

                if(isLeftLegForeign)
                {
                    continue;
                }

                line.direction = new Vector2f(leftLegDirection);

                Vector2f vectorForPoint = new Vector2f(-line.direction.getY(), line.direction.getX());
                vectorForPoint.multLocal(invTimeHorizonObst);
                vectorForPoint.multLocal(knowledgeModule.getSelf().getRadius());
                vectorForPoint.addLocal(LEFTCUTOFF);
                line.point = new Vector2f(vectorForPoint);
                addOrcaLine(obstacle1, obstacle2, line, "Adding from seventh");
                continue;
            }
            else
            { /* Project on right LEG. */

                // System.out.println("Project on right leg");
                if(isRightLegForeign)
                {
                    continue;
                }

                line.direction = new Vector2f(rightLegDirection);
                line.direction.negateLocal();

                Vector2f vectorForPoint = new Vector2f(-line.direction.getY(), line.direction.getX());
                vectorForPoint.multLocal(invTimeHorizonObst);
                vectorForPoint.multLocal(knowledgeModule.getSelf().getRadius());
                vectorForPoint.addLocal(RIGHTCUTOFF);
                line.point = new Vector2f(vectorForPoint);
                addOrcaLine(obstacle1, obstacle2, line, "Adding from eigth");
                continue;
            }

        }
    }

    protected void printAllObstacles(List<RVOObstacle> obstacles)
    {
        int count = 1;
        for(RVOObstacle curObs : obstacles)
        {
            RVOObstacle temp = curObs;
            System.out.println("Obstacle: " + count);
            while(!temp.getNextObstacle().equals(curObs))
            {
                System.out.println(temp.getPoint());
                temp = temp.getNextObstacle();
            }
            System.out.println(temp.getPoint());
            count++;
        }

        count = 1;
    }

    protected void addOrcaLine(RVOObstacle obstacle1, RVOObstacle obstacle2, OrcaLine line, String out)
    {
        orcaLines.add(line);
        // System.out.println("obs1 point: " + obstacle1.getPoint());
        // System.out.println("obs2 point: " + obstacle2.getPoint());
        // System.out.println("obsID point: " + obstacle2.getID());
        // System.out.println(out);
        // System.out.println("orcaline point: " + line.point);
        // System.out.println("orcaline dir: " + line.direction);
    }

    private boolean checkIfSafe(float invTimeHorizonObst, Vector2f relativePosition1, Vector2f relativePosition2, OrcaLine line, float radius)
    {

        Vector2f a = new Vector2f(relativePosition1);
        a.multLocal(invTimeHorizonObst);
        a.subtractLocal(line.point);

        Vector2f b = new Vector2f(relativePosition2);
        b.multLocal(invTimeHorizonObst);
        b.subtractLocal(line.point);

        return((Float.compare((RVO_Utils.det(a, line.direction) - invTimeHorizonObst * radius), -RVO_EPSILON) >= 0) && (Float.compare((RVO_Utils.det(b, line.direction) - invTimeHorizonObst * radius), -RVO_EPSILON) >= 0));

    }

    boolean linearProgram1(List<OrcaLine> lines, int lineNo, float radius, Vector2f optVelocity, boolean directionOpt, Vector2f result)
    {

        Vector2f lineNoPoint = new Vector2f(lines.get(lineNo).point);
        Vector2f lineNoDirection = new Vector2f(lines.get(lineNo).direction);
        float dotProduct = lineNoPoint.dot(lineNoDirection);

        // final float detProduct = det(lines.get(lineNo).direction, lineNoPoint);
        // final float detProduct2 = lineNoPoint.dot(lineNoPoint);
        final float discriminant = FastMath.pow(dotProduct, 2.0f) + FastMath.pow(radius, 2.0f) - lineNoPoint.dot(lineNoPoint);

        if(Float.compare(discriminant, RVO_EPSILON) < 0)
        {
            /* Max speed circle fully invalidates line lineNo. */
            return false;
        }

        final float sqrtDiscriminant = FastMath.sqrt(discriminant);
        float tLeft = -(dotProduct) - sqrtDiscriminant;
        float tRight = -(dotProduct) + sqrtDiscriminant;

        for(int i = 0; i < lineNo; ++i)
        {
            final float denominator = RVO_Utils.det(lineNoDirection, lines.get(i).direction);
            Vector2f tempVector = new Vector2f(lineNoPoint);
            tempVector.subtractLocal(new Vector2f(lines.get(i).point));
            final float numerator = RVO_Utils.det(lines.get(i).direction, tempVector);

            if(Float.compare(Math.abs(denominator), RVO_EPSILON) <= 0)
            {
                /* Lines lineNo and i are (almost) parallel. */

                if(Float.compare(numerator, RVO_EPSILON) < 0)
                {
                    /* Line i fully invalidates line lineNo. */
                    return false;
                }
                else
                {
                    /* Line i does not impose constraint on line lineNo. */
                    continue;
                }
            }

            final float t = numerator / denominator;
            if(denominator >= 0)
            {
                /* Line i bounds line lineNo on the right. */
                tRight = Math.min(tRight, t);
            }
            else
            {
                /* Line i bounds line lineNo on the left. */
                tLeft = Math.max(tLeft, t);
            }

            if(tLeft > tRight)
            {
                return false;
            }
        }

        if(directionOpt)
        {
            /* Optimize direction. */
            Vector2f tempLineNoDirection = new Vector2f(lineNoDirection);
            if(Float.compare(optVelocity.dot(tempLineNoDirection), -RVO_EPSILON) > 0)
            {
                /* Take right extreme. */
                tempLineNoDirection.multLocal(tRight);
            }
            else
            {
                /* Take left extreme. */
                tempLineNoDirection.multLocal(tLeft);
            }
            tempLineNoDirection.addLocal(new Vector2f(lineNoPoint));
            result.x = tempLineNoDirection.x;
            result.y = tempLineNoDirection.y;
        }
        else
        {
            /* Optimize closest point. */
            Vector2f tempOptVector = new Vector2f(optVelocity);
            tempOptVector.subtractLocal(lineNoPoint);
            final float t = lineNoDirection.dot(tempOptVector);
            Vector2f tempLineNoDirection = new Vector2f(lineNoDirection);
            if(Float.compare(t, tLeft) < 0)
            {
                tempLineNoDirection.multLocal(tLeft);
            }
            else if(Float.compare(t, tRight) > 0)
            {
                tempLineNoDirection.multLocal(tRight);
            }
            else
            {
                tempLineNoDirection.multLocal(t);
            }
            tempLineNoDirection.addLocal(new Vector2f(lineNoPoint));
            result.x = tempLineNoDirection.x;
            result.y = tempLineNoDirection.y;

        }

        return true;
    }

    int linearProgram2(List<OrcaLine> lines, float radius, Vector2f optVelocity, boolean directionOpt, Vector2f result)
    {

        if(directionOpt)
        {
            /*
             * Optimize direction. Note that the optimization velocity is of unit
             * length in this case.
             */
            if(Float.compare(Math.abs(optVelocity.length() - 1), RVO_EPSILON) > 0)
            {
                // System.out.println("??" + optVelocity.length());
            }
            Vector2f tempOpt = new Vector2f(optVelocity);

            result.x = tempOpt.x;
            result.y = tempOpt.y;
            result.multLocal(radius);
        }
        else if(optVelocity.dot(optVelocity) > Math.pow(radius, 2.0f))
        {
            /* Optimize closest point and outside circle. */

            result.x = optVelocity.x;
            result.y = optVelocity.y;
            result.normalizeLocal();
            result.multLocal(radius);
        }
        else
        {
            /* Optimize closest point and inside circle. */

            result.x = optVelocity.x;
            result.y = optVelocity.y;
        }

        for(int i = 0; i < lines.size(); ++i)
        {

            Vector2f tempPoint = new Vector2f(lines.get(i).point);
            tempPoint.subtractLocal(new Vector2f(result));

            if(Float.compare(RVO_Utils.det(lines.get(i).direction, tempPoint), 0) > 0)
            {
                /* Result does not satisfy constraint i. Compute new optimal result. */
                Vector2f tempResult = new Vector2f(result);
                if(!linearProgram1(lines, i, radius, optVelocity, directionOpt, result))
                {
                    result.x = tempResult.x;
                    result.y = tempResult.y;
                    return i;
                }
            }
        }

        return lines.size();
    }

    void linearProgram3(List<OrcaLine> lines, int numObstLines, int beginLine, float radius, Vector2f result)
    {

        float distance = 0.0f;

        for(int i = beginLine; i < lines.size(); i++)
        {
            Vector2f tempPoint = new Vector2f(lines.get(i).point);
            tempPoint.subtractLocal(result);

            if(RVO_Utils.det(lines.get(i).direction, tempPoint) > distance)
            {
                /* Result does not satisfy constraint of line i. */
                List<OrcaLine> projLines = new ArrayList<>();
                for(int j = 0; j < numObstLines; j++)
                {
                    projLines.add(new OrcaLine(lines.get(j)));

                }

                for(int j = numObstLines; j < i; j++)
                {
                    OrcaLine line = new OrcaLine();

                    float determinant = RVO_Utils.det(lines.get(i).direction, lines.get(j).direction);
                    if(Float.compare(Math.abs(determinant), RVO_EPSILON) <= 0)
                    {
                        /* Line i and line j are (almost) parallel. */
                        if(Float.compare(lines.get(i).direction.dot(lines.get(j).direction), -RVO_EPSILON) > 0)
                        {
                            /* Line i and line j point in the same direction. */
                            continue;
                        }
                        else
                        {
                            /* Line i and line j point in opposite direction. */
                            line.point = new Vector2f(lines.get(j).point);
                            line.point.addLocal(lines.get(i).point);
                            line.point.multLocal(0.5f);

                        }
                    }
                    else
                    {

                        Vector2f tempVector = new Vector2f(lines.get(i).point);
                        tempVector.subtractLocal(new Vector2f(lines.get(j).point));
                        Vector2f newTempVector = new Vector2f(lines.get(i).direction);
                        newTempVector.multLocal(RVO_Utils.det(lines.get(j).direction, tempVector) / determinant);

                        line.point = new Vector2f(lines.get(i).point);
                        line.point.addLocal(newTempVector);

                    }
                    line.direction = new Vector2f(lines.get(j).direction);
                    line.direction.subtractLocal(lines.get(i).direction);
                    line.direction.normalizeLocal();

                    projLines.add(line);
                }

                final Vector2f tempResult = new Vector2f(result);

                if(linearProgram2(projLines, radius, new Vector2f(-lines.get(i).direction.y, lines.get(i).direction.x), true, result) < projLines.size())
                {
                    /*
                     * This should in principle not happen. The result is by definition
                     * already in the feasible region of this linear program. If it fails,
                     * it is due to small floating point error, and the current result is
                     * kept.
                     */
                    //
                    result.x = tempResult.x;
                    result.y = tempResult.y;

                    // result.x = 0.0f;
                    // result.y = 0.0f;

                }

                Vector2f tempVector = new Vector2f(lines.get(i).point);
                tempVector.subtractLocal(result);
                distance = RVO_Utils.det(lines.get(i).direction, tempVector);
            }
        }
    }
}