private void LinearProgram3(IList <OrcaLine> lines, int numObstLines, int beginLine, float radius, Vector3 curVelocity, ref Vector3 result) { float distance = 0.0f; for (int i = beginLine; i < lines.Count; ++i) { if (VectorMath.det(lines[i].direction, lines[i].point - result) > distance) { /* Result does not satisfy constraint of line i. */ //std::vector<Line> projLines(lines.begin(), lines.begin() + numObstLines); IList <OrcaLine> projLines = new List <OrcaLine>(); for (int ii = 0; ii < numObstLines; ++ii) { projLines.Add(lines[ii]); } for (int j = numObstLines; j < i; ++j) { OrcaLine line; float determinant = VectorMath.det(lines[i].direction, lines[j].direction); if (VectorMath.fabs(determinant) <= VectorMath.EPSILON) { /* Line i and line j are parallel. */ if (VectorMath.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 + (VectorMath.det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction; } line.direction = VectorMath.normalize(lines[j].direction - lines[i].direction); projLines.Add(line); } Vector3 tempResult = result; if (LinearProgram2(projLines, radius, new Vector3(-lines[i].direction.Z, 0, lines[i].direction.X), curVelocity, 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 = VectorMath.det(lines[i].direction, lines[i].point - result); } } }
private int LinearProgram2(IList <OrcaLine> lines, float radius, Vector3 optDir, Vector3 curVelocity, bool directionOpt, ref Vector3 result) { Vector3 optVelocity = optDir * radius; result = optVelocity; for (int i = 0; i < lines.Count; ++i) { if (VectorMath.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, directionOpt ? optDir : optVelocity, curVelocity, directionOpt, ref result)) { result = tempResult; return(i); } } } return(lines.Count); }
/* Search for the best new velocity. */ public Vector3 ComputeNewVelocity(ISpaceObject obj, Vector3 prefDir, float timeStep, KdTree kdTree, KdObstacleTree kdObstacleTree, float maxSpeed, float neighborDist, bool isUsingAvoidanceVelocity) { ComputeNeighbors(obj, kdTree, kdObstacleTree, maxSpeed, neighborDist); ClearOrcaLine(); Vector3 position = obj.GetPosition(); float radius = (float)obj.GetRadius(); Vector3 velocity = obj.GetVelocity(); Vector3 newVelocity = new Vector3(); float invTimeHorizonObst = 1.0f / m_TimeHorizonObst; /* Create obstacle ORCA lines. */ for (int i = 0; i < m_ObstacleNeighbors.Count; ++i) { Obstacle obstacle1 = m_ObstacleNeighbors[i].Value; Obstacle obstacle2 = obstacle1.m_NextObstacle; Vector3 relativePosition1 = obstacle1.m_Point - position; Vector3 relativePosition2 = obstacle2.m_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 < m_OrcaLines.Count; ++j) { if (VectorMath.det(invTimeHorizonObst * relativePosition1 - m_OrcaLines[j].point, m_OrcaLines[j].direction) - invTimeHorizonObst * radius >= -VectorMath.EPSILON && VectorMath.det(invTimeHorizonObst * relativePosition2 - m_OrcaLines[j].point, m_OrcaLines[j].direction) - invTimeHorizonObst * radius >= -VectorMath.EPSILON) { alreadyCovered = true; break; } } if (alreadyCovered) { continue; } /* Not yet covered. Check for collisions. */ float distSq1 = VectorMath.absSq(relativePosition1); float distSq2 = VectorMath.absSq(relativePosition2); float radiusSq = VectorMath.sqr(radius); Vector3 obstacleVector = obstacle2.m_Point - obstacle1.m_Point; float s = -VectorMath.dot(relativePosition1, obstacleVector) / VectorMath.absSq(obstacleVector); float distSqLine = VectorMath.absSq(-relativePosition1 - s * obstacleVector); OrcaLine line; if (s < 0 && distSq1 <= radiusSq) { /* Collision with left vertex. Ignore if non-convex. */ if (obstacle1.m_IsConvex) { line.point = new Vector3(); line.direction = VectorMath.normalize(new Vector3(-relativePosition1.Z, 0, relativePosition1.X)); AddOrcaLine(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 neighoring obstace */ if (obstacle2.m_IsConvex && VectorMath.det(relativePosition2, obstacle2.m_UnitDir) >= 0) { line.point = new Vector3(); line.direction = VectorMath.normalize(new Vector3(-relativePosition2.Z, 0, relativePosition2.X)); AddOrcaLine(line); } continue; } else if (s >= 0 && s < 1 && distSqLine <= radiusSq) { /* Collision with obstacle segment. */ line.point = new Vector3(); line.direction = -obstacle1.m_UnitDir; AddOrcaLine(line); continue; } /* * No collision. * Compute legs. When obliquely viewed, both legs can come from a single * vertex. Legs extend cut-off line when nonconvex vertex. */ Vector3 leftLegDirection, rightLegDirection; if (s < 0 && distSqLine <= radiusSq) { /* * Obstacle viewed obliquely so that left vertex * defines velocity obstacle. */ if (!obstacle1.m_IsConvex) { /* Ignore obstacle. */ continue; } obstacle2 = obstacle1; float leg1 = VectorMath.sqrt(distSq1 - radiusSq); leftLegDirection = new Vector3(relativePosition1.X * leg1 - relativePosition1.Z * radius, 0, relativePosition1.X * radius + relativePosition1.Z * leg1) / distSq1; rightLegDirection = new Vector3(relativePosition1.X * leg1 + relativePosition1.Z * radius, 0, -relativePosition1.X * radius + relativePosition1.Z * leg1) / distSq1; } else if (s > 1 && distSqLine <= radiusSq) { /* * Obstacle viewed obliquely so that * right vertex defines velocity obstacle. */ if (!obstacle2.m_IsConvex) { /* Ignore obstacle. */ continue; } obstacle1 = obstacle2; float leg2 = VectorMath.sqrt(distSq2 - radiusSq); leftLegDirection = new Vector3(relativePosition2.X * leg2 - relativePosition2.Z * radius, 0, relativePosition2.X * radius + relativePosition2.Z * leg2) / distSq2; rightLegDirection = new Vector3(relativePosition2.X * leg2 + relativePosition2.Z * radius, 0, -relativePosition2.X * radius + relativePosition2.Z * leg2) / distSq2; } else { /* Usual situation. */ if (obstacle1.m_IsConvex) { float leg1 = VectorMath.sqrt(distSq1 - radiusSq); leftLegDirection = new Vector3(relativePosition1.X * leg1 - relativePosition1.Z * radius, 0, relativePosition1.X * radius + relativePosition1.Z * leg1) / distSq1; } else { /* Left vertex non-convex; left leg extends cut-off line. */ leftLegDirection = -obstacle1.m_UnitDir; } if (obstacle2.m_IsConvex) { float leg2 = VectorMath.sqrt(distSq2 - radiusSq); rightLegDirection = new Vector3(relativePosition2.X * leg2 + relativePosition2.Z * radius, 0, -relativePosition2.X * radius + relativePosition2.Z * leg2) / distSq2; } else { /* Right vertex non-convex; right leg extends cut-off line. */ rightLegDirection = obstacle1.m_UnitDir; } } /* * 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.m_PrevObstacle; bool isLeftLegForeign = false; bool isRightLegForeign = false; if (obstacle1.m_IsConvex && VectorMath.det(leftLegDirection, -leftNeighbor.m_UnitDir) >= 0.0f) { /* Left leg points into obstacle. */ leftLegDirection = -leftNeighbor.m_UnitDir; isLeftLegForeign = true; } if (obstacle2.m_IsConvex && VectorMath.det(rightLegDirection, obstacle2.m_UnitDir) <= 0.0f) { /* Right leg points into obstacle. */ rightLegDirection = obstacle2.m_UnitDir; isRightLegForeign = true; } /* Compute cut-off centers. */ Vector3 leftCutoff = invTimeHorizonObst * (obstacle1.m_Point - position); Vector3 rightCutoff = invTimeHorizonObst * (obstacle2.m_Point - position); Vector3 cutoffVec = rightCutoff - leftCutoff; /* Project current velocity on velocity obstacle. */ /* Check if current velocity is projected on cutoff circles. */ float t = (obstacle1 == obstacle2 ? 0.5f : VectorMath.dot((velocity - leftCutoff), cutoffVec) / VectorMath.absSq(cutoffVec)); float tLeft = VectorMath.dot((velocity - leftCutoff), leftLegDirection); float tRight = VectorMath.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. */ Vector3 unitW = VectorMath.normalize(velocity - leftCutoff); line.direction = new Vector3(unitW.Z, 0, -unitW.X); line.point = leftCutoff + radius * invTimeHorizonObst * unitW; AddOrcaLine(line); continue; } else if (t > 1.0f && tRight < 0.0f) { /* Project on right cut-off circle. */ Vector3 unitW = VectorMath.normalize(velocity - rightCutoff); line.direction = new Vector3(unitW.Z, 0, -unitW.X); line.point = rightCutoff + radius * invTimeHorizonObst * unitW; AddOrcaLine(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 : VectorMath.absSq(velocity - (leftCutoff + t * cutoffVec))); float distSqLeft = ((tLeft < 0.0f) ? float.PositiveInfinity : VectorMath.absSq(velocity - (leftCutoff + tLeft * leftLegDirection))); float distSqRight = ((tRight < 0.0f) ? float.PositiveInfinity : VectorMath.absSq(velocity - (rightCutoff + tRight * rightLegDirection))); if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) { /* Project on cut-off line. */ line.direction = -obstacle1.m_UnitDir; line.point = leftCutoff + radius * invTimeHorizonObst * new Vector3(-line.direction.Z, 0, line.direction.X); AddOrcaLine(line); continue; } else if (distSqLeft <= distSqRight) { /* Project on left leg. */ if (isLeftLegForeign) { continue; } line.direction = leftLegDirection; line.point = leftCutoff + radius * invTimeHorizonObst * new Vector3(-line.direction.Z, 0, line.direction.X); AddOrcaLine(line); continue; } else { /* Project on right leg. */ if (isRightLegForeign) { continue; } line.direction = -rightLegDirection; line.point = rightCutoff + radius * invTimeHorizonObst * new Vector3(-line.direction.Z, 0, line.direction.X); AddOrcaLine(line); continue; } } int numObstLines = m_OrcaLines.Count; float invTimeHorizon = 1.0f / m_TimeHorizon; /* Create obj ORCA lines. */ for (int i = 0; i < m_ObjNeighbors.Count; ++i) { KdTreeObject other = m_ObjNeighbors[i].Value; if (obj == other.SpaceObject) { continue; } Vector3 relativePosition = other.Position - position; Vector3 relativeVelocity = velocity - other.Velocity; float distSq = VectorMath.absSq(relativePosition); float combinedRadius = radius + other.Radius; float combinedRadiusSq = VectorMath.sqr(combinedRadius); OrcaLine line; Vector3 u; if (distSq > combinedRadiusSq) { /* No collision. */ Vector3 w = relativeVelocity - invTimeHorizon * relativePosition; /* Vector from cutoff center to relative velocity. */ float wLengthSq = VectorMath.absSq(w); float dotProduct1 = VectorMath.dot(w, relativePosition); if (dotProduct1 < 0.0f && VectorMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq) { /* Project on cut-off circle. */ float wLength = VectorMath.sqrt(wLengthSq); Vector3 unitW = w / wLength; line.direction = new Vector3(unitW.Z, 0, -unitW.X); u = (combinedRadius * invTimeHorizon - wLength) * unitW; } else { /* Project on legs. */ float leg = VectorMath.sqrt(distSq - combinedRadiusSq); if (VectorMath.det(relativePosition, w) > 0.0f) { /* Project on left leg. */ line.direction = new Vector3(relativePosition.X * leg - relativePosition.Z * combinedRadius, 0, relativePosition.X * combinedRadius + relativePosition.Z * leg) / distSq; } else { /* Project on right leg. */ line.direction = -new Vector3(relativePosition.X * leg + relativePosition.Z * combinedRadius, 0, -relativePosition.X * combinedRadius + relativePosition.Z * leg) / distSq; } float dotProduct2 = VectorMath.dot(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. */ Vector3 w = relativeVelocity - invTimeStep * relativePosition; float wLength = VectorMath.abs(w); if (wLength < VectorMath.EPSILON) { //LogSystem.Error("RvoAlgorithm error: Objects already overlap ! {0}({1}):{2}-{3}({4}):{5}", obj.GetID(), obj.GetObjType(), obj.GetPosition().ToString(), other.SpaceObject.GetID(), other.SpaceObject.GetObjType(), other.Position.ToString()); continue; } Vector3 unitW = w / wLength; line.direction = new Vector3(unitW.Z, 0, -unitW.X); u = (combinedRadius * invTimeStep - wLength) * unitW; } line.point = velocity + 0.5f * u; AddOrcaLine(line); } Vector3 avoidanceVelocity; if (isUsingAvoidanceVelocity) { avoidanceVelocity = obj.GetVelocity(); } else { avoidanceVelocity = new Vector3(); } int lineFail = LinearProgram2(m_OrcaLines, maxSpeed, prefDir, avoidanceVelocity, false, ref newVelocity); if (lineFail < m_OrcaLines.Count) { LinearProgram3(m_OrcaLines, numObstLines, lineFail, maxSpeed, avoidanceVelocity, ref newVelocity); } return(newVelocity); }
private bool LinearProgram1(IList <OrcaLine> lines, int lineNo, float radius, Vector3 optVelocity, Vector3 curVelocity, bool directionOpt, ref Vector3 result) { float dotProduct = VectorMath.dot(lines[lineNo].point, lines[lineNo].direction); float discriminant = VectorMath.sqr(dotProduct) + VectorMath.sqr(radius) - VectorMath.absSq(lines[lineNo].point); if (discriminant < 0.0f) { /* Max speed circle fully invalidates line lineNo. */ return(false); } float sqrtDiscriminant = VectorMath.sqrt(discriminant); float tLeft = -dotProduct - sqrtDiscriminant; //最大速度圆与半平面分隔线的左交点(远离射线方向) float tRight = -dotProduct + sqrtDiscriminant; //最大速度圆与半平面分隔线的右交点(靠近射线方向) for (int i = 0; i < lineNo; ++i) { float denominator = VectorMath.det(lines[lineNo].direction, lines[i].direction); float numerator = VectorMath.det(lines[i].direction, lines[lineNo].point - lines[i].point); if (VectorMath.fabs(denominator) <= VectorMath.EPSILON) { /* Lines lineNo and i are (almost) parallel. */ if (numerator < 0.0f) { return(false);//此情形line i的半平面与line No半平面没有交集 } else { continue;//此情形line i的半平面完全包含line No半平面 } } float t = numerator / denominator;//t是line i半平面分隔线与line No分隔线的交点到line No参考点的距离(符号表示在参考点左边还是右边) if (denominator >= 0.0f) { /* Line i bounds line lineNo on the right. */ tRight = Math.Min(tRight, t); } else { /* Line i bounds line lineNo on the left. */ tLeft = Math.Max(tLeft, t); } if (tLeft > tRight) { return(false);//i之前的半平面无交集 } } if (directionOpt) { if (curVelocity.LengthSquared() > VectorMath.EPSILON) { if (VectorMath.dot(curVelocity, lines[lineNo].direction) > 0.0f) { result = lines[lineNo].point + tRight * lines[lineNo].direction; } else { result = lines[lineNo].point + tLeft * lines[lineNo].direction; } } else { /* Optimize direction. */ if (Math.Abs(tLeft) < VectorMath.EPSILON || VectorMath.dot(optVelocity, lines[lineNo].direction) > 0.0f && (Math.Abs(tRight) > radius * 0.5f || Math.Abs(tRight) > Math.Abs(tLeft))) { /* Take right extreme. */ result = lines[lineNo].point + tRight * lines[lineNo].direction; } else { /* Take left extreme. */ result = lines[lineNo].point + tLeft * lines[lineNo].direction; } } } else { /* Optimize closest point. */ float t = VectorMath.dot(lines[lineNo].direction, (optVelocity - lines[lineNo].point));//预选速度矢量与line No的交点到参考点的距离(带符号) if (t < tLeft) { result = lines[lineNo].point + tLeft * lines[lineNo].direction; } else if (t > tRight) { result = lines[lineNo].point + tRight * lines[lineNo].direction; } else { result = lines[lineNo].point + t * lines[lineNo].direction; } } return(true); }
private ObstacleTreeNode BuildRecursive(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.m_NextObstacle; /* Compute optimal split node. */ for (int j = 0; j < obstacles.Count; ++j) { if (i == j) { continue; } Obstacle obstacleJ1 = obstacles[j]; Obstacle obstacleJ2 = obstacleJ1.m_NextObstacle; float j1LeftOfI = VectorMath.leftOf(obstacleI1.m_Point, obstacleI2.m_Point, obstacleJ1.m_Point); float j2LeftOfI = VectorMath.leftOf(obstacleI1.m_Point, obstacleI2.m_Point, obstacleJ2.m_Point); if (j1LeftOfI >= -VectorMath.EPSILON && j2LeftOfI >= -VectorMath.EPSILON) { ++leftSize; } else if (j1LeftOfI <= VectorMath.EPSILON && j2LeftOfI <= VectorMath.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.m_NextObstacle; for (int j = 0; j < obstacles.Count; ++j) { if (i == j) { continue; } Obstacle obstacleJ1 = obstacles[j]; Obstacle obstacleJ2 = obstacleJ1.m_NextObstacle; float j1LeftOfI = VectorMath.leftOf(obstacleI1.m_Point, obstacleI2.m_Point, obstacleJ1.m_Point); float j2LeftOfI = VectorMath.leftOf(obstacleI1.m_Point, obstacleI2.m_Point, obstacleJ2.m_Point); if (j1LeftOfI >= -VectorMath.EPSILON && j2LeftOfI >= -VectorMath.EPSILON) { leftObstacles[leftCounter++] = obstacles[j]; } else if (j1LeftOfI <= VectorMath.EPSILON && j2LeftOfI <= VectorMath.EPSILON) { rightObstacles[rightCounter++] = obstacles[j]; } else { /* Split obstacle j. */ float t = VectorMath.det(obstacleI2.m_Point - obstacleI1.m_Point, obstacleJ1.m_Point - obstacleI1.m_Point) / VectorMath.det(obstacleI2.m_Point - obstacleI1.m_Point, obstacleJ1.m_Point - obstacleJ2.m_Point); Vector3 splitpoint = obstacleJ1.m_Point + t * (obstacleJ2.m_Point - obstacleJ1.m_Point); Obstacle newObstacle = new Obstacle(); newObstacle.m_Point = splitpoint; newObstacle.m_PrevObstacle = obstacleJ1; newObstacle.m_NextObstacle = obstacleJ2; newObstacle.m_IsConvex = true; newObstacle.m_UnitDir = obstacleJ1.m_UnitDir; m_Obstacles.Add(newObstacle); obstacleJ1.m_NextObstacle = newObstacle; obstacleJ2.m_PrevObstacle = newObstacle; if (j1LeftOfI > 0.0f) { leftObstacles[leftCounter++] = obstacleJ1; rightObstacles[rightCounter++] = newObstacle; } else { rightObstacles[rightCounter++] = obstacleJ1; leftObstacles[leftCounter++] = newObstacle; } } } node.m_Obstacle = obstacleI1; node.m_Left = BuildRecursive(leftObstacles); node.m_Right = BuildRecursive(rightObstacles); return(node); } } }