/** * <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 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.RVO_EPSILON) { /* 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); } } }
private void setAgentPrefVelocity() { Vector2 goalVector = GoalAgent - CurrentAgent.position_; // Simulator.Instance.getAgentPosition(i); if (RVOMath.absSq(goalVector) > 1.0f) { goalVector = RVOMath.normalize(goalVector); } CurrentAgent.prefVelocity_ = goalVector; // Simulator.Instance.setAgentPrefVelocity(i, goalVector); ///* Perturb a little to avoid deadlocks due to perfect symmetry. */ //float angle = (float)random.NextDouble() * 2.0f * (float)Math.PI; //float dist = (float)random.NextDouble() * 0.0001f; //CurrentAgent.prefVelocity_ = CurrentAgent.prefVelocity_ + dist * new Vector2((float)Math.Cos(angle), (float)Math.Sin(angle)); }
/** * <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(); 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 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.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 = (-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 / 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_); } }