/**
* <summary>Solves a two-dimensional linear program subject to linear
* constraints defined by lines and a circular constraint.</summary>
*
* <param name="lines">Lines defining the linear constraints.</param>
* <param name="numObstLines">Count of obstacle lines.</param>
* <param name="beginLine">The line on which the 2-d linear program
* failed.</param>
* <param name="radius">The radius of the circular constraint.</param>
* <param name="result">A reference to the result of the linear program.
* </param>
*/
private static void LinearProgram3(IList <Line> lines, int numObstLines, int beginLine, float radius, ref Vector2 result)
{
float distance = 0.0f;
for (int i = beginLine; i < lines.Count; ++i)
{
if (RVOMath.Det(lines[i].Direction, lines[i].Point - result) > distance)
{
/* Result does not satisfy constraint of line i. */
IList <Line> projLines = new List <Line>();
for (int ii = 0; ii < numObstLines; ++ii)
{
projLines.Add(lines[ii]);
}
for (int j = numObstLines; j < i; ++j)
{
Line line;
float determinant = RVOMath.Det(lines[i].Direction, lines[j].Direction);
if (RVOMath.Fabs(determinant) <= RVOMath.RvoEpsilon)
{
/* Line i and line j are parallel. */
if (lines[i].Direction * lines[j].Direction > 0.0f)
{
/* Line i and line j point in the same direction. */
continue;
}
else
{
/* Line i and line j point in opposite direction. */
line.Point = 0.5f * (lines[i].Point + lines[j].Point);
}
}
else
{
line.Point = lines[i].Point + (RVOMath.Det(lines[j].Direction, lines[i].Point - lines[j].Point) / determinant) * lines[i].Direction;
}
line.Direction = RVOMath.Normalize(lines[j].Direction - lines[i].Direction);
projLines.Add(line);
}
Vector2 tempResult = result;
if (LinearProgram2(projLines, radius, new Vector2(-lines[i].Direction.Y, lines[i].Direction.X), true, ref result) < projLines.Count)
{
/*
* This should in principle not happen. The result is by
* definition already in the feasible region of this
* linear program. If it fails, it is due to small
* floating point error, and the current result is kept.
*/
result = tempResult;
}
distance = RVOMath.Det(lines[i].Direction, lines[i].Point - result);
}
}
}
/**
* <summary>Adds a new obstacle to the simulation.</summary>
*
* <returns>The number of the first vertex of the obstacle, or -1 when
* the number of vertices is less than two.</returns>
*
* <param name="vertices">List of the vertices of the polygonal obstacle
* in counterclockwise order.</param>
*
* <remarks>To add a "negative" obstacle, e.g. a bounding polygon around
* the environment, the vertices should be listed in clockwise order.
* </remarks>
*/
public int AddObstacle(IList <Vector2> vertices)
{
if (vertices.Count < 2)
{
return(-1);
}
int obstacleNo = Obstacles.Count;
for (int i = 0; i < vertices.Count; ++i)
{
Obstacle obstacle = new Obstacle {
Point = vertices[i]
};
if (i != 0)
{
obstacle.Previous = Obstacles[Obstacles.Count - 1];
obstacle.Previous.Next = obstacle;
}
if (i == vertices.Count - 1)
{
obstacle.Next = Obstacles[obstacleNo];
obstacle.Next.Previous = obstacle;
}
obstacle.Direction = RVOMath.Normalize(vertices[(i == vertices.Count - 1 ? 0 : i + 1)] - vertices[i]);
if (vertices.Count == 2)
{
obstacle.Convex = true;
}
else
{
obstacle.Convex = (RVOMath.LeftOf(vertices[(i == 0 ? vertices.Count - 1 : i - 1)], vertices[i], vertices[(i == vertices.Count - 1 ? 0 : i + 1)]) >= 0.0f);
}
obstacle.Id = Obstacles.Count;
Obstacles.Add(obstacle);
}
return(obstacleNo);
}
/**
* <summary>Solves a two-dimensional linear program subject to linear
* constraints defined by lines and a circular constraint.</summary>
*
* <returns>The number of the line it fails on, and the number of lines
* if successful.</returns>
*
* <param name="lines">Lines defining the linear constraints.</param>
* <param name="radius">The radius of the circular constraint.</param>
* <param name="optVelocity">The optimization velocity.</param>
* <param name="directionOpt">True if the direction should be optimized.
* </param>
* <param name="result">A reference to the result of the linear program.
* </param>
*/
private static int LinearProgram2(IList <Line> lines, float radius, Vector2 optVelocity, bool directionOpt, ref Vector2 result)
{
if (directionOpt)
{
/*
* Optimize direction. Note that the optimization velocity is of
* unit length in this case.
*/
result = optVelocity * radius;
}
else if (RVOMath.AbsSq(optVelocity) > RVOMath.Sqr(radius))
{
/* Optimize closest point and outside circle. */
result = RVOMath.Normalize(optVelocity) * radius;
}
else
{
/* Optimize closest point and inside circle. */
result = optVelocity;
}
for (int i = 0; i < lines.Count; ++i)
{
if (RVOMath.Det(lines[i].Direction, lines[i].Point - result) > 0.0f)
{
/* Result does not satisfy constraint i. Compute new optimal result. */
Vector2 tempResult = result;
if (!LinearProgram1(lines, i, radius, optVelocity, directionOpt, ref result))
{
result = tempResult;
return(i);
}
}
}
return(lines.Count);
}
/**
* <summary>Computes the new velocity of this agent.</summary>
*/
internal void ComputeNewVelocity()
{
OrcaLines.Clear();
float invTimeHorizonObst = 1.0f / TimeHorizonObst;
/* Create obstacle ORCA lines. */
for (int i = 0; i < ObstacleNeighbors.Count; ++i)
{
Obstacle obstacle1 = ObstacleNeighbors[i].Value;
Obstacle obstacle2 = obstacle1.Next;
Vector2 relativePosition1 = obstacle1.Point - Position;
Vector2 relativePosition2 = obstacle2.Point - Position;
/*
* Check if velocity obstacle of obstacle is already taken care
* of by previously constructed obstacle ORCA lines.
*/
bool alreadyCovered = false;
for (int j = 0; j < OrcaLines.Count; ++j)
{
if (RVOMath.Det(invTimeHorizonObst * relativePosition1 - OrcaLines[j].Point, OrcaLines[j].Direction) - invTimeHorizonObst * Radius >= -RVOMath.RvoEpsilon && RVOMath.Det(invTimeHorizonObst * relativePosition2 - OrcaLines[j].Point, OrcaLines[j].Direction) - invTimeHorizonObst * Radius >= -RVOMath.RvoEpsilon)
{
alreadyCovered = true;
break;
}
}
if (alreadyCovered)
{
continue;
}
/* Not yet covered. Check for collisions. */
float distSq1 = RVOMath.AbsSq(relativePosition1);
float distSq2 = RVOMath.AbsSq(relativePosition2);
float radiusSq = RVOMath.Sqr(Radius);
Vector2 obstacleVector = obstacle2.Point - obstacle1.Point;
float s = (-relativePosition1 * obstacleVector) / RVOMath.AbsSq(obstacleVector);
float distSqLine = RVOMath.AbsSq(-relativePosition1 - s * obstacleVector);
Line line;
if (s < 0.0f && distSq1 <= radiusSq)
{
/* Collision with left vertex. Ignore if non-convex. */
if (obstacle1.Convex)
{
line.Point = new Vector2(0.0f, 0.0f);
line.Direction = RVOMath.Normalize(new Vector2(-relativePosition1.Y, relativePosition1.X));
OrcaLines.Add(line);
}
continue;
}
else if (s > 1.0f && distSq2 <= radiusSq)
{
/*
* Collision with right vertex. Ignore if non-convex or if
* it will be taken care of by neighboring obstacle.
*/
if (obstacle2.Convex && RVOMath.Det(relativePosition2, obstacle2.Direction) >= 0.0f)
{
line.Point = new Vector2(0.0f, 0.0f);
line.Direction = RVOMath.Normalize(new Vector2(-relativePosition2.Y, relativePosition2.X));
OrcaLines.Add(line);
}
continue;
}
else if (s >= 0.0f && s < 1.0f && distSqLine <= radiusSq)
{
/* Collision with obstacle segment. */
line.Point = new Vector2(0.0f, 0.0f);
line.Direction = -obstacle1.Direction;
OrcaLines.Add(line);
continue;
}
/*
* No collision. Compute legs. When obliquely viewed, both legs
* can come from a single vertex. Legs extend cut-off line when
* non-convex vertex.
*/
Vector2 leftLegDirection, rightLegDirection;
if (s < 0.0f && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that left vertex
* defines velocity obstacle.
*/
if (!obstacle1.Convex)
{
/* Ignore obstacle. */
continue;
}
obstacle2 = obstacle1;
float leg1 = RVOMath.Sqrt(distSq1 - radiusSq);
leftLegDirection = new Vector2(relativePosition1.X * leg1 - relativePosition1.Y * Radius, relativePosition1.X * Radius + relativePosition1.Y * leg1) / distSq1;
rightLegDirection = new Vector2(relativePosition1.X * leg1 + relativePosition1.Y * Radius, -relativePosition1.X * Radius + relativePosition1.Y * leg1) / distSq1;
}
else if (s > 1.0f && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that
* right vertex defines velocity obstacle.
*/
if (!obstacle2.Convex)
{
/* Ignore obstacle. */
continue;
}
obstacle1 = obstacle2;
float leg2 = RVOMath.Sqrt(distSq2 - radiusSq);
leftLegDirection = new Vector2(relativePosition2.X * leg2 - relativePosition2.Y * Radius, relativePosition2.X * Radius + relativePosition2.Y * leg2) / distSq2;
rightLegDirection = new Vector2(relativePosition2.X * leg2 + relativePosition2.Y * Radius, -relativePosition2.X * Radius + relativePosition2.Y * leg2) / distSq2;
}
else
{
/* Usual situation. */
if (obstacle1.Convex)
{
float leg1 = RVOMath.Sqrt(distSq1 - radiusSq);
leftLegDirection = new Vector2(relativePosition1.X * leg1 - relativePosition1.Y * Radius, relativePosition1.X * Radius + relativePosition1.Y * leg1) / distSq1;
}
else
{
/* Left vertex non-convex; left leg extends cut-off line. */
leftLegDirection = -obstacle1.Direction;
}
if (obstacle2.Convex)
{
float leg2 = RVOMath.Sqrt(distSq2 - radiusSq);
rightLegDirection = new Vector2(relativePosition2.X * leg2 + relativePosition2.Y * Radius, -relativePosition2.X * Radius + relativePosition2.Y * leg2) / distSq2;
}
else
{
/* Right vertex non-convex; right leg extends cut-off line. */
rightLegDirection = obstacle1.Direction;
}
}
/*
* Legs can never point into neighboring edge when convex
* vertex, take cutoff-line of neighboring edge instead. If
* velocity projected on "foreign" leg, no constraint is added.
*/
Obstacle leftNeighbor = obstacle1.Previous;
bool isLeftLegForeign = false;
bool isRightLegForeign = false;
if (obstacle1.Convex && RVOMath.Det(leftLegDirection, -leftNeighbor.Direction) >= 0.0f)
{
/* Left leg points into obstacle. */
leftLegDirection = -leftNeighbor.Direction;
isLeftLegForeign = true;
}
if (obstacle2.Convex && RVOMath.Det(rightLegDirection, obstacle2.Direction) <= 0.0f)
{
/* Right leg points into obstacle. */
rightLegDirection = obstacle2.Direction;
isRightLegForeign = true;
}
/* Compute cut-off centers. */
Vector2 leftCutOff = invTimeHorizonObst * (obstacle1.Point - Position);
Vector2 rightCutOff = invTimeHorizonObst * (obstacle2.Point - Position);
Vector2 cutOffVector = rightCutOff - leftCutOff;
/* Project current velocity on velocity obstacle. */
/* Check if current velocity is projected on cutoff circles. */
float t = obstacle1 == obstacle2 ? 0.5f : ((Velocity - leftCutOff) * cutOffVector) / RVOMath.AbsSq(cutOffVector);
float tLeft = (Velocity - leftCutOff) * leftLegDirection;
float tRight = (Velocity - rightCutOff) * rightLegDirection;
if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f))
{
/* Project on left cut-off circle. */
Vector2 unitW = RVOMath.Normalize(Velocity - leftCutOff);
line.Direction = new Vector2(unitW.Y, -unitW.X);
line.Point = leftCutOff + Radius * invTimeHorizonObst * unitW;
OrcaLines.Add(line);
continue;
}
else if (t > 1.0f && tRight < 0.0f)
{
/* Project on right cut-off circle. */
Vector2 unitW = RVOMath.Normalize(Velocity - rightCutOff);
line.Direction = new Vector2(unitW.Y, -unitW.X);
line.Point = rightCutOff + Radius * invTimeHorizonObst * unitW;
OrcaLines.Add(line);
continue;
}
/*
* Project on left leg, right leg, or cut-off line, whichever is
* closest to velocity.
*/
float distSqCutoff = (t <0.0f || t> 1.0f || obstacle1 == obstacle2) ? float.PositiveInfinity : RVOMath.AbsSq(Velocity - (leftCutOff + t * cutOffVector));
float distSqLeft = tLeft < 0.0f ? float.PositiveInfinity : RVOMath.AbsSq(Velocity - (leftCutOff + tLeft * leftLegDirection));
float distSqRight = tRight < 0.0f ? float.PositiveInfinity : RVOMath.AbsSq(Velocity - (rightCutOff + tRight * rightLegDirection));
if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight)
{
/* Project on cut-off line. */
line.Direction = -obstacle1.Direction;
line.Point = leftCutOff + Radius * invTimeHorizonObst * new Vector2(-line.Direction.Y, line.Direction.X);
OrcaLines.Add(line);
continue;
}
if (distSqLeft <= distSqRight)
{
/* Project on left leg. */
if (isLeftLegForeign)
{
continue;
}
line.Direction = leftLegDirection;
line.Point = leftCutOff + Radius * invTimeHorizonObst * new Vector2(-line.Direction.Y, line.Direction.X);
OrcaLines.Add(line);
continue;
}
/* Project on right leg. */
if (isRightLegForeign)
{
continue;
}
line.Direction = -rightLegDirection;
line.Point = rightCutOff + Radius * invTimeHorizonObst * new Vector2(-line.Direction.Y, line.Direction.X);
OrcaLines.Add(line);
}
int numObstLines = OrcaLines.Count;
float invTimeHorizon = 1.0f / TimeHorizon;
/* Create agent ORCA lines. */
for (int i = 0; i < AgentNeighbors.Count; ++i)
{
Agent other = AgentNeighbors[i].Value;
Vector2 relativePosition = other.Position - Position;
Vector2 relativeVelocity = Velocity - other.Velocity;
float distSq = RVOMath.AbsSq(relativePosition);
float combinedRadius = Radius + other.Radius;
float combinedRadiusSq = RVOMath.Sqr(combinedRadius);
Line line;
Vector2 u;
if (distSq > combinedRadiusSq)
{
/* No collision. */
Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;
/* Vector from cutoff center to relative velocity. */
float wLengthSq = RVOMath.AbsSq(w);
float dotProduct1 = w * relativePosition;
if (dotProduct1 < 0.0f && RVOMath.Sqr(dotProduct1) > combinedRadiusSq * wLengthSq)
{
/* Project on cut-off circle. */
float wLength = RVOMath.Sqrt(wLengthSq);
Vector2 unitW = w / wLength;
line.Direction = new Vector2(unitW.Y, -unitW.X);
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
}
else
{
/* Project on legs. */
float leg = RVOMath.Sqrt(distSq - combinedRadiusSq);
if (RVOMath.Det(relativePosition, w) > 0.0f)
{
/* Project on left leg. */
line.Direction = new Vector2(relativePosition.X * leg - relativePosition.Y * combinedRadius, relativePosition.X * combinedRadius + relativePosition.Y * leg) / distSq;
}
else
{
/* Project on right leg. */
line.Direction = -new Vector2(relativePosition.X * leg + relativePosition.Y * combinedRadius, -relativePosition.X * combinedRadius + relativePosition.Y * leg) / distSq;
}
float dotProduct2 = relativeVelocity * line.Direction;
u = dotProduct2 * line.Direction - relativeVelocity;
}
}
else
{
/* Collision. Project on cut-off circle of time timeStep. */
float invTimeStep = 1.0f / Simulator.Instance.TimeStep;
/* Vector from cutoff center to relative velocity. */
Vector2 w = relativeVelocity - invTimeStep * relativePosition;
float wLength = RVOMath.Abs(w);
Vector2 unitW = w / wLength;
line.Direction = new Vector2(unitW.Y, -unitW.X);
u = (combinedRadius * invTimeStep - wLength) * unitW;
}
line.Point = Velocity + 0.5f * u;
OrcaLines.Add(line);
}
int lineFail = LinearProgram2(OrcaLines, MaxSpeed, PrefVelocity, false, ref _newVelocity);
if (lineFail < OrcaLines.Count)
{
LinearProgram3(OrcaLines, numObstLines, lineFail, MaxSpeed, ref _newVelocity);
}
}