public static normalize ( Vector2 vector ) : Vector2 | ||
vector | Vector2 | The two-dimensional vector whose normalization * is to be computed. |
return | Vector2 |
/** * <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 (Vector2.Dot(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); } } }
void setPreferredVelocities() { /* * Set the preferred velocity to be a vector of unit magnitude * (speed) in the direction of the goal. */ for (int i = 0; i < Simulator.Instance.getNumAgents(); ++i) { Vector2 goalVector = goals[i] - Simulator.Instance.getAgentPosition(i); if (RVOMath.absSq(goalVector) > 1.0f) { goalVector = RVOMath.normalize(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; Simulator.Instance.setAgentPrefVelocity(i, Simulator.Instance.getAgentPrefVelocity(i) + dist * new Vector2((float)Math.Cos(angle), (float)Math.Sin(angle))); } }
public void setPreferred() { goalDirection = new Vector2(0.0f, 0.0f); if (path != null && pathStatus < path.corners.Length) { goalDirection = new Vector2(path.corners[pathStatus].x, path.corners[pathStatus].z) - new Vector2(transform.position.x, transform.position.z); if (RVOMath.absSq(goalDirection) < goalOffset) { pathStatus++; } if (RVOMath.absSq(goalDirection) > AgentReference.maxSpeed_ * 0.9) { //Choose between two; max speed or less. (This provides somewhat smooth movement) // float speed = userCoefficient * (float)((RVOMath.abs(RVOMath.normalize(goalDirection) / 1f) > 0.1) ? 0.1 : RVOMath.abs(RVOMath.normalize(goalDirection) / 1f)); float speed = (float)((RVOMath.abs(RVOMath.normalize(goalDirection)) > AgentReference.maxSpeed_ * 0.9) ? AgentReference.maxSpeed_ * 0.9 : RVOMath.abs(RVOMath.normalize(goalDirection)) * AgentReference.maxSpeed_); goalDirection = new Vector2(RVOMath.normalize(goalDirection).x() * speed, RVOMath.normalize(goalDirection).y() * speed); } } else { pathStatus = -1; path = null; //setDestination(transform.position); } AgentReference.prefVelocity_ = goalDirection * RVOMagnify.Magnify; }
public void addObstacle(KInt2 a, KInt2 b) { Obstacle first = new Obstacle(); first.convex_ = true; Obstacle second = new Obstacle(); second.convex_ = true; first.previous_ = second; second.previous_ = first; first.next_ = second; second.next_ = first; first.point_ = a; second.point_ = b; first.direction_ = RVOMath.normalize(KInt2.ToInt2(b.x - a.x, b.y - a.y)); second.direction_ = -first.direction_; first.id_ = obstacles_.Count; obstacles_.Add(first); second.id_ = obstacles_.Count; obstacles_.Add(second); }
/** * <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 <Vector3> vertices3, float height) { List <Vector2> vertices = new List <Vector2>(); List <float> yPos = new List <float>(); foreach (Vector3 avertex3 in vertices3) { vertices.Add(new Vector2(avertex3.x, avertex3.z)); yPos.Add(avertex3.y); } 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]; obstacle.curHeight_ = yPos[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.ObsHeight_ = height; obstacle.id_ = obstacles_.Count; obstacles_.Add(obstacle); } RefreshList(); return(obstacleNo); }
/** * <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 Obstacle addObstacle(IList <Vector2> vertices, bool _isMapBound) { if (vertices.Count < 2) { return(null); } int obstacleNo = obstacles_.Count; Obstacle result = null; for (int i = 0; i < vertices.Count; ++i) { Obstacle obstacle = new Obstacle(); obstacle.isMapBound = _isMapBound; if (result == null) { result = 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.0); } obstacles_.Add(obstacle); } return(result); }
/** * <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. * 结果不满足约束i,计算新的最优结果 */ Vector2 tempResult = result; if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, ref result)) { result = tempResult; return(i); } } } return(lines.Count); }
/** * <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); }
void setPreferredVelocities() { /* * Set the preferred velocity to be a vector of unit magnitude * (speed) in the direction of the goal. */ for (int i = 0; i < Simulator.Instance.getNumAgents(); ++i) { Vector3 goalVector = goals[i] - Simulator.Instance.getAgentPosition(i); if (RVOMath.absSq(goalVector) > 1.0f) { goalVector = RVOMath.normalize(goalVector); } Simulator.Instance.setAgentPrefVelocity(i, goalVector); } }
public void RunTest() { //Blocks blocks = new Blocks(); ///* Set up the scenario. */ //blocks.setupScenario(); Parallel.For(0, Simulator.Instance.getNumAgents(), i => // for (int i = 0; i < Simulator.Instance.getNumAgents(); i++) { Simulator.Instance.DoTree(); Vector2 goalVector = goals[i] - Simulator.Instance.getAgentPosition(i); if (RVOMath.absSq(goalVector) > 1.0f) { goalVector = RVOMath.normalize(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; Simulator.Instance.setAgentPrefVelocity(i, Simulator.Instance.getAgentPrefVelocity(i) + dist * new Vector2((float)Math.Cos(angle), (float)Math.Sin(angle))); Simulator.Instance.Compute(i); Simulator.Instance.Update(i); } ); // blocks.setPreferredVelocities(); // Simulator.Instance.doStep(); //} //while (!blocks.reachedGoal()); }
/** * <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) { // directionOpt 第一次为false,第二次为true,directionOpt主要用在 linearProgram1 里面 if (directionOpt) { /* * Optimize direction. Note that the optimization velocity is of * unit length in this case. */ // 1.这个其实没什么用,只是因为velocity是归一化的所以直接乘 radius result = optVelocity * radius; } else if (RVOMath.absSq(optVelocity) > RVOMath.sqr(radius)) { /* Optimize closest point and outside circle. */ // 2.当 optVelocity 太大时,先归一化optVelocity,再乘 radius result = RVOMath.normalize(optVelocity) * radius; } else { /* Optimize closest point and inside circle. */ // 3.当 optVelocity 小于maxSpeed时 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>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, Vector3 optVelocity, bool directionOpt, ref Vector3 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. */ Vector3 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(); //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 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() { 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_); } }
private void acceleratedProgram(Vector2 velocity, ref Vector2 result) { //Debug.Log(velocity.ToString() + " " + result.ToString()); float speed = RVOMath.abs(velocity); float maxSpeed = RVOMath.Min(speed + accelerated_, maxSpeed_); float minSpeed = RVOMath.Max(speed - accelerated_, 0.0f); Vector2 tempResult = result; //Debug.Log(speed); if (speed >= minSpeedToTurn_) { Vector2 leftVerticalVelocity = new Vector2(-velocity.y_, velocity.x_); Vector2 rightVerticalVelocity = new Vector2(velocity.y_, -velocity.x_); leftVerticalVelocity = RVOMath.normalize(leftVerticalVelocity) * speed * angularSpeed_; rightVerticalVelocity = RVOMath.normalize(rightVerticalVelocity) * speed * angularSpeed_; Vector2 leftMin = RVOMath.normalize(velocity + leftVerticalVelocity) * minSpeed; Vector2 leftMax = RVOMath.normalize(velocity + leftVerticalVelocity) * maxSpeed; Vector2 rightMin = RVOMath.normalize(velocity + rightVerticalVelocity) * minSpeed; Vector2 rightMax = RVOMath.normalize(velocity + rightVerticalVelocity) * maxSpeed; Vector2 leftVector2 = leftMax - leftMin; Vector2 rightVector2 = rightMax - rightMin; if (RVOMath.det(leftVector2, result) < 0.0f && RVOMath.det(rightVector2, result) > 0.0f) //middle { float resultSpeed = RVOMath.abs(result); if (resultSpeed > maxSpeed) { tempResult = RVOMath.normalize(result) * maxSpeed; } if (resultSpeed < minSpeed) { tempResult = RVOMath.normalize(result) * minSpeed; } } else { if (RVOMath.det(leftVector2, result) > 0.0f) //left { float r = ((result - leftMin) * (leftMax - leftMin)) / RVOMath.absSq(leftMax - leftMin); if (id_ == 2) { Debug.Log(r); } if (r < 0.0f) { tempResult = RVOMath.normalize(leftVector2) * minSpeed; } if (r > 1.0f) { tempResult = RVOMath.normalize(leftVector2) * maxSpeed; } if (r >= 0.0f && r <= 1.0f) { float resultSpeed = RVOMath.absSq(result) - RVOMath.distSqPointLineSegment(leftMin, leftMax, result); resultSpeed = RVOMath.sqrt(resultSpeed); if (resultSpeed > maxSpeed) { tempResult = RVOMath.normalize(leftVector2) * maxSpeed; } if (resultSpeed < minSpeed) { tempResult = RVOMath.normalize(leftVector2) * minSpeed; } if (resultSpeed >= minSpeed && resultSpeed <= maxSpeed) { tempResult = RVOMath.normalize(leftVector2) * resultSpeed; } } } if (RVOMath.det(rightVector2, result) < 0.0f) //right { float r = ((result - rightMin) * (rightMax - rightMin)) / RVOMath.absSq(rightMax - rightMin); if (r < 0.0f) { tempResult = RVOMath.normalize(rightVector2) * minSpeed; } if (r > 1.0f) { tempResult = RVOMath.normalize(rightVector2) * maxSpeed; } if (r >= 0.0f && r <= 1.0f) { float resultSpeed = RVOMath.absSq(result) - RVOMath.distSqPointLineSegment(rightMin, rightMax, result); resultSpeed = RVOMath.sqrt(resultSpeed); if (resultSpeed > maxSpeed) { tempResult = RVOMath.normalize(rightVector2) * maxSpeed; } if (resultSpeed < minSpeed) { tempResult = RVOMath.normalize(rightVector2) * minSpeed; } if (resultSpeed >= minSpeed && resultSpeed <= maxSpeed) { tempResult = RVOMath.normalize(rightVector2) * resultSpeed; } } } } } else { float resultSpeed = RVOMath.abs(result); if (resultSpeed > maxSpeed) { if (RVOMath.abs(velocity) < 1e-6f) { tempResult = RVOMath.normalize(prefVelocity_) * maxSpeed; } else { tempResult = RVOMath.normalize(velocity) * maxSpeed; } } if (resultSpeed < minSpeed) { tempResult = RVOMath.normalize(velocity) * minSpeed; } } result = tempResult; }
/** * <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) { if (mass_ != 1) { Debug.Log("linearProgram3 beginLine:" + beginLine); } float distance = 0.0f; // 遍历所有剩余ORCA线 for (int i = beginLine; i < lines.Count; ++i) { // 每一条 ORCA 线都需要精确的做出处理,distance 为 最大违规的速度 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>(); // 1.静态阻挡的orca线直接加到projLines中 for (int ii = 0; ii < numObstLines; ++ii) { projLines.Add(lines[ii]); } // 2.动态阻挡的orca线需要重新计算line,从第一个非静态阻挡到当前的orca线 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. */ // 2-1 两条线平行且同向 continue; } else { /* Line i and line j point in opposite direction. */ // 2-2 两条线平行且反向 line.point = 0.5f * (lines[i].point + lines[j].point); } } else { // 2-3 两条线不平行 line.point = lines[i].point + (RVOMath.det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction; } // 计算ORCA线的方向 line.direction = RVOMath.normalize(lines[j].direction - lines[i].direction); projLines.Add(line); } // 3.再次计算最优速度 Vector2 tempResult = result; // 注意这里的 new Vector2(-lines[i].direction.y(), lines[i].direction.x()) 是方向向量 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); } } }