From 6bd842f3f977d62d965bd375cffca01eddf263dd Mon Sep 17 00:00:00 2001 From: Jamie Snape Date: Sat, 24 Sep 2022 19:53:54 -0400 Subject: [PATCH] Reorder branches --- src/Agent.cc | 35 ++--- src/Agent.h | 1 - src/KdTree.cc | 325 ++++++++++++++++++++++---------------------- src/KdTree.h | 6 +- src/Obstacle.h | 1 - src/RVOSimulator.cc | 99 +++++++------- 6 files changed, 232 insertions(+), 235 deletions(-) diff --git a/src/Agent.cc b/src/Agent.cc index 96ef409..612c260 100644 --- a/src/Agent.cc +++ b/src/Agent.cc @@ -168,6 +168,7 @@ std::size_t linearProgram2(const std::vector &lines, float radius, if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) { result = tempResult; + return i; } } @@ -195,12 +196,13 @@ void linearProgram3(const std::vector &lines, std::size_t numObstLines, if (det(lines[i].direction, lines[i].point - result) > distance) { /* Result does not satisfy constraint of line i. */ std::vector projLines( - lines.begin(), lines.begin() + static_cast(numObstLines)); + lines.begin(), + lines.begin() + static_cast(numObstLines)); for (std::size_t j = numObstLines; j < i; ++j) { Line line; - float determinant = det(lines[i].direction, lines[j].direction); + const float determinant = det(lines[i].direction, lines[j].direction); if (std::fabs(determinant) <= RVO_EPSILON) { /* Line i and line j are parallel. */ @@ -256,13 +258,12 @@ Agent::~Agent() {} void Agent::computeNeighbors() { obstacleNeighbors_.clear(); const float range = timeHorizonObst_ * maxSpeed_ + radius_; - float rangeSq = range * range; - simulator_->kdTree_->computeObstacleNeighbors(this, rangeSq); + simulator_->kdTree_->computeObstacleNeighbors(this, range * range); agentNeighbors_.clear(); if (maxNeighbors_ > 0U) { - rangeSq = neighborDist_ * neighborDist_; + float rangeSq = neighborDist_ * neighborDist_; simulator_->kdTree_->computeAgentNeighbors(this, rangeSq); } } @@ -451,14 +452,15 @@ void Agent::computeNewVelocity() { invTimeHorizonObst * (obstacle1->point_ - position_); const Vector2 rightCutoff = invTimeHorizonObst * (obstacle2->point_ - position_); - const Vector2 cutoffVec = rightCutoff - leftCutoff; + const Vector2 cutoffVector = 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 t = + obstacle1 == obstacle2 + ? 0.5F + : (velocity_ - leftCutoff) * cutoffVector / absSq(cutoffVector); const float tLeft = (velocity_ - leftCutoff) * leftLegDirection; const float tRight = (velocity_ - rightCutoff) * rightLegDirection; @@ -488,7 +490,7 @@ void Agent::computeNewVelocity() { const float distSqCutoff = (t < 0.0F || t > 1.0F || obstacle1 == obstacle2) ? std::numeric_limits::infinity() - : absSq(velocity_ - (leftCutoff + t * cutoffVec)); + : absSq(velocity_ - (leftCutoff + t * cutoffVector)); const float distSqLeft = tLeft < 0.0F ? std::numeric_limits::infinity() @@ -557,10 +559,10 @@ void Agent::computeNewVelocity() { /* Vector from cutoff center to relative velocity. */ const float wLengthSq = absSq(w); - const float dotProduct1 = w * relativePosition; + const float dotProduct = w * relativePosition; - if (dotProduct1 < 0.0F && - dotProduct1 * dotProduct1 > combinedRadiusSq * wLengthSq) { + if (dotProduct < 0.0F && + dotProduct * dotProduct > combinedRadiusSq * wLengthSq) { /* Project on cut-off circle. */ const float wLength = std::sqrt(wLengthSq); const Vector2 unitW = w / wLength; @@ -587,9 +589,8 @@ void Agent::computeNewVelocity() { distSq; } - const float dotProduct2 = relativeVelocity * line.direction; - - u = dotProduct2 * line.direction - relativeVelocity; + u = (relativeVelocity * line.direction) * line.direction - + relativeVelocity; } } else { /* Collision. Project on cut-off circle of time timeStep. */ @@ -609,7 +610,7 @@ void Agent::computeNewVelocity() { orcaLines_.push_back(line); } - std::size_t lineFail = + const std::size_t lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocity_); if (lineFail < orcaLines_.size()) { diff --git a/src/Agent.h b/src/Agent.h index 1d99053..b6c88dc 100644 --- a/src/Agent.h +++ b/src/Agent.h @@ -120,7 +120,6 @@ class Agent { float radius_; float timeHorizon_; float timeHorizonObst_; - char padding_[4]; friend class KdTree; friend class RVOSimulator; diff --git a/src/KdTree.cc b/src/KdTree.cc index 30ffc8c..aacd16f 100644 --- a/src/KdTree.cc +++ b/src/KdTree.cc @@ -49,7 +49,8 @@ namespace RVO { namespace { /** - * @brief Maximum k-D tree node leaf size. + @ @relates KdTree + * @brief The maximum k-D tree node leaf size. */ const std::size_t RVO_MAX_LEAF_SIZE = 10U; } /* namespace */ @@ -253,131 +254,126 @@ void KdTree::buildObstacleTree() { KdTree::ObstacleTreeNode *KdTree::buildObstacleTreeRecursive( const std::vector &obstacles) { - if (obstacles.empty()) { - return NULL; - } - - ObstacleTreeNode *const node = new ObstacleTreeNode(); - - std::size_t optimalSplit = 0U; - std::size_t minLeft = obstacles.size(); - std::size_t minRight = obstacles.size(); - - for (std::size_t i = 0U; i < obstacles.size(); ++i) { - std::size_t leftSize = 0U; - std::size_t rightSize = 0U; - - const Obstacle *const obstacleI1 = obstacles[i]; - const Obstacle *const obstacleI2 = obstacleI1->next_; - - /* Compute optimal split node. */ - for (std::size_t j = 0U; j < obstacles.size(); ++j) { - if (i == j) { - continue; - } - - const Obstacle *const obstacleJ1 = obstacles[j]; - const Obstacle *const obstacleJ2 = obstacleJ1->next_; - - const float j1LeftOfI = - leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_); - const float j2LeftOfI = - leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_); - - if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { - ++leftSize; - } else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { - ++rightSize; - } else { - ++leftSize; - ++rightSize; + if (!obstacles.empty()) { + ObstacleTreeNode *const node = new ObstacleTreeNode(); + + std::size_t optimalSplit = 0U; + std::size_t minLeft = obstacles.size(); + std::size_t minRight = obstacles.size(); + + for (std::size_t i = 0U; i < obstacles.size(); ++i) { + std::size_t leftSize = 0U; + std::size_t rightSize = 0U; + + const Obstacle *const obstacleI1 = obstacles[i]; + const Obstacle *const obstacleI2 = obstacleI1->next_; + + /* Compute optimal split node. */ + for (std::size_t j = 0U; j < obstacles.size(); ++j) { + if (i != j) { + const Obstacle *const obstacleJ1 = obstacles[j]; + const Obstacle *const obstacleJ2 = obstacleJ1->next_; + + const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, + obstacleJ1->point_); + const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, + obstacleJ2->point_); + + if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { + ++leftSize; + } else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { + ++rightSize; + } else { + ++leftSize; + ++rightSize; + } + + if (std::make_pair(std::max(leftSize, rightSize), + std::min(leftSize, rightSize)) >= + std::make_pair(std::max(minLeft, minRight), + std::min(minLeft, minRight))) { + break; + } + } } if (std::make_pair(std::max(leftSize, rightSize), - std::min(leftSize, rightSize)) >= + std::min(leftSize, rightSize)) < std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) { - break; + minLeft = leftSize; + minRight = rightSize; + optimalSplit = i; } } - if (std::make_pair(std::max(leftSize, rightSize), - std::min(leftSize, rightSize)) < - std::make_pair(std::max(minLeft, minRight), - std::min(minLeft, minRight))) { - minLeft = leftSize; - minRight = rightSize; - optimalSplit = i; - } - } - - /* Build split node. */ - std::vector leftObstacles(minLeft); - std::vector rightObstacles(minRight); + /* Build split node. */ + std::vector leftObstacles(minLeft); + std::vector rightObstacles(minRight); - std::size_t leftCounter = 0U; - std::size_t rightCounter = 0U; - const std::size_t i = optimalSplit; + std::size_t leftCounter = 0U; + std::size_t rightCounter = 0U; + const std::size_t i = optimalSplit; - const Obstacle *const obstacleI1 = obstacles[i]; - const Obstacle *const obstacleI2 = obstacleI1->next_; - - for (std::size_t j = 0U; j < obstacles.size(); ++j) { - if (i == j) { - continue; - } + const Obstacle *const obstacleI1 = obstacles[i]; + const Obstacle *const obstacleI2 = obstacleI1->next_; - Obstacle *const obstacleJ1 = obstacles[j]; - Obstacle *const obstacleJ2 = obstacleJ1->next_; - - const float j1LeftOfI = - leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_); - const float j2LeftOfI = - leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_); - - if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { - leftObstacles[leftCounter++] = obstacles[j]; - } else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { - rightObstacles[rightCounter++] = obstacles[j]; - } else { - /* Split obstacle j. */ - const float t = det(obstacleI2->point_ - obstacleI1->point_, - obstacleJ1->point_ - obstacleI1->point_) / - det(obstacleI2->point_ - obstacleI1->point_, - obstacleJ1->point_ - obstacleJ2->point_); - - const Vector2 splitpoint = - obstacleJ1->point_ + t * (obstacleJ2->point_ - obstacleJ1->point_); - - Obstacle *const newObstacle = new Obstacle(); - newObstacle->point_ = splitpoint; - newObstacle->previous_ = obstacleJ1; - newObstacle->next_ = obstacleJ2; - newObstacle->isConvex_ = true; - newObstacle->direction_ = obstacleJ1->direction_; - - newObstacle->id_ = simulator_->obstacles_.size(); - - simulator_->obstacles_.push_back(newObstacle); - - obstacleJ1->next_ = newObstacle; - obstacleJ2->previous_ = newObstacle; - - if (j1LeftOfI > 0.0F) { - leftObstacles[leftCounter++] = obstacleJ1; - rightObstacles[rightCounter++] = newObstacle; - } else { - rightObstacles[rightCounter++] = obstacleJ1; - leftObstacles[leftCounter++] = newObstacle; + for (std::size_t j = 0U; j < obstacles.size(); ++j) { + if (i != j) { + Obstacle *const obstacleJ1 = obstacles[j]; + Obstacle *const obstacleJ2 = obstacleJ1->next_; + + const float j1LeftOfI = + leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_); + const float j2LeftOfI = + leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_); + + if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { + leftObstacles[leftCounter++] = obstacles[j]; + } else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { + rightObstacles[rightCounter++] = obstacles[j]; + } else { + /* Split obstacle j. */ + const float t = det(obstacleI2->point_ - obstacleI1->point_, + obstacleJ1->point_ - obstacleI1->point_) / + det(obstacleI2->point_ - obstacleI1->point_, + obstacleJ1->point_ - obstacleJ2->point_); + + const Vector2 splitPoint = + obstacleJ1->point_ + + t * (obstacleJ2->point_ - obstacleJ1->point_); + + Obstacle *const newObstacle = new Obstacle(); + newObstacle->direction_ = obstacleJ1->direction_; + newObstacle->point_ = splitPoint; + newObstacle->next_ = obstacleJ2; + newObstacle->previous_ = obstacleJ1; + newObstacle->id_ = simulator_->obstacles_.size(); + newObstacle->isConvex_ = true; + simulator_->obstacles_.push_back(newObstacle); + + obstacleJ1->next_ = newObstacle; + obstacleJ2->previous_ = newObstacle; + + if (j1LeftOfI > 0.0F) { + leftObstacles[leftCounter++] = obstacleJ1; + rightObstacles[rightCounter++] = newObstacle; + } else { + rightObstacles[rightCounter++] = obstacleJ1; + leftObstacles[leftCounter++] = newObstacle; + } + } } } - } - node->obstacle = obstacleI1; - node->left = buildObstacleTreeRecursive(leftObstacles); - node->right = buildObstacleTreeRecursive(rightObstacles); + node->obstacle = obstacleI1; + node->left = buildObstacleTreeRecursive(leftObstacles); + node->right = buildObstacleTreeRecursive(rightObstacles); - return node; + return node; + } + + return NULL; } void KdTree::computeAgentNeighbors(Agent *agent, float &rangeSq) const { @@ -450,32 +446,30 @@ void KdTree::queryAgentTreeRecursive(Agent *agent, float &rangeSq, void KdTree::queryObstacleTreeRecursive(Agent *agent, float rangeSq, const ObstacleTreeNode *node) const { - if (node == NULL) { - return; - } + if (node != NULL) { + const Obstacle *const obstacle1 = node->obstacle; + const Obstacle *const obstacle2 = obstacle1->next_; - const Obstacle *const obstacle1 = node->obstacle; - const Obstacle *const obstacle2 = obstacle1->next_; + const float agentLeftOfLine = + leftOf(obstacle1->point_, obstacle2->point_, agent->position_); - const float agentLeftOfLine = - leftOf(obstacle1->point_, obstacle2->point_, agent->position_); + queryObstacleTreeRecursive( + agent, rangeSq, agentLeftOfLine >= 0.0F ? node->left : node->right); - queryObstacleTreeRecursive( - agent, rangeSq, agentLeftOfLine >= 0.0F ? node->left : node->right); + const float distSqLine = agentLeftOfLine * agentLeftOfLine / + absSq(obstacle2->point_ - obstacle1->point_); - const float distSqLine = agentLeftOfLine * 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); + } - 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); } - - /* Try other side of line. */ - queryObstacleTreeRecursive( - agent, rangeSq, agentLeftOfLine >= 0.0F ? node->right : node->left); } } @@ -487,45 +481,48 @@ bool KdTree::queryVisibility(const Vector2 &vector1, const Vector2 &vector2, bool KdTree::queryVisibilityRecursive(const Vector2 &vector1, const Vector2 &vector2, float radius, const ObstacleTreeNode *node) const { - if (node == NULL) { - return true; - } - - const Obstacle *const obstacle1 = node->obstacle; - const Obstacle *const obstacle2 = obstacle1->next_; + if (node != NULL) { + const Obstacle *const obstacle1 = node->obstacle; + const Obstacle *const obstacle2 = obstacle1->next_; + + const float q1LeftOfI = + leftOf(obstacle1->point_, obstacle2->point_, vector1); + const float q2LeftOfI = + leftOf(obstacle1->point_, obstacle2->point_, vector2); + const float invLengthI = + 1.0F / absSq(obstacle2->point_ - obstacle1->point_); + + if (q1LeftOfI >= 0.0F && q2LeftOfI >= 0.0F) { + return queryVisibilityRecursive(vector1, vector2, radius, node->left) && + ((q1LeftOfI * q1LeftOfI * invLengthI >= radius * radius && + q2LeftOfI * q2LeftOfI * invLengthI >= radius * radius) || + queryVisibilityRecursive(vector1, vector2, radius, node->right)); + } - const float q1LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, vector1); - const float q2LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, vector2); - const float invLengthI = 1.0F / absSq(obstacle2->point_ - obstacle1->point_); + if (q1LeftOfI <= 0.0F && q2LeftOfI <= 0.0F) { + return queryVisibilityRecursive(vector1, vector2, radius, node->right) && + ((q1LeftOfI * q1LeftOfI * invLengthI >= radius * radius && + q2LeftOfI * q2LeftOfI * invLengthI >= radius * radius) || + queryVisibilityRecursive(vector1, vector2, radius, node->left)); + } - if (q1LeftOfI >= 0.0F && q2LeftOfI >= 0.0F) { - return queryVisibilityRecursive(vector1, vector2, radius, node->left) && - ((q1LeftOfI * q1LeftOfI * invLengthI >= radius * radius && - q2LeftOfI * q2LeftOfI * invLengthI >= radius * radius) || - queryVisibilityRecursive(vector1, vector2, radius, node->right)); - } + if (q1LeftOfI >= 0.0F && q2LeftOfI <= 0.0F) { + /* One can see through obstacle from left to right. */ + return queryVisibilityRecursive(vector1, vector2, radius, node->left) && + queryVisibilityRecursive(vector1, vector2, radius, node->right); + } - if (q1LeftOfI <= 0.0F && q2LeftOfI <= 0.0F) { - return queryVisibilityRecursive(vector1, vector2, radius, node->right) && - ((q1LeftOfI * q1LeftOfI * invLengthI >= radius * radius && - q2LeftOfI * q2LeftOfI * invLengthI >= radius * radius) || - queryVisibilityRecursive(vector1, vector2, radius, node->left)); - } + const float point1LeftOfQ = leftOf(vector1, vector2, obstacle1->point_); + const float point2LeftOfQ = leftOf(vector1, vector2, obstacle2->point_); + const float invLengthQ = 1.0F / absSq(vector2 - vector1); - if (q1LeftOfI >= 0.0F && q2LeftOfI <= 0.0F) { - /* One can see through obstacle from left to right. */ - return queryVisibilityRecursive(vector1, vector2, radius, node->left) && + return point1LeftOfQ * point2LeftOfQ >= 0.0F && + point1LeftOfQ * point1LeftOfQ * invLengthQ > radius * radius && + point2LeftOfQ * point2LeftOfQ * invLengthQ > radius * radius && + queryVisibilityRecursive(vector1, vector2, radius, node->left) && queryVisibilityRecursive(vector1, vector2, radius, node->right); } - const float point1LeftOfQ = leftOf(vector1, vector2, obstacle1->point_); - const float point2LeftOfQ = leftOf(vector1, vector2, obstacle2->point_); - const float invLengthQ = 1.0F / absSq(vector2 - vector1); - - return point1LeftOfQ * point2LeftOfQ >= 0.0F && - point1LeftOfQ * point1LeftOfQ * invLengthQ > radius * radius && - point2LeftOfQ * point2LeftOfQ * invLengthQ > radius * radius && - queryVisibilityRecursive(vector1, vector2, radius, node->left) && - queryVisibilityRecursive(vector1, vector2, radius, node->right); + return true; } } /* namespace RVO */ diff --git a/src/KdTree.h b/src/KdTree.h index 33c3c5b..e50b289 100644 --- a/src/KdTree.h +++ b/src/KdTree.h @@ -89,7 +89,7 @@ class KdTree { /** * @brief Recursive function to build an obstacle k-D tree. * @param[in] obstacles List of obstacles from which to build the obstacle k-D - * tree. + * tree. */ ObstacleTreeNode *buildObstacleTreeRecursive( const std::vector &obstacles); @@ -156,9 +156,9 @@ class KdTree { /** * @brief Recursive function to query the visibility between two points - * within a specified radius. + * within a specified radius. * @param[in] vector1 The first point between which visibility is to be - * tested. + * tested. * @param[in] vector2 The second point between which visibility is to be * tested. * @param[in] radius The radius within which visibility is to be tested. diff --git a/src/Obstacle.h b/src/Obstacle.h index ba86819..e2008a7 100644 --- a/src/Obstacle.h +++ b/src/Obstacle.h @@ -71,7 +71,6 @@ class Obstacle { Obstacle *previous_; std::size_t id_; bool isConvex_; - char padding_[7]; friend class Agent; friend class KdTree; diff --git a/src/RVOSimulator.cc b/src/RVOSimulator.cc index 4302a76..703ace3 100644 --- a/src/RVOSimulator.cc +++ b/src/RVOSimulator.cc @@ -106,23 +106,23 @@ RVOSimulator::~RVOSimulator() { } std::size_t RVOSimulator::addAgent(const Vector2 &position) { - if (defaultAgent_ == NULL) { - return RVO_ERROR; + if (defaultAgent_ != NULL) { + Agent *const agent = new Agent(this); + agent->position_ = position; + agent->velocity_ = defaultAgent_->velocity_; + agent->id_ = agents_.size(); + agent->maxNeighbors_ = defaultAgent_->maxNeighbors_; + agent->maxSpeed_ = defaultAgent_->maxSpeed_; + agent->neighborDist_ = defaultAgent_->neighborDist_; + agent->radius_ = defaultAgent_->radius_; + agent->timeHorizon_ = defaultAgent_->timeHorizon_; + agent->timeHorizonObst_ = defaultAgent_->timeHorizonObst_; + agents_.push_back(agent); + + return agents_.size() - 1U; } - Agent *agent = new Agent(this); - agent->position_ = position; - agent->velocity_ = defaultAgent_->velocity_; - agent->id_ = agents_.size(); - agent->maxNeighbors_ = defaultAgent_->maxNeighbors_; - agent->maxSpeed_ = defaultAgent_->maxSpeed_; - agent->neighborDist_ = defaultAgent_->neighborDist_; - agent->radius_ = defaultAgent_->radius_; - agent->timeHorizon_ = defaultAgent_->timeHorizon_; - agent->timeHorizonObst_ = defaultAgent_->timeHorizonObst_; - agents_.push_back(agent); - - return agents_.size() - 1U; + return RVO_ERROR; } std::size_t RVOSimulator::addAgent(const Vector2 &position, float neighborDist, @@ -137,7 +137,7 @@ std::size_t RVOSimulator::addAgent(const Vector2 &position, float neighborDist, std::size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity) { - Agent *agent = new Agent(this); + Agent *const agent = new Agent(this); agent->position_ = position; agent->velocity_ = velocity; agent->id_ = agents_.size(); @@ -153,43 +153,44 @@ std::size_t RVOSimulator::addAgent(const Vector2 &position, float neighborDist, } std::size_t RVOSimulator::addObstacle(const std::vector &vertices) { - if (vertices.size() < 2U) { - return RVO_ERROR; - } - - const std::size_t obstacleNo = obstacles_.size(); - - for (std::size_t i = 0U; i < vertices.size(); ++i) { - Obstacle *obstacle = new Obstacle(); - obstacle->point_ = vertices[i]; - - if (i != 0U) { - obstacle->previous_ = obstacles_.back(); - obstacle->previous_->next_ = obstacle; - } - - if (i == vertices.size() - 1U) { - obstacle->next_ = obstacles_[obstacleNo]; - obstacle->next_->previous_ = obstacle; + if (vertices.size() > 1U) { + const std::size_t obstacleNo = obstacles_.size(); + + for (std::size_t i = 0U; i < vertices.size(); ++i) { + Obstacle *const obstacle = new Obstacle(); + obstacle->point_ = vertices[i]; + + if (i != 0U) { + obstacle->previous_ = obstacles_.back(); + obstacle->previous_->next_ = obstacle; + } + + if (i == vertices.size() - 1U) { + obstacle->next_ = obstacles_[obstacleNo]; + obstacle->next_->previous_ = obstacle; + } + + obstacle->direction_ = normalize( + vertices[(i == vertices.size() - 1U ? 0U : i + 1U)] - vertices[i]); + + if (vertices.size() == 2U) { + obstacle->isConvex_ = true; + } else { + obstacle->isConvex_ = + leftOf(vertices[i == 0U ? vertices.size() - 1U : i - 1U], + vertices[i], + vertices[i == vertices.size() - 1U ? 0U : i + 1U]) >= 0.0F; + } + + obstacle->id_ = obstacles_.size(); + + obstacles_.push_back(obstacle); } - obstacle->direction_ = normalize( - vertices[(i == vertices.size() - 1U ? 0U : i + 1U)] - vertices[i]); - - if (vertices.size() == 2U) { - obstacle->isConvex_ = true; - } else { - obstacle->isConvex_ = - leftOf(vertices[i == 0U ? vertices.size() - 1U : i - 1U], vertices[i], - vertices[i == vertices.size() - 1U ? 0U : i + 1U]) >= 0.0F; - } - - obstacle->id_ = obstacles_.size(); - - obstacles_.push_back(obstacle); + return obstacleNo; } - return obstacleNo; + return RVO_ERROR; } void RVOSimulator::doStep() {