void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float radius, Vector3 &result)
{
float distance = 0.0f;
for (size_t i = beginPlane; i < planes.size(); ++i) {
if (planes[i].normal * (planes[i].point - result) > distance) {
/* Result does not satisfy constraint of plane i. */
std::vector<Plane> projPlanes;
for (size_t j = 0; j < i; ++j) {
Plane plane;
const Vector3 crossProduct = cross(planes[j].normal, planes[i].normal);
if (absSq(crossProduct) <= RVO_EPSILON) {
/* Plane i and plane j are (almost) parallel. */
if (planes[i].normal * planes[j].normal > 0.0f) {
/* Plane i and plane j point in the same direction. */
continue;
}
else {
/* Plane i and plane j point in opposite direction. */
plane.point = 0.5f * (planes[i].point + planes[j].point);
}
}
else {
/* Plane.point is point on line of intersection between plane i and plane j. */
const Vector3 lineNormal = cross(crossProduct, planes[i].normal);
plane.point = planes[i].point + (((planes[j].point - planes[i].point) * planes[j].normal) / (lineNormal * planes[j].normal)) * lineNormal;
}
plane.normal = normalize(planes[j].normal - planes[i].normal);
projPlanes.push_back(plane);
}
const Vector3 tempResult = result;
if (linearProgram3(projPlanes, radius, planes[i].normal, true, result) < projPlanes.size()) {
/* 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 = planes[i].normal * (planes[i].point - result);
}
}
}
void Agent::computeNewVelocity()
{
orcaPlanes_.clear();
const float invTimeHorizon = 1.0f / timeHorizon_;
/* Create agent ORCA planes. */
for (size_t i = 0; i < agentNeighbors_.size(); ++i) {
const Agent *const other = agentNeighbors_[i].second;
const Vector3 relativePosition = other->position_ - position_;
const Vector3 relativeVelocity = velocity_ - other->velocity_;
const float distSq = absSq(relativePosition);
const float combinedRadius = radius_ + other->radius_;
const float combinedRadiusSq = sqr(combinedRadius);
Plane plane;
Vector3 u;
if (distSq > combinedRadiusSq) {
/* No collision. */
const Vector3 w = relativeVelocity - invTimeHorizon * relativePosition;
/* Vector from cutoff center to relative velocity. */
const float wLengthSq = absSq(w);
const float dotProduct = w * relativePosition;
if (dotProduct < 0.0f && sqr(dotProduct) > combinedRadiusSq * wLengthSq) {
/* Project on cut-off circle. */
const float wLength = std::sqrt(wLengthSq);
const Vector3 unitW = w / wLength;
plane.normal = unitW;
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
}
else {
/* Project on cone. */
const float a = distSq;
const float b = relativePosition * relativeVelocity;
const float c = absSq(relativeVelocity) - absSq(cross(relativePosition, relativeVelocity)) / (distSq - combinedRadiusSq);
const float t = (b + std::sqrt(sqr(b) - a * c)) / a;
const Vector3 w = relativeVelocity - t * relativePosition;
const float wLength = abs(w);
const Vector3 unitW = w / wLength;
plane.normal = unitW;
u = (combinedRadius * t - wLength) * unitW;
}
}
else {
/* Collision. */
const float invTimeStep = 1.0f / sim_->timeStep_;
const Vector3 w = relativeVelocity - invTimeStep * relativePosition;
const float wLength = abs(w);
const Vector3 unitW = w / wLength;
plane.normal = unitW;
u = (combinedRadius * invTimeStep - wLength) * unitW;
}
plane.point = velocity_ + 0.5f * u;
orcaPlanes_.push_back(plane);
}
const size_t planeFail = linearProgram3(orcaPlanes_, maxSpeed_, prefVelocity_, false, newVelocity_);
if (planeFail < orcaPlanes_.size()) {
linearProgram4(orcaPlanes_, planeFail, maxSpeed_, newVelocity_);
}
}
/* Search for the best new velocity. */
void Agent::computeNewVelocity()
{
orcaLines_.clear();
const float invTimeHorizonObst = 1.0f / timeHorizonObst_;
/* Create obstacle ORCA lines. */
for (size_t i = 0; i < obstacleNeighbors_.size(); ++i) {
const Obstacle* obstacle1 = obstacleNeighbors_[i].second;
const Obstacle* obstacle2 = obstacle1->nextObstacle;
const Vector2 relativePosition1 = obstacle1->point_ - position_;
const 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 (size_t j = 0; j < orcaLines_.size(); ++j) {
if (det(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON && det(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON) {
alreadyCovered = true;
break;
}
}
if (alreadyCovered) {
continue;
}
/* Not yet covered. Check for collisions. */
const float distSq1 = absSq(relativePosition1);
const float distSq2 = absSq(relativePosition2);
const float radiusSq = sqr(radius_);
const Vector2 obstacleVector = obstacle2->point_ - obstacle1->point_;
const float s = (-relativePosition1 * obstacleVector) / absSq(obstacleVector);
const float distSqLine = absSq(-relativePosition1 - s * obstacleVector);
Line line;
if (s < 0 && distSq1 <= radiusSq) {
/* Collision with left vertex. Ignore if non-convex. */
if (obstacle1->isConvex_) {
line.point = Vector2(0,0);
line.direction = normalize(Vector2(-relativePosition1.y(), relativePosition1.x()));
orcaLines_.push_back(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->isConvex_ && det(relativePosition2, obstacle2->unitDir_) >= 0) {
line.point = Vector2(0,0);
line.direction = normalize(Vector2(-relativePosition2.y(), relativePosition2.x()));
orcaLines_.push_back(line);
}
continue;
} else if (s >= 0 && s < 1 && distSqLine <= radiusSq) {
/* Collision with obstacle segment. */
line.point = Vector2(0,0);
line.direction = -obstacle1->unitDir_;
orcaLines_.push_back(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.
*/
Vector2 leftLegDirection, rightLegDirection;
if (s < 0 && distSqLine <= radiusSq) {
/*
* Obstacle viewed obliquely so that left vertex
* defines velocity obstacle.
*/
if (!obstacle1->isConvex_) {
/* Ignore obstacle. */
continue;
}
obstacle2 = obstacle1;
const float leg1 = std::sqrt(distSq1 - radiusSq);
leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
rightLegDirection = Vector2(relativePosition1.x() * leg1 + relativePosition1.y() * radius_, -relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
} else if (s > 1 && distSqLine <= radiusSq) {
/*
* Obstacle viewed obliquely so that
* right vertex defines velocity obstacle.
*/
if (!obstacle2->isConvex_) {
/* Ignore obstacle. */
continue;
}
obstacle1 = obstacle2;
const float leg2 = std::sqrt(distSq2 - radiusSq);
leftLegDirection = Vector2(relativePosition2.x() * leg2 - relativePosition2.y() * radius_, relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
} else {
/* Usual situation. */
if (obstacle1->isConvex_) {
const float leg1 = std::sqrt(distSq1 - radiusSq);
leftLegDirection = 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->unitDir_;
}
if (obstacle2->isConvex_) {
const float leg2 = std::sqrt(distSq2 - radiusSq);
rightLegDirection = 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->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.
*/
const Obstacle* const leftNeighbor = obstacle1->prevObstacle;
bool isLeftLegForeign = false;
bool isRightLegForeign = false;
if (obstacle1->isConvex_ && det(leftLegDirection, -leftNeighbor->unitDir_) >= 0.0f) {
/* Left leg points into obstacle. */
leftLegDirection = -leftNeighbor->unitDir_;
isLeftLegForeign = true;
}
if (obstacle2->isConvex_ && det(rightLegDirection, obstacle2->unitDir_) <= 0.0f) {
/* Right leg points into obstacle. */
rightLegDirection = obstacle2->unitDir_;
isRightLegForeign = true;
}
/* Compute cut-off centers. */
const Vector2 leftCutoff = invTimeHorizonObst * (obstacle1->point_ - position_);
const Vector2 rightCutoff = invTimeHorizonObst * (obstacle2->point_ - position_);
const Vector2 cutoffVec = rightCutoff - leftCutoff;
/* Project current velocity on velocity obstacle. */
/* Check if current velocity is projected on cutoff circles. */
const float t = (obstacle1 == obstacle2 ? 0.5f : ((velocity_ - leftCutoff) * cutoffVec) / absSq(cutoffVec));
const float tLeft = ((velocity_ - leftCutoff) * leftLegDirection);
const float tRight = ((velocity_ - rightCutoff) * rightLegDirection);
if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f)) {
/* Project on left cut-off circle. */
const Vector2 unitW = normalize(velocity_ - leftCutoff);
line.direction = Vector2(unitW.y(), -unitW.x());
line.point = leftCutoff + radius_ * invTimeHorizonObst * unitW;
orcaLines_.push_back(line);
continue;
} else if (t > 1.0f && tRight < 0.0f) {
/* Project on right cut-off circle. */
const Vector2 unitW = normalize(velocity_ - rightCutoff);
line.direction = Vector2(unitW.y(), -unitW.x());
line.point = rightCutoff + radius_ * invTimeHorizonObst * unitW;
orcaLines_.push_back(line);
continue;
}
/*
* Project on left leg, right leg, or cut-off line, whichever is closest
* to velocity.
*/
const float distSqCutoff = ((t < 0.0f || t > 1.0f || obstacle1 == obstacle2) ? std::numeric_limits<float>::infinity() : absSq(velocity_ - (leftCutoff + t * cutoffVec)));
const float distSqLeft = ((tLeft < 0.0f) ? std::numeric_limits<float>::infinity() : absSq(velocity_ - (leftCutoff + tLeft * leftLegDirection)));
const float distSqRight = ((tRight < 0.0f) ? std::numeric_limits<float>::infinity() : absSq(velocity_ - (rightCutoff + tRight * rightLegDirection)));
if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) {
/* Project on cut-off line. */
line.direction = -obstacle1->unitDir_;
line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x());
orcaLines_.push_back(line);
continue;
} else if (distSqLeft <= distSqRight) {
/* Project on left leg. */
if (isLeftLegForeign) {
continue;
}
line.direction = leftLegDirection;
line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x());
orcaLines_.push_back(line);
continue;
} else {
/* Project on right leg. */
if (isRightLegForeign) {
continue;
}
line.direction = -rightLegDirection;
line.point = rightCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x());
orcaLines_.push_back(line);
continue;
}
}
const size_t numObstLines = orcaLines_.size();
const float invTimeHorizon = 1.0f / timeHorizon_;
/* Create agent ORCA lines. */
for (size_t i = 0; i < agentNeighbors_.size(); ++i) {
const Agent* const other = agentNeighbors_[i].second;
const Vector2 relativePosition = other->position_ - position_;
const Vector2 relativeVelocity = velocity_ - other->velocity_;
const float distSq = absSq(relativePosition);
const float combinedRadius = radius_ + other->radius_;
const float combinedRadiusSq = sqr(combinedRadius);
Line line;
Vector2 u;
if (distSq > combinedRadiusSq) {
/* No collision. */
const Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;
/* Vector from cutoff center to relative velocity. */
const float wLengthSq = absSq(w);
const float dotProduct1 = w * relativePosition;
if (dotProduct1 < 0.0f && sqr(dotProduct1) > combinedRadiusSq * wLengthSq) {
/* Project on cut-off circle. */
const float wLength = std::sqrt(wLengthSq);
const Vector2 unitW = w / wLength;
line.direction = Vector2(unitW.y(), -unitW.x());
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
} else {
/* Project on legs. */
const float leg = std::sqrt(distSq - combinedRadiusSq);
if (det(relativePosition, w) > 0.0f) {
/* Project on left leg. */
line.direction = Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
} else {
/* Project on right leg. */
line.direction = -Vector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
}
const float dotProduct2 = relativeVelocity * line.direction;
u = dotProduct2 * line.direction - relativeVelocity;
}
} else {
/* Collision. Project on cut-off circle of time timeStep. */
const float invTimeStep = 1.0f / sim_->timeStep_;
/* Vector from cutoff center to relative velocity. */
const Vector2 w = relativeVelocity - invTimeStep * relativePosition;
const float wLength = abs(w);
const Vector2 unitW = w / wLength;
line.direction = Vector2(unitW.y(), -unitW.x());
u = (combinedRadius * invTimeStep - wLength) * unitW;
}
line.point = velocity_ + 0.5f * u;
orcaLines_.push_back(line);
}
size_t lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocity_);
if (lineFail < orcaLines_.size()) {
linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, newVelocity_);
}
}