// Update is called once per frame
void Update()
{
float rangeSqr = neighborsDist_ * neighborsDist_;
Vector2 position2D = new Vector2(transform.position.x, transform.position.z);
//Check neighbors
if (maxNeighbors_ > 0)
{
agentNeighbors_.Clear();
foreach (var agent in FindObjectsOfType <Agent>())
{
if (this != agent)
{
Vector3 dir = transform.position - agent.transform.position;
float distSqr = Vector3.Dot(dir, dir);
//If the other agent is under the minimum range => add it
if (distSqr < rangeSqr)
{
//If there is a free space, add it immediatly
if (agentNeighbors_.Count < maxNeighbors_)
{
agentNeighbors_.Add(new KeyValuePair <float, Agent>(distSqr, agent));
}
//Make sure the list is sorted
int i = agentNeighbors_.Count - 1;
while (i != 0 && distSqr < agentNeighbors_[i - 1].Key)
{
agentNeighbors_[i] = agentNeighbors_[i - 1];
i--;
}
//Once a spot with a further agent is found, place if
agentNeighbors_[i] = new KeyValuePair <float, Agent>(distSqr, agent);
//If the list is full, only check agent nearer than the farrest neighbor.
if (agentNeighbors_.Count == maxNeighbors_)
{
rangeSqr = agentNeighbors_[agentNeighbors_.Count - 1].Key;
}
}
}
}
}
//Find obstacles
obstacleNeighbors_.Clear();
foreach (var obstacle in obstacleManager_.GetObstacles())
{
Obstacle nextObstacle = obstacle.next;
float distSqr = DistSqrPointLine(
obstacle.line.point,
nextObstacle.line.point,
position2D);
//If the other agent is under the minimum range => add it
if (distSqr < rangeSqr)
{
obstacleNeighbors_.Add(new KeyValuePair <float, Obstacle>(distSqr, obstacle));
int i = obstacleNeighbors_.Count - 1;
while (i != 0 && distSqr < obstacleNeighbors_[i - 1].Key)
{
obstacleNeighbors_[i] = obstacleNeighbors_[i - 1];
i--;
}
obstacleNeighbors_[i] = new KeyValuePair <float, Obstacle>(distSqr, obstacle);
}
}
//Update movement
float dist = Vector3.Distance(transform.position, target_);
if (dist < stopDistance)
{
desiredVelocity_ = Vector2.zero;
}
else if (dist < arriveDistance)
{
Vector3 vel = (target_ - transform.position).normalized * (maxSpeed_ * (dist / arriveDistance));
desiredVelocity_ = new Vector2(vel.x, vel.z);
}
else
{
Vector3 vel = (target_ - transform.position).normalized * maxSpeed_;
desiredVelocity_ = new Vector2(vel.x, vel.z);
}
// if (desiredVelocity_ == Vector2.zero)
// {
// return;
// }
//Update velocity for static obstacles
orcaLines_.Clear();
float invTimeHorizon = 1.0f / timeHorizon_;
for (int i = 0; i < obstacleNeighbors_.Count; ++i)
{
Obstacle obstacle1 = obstacleNeighbors_[i].Value;
Obstacle obstacle2 = obstacle1.next;
Vector2 relativePosition1 = obstacle1.line.point - position2D;
Vector2 relativePosition2 = obstacle2.line.point - position2D;
bool alreadyCovered = false;
for (int j = 0; j < orcaLines_.Count; ++j)
{
if (Det(invTimeHorizon * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) -
invTimeHorizon * radius_ >= -0.0000001f &&
Det(invTimeHorizon * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) -
invTimeHorizon * radius_ >= -0.0000001f)
{
alreadyCovered = true;
break;
}
}
if (alreadyCovered)
{
continue;
}
//Check for collision
float distSqr1 = relativePosition1.sqrMagnitude;
float distSqr2 = relativePosition2.sqrMagnitude;
float radiusSqr = Mathf.Pow(radius_, 2);
Vector2 obstacleVector = obstacle2.line.point - obstacle1.line.point;
float s = -Vector2.Dot(relativePosition1, obstacleVector) / obstacleVector.sqrMagnitude;
float distSqrLine = (-relativePosition1 - s * obstacleVector).sqrMagnitude;
Line line;
if (s < 0.0f && distSqr1 <= radiusSqr)
{
//Collision with left
if (obstacle1.convex)
{
line.point = Vector2.zero;
line.direction = new Vector2(-relativePosition1.y, relativePosition1.x).normalized;
orcaLines_.Add(line);
}
continue;
}
else if (s > 1.0f && distSqr2 <= radiusSqr)
{
//Collision with right vertex
if (obstacle2.convex && Det(relativePosition2, obstacle2.line.direction) >= 0.0f)
{
line.point = Vector2.zero;
line.direction = new Vector2(-relativePosition2.y, relativePosition2.x).normalized;
orcaLines_.Add(line);
}
continue;
}
else if (s > 0.0f && s < 1.0f && distSqrLine <= radiusSqr)
{
//Collision with obstacle segement
line.point = Vector2.zero;
line.direction = -obstacle1.line.direction;
orcaLines_.Add(line);
continue;
}
//No collision => Compute legs
Vector2 leftLegDirection, rightLegDirection;
if (s < 0.0f && distSqrLine <= radiusSqr)
{
if (!obstacle1.convex)
{
continue;
}
obstacle2 = obstacle1;
float leg1 = Mathf.Sqrt(distSqr1 - radiusSqr);
leftLegDirection = new Vector2(relativePosition1.x * leg1 - relativePosition1.y * radius_, relativePosition1.x * radius_ + relativePosition1.y * leg1) / distSqr1;
rightLegDirection = new Vector2(relativePosition1.x * leg1 - relativePosition1.y * radius_, -relativePosition1.x * radius_ + relativePosition1.y * leg1) / distSqr1;
}
else if (s > 1.0f && distSqrLine <= radiusSqr)
{
if (!obstacle2.convex)
{
continue;
}
obstacle1 = obstacle2;
float leg1 = Mathf.Sqrt(distSqr2 - radiusSqr);
leftLegDirection = new Vector2(relativePosition2.x * leg1 - relativePosition2.y * radius_, relativePosition2.x * radius_ + relativePosition2.y * leg1) / distSqr2;
rightLegDirection = new Vector2(relativePosition2.x * leg1 - relativePosition2.y * radius_, -relativePosition2.x * radius_ + relativePosition2.y * leg1) / distSqr2;
}
else
{
if (obstacle1.convex)
{
float leg1 = Mathf.Sqrt(distSqr1 - radiusSqr);
leftLegDirection = new Vector2(relativePosition1.x * leg1 - relativePosition1.y * radius_, relativePosition1.x * radius_ + relativePosition1.y * leg1) / distSqr1;
}
else
{
leftLegDirection = -obstacle1.line.direction;
}
if (obstacle2.convex)
{
float leg1 = Mathf.Sqrt(distSqr2 - radiusSqr);
rightLegDirection = new Vector2(relativePosition2.x * leg1 - relativePosition2.y * radius_, relativePosition2.x * radius_ + relativePosition2.y * leg1) / distSqr2;
}
else
{
rightLegDirection = obstacle1.line.direction;
}
}
//Make sure leg doesn't go throught other leg
Obstacle leftNeighbor = obstacle1.previous;
bool isLeftLegForeign = false;
bool isRightLegForeign = false;
if (obstacle1.convex && Det(leftLegDirection, -leftNeighbor.line.direction) >= 0.0f)
{
leftLegDirection = -leftNeighbor.line.direction;
isLeftLegForeign = true;
}
if (obstacle2.convex && Det(rightLegDirection, obstacle2.line.direction) <= 0.0f)
{
rightLegDirection = obstacle2.line.direction;
isRightLegForeign = true;
}
//Compute cut-off centers
Vector2 leftCutOff = invTimeHorizon * (obstacle1.line.point - position2D);
Vector2 rightCutOff = invTimeHorizon * (obstacle2.line.point - position2D);
Vector2 cutOffVector = rightCutOff - leftCutOff;
//Check if current velocity if projected on cutoff circle
float t = obstacle1 == obstacle2
? 0.5f
: Vector2.Dot((velocity_ - leftCutOff), cutOffVector) / cutOffVector.sqrMagnitude;
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))
{
Vector2 unitW = (velocity_ - leftCutOff).normalized;
line.direction = new Vector2(unitW.y, -unitW.x);
line.point = leftCutOff + radius_ * invTimeHorizon * unitW;
orcaLines_.Add(line);
continue;
}
else if (t > 1.0f && tRight < 0.0f)
{
Vector2 unitW = (velocity_ - rightCutOff).normalized;
line.direction = new Vector2(unitW.y, -unitW.x);
line.point = rightCutOff + radius_ * invTimeHorizon * unitW;
orcaLines_.Add(line);
continue;
}
float distSqrCutoff = t <0.0f || t> 1.0f || obstacle1 == obstacle2
? float.PositiveInfinity
: (velocity_ - (leftCutOff + t * cutOffVector)).sqrMagnitude;
float distSqrLeft = tLeft < 0.0f
? float.PositiveInfinity
: (velocity_ - (leftCutOff + tLeft * leftLegDirection)).sqrMagnitude;
float distSqrRight = tRight < 0.0f
? float.PositiveInfinity
: (velocity_ - (rightCutOff + tRight * rightLegDirection)).sqrMagnitude;
if (distSqrCutoff <= distSqrLeft && distSqrCutoff <= distSqrRight)
{
line.direction = -obstacle1.line.direction;
line.point =
leftCutOff + radius_ * invTimeHorizon * new Vector2(-line.direction.y, line.direction.x);
orcaLines_.Add(line);
continue;
}
if (distSqrLeft <= distSqrRight)
{
if (isLeftLegForeign)
{
continue;
}
line.direction = leftLegDirection;
line.point =
leftCutOff + radius_ * invTimeHorizon * new Vector2(-line.direction.y, line.direction.x);
orcaLines_.Add(line);
continue;
}
if (isRightLegForeign)
{
continue;
}
line.direction = -rightLegDirection;
line.point =
rightCutOff + radius_ * invTimeHorizon * new Vector2(-line.direction.y, line.direction.x);
orcaLines_.Add(line);
}
int nbObstacleLine = orcaLines_.Count;
//Update velocity by looking at other agents
foreach (var pair in agentNeighbors_)
{
Agent otherAgent = pair.Value;
Vector3 relPos = otherAgent.transform.position - transform.position;
Vector2 relativePosition = new Vector2(relPos.x, relPos.z);
Vector2 relativeVelocity = velocity_ - otherAgent.velocity_;
float distSqr = relativePosition.sqrMagnitude;
float combinedRadius = radius_ + otherAgent.radius_;
float combinedRadiusSqr = Mathf.Pow(combinedRadius, 2);
Line line;
Vector2 u;
if (distSqr > combinedRadiusSqr)
{
// No Collision
Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;
// Vector from center to relative velocity
float wLengthSqr = w.sqrMagnitude;
float dotProduct1 = Vector2.Dot(w, relativePosition);
if (dotProduct1 < 0.0f && Mathf.Pow(dotProduct1, 2) > combinedRadiusSqr * wLengthSqr)
{
//Project on circle
float wLength = Mathf.Sqrt(wLengthSqr);
Vector2 unitW = w / wLength;
line.direction = new Vector2(unitW.y, -unitW.x);
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
}
else
{
//Projection on legs
float leg = Mathf.Sqrt(distSqr - combinedRadiusSqr);
if (Det(relativePosition, w) > 0.0f)
{
line.direction = new Vector2(
relativePosition.x * leg - relativePosition.y * combinedRadius,
relativePosition.x * combinedRadius + relativePosition.y * leg) / distSqr;
}
else
{
line.direction = -new Vector2(
relativePosition.x * leg - relativePosition.y * combinedRadius,
-relativePosition.x * combinedRadius + relativePosition.y * leg) / distSqr;
}
float dotProduct2 = Vector2.Dot(relativeVelocity, line.direction);
u = dotProduct2 * line.direction - relativeVelocity;
}
}
else
{
//Collision
float invTimeStep = 1.0f / Time.deltaTime;
Vector2 w = relativeVelocity - invTimeStep * relativePosition;
float wLength = w.magnitude;
Vector2 wUnit = w / wLength;
line.direction = new Vector2(wUnit.y, -wUnit.x);
u = (combinedRadius * invTimeStep - wLength) * wUnit;
}
line.point = velocity_ + 0.5f * u;
orcaLines_.Add(line);
}
int lineFail = LinearProgram2(orcaLines_, maxSpeed_, desiredVelocity_, false, ref newVelocity_);
if (lineFail < orcaLines_.Count)
{
LinearProgram3(orcaLines_, nbObstacleLine, lineFail, maxSpeed_, ref newVelocity_);
}
}