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