internal void insertObstacleNeighbor(Obstacle obstacle, float rangeSq)
{
Obstacle nextObstacle = obstacle.nextObstacle;
float distSq = RVOMath.distSqPointLineSegment(obstacle.point_, nextObstacle.point_, position_);
if (distSq < rangeSq)
{
obstacleNeighbors_.Add(new KeyValuePair<float, Obstacle>(distSq, obstacle));
int i = obstacleNeighbors_.Count - 1;
while (i != 0 && distSq < obstacleNeighbors_[i - 1].Key)
{
obstacleNeighbors_[i] = obstacleNeighbors_[i - 1];
--i;
}
obstacleNeighbors_[i] = new KeyValuePair<float, Obstacle>(distSq, obstacle);
}
}
/**
* <summary>Recursive method for building an obstacle k-D tree.
* </summary>
*
* <returns>An obstacle k-D tree node.</returns>
*
* <param name="obstacles">A list of obstacles.</param>
*/
private ObstacleTreeNode buildObstacleTreeRecursive(IList <Obstacle> obstacles)
{
if (obstacles.Count == 0)
{
return(null);
}
ObstacleTreeNode node = new ObstacleTreeNode();
int optimalSplit = 0;
int minLeft = obstacles.Count;
int minRight = obstacles.Count;
for (int i = 0; i < obstacles.Count; ++i)
{
int leftSize = 0;
int rightSize = 0;
Obstacle obstacleI1 = obstacles[i];
Obstacle obstacleI2 = obstacleI1.next_;
/* Compute optimal split node. */
for (int j = 0; j < obstacles.Count; ++j)
{
if (i == j)
{
continue;
}
Obstacle obstacleJ1 = obstacles[j];
Obstacle obstacleJ2 = obstacleJ1.next_;
KInt j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
KInt j2LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_);
if (j1LeftOfI >= 0 && j2LeftOfI >= 0)
{
++leftSize;
}
else if (j1LeftOfI <= 0 && j2LeftOfI <= 0)
{
++rightSize;
}
else
{
++leftSize;
++rightSize;
}
if (!Less(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize), Math.Max(minLeft, minRight), Math.Min(minLeft, minRight)))
{
break;
}
}
if (Less(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize), Math.Max(minLeft, minRight), Math.Min(minLeft, minRight)))
{
minLeft = leftSize;
minRight = rightSize;
optimalSplit = i;
}
}
{
/* Build split node. */
IList <Obstacle> leftObstacles = new List <Obstacle>(minLeft);
for (int n = 0; n < minLeft; ++n)
{
leftObstacles.Add(null);
}
IList <Obstacle> rightObstacles = new List <Obstacle>(minRight);
for (int n = 0; n < minRight; ++n)
{
rightObstacles.Add(null);
}
int leftCounter = 0;
int rightCounter = 0;
int i = optimalSplit;
Obstacle obstacleI1 = obstacles[i];
Obstacle obstacleI2 = obstacleI1.next_;
for (int j = 0; j < obstacles.Count; ++j)
{
if (i == j)
{
continue;
}
Obstacle obstacleJ1 = obstacles[j];
Obstacle obstacleJ2 = obstacleJ1.next_;
KInt j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
KInt j2LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_);
if (j1LeftOfI >= 0 && j2LeftOfI >= 0)
{
leftObstacles[leftCounter++] = obstacles[j];
}
else if (j1LeftOfI <= 0 && j2LeftOfI <= 0)
{
rightObstacles[rightCounter++] = obstacles[j];
}
else
{
/* Split obstacle j. */
//KInt t = RVOMath.det(obstacleI2.point_ - obstacleI1.point_, obstacleJ1.point_ - obstacleI1.point_) / RVOMath.det(obstacleI2.point_ - obstacleI1.point_, obstacleJ1.point_ - obstacleJ2.point_);
KInt2 splitPoint = obstacleJ1.point_ + RVOMath.det(obstacleI2.point_ - obstacleI1.point_, obstacleJ1.point_ - obstacleI1.point_) * (obstacleJ2.point_ - obstacleJ1.point_) / RVOMath.det(obstacleI2.point_ - obstacleI1.point_, obstacleJ1.point_ - obstacleJ2.point_);
Obstacle newObstacle = new Obstacle();
newObstacle.point_ = splitPoint;
newObstacle.previous_ = obstacleJ1;
newObstacle.next_ = obstacleJ2;
newObstacle.convex_ = true;
newObstacle.direction_ = obstacleJ1.direction_;
newObstacle.id_ = Simulator.Instance.obstacles_.Count;
Simulator.Instance.obstacles_.Add(newObstacle);
obstacleJ1.next_ = newObstacle;
obstacleJ2.previous_ = newObstacle;
if (j1LeftOfI > 0)
{
leftObstacles[leftCounter++] = obstacleJ1;
rightObstacles[rightCounter++] = newObstacle;
}
else
{
rightObstacles[rightCounter++] = obstacleJ1;
leftObstacles[leftCounter++] = newObstacle;
}
}
}
node.obstacle_ = obstacleI1;
node.left_ = buildObstacleTreeRecursive(leftObstacles);
node.right_ = buildObstacleTreeRecursive(rightObstacles);
return(node);
}
}
/**
* <summary>Computes the new velocity of this agent.</summary>
*/
internal void computeNewVelocity()
{
if (isKinematic)
{
newVelocity_ = prefVelocity_ / prefVelocity_.magnitude * maxSpeed_;
return;
}
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.RVO_EPSILON && RVOMath.det(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVOMath.RVO_EPSILON)
{
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 = Vector2.Dot(-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 : Vector2.Dot((velocity_ - leftCutOff), cutOffVector) / RVOMath.absSq(cutOffVector);
float tLeft = Vector2.Dot(velocity_ - leftCutOff, leftLegDirection);
float tRight = Vector2.Dot(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 = Vector2.Dot(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 = Vector2.Dot(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;
}
if (!other.isKinematic)
{
line.point = velocity_ + 0.5f * u;
}
else
{
line.point = relativeVelocity;
}
orcaLines_.Add(line);
}
int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, ref newVelocity_);
if (lineFail < orcaLines_.Count)
{
linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, ref newVelocity_);
}
}
ObstacleTreeNode buildObstacleTreeRecursive(IList <Obstacle> obstacles)
{
if (obstacles.Count == 0)
{
return(null);
}
else
{
ObstacleTreeNode node = new ObstacleTreeNode();
int optimalSplit = 0;
int minLeft = obstacles.Count;
int minRight = obstacles.Count;
for (int i = 0; i < obstacles.Count; ++i)
{
int leftSize = 0;
int rightSize = 0;
Obstacle obstacleI1 = obstacles[i];
Obstacle obstacleI2 = obstacleI1.nextObstacle;
/* Compute optimal split node. */
for (int j = 0; j < obstacles.Count; ++j)
{
if (i == j)
{
continue;
}
Obstacle obstacleJ1 = obstacles[j];
Obstacle obstacleJ2 = obstacleJ1.nextObstacle;
float j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
float j2LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_);
if (j1LeftOfI >= -RVOMath.RVO_EPSILON && j2LeftOfI >= -RVOMath.RVO_EPSILON)
{
++leftSize;
}
else if (j1LeftOfI <= RVOMath.RVO_EPSILON && j2LeftOfI <= RVOMath.RVO_EPSILON)
{
++rightSize;
}
else
{
++leftSize;
++rightSize;
}
if (new FloatPair(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize)) >= new FloatPair(Math.Max(minLeft, minRight), Math.Min(minLeft, minRight)))
{
break;
}
}
if (new FloatPair(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize)) < new FloatPair(Math.Max(minLeft, minRight), Math.Min(minLeft, minRight)))
{
minLeft = leftSize;
minRight = rightSize;
optimalSplit = i;
}
}
{
/* Build split node. */
IList <Obstacle> leftObstacles = new List <Obstacle>(minLeft);
for (int n = 0; n < minLeft; ++n)
{
leftObstacles.Add(null);
}
IList <Obstacle> rightObstacles = new List <Obstacle>(minRight);
for (int n = 0; n < minRight; ++n)
{
rightObstacles.Add(null);
}
int leftCounter = 0;
int rightCounter = 0;
int i = optimalSplit;
Obstacle obstacleI1 = obstacles[i];
Obstacle obstacleI2 = obstacleI1.nextObstacle;
for (int j = 0; j < obstacles.Count; ++j)
{
if (i == j)
{
continue;
}
Obstacle obstacleJ1 = obstacles[j];
Obstacle obstacleJ2 = obstacleJ1.nextObstacle;
float j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
float j2LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_);
if (j1LeftOfI >= -RVOMath.RVO_EPSILON && j2LeftOfI >= -RVOMath.RVO_EPSILON)
{
leftObstacles[leftCounter++] = obstacles[j];
}
else if (j1LeftOfI <= RVOMath.RVO_EPSILON && j2LeftOfI <= RVOMath.RVO_EPSILON)
{
rightObstacles[rightCounter++] = obstacles[j];
}
else
{
/* Split obstacle j. */
float t = RVOMath.det(obstacleI2.point_ - obstacleI1.point_, obstacleJ1.point_ - obstacleI1.point_) / RVOMath.det(obstacleI2.point_ - obstacleI1.point_, obstacleJ1.point_ - obstacleJ2.point_);
Vector2 splitpoint = obstacleJ1.point_ + t * (obstacleJ2.point_ - obstacleJ1.point_);
Obstacle newObstacle = new Obstacle();
newObstacle.point_ = splitpoint;
newObstacle.prevObstacle = obstacleJ1;
newObstacle.nextObstacle = obstacleJ2;
newObstacle.isConvex_ = true;
newObstacle.unitDir_ = obstacleJ1.unitDir_;
newObstacle.id_ = Simulator.Instance.obstacles_.Count;
Simulator.Instance.obstacles_.Add(newObstacle);
obstacleJ1.nextObstacle = newObstacle;
obstacleJ2.prevObstacle = newObstacle;
if (j1LeftOfI > 0.0f)
{
leftObstacles[leftCounter++] = obstacleJ1;
rightObstacles[rightCounter++] = newObstacle;
}
else
{
rightObstacles[rightCounter++] = obstacleJ1;
leftObstacles[leftCounter++] = newObstacle;
}
}
}
node.obstacle = obstacleI1;
node.left = buildObstacleTreeRecursive(leftObstacles);
node.right = buildObstacleTreeRecursive(rightObstacles);
return(node);
}
}
}
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();
obstacle.point_ = vertices[i];
if (i != 0)
{
obstacle.prevObstacle = obstacles_[obstacles_.Count - 1];
obstacle.prevObstacle.nextObstacle = obstacle;
}
if (i == vertices.Count - 1)
{
obstacle.nextObstacle = obstacles_[obstacleNo];
obstacle.nextObstacle.prevObstacle = obstacle;
}
obstacle.unitDir_ = RVOMath.normalize(vertices[(i == vertices.Count - 1 ? 0 : i + 1)] - vertices[i]);
if (vertices.Count == 2)
{
obstacle.isConvex_ = true;
}
else
{
obstacle.isConvex_ = (RVOMath.leftOf(vertices[(i == 0 ? vertices.Count - 1 : i - 1)], vertices[i], vertices[(i == vertices.Count - 1 ? 0 : i + 1)]) >= 0);
}
obstacle.id_ = obstacles_.Count;
obstacles_.Add(obstacle);
}
return obstacleNo;
}
/**
* <summary>Returns the number of the obstacle vertex succeeding the
* specified obstacle vertex in its polygon.</summary>
*
* <returns>The number of the obstacle vertex succeeding the specified
* obstacle vertex in its polygon.</returns>
*
* <param name="vertexNo">The number of the obstacle vertex whose
* successor is to be retrieved.</param>
*/
internal Obstacle getNextObstacleVertexNo(Obstacle _obstacle)
{
return(_obstacle.next_);
}
/**
* <summary>Returns the number of the obstacle vertex preceding the
* specified obstacle vertex in its polygon.</summary>
*
* <returns>The number of the obstacle vertex preceding the specified
* obstacle vertex in its polygon.</returns>
*
* <param name="vertexNo">The number of the obstacle vertex whose
* predecessor is to be retrieved.</param>
*/
internal Obstacle getPrevObstacleVertexNo(Obstacle _obstacle)
{
return(_obstacle.previous_);
}
/**
* <summary>Recursive method for building an obstacle k-D tree.
* </summary>
*
* <returns>An obstacle k-D tree node.</returns>
*
* <param name="obstacles">A list of obstacles.</param>
*/
private ObstacleTreeNode buildObstacleTreeRecursive(IList<Obstacle> obstacles)
{
if (obstacles.Count == 0)
{
return null;
}
ObstacleTreeNode node = new ObstacleTreeNode();
int optimalSplit = 0;
int minLeft = obstacles.Count;
int minRight = obstacles.Count;
for (int i = 0; i < obstacles.Count; ++i)
{
int leftSize = 0;
int rightSize = 0;
Obstacle obstacleI1 = obstacles[i];
Obstacle obstacleI2 = obstacleI1.next_;
/* Compute optimal split node. */
for (int j = 0; j < obstacles.Count; ++j)
{
if (i == j)
{
continue;
}
Obstacle obstacleJ1 = obstacles[j];
Obstacle obstacleJ2 = obstacleJ1.next_;
float j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
float j2LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_);
if (j1LeftOfI >= -RVOMath.RVO_EPSILON && j2LeftOfI >= -RVOMath.RVO_EPSILON)
{
++leftSize;
}
else if (j1LeftOfI <= RVOMath.RVO_EPSILON && j2LeftOfI <= RVOMath.RVO_EPSILON)
{
++rightSize;
}
else
{
++leftSize;
++rightSize;
}
if (new FloatPair(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize)) >= new FloatPair(Math.Max(minLeft, minRight), Math.Min(minLeft, minRight)))
{
break;
}
}
if (new FloatPair(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize)) < new FloatPair(Math.Max(minLeft, minRight), Math.Min(minLeft, minRight)))
{
minLeft = leftSize;
minRight = rightSize;
optimalSplit = i;
}
}
{
/* Build split node. */
IList<Obstacle> leftObstacles = new List<Obstacle>(minLeft);
for (int n = 0; n < minLeft; ++n)
{
leftObstacles.Add(null);
}
IList<Obstacle> rightObstacles = new List<Obstacle>(minRight);
for (int n = 0; n < minRight; ++n)
{
rightObstacles.Add(null);
}
int leftCounter = 0;
int rightCounter = 0;
int i = optimalSplit;
Obstacle obstacleI1 = obstacles[i];
Obstacle obstacleI2 = obstacleI1.next_;
for (int j = 0; j < obstacles.Count; ++j)
{
if (i == j)
{
continue;
}
Obstacle obstacleJ1 = obstacles[j];
Obstacle obstacleJ2 = obstacleJ1.next_;
float j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
float j2LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_);
if (j1LeftOfI >= -RVOMath.RVO_EPSILON && j2LeftOfI >= -RVOMath.RVO_EPSILON)
{
leftObstacles[leftCounter++] = obstacles[j];
}
else if (j1LeftOfI <= RVOMath.RVO_EPSILON && j2LeftOfI <= RVOMath.RVO_EPSILON)
{
rightObstacles[rightCounter++] = obstacles[j];
}
else
{
/* Split obstacle j. */
float t = RVOMath.det(obstacleI2.point_ - obstacleI1.point_, obstacleJ1.point_ - obstacleI1.point_) / RVOMath.det(obstacleI2.point_ - obstacleI1.point_, obstacleJ1.point_ - obstacleJ2.point_);
Vector2 splitPoint = obstacleJ1.point_ + t * (obstacleJ2.point_ - obstacleJ1.point_);
Obstacle newObstacle = new Obstacle();
newObstacle.point_ = splitPoint;
newObstacle.previous_ = obstacleJ1;
newObstacle.next_ = obstacleJ2;
newObstacle.convex_ = true;
newObstacle.direction_ = obstacleJ1.direction_;
newObstacle.id_ = Simulator.Instance.obstacles_.Count;
Simulator.Instance.obstacles_.Add(newObstacle);
obstacleJ1.next_ = newObstacle;
obstacleJ2.previous_ = newObstacle;
if (j1LeftOfI > 0.0f)
{
leftObstacles[leftCounter++] = obstacleJ1;
rightObstacles[rightCounter++] = newObstacle;
}
else
{
rightObstacles[rightCounter++] = obstacleJ1;
leftObstacles[leftCounter++] = newObstacle;
}
}
}
node.obstacle_ = obstacleI1;
node.left_ = buildObstacleTreeRecursive(leftObstacles);
node.right_ = buildObstacleTreeRecursive(rightObstacles);
return node;
}
}
/**
* <summary>Returns the two-dimensional position of a specified obstacle
* vertex.</summary>
*
* <returns>The two-dimensional position of the specified obstacle
* vertex.</returns>
*
* <param name="vertexNo">The number of the obstacle vertex to be
* retrieved.</param>
*/
internal Vector2 getObstacleVertex(Obstacle _obstacle)
{
return(_obstacle.point_);
}
public override void interactWith(Obstacle obstacle)
{
Obstacle obstacle1 = obstacle;
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 (Vector2.det((1.0f / timeHorizonObst_) * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) - (1.0f / timeHorizonObst_) * radius_ >= -RVO_EPSILON &&
Vector2.det((1.0f / timeHorizonObst_) * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - (1.0f / timeHorizonObst_) * radius_ >= -RVO_EPSILON)
{
alreadyCovered = true;
break;
}
}
if (alreadyCovered)
{
return;
}
/* Not yet covered. Check for collisions. */
float distSq1 = Vector2.absSq(relativePosition1);
float distSq2 = Vector2.absSq(relativePosition2);
float radiusSq = Vector2.sqr(radius_);
Vector2 obstacleVector = obstacle2.point_ - obstacle1.point_;
float s = (-relativePosition1 * obstacleVector) / Vector2.absSq(obstacleVector);
float distSqLine = Vector2.absSq(-relativePosition1 - s * obstacleVector);
Line line;
if (s < 0 && distSq1 <= radiusSq)
{
/* Collision with left vertex. Ignore if non-convex. */
if (obstacle1.convex_)
{
line.point = new Vector2(0, 0);
line.direction = Vector2.normalize(new Vector2(-relativePosition1.y(), relativePosition1.x()));
orcaLines_.Add(line);
}
return;
}
else if (s > 1 && distSq2 <= radiusSq)
{
/* Collision with right vertex. Ignore if non-convex
* or if it will be taken care of by neighoring obstace */
if (obstacle2.convex_ && Vector2.det(relativePosition2, obstacle2.direction_) >= 0)
{
line.point = new Vector2(0, 0);
line.direction = Vector2.normalize(new Vector2(-relativePosition2.y(), relativePosition2.x()));
orcaLines_.Add(line);
}
return;
}
else if (s >= 0 && s < 1 && distSqLine <= radiusSq)
{
/* Collision with obstacle segment. */
line.point = new Vector2(0, 0);
line.direction = -obstacle1.direction_;
orcaLines_.Add(line);
return;
}
/*
* No collision.
* Compute legs. When obliquely viewed, both legs can come from a single
* vertex. Legs extend cut-off line when nonconvex vertex.
*/
Vector2 leftLegDirection, rightLegDirection;
if (s < 0 && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that left vertex
* defines velocity obstacle.
*/
if (!obstacle1.convex_)
{
/* Ignore obstacle. */
return;
}
obstacle2 = obstacle1;
float leg1 = (float)Math.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 && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that
* right vertex defines velocity obstacle.
*/
if (!obstacle2.convex_)
{
/* Ignore obstacle. */
return;
}
obstacle1 = obstacle2;
float leg2 = (float)Math.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 = (float)Math.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 = (float)Math.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 (obstacle.convex_ && Vector2.det(leftLegDirection, -leftNeighbor.direction_) >= 0.0f)
{
/* Left leg points into obstacle. */
leftLegDirection = -leftNeighbor.direction_;
isLeftLegForeign = true;
}
if (obstacle2.convex_ && Vector2.det(rightLegDirection, obstacle2.direction_) <= 0.0f)
{
/* Right leg points into obstacle. */
rightLegDirection = obstacle2.direction_;
isRightLegForeign = true;
}
/* Compute cut-off centers. */
Vector2 leftCutoff = (1.0f / timeHorizonObst_) * (obstacle1.point_ - position_);
Vector2 rightCutoff = (1.0f / timeHorizonObst_) * (obstacle2.point_ - position_);
Vector2 cutoffVec = 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) * cutoffVec) / Vector2.absSq(cutoffVec));
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 = Vector2.normalize(velocity_ - leftCutoff);
line.direction = new Vector2(unitW.y(), -unitW.x());
line.point = leftCutoff + radius_ * (1.0f / timeHorizonObst_) * unitW;
orcaLines_.Add(line);
return;
}
else if (t > 1.0f && tRight < 0.0f)
{
/* Project on right cut-off circle. */
Vector2 unitW = Vector2.normalize(velocity_ - rightCutoff);
line.direction = new Vector2(unitW.y(), -unitW.x());
line.point = rightCutoff + radius_ * (1.0f / timeHorizonObst_) * unitW;
orcaLines_.Add(line);
return;
}
/*
* 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) ? Single.PositiveInfinity : Vector2.absSq(velocity_ - (leftCutoff + t * cutoffVec)));
float distSqLeft = ((tLeft < 0.0f) ? Single.PositiveInfinity : Vector2.absSq(velocity_ - (leftCutoff + tLeft * leftLegDirection)));
float distSqRight = ((tRight < 0.0f) ? Single.PositiveInfinity : Vector2.absSq(velocity_ - (rightCutoff + tRight * rightLegDirection)));
if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight)
{
/* Project on cut-off line. */
line.direction = -obstacle1.direction_;
line.point = leftCutoff + radius_ * (1.0f / timeHorizonObst_) * new Vector2(-line.direction.y(), line.direction.x());
orcaLines_.Add(line);
return;
}
else if (distSqLeft <= distSqRight)
{
/* Project on left leg. */
if (isLeftLegForeign)
{
return;
}
line.direction = leftLegDirection;
line.point = leftCutoff + radius_ * (1.0f / timeHorizonObst_) * new Vector2(-line.direction.y(), line.direction.x());
orcaLines_.Add(line);
return;
}
else
{
/* Project on right leg. */
if (isRightLegForeign)
{
return;
}
line.direction = -rightLegDirection;
line.point = rightCutoff + radius_ * (1.0f / timeHorizonObst_) * new Vector2(-line.direction.y(), line.direction.x());
orcaLines_.Add(line);
return;
}
}
/**
* <summary>Recursive method for building an obstacle k-D tree.
* </summary>
*
* <returns>An obstacle k-D tree node.</returns>
*
* <param name="obstacles">A list of obstacles.</param>
*/
private ObstacleTreeNode buildObstacleTreeRecursive(IList <Obstacle> obstacles)
{
if (obstacles.Count == 0)
{
return(null);
}
ObstacleTreeNode node = new ObstacleTreeNode();
int optimalSplit = 0;
int minLeft = obstacles.Count;
int minRight = obstacles.Count;
for (int i = 0; i < obstacles.Count; ++i)
{
//遍历多边形obstacle的每一条边
//以这条边拆分二维空间
Obstacle obstacleI1 = obstacles[i];
Obstacle obstacleI2 = obstacleI1.next_;
//统计在这条边左边和右边的所有其他边的数据
//以此来选择最优拆分边
int leftSize = 0;
int rightSize = 0;
/* Compute optimal split node. */
for (int j = 0; j < obstacles.Count; ++j)
{
if (i == j)
{
continue;
}
//遍历所有其他边
Obstacle obstacleJ1 = obstacles[j];
Obstacle obstacleJ2 = obstacleJ1.next_;
float j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
float j2LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_);
if (j1LeftOfI >= -RVOMath.RVO_EPSILON && j2LeftOfI >= -RVOMath.RVO_EPSILON)
{
++leftSize;
}
else if (j1LeftOfI <= RVOMath.RVO_EPSILON && j2LeftOfI <= RVOMath.RVO_EPSILON)
{
++rightSize;
}
else
{
//当拆分边所在直线与其他边相交时
++leftSize;
++rightSize;
}
if (new FloatPair(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize)) >= new FloatPair(Math.Max(minLeft, minRight), Math.Min(minLeft, minRight)))
{
//已经不如当前最优的拆分边，提前结束遍历
break;
}
}
//
if (new FloatPair(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize)) < new FloatPair(Math.Max(minLeft, minRight), Math.Min(minLeft, minRight)))
{
minLeft = leftSize;
minRight = rightSize;
optimalSplit = i;
}
}
{
/* Build split node. */
IList <Obstacle> leftObstacles = new List <Obstacle>(minLeft);
for (int n = 0; n < minLeft; ++n)
{
leftObstacles.Add(null);
}
IList <Obstacle> rightObstacles = new List <Obstacle>(minRight);
for (int n = 0; n < minRight; ++n)
{
rightObstacles.Add(null);
}
int leftCounter = 0;
int rightCounter = 0;
int i = optimalSplit;
Obstacle obstacleI1 = obstacles[i];
Obstacle obstacleI2 = obstacleI1.next_;
for (int j = 0; j < obstacles.Count; ++j)
{
if (i == j)
{
continue;
}
Obstacle obstacleJ1 = obstacles[j];
Obstacle obstacleJ2 = obstacleJ1.next_;
float j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
float j2LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_);
if (j1LeftOfI >= -RVOMath.RVO_EPSILON && j2LeftOfI >= -RVOMath.RVO_EPSILON)
{
leftObstacles[leftCounter++] = obstacles[j];
}
else if (j1LeftOfI <= RVOMath.RVO_EPSILON && j2LeftOfI <= RVOMath.RVO_EPSILON)
{
rightObstacles[rightCounter++] = obstacles[j];
}
else
{
/* Split obstacle j. */
float t = RVOMath.det(obstacleI2.point_ - obstacleI1.point_, obstacleJ1.point_ - obstacleI1.point_)
/ RVOMath.det(obstacleI2.point_ - obstacleI1.point_, obstacleJ1.point_ - obstacleJ2.point_);
Vector2 splitPoint = obstacleJ1.point_ + t * (obstacleJ2.point_ - obstacleJ1.point_);
Obstacle newObstacle = new Obstacle();
newObstacle.point_ = splitPoint;
newObstacle.previous_ = obstacleJ1;
newObstacle.next_ = obstacleJ2;
newObstacle.convex_ = true;
newObstacle.direction_ = obstacleJ1.direction_;
newObstacle.id_ = Simulator.Instance.obstacles_.Count;
Simulator.Instance.obstacles_.Add(newObstacle);
obstacleJ1.next_ = newObstacle;
obstacleJ2.previous_ = newObstacle;
if (j1LeftOfI > 0.0f)
{
leftObstacles[leftCounter++] = obstacleJ1;
rightObstacles[rightCounter++] = newObstacle;
}
else
{
rightObstacles[rightCounter++] = obstacleJ1;
leftObstacles[leftCounter++] = newObstacle;
}
}
}
node.obstacle_ = obstacleI1;
node.left_ = buildObstacleTreeRecursive(leftObstacles);
node.right_ = buildObstacleTreeRecursive(rightObstacles);
return(node);
}
}
/**
* <summary>Computes the new velocity of this agent.</summary>
*/
internal void computeNewVelocity()
{
orcaLines_.Clear();
Fix64 invTimeHorizonObst = Fix64.One / timeHorizonObst_;
/* Create obstacle ORCA lines. */
for (int i = 0; i < obstacleNeighbors_.Count; ++i)
{
Obstacle obstacle1 = obstacleNeighbors_[i].Value;
Obstacle obstacle2 = obstacle1.next_;
Vec2 relativePosition1 = obstacle1.point_ - position_;
Vec2 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.RVO_EPSILON && RVOMath.det(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVOMath.RVO_EPSILON)
{
alreadyCovered = true;
break;
}
}
if (alreadyCovered)
{
continue;
}
/* Not yet covered. Check for collisions. */
Fix64 distSq1 = RVOMath.absSq(relativePosition1);
Fix64 distSq2 = RVOMath.absSq(relativePosition2);
Fix64 radiusSq = RVOMath.sqr(radius_);
Vec2 obstacleVector = obstacle2.point_ - obstacle1.point_;
Fix64 obstacleVectorSq = RVOMath.absSq(obstacleVector);
Fix64 s = obstacleVectorSq == Fix64.Zero ? Fix64.One : (-relativePosition1 * obstacleVector) / obstacleVectorSq;
Fix64 distSqLine = RVOMath.absSq(-relativePosition1 - s * obstacleVector);
Line line;
if (s < Fix64.Zero && distSq1 <= radiusSq)
{
/* Collision with left vertex. Ignore if non-convex. */
if (obstacle1.convex_)
{
line.point = Vec2.Zero;
line.direction = RVOMath.normalize(new Vec2(-relativePosition1.y, relativePosition1.x));
orcaLines_.Add(line);
}
continue;
}
else if (s > Fix64.One && 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_) >= Fix64.Zero)
{
line.point = Vec2.Zero;
line.direction = RVOMath.normalize(new Vec2(-relativePosition2.y, relativePosition2.x));
orcaLines_.Add(line);
}
continue;
}
else if (s >= Fix64.Zero && s < Fix64.One && distSqLine <= radiusSq)
{
/* Collision with obstacle segment. */
line.point = Vec2.Zero;
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.
*/
Vec2 leftLegDirection, rightLegDirection;
if (s < Fix64.Zero && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that left vertex
* defines velocity obstacle.
*/
if (!obstacle1.convex_)
{
/* Ignore obstacle. */
continue;
}
obstacle2 = obstacle1;
Fix64 leg1 = RVOMath.sqrt((distSq1 - radiusSq).Clamp(Fix64.Zero, Fix64.MaxValue));
leftLegDirection = new Vec2(relativePosition1.x * leg1 - relativePosition1.y * radius_, relativePosition1.x * radius_ + relativePosition1.y * leg1) / distSq1;
rightLegDirection = new Vec2(relativePosition1.x * leg1 + relativePosition1.y * radius_, -relativePosition1.x * radius_ + relativePosition1.y * leg1) / distSq1;
}
else if (s > Fix64.One && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that
* right vertex defines velocity obstacle.
*/
if (!obstacle2.convex_)
{
/* Ignore obstacle. */
continue;
}
obstacle1 = obstacle2;
Fix64 leg2 = RVOMath.sqrt((distSq2 - radiusSq).Clamp(Fix64.Zero, Fix64.MaxValue));
leftLegDirection = new Vec2(relativePosition2.x * leg2 - relativePosition2.y * radius_, relativePosition2.x * radius_ + relativePosition2.y * leg2) / distSq2;
rightLegDirection = new Vec2(relativePosition2.x * leg2 + relativePosition2.y * radius_, -relativePosition2.x * radius_ + relativePosition2.y * leg2) / distSq2;
}
else
{
/* Usual situation. */
if (obstacle1.convex_)
{
Fix64 leg1 = RVOMath.sqrt((distSq1 - radiusSq).Clamp(Fix64.Zero, Fix64.MaxValue));
leftLegDirection = new Vec2(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_)
{
Fix64 leg2 = RVOMath.sqrt((distSq2 - radiusSq).Clamp(Fix64.Zero, Fix64.MaxValue));
rightLegDirection = new Vec2(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_) >= Fix64.Zero)
{
/* Left leg points into obstacle. */
leftLegDirection = -leftNeighbor.direction_;
isLeftLegForeign = true;
}
if (obstacle2.convex_ && RVOMath.det(rightLegDirection, obstacle2.direction_) <= Fix64.Zero)
{
/* Right leg points into obstacle. */
rightLegDirection = obstacle2.direction_;
isRightLegForeign = true;
}
/* Compute cut-off centers. */
Vec2 leftCutOff = invTimeHorizonObst * (obstacle1.point_ - position_);
Vec2 rightCutOff = invTimeHorizonObst * (obstacle2.point_ - position_);
Vec2 cutOffVector = rightCutOff - leftCutOff;
/* Project current velocity on velocity obstacle. */
/* Check if current velocity is projected on cutoff circles. */
Fix64 cutOffVectorSq = RVOMath.absSq(cutOffVector);
Fix64 t = obstacle1 == obstacle2 || cutOffVectorSq == Fix64.Zero ? 0.5f : ((velocity_ - leftCutOff) * cutOffVector) / cutOffVectorSq;
Fix64 tLeft = (velocity_ - leftCutOff) * leftLegDirection;
Fix64 tRight = (velocity_ - rightCutOff) * rightLegDirection;
if ((t < Fix64.Zero && tLeft < Fix64.Zero) || (obstacle1 == obstacle2 && tLeft < Fix64.Zero && tRight < Fix64.Zero))
{
/* Project on left cut-off circle. */
Vec2 unitW = RVOMath.normalize(velocity_ - leftCutOff);
line.direction = new Vec2(unitW.y, -unitW.x);
line.point = leftCutOff + radius_ * invTimeHorizonObst * unitW;
orcaLines_.Add(line);
continue;
}
else if (t > Fix64.One && tRight < Fix64.Zero)
{
/* Project on right cut-off circle. */
Vec2 unitW = RVOMath.normalize(velocity_ - rightCutOff);
line.direction = new Vec2(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.
*/
Fix64 distSqCutoff = (t <Fix64.Zero || t> Fix64.One || obstacle1 == obstacle2) ? Fix64.MaxValue : RVOMath.absSq(velocity_ - (leftCutOff + t * cutOffVector));
Fix64 distSqLeft = tLeft < Fix64.Zero ? Fix64.MaxValue : RVOMath.absSq(velocity_ - (leftCutOff + tLeft * leftLegDirection));
Fix64 distSqRight = tRight < Fix64.Zero ? Fix64.MaxValue : 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 Vec2(-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 Vec2(-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 Vec2(-line.direction.y, line.direction.x);
orcaLines_.Add(line);
}
int numObstLines = orcaLines_.Count;
Fix64 invTimeHorizon = Fix64.One / timeHorizon_;
/* Create agent ORCA lines. */
for (int i = 0; i < agentNeighbors_.Count; ++i)
{
Agent other = agentNeighbors_[i].Value;
Vec2 relativePosition = other.position_ - position_;
Vec2 relativeVelocity = velocity_ - other.velocity_;
Fix64 distSq = RVOMath.absSq(relativePosition);
Fix64 combinedRadius = radius_ + other.radius_;
Fix64 combinedRadiusSq = RVOMath.sqr(combinedRadius);
Line line;
Vec2 u;
if (distSq > combinedRadiusSq)
{
/* No collision. */
Vec2 w = relativeVelocity - invTimeHorizon * relativePosition;
/* the w has a change to be zero exactly.
* for example: the two objects are at the exactly same position and with the exactly same velocity
* so we have to make some perturbation to separate them */
if (w == Vec2.Zero)
{
w = id_ > other.id_ ? Vec2.Left : Vec2.Right;
}
/* Vector from cutoff center to relative velocity. */
Fix64 wLengthSq = RVOMath.absSq(w);
Fix64 dotProduct1 = w * relativePosition;
if (dotProduct1 < Fix64.Zero && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq)
{
/* Project on cut-off circle. */
Fix64 wLength = RVOMath.sqrt(wLengthSq);
Vec2 unitW = w / wLength;
line.direction = new Vec2(unitW.y, -unitW.x);
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
}
else
{
/* Project on legs. */
Fix64 leg = RVOMath.sqrt(distSq - combinedRadiusSq);
if (RVOMath.det(relativePosition, w) > Fix64.Zero)
{
/* Project on left leg. */
line.direction = new Vec2(relativePosition.x * leg - relativePosition.y * combinedRadius, relativePosition.x * combinedRadius + relativePosition.y * leg) / distSq;
}
else
{
/* Project on right leg. */
line.direction = -new Vec2(relativePosition.x * leg + relativePosition.y * combinedRadius, -relativePosition.x * combinedRadius + relativePosition.y * leg) / distSq;
}
Fix64 dotProduct2 = relativeVelocity * line.direction;
u = dotProduct2 * line.direction - relativeVelocity;
}
}
else
{
/* Collision. Project on cut-off circle of time timeStep. */
Fix64 invTimeStep = Fix64.One / simulator_.timeStep_;
/* Vector from cutoff center to relative velocity. */
Vec2 w = relativeVelocity - invTimeStep * relativePosition;
/* the w has a change to be zero exactly.
* for example: the two objects are at the exactly same position and with the exactly same velocity
* so we have to make some perturbation to separate them */
if (w == Vec2.Zero)
{
w = id_ > other.id_ ? Vec2.Left : Vec2.Right;
}
Fix64 wLength = RVOMath.abs(w);
Vec2 unitW = wLength == Fix64.Zero ? Vec2.Zero : w / wLength;
line.direction = new Vec2(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_);
}
}
/**
* <summary>Computes the new velocity of this agent.</summary>
*/
internal void computeNewVelocity()
{
orcaLines_.Clear();
//KInt invTimeHorizonObst = 1 / timeHorizonObst_;
KInt tempradius = radius_ / timeHorizonObst_;
/* Create obstacle ORCA lines. */
for (int i = 0; i < obstacleNeighbors_.Count; ++i)
{
Obstacle obstacle1 = obstacleNeighbors_[i].Value;
Obstacle obstacle2 = obstacle1.next_;
KInt2 relativePosition1 = obstacle1.point_ - position_;
KInt2 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(relativePosition1 / timeHorizonObst_ - orcaLines_[j].point, orcaLines_[j].direction) - tempradius >= 0 && RVOMath.det(relativePosition2 / timeHorizonObst_ - orcaLines_[j].point, orcaLines_[j].direction) - tempradius >= 0)
{
alreadyCovered = true;
break;
}
}
if (alreadyCovered)
{
continue;
}
/* Not yet covered. Check for collisions. */
KInt distSq1 = RVOMath.absSq(relativePosition1);
KInt distSq2 = RVOMath.absSq(relativePosition2);
KInt radiusSq = RVOMath.sqr(radius_);
KInt2 obstacleVector = obstacle2.point_ - obstacle1.point_;
KInt s = (-RVOMath.Dot(relativePosition1, obstacleVector)) / RVOMath.absSq(obstacleVector);
KInt distSqLine = RVOMath.absSq(-relativePosition1 - s * obstacleVector);
Line line = new Line();
if (s < 0 && distSq1 <= radiusSq)
{
/* Collision with left vertex. Ignore if non-convex. */
if (obstacle1.convex_)
{
line.point = KInt2.zero;
line.direction = RVOMath.normalize(KInt2.ToInt2(-relativePosition1.IntY, relativePosition1.IntX));
orcaLines_.Add(line);
}
continue;
}
else if (s > 1 && 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)
{
line.point = KInt2.zero;
line.direction = RVOMath.normalize(KInt2.ToInt2(-relativePosition2.IntY, relativePosition2.IntX));
orcaLines_.Add(line);
}
continue;
}
else if (s >= 0 && s < 1 && distSqLine <= radiusSq)
{
/* Collision with obstacle segment. */
line.point = KInt2.zero;
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.
*/
KInt2 leftLegDirection, rightLegDirection;
if (s < 0 && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that left vertex
* defines velocity obstacle.
*/
if (!obstacle1.convex_)
{
/* Ignore obstacle. */
continue;
}
obstacle2 = obstacle1;
KInt leg1 = RVOMath.sqrt(distSq1 - radiusSq);
leftLegDirection = KInt2.ToInt2(relativePosition1.IntX * leg1 - relativePosition1.IntY * radius_, relativePosition1.IntX * radius_ + relativePosition1.IntY * leg1) / distSq1;
rightLegDirection = KInt2.ToInt2(relativePosition1.IntX * leg1 + relativePosition1.IntY * radius_, -relativePosition1.IntX * radius_ + relativePosition1.IntY * leg1) / distSq1;
if (isover(leftLegDirection) || isover(rightLegDirection))
{
LogMgr.LogError("!!!");
}
}
else if (s > 1 && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that
* right vertex defines velocity obstacle.
*/
if (!obstacle2.convex_)
{
/* Ignore obstacle. */
continue;
}
obstacle1 = obstacle2;
KInt leg2 = RVOMath.sqrt(distSq2 - radiusSq);
leftLegDirection = KInt2.ToInt2(relativePosition2.IntX * leg2 - relativePosition2.IntY * radius_, relativePosition2.IntX * radius_ + relativePosition2.IntY * leg2) / distSq2;
rightLegDirection = KInt2.ToInt2(relativePosition2.IntX * leg2 + relativePosition2.IntY * radius_, -relativePosition2.IntX * radius_ + relativePosition2.IntY * leg2) / distSq2;
if (isover(leftLegDirection) || isover(rightLegDirection))
{
LogMgr.LogError("!!!");
}
}
else
{
/* Usual situation. */
if (obstacle1.convex_)
{
KInt leg1 = RVOMath.sqrt(distSq1 - radiusSq);
leftLegDirection = KInt2.ToInt2(relativePosition1.IntX * leg1 - relativePosition1.IntY * radius_, relativePosition1.IntX * radius_ + relativePosition1.IntY * leg1) / distSq1;
if (isover(leftLegDirection))
{
LogMgr.LogError("!!!");
}
}
else
{
/* Left vertex non-convex; left leg extends cut-off line. */
leftLegDirection = -obstacle1.direction_;
if (isover(leftLegDirection))
{
LogMgr.LogError("!!!");
}
}
if (obstacle2.convex_)
{
KInt leg2 = RVOMath.sqrt(distSq2 - radiusSq);
rightLegDirection = KInt2.ToInt2(relativePosition2.IntX * leg2 + relativePosition2.IntY * radius_, -relativePosition2.IntX * radius_ + relativePosition2.IntY * leg2) / distSq2;
if (isover(rightLegDirection))
{
LogMgr.LogError("!!!");
}
}
else
{
/* Right vertex non-convex; right leg extends cut-off line. */
rightLegDirection = obstacle1.direction_;
if (isover(rightLegDirection))
{
LogMgr.LogError("!!!");
}
}
}
/*
* 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)
{
/* Left leg points into obstacle. */
leftLegDirection = -leftNeighbor.direction_;
if (isover(leftLegDirection))
{
LogMgr.LogError("!!!");
}
isLeftLegForeign = true;
}
if (obstacle2.convex_ && RVOMath.det(rightLegDirection, obstacle2.direction_) <= 0)
{
/* Right leg points into obstacle. */
rightLegDirection = obstacle2.direction_;
isRightLegForeign = true;
if (isover(rightLegDirection))
{
LogMgr.LogError("!!!");
}
}
/* Compute cut-off centers. */
KInt2 leftCutOff = (obstacle1.point_ - position_) / timeHorizonObst_;
KInt2 rightCutOff = (obstacle2.point_ - position_) / timeHorizonObst_;
KInt2 cutOffVector = rightCutOff - leftCutOff;
/* Project current velocity on velocity obstacle. */
/* Check if current velocity is projected on cutoff circles. */
KInt sqvalue = RVOMath.absSq(cutOffVector);
KInt t = KInt.ToInt(KInt.divscale / 2);
if (obstacle1 != obstacle2)
{
if (sqvalue == 0)
{
t = KInt.MaxValue;
}
else
{
t = RVOMath.Dot((velocity_ - leftCutOff), cutOffVector) / sqvalue;
}
}
KInt tLeft = RVOMath.Dot((velocity_ - leftCutOff), leftLegDirection);
KInt tRight = RVOMath.Dot((velocity_ - rightCutOff), rightLegDirection);
if ((t < 0 && tLeft < 0) || (obstacle1 == obstacle2 && tLeft < 0 && tRight < 0))
{
/* Project on left cut-off circle. */
KInt2 unitW = RVOMath.normalize((velocity_ - leftCutOff));
line.direction = KInt2.ToInt2(unitW.IntY, -unitW.IntX);
line.point = leftCutOff + radius_ * unitW / timeHorizonObst_;
orcaLines_.Add(line);
continue;
}
else if (t > 1 && tRight < 0)
{
/* Project on right cut-off circle. */
KInt2 unitW = RVOMath.normalize((velocity_ - rightCutOff));
line.direction = KInt2.ToInt2(unitW.IntY, -unitW.IntX);
line.point = rightCutOff + radius_ * unitW / timeHorizonObst_;
orcaLines_.Add(line);
continue;
}
/*
* Project on left leg, right leg, or cut-off line, whichever is
* closest to velocity.
*/
KInt distSqCutoff = (t < 0 || t > 1 || obstacle1 == obstacle2) ? KInt.MaxValue : RVOMath.absSq(velocity_ - (leftCutOff + t * cutOffVector));
KInt distSqLeft = tLeft < 0 ? KInt.MaxValue : RVOMath.absSq(velocity_ - (leftCutOff + tLeft * leftLegDirection));
KInt distSqRight = tRight < 0 ? KInt.MaxValue : RVOMath.absSq(velocity_ - (rightCutOff + tRight * rightLegDirection));
if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight)
{
/* Project on cut-off line. */
line.direction = -obstacle1.direction_;
line.point = leftCutOff + radius_ * KInt2.ToInt2(-line.direction.IntY, line.direction.IntX) / timeHorizonObst_;
orcaLines_.Add(line);
continue;
}
if (distSqLeft <= distSqRight)
{
/* Project on left leg. */
if (isLeftLegForeign)
{
continue;
}
line.direction = leftLegDirection;
line.point = leftCutOff + radius_ * KInt2.ToInt2(-line.direction.IntY, line.direction.IntX) / timeHorizonObst_;
orcaLines_.Add(line);
continue;
}
/* Project on right leg. */
if (isRightLegForeign)
{
continue;
}
line.direction = -rightLegDirection;
line.point = rightCutOff + radius_ * KInt2.ToInt2(-line.direction.IntY, line.direction.IntX) / timeHorizonObst_;
orcaLines_.Add(line);
}
int numObstLines = orcaLines_.Count;
//KInt invTimeHorizon = 1 / timeHorizon_;
/* Create agent ORCA lines. */
for (int i = 0; i < agentNeighbors_.Count; ++i)
{
Agent other = agentNeighbors_[i].Value;
KInt2 relativePosition = other.position_ - position_;
KInt2 relativeVelocity = velocity_ - other.velocity_;
KInt distSq = RVOMath.absSq(relativePosition);
KInt combinedRadius = radius_ + other.radius_;
KInt combinedRadiusSq = RVOMath.sqr(combinedRadius);
Line line = new Line();
KInt2 u;
if (distSq > combinedRadiusSq)
{
/* No collision. */
KInt2 w = relativeVelocity - relativePosition / timeHorizon_;
/* Vector from cutoff center to relative velocity. */
KInt wLengthSq = RVOMath.absSq(w);
KInt dotProduct1 = RVOMath.Dot(w, relativePosition);
if (dotProduct1 < 0 && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq)
{
/* Project on cut-off circle. */
KInt wLength = RVOMath.sqrt(wLengthSq);
if (wLength == 0)
{
continue;
}
KInt2 unitW = w / wLength;
line.direction = KInt2.ToInt2(unitW.IntY, -unitW.IntX);
u = (combinedRadius / timeHorizon_ - wLength) * unitW;
}
else
{
/* Project on legs. */
KInt leg = RVOMath.sqrt(distSq - combinedRadiusSq);
if (RVOMath.det(relativePosition, w) > 0)
{
/* Project on left leg. */
line.direction = KInt2.ToInt2(relativePosition.IntX * leg - relativePosition.IntY * combinedRadius, relativePosition.IntX * combinedRadius + relativePosition.IntY * leg) / distSq;
}
else
{
/* Project on right leg. */
line.direction = -KInt2.ToInt2(relativePosition.IntX * leg + relativePosition.IntY * combinedRadius, -relativePosition.IntX * combinedRadius + relativePosition.IntY * leg) / distSq;
}
KInt dotProduct2 = RVOMath.Dot(relativeVelocity, line.direction);
u = dotProduct2 * line.direction - relativeVelocity;
}
}
else
{
/* Collision. Project on cut-off circle of time timeStep. */
//KInt invTimeStep = 1 / Simulator.Instance.timeStep_;
/* Vector from cutoff center to relative velocity. */
KInt2 w = relativeVelocity - relativePosition / Simulator.Instance.timeStep_;
KInt wLength = RVOMath.abs(w);
if (wLength == 0)
{
continue;
}
KInt2 unitW = w / wLength;
line.direction = KInt2.ToInt2(unitW.IntY, -unitW.IntX);
u = (combinedRadius / Simulator.Instance.timeStep_ - wLength) * unitW;
}
line.point = velocity_ + u / 2;
orcaLines_.Add(line);
}
int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, ref newVelocity_);
if (lineFail < orcaLines_.Count)
{
linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, ref newVelocity_);
}
}
/** * <summary>Computes the interaction between this agent and the obstacles in parameter.</summary> */ public abstract void interactWith(Obstacle obstacle);