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