/// <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; }
/** 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(); }