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