/** Removes obstacles which were added with AddGraphObstacles */ public void RemoveObstacles () { if (lastSim == null) return; Simulator sim = lastSim; lastSim = null; for (int i=0;i<obstacles.Count;i++) sim.RemoveObstacle (obstacles[i]); obstacles.Clear (); }
public void Start () { cam = Camera.main; var simu = FindObjectOfType(typeof(RVOSimulator)) as RVOSimulator; if ( simu == null ) { this.enabled = false; throw new System.Exception ("No RVOSimulator in the scene. Please add one"); } sim = simu.GetSimulator(); }
void Awake () { if (desiredSimulationFPS < 1) desiredSimulationFPS = 1; if (simulator == null) { int threadCount = AstarPath.CalculateThreadCount (workerThreads); simulator = new Simulator (threadCount, doubleBuffering); simulator.Interpolation = interpolation; simulator.DesiredDeltaTime = 1.0f / desiredSimulationFPS; } /*Debug.LogWarning ("RVO Local Avoidance is temporarily disabled in the A* Pathfinding Project due to licensing issues.\n" + "I am working to get it back as soon as possible. All agents will fall back to not avoiding other agents.\n" + "Sorry for the inconvenience.");*/ }
public void Awake () { tr = transform; var sim = FindObjectOfType(typeof(RVOSimulator)) as RVOSimulator; if (sim == null) { Debug.LogError ("No RVOSimulator component found in the scene. Please add one."); return; } simulator = sim.GetSimulator (); }
/** Adds obstacles for a graph */ public void AddGraphObstacles (Simulator sim, NavGraph graph) { if (obstacles.Count > 0 && lastSim != null && lastSim != sim) { Debug.LogError ("Simulator has changed but some old obstacles are still added for the previous simulator. Deleting previous obstacles."); RemoveObstacles (); } //Remember which simulator these obstacles were added to lastSim = sim; INavmesh ng = graph as INavmesh; if (ng == null) return; //Assume less than 20 vertices per node (actually assumes 3, but I will change that some day) int[] uses = new int[20]; ng.GetNodes (delegate(GraphNode _node) { TriangleMeshNode node = _node as TriangleMeshNode; uses[0] = uses[1] = uses[2] = 0; if (node != null) { //Find out which edges are shared with other nodes for (int j=0;j<node.connections.Length;j++) { TriangleMeshNode other = node.connections[j] as TriangleMeshNode; // Not necessarily a TriangleMeshNode if (other != null) { int a = node.SharedEdge(other); if (a != -1) uses[a] = 1; } } //Loop through all edges on the node for (int j=0;j<3;j++) { //The edge is not shared with any other node //I.e it is an exterior edge on the mesh if (uses[j] == 0) { //The two vertices of the edge Vector3 v1 = (Vector3)node.GetVertex(j); Vector3 v2 = (Vector3)node.GetVertex((j+1) % node.GetVertexCount()); //I think node vertices always should be clockwise, but it's good to be certain /*if (!Polygon.IsClockwise (v1,v2,(Vector3)node.GetVertex((j+2) % node.GetVertexCount()))) { Vector3 tmp = v2; v2 = v1; v1 = tmp; }*/ #if ASTARDEBUG Debug.DrawLine (v1,v2,Color.red); Debug.DrawRay (v1,Vector3.up*wallHeight,Color.red); #endif //Find out the height of the wall/obstacle we are about to add float height = System.Math.Abs(v1.y-v2.y); height = System.Math.Max (height,5); //Enqueue the edge as a line obstacle obstacles.Add (sim.AddObstacle (v1, v2, wallHeight)); } } } return true; }); }
internal void CalculateVelocity ( Simulator.WorkerContext context ) { if ( locked ) { newVelocity = Vector2.zero; return; } if ( context.vos.Length < neighbours.Count+simulator.obstacles.Count ) { context.vos = new VO[Mathf.Max(context.vos.Length*2, neighbours.Count+simulator.obstacles.Count)]; } Vector2 position2D = new Vector2(position.x,position.z); var vos = context.vos; var voCount = 0; Vector2 optimalVelocity = new Vector2(velocity.x, velocity.z); float inverseAgentTimeHorizon = 1.0f/agentTimeHorizon; float wallThickness = simulator.WallThickness; float wallWeight = simulator.algorithm == Simulator.SamplingAlgorithm.GradientDecent ? 1 : WallWeight; for ( int i = 0; i < simulator.obstacles.Count; i++ ) { var obstacle = simulator.obstacles[i]; var vertex = obstacle; do { if ( vertex.ignore || position.y > vertex.position.y + vertex.height || position.y+height < vertex.position.y || (vertex.layer & collidesWith) == 0 ) { vertex = vertex.next; continue; } float cross = VO.Det (new Vector2(vertex.position.x,vertex.position.z), vertex.dir, position2D);// vertex.dir.x * ( vertex.position.z - position.z ) - vertex.dir.y * ( vertex.position.x - position.x ); // Signed distance from the line (not segment), lines are infinite // Usually divided by vertex.dir.magnitude, but that is known to be 1 float signedDist = cross; float dotFactor = Vector2.Dot (vertex.dir, position2D - new Vector2(vertex.position.x,vertex.position.z)); // It is closest to the segment // if the dotFactor is <= 0 or >= length of the segment // WallThickness*0.1 is added as a margin to avoid false positives when moving along the edges of square obstacles bool closestIsEndpoints = dotFactor <= wallThickness*0.05f || dotFactor >= (new Vector2(vertex.position.x,vertex.position.z) - new Vector2(vertex.next.position.x,vertex.next.position.z)).magnitude - wallThickness*0.05f; if ( Mathf.Abs (signedDist) < neighbourDist ) { if ( signedDist <= 0 && !closestIsEndpoints && signedDist > -wallThickness ) { // Inside the wall on the "wrong" side vos[voCount] = new VO (position2D, new Vector2(vertex.position.x, vertex.position.z) - position2D, vertex.dir, wallWeight*2); voCount++; } else if ( signedDist > 0 ) { //Debug.DrawLine (position, (vertex.position+vertex.next.position)*0.5f, Color.yellow); Vector2 p1 = new Vector2(vertex.position.x, vertex.position.z) - position2D; Vector2 p2 = new Vector2(vertex.next.position.x, vertex.next.position.z) - position2D; Vector2 tang1 = (p1).normalized; Vector2 tang2 = (p2).normalized; vos[voCount] = new VO (position2D, p1, p2, tang1, tang2, wallWeight); voCount++; } } vertex = vertex.next; } while (vertex != obstacle); } for ( int o = 0; o < neighbours.Count; o++ ) { Agent other = neighbours[o]; if ( other == this ) continue; 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; } Vector2 otherOptimalVelocity = new Vector2(other.Velocity.x, other.velocity.z); float totalRadius = radius + other.radius; // Describes a circle on the border of the VO //float boundingRadius = totalRadius * inverseAgentTimeHorizon; Vector2 voBoundingOrigin = new Vector2(other.position.x,other.position.z) - position2D; //float boundingDist = voBoundingOrigin.magnitude; Vector2 relativeVelocity = optimalVelocity - otherOptimalVelocity; { //voBoundingOrigin *= inverseAgentTimeHorizon; //boundingDist *= inverseAgentTimeHorizon; // Common case, no collision Vector2 voCenter; if ( other.locked ) { voCenter = otherOptimalVelocity; } else { voCenter = (optimalVelocity + otherOptimalVelocity)*0.5f; } vos[voCount] = new VO( voBoundingOrigin, voCenter, totalRadius, relativeVelocity, inverseAgentTimeHorizon, 1); voCount++; if (DebugDraw) DrawVO ( position2D + voBoundingOrigin*inverseAgentTimeHorizon + voCenter, totalRadius*inverseAgentTimeHorizon, position2D + voCenter); } } Vector2 result = Vector2.zero; if ( simulator.algorithm == Simulator.SamplingAlgorithm.GradientDecent ) { if ( DebugDraw ) { const int PlotWidth = 40; const float WorldPlotWidth = 15; for ( int x = 0; x < PlotWidth; x++ ) { for ( int y = 0; y < PlotWidth; y++ ) { Vector2 p = new Vector2 (x*WorldPlotWidth / PlotWidth, y*WorldPlotWidth / PlotWidth); Vector2 dir = Vector2.zero; float weight = 0; for ( int i = 0; i < voCount; i++ ) { float w; dir += vos[i].Sample (p-position2D, out w); if ( w > weight ) weight = w; } Vector2 d2 = (new Vector2(desiredVelocity.x,desiredVelocity.z) - (p-position2D)); dir += d2*DesiredVelocityScale; if ( d2.magnitude * DesiredVelocityWeight > weight ) weight = d2.magnitude * DesiredVelocityWeight; if ( weight > 0 ) dir /= weight; //Vector2 d3 = simulator.SampleDensity (p+position2D); Debug.DrawRay ( To3D(p), To3D(d2*0.00f), Color.blue ); //simulator.Plot (p, Rainbow(weight*simulator.colorScale)); float sc = 0; Vector2 p0 = p - Vector2.one*WorldPlotWidth*0.5f; Vector2 p1 = Trace (vos, voCount, p0, 0.01f, out sc); if ( (p0 - p1).sqrMagnitude < Sqr(WorldPlotWidth / PlotWidth)*2.6f ) { Debug.DrawRay ( To3D(p1 + position2D), Vector3.up*1, Color.red); } } } } //if ( debug ) { float best = float.PositiveInfinity; float cutoff = new Vector2(velocity.x,velocity.z).magnitude*simulator.qualityCutoff; //for ( int i = 0; i < 10; i++ ) { { result = Trace ( vos, voCount, new Vector2(desiredVelocity.x, desiredVelocity.z), cutoff, out best ); if (DebugDraw) DrawCross (result+position2D, Color.yellow, 0.5f); } // Can be uncommented for higher quality local avoidance /*for ( int i = 0; i < 3; i++ ) { Vector2 p = desiredVelocity + new Vector2(Mathf.Cos(Mathf.PI*2*(i/3.0f)), Mathf.Sin(Mathf.PI*2*(i/3.0f))); float score; Vector2 res = Trace ( vos, voCount, p, velocity.magnitude*simulator.qualityCutoff, out score ); if ( score < best ) { //if ( score < best*0.9f ) Debug.Log ("Better " + score + " < " + best); result = res; best = score; } }*/ { Vector2 p = Velocity; float score; Vector2 res = Trace ( vos, voCount, p, cutoff, out score ); if ( score < best ) { //if ( score < best*0.9f ) Debug.Log ("Better " + score + " < " + best); result = res; best = score; } if (DebugDraw) DrawCross (res+position2D, Color.magenta, 0.5f); } } else { // Adaptive sampling Vector2[] samplePos = context.samplePos; float[] sampleSize = context.sampleSize; int samplePosCount = 0; Vector2 desired2D = new Vector2(desiredVelocity.x,desiredVelocity.z); float sampleScale = Mathf.Max (radius, Mathf.Max (desired2D.magnitude, Velocity.magnitude)); samplePos[samplePosCount] = desired2D; sampleSize[samplePosCount] = sampleScale*0.3f; samplePosCount++; const float GridScale = 0.3f; // Initial 9 samples samplePos[samplePosCount] = optimalVelocity; sampleSize[samplePosCount] = sampleScale*GridScale; samplePosCount++; { Vector2 fw = optimalVelocity * 0.5f; Vector2 rw = new Vector2(fw.y, -fw.x); const int Steps = 8; for ( int i = 0; i < Steps; i++ ) { samplePos[samplePosCount] = rw * Mathf.Sin(i*Mathf.PI*2 / Steps) + fw * ( 1 + Mathf.Cos(i*Mathf.PI*2 / Steps) ); sampleSize[samplePosCount] = (1.0f - (Mathf.Abs(i - Steps*0.5f)/Steps))*sampleScale*0.5f; samplePosCount++; } const float InnerScale = 0.6f; fw *= InnerScale; rw *= InnerScale; const int Steps2 = 6; for ( int i = 0; i < Steps2; i++ ) { samplePos[samplePosCount] = rw * Mathf.Cos((i+0.5f)*Mathf.PI*2 / Steps2) + fw * ( (1.0f/InnerScale) + Mathf.Sin((i+0.5f)*Mathf.PI*2 / Steps2) ); sampleSize[samplePosCount] = sampleScale*0.3f; samplePosCount++; } const float TargetScale = 0.2f; const int Steps3 = 6; for ( int i = 0; i < Steps3; i++ ) { samplePos[samplePosCount] = optimalVelocity + new Vector2 ( sampleScale * TargetScale * Mathf.Cos((i+0.5f)*Mathf.PI*2 / Steps3), sampleScale * TargetScale * Mathf.Sin((i+0.5f)*Mathf.PI*2 / Steps3) ); sampleSize[samplePosCount] = sampleScale*TargetScale*2; samplePosCount++; } } samplePos[samplePosCount] = optimalVelocity*0.5f; sampleSize[samplePosCount] = sampleScale*0.4f; samplePosCount++; const int KeepCount = Simulator.WorkerContext.KeepCount; Vector2[] bestPos = context.bestPos; float[] bestSizes = context.bestSizes; float[] bestScores = context.bestScores; for ( int i = 0; i < KeepCount; i++ ) { bestScores[i] = float.PositiveInfinity; } bestScores[KeepCount] = float.NegativeInfinity; Vector2 bestEver = optimalVelocity; float bestEverScore = float.PositiveInfinity; for ( int sub = 0; sub < 3; sub++ ) { for ( int i = 0; i < samplePosCount; i++ ) { float score = 0; for ( int vo = 0; vo < voCount; vo++ ) { score = System.Math.Max (score, vos[vo].ScalarSample ( samplePos[i] )); } // Note that velocity is a vector and speed is a scalar, not the same thing float bonusForDesiredVelocity = (samplePos[i] - desired2D).magnitude; // This didn't work out as well as I though // Code left here because I might reenable it later //float bonusForDesiredSpeed = Mathf.Abs (samplePos[i].magnitude - desired2D.magnitude); float biasedScore = score + bonusForDesiredVelocity*DesiredVelocityWeight;// + bonusForDesiredSpeed*0; score += bonusForDesiredVelocity*0.001f; if ( DebugDraw ) { DrawCross ( position2D + samplePos[i], Rainbow(Mathf.Log(score+1)*5), sampleSize[i]*0.5f ); } if ( biasedScore < bestScores[0] ) { for ( int j = 0; j < KeepCount; j++ ) { if ( biasedScore >= bestScores[j+1]) { bestScores[j] = biasedScore; bestSizes[j] = sampleSize[i]; bestPos[j] = samplePos[i]; break; } } } if ( score < bestEverScore ) { bestEver = samplePos[i]; bestEverScore = score; if ( score == 0 ) { sub = 100; break; } } } samplePosCount = 0; for ( int i = 0; i < KeepCount; i++ ) { Vector2 p = bestPos[i]; float s = bestSizes[i]; bestScores[i] = float.PositiveInfinity; const float Half = 0.6f; float offset = s * Half * 0.5f; samplePos[samplePosCount+0] = ( p + new Vector2 ( +offset, +offset ) ); samplePos[samplePosCount+1] = ( p + new Vector2 ( -offset, +offset ) ); samplePos[samplePosCount+2] = ( p + new Vector2 ( -offset, -offset ) ); samplePos[samplePosCount+3] = ( p + new Vector2 ( +offset, -offset ) ); s *= s * Half; sampleSize[samplePosCount+0] = ( s ); sampleSize[samplePosCount+1] = ( s ); sampleSize[samplePosCount+2] = ( s ); sampleSize[samplePosCount+3] = ( s ); samplePosCount+= 4; } } result = bestEver; } if (DebugDraw) DrawCross (result+position2D); newVelocity = To3D(Vector2.ClampMagnitude (result, maxSpeed)); }
public Worker (Simulator sim) { this.simulator = sim; thread = new Thread (new ThreadStart (Run)); thread.IsBackground = true; thread.Name = "RVO Simulator Thread"; thread.Start (); }
public void Start () { mesh = new Mesh(); RVOSimulator rvoSim = FindObjectOfType (typeof(RVOSimulator)) as RVOSimulator; if (rvoSim == null) { Debug.LogError ("No RVOSimulator could be found in the scene. Please add a RVOSimulator component to any GameObject"); return; } sim = rvoSim.GetSimulator(); GetComponent<MeshFilter>().mesh = mesh; CreateAgents (agentCount); }
/** Finds a simulator in the scene. * * Saves found simulator in #sim. * * \throws System.InvalidOperationException When no RVOSimulator could be found. */ protected void FindSimulator () { RVOSimulator rvosim = FindObjectOfType(typeof(RVOSimulator)) as RVOSimulator; if (rvosim == null) throw new System.InvalidOperationException ("No RVOSimulator could be found in the scene. Please add one to any GameObject"); sim = rvosim.GetSimulator (); }