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;
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));
}
