internal void CalculateVelocity(Pathfinding.RVO.Simulator.WorkerContext context)
{
if (manuallyControlled)
{
return;
}
if (locked)
{
calculatedSpeed = 0;
calculatedTargetPoint = position;
return;
}
// Calculate the desired velocity from the point we want to reach
desiredVelocity = desiredTargetPointInVelocitySpace.normalized * desiredSpeed;
// Buffer which will be filled up with velocity obstacles (VOs)
var vos = context.vos;
vos.Clear();
GenerateObstacleVOs(vos);
GenerateNeighbourAgentVOs(vos);
bool insideAnyVO = BiasDesiredVelocity(vos, ref desiredVelocity, ref desiredTargetPointInVelocitySpace, simulator.symmetryBreakingBias);
if (!insideAnyVO)
{
// Desired velocity can be used directly since it was not inside any velocity obstacle.
// No need to run optimizer because this will be the global minima.
// This is also a special case in which we can set the
// calculated target point to the desired target point
// instead of calculating a point based on a calculated velocity
// which is an important difference when the agent is very close
// to the target point
// TODO: Not actually guaranteed to be global minima if desiredTargetPointInVelocitySpace.magnitude < desiredSpeed
// maybe do something different here?
calculatedTargetPoint = desiredTargetPointInVelocitySpace + position;
calculatedSpeed = desiredSpeed;
if (DebugDraw)
{
DrawCross(calculatedTargetPoint);
}
return;
}
Vector2 result = Vector2.zero;
result = GradientDescent(vos, currentVelocity, desiredVelocity);
if (DebugDraw)
{
DrawCross(result + position);
}
//Debug.DrawRay (To3D (position), To3D (result));
calculatedTargetPoint = position + result;
calculatedSpeed = Mathf.Min(result.magnitude, maxSpeed);
}
internal void CalculateVelocity(Pathfinding.RVO.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;
//#if !BNICKSON_UPDATED
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;
if (Mathf.Abs(signedDist) < neighbourDist)
{
#if BNICKSON_UPDATED
//from up move to there less call NearestPoint ---lzt
Vector3 lineToAgent = position - VectorMath.ClosestPointOnLine(obstacle.position, vertex.position, position);
lineToAgent.y = 0f;
if (Vector3.Dot(lineToAgent, obstacle.pushAwayDir) < 0f) // if the agent is behind the obstacle
{
continue; // we don't want this obstacle to stop the agent from getting back onto the nav mesh, alternately
// we could consider the agent to be in front of the obstacle for the orca calculations, the agent will then be pushed away from the obstacle
// which is probably desirable
}
#endif
//from up move to there less call ---lzt
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 (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);
}
//#endif
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 )EB.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 )EB.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));
}
internal void CalculateVelocity(Pathfinding.RVO.Simulator.WorkerContext context)
{
if (locked)
{
newVelocity = Vector2.zero;
return;
}
if (context.vos.Length < neighbours.Count)
{
context.vos = new VO[Mathf.Max(context.vos.Length * 2, neighbours.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;
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
vos[voCount] = new VO(voBoundingOrigin, (optimalVelocity + otherOptimalVelocity) * 0.5f, totalRadius, relativeVelocity, inverseAgentTimeHorizon);
voCount++;
}
#if OLD
//Debug.Log (boundingDist + " " + total
if (boundingDist < totalRadius)
{
// Collision
// We want the collision to end this frame, so ignore the inverseAgentTimeHorizon and
// instead use the frame's delta time instead. The closest point will not always be
// on the circle, but the velocity will have to be really
// high for it not to be, so just assume the velocity is low
// (if not, the agents will jump through each other)
voBoundingOrigin *= (1.0f / simulator.DeltaTime);
boundingDist *= (1.0f / simulator.DeltaTime);
Vector2 d = relativeVelocity - voBoundingOrigin;
float dm = d.magnitude;
// Normalize
d /= dm;
float resMagn = (totalRadius * (1.0f / simulator.DeltaTime) - dm);
maxRelVelMagn = System.Math.Max(resMagn, maxRelVelMagn);
Vector2 closestOnCircleDelta = d * resMagn; //d*(boundingDist-dm);
Vector2 closestOnCircleVector = d;
halfPlanes.Add(new HalfPlane(closestOnCircleDelta * 0.5f + optimalVelocity, closestOnCircleVector));
if (debug)
{
halfPlanes[halfPlanes.Count - 1].Draw(Color.blue);
}
}
else
{
voBoundingOrigin *= inverseAgentTimeHorizon;
boundingDist *= inverseAgentTimeHorizon;
// Common case, no collision
VO vo = new VO(voBoundingOrigin + (optimalVelocity + otherOptimalVelocity) * 0.5f, voBoundingOrigin.normalized, totalRadius * inverseAgentTimeHorizon);
maxRelVelMagn = System.Math.Max(maxRelVelMagn, relativeVelocity.magnitude);
// Optimized: float factor = Vector3.Dot ( optimalVelocity, voBoundingOrigin ) < (boundingDist^2);
if (debug)
{
DrawVO(voBoundingOrigin, boundingRadius, Vector2.zero);
}
if (debug)
{
//Debug.DrawLine ( To3D(position), To3D ( relativeVelocity ), Color.blue );
DrawCross(relativeVelocity, Color.white, 0.5f);
}
//DrawCross ( voBoundingOrigin );
float alpha = Mathf.Atan2(-voBoundingOrigin.y, -voBoundingOrigin.x);
float delta = Mathf.Abs(Mathf.Acos(boundingRadius / (voBoundingOrigin).magnitude));
float actualDot = Vector2.Dot(-voBoundingOrigin.normalized, (relativeVelocity - voBoundingOrigin).normalized);
float dot = Mathf.Abs(Mathf.Acos(Mathf.Clamp01(actualDot)));
bool closestIsCircle = dot < delta;
/*DrawCircle ( circleCenter, radius, alpha-delta, alpha+delta, Color.black );
* Vector2 p1 = circleCenter + new Vector2 ( Mathf.Cos (alpha-delta), Mathf.Sin (alpha-delta) ) * boundingRadius;
* Vector2 p2 = circleCenter + new Vector2 ( Mathf.Cos (alpha+delta), Mathf.Sin (alpha+delta) ) * boundingRadius;*/
if (debug)
{
Debug.Log(" <= 1 && " + dot + " < " + delta + " ( " + actualDot + ")");
}
if (closestIsCircle)
{
// The cutoff circle is the closest
// part of the VO
// Find the closest point on the VO
Vector2 closestOnCircleVector = (relativeVelocity - voBoundingOrigin);
float magn = closestOnCircleVector.magnitude;
closestOnCircleVector = closestOnCircleVector / magn; //(closestOnCircleVector/magn)*boundingRadius*(1-(magn/boundingRadius));
Vector2 closestOnCircle = closestOnCircleVector * boundingRadius + voBoundingOrigin;
//if ( magn > boundingRadius ) closestOnCircleVector = -closestOnCircleVector;
if (debug)
{
DrawCross(closestOnCircle, 1);
}
halfPlanes.Add(new HalfPlane((closestOnCircle - relativeVelocity) * 0.5f + optimalVelocity, closestOnCircleVector.normalized));
if (debug)
{
halfPlanes[halfPlanes.Count - 1].Draw(closestIsCircle ? Color.blue : Color.red);
}
}
else
{
// One of the legs of the VO is the closest part
// find the closest point on the VO
bool left = Polygon.Left(Vector2.zero, voBoundingOrigin, relativeVelocity);
float gamma = alpha + (left ? delta : -delta);
// Point on circle
Vector2 line = new Vector2(Mathf.Cos(gamma), Mathf.Sin(gamma)) * boundingRadius;
// Vector tangent to circle which is the correct line tangent
Vector2 dir = new Vector2(line.y, -line.x).normalized;
// Point in space
line += voBoundingOrigin;
Vector2 onLine = line + dir * Vector2.Dot(dir, relativeVelocity - line);
if (debug)
{
DrawCross(onLine, Color.red, 1);
Debug.DrawRay(To3D(line), To3D(dir * 10), Color.red);
Debug.Log(line + " " + dir + " " + gamma + " " + relativeVelocity);
}
if (!left)
{
dir = -dir;
}
halfPlanes.Add(new HalfPlane((onLine - relativeVelocity) * 0.5f + optimalVelocity, (line - voBoundingOrigin).normalized));
if (debug)
{
halfPlanes[halfPlanes.Count - 1].Draw(Color.blue);
}
}
}
#endif
}
if (this.DebugDraw)
{
/*for ( int x = 0; x < simulator.tex.width; x++ ) {
* for ( int y = 0; y < simulator.tex.height; y++ ) {
* Vector2 p = new Vector2 (x*simulator.textureSize / simulator.tex.width, y*simulator.textureSize / simulator.tex.height);
*
* Vector2 dir = Vector2.zero;
* float weight = 0;
* for ( int i = 0; i < voCount; i++ ) {
* float w = 0;
* dir += vos[i].Sample (p-position, out w);
* if ( w > weight ) weight = w;
* }
* Vector2 d2 = (desiredVelocity - (p-position));
* dir += d2*DesiredVelocityScale;
*
* if ( d2.magnitude * DesiredVelocityWeight > weight ) weight = d2.magnitude * DesiredVelocityWeight;
*
* if ( weight > 0 ) dir /= weight;
*
* Vector2 d3 = simulator.SampleDensity (p+position);
* Debug.DrawRay ( To3D(p), To3D(d3*1f), Color.blue );
* simulator.Plot (p, Rainbow(weight*simulator.colorScale));
* }
* }*/
}
//if ( debug ) {
float best = float.PositiveInfinity;
Vector2 result = Vector2.zero;
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);
}
// 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 = Vector2.zero;
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(result + position2D);
}
newVelocity = To3D(Vector2.ClampMagnitude(result, maxSpeed));
}
