Goal * FarthestGoalSelector::getGoal( const Agents::BaseAgent * agent ) const {
assert( agent != 0x0 && "FarthestGoalSelector requires a valid base agent!" );
const size_t GOAL_COUNT = _goalSet->size();
if ( GOAL_COUNT == 0 ) {
logger << Logger::ERR_MSG << "FarthestGoalSelector was unable to provide a goal for agent " << agent->_id << ". There were no available goals in the goal set.";
return 0x0;
}
const Vector2 p = agent->_pos;
Goal * bestGoal;
bestGoal = _goalSet->getIthGoal( 0 );
Vector2 disp = bestGoal->getCentroid() - p;
float bestDist = absSq( disp );
for ( size_t i = 1; i < GOAL_COUNT; ++i ) {
Goal * testGoal = _goalSet->getIthGoal( i );
disp = testGoal->getCentroid() - p;
float testDist = absSq( disp );
if ( testDist > bestDist ) {
bestDist = testDist;
bestGoal = testGoal;
}
}
return bestGoal;
}
bool KdTree::queryVisibilityRecursive(const Vector2& q1, const Vector2& q2, float radius, const ObstacleTreeNode* node) const
{
if (node == 0) {
return true;
} else {
const Obstacle* const obstacle1 = node->obstacle;
const Obstacle* const obstacle2 = obstacle1->nextObstacle;
const float q1LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q1);
const float q2LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q2);
const float invLengthI = 1.0f / absSq(obstacle2->point_ - obstacle1->point_);
if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f) {
return queryVisibilityRecursive(q1, q2, radius, node->left) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->right));
} else if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f) {
return queryVisibilityRecursive(q1, q2, radius, node->right) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->left));
} else if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f) {
/* One can see through obstacle from left to right. */
return queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right);
} else {
const float point1LeftOfQ = leftOf(q1, q2, obstacle1->point_);
const float point2LeftOfQ = leftOf(q1, q2, obstacle2->point_);
const float invLengthQ = 1.0f / absSq(q2 - q1);
return (point1LeftOfQ * point2LeftOfQ >= 0.0f && sqr(point1LeftOfQ) * invLengthQ > sqr(radius) && sqr(point2LeftOfQ) * invLengthQ > sqr(radius) && queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right));
}
}
}
size_t linearProgram3(const std::vector<Plane> &planes, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result)
{
if (directionOpt) {
/* Optimize direction. Note that the optimization velocity is of unit length in this case. */
result = optVelocity * radius;
}
else if (absSq(optVelocity) > sqr(radius)) {
/* Optimize closest point and outside circle. */
result = normalize(optVelocity) * radius;
}
else {
/* Optimize closest point and inside circle. */
result = optVelocity;
}
for (size_t i = 0; i < planes.size(); ++i) {
if (planes[i].normal * (planes[i].point - result) > 0.0f) {
/* Result does not satisfy constraint i. Compute new optimal result. */
const Vector3 tempResult = result;
if (!linearProgram2(planes, i, radius, optVelocity, directionOpt, result)) {
result = tempResult;
return i;
}
}
}
return planes.size();
}
void Agent::insertAgentNeighbor(const Agent *agent, float &rangeSq)
{
if (this != agent) {
const float distSq = absSq(position_ - agent->position_);
if (distSq < rangeSq) {
if (agentNeighbors_.size() < maxNeighbors_) {
agentNeighbors_.push_back(std::make_pair(distSq, agent));
}
size_t i = agentNeighbors_.size() - 1;
while (i != 0 && distSq < agentNeighbors_[i - 1].first) {
agentNeighbors_[i] = agentNeighbors_[i - 1];
--i;
}
agentNeighbors_[i] = std::make_pair(distSq, agent);
if (agentNeighbors_.size() == maxNeighbors_) {
rangeSq = agentNeighbors_.back().first;
}
}
}
}
void KdTree::queryObstacleTreeRecursive(Agent* agent, float rangeSq, const ObstacleTreeNode* node) const
{
if (node == 0) {
return;
} else {
const Obstacle* const obstacle1 = node->obstacle;
const Obstacle* const obstacle2 = obstacle1->nextObstacle;
const float agentLeftOfLine = leftOf(obstacle1->point_, obstacle2->point_, agent->position_);
queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->left : node->right));
const float distSqLine = sqr(agentLeftOfLine) / absSq(obstacle2->point_ - obstacle1->point_);
if (distSqLine < rangeSq) {
if (agentLeftOfLine < 0.0f) {
/*
* Try obstacle at this node only if agent is on right side of
* obstacle (and can see obstacle).
*/
agent->insertObstacleNeighbor(node->obstacle, rangeSq);
}
/* Try other side of line. */
queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->right : node->left));
}
}
}
void ObstacleKDTree::queryTreeRecursive( ProximityQuery *filter, Vector2 pt, float& rangeSq, const ObstacleTreeNode* node) const {
if (node == 0) {
return;
} else {
const Obstacle* const obstacle1 = node->_obstacle;
const Vector2 P0 = obstacle1->getP0();
const Vector2 P1 = obstacle1->getP1();
const float agentLeftOfLine = leftOf( P0, P1, pt);
queryTreeRecursive(filter, pt, rangeSq, (agentLeftOfLine >= 0.0f ? node->_left : node->_right));
const float distSqLine = sqr(agentLeftOfLine) / absSq(P1 - P0);
if (distSqLine < rangeSq) {
if ( obstacle1->_doubleSided || agentLeftOfLine < 0.0f) {
/*
* Try obstacle at this node only if agent is on right side of
* obstacle (and can see obstacle).
*/
float distSq = distSqPointLineSegment(node->_obstacle->getP0(), node->_obstacle->getP1(), pt);
filter->filterObstacle(node->_obstacle, distSq);
rangeSq = filter->getMaxObstacleRange();
}
/* Try other side of line. */
queryTreeRecursive(filter, pt, rangeSq, (agentLeftOfLine >= 0.0f ? node->_right : node->_left));
}
}
}
// determines the time to collision of a ray from the origin with a circle (center, radius)
// Returns an "infinity" style number if no collision
float rayCircleTTC(const Vector2& dir, const Vector2& center, float radius) {
float a = absSq(dir);
float b = -2 * (dir * center);
float c = absSq(center) - (radius * radius);
float discr = b * b - 4 * a * c;
if (discr < 0.f) {
return INFTY;
}
const float sqrtDiscr = sqrtf(discr);
float t0 = (-b - sqrtDiscr) / (2.f * a);
float t1 = (-b + sqrtDiscr) / (2.f * a);
// If the points of collision have different signs, it means I'm already colliding
if ((t0 < 0.f && t1 > 0.f) || (t1 < 0.f && t0 > 0.f)) return 0.f;
if (t0 < t1 && t0 > 0.f)
return t0;
else if (t1 > 0.f)
return t1;
else
return INFTY;
}
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);
}
}
}
size_t Graph::getClosestVertex( const Vector2 & point, float radius ) {
assert( _vCount > 0 && "Trying to operate on an empty roadmap" );
// TODO: Make this faster
float bestDistSq = INFTY;
size_t bestID = -1;
for ( size_t i = 0; i < _vCount; ++i ) {
float testDistSq = absSq( _vertices[i].getPosition() - point );
if ( testDistSq < bestDistSq ) {
if ( Menge::SPATIAL_QUERY->queryVisibility( point, _vertices[i].getPosition(), radius ) ) {
bestDistSq = testDistSq;
bestID = i;
}
}
}
assert( bestID != -1 && "Roadmap Graph was unable to find a visible vertex" );
return bestID;
}
bool linearProgram2(const std::vector<Plane> &planes, size_t planeNo, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result)
{
const float planeDist = planes[planeNo].point * planes[planeNo].normal;
const float planeDistSq = sqr(planeDist);
const float radiusSq = sqr(radius);
if (planeDistSq > radiusSq) {
/* Max speed sphere fully invalidates plane planeNo. */
return false;
}
const float planeRadiusSq = radiusSq - planeDistSq;
const Vector3 planeCenter = planeDist * planes[planeNo].normal;
if (directionOpt) {
/* Project direction optVelocity on plane planeNo. */
const Vector3 planeOptVelocity = optVelocity - (optVelocity * planes[planeNo].normal) * planes[planeNo].normal;
const float planeOptVelocityLengthSq = absSq(planeOptVelocity);
if (planeOptVelocityLengthSq <= RVO_EPSILON) {
result = planeCenter;
}
else {
result = planeCenter + std::sqrt(planeRadiusSq / planeOptVelocityLengthSq) * planeOptVelocity;
}
}
else {
/* Project point optVelocity on plane planeNo. */
result = optVelocity + ((planes[planeNo].point - optVelocity) * planes[planeNo].normal) * planes[planeNo].normal;
/* If outside planeCircle, project on planeCircle. */
if (absSq(result) > radiusSq) {
const Vector3 planeResult = result - planeCenter;
const float planeResultLengthSq = absSq(planeResult);
result = planeCenter + std::sqrt(planeRadiusSq / planeResultLengthSq) * planeResult;
}
}
for (size_t i = 0; i < planeNo; ++i) {
if (planes[i].normal * (planes[i].point - result) > 0.0f) {
/* Result does not satisfy constraint i. Compute new optimal result. */
/* Compute intersection line of plane i and plane planeNo. */
Vector3 crossProduct = cross(planes[i].normal, planes[planeNo].normal);
if (absSq(crossProduct) <= RVO_EPSILON) {
/* Planes planeNo and i are (almost) parallel, and plane i fully invalidates plane planeNo. */
return false;
}
Line line;
line.direction = normalize(crossProduct);
const Vector3 lineNormal = cross(line.direction, planes[planeNo].normal);
line.point = planes[planeNo].point + (((planes[i].point - planes[planeNo].point) * planes[i].normal) / (lineNormal * planes[i].normal)) * lineNormal;
if (!linearProgram1(planes, i, line, radius, optVelocity, directionOpt, result)) {
return false;
}
}
}
return true;
}
bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result)
{
const float dotProduct = line.point * line.direction;
const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(line.point);
if (discriminant < 0.0f) {
/* Max speed sphere fully invalidates line. */
return false;
}
const float sqrtDiscriminant = std::sqrt(discriminant);
float tLeft = -dotProduct - sqrtDiscriminant;
float tRight = -dotProduct + sqrtDiscriminant;
for (size_t i = 0; i < planeNo; ++i) {
const float numerator = (planes[i].point - line.point) * planes[i].normal;
const float denominator = line.direction * planes[i].normal;
if (sqr(denominator) <= RVO_EPSILON) {
/* Lines line is (almost) parallel to plane i. */
if (numerator > 0.0f) {
return false;
}
else {
continue;
}
}
const float t = numerator / denominator;
if (denominator >= 0.0f) {
/* Plane i bounds line on the left. */
tLeft = std::max(tLeft, t);
}
else {
/* Plane i bounds line on the right. */
tRight = std::min(tRight, t);
}
if (tLeft > tRight) {
return false;
}
}
if (directionOpt) {
/* Optimize direction. */
if (optVelocity * line.direction > 0.0f) {
/* Take right extreme. */
result = line.point + tRight * line.direction;
}
else {
/* Take left extreme. */
result = line.point + tLeft * line.direction;
}
}
else {
/* Optimize closest point. */
const float t = line.direction * (optVelocity - line.point);
if (t < tLeft) {
result = line.point + tLeft * line.direction;
}
else if (t > tRight) {
result = line.point + tRight * line.direction;
}
else {
result = line.point + t * line.direction;
}
}
return true;
}
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_);
}
}
bool linearProgram1(const std::vector<Line>& lines, size_t lineNo, float radius, const Vector2& optVelocity, bool directionOpt, Vector2& result)
{
const float dotProduct = lines[lineNo].point * lines[lineNo].direction;
const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(lines[lineNo].point);
if (discriminant < 0.0f) {
/* Max speed circle fully invalidates line lineNo. */
return false;
}
const float sqrtDiscriminant = std::sqrt(discriminant);
float tLeft = -dotProduct - sqrtDiscriminant;
float tRight = -dotProduct + sqrtDiscriminant;
for (size_t i = 0; i < lineNo; ++i) {
const float denominator = det(lines[lineNo].direction, lines[i].direction);
const float numerator = det(lines[i].direction, lines[lineNo].point - lines[i].point);
if (std::fabs(denominator) <= RVO_EPSILON) {
/* Lines lineNo and i are (almost) parallel. */
if (numerator < 0.0f) {
return false;
} else {
continue;
}
}
const float t = numerator / denominator;
if (denominator >= 0.0f) {
/* Line i bounds line lineNo on the right. */
tRight = std::min(tRight, t);
} else {
/* Line i bounds line lineNo on the left. */
tLeft = std::max(tLeft, t);
}
if (tLeft > tRight) {
return false;
}
}
if (directionOpt) {
/* Optimize direction. */
if (optVelocity * lines[lineNo].direction > 0.0f) {
/* 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. */
const float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point);
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;
}
void Agent::computeNewVelocity() {
const float TAU = Simulator::REACTION_TIME;
const float STEP_TIME = Simulator::STRIDE_TIME;
float B = Simulator::FORCE_DISTANCE;
// driving force
Vector2 force( ( _velPref.getPreferredVel() - _vel ) / TAU );
// agent forces
float A = Simulator::AGENT_SCALE;
for ( size_t i = 0; i < _nearAgents.size(); ++i ) {
const Agents::BaseAgent * otherBase = _nearAgents[i].agent;
const Agent * const other = static_cast< const Agent *>( otherBase );
Vector2 relPos = _pos - other->_pos;
float dist = abs( relPos );
Vector2 relDir = relPos / dist;
// directional weight of force
float cosTheta = relDir * _orient;
float magnitude = A * ( _dirWeight + (1.f - _dirWeight) * ( 1 + cosTheta ) * 0.5f );
// elliptical term
Vector2 stepOffset = other->_vel * STEP_TIME;
Vector2 relPosOffset = relPos - stepOffset;
float relPosOffsetDist = abs( relPosOffset );
float term1 = dist + relPosOffsetDist;
float offsetDistSq = absSq( stepOffset );
float b = 0.5f * sqrtf( term1 * term1 - offsetDistSq );
float twoB = 2.f * b;
// Extra magnitude scaling term
magnitude *= term1 / twoB;
magnitude *= expf( -b / B );
// Force direction
Vector2 forceDir = 0.5f * ( relDir + ( relPosOffset / relPosOffsetDist ) );
force += magnitude * forceDir;
}
// wall forces
A = Simulator::OBST_SCALE;
for ( size_t i = 0; i < _nearObstacles.size(); ++i ) {
Vector2 nearPt; // set by distanceSqToPoint
float distSq; // set by distanceSqToPoint
if ( _nearObstacles[ i ].obstacle->distanceSqToPoint( _pos, nearPt, distSq ) == Agents::Obstacle::LAST ) continue;
float dist = sqrtf( distSq );
Vector2 relPos = _pos - nearPt;
Vector2 relDir = relPos / dist;
// directional weight of force
float cosTheta = relDir * _orient;
// NOTE: Below I have 1 - cosTheta instead of 1 + cosTheta
// The reason is that I have relDir defined in the opposite direction
float magnitude = A * ( _dirWeight + (1.f - _dirWeight) * ( 1 - cosTheta ) * 0.5f );
// Assuming stationary wall - elliptical term goes to distance
magnitude *= expf( -dist / B );
// Force direction is just relative direction (for stationary wall)
force += magnitude * relDir;
}
// assume unit mass!
_velNew = _vel + force * Simulator::TIME_STEP;
}
void PortalPath::setPreferredDirection( const Agents::BaseAgent * agent, float headingCos, Agents::PrefVelocity & pVel ) {
const size_t PORTAL_COUNT = _route->getPortalCount();
Vector2 dir;
if ( _currPortal >= PORTAL_COUNT ) {
// assume that the path is clear
// TODO: See GoalVC
_goal->setDirections( agent->_pos, agent->_radius, pVel );
// speed
Vector2 goalPoint = pVel.getTarget();
Vector2 disp = goalPoint - agent->_pos;
const float distSq = absSq( disp );
float speed = agent->_prefSpeed;
if ( distSq <= 0.0001f ) {
// I've basically arrived -- speed should be zero.
speed = 0.f;
} else {
const float speedSq = speed * speed;
const float TS_SQD = SIM_TIME_STEP * SIM_TIME_STEP;
if ( distSq / speedSq < TS_SQD ) {
// The distance is less than I would travel in a single time step.
speed = sqrtf( distSq ) / SIM_TIME_STEP;
}
}
pVel.setSpeed( speed );
} else {
const WayPortal * portal = _route->getPortal( _currPortal );
Vector2 goalDir( _waypoints[ _currPortal ] - agent->_pos );
float dist = abs( goalDir );
// If the displacement to the next way point is large enough
// (i.e., not essentially zero), use it, otherwise, peek
// into the next waypoint.
//
// The goal is to always provide a goalDir to the portal
// that is well-defined and unit-length.
bool bigEnough = dist >= EPS;
if ( bigEnough ) {
goalDir /= dist;
if ( goalDir * _headings[ _currPortal ] < headingCos ) {
// Heading has deviated too far recompute crossing
FunnelPlanner planner;
planner.computeCrossing( agent->_radius, agent->_pos, this, _currPortal );
goalDir = _waypoints[ _currPortal ] - agent->_pos;
dist = abs( goalDir );
if ( bigEnough = ( dist >= EPS ) ) {
goalDir /= dist;
}
}
}
if ( ! bigEnough ) {
// simply cross the wayportal perpendicularly
//goalDir.set( portal->getCrossingDir( agent->_pos ) );
if ( _currPortal + 1 < getPortalCount() ) {
// calculate w.r.t. next waypoint
goalDir = norm( _waypoints[ _currPortal + 1 ] - agent->_pos );
} else {
// calculate w.r.t. goal
Vector2 gp;
_goal->getTargetPoint( gp, agent->_radius );
goalDir = norm( gp - agent->_pos );
}
}
assert( abs( goalDir ) > EPS && "Providing a goal direction that is too small" );
pVel.setTarget( _waypoints[ _currPortal ] );
portal->setPreferredDirection( agent->_pos, agent->_radius, goalDir, pVel );
}
}