/** Adds an obstacle with the specified vertices.
* The vertices array might be changed by this function. */
private void AddObstacle(Vector3[] vertices, float height, Pathfinding.RVO.Simulator sim, ref List <ObstacleVertex> obstacles)
{
if (vertices == null)
{
throw new System.ArgumentNullException("Vertices Must Not Be Null");
}
if (height < 0)
{
throw new System.ArgumentOutOfRangeException("Height must be non-negative");
}
if (vertices.Length < 2)
{
throw new System.ArgumentException("An obstacle must have at least two vertices");
}
if (null == obstacles)
{
obstacles = new List <ObstacleVertex>();
}
obstacles.Clear();
if (vertices.Length > 2)
{
WindCorrectly(vertices);
}
for (int vert = 0; vert < vertices.Length; ++vert)
{
Vector3 v1 = vertices[vert];
Vector3 v2 = vertices[(vert + 1) % vertices.Length];
Vector3 dir = v2 - v1;
Vector3 pushAwayDir = new Vector3(dir.z, 0f, -dir.x);
obstacles.Add(sim.CreateObstacle(v1, v2, pushAwayDir, height, _isActive));
}
}
public override void OnLatePostScan()
{
if (!Application.isPlaying)
{
return;
}
RemoveObstacles();
NavGraph[] graphs = AstarPath.active.graphs;
RVOSimulator rvosim = FindObjectOfType(typeof(RVOSimulator)) as RVOSimulator;
if (rvosim == null)
{
throw new System.NullReferenceException("No RVOSimulator could be found in the scene. Please add one to any GameObject");
}
Pathfinding.RVO.Simulator sim = rvosim.GetSimulator();
for (int i = 0; i < graphs.Length; i++)
{
AddGraphObstacles(sim, graphs[i]);
}
sim.UpdateObstacles();
}
/** Removes obstacles which were added with AddGraphObstacles */
public void RemoveObstacles()
{
#if BNICKSON_UPDATED
if (lastSim != null)
{
// if our usage of APP changes, this will throw an error
throw new System.Exception("This section of A Star Pathfinding code should never be executed");
}
#else
if (lastSim == null)
{
return;
}
Pathfinding.RVO.Simulator sim = lastSim;
lastSim = null;
for (int i = 0; i < obstacles.Count; i++)
{
sim.RemoveObstacle(obstacles[i]);
}
obstacles.Clear();
#endif
}
private Pathfinding.RVO.Simulator GetSimulator()
{
if (null == _sim)
{
_sim = AStarPathfindingUtils.GetSimulator();
}
return(_sim);
}
// we want dynamic obstacles, but only if we do not have any static obstacles
public void CreateDynamicEdgesIfNoStaticEdgesExist(Pathfinding.RVO.Simulator sim)
{
if (null != _staticObstacles && _staticObstacles.Count > 0)
{
return;
}
_staticObstacles = null;
RemoveDynamicObstaclesAndCreateNew(sim);
}
/** Removes obstacles which were added with AddGraphObstacles */
public void RemoveObstacles () {
if (lastSim == null) return;
Pathfinding.RVO.Simulator sim = lastSim;
lastSim = null;
for (int i = 0; i < obstacles.Count; i++) sim.RemoveObstacle(obstacles[i]);
obstacles.Clear();
}
void Awake () {
if (desiredSimulatonFPS < 1) desiredSimulatonFPS = 1;
if (simulator == null) {
int threadCount = AstarPath.CalculateThreadCount (workerThreads);
simulator = new Pathfinding.RVO.Simulator (threadCount, doubleBuffering);
simulator.Interpolation = interpolation;
simulator.DesiredDeltaTime = 1.0f / desiredSimulatonFPS;
}
}
// the obstacle has move
public void Move(Pathfinding.RVO.Simulator sim)
{
if (null != _staticObstacles)
{
SetObstaclesActive(sim, false);
_staticObstacles = null;
}
RemoveDynamicObstaclesAndCreateNew(sim);
}
// destroy the static obstacles we already have
private void RemoveStaticObstacles(Pathfinding.RVO.Simulator sim)
{
if (null != _staticObstacles)
{
for (int i = 0; i < _staticObstacles.Count; i++)
{
sim.RemoveObstacle(_staticObstacles[i]);
}
_staticObstacles.Clear();
}
}
/** Finds a simulator in the scene.
*
* Saves found simulator in #sim.
*
* \throws System.InvalidOperationException When no RVOSimulator could be found.
*/
protected void FindSimulator()
{
RVOSimulator rvosim = FindObjectOfType(typeof(RVOSimulator)) as RVOSimulator;
if (rvosim == null)
{
throw new System.InvalidOperationException("No RVOSimulator could be found in the scene. Please add one to any GameObject");
}
sim = rvosim.GetSimulator();
}
// Use this for initialization
void Start()
{
//Get the simulator for this scene
Pathfinding.RVO.Simulator sim = (FindObjectOfType(typeof(Pathfinding.RVO.RVOSimulator)) as Pathfinding.RVO.RVOSimulator).GetSimulator();
//Define the vertices of our obstacle
Vector3[] verts = new Vector3[] { new Vector3(1, 0, -1), new Vector3(1, 0, 1), new Vector3(-1, 0, 1), new Vector3(-1, 0, -1) };
//Add our obstacle to the simulation, we set the height to 2 units
sim.AddObstacle(verts, 2);
}
public void Start () {
mesh = new Mesh();
RVOSimulator rvoSim = FindObjectOfType (typeof(RVOSimulator)) as RVOSimulator;
if (rvoSim == null) {
Debug.LogError ("No RVOSimulator could be found in the scene. Please add a RVOSimulator component to any GameObject");
return;
}
sim = rvoSim.GetSimulator();
GetComponent<MeshFilter>().mesh = mesh;
CreateAgents (agentCount);
}
// remove old abstacles and create new ones
private void RemoveStaticObstaclesAndCreateNew(Pathfinding.RVO.Simulator sim)
{
RemoveStaticObstacles(sim);
CreateObstacles(sim, ref _staticObstacles);
if (null != _staticObstacles)
{
for (int i = 0; i < _staticObstacles.Count; i++)
{
sim.AddObstacle(_staticObstacles[i]);
}
}
}
private void Awake()
{
if (this.desiredSimulationFPS < 1)
{
this.desiredSimulationFPS = 1;
}
if (this.simulator == null)
{
int workers = AstarPath.CalculateThreadCount(this.workerThreads);
this.simulator = new Simulator(workers, this.doubleBuffering);
this.simulator.Interpolation = this.interpolation;
this.simulator.DesiredDeltaTime = 1f / (float)this.desiredSimulationFPS;
}
}
void Awake () {
if (desiredSimulatonFPS < 1) desiredSimulatonFPS = 1;
if (simulator == null) {
int threadCount = AstarPath.CalculateThreadCount (workerThreads);
simulator = new Pathfinding.RVO.Simulator (threadCount, doubleBuffering);
simulator.Interpolation = interpolation;
simulator.DesiredDeltaTime = 1.0f / desiredSimulatonFPS;
}
Debug.LogWarning ("RVO Local Avoidance is temporarily disabled in the A* Pathfinding Project due to licensing issues.\n" +
"I am working to get it back as soon as possible. All agents will fall back to not avoiding other agents.\n" +
"Sorry for the inconvenience.");
}
// destroy the dynamic obstacles we already have
public void RemoveDynamicObstacles(Pathfinding.RVO.Simulator sim)
{
if (null != _dynamicObstacles)
{
if (null != sim)
{
for (int i = 0; i < _dynamicObstacles.Count; i++)
{
sim.RemoveDynamicObstacle(_dynamicObstacles[i]);
}
}
_dynamicObstacles.Clear();
}
}
public void Start()
{
mesh = new Mesh();
RVOSimulator rvoSim = FindObjectOfType(typeof(RVOSimulator)) as RVOSimulator;
if (rvoSim == null)
{
Debug.LogError("No RVOSimulator could be found in the scene. Please add a RVOSimulator component to any GameObject");
return;
}
sim = rvoSim.GetSimulator();
GetComponent <MeshFilter>().mesh = mesh;
CreateAgents(agentCount);
}
void Awake()
{
if (desiredSimulatonFPS < 1)
{
desiredSimulatonFPS = 1;
}
if (simulator == null)
{
int threadCount = AstarPath.CalculateThreadCount(workerThreads);
simulator = new Pathfinding.RVO.Simulator(threadCount, doubleBuffering);
simulator.Interpolation = interpolation;
simulator.DesiredDeltaTime = 1.0f / desiredSimulatonFPS;
}
}
/** Removes obstacles which were added with AddGraphObstacles */
public void RemoveObstacles()
{
if (lastSim == null)
{
return;
}
Pathfinding.RVO.Simulator sim = lastSim;
lastSim = null;
for (int i = 0; i < obstacles.Count; i++)
{
sim.RemoveObstacle(obstacles[i]);
}
obstacles.Clear();
}
void Awake()
{
if (desiredSimulatonFPS < 1)
{
desiredSimulatonFPS = 1;
}
if (simulator == null)
{
int threadCount = AstarPath.CalculateThreadCount(workerThreads);
simulator = new Pathfinding.RVO.Simulator(threadCount, doubleBuffering);
simulator.Interpolation = interpolation;
simulator.DesiredDeltaTime = 1.0f / desiredSimulatonFPS;
}
/*Debug.LogWarning ("RVO Local Avoidance is temporarily disabled in the A* Pathfinding Project due to licensing issues.\n" +
* "I am working to get it back as soon as possible. All agents will fall back to not avoiding other agents.\n" +
* "Sorry for the inconvenience.");*/
}
// Use this for initialization
public void Start()
{
#if RVOImp
if (!astarRVO)
{
//#if !AstarRelease
agentID = RVO.Simulator.Instance.addAgent(new RVO.Vector2(transform.position.x, transform.position.z));
//#endif
}
else
{
Pathfinding.RVO.Simulator sim = (FindObjectOfType(typeof(RVOSimulator)) as RVOSimulator).GetSimulator();
rvoAgent = sim.AddAgent(transform.position);
rvoAgent.Radius = radius;
rvoAgent.MaxSpeed = maxSpeed;
rvoAgent.Height = height;
rvoAgent.AgentTimeHorizon = agentTimeHorizon;
rvoAgent.ObstacleTimeHorizon = obstacleTimeHorizon;
}
#endif
SetTarget(-transform.position); // + transform.forward * 400);
}
// create the obstacles we need
private void CreateObstacles(Pathfinding.RVO.Simulator sim, ref List <ObstacleVertex> obstacles)
{
NavmeshCut cut = gameObject.GetComponent <NavmeshCut>();
if (null == cut)
{
return;
}
Vector3 leftFront = new Vector3();
Vector3 rightFront = new Vector3();
Vector3 rightBack = new Vector3();
Vector3 leftBack = new Vector3();
cut.CalculateRectangleContourPoints(ref leftFront, ref rightFront, ref rightBack, ref leftBack);
Bounds bounds = cut.GetBounds();
Vector3 cutTopCenter = Vector3.Lerp(leftFront, rightBack, 0.5f); // calculate the top center of the nav mesh cut
cutTopCenter.y = bounds.max.y;
float edgeHeight = bounds.min.y;
// do a sphere cast downwards to see where the ground is beneath the nav mesh cut in order to put the obstacles on the ground
RaycastHit hitInfo;
const float SphereCastRadius = 0.35f; // this value ensures the sphere cast is large enough to not go through seems
if (Physics.SphereCast(cutTopCenter, SphereCastRadius, Vector3.down, out hitInfo, bounds.max.y - bounds.min.y, 1 << LayerMask.NameToLayer("Ground")))
{
edgeHeight = hitInfo.point.y;
}
leftFront.y = rightFront.y = rightBack.y = leftBack.y = edgeHeight; // set the obstacle height
Vector3[] verts = new Vector3[] { rightFront, rightBack, leftBack, leftFront };
AddObstacle(verts, Mathf.Max(0f, bounds.max.y - edgeHeight), sim, ref obstacles);
}
// turn obstacles on/off
public void SetObstaclesActive(Pathfinding.RVO.Simulator sim, bool isActive)
{
_isActive = isActive;
if (null != sim)
{
if (null != _staticObstacles)
{
for (int i = 0; i < _staticObstacles.Count; i++)
{
sim.SetObstacleActive(_staticObstacles[i], _isActive);
}
}
if (null != _dynamicObstacles)
{
for (int i = 0; i < _dynamicObstacles.Count; i++)
{
sim.SetObstacleActive(_dynamicObstacles[i], _isActive);
}
}
}
}
public KDTree(Simulator simulator)
{
this.simulator = simulator;
//agentTree = new List<AgentTreeNode>();
agentTree = new AgentTreeNode[0];
}
/** Adds obstacles for a graph */
public void AddGraphObstacles(Pathfinding.RVO.Simulator sim, NavGraph graph)
{
if (obstacles.Count > 0 && lastSim != null && lastSim != sim)
{
Debug.LogError("Simulator has changed but some old obstacles are still added for the previous simulator. Deleting previous obstacles.");
RemoveObstacles();
}
//Remember which simulator these obstacles were added to
lastSim = sim;
INavmesh ng = graph as INavmesh;
if (ng == null)
{
return;
}
//Assume less than 20 vertices per node (actually assumes 3, but I will change that some day)
int[] uses = new int[20];
ng.GetNodes(delegate(GraphNode _node) {
TriangleMeshNode node = _node as TriangleMeshNode;
uses[0] = uses[1] = uses[2] = 0;
if (node != null)
{
//Find out which edges are shared with other nodes
for (int j = 0; j < node.connections.Length; j++)
{
TriangleMeshNode other = node.connections[j] as TriangleMeshNode;
// Not necessarily a TriangleMeshNode
if (other != null)
{
int a = node.SharedEdge(other);
if (a != -1)
{
uses[a] = 1;
}
}
}
//Loop through all edges on the node
for (int j = 0; j < 3; j++)
{
//The edge is not shared with any other node
//I.e it is an exterior edge on the mesh
if (uses[j] == 0)
{
//The two vertices of the edge
Vector3 v1 = (Vector3)node.GetVertex(j);
Vector3 v2 = (Vector3)node.GetVertex((j + 1) % node.GetVertexCount());
//I think node vertices always should be clockwise, but it's good to be certain
/*if (!Polygon.IsClockwise (v1,v2,(Vector3)node.GetVertex((j+2) % node.GetVertexCount()))) {
* Vector3 tmp = v2;
* v2 = v1;
* v1 = tmp;
* }*/
#if ASTARDEBUG
Debug.DrawLine(v1, v2, Color.red);
Debug.DrawRay(v1, Vector3.up * wallHeight, Color.red);
#endif
//Find out the height of the wall/obstacle we are about to add
float height = System.Math.Abs(v1.y - v2.y);
height = System.Math.Max(height, 5);
//Add the edge as a line obstacle
obstacles.Add(sim.AddObstacle(v1, v2, wallHeight));
}
}
}
return(true);
});
}
protected void FindSimulator()
{
RVOSimulator rVOSimulator = UnityEngine.Object.FindObjectOfType(typeof(RVOSimulator)) as RVOSimulator;
if (rVOSimulator == null)
{
throw new InvalidOperationException("No RVOSimulator could be found in the scene. Please add one to any GameObject");
}
this.sim = rVOSimulator.GetSimulator();
}
private List <ObstacleVertex> _dynamicObstacles = null; // will only be used if the RVOContourObstacle moves
public void OnLatePostScan(Pathfinding.RVO.Simulator sim)
{
RemoveDynamicObstacles(sim);
RemoveStaticObstaclesAndCreateNew(sim);
}
public void Awake () {
tr = transform;
// Find the RVOSimulator in this scene
cachedSimulator = cachedSimulator ?? FindObjectOfType(typeof(RVOSimulator)) as RVOSimulator;
if (cachedSimulator == null) {
Debug.LogError("No RVOSimulator component found in the scene. Please add one.");
} else {
simulator = cachedSimulator.GetSimulator();
}
}
public Worker(Simulator sim)
{
this.simulator = sim;
thread = new Thread (new ThreadStart (Run));
thread.Start ();
}
/** Adds obstacles for a graph */
public void AddGraphObstacles (Pathfinding.RVO.Simulator sim, NavGraph graph) {
if (obstacles.Count > 0 && lastSim != null && lastSim != sim) {
Debug.LogError("Simulator has changed but some old obstacles are still added for the previous simulator. Deleting previous obstacles.");
RemoveObstacles();
}
//Remember which simulator these obstacles were added to
lastSim = sim;
var ng = graph as INavmesh;
if (ng == null) return;
//Assume less than 20 vertices per node (actually assumes 3, but I will change that some day)
var uses = new int[20];
ng.GetNodes(delegate(GraphNode _node) {
var node = _node as TriangleMeshNode;
uses[0] = uses[1] = uses[2] = 0;
if (node != null) {
//Find out which edges are shared with other nodes
for (int j = 0; j < node.connections.Length; j++) {
var other = node.connections[j] as TriangleMeshNode;
// Not necessarily a TriangleMeshNode
if (other != null) {
int a = node.SharedEdge(other);
if (a != -1) uses[a] = 1;
}
}
//Loop through all edges on the node
for (int j = 0; j < 3; j++) {
//The edge is not shared with any other node
//I.e it is an exterior edge on the mesh
if (uses[j] == 0) {
//The two vertices of the edge
var v1 = (Vector3)node.GetVertex(j);
var v2 = (Vector3)node.GetVertex((j+1) % node.GetVertexCount());
//I think node vertices always should be clockwise, but it's good to be certain
/*if (!Polygon.IsClockwise (v1,v2,(Vector3)node.GetVertex((j+2) % node.GetVertexCount()))) {
* Vector3 tmp = v2;
* v2 = v1;
* v1 = tmp;
* }*/
#if ASTARDEBUG
Debug.DrawLine(v1, v2, Color.red);
Debug.DrawRay(v1, Vector3.up*wallHeight, Color.red);
#endif
//Find out the height of the wall/obstacle we are about to add
float height = System.Math.Abs(v1.y-v2.y);
height = System.Math.Max(height, 5);
//Add the edge as a line obstacle
obstacles.Add(sim.AddObstacle(v1, v2, wallHeight));
}
}
}
return true;
});
}
public KDTree (Simulator simulator) {
this.simulator = simulator;
//agentTree = new List<AgentTreeNode>();
agentTree = new AgentTreeNode[0];
}
public void AddGraphObstacles(Simulator sim, NavGraph graph)
{
/** Adds obstacles for a graph */
public void AddGraphObstacles(Pathfinding.RVO.Simulator sim, NavGraph graph)
{
if (obstacles.Count > 0 && lastSim != null && lastSim != sim)
{
Debug.LogError("Simulator has changed but some old obstacles are still added for the previous simulator. Deleting previous obstacles.");
RemoveObstacles();
}
//Remember which simulator these obstacles were added to
lastSim = sim;
INavmesh ng = graph as INavmesh;
if (ng == null)
{
return;
}
Node[] nodes = graph.nodes;
Int3[] vertices = ng.vertices;
int[] uses = new int[3];
for (int i = 0; i < nodes.Length; i++)
{
MeshNode node = nodes[i] as MeshNode;
uses[0] = uses[1] = uses[2] = 0;
if (node != null)
{
for (int j = 0; j < node.connections.Length; j++)
{
MeshNode other = node.connections[j] as MeshNode;
if (other == null)
{
continue;
}
int first = -1;
int second = -1;
for (int x = 0; x < 3; x++)
{
for (int y = 0; y < 3; y++)
{
if (node[x] == other[y] && first < 0)
{
first = x;
break;
}
else if (node[x] == other[y])
{
second = x;
break;
}
}
if (second >= 0)
{
break;
}
}
//Only shared one vertex
if (second == -1)
{
continue;
}
if ((first + 1) % 3 == second)
{
uses[first]++;
}
else //if ((second+1) % 3 == first) {
{
uses[second]++;
}
}
}
for (int j = 0; j < 3; j++)
{
if (uses[j] == 0)
{
Vector3 v1 = (Vector3)vertices[node[j]];
Vector3 v2 = (Vector3)vertices[node[(j + 1) % 3]];
//I think node vertices always should be clockwise, but it's good to be certain
if (!Polygon.IsClockwise(v1, v2, (Vector3)vertices[node[(j + 2) % 3]]))
{
Vector3 tmp = v2;
v2 = v1;
v1 = tmp;
}
#if ASTARDEBUG
Debug.DrawLine(v1, v2, Color.red);
Debug.DrawRay(v1, Vector3.up * wallHeight, Color.red);
#endif
float height = System.Math.Abs(v1.y - v2.y);
height = System.Math.Max(height, 5);
obstacles.Add(sim.AddObstacle(v1, v2, wallHeight));
}
}
}
}
/** Finds a simulator in the scene.
*
* Saves found simulator in #sim.
*
* \throws System.InvalidOperationException When no RVOSimulator could be found.
*/
protected void FindSimulator () {
RVOSimulator rvosim = FindObjectOfType(typeof(RVOSimulator)) as RVOSimulator;
if (rvosim == null) throw new System.InvalidOperationException ("No RVOSimulator could be found in the scene. Please add one to any GameObject");
sim = rvosim.GetSimulator ();
}
public Worker (Simulator sim) {
this.simulator = sim;
thread = new Thread (new ThreadStart (Run));
thread.IsBackground = true;
thread.Name = "RVO Simulator Thread";
thread.Start ();
}
public void Awake () {
tr = transform;
var sim = FindObjectOfType(typeof(RVOSimulator)) as RVOSimulator;
if (sim == null) {
Debug.LogError ("No RVOSimulator component found in the scene. Please add one.");
return;
}
simulator = sim.GetSimulator ();
}
