public static void ComputeNewVelocity(RVOSettingsComponent agent, float2 pos, float radius, NativeList <VelocityObstacle> neighbours, NativeList <ObstacleDistance> obstacleNeighbours, float invTimeStep, float2 prefVelocity, float2 velocity, float maxSpeed, ref float2 newVelocity) { Assert.IsTrue(math.all(prefVelocity.IsNumber())); var orcaLines = new NativeList <Line>(Allocator.Temp); var invTimeHorizonObst = 1 / agent.TimeHorizonObst; /* Create obstacle ORCA lines. */ for (var i = 0; i < obstacleNeighbours.Length; ++i) { var obstacle1 = obstacleNeighbours[i].Obstacle; var obstacle2 = obstacle1->Next; var relativePosition1 = obstacle1->Point - pos; var relativePosition2 = obstacle2->Point - pos; /* * Check if velocity obstacle of obstacle is already taken care * of by previously constructed obstacle ORCA lines. */ var alreadyCovered = false; for (var j = 0; j < orcaLines.Length; ++j) { if (math.determinant(new float2x2(invTimeHorizonObst * relativePosition1 - orcaLines[j].Point, orcaLines[j].Direction)) - invTimeHorizonObst * radius >= -Epsilon && math.determinant(new float2x2(invTimeHorizonObst * relativePosition2 - orcaLines[j].Point, orcaLines[j].Direction)) - invTimeHorizonObst * radius >= -Epsilon) { alreadyCovered = true; break; } } if (alreadyCovered) { continue; } /* Not yet covered. Check for collisions. */ var distSq1 = math.lengthsq(relativePosition1); var distSq2 = math.lengthsq(relativePosition2); var radiusSq = Math.Square(radius); var obstacleVector = obstacle2->Point - obstacle1->Point; var s = math.dot(-relativePosition1, obstacleVector) / math.lengthsq(obstacleVector); var distSqLine = math.lengthsq(-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 float2(0.0f, 0.0f); line.Direction = math.normalize(new float2(-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 && math.determinant(new float2x2(relativePosition2, obstacle2->Direction)) >= 0.0f) { line.Point = new float2(0.0f, 0.0f); line.Direction = math.normalize(new float2(-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 float2(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. */ float2 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; var leg1 = math.sqrt(distSq1 - radiusSq); leftLegDirection = new float2(relativePosition1.x * leg1 - relativePosition1.y * radius, relativePosition1.x * radius + relativePosition1.y * leg1) / distSq1; rightLegDirection = new float2(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; var leg2 = math.sqrt(distSq2 - radiusSq); leftLegDirection = new float2(relativePosition2.x * leg2 - relativePosition2.y * radius, relativePosition2.x * radius + relativePosition2.y * leg2) / distSq2; rightLegDirection = new float2(relativePosition2.x * leg2 + relativePosition2.y * radius, -relativePosition2.x * radius + relativePosition2.y * leg2) / distSq2; } else { /* Usual situation. */ if (obstacle1->Convex) { var leg1 = math.sqrt(distSq1 - radiusSq); leftLegDirection = new float2(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) { var leg2 = math.sqrt(distSq2 - radiusSq); rightLegDirection = new float2(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. */ var leftNeighbor = obstacle1->Previous; var isLeftLegForeign = false; var isRightLegForeign = false; if (obstacle1->Convex && math.determinant(new float2x2(leftLegDirection, -leftNeighbor->Direction)) >= 0.0f) { /* Left leg points into obstacle. */ leftLegDirection = -leftNeighbor->Direction; isLeftLegForeign = true; } if (obstacle2->Convex && math.determinant(new float2x2(rightLegDirection, obstacle2->Direction)) <= 0.0f) { /* Right leg points into obstacle. */ rightLegDirection = obstacle2->Direction; isRightLegForeign = true; } /* Compute cut-off centers. */ var leftCutOff = invTimeHorizonObst * (obstacle1->Point - pos); var rightCutOff = invTimeHorizonObst * (obstacle2->Point - pos); var cutOffVector = rightCutOff - leftCutOff; /* Project current velocity on velocity obstacle. */ /* Check if current velocity is projected on cutoff circles. */ var t = obstacle1 == obstacle2 ? 0.5f : math.dot((velocity - leftCutOff), cutOffVector) / math.lengthsq(cutOffVector); var tLeft = math.dot(velocity - leftCutOff, leftLegDirection); var tRight = math.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. */ var unitW = math.normalize(velocity - leftCutOff); line.Direction = new float2(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. */ var unitW = math.normalize(velocity - rightCutOff); line.Direction = new float2(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. */ var distSqCutoff = (t <0.0f || t> 1.0f || obstacle1 == obstacle2) ? float.PositiveInfinity : math.lengthsq(velocity - (leftCutOff + t * cutOffVector)); var distSqLeft = tLeft < 0.0f ? float.PositiveInfinity : math.lengthsq(velocity - (leftCutOff + tLeft * leftLegDirection)); var distSqRight = tRight < 0.0f ? float.PositiveInfinity : math.lengthsq(velocity - (rightCutOff + tRight * rightLegDirection)); if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) { /* Project on cut-off line. */ line.Direction = -obstacle1->Direction; line.Point = leftCutOff + radius * invTimeHorizonObst * new float2(-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 float2(-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 float2(-line.Direction.y, line.Direction.x); orcaLines.Add(line); } var numObstLines = orcaLines.Length; var invTimeHorizon = 1 / agent.TimeHorizon; for (var i = 0; i < neighbours.Length; ++i) { var neighbour = neighbours[i]; var relativePosition = neighbour.Position - pos; var relativeVelocity = velocity - neighbour.Velocity; var distSq = math.lengthsq(relativePosition); var combinedRadius = radius + neighbour.Radius; var combinedRadiusSq = Math.Square(combinedRadius); Line line; float2 u; if (distSq > combinedRadiusSq) { /* No collision. */ var w = relativeVelocity - invTimeHorizon * relativePosition; /* Vector from cutoff center to relative velocity. */ var wLengthSq = math.lengthsq(w); var dotProduct1 = math.dot(w, relativePosition); if (dotProduct1 < 0.0f && Math.Square(dotProduct1) > combinedRadiusSq * wLengthSq) { /* Project on cut-off circle. */ var wLength = math.sqrt(wLengthSq); var unitW = w / wLength; line.Direction = new float2(unitW.y, -unitW.x); u = (combinedRadius * invTimeHorizon - wLength) * unitW; } else { /* Project on legs. */ var leg = math.sqrt(distSq - combinedRadiusSq); if (math.determinant(new float2x2(relativePosition, w)) > 0.0f) { /* Project on left leg. */ line.Direction = new float2(relativePosition.x * leg - relativePosition.y * combinedRadius, relativePosition.x * combinedRadius + relativePosition.y * leg) / distSq; } else { /* Project on right leg. */ line.Direction = -new float2(relativePosition.x * leg + relativePosition.y * combinedRadius, -relativePosition.x * combinedRadius + relativePosition.y * leg) / distSq; } var dotProduct2 = math.dot(relativeVelocity, line.Direction); u = dotProduct2 * line.Direction - relativeVelocity; } } else { /* Collision. Project on cut-off circle of time timeStep. */ /* Vector from cutoff center to relative velocity. */ var w = relativeVelocity - invTimeStep * relativePosition; var wLength = math.length(w); var unitW = w / wLength; line.Direction = new float2(unitW.y, -unitW.x); u = (combinedRadius * invTimeStep - wLength) * unitW; } line.Point = velocity + 0.5f * u; orcaLines.Add(line); } var lineFail = LinearProgram2(orcaLines, maxSpeed, prefVelocity, false, ref newVelocity); if (lineFail < orcaLines.Length) { LinearProgram3(orcaLines, numObstLines, lineFail, maxSpeed, ref newVelocity); } }