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