/// <summary>
/// Updates the vertices of an obstacle.
///
/// The number of vertices in an obstacle cannot be changed, existing vertices can only be moved.
/// </summary>
/// <param name="obstacle">%Obstacle to update</param>
/// <param name="vertices">New vertices for the obstacle, must have at least the number of vertices in the original obstacle</param>
/// <param name="matrix">%Matrix to multiply vertices with before updating obstacle</param>
public void UpdateObstacle(ObstacleVertex obstacle, Vector3[] vertices, Matrix4x4 matrix)
{
if (vertices == null)
{
throw new System.ArgumentNullException("Vertices must not be null");
}
if (obstacle == null)
{
throw new System.ArgumentNullException("Obstacle must not be null");
}
if (vertices.Length < 2)
{
throw new System.ArgumentException("Less than 2 vertices in an obstacle");
}
bool identity = matrix == Matrix4x4.identity;
// Don't interfere with ongoing calculations
BlockUntilSimulationStepIsDone();
int count = 0;
// Obstacles are represented using linked lists
var vertex = obstacle;
do
{
if (count >= vertices.Length)
{
Debug.DrawLine(vertex.prev.position, vertex.position, Color.red);
throw new System.ArgumentException("Obstacle has more vertices than supplied for updating (" + vertices.Length + " supplied)");
}
// Premature optimization ftw!
vertex.position = identity ? vertices[count] : matrix.MultiplyPoint3x4(vertices[count]);
vertex = vertex.next;
count++;
} while (vertex != obstacle && vertex != null);
vertex = obstacle;
do
{
if (vertex.next == null)
{
vertex.dir = Vector2.zero;
}
else
{
Vector3 dir = vertex.next.position - vertex.position;
vertex.dir = new Vector2(dir.x, dir.z).normalized;
}
vertex = vertex.next;
} while (vertex != obstacle && vertex != null);
ScheduleCleanObstacles();
UpdateObstacles();
}
public void UpdateObstacle(ObstacleVertex obstacle, Int3[] vertices, Matrix4x4 matrix)
{
if (vertices == null)
{
throw new ArgumentNullException("Vertices must not be null");
}
if (obstacle == null)
{
throw new ArgumentNullException("Obstacle must not be null");
}
if (vertices.Length < 2)
{
throw new ArgumentException("Less than 2 vertices in an obstacle");
}
if (obstacle.split)
{
throw new ArgumentException("Obstacle is not a start vertex. You should only pass those ObstacleVertices got from AddObstacle method calls");
}
if (this.Multithreading && this.doubleBuffering)
{
for (int i = 0; i < this.workers.Length; i++)
{
this.workers[i].WaitOne();
}
}
int num = 0;
ObstacleVertex obstacleVertex = obstacle;
while (true)
{
while (obstacleVertex.next.split)
{
obstacleVertex.next = obstacleVertex.next.next;
obstacleVertex.next.prev = obstacleVertex;
}
if (num >= vertices.Length)
{
break;
}
obstacleVertex.position = (Int3)matrix.MultiplyPoint3x4((Vector3)vertices[num]);
num++;
obstacleVertex = obstacleVertex.next;
if (obstacleVertex == obstacle)
{
goto Block_9;
}
}
Debug.DrawLine((Vector3)obstacleVertex.prev.position, (Vector3)obstacleVertex.position, Color.red);
throw new ArgumentException("Obstacle has more vertices than supplied for updating (" + vertices.Length + " supplied)");
Block_9:
obstacleVertex = obstacle;
do
{
obstacleVertex.dir = (obstacleVertex.next.position - obstacleVertex.position).xz.normalized;
obstacleVertex = obstacleVertex.next;
}while (obstacleVertex != obstacle);
this.ScheduleCleanObstacles();
this.UpdateObstacles();
}
// Token: 0x06002865 RID: 10341 RVA: 0x001B7B09 File Offset: 0x001B5D09
public void RemoveObstacle(ObstacleVertex v)
{
if (v == null)
{
throw new ArgumentNullException("Vertex must not be null");
}
this.BlockUntilSimulationStepIsDone();
this.obstacles.Remove(v);
this.UpdateObstacles();
}
// turn obstacles off/on
public void SetObstacleActive(ObstacleVertex obstacle, bool isActive)
{
ObstacleVertex c = obstacle;
do
{
c.isActive = isActive;
c = c.next;
} while (c != obstacle);
}
public void UpdateObstacle(ObstacleVertex obstacle, Vector3[] vertices, Matrix4x4 matrix)
{
if (vertices == null)
{
throw new ArgumentNullException("Vertices must not be null");
}
if (obstacle == null)
{
throw new ArgumentNullException("Obstacle must not be null");
}
if (vertices.Length < 2)
{
throw new ArgumentException("Less than 2 vertices in an obstacle");
}
if (obstacle.split)
{
throw new ArgumentException("Obstacle is not a start vertex. You should only pass those ObstacleVertices got from AddObstacle method calls");
}
if (this.Multithreading && this.doubleBuffering)
{
for (int i = 0; i < this.workers.Length; i++)
{
this.workers[i].WaitOne();
}
}
int index = 0;
ObstacleVertex next = obstacle;
do
{
while (next.next.split)
{
next.next = next.next.next;
next.next.prev = next;
}
if (index >= vertices.Length)
{
Debug.DrawLine(next.prev.position, next.position, Color.red);
throw new ArgumentException("Obstacle has more vertices than supplied for updating (" + vertices.Length + " supplied)");
}
next.position = matrix.MultiplyPoint3x4(vertices[index]);
index++;
next = next.next;
}while (next != obstacle);
next = obstacle;
do
{
Vector3 vector = next.next.position - next.position;
Vector2 vector2 = new Vector2(vector.x, vector.z);
next.dir = vector2.normalized;
next = next.next;
}while (next != obstacle);
this.ScheduleCleanObstacles();
this.UpdateObstacles();
}
public ObstacleVertex AddObstacle(Vector3[] vertices, float height, Matrix4x4 matrix, RVOLayer layer = RVOLayer.DefaultObstacle)
{
if (vertices == null)
{
throw new ArgumentNullException("Vertices must not be null");
}
if (vertices.Length < 2)
{
throw new ArgumentException("Less than 2 vertices in an obstacle");
}
ObstacleVertex obstacleVertex = null;
ObstacleVertex obstacleVertex2 = null;
bool flag = matrix == Matrix4x4.identity;
if (this.Multithreading && this.doubleBuffering)
{
for (int i = 0; i < this.workers.Length; i++)
{
this.workers[i].WaitOne();
}
}
for (int j = 0; j < vertices.Length; j++)
{
ObstacleVertex obstacleVertex3 = new ObstacleVertex();
if (obstacleVertex == null)
{
obstacleVertex = obstacleVertex3;
}
else
{
obstacleVertex2.next = obstacleVertex3;
}
obstacleVertex3.prev = obstacleVertex2;
obstacleVertex3.layer = layer;
obstacleVertex3.position = ((!flag) ? matrix.MultiplyPoint3x4(vertices[j]) : vertices[j]);
obstacleVertex3.height = height;
obstacleVertex2 = obstacleVertex3;
}
obstacleVertex2.next = obstacleVertex;
obstacleVertex.prev = obstacleVertex2;
ObstacleVertex obstacleVertex4 = obstacleVertex;
do
{
Vector3 vector = obstacleVertex4.next.position - obstacleVertex4.position;
ObstacleVertex obstacleVertex5 = obstacleVertex4;
Vector2 vector2;
vector2..ctor(vector.x, vector.z);
obstacleVertex5.dir = vector2.normalized;
obstacleVertex4 = obstacleVertex4.next;
}while (obstacleVertex4 != obstacleVertex);
this.obstacles.Add(obstacleVertex);
this.UpdateObstacles();
return(obstacleVertex);
}
// Token: 0x0600285E RID: 10334 RVA: 0x001B7845 File Offset: 0x001B5A45
public ObstacleVertex AddObstacle(ObstacleVertex v)
{
if (v == null)
{
throw new ArgumentNullException("Obstacle must not be null");
}
this.BlockUntilSimulationStepIsDone();
this.obstacles.Add(v);
this.UpdateObstacles();
return(v);
}
public ObstacleVertex AddObstacle(Vector3[] vertices, float height, Matrix4x4 matrix, [Optional, DefaultParameterValue(2)] RVOLayer layer)
{
if (vertices == null)
{
throw new ArgumentNullException("Vertices must not be null");
}
if (vertices.Length < 2)
{
throw new ArgumentException("Less than 2 vertices in an obstacle");
}
ObstacleVertex item = null;
ObstacleVertex vertex2 = null;
bool flag = matrix == Matrix4x4.identity;
if (this.Multithreading && this.doubleBuffering)
{
for (int j = 0; j < this.workers.Length; j++)
{
this.workers[j].WaitOne();
}
}
for (int i = 0; i < vertices.Length; i++)
{
ObstacleVertex vertex3 = new ObstacleVertex();
if (item == null)
{
item = vertex3;
}
else
{
vertex2.next = vertex3;
}
vertex3.prev = vertex2;
vertex3.layer = layer;
vertex3.position = !flag?matrix.MultiplyPoint3x4(vertices[i]) : vertices[i];
vertex3.height = height;
vertex2 = vertex3;
}
vertex2.next = item;
item.prev = vertex2;
ObstacleVertex next = item;
do
{
Vector3 vector = next.next.position - next.position;
Vector2 vector2 = new Vector2(vector.x, vector.z);
next.dir = vector2.normalized;
next = next.next;
}while (next != item);
this.obstacles.Add(item);
this.UpdateObstacles();
return(item);
}
public void AddDynamicObstacle(ObstacleVertex obstacle)
{
_dynamicObstacles.Add(obstacle);
#if DEBUG
const int MaxDynamicObstacles = 8; // this allows for two four sided obstacles, there really shouldn't be many moving obstacles
if (_dynamicObstacles.Count > MaxDynamicObstacles)
{
EB.Debug.LogWarning("RVOCoreSimulator.AddDynamicObstacle() : There are a lot of moving obstacles in the scene - this has performance implications");
}
#endif
}
/// <summary>
/// Removes the obstacle identified by the vertex.
/// This must be the same vertex as the one returned by the AddObstacle call.
///
/// See: AddObstacle
/// </summary>
public void RemoveObstacle(ObstacleVertex v)
{
if (v == null)
{
throw new System.ArgumentNullException("Vertex must not be null");
}
// Don't interfere with ongoing calculations
BlockUntilSimulationStepIsDone();
obstacles.Remove(v);
UpdateObstacles();
}
public void UpdateObstacle(ObstacleVertex obstacle, Vector3[] vertices, Matrix4x4 matrix)
{
if (vertices == null)
{
throw new ArgumentNullException("Vertices must not be null");
}
if (obstacle == null)
{
throw new ArgumentNullException("Obstacle must not be null");
}
if (vertices.Length < 2)
{
throw new ArgumentException("Less than 2 vertices in an obstacle");
}
bool flag = matrix == Matrix4x4.identity;
this.BlockUntilSimulationStepIsDone();
int i = 0;
ObstacleVertex obstacleVertex = obstacle;
while (i < vertices.Length)
{
obstacleVertex.position = ((!flag) ? matrix.MultiplyPoint3x4(vertices[i]) : vertices[i]);
obstacleVertex = obstacleVertex.next;
i++;
if (obstacleVertex == obstacle || obstacleVertex == null)
{
obstacleVertex = obstacle;
do
{
if (obstacleVertex.next == null)
{
obstacleVertex.dir = Vector2.zero;
}
else
{
Vector3 vector = obstacleVertex.next.position - obstacleVertex.position;
ObstacleVertex obstacleVertex2 = obstacleVertex;
Vector2 vector2 = new Vector2(vector.x, vector.z);
obstacleVertex2.dir = vector2.normalized;
}
obstacleVertex = obstacleVertex.next;
}while (obstacleVertex != obstacle && obstacleVertex != null);
this.ScheduleCleanObstacles();
this.UpdateObstacles();
return;
}
}
Debug.DrawLine(obstacleVertex.prev.position, obstacleVertex.position, Color.red);
throw new ArgumentException("Obstacle has more vertices than supplied for updating (" + vertices.Length + " supplied)");
}
// StaticAndDynamicObstacle query functions
public void GetStaticAndDynamicObstacles(List <ObstacleVertex> obstacles, Vector3 position, float rangeSq)
{
for (int i = 0; i < this.obstacles.Count; ++i)
{
if (this.obstacles[i].isActive)
{
// this section is a copy of RVOKDTree.QueryObstacleTreeRecursive() ---------------------------------
ObstacleVertex ob1 = this.obstacles[i];
float agentLeftOfLine = VectorMath.SignedTriangleAreaTimes2XZ(ob1.position, ob1.next.position, position);
Vector3 dir2D = ob1.position - ob1.next.position;
dir2D.y = 0;
float distSqLine = (agentLeftOfLine * agentLeftOfLine) / dir2D.sqrMagnitude; //Isn't this 4 times too large since TriangleArea is actually 2*triangle area
if (distSqLine < rangeSq)
{
float distSqr = VectorMath.SqrDistancePointSegment(ob1.position, ob1.next.position, position);
if (distSqr < rangeSq)
{
obstacles.Add(this.obstacles[i]);
}
}
}
}
for (int dynamic = 0; dynamic < _dynamicObstacles.Count; ++dynamic)
{
if (_dynamicObstacles[dynamic].isActive)
{
// this section is a copy of RVOKDTree.QueryObstacleTreeRecursive() ---------------------------------
ObstacleVertex ob1 = _dynamicObstacles[dynamic];
float agentLeftOfLine = VectorMath.SignedTriangleAreaTimes2XZ(ob1.position, ob1.next.position, position);
Vector3 dir2D = ob1.position - ob1.next.position;
dir2D.y = 0;
float distSqLine = (agentLeftOfLine * agentLeftOfLine) / dir2D.sqrMagnitude; //Isn't this 4 times too large since TriangleArea is actually 2*triangle area
if (distSqLine < rangeSq)
{
float distSqr = VectorMath.SqrDistancePointSegment(ob1.position, ob1.next.position, position);
if (distSqr < rangeSq)
{
obstacles.Add(_dynamicObstacles[dynamic]);
}
}
}
}
}
/// <summary>
/// Adds a previously removed obstacle.
/// This does not check if the obstacle is already added to the simulation, so please do not add an obstacle multiple times.
///
/// It is assumed that this is a valid obstacle.
/// </summary>
public ObstacleVertex AddObstacle(ObstacleVertex v)
{
if (v == null)
{
throw new System.ArgumentNullException("Obstacle must not be null");
}
//Don't interfere with ongoing calculations
BlockUntilSimulationStepIsDone();
obstacles.Add(v);
UpdateObstacles();
return(v);
}
/// <summary>
/// Adds an obstacle described by the vertices.
///
/// See: RemoveObstacle
/// </summary>
public ObstacleVertex AddObstacle(Vector3[] vertices, float height, Matrix4x4 matrix, RVOLayer layer = RVOLayer.DefaultObstacle, bool cycle = true)
{
if (vertices == null)
{
throw new System.ArgumentNullException("Vertices must not be null");
}
if (vertices.Length < 2)
{
throw new System.ArgumentException("Less than 2 vertices in an obstacle");
}
ObstacleVertex first = null;
ObstacleVertex prev = null;
// Don't interfere with ongoing calculations
BlockUntilSimulationStepIsDone();
for (int i = 0; i < vertices.Length; i++)
{
var v = new ObstacleVertex
{
prev = prev,
layer = layer,
height = height
};
if (first == null)
{
first = v;
}
else
{
prev.next = v;
}
prev = v;
}
if (cycle)
{
prev.next = first;
first.prev = prev;
}
UpdateObstacle(first, vertices, matrix);
obstacles.Add(first);
return(first);
}
public void RemoveObstacle(ObstacleVertex v)
{
if (v == null)
{
throw new ArgumentNullException("Vertex must not be null");
}
if (this.Multithreading && this.doubleBuffering)
{
for (int i = 0; i < this.workers.Length; i++)
{
this.workers[i].WaitOne();
}
}
this.obstacles.Remove(v);
this.UpdateObstacles();
}
private void CleanObstacles()
{
for (int i = 0; i < obstacles.Count; i++)
{
ObstacleVertex first = obstacles[i];
ObstacleVertex c = first;
do
{
while (c.next.split)
{
c.next = c.next.next;
c.next.prev = c;
}
c = c.next;
} while (c != first);
}
}
private void CleanObstacles()
{
for (int i = 0; i < this.obstacles.Count; i++)
{
ObstacleVertex vertex = this.obstacles[i];
ObstacleVertex next = vertex;
do
{
while (next.next.split)
{
next.next = next.next.next;
next.next.prev = next;
}
next = next.next;
}while (next != vertex);
}
}
private void CleanObstacles()
{
for (int i = 0; i < this.obstacles.get_Count(); i++)
{
ObstacleVertex obstacleVertex = this.obstacles.get_Item(i);
ObstacleVertex obstacleVertex2 = obstacleVertex;
do
{
while (obstacleVertex2.next.split)
{
obstacleVertex2.next = obstacleVertex2.next.next;
obstacleVertex2.next.prev = obstacleVertex2;
}
obstacleVertex2 = obstacleVertex2.next;
}while (obstacleVertex2 != obstacleVertex);
}
}
/**
* Adds a line obstacle with a specified height.
*
* \see RemoveObstacle
*/
public ObstacleVertex AddObstacle(Vector3 a, Vector3 b, Vector3 pushAwayDir, float height, bool isActive)
{
ObstacleVertex first = CreateObstacle(a, b, pushAwayDir, height, isActive);
//Don't interfere with ongoing calculations
if (Multithreading && doubleBuffering)
{
for (int j = 0; j < workers.Length; j++)
{
workers[j].WaitOne();
}
}
obstacles.Add(first);
UpdateObstacles();
return(first);
}
/** Removes the obstacle identified by the vertex.
* This must be the same vertex as the one returned by the AddObstacle call.
*
* \see AddObstacle
*/
public void RemoveObstacle(ObstacleVertex v)
{
if (v == null)
{
throw new System.ArgumentNullException("Vertex must not be null");
}
//Don't interfere with ongoing calculations
if (Multithreading && doubleBuffering)
{
for (int j = 0; j < workers.Length; j++)
{
workers[j].WaitOne();
}
}
obstacles.Remove(v);
UpdateObstacles();
}
public void BuildObstacleTree()
{
List <ObstacleVertex> obstacles = Pathfinding.Util.ListPool <ObstacleVertex> .Claim();
List <ObstacleVertex> src = simulator.GetObstacles();
for (int i = 0; i < src.Count; i++)
{
ObstacleVertex c = src[i];
do
{
obstacles.Add(c);
c = c.next;
} while (c != src[i]);
}
RecycleOTN(obstacleRoot);
obstacleRoot = BuildObstacleTreeRecursive(obstacles);
}
// Token: 0x06002860 RID: 10336 RVA: 0x001B7880 File Offset: 0x001B5A80
public ObstacleVertex AddObstacle(Vector3[] vertices, float height, Matrix4x4 matrix, RVOLayer layer = RVOLayer.DefaultObstacle, bool cycle = true)
{
if (vertices == null)
{
throw new ArgumentNullException("Vertices must not be null");
}
if (vertices.Length < 2)
{
throw new ArgumentException("Less than 2 vertices in an obstacle");
}
ObstacleVertex obstacleVertex = null;
ObstacleVertex obstacleVertex2 = null;
this.BlockUntilSimulationStepIsDone();
for (int i = 0; i < vertices.Length; i++)
{
ObstacleVertex obstacleVertex3 = new ObstacleVertex
{
prev = obstacleVertex2,
layer = layer,
height = height
};
if (obstacleVertex == null)
{
obstacleVertex = obstacleVertex3;
}
else
{
obstacleVertex2.next = obstacleVertex3;
}
obstacleVertex2 = obstacleVertex3;
}
if (cycle)
{
obstacleVertex2.next = obstacleVertex;
obstacleVertex.prev = obstacleVertex2;
}
this.UpdateObstacle(obstacleVertex, vertices, matrix);
this.obstacles.Add(obstacleVertex);
return(obstacleVertex);
}
public void OnEnable()
{
if (this.addedObstacles != null)
{
if (this.sim == null)
{
throw new Exception("This should not happen! Make sure you are not overriding the OnDisable function");
}
for (int i = 0; i < this.addedObstacles.Count; i++)
{
ObstacleVertex obstacleVertex = this.addedObstacles[i];
ObstacleVertex obstacleVertex2 = obstacleVertex;
do
{
obstacleVertex.layer = this.layer;
obstacleVertex = obstacleVertex.next;
}while (obstacleVertex != obstacleVertex2);
this.sim.AddObstacle(this.addedObstacles[i]);
}
}
}
public void InsertObstacleNeighbour(ObstacleVertex ob1, float rangeSq)
{
ObstacleVertex ob2 = ob1.next;
float dist = Mathfx.DistancePointSegmentStrict(ob1.position, ob2.position, Position);
if (dist < rangeSq)
{
obstacles.Add(ob1);
obstacleDists.Add(dist);
int i = obstacles.Count - 1;
while (i != 0 && dist < obstacleDists[i - 1])
{
obstacles[i] = obstacles[i - 1];
obstacleDists[i] = obstacleDists[i - 1];
i--;
}
obstacles[i] = ob1;
obstacleDists[i] = dist;
}
}
/**
* Adds a line obstacle with a specified height.
*
* \see RemoveObstacle
*/
public ObstacleVertex AddObstacle(Vector3 a, Vector3 b, float height)
{
ObstacleVertex first = new ObstacleVertex();
ObstacleVertex second = new ObstacleVertex();
first.layer = RVOLayer.DefaultObstacle;
second.layer = RVOLayer.DefaultObstacle;
first.prev = second;
second.prev = first;
first.next = second;
second.next = first;
first.position = a;
second.position = b;
first.height = height;
second.height = height;
second.ignore = true;
first.dir = new Vector2(b.x - a.x, b.z - a.z).normalized;
second.dir = -first.dir;
//Don't interfere with ongoing calculations
if (Multithreading && doubleBuffering)
{
for (int j = 0; j < workers.Length; j++)
{
workers[j].WaitOne();
}
}
obstacles.Add(first);
UpdateObstacles();
return(first);
}
// Token: 0x06002861 RID: 10337 RVA: 0x001B7918 File Offset: 0x001B5B18
public ObstacleVertex AddObstacle(Vector3 a, Vector3 b, float height)
{
ObstacleVertex obstacleVertex = new ObstacleVertex();
ObstacleVertex obstacleVertex2 = new ObstacleVertex();
obstacleVertex.layer = RVOLayer.DefaultObstacle;
obstacleVertex2.layer = RVOLayer.DefaultObstacle;
obstacleVertex.prev = obstacleVertex2;
obstacleVertex2.prev = obstacleVertex;
obstacleVertex.next = obstacleVertex2;
obstacleVertex2.next = obstacleVertex;
obstacleVertex.position = a;
obstacleVertex2.position = b;
obstacleVertex.height = height;
obstacleVertex2.height = height;
obstacleVertex2.ignore = true;
obstacleVertex.dir = new Vector2(b.x - a.x, b.z - a.z).normalized;
obstacleVertex2.dir = -obstacleVertex.dir;
this.BlockUntilSimulationStepIsDone();
this.obstacles.Add(obstacleVertex);
this.UpdateObstacles();
return(obstacleVertex);
}
public ObstacleVertex AddObstacle(Vector3 a, Vector3 b, float height)
{
ObstacleVertex obstacleVertex = new ObstacleVertex();
ObstacleVertex obstacleVertex2 = new ObstacleVertex();
obstacleVertex.layer = RVOLayer.DefaultObstacle;
obstacleVertex2.layer = RVOLayer.DefaultObstacle;
obstacleVertex.prev = obstacleVertex2;
obstacleVertex2.prev = obstacleVertex;
obstacleVertex.next = obstacleVertex2;
obstacleVertex2.next = obstacleVertex;
obstacleVertex.position = a;
obstacleVertex2.position = b;
obstacleVertex.height = height;
obstacleVertex2.height = height;
obstacleVertex2.ignore = true;
ObstacleVertex arg_86_0 = obstacleVertex;
Vector2 vector = new Vector2(b.x - a.x, b.z - a.z);
arg_86_0.dir = vector.normalized;
obstacleVertex2.dir = -obstacleVertex.dir;
if (this.Multithreading && this.doubleBuffering)
{
for (int i = 0; i < this.workers.Length; i++)
{
this.workers[i].WaitOne();
}
}
this.obstacles.Add(obstacleVertex);
this.UpdateObstacles();
return obstacleVertex;
}
public void RemoveDynamicObstacle(ObstacleVertex obstacle)
{
_dynamicObstacles.Remove(obstacle);
}
private ObstacleTreeNode BuildObstacleTreeRecursive (List<ObstacleVertex> obstacles) {
//Debug.Log ("REC " + obstacles.Count);
if (obstacles.Count == 0) {
Pathfinding.Util.ListPool<ObstacleVertex>.Release (obstacles);
return null;
}
ObstacleTreeNode node = GetOTN ();//new ObstacleTreeNode ();
int optimalSplit = 0;
int minLeft = obstacles.Count;
int minRight = obstacles.Count;
int step = Mathf.Max (1,obstacles.Count / ((int)(System.Math.Log(obstacles.Count,2)+1)*4));
for (int i=0;i<obstacles.Count;i+=step){
int leftSize = 0;
int rightSize = 0;
ObstacleVertex obI1 = obstacles[i];
ObstacleVertex obI2 = obI1.next;
for (int j=0;j<obstacles.Count;j++) {
if (i == j) continue;
ObstacleVertex obJ1 = obstacles[j];
ObstacleVertex obJ2 = obJ1.next;
float j1LeftOfI = Polygon.TriangleArea (obI1.position, obI2.position, obJ1.position);
float j2LeftOfI = Polygon.TriangleArea (obI1.position, obI2.position, obJ2.position);
if (j1LeftOfI >= -float.Epsilon && j2LeftOfI >= -float.Epsilon) {
leftSize++;
} else if (j1LeftOfI <= float.Epsilon && j2LeftOfI <= float.Epsilon) {
rightSize++;
} else {
leftSize++;
rightSize++;
}
int v1a = Math.Max (leftSize,rightSize);
int v1b = Math.Min (leftSize, rightSize);
int v2a = Math.Max (minLeft, minRight);
int v2b = Math.Min (minLeft, minRight);
// (v1a, v1b) >= (v2a, v2b)
//!(lhs._a < rhs._a || !(rhs._a < lhs._a) && lhs._b < rhs._b);
if (!(v1a < v2a || !(v2a < v1a) && v1b < v2b)) {
break;
}
}
int x1a = Math.Max (leftSize,rightSize);
int x1b = Math.Min (leftSize, rightSize);
int x2a = Math.Max (minLeft, minRight);
int x2b = Math.Min (minLeft, minRight);
if (x1a < x2a || !(x2a < x1a) && x1b < x2b) {
minLeft = leftSize;
minRight = rightSize;
optimalSplit = i;
}
}
//Split node
{
List<ObstacleVertex> leftObstacles = Pathfinding.Util.ListPool<ObstacleVertex>.Claim (minLeft);//new List<ObstacleVertex>(minLeft);
List<ObstacleVertex> rightObstacles = Pathfinding.Util.ListPool<ObstacleVertex>.Claim (minRight);//new List<ObstacleVertex>(minRight);
//int leftCounter = 0;
//int rightCounter = 0;
int i = optimalSplit;
ObstacleVertex obI1 = obstacles[i];
ObstacleVertex obI2 = obI1.next;
for (int j=0;j<obstacles.Count;j++) {
if (optimalSplit == j) continue;
ObstacleVertex obJ1 = obstacles[j];
ObstacleVertex obJ2 = obJ1.next;
float j1LeftOfI = Polygon.TriangleArea (obI1.position, obI2.position, obJ1.position);
float j2LeftOfI = Polygon.TriangleArea (obI1.position, obI2.position, obJ2.position);
if (j1LeftOfI >= -float.Epsilon && j2LeftOfI >= -float.Epsilon) {
leftObstacles.Add (obJ1);
} else if (j1LeftOfI <= float.Epsilon && j2LeftOfI <= float.Epsilon) {
rightObstacles.Add (obJ1);
} else {
//Split Obstacle
float t = Polygon.IntersectionFactor (obJ1.position,obJ2.position,obI1.position,obI2.position);
Vector3 splitPoint = obJ1.position + (obJ2.position - obJ1.position) * t;
ObstacleVertex obst = new ObstacleVertex();
obst.position = splitPoint;
obJ1.next = obst;
obst.prev = obJ1;
obst.next = obJ2;
obJ2.prev = obst;
obst.dir = obJ1.dir;
//Mark as split point so that it can be identified
//and removed if an update of this obstacle should be done
obst.split = true;
//New vertex is added at split, angle will be 180 degrees
obst.convex = true;
#if ASTARDEBUG
Debug.DrawRay (splitPoint,Vector3.up,Color.cyan);
#endif
if (j1LeftOfI > 0.0f) {
leftObstacles.Add (obJ1);
rightObstacles.Add (obst);
} else {
rightObstacles.Add (obJ1);
leftObstacles.Add (obst);
}
}
}
Pathfinding.Util.ListPool<ObstacleVertex>.Release (obstacles);
node.obstacle = obI1;
node.left = BuildObstacleTreeRecursive (leftObstacles);
node.right = BuildObstacleTreeRecursive (rightObstacles);
return node;
}
}
/*public void UpdateNeighbours () {
neighbours.Clear ();
float sqrDist = neighbourDistance*neighbourDistance;
for ( int i = 0; i < simulator.agents.Count; i++ ) {
float dist = (simulator.agents[i].position - position).sqrMagnitude;
if ( dist <= sqrDist ) {
neighbours.Add ( simulator.agents[i] );
}
}
}*/
public void InsertObstacleNeighbour(ObstacleVertex ob1, float rangeSq)
{
ObstacleVertex ob2 = ob1.next;
float dist = AstarMath.DistancePointSegmentStrict(ob1.position, ob2.position, Position);
if (dist < rangeSq)
{
obstacles.Add(ob1);
obstacleDists.Add(dist);
int i = obstacles.Count - 1;
while (i != 0 && dist < obstacleDists[i - 1])
{
obstacles[i] = obstacles[i - 1];
obstacleDists[i] = obstacleDists[i - 1];
i--;
}
obstacles[i] = ob1;
obstacleDists[i] = dist;
}
}
public void InsertObstacleNeighbour (ObstacleVertex ob1, float rangeSq) {
}
/** Updates the vertices of an obstacle.
* \param obstacle %Obstacle to update
* \param vertices New vertices for the obstacle, must have at least the number of vertices in the original obstacle
* \param matrix %Matrix to multiply vertices with before updating obstacle
*
* The number of vertices in an obstacle cannot be changed, existing vertices can only be moved.
*/
public void UpdateObstacle(ObstacleVertex obstacle, Vector3[] vertices, Matrix4x4 matrix)
{
if (vertices == null)
{
throw new System.ArgumentNullException("Vertices must not be null");
}
if (obstacle == null)
{
throw new System.ArgumentNullException("Obstacle must not be null");
}
if (vertices.Length < 2)
{
throw new System.ArgumentException("Less than 2 vertices in an obstacle");
}
if (obstacle.split)
{
throw new System.ArgumentException("Obstacle is not a start vertex. You should only pass those ObstacleVertices got from AddObstacle method calls");
}
//Don't interfere with ongoing calculations
if (Multithreading && doubleBuffering)
{
for (int j = 0; j < workers.Length; j++)
{
workers[j].WaitOne();
}
}
//Compact obstacle and count
int count = 0;
ObstacleVertex c = obstacle;
do
{
while (c.next.split)
{
c.next = c.next.next;
c.next.prev = c;
}
if (count >= vertices.Length)
{
Debug.DrawLine(c.prev.position, c.position, Color.red);
throw new System.ArgumentException("Obstacle has more vertices than supplied for updating (" + vertices.Length + " supplied)");
}
c.position = matrix.MultiplyPoint3x4(vertices[count]);
count++;
c = c.next;
} while (c != obstacle);
c = obstacle;
do
{
Vector3 dir = c.next.position - c.position;
c.dir = new Vector2(dir.x, dir.z).normalized;
if (vertices.Length == 2)
{
c.convex = true;
}
else
{
c.convex = Polygon.IsClockwiseMargin(c.next.position, c.position, c.prev.position);
}
c = c.next;
} while (c != obstacle);
ScheduleCleanObstacles();
UpdateObstacles();
}
/** Adds an obstacle described by the vertices.
*
* \see RemoveObstacle
*/
public ObstacleVertex AddObstacle(Vector3[] vertices, float height, Matrix4x4 matrix)
{
if (vertices == null)
{
throw new System.ArgumentNullException("Vertices must not be null");
}
if (vertices.Length < 2)
{
throw new System.ArgumentException("Less than 2 vertices in an obstacle");
}
ObstacleVertex first = null;
ObstacleVertex prev = null;
bool identity = matrix == Matrix4x4.identity;
//Don't interfere with ongoing calculations
if (Multithreading && doubleBuffering)
{
for (int j = 0; j < workers.Length; j++)
{
workers[j].WaitOne();
}
}
for (int i = 0; i < vertices.Length; i++)
{
ObstacleVertex v = new ObstacleVertex();
if (first == null)
{
first = v;
}
else
{
prev.next = v;
}
v.prev = prev;
//Premature optimization ftw!
v.position = identity ? vertices[i] : matrix.MultiplyPoint3x4(vertices[i]);
//v.thin = thin;
v.height = height;
prev = v;
}
prev.next = first;
first.prev = prev;
ObstacleVertex c = first;
do
{
Vector3 dir = c.next.position - c.position;
c.dir = new Vector2(dir.x, dir.z).normalized;
if (vertices.Length == 2)
{
c.convex = true;
}
else
{
c.convex = Polygon.IsClockwiseMargin(c.next.position, c.position, c.prev.position);
}
c = c.next;
} while (c != first);
obstacles.Add(first);
UpdateObstacles();
return(first);
}
public void CalculateVelocity()
{
orcaLines.Clear();
if (locked)
{
newVelocity = new Vector3(0, 0, 0);
return;
}
Vector2 velocity2D = new Vector2(Velocity.x, Velocity.z);
#if ASTARDEBUG
if (DebugDraw)
{
Debug.DrawRay(position, desiredVelocity, Color.green);
Debug.DrawRay(position, Velocity, Color.blue);
}
#endif
for (int i = 0; i < obstacles.Count; i++)
{
float invTimeHorizonObst = 1.0f / obstacleTimeHorizon;
ObstacleVertex ob1 = obstacles[i];
ObstacleVertex ob2 = ob1.next;
float agentRadius = radius;
float obstacleYMin = System.Math.Min(ob1.position.y, ob2.position.y);
float obstacleYMax = System.Math.Max(ob1.position.y + ob1.height, ob2.position.y + ob2.height);
float intersectYMin = System.Math.Max(obstacleYMin, position.y);
float intersectYMax = System.Math.Min(obstacleYMax, position.y + height);
if (intersectYMax - intersectYMin < 0)
{
continue;
}
Vector2 dir1 = new Vector2(ob1.position.x - position.x, ob1.position.z - position.z);
Vector2 dir2 = new Vector2(ob2.position.x - position.x, ob2.position.z - position.z);
Vector2 obstVector = new Vector2(ob2.position.x - ob1.position.x, ob2.position.z - ob1.position.z);
/*
* Check if velocity obstacle of obstacle is already taken care of by
* previously constructed obstacle ORCA lines.
*/
bool alreadyCovered = false;
for (int j = 0; j < orcaLines.Count; j++)
{
if (det(invTimeHorizonObst * dir1 - orcaLines[j].point, orcaLines[j].dir) - invTimeHorizonObst * agentRadius >= -float.Epsilon && det(invTimeHorizonObst * dir2 - orcaLines[j].point, orcaLines[j].dir) - invTimeHorizonObst * agentRadius >= -float.Epsilon)
{
alreadyCovered = true;
break;
}
}
if (alreadyCovered)
{
continue;
}
#if ASTARDEBUG
Debug.DrawLine(ob1.position + Vector3.up, ob2.position + Vector3.up, Color.black);
Debug.DrawRay(ob1.position + Vector3.up, new Vector3(ob1.dir.x, 0, ob1.dir.y), Color.red);
Debug.DrawRay(ob2.position + Vector3.up, new Vector3(ob2.dir.x, 0, ob2.dir.y), Color.red);
#endif
/*
* Compute legs. When obliquely viewed, both legs can come from a single
* vertex. 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. Legs extend cut-off line when
* nonconvex vertex.
*/
float distSq1 = dir1.sqrMagnitude;
float distSq2 = dir2.sqrMagnitude;
float radiusSq = agentRadius * agentRadius;
Vector2 leftLegDir, rightLegDir;
float s = Vector2.Dot(-dir1, obstVector) / obstVector.sqrMagnitude;
float distSqLine = (-dir1 - s * obstVector).sqrMagnitude;
//invTimeHorizonObst = 1.0f / simulator.DeltaTime;
//ORCA Line
Line line;
if (s < 0 && distSq1 <= radiusSq)
{
/* Left Vertex Collision */
if (ob1.convex)
{
line.point = new Vector2(0, 0);
line.dir = (new Vector2(-dir1.y, dir1.x)).normalized;
orcaLines.Add(line);
}
continue;
}
else if (s > 1.0f && distSq2 <= radiusSq)
{
/* Right Vertex Collision */
if (ob2.convex && det(dir2, ob2.dir) >= 0)
{
line.point = new Vector2(0, 0);
line.dir = (new Vector2(-dir2.y, dir2.x)).normalized;
orcaLines.Add(line);
}
continue;
}
else if (s >= 0 && s < 1 && distSqLine <= radiusSq)
{
/* Obstacle Segment Collision */
line.point = new Vector2(0, 0);
line.dir = -ob1.dir;
orcaLines.Add(line);
continue;
}
/*
* No collision.
* Compute legs. When obliquely viewed, both legs can come from a single
* vertex. Legs extend cut-off line when nonconvex vertex.
*/
if (s < 0 && distSqLine <= radiusSq)
{
/* No Collision, but obstacle viewed obliquely so that left vertex
* defines velocity obstacle
*/
if (!ob1.convex)
{
continue;
}
ob2 = ob1;
//Wait wut... what is being done here?
float leg1 = (float)System.Math.Sqrt(distSq1 - radiusSq);
leftLegDir = new Vector2(dir1.x * leg1 - dir1.y * agentRadius, dir1.x * agentRadius + dir1.y * leg1) / distSq1;
rightLegDir = new Vector2(dir1.x * leg1 + dir1.y * agentRadius, -dir1.x * agentRadius + dir1.y * leg1) / distSq1;
}
else if (s > 1 && distSqLine <= radiusSq)
{
/* No Collision, but obstacle viewed obliquely so that right vertex
* defines velocity obstacle
*/
if (!ob1.convex)
{
continue;
}
ob1 = ob2;
//Wait wut... what is being done here?
float leg2 = (float)System.Math.Sqrt(distSq2 - radiusSq);
leftLegDir = new Vector2(dir2.x * leg2 - dir2.y * agentRadius, dir2.x * agentRadius + dir2.y * leg2) / distSq2;
rightLegDir = new Vector2(dir2.x * leg2 + dir2.y * agentRadius, -dir2.x * agentRadius + dir2.y * leg2) / distSq2;
}
else
{
/* Usual situation */
if (ob1.convex)
{
float leg1 = (float)System.Math.Sqrt(distSq1 - radiusSq);
leftLegDir = new Vector2(dir1.x * leg1 - dir1.y * agentRadius, dir1.x * agentRadius + dir1.y * leg1) / distSq1;
}
else
{
/* Left vertex non-convex; left leg extends cut-off line. */
leftLegDir = -ob1.dir;
}
if (ob2.convex)
{
float leg2 = (float)System.Math.Sqrt(distSq2 - radiusSq);
rightLegDir = new Vector2(dir2.x * leg2 + dir2.y * agentRadius, -dir2.x * agentRadius + dir2.y * leg2) / distSq2;
}
else
{
/* Right vertex non-convex; right leg extends cut-off line. */
rightLegDir = ob1.dir; //(ob1.convex ? obstVector.normalized : -leftLegDir);
}
}
/*
* 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.
*/
ObstacleVertex leftNeighbor = ob1.prev;
bool isLeftLegForeign = false;
bool isRightLegForeign = false;
if (ob1.convex && det(leftLegDir, -leftNeighbor.dir) >= 0.0f)
{
/* Left leg points into obstacle. */
leftLegDir = -leftNeighbor.dir;
isLeftLegForeign = true;
}
if (ob2.convex && det(rightLegDir, ob2.dir) <= 0.0f)
{
/* Right leg points into obstacle. */
rightLegDir = ob2.dir;
isRightLegForeign = true;
}
/* Compute cut-off centers. */
Vector2 leftCutoff = invTimeHorizonObst * new Vector2(ob1.position.x - position.x, ob1.position.z - position.z);
Vector2 rightCutoff = invTimeHorizonObst * new Vector2(ob2.position.x - position.x, ob2.position.z - position.z);
Vector2 cutoffVec = rightCutoff - leftCutoff;
/* Project current velocity on velocity obstacle. */
/* Check if current velocity is projected on cutoff circles. */
float t = (ob1 == ob2 ? 0.5f : Vector2.Dot((velocity2D - leftCutoff), cutoffVec)) / cutoffVec.sqrMagnitude;
float tLeft = Vector2.Dot(velocity2D - leftCutoff, leftLegDir);
float tRight = Vector2.Dot(velocity2D - rightCutoff, rightLegDir);
if ((t < 0.0f && tLeft < 0.0f) || (ob1 == ob2 && tLeft < 0.0f && tRight < 0.0f))
{
/* Project on left cut-off circle. */
Vector2 unitW = (velocity2D - leftCutoff).normalized;
line.dir = new Vector2(unitW.y, -unitW.x);
line.point = leftCutoff + agentRadius * invTimeHorizonObst * unitW;
orcaLines.Add(line);
continue;
}
else if (t > 1.0f && tRight < 0.0f)
{
/* Project on right cut-off circle. */
Vector2 unitW = (velocity2D - rightCutoff).normalized;
line.dir = new Vector2(unitW.y, -unitW.x);
line.point = rightCutoff + agentRadius * invTimeHorizonObst * unitW;
orcaLines.Add(line);
continue;
}
/*
* Project on left leg, right leg, or cut-off line, whichever is closest
* to velocity.
*/
float distSqCutoff = ((t <0.0f || t> 1.0f || ob1 == ob2) ?
float.PositiveInfinity : (velocity2D - (leftCutoff + t * cutoffVec)).sqrMagnitude);
float distSqLeft = ((tLeft < 0.0f) ?
float.PositiveInfinity : (velocity2D - (leftCutoff + tLeft * leftLegDir)).sqrMagnitude);
float distSqRight = ((tRight < 0.0f) ?
float.PositiveInfinity : (velocity2D - (rightCutoff + tRight * rightLegDir)).sqrMagnitude);
if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight)
{
/* Project on cut-off line. */
line.dir = -ob1.dir;
line.point = leftCutoff + agentRadius * invTimeHorizonObst * new Vector2(-line.dir.y, line.dir.x);
orcaLines.Add(line);
continue;
}
else if (distSqLeft <= distSqRight)
{
/* Project on left leg. */
if (isLeftLegForeign)
{
continue;
}
line.dir = leftLegDir;
line.point = leftCutoff + agentRadius * invTimeHorizonObst * new Vector2(-line.dir.y, line.dir.x);
orcaLines.Add(line);
continue;
}
else
{
/* Project on right leg. */
if (isRightLegForeign)
{
continue;
}
line.dir = -rightLegDir;
line.point = rightCutoff + agentRadius * invTimeHorizonObst * new Vector2(-line.dir.y, line.dir.x);
orcaLines.Add(line);
continue;
}
}
//Phase II
float invTimeHorizon = 1.0f / agentTimeHorizon;
int numObstacleLines = orcaLines.Count;
/* Create agent ORCA lines. */
for (int i = 0; i < neighbours.Count; ++i)
{
Agent other = neighbours[i];
//Debug.DrawLine (position,other.position,Color.red);
float maxY = System.Math.Min(position.y + height, other.position.y + other.height);
float minY = System.Math.Max(position.y, other.position.y);
//The agents cannot collide since they
//are on different y-levels
if (maxY - minY < 0)
{
continue;
}
Vector3 relativePosition3D = (other.position - position);
Vector3 relativeVelocity3D = Velocity - other.Velocity;
Vector2 relativePosition = new Vector2(relativePosition3D.x, relativePosition3D.z);
Vector2 relativeVelocity = new Vector2(relativeVelocity3D.x, relativeVelocity3D.z);
float distSq = relativePosition.sqrMagnitude;
float combinedRadius = radius + other.radius;
float combinedRadiusSq = combinedRadius * combinedRadius;
Line line;
Vector2 u;
if (distSq > combinedRadiusSq)
{
/* No collision. */
Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;
/* Vector from cutoff center to relative velocity. */
float wLengthSq = w.sqrMagnitude;
float dotProduct1 = Vector2.Dot(w, relativePosition);
if (dotProduct1 < 0.0f && dotProduct1 * dotProduct1 > combinedRadiusSq * wLengthSq)
{
/* Project on cut-off circle. */
float wLength = (float)System.Math.Sqrt(wLengthSq);
//Normalized w
Vector2 unitW = w / wLength;
line.dir = new Vector2(unitW.y, -unitW.x);
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
#if ASTARDEBUG
if (DebugDraw)
{
Debug.DrawRay(position, u, Color.red);
}
#endif
}
else
{
/* Project on legs. */
float leg = (float)System.Math.Sqrt(distSq - combinedRadiusSq);
if (det(relativePosition, w) > 0.0f)
{
/* Project on left leg. */
line.dir = new Vector2(relativePosition.x * leg - relativePosition.y * combinedRadius, relativePosition.x * combinedRadius + relativePosition.y * leg) / distSq;
}
else
{
/* Project on right leg. */
line.dir = -new Vector2(relativePosition.x * leg + relativePosition.y * combinedRadius, -relativePosition.x * combinedRadius + relativePosition.y * leg) / distSq;
}
float dotProduct2 = Vector2.Dot(relativeVelocity, line.dir);
u = dotProduct2 * line.dir - relativeVelocity;
}
}
else
{
/* Collision. Project on cut-off circle of time timeStep. */
float invTimeStep = 1.0f / simulator.DeltaTime;
/* Vector from cutoff center to relative velocity. */
Vector2 w = relativeVelocity - invTimeStep * relativePosition;
//vel - pos/delta
float wLength = w.magnitude;
//Normalized w
Vector2 unitW = w / wLength;
line.dir = new Vector2(unitW.y, -unitW.x);
u = (combinedRadius * invTimeStep - wLength) * unitW;
}
if (other.locked)
{
line.point = velocity2D + 1.0f * u;
}
line.point = velocity2D + 0.5f * u;
orcaLines.Add(line);
}
#if ASTARDEBUG
if (DebugDraw)
{
for (int i = 0; i < orcaLines.Count; i++)
{
Debug.DrawRay(position + new Vector3(orcaLines[i].point.x, 0, orcaLines[i].point.y) - new Vector3(orcaLines[i].dir.x, 0, orcaLines[i].dir.y) * 100, new Vector3(orcaLines[i].dir.x, 0, orcaLines[i].dir.y) * 200, Color.blue);
}
}
#endif
Vector2 resultVelocity = Vector2.zero;
int lineFail = LinearProgram2(orcaLines, maxSpeed, new Vector2(desiredVelocity.x, desiredVelocity.z), false, ref resultVelocity);
if (lineFail < orcaLines.Count)
{
LinearProgram3(orcaLines, numObstacleLines, lineFail, maxSpeed, ref resultVelocity);
}
newVelocity = new Vector3(resultVelocity.x, 0, resultVelocity.y);
newVelocity = Vector3.ClampMagnitude(newVelocity, maxSpeed);
#if ASTARDEBUG
if (DebugDraw)
{
Debug.DrawRay(position, newVelocity, Color.magenta);
}
#endif
}
public ObstacleVertex AddObstacle(Vector3[] vertices, float height, Matrix4x4 matrix, RVOLayer layer = RVOLayer.DefaultObstacle)
{
if (vertices == null)
{
throw new ArgumentNullException("Vertices must not be null");
}
if (vertices.Length < 2)
{
throw new ArgumentException("Less than 2 vertices in an obstacle");
}
ObstacleVertex obstacleVertex = null;
ObstacleVertex obstacleVertex2 = null;
bool flag = matrix == Matrix4x4.identity;
if (this.Multithreading && this.doubleBuffering)
{
for (int i = 0; i < this.workers.Length; i++)
{
this.workers[i].WaitOne();
}
}
for (int j = 0; j < vertices.Length; j++)
{
ObstacleVertex obstacleVertex3 = new ObstacleVertex();
if (obstacleVertex == null)
{
obstacleVertex = obstacleVertex3;
}
else
{
obstacleVertex2.next = obstacleVertex3;
}
obstacleVertex3.prev = obstacleVertex2;
obstacleVertex3.layer = layer;
obstacleVertex3.position = ((!flag) ? matrix.MultiplyPoint3x4(vertices[j]) : vertices[j]);
obstacleVertex3.height = height;
obstacleVertex2 = obstacleVertex3;
}
obstacleVertex2.next = obstacleVertex;
obstacleVertex.prev = obstacleVertex2;
ObstacleVertex obstacleVertex4 = obstacleVertex;
do
{
Vector3 vector = obstacleVertex4.next.position - obstacleVertex4.position;
ObstacleVertex arg_13E_0 = obstacleVertex4;
Vector2 vector2 = new Vector2(vector.x, vector.z);
arg_13E_0.dir = vector2.normalized;
obstacleVertex4 = obstacleVertex4.next;
}
while (obstacleVertex4 != obstacleVertex);
this.obstacles.Add(obstacleVertex);
this.UpdateObstacles();
return obstacleVertex;
}
/** Adds an obstacle described by the vertices.
*
* \see RemoveObstacle
*/
public ObstacleVertex AddObstacle (Vector3[] vertices, float height, Matrix4x4 matrix, RVOLayer layer = RVOLayer.DefaultObstacle) {
if (vertices == null) throw new System.ArgumentNullException ("Vertices must not be null");
if (vertices.Length < 2) throw new System.ArgumentException ("Less than 2 vertices in an obstacle");
ObstacleVertex first = null;
ObstacleVertex prev = null;
bool identity = matrix == Matrix4x4.identity;
//Don't interfere with ongoing calculations
if (Multithreading && doubleBuffering) for (int j=0;j<workers.Length;j++) workers[j].WaitOne();
for (int i=0;i<vertices.Length;i++) {
ObstacleVertex v = new ObstacleVertex();
if (first == null) first = v;
else prev.next = v;
v.prev = prev;
v.layer = layer;
//Premature optimization ftw!
v.position = identity ? vertices[i] : matrix.MultiplyPoint3x4(vertices[i]);
//v.thin = thin;
v.height = height;
prev = v;
}
prev.next = first;
first.prev = prev;
ObstacleVertex c = first;
do {
Vector3 dir = c.next.position - c.position;
c.dir = new Vector2 (dir.x,dir.z).normalized;
c = c.next;
} while (c != first);
obstacles.Add (first);
UpdateObstacles ();
return first;
}
/** Updates the vertices of an obstacle.
* \param obstacle %Obstacle to update
* \param vertices New vertices for the obstacle, must have at least the number of vertices in the original obstacle
* \param matrix %Matrix to multiply vertices with before updating obstacle
*
* The number of vertices in an obstacle cannot be changed, existing vertices can only be moved.
*/
public void UpdateObstacle (ObstacleVertex obstacle, Vector3[] vertices, Matrix4x4 matrix) {
if (vertices == null) throw new System.ArgumentNullException ("Vertices must not be null");
if (obstacle == null) throw new System.ArgumentNullException ("Obstacle must not be null");
if (vertices.Length < 2) throw new System.ArgumentException ("Less than 2 vertices in an obstacle");
if (obstacle.split) throw new System.ArgumentException ("Obstacle is not a start vertex. You should only pass those ObstacleVertices got from AddObstacle method calls");
//Don't interfere with ongoing calculations
if (Multithreading && doubleBuffering) for (int j=0;j<workers.Length;j++) workers[j].WaitOne();
//Compact obstacle and count
int count = 0;
ObstacleVertex c = obstacle;
do {
while (c.next.split) {
c.next = c.next.next;
c.next.prev = c;
}
if (count >= vertices.Length) {
Debug.DrawLine (c.prev.position, c.position,Color.red);
throw new System.ArgumentException ("Obstacle has more vertices than supplied for updating (" + vertices.Length+ " supplied)");
}
c.position = matrix.MultiplyPoint3x4 (vertices[count]);
count++;
c = c.next;
} while (c != obstacle);
c = obstacle;
do {
Vector3 dir = c.next.position - c.position;
c.dir = new Vector2 (dir.x,dir.z).normalized;
c = c.next;
} while (c != obstacle);
ScheduleCleanObstacles ();
UpdateObstacles();
}
/**
* Adds a line obstacle with a specified height.
*
* \see RemoveObstacle
*/
public ObstacleVertex AddObstacle (Vector3 a, Vector3 b, float height) {
ObstacleVertex first = new ObstacleVertex ();
ObstacleVertex second = new ObstacleVertex ();
first.layer = RVOLayer.DefaultObstacle;
second.layer = RVOLayer.DefaultObstacle;
first.prev = second;
second.prev = first;
first.next = second;
second.next = first;
first.position = a;
second.position = b;
first.height = height;
second.height = height;
second.ignore = true;
first.dir = new Vector2 (b.x-a.x,b.z-a.z).normalized;
second.dir = -first.dir;
//Don't interfere with ongoing calculations
if (Multithreading && doubleBuffering) for (int j=0;j<workers.Length;j++) workers[j].WaitOne();
obstacles.Add (first);
UpdateObstacles ();
return first;
}
public void RemoveObstacle(ObstacleVertex v)
{
if (v == null)
{
throw new ArgumentNullException("Vertex must not be null");
}
if (this.Multithreading && this.doubleBuffering)
{
for (int i = 0; i < this.workers.Length; i++)
{
this.workers[i].WaitOne();
}
}
this.obstacles.Remove(v);
this.UpdateObstacles();
}
/** Removes the obstacle identified by the vertex.
* This must be the same vertex as the one returned by the AddObstacle call.
*
* \see AddObstacle
*/
public void RemoveObstacle (ObstacleVertex v) {
if (v == null) throw new System.ArgumentNullException ("Vertex must not be null");
//Don't interfere with ongoing calculations
if (Multithreading && doubleBuffering) for (int j=0;j<workers.Length;j++) workers[j].WaitOne();
obstacles.Remove (v);
UpdateObstacles ();
}
public void UpdateObstacle(ObstacleVertex obstacle, Vector3[] vertices, Matrix4x4 matrix)
{
if (vertices == null)
{
throw new ArgumentNullException("Vertices must not be null");
}
if (obstacle == null)
{
throw new ArgumentNullException("Obstacle must not be null");
}
if (vertices.Length < 2)
{
throw new ArgumentException("Less than 2 vertices in an obstacle");
}
if (obstacle.split)
{
throw new ArgumentException("Obstacle is not a start vertex. You should only pass those ObstacleVertices got from AddObstacle method calls");
}
if (this.Multithreading && this.doubleBuffering)
{
for (int i = 0; i < this.workers.Length; i++)
{
this.workers[i].WaitOne();
}
}
int num = 0;
ObstacleVertex obstacleVertex = obstacle;
while (true)
{
while (obstacleVertex.next.split)
{
obstacleVertex.next = obstacleVertex.next.next;
obstacleVertex.next.prev = obstacleVertex;
}
if (num >= vertices.Length)
{
break;
}
obstacleVertex.position = matrix.MultiplyPoint3x4(vertices[num]);
num++;
obstacleVertex = obstacleVertex.next;
if (obstacleVertex == obstacle)
{
goto Block_9;
}
}
Debug.DrawLine(obstacleVertex.prev.position, obstacleVertex.position, Color.red);
throw new ArgumentException("Obstacle has more vertices than supplied for updating (" + vertices.Length + " supplied)");
Block_9:
obstacleVertex = obstacle;
do
{
Vector3 vector = obstacleVertex.next.position - obstacleVertex.position;
ObstacleVertex arg_160_0 = obstacleVertex;
Vector2 vector2 = new Vector2(vector.x, vector.z);
arg_160_0.dir = vector2.normalized;
obstacleVertex = obstacleVertex.next;
}
while (obstacleVertex != obstacle);
this.ScheduleCleanObstacles();
this.UpdateObstacles();
}
