/** Bias towards the right side of agents.
* Rotate desiredVelocity at most [value] number of radians. 1 radian ≈ 57°
* This breaks up symmetries.
*
* The desired velocity will only be rotated if it is inside a velocity obstacle (VO).
* If it is inside one, it will not be rotated further than to the edge of it
*
* The targetPointInVelocitySpace will be rotated by the same amount as the desired velocity
*
* \returns True if the desired velocity was inside any VO
*/
static bool BiasDesiredVelocity(VOBuffer vos, ref TSVector2 desiredVelocity, ref TSVector2 targetPointInVelocitySpace, FP maxBiasRadians)
{
var desiredVelocityMagn = desiredVelocity.magnitude;
FP maxValue = FP.Zero;
for (int i = 0; i < vos.length; i++)
{
FP value;
// The value is approximately the distance to the edge of the VO
// so taking the maximum will give us the distance to the edge of the VO
// which the desired velocity is furthest inside
vos.buffer[i].Gradient(desiredVelocity, out value);
maxValue = TSMath.Max(maxValue, value);
}
// Check if the agent was inside any VO
var inside = maxValue > 0;
// Avoid division by zero below
if (desiredVelocityMagn < FP.One / 1000)
{
return(inside);
}
// Rotate the desired velocity clockwise (to the right) at most maxBiasRadians number of radians
// Assuming maxBiasRadians is small, we can just move it instead and it will give approximately the same effect
// See https://en.wikipedia.org/wiki/Small-angle_approximation
var angle = TSMath.Min(maxBiasRadians, maxValue / desiredVelocityMagn);
desiredVelocity += new TSVector2(desiredVelocity.y, -desiredVelocity.x) * angle;
targetPointInVelocitySpace += new TSVector2(targetPointInVelocitySpace.y, -targetPointInVelocitySpace.x) * angle;
return(inside);
}
Vector2 GradientDescent(VOBuffer vos, Vector2 sampleAround1, Vector2 sampleAround2)
{
float score1;
var minima1 = Trace(vos, sampleAround1, out score1);
if (DebugDraw)
{
DrawCross(minima1 + position, 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, p, velocity.magnitude*simulator.qualityCutoff, out score );
//
// if ( score < best ) {
// result = res;
// best = score;
// }
// }
float score2;
Vector2 minima2 = Trace(vos, sampleAround2, out score2);
if (DebugDraw)
{
DrawCross(minima2 + position, Color.magenta, 0.5f);
}
return(score1 < score2 ? minima1 : minima2);
}
TSVector2 GradientDescent(VOBuffer vos, TSVector2 sampleAround1, TSVector2 sampleAround2)
{
FP score1;
var minima1 = Trace(vos, sampleAround1, out score1);
// if (DebugDraw) Draw.Debug.CrossXZ(FromXZ(minima1 + position), Color.yellow, 0.5f);
// Can be uncommented for higher quality local avoidance
// for ( int i = 0; i < 3; i++ ) {
// TSVector2 p = desiredVelocity + new TSVector2(TSMath.Cos(TSMath.PI*2*(i/3)), TSMath.Sin(TSMath.PI*2*(i/3)));
// FP score;TSVector2 res = Trace ( vos, p, velocity.magnitude*simulator.qualityCutoff, out score );
//
// if ( score < best ) {
// result = res;
// best = score;
// }
// }
FP score2;
TSVector2 minima2 = Trace(vos, sampleAround2, out score2);
// if (DebugDraw) Draw.Debug.CrossXZ(FromXZ(minima2 + position), Color.magenta, 0.5f);
return(score1 < score2 ? minima1 : minima2);
}
/** Evaluate gradient and value of the cost function at velocity p */
TSVector2 EvaluateGradient(VOBuffer vos, TSVector2 p, out FP value)
{
TSVector2 gradient = TSVector2.zero;
value = 0;
// Avoid other agents
for (int i = 0; i < vos.length; i++)
{
FP w;
var grad = vos.buffer[i].ScaledGradient(p, out w);
if (w > value)
{
value = w;
gradient = grad;
}
}
// Move closer to the desired velocity
var dirToDesiredVelocity = desiredVelocity - p;
var distToDesiredVelocity = dirToDesiredVelocity.magnitude;
if (distToDesiredVelocity > CustomMath.EPSILON)
{
gradient += dirToDesiredVelocity * (DesiredVelocityWeight / distToDesiredVelocity);
value += distToDesiredVelocity * DesiredVelocityWeight;
}
// Prefer speeds lower or equal to the desired speed
// and avoid speeds greater than the max speed
var sqrSpeed = p.LengthSquared();
if (sqrSpeed > desiredSpeed * desiredSpeed)
{
var speed = TSMath.Sqrt(sqrSpeed);
if (speed > maxSpeed)
{
FP MaxSpeedWeight = 3;
value += MaxSpeedWeight * (speed - maxSpeed);
gradient -= MaxSpeedWeight * (p / speed);
}
// Scale needs to be strictly greater than DesiredVelocityWeight
// otherwise the agent will not prefer the desired speed over
// the maximum speed
FP scale = 2 * DesiredVelocityWeight;
value += scale * (speed - desiredSpeed);
gradient -= scale * (p / speed);
}
return(gradient);
}
void GenerateObstacleVOs(VOBuffer vos)
{
var range = maxSpeed * obstacleTimeHorizon;
// Iterate through all obstacles that we might need to avoid
for (int i = 0; i < simulator.obstacles.Count; i++)
{
var obstacle = simulator.obstacles[i];
var vertex = obstacle;
// Iterate through all edges (defined by vertex and vertex.dir) in the obstacle
do
{
// Ignore the edge if the agent should not collide with it
if (vertex.ignore || (vertex.layer & collidesWith) == 0)
{
vertex = vertex.next;
continue;
}
// Start and end points of the current segment
float elevation1, elevation2;
var p1 = To2D(vertex.position, out elevation1);
var p2 = To2D(vertex.next.position, out elevation2);
Vector2 dir = (p2 - p1).normalized;
// Signed distance from the line (not segment, lines are infinite)
// TODO: Can be optimized
float dist = VO.SignedDistanceFromLine(p1, dir, position);
if (dist >= -0.01f && dist < range)
{
float factorAlongSegment = Vector2.Dot(position - p1, p2 - p1) / (p2 - p1).sqrMagnitude;
// Calculate the elevation (y) coordinate of the point on the segment closest to the agent
var segmentY = Mathf.Lerp(elevation1, elevation2, factorAlongSegment);
// Calculate distance from the segment (not line)
var sqrDistToSegment = (Vector2.Lerp(p1, p2, factorAlongSegment) - position).sqrMagnitude;
// Ignore the segment if it is too far away
// or the agent is too high up (or too far down) on the elevation axis (usually y axis) to avoid it.
// If the XY plane is used then all elevation checks are disabled
if (sqrDistToSegment < range * range && (simulator.movementPlane == MovementPlane.XY || (elevationCoordinate <= segmentY + vertex.height && elevationCoordinate + height >= segmentY)))
{
vos.Add(VO.SegmentObstacle(p2 - position, p1 - position, Vector2.zero, radius * 0.01f, 1f / ObstacleTimeHorizon, 1f / simulator.DeltaTime));
}
}
vertex = vertex.next;
} while (vertex != obstacle && vertex != null && vertex.next != null);
}
}
/// <summary>
/// Traces the vector field constructed out of the velocity obstacles.
/// Returns the position which gives the minimum score (approximately).
///
/// See: https://en.wikipedia.org/wiki/Gradient_descent
/// </summary>
Vector2 Trace(VOBuffer vos, Vector2 p, out float score)
{
// Pick a reasonable initial step size
float stepSize = Mathf.Max(radius, 0.2f * desiredSpeed);
float bestScore = float.PositiveInfinity;
Vector2 bestP = p;
// TODO: Add momentum to speed up convergence?
const int MaxIterations = 50;
for (int s = 0; s < MaxIterations; s++)
{
float step = 1.0f - (s / (float)MaxIterations);
step = Sqr(step) * stepSize;
float value;
var gradient = EvaluateGradient(vos, p, out value);
if (value < bestScore)
{
bestScore = value;
bestP = p;
}
// TODO: Add cutoff for performance
gradient.Normalize();
gradient *= step;
Vector2 prev = p;
p += gradient;
if (DebugDraw)
{
Debug.DrawLine(FromXZ(prev + position), FromXZ(p + position), Rainbow(s * 0.1f) * new Color(1, 1, 1, 1f));
}
}
score = bestScore;
return(bestP);
}
/** Traces the vector field constructed out of the velocity obstacles.
* Returns the position which gives the minimum score (approximately).
*
* \see https://en.wikipedia.org/wiki/Gradient_descent
*/
TSVector2 Trace(VOBuffer vos, TSVector2 p, out FP score)
{
// Pick a reasonable initial step size
FP stepSize = TSMath.Max(radius, FP.One / 5 * desiredSpeed);
FP bestScore = FP.MaxValue;
TSVector2 bestP = p;
// TODO: Add momentum to speed up convergence?
const int MaxIterations = 50;
for (int s = 0; s < MaxIterations; s++)
{
FP step = 1 - (s * FP.One / MaxIterations);
step = Sqr(step) * stepSize;
FP value;
var gradient = EvaluateGradient(vos, p, out value);
if (value < bestScore)
{
bestScore = value;
bestP = p;
}
// TODO: Add cutoff for performance
gradient.Normalize();
gradient *= step;
TSVector2 prev = p;
p += gradient;
// if (DebugDraw) Debug.DrawLine(FromXZ(prev + position), FromXZ(p + position), Rainbow(s * 0.1f) * new Color(1, 1, 1, 1f));
}
score = bestScore;
return(bestP);
}
//void GenerateObstacleVOs(VOBuffer vos)
//{
// var range = maxSpeed * obstacleTimeHorizon;
// // Iterate through all obstacles that we might need to avoid
// for (int i = 0; i < simulator.obstacles.Count; i++)
// {
// var obstacle = simulator.obstacles[i];
// var vertex = obstacle;
// // Iterate through all edges (defined by vertex and vertex.dir) in the obstacle
// do
// {
// // Ignore the edge if the agent should not collide with it
// if (vertex.ignore || (vertex.layer & collidesWith) == 0)
// {
// vertex = vertex.next;
// continue;
// }
// // Start and end points of the current segment
// FP elevation1, elevation2;
// var p1 = To2D(vertex.position, out elevation1);
// var p2 = To2D(vertex.next.position, out elevation2);
// TSVector2 dir = (p2 - p1).normalized;
// // Signed distance from the line (not segment, lines are infinite)
// // TODO: Can be optimized
// FP dist = VO.SignedDistanceFromLine(p1, dir, position);
// if (dist >= -FP.One/100 && dist < range)
// {
// FP factorAlongSegment = TSVector2.Dot(position - p1, p2 - p1) / (p2 - p1).sqrMagnitude;
// // Calculate the elevation (y) coordinate of the point on the segment closest to the agent
// var segmentY = TSMath.Lerp(elevation1, elevation2, factorAlongSegment);
// // Calculate distance from the segment (not line)
// var sqrDistToSegment = (TSVector2.Lerp(p1, p2, factorAlongSegment) - position).sqrMagnitude;
// // Ignore the segment if it is too far away
// // or the agent is too high up (or too far down) on the elevation axis (usually y axis) to avoid it.
// // If the XY plane is used then all elevation checks are disabled
// if (sqrDistToSegment < range * range && (simulator.movementPlane == MovementPlane.XY || (elevationCoordinate <= segmentY + vertex.height && elevationCoordinate + height >= segmentY)))
// {
// vos.Add(VO.SegmentObstacle(p2 - position, p1 - position, TSVector2.zero, radius * FP.One/100, 1 / ObstacleTimeHorizon, 1 / simulator.DeltaTime));
// }
// }
// vertex = vertex.next;
// } while (vertex != obstacle && vertex != null && vertex.next != null);
// }
//}
#if USING_RVO
void GenerateNeighbourAgentVOs(VOBuffer vos, FP deltaTime)
{
FP inverseAgentTimeHorizon = 1 / agentTimeHorizon;
// The RVO algorithm assumes we will continue to
// move in roughly the same direction
TSVector2 optimalVelocity = currentVelocity;
int count = _behaviour.neighbours.Count;
for (int o = 0; o < count; o++)
{
IAgentBehaviour other = _behaviour.neighbours[o];
// Don't avoid ourselves
if (other == this)
{
continue;
}
// Interval along the y axis in which the agents overlap
FP maxY = TSMath.Min(elevationCoordinate + height, other.OrcaAgent.elevationCoordinate + other.OrcaAgent.height);
FP minY = TSMath.Max(elevationCoordinate, other.OrcaAgent.elevationCoordinate);
// The agents cannot collide since they are on different y-levels
if (maxY - minY < 0)
{
continue;
}
FP totalRadius = radius + other.colliderRadius;
// Describes a circle on the border of the VO
TSVector2 voBoundingOrigin = CustomMath.TSVecToVec2(other.position - _behaviour.position);
FP avoidanceStrength;
if (other.OrcaAgent.locked || other.OrcaAgent.manuallyControlled)
{
avoidanceStrength = 1;
}
else if (other.OrcaAgent.Priority > CustomMath.EPSILON || Priority > CustomMath.EPSILON)
{
avoidanceStrength = other.OrcaAgent.Priority / (Priority + other.OrcaAgent.Priority);
}
else
{
// Both this agent's priority and the other agent's priority is zero or negative
// Assume they have the same priority
avoidanceStrength = CustomMath.FPHalf;
}
// We assume that the other agent will continue to move with roughly the same velocity if the priorities for the agents are similar.
// If the other agent has a higher priority than this agent (avoidanceStrength > 0.5) then we will assume it will move more along its
// desired velocity. This will have the effect of other agents trying to clear a path for where a high priority agent wants to go.
// If this is not done then even high priority agents can get stuck when it is really crowded and they have had to slow down.
TSVector2 otherOptimalVelocity = TSVector2.Lerp(other.OrcaAgent.currentVelocity, other.OrcaAgent.desiredVelocity, 2 * avoidanceStrength - 1);
var voCenter = TSVector2.Lerp(optimalVelocity, otherOptimalVelocity, avoidanceStrength);
vos.Add(new VO(voBoundingOrigin, voCenter, totalRadius, inverseAgentTimeHorizon, 1 / deltaTime));
if (DebugDraw)
{
DrawVO(position + voBoundingOrigin * inverseAgentTimeHorizon + voCenter, totalRadius * inverseAgentTimeHorizon, position + voCenter);
}
}
}
void GenerateNeighbourAgentVOs(VOBuffer vos)
{
float inverseAgentTimeHorizon = 1.0f / agentTimeHorizon;
// The RVO algorithm assumes we will continue to
// move in roughly the same direction
Vector2 optimalVelocity = currentVelocity;
for (int o = 0; o < neighbours.Count; o++)
{
Agent other = neighbours[o];
// Don't avoid ourselves
if (other == this)
{
continue;
}
// Interval along the y axis in which the agents overlap
float maxY = System.Math.Min(elevationCoordinate + height, other.elevationCoordinate + other.height);
float minY = System.Math.Max(elevationCoordinate, other.elevationCoordinate);
// The agents cannot collide since they are on different y-levels
if (maxY - minY < 0)
{
continue;
}
float totalRadius = radius + other.radius;
// Describes a circle on the border of the VO
Vector2 voBoundingOrigin = other.position - position;
float avoidanceStrength;
if (other.locked || other.manuallyControlled)
{
avoidanceStrength = 1;
}
else if (other.Priority > 0.00001f || Priority > 0.00001f)
{
avoidanceStrength = other.Priority / (Priority + other.Priority);
}
else
{
// Both this agent's priority and the other agent's priority is zero or negative
// Assume they have the same priority
avoidanceStrength = 0.5f;
}
// We assume that the other agent will continue to move with roughly the same velocity if the priorities for the agents are similar.
// If the other agent has a higher priority than this agent (avoidanceStrength > 0.5) then we will assume it will move more along its
// desired velocity. This will have the effect of other agents trying to clear a path for where a high priority agent wants to go.
// If this is not done then even high priority agents can get stuck when it is really crowded and they have had to slow down.
Vector2 otherOptimalVelocity = Vector2.Lerp(other.currentVelocity, other.desiredVelocity, 2 * avoidanceStrength - 1);
var voCenter = Vector2.Lerp(optimalVelocity, otherOptimalVelocity, avoidanceStrength);
vos.Add(new VO(voBoundingOrigin, voCenter, totalRadius, inverseAgentTimeHorizon, 1 / simulator.DeltaTime));
if (DebugDraw)
{
DrawVO(position + voBoundingOrigin * inverseAgentTimeHorizon + voCenter, totalRadius * inverseAgentTimeHorizon, position + voCenter);
}
}
}
