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;
Debug.Log("continue");
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 (this.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 = 0;
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 = this.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));
if (newVelocity != Vector3.zero)
{
float angle = Vector3.Angle(DesiredVelocity, newVelocity);
//:限制新速度方向
//如果计算出来的新速度是与设计速度的夹角大于angleMax
//则新速度旋转到最大夹角
float angleMax = 45;
if (angle > angleMax)
{
Vector2 firstObject = new Vector2(DesiredVelocity.x, DesiredVelocity.z);
Vector2 secondObject = new Vector2(newVelocity.x, newVelocity.z);
float det = firstObject.x * secondObject.y - firstObject.y * secondObject.x;
if (det > 0)//param2 is unclockwise
{
Vector2 verticalObject = secondObject.Rotate(-(angle - angleMax));
newVelocity.x = verticalObject.x;
newVelocity.z = verticalObject.y;
if (newVelocity.magnitude < minSpeed)
{
newVelocity = newVelocity.normalized * minSpeed;
}
}
else if (det < 0)
{
Vector2 verticalObject = secondObject.Rotate((angle - angleMax));
newVelocity.x = verticalObject.x;
newVelocity.z = verticalObject.y;
if (newVelocity.magnitude < minSpeed)
{
newVelocity = newVelocity.normalized * minSpeed;
}
}
else
{
newVelocity = Vector3.zero;
}
}
if (printVelocity)
{
Debug.Log("vel " + velocity + "newVelocity " + newVelocity);
}
}
}
public Vector2 RelativeToWorld(Vector2 vector)
{
return vector.Rotate(Angle - 90);
}
public Vector2 WorldToRelative(Vector2 vector)
{
return vector.Rotate(-Angle + 90);
}
int[][][] CreateRotated(int[][] pixels, ref int w, ref int h, int count)
{
var pw = w;
var ph = h;
var pcx = w / 2;
var pcy = h / 2;
w = Mathf.RoundToInt(w * 1.5f / 2) * 2;
h = Mathf.RoundToInt(h * 1.5f / 2) * 2;
var cx = w / 2;
var cy = h / 2;
var _w = w;
var _h = h;
var offset = new Vector2();
return count.Range().Select(ai =>
{
var angle = (float)ai * 360 / count;
return _h.Range().Select(y =>
{
return _w.Range().Select(x =>
{
offset.x = x - cx;
offset.y = y - cy;
offset = offset.Rotate(angle);
var px = Mathf.RoundToInt(offset.x + pcx);
var py = Mathf.RoundToInt(offset.y + pcy);
if (px < 0 || px >= pw || py < 0 || py >= ph)
{
return 0;
}
return pixels[py][px];
}).ToArray();
}).ToArray();
}).ToArray();
}
