Merge pull request #78099 from DeeJayLSP/rvo2023

Update RVO2 to git 2022.09

COPYRIGHT.txt

@@ -425,7 +425,7 @@ License: Zlib
 
 Files: ./thirdparty/rvo2/
 Comment: RVO2
-Copyright: 2016, University of North Carolina at Chapel Hill
+Copyright: 2008, University of North Carolina at Chapel Hill
 License: Apache-2.0
 
 Files: ./thirdparty/spirv-reflect/

modules/navigation/SCsub

@@ -37,10 +37,12 @@ if env["builtin_recastnavigation"]:
 if env["builtin_rvo2_2d"]:
     thirdparty_dir = "#thirdparty/rvo2/rvo2_2d/"
     thirdparty_sources = [
-        "Agent2d.cpp",
-        "Obstacle2d.cpp",
-        "KdTree2d.cpp",
-        "RVOSimulator2d.cpp",
+        "Agent2d.cc",
+        "Obstacle2d.cc",
+        "KdTree2d.cc",
+        "Line.cc",
+        "RVOSimulator2d.cc",
+        "Vector2.cc",
     ]
     thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources]
 
@@ -54,9 +56,11 @@ if env["builtin_rvo2_2d"]:
 if env["builtin_rvo2_3d"]:
     thirdparty_dir = "#thirdparty/rvo2/rvo2_3d/"
     thirdparty_sources = [
-        "Agent3d.cpp",
-        "KdTree3d.cpp",
-        "RVOSimulator3d.cpp",
+        "Agent3d.cc",
+        "KdTree3d.cc",
+        "Plane.cc",
+        "RVOSimulator3d.cc",
+        "Vector3.cc",
     ]
     thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources]
 

modules/navigation/nav_map.cpp

@@ -1022,16 +1022,16 @@ void NavMap::_update_rvo_obstacles_tree_2d() {
 			rvo_2d_obstacle->avoidance_layers_ = _obstacle_avoidance_layers;
 
 			if (i != 0) {
-				rvo_2d_obstacle->prevObstacle_ = raw_obstacles.back();
-				rvo_2d_obstacle->prevObstacle_->nextObstacle_ = rvo_2d_obstacle;
+				rvo_2d_obstacle->previous_ = raw_obstacles.back();
+				rvo_2d_obstacle->previous_->next_ = rvo_2d_obstacle;
 			}
 
 			if (i == rvo_2d_vertices.size() - 1) {
-				rvo_2d_obstacle->nextObstacle_ = raw_obstacles[obstacleNo];
-				rvo_2d_obstacle->nextObstacle_->prevObstacle_ = rvo_2d_obstacle;
+				rvo_2d_obstacle->next_ = raw_obstacles[obstacleNo];
+				rvo_2d_obstacle->next_->previous_ = rvo_2d_obstacle;
 			}
 
-			rvo_2d_obstacle->unitDir_ = normalize(rvo_2d_vertices[(i == rvo_2d_vertices.size() - 1 ? 0 : i + 1)] - rvo_2d_vertices[i]);
+			rvo_2d_obstacle->direction_ = normalize(rvo_2d_vertices[(i == rvo_2d_vertices.size() - 1 ? 0 : i + 1)] - rvo_2d_vertices[i]);
 
 			if (rvo_2d_vertices.size() == 2) {
 				rvo_2d_obstacle->isConvex_ = true;
@@ -1079,9 +1079,9 @@ void NavMap::_update_rvo_simulation() {
 }
 
 void NavMap::compute_single_avoidance_step_2d(uint32_t index, NavAgent **agent) {
-	(*(agent + index))->get_rvo_agent_2d()->computeNeighbors(&rvo_simulation_2d);
-	(*(agent + index))->get_rvo_agent_2d()->computeNewVelocity(&rvo_simulation_2d);
-	(*(agent + index))->get_rvo_agent_2d()->update(&rvo_simulation_2d);
+	(*(agent + index))->get_rvo_agent_2d()->computeNeighbors(rvo_simulation_2d.kdTree_);
+	(*(agent + index))->get_rvo_agent_2d()->computeNewVelocity(rvo_simulation_2d.timeStep_);
+	(*(agent + index))->get_rvo_agent_2d()->update(rvo_simulation_2d.timeStep_);
 	(*(agent + index))->update();
 }
 
@@ -1104,9 +1104,9 @@ void NavMap::step(real_t p_deltatime) {
 			WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
 		} else {
 			for (NavAgent *agent : active_2d_avoidance_agents) {
-				agent->get_rvo_agent_2d()->computeNeighbors(&rvo_simulation_2d);
-				agent->get_rvo_agent_2d()->computeNewVelocity(&rvo_simulation_2d);
-				agent->get_rvo_agent_2d()->update(&rvo_simulation_2d);
+				agent->get_rvo_agent_2d()->computeNeighbors(rvo_simulation_2d.kdTree_);
+				agent->get_rvo_agent_2d()->computeNewVelocity(rvo_simulation_2d.timeStep_);
+				agent->get_rvo_agent_2d()->update(rvo_simulation_2d.timeStep_);
 				agent->update();
 			}
 		}

thirdparty/README.md

@@ -639,13 +639,13 @@ Files extracted from upstream source:
 For 2D in `rvo2_2d` folder
 
 - Upstream: https://github.com/snape/RVO2
-- Version: git (f7c5380235f6c9ac8d19cbf71fc94e2d4758b0a3, 2021)
+- Version: git (5961f05ed310f3a5e902aa70ad54e010ba6dcdfd, 2022)
 - License: Apache 2.0
 
 For 3D in `rvo2_3d` folder
 
 - Upstream: https://github.com/snape/RVO2-3D
-- Version: git (bfc048670a4e85066e86a1f923d8ea92e3add3b2, 2021)
+- Version: git (8be355eb84dc763267b5acf7070d6d623d752e51, 2022)
 - License: Apache 2.0
 
 Files extracted from upstream source:

thirdparty/rvo2/rvo2_2d/Agent2d.cc

@@ -0,0 +1,693 @@
+/*
+ * Agent2d.cpp
+ * RVO2 Library
+ *
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     https://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Please send all bug reports to <[email protected]>.
+ *
+ * The authors may be contacted via:
+ *
+ * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
+ * Dept. of Computer Science
+ * 201 S. Columbia St.
+ * Frederick P. Brooks, Jr. Computer Science Bldg.
+ * Chapel Hill, N.C. 27599-3175
+ * United States of America
+ *
+ * <https://gamma.cs.unc.edu/RVO2/>
+ */
+
+/**
+ * @file  Agent2d.cpp
+ * @brief Defines the Agent2D class.
+ */
+
+#include "Agent2d.h"
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+
+#include "KdTree2d.h"
+#include "Obstacle2d.h"
+
+namespace RVO2D {
+namespace {
+/**
+ * @relates        Agent2D
+ * @brief          Solves a one-dimensional linear program on a specified line
+ *                 subject to linear constraints defined by lines and a circular
+ *                 constraint.
+ * @param[in]      lines        Lines defining the linear constraints.
+ * @param[in]      lineNo       The specified line constraint.
+ * @param[in]      radius       The radius of the circular constraint.
+ * @param[in]      optVelocity  The optimization velocity.
+ * @param[in]      directionOpt True if the direction should be optimized.
+ * @param[in, out] result       A reference to the result of the linear program.
+ * @return         True if successful.
+ */
+bool linearProgram1(const std::vector<Line> &lines, std::size_t lineNo,
+                    float radius, const Vector2 &optVelocity, bool directionOpt,
+                    Vector2 &result) { /* NOLINT(runtime/references) */
+  const float dotProduct = lines[lineNo].point * lines[lineNo].direction;
+  const float discriminant =
+      dotProduct * dotProduct + radius * 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 (std::size_t i = 0U; 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) <= RVO2D_EPSILON) {
+      /* Lines lineNo and i are (almost) parallel. */
+      if (numerator < 0.0F) {
+        return false;
+      }
+
+      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;
+}
+
+/**
+ * @relates        Agent2D
+ * @brief          Solves a two-dimensional linear program subject to linear
+ *                 constraints defined by lines and a circular constraint.
+ * @param[in]      lines        Lines defining the linear constraints.
+ * @param[in]      radius       The radius of the circular constraint.
+ * @param[in]      optVelocity  The optimization velocity.
+ * @param[in]      directionOpt True if the direction should be optimized.
+ * @param[in, out] result       A reference to the result of the linear program.
+ * @return         The number of the line it fails on, and the number of lines
+ *                 if successful.
+ */
+std::size_t linearProgram2(const std::vector<Line> &lines, float radius,
+                           const Vector2 &optVelocity, bool directionOpt,
+                           Vector2 &result) { /* NOLINT(runtime/references) */
+  if (directionOpt) {
+    /* Optimize direction. Note that the optimization velocity is of unit length
+     * in this case.
+     */
+    result = optVelocity * radius;
+  } else if (absSq(optVelocity) > radius * radius) {
+    /* Optimize closest point and outside circle. */
+    result = normalize(optVelocity) * radius;
+  } else {
+    /* Optimize closest point and inside circle. */
+    result = optVelocity;
+  }
+
+  for (std::size_t i = 0U; i < lines.size(); ++i) {
+    if (det(lines[i].direction, lines[i].point - result) > 0.0F) {
+      /* Result does not satisfy constraint i. Compute new optimal result. */
+      const Vector2 tempResult = result;
+
+      if (!linearProgram1(lines, i, radius, optVelocity, directionOpt,
+                          result)) {
+        result = tempResult;
+
+        return i;
+      }
+    }
+  }
+
+  return lines.size();
+}
+
+/**
+ * @relates        Agent2D
+ * @brief          Solves a two-dimensional linear program subject to linear
+ *                 constraints defined by lines and a circular constraint.
+ * @param[in]      lines        Lines defining the linear constraints.
+ * @param[in]      numObstLines Count of obstacle lines.
+ * @param[in]      beginLine    The line on which the 2-d linear program failed.
+ * @param[in]      radius       The radius of the circular constraint.
+ * @param[in, out] result       A reference to the result of the linear program.
+ */
+void linearProgram3(const std::vector<Line> &lines, std::size_t numObstLines,
+                    std::size_t beginLine, float radius,
+                    Vector2 &result) { /* NOLINT(runtime/references) */
+  float distance = 0.0F;
+
+  for (std::size_t i = beginLine; i < lines.size(); ++i) {
+    if (det(lines[i].direction, lines[i].point - result) > distance) {
+      /* Result does not satisfy constraint of line i. */
+      std::vector<Line> projLines(
+          lines.begin(),
+          lines.begin() + static_cast<std::ptrdiff_t>(numObstLines));
+
+      for (std::size_t j = numObstLines; j < i; ++j) {
+        Line line;
+
+        const float determinant = det(lines[i].direction, lines[j].direction);
+
+        if (std::fabs(determinant) <= RVO2D_EPSILON) {
+          /* Line i and line j are parallel. */
+          if (lines[i].direction * lines[j].direction > 0.0F) {
+            /* Line i and line j point in the same direction. */
+            continue;
+          }
+
+          /* Line i and line j point in opposite direction. */
+          line.point = 0.5F * (lines[i].point + lines[j].point);
+        } else {
+          line.point = lines[i].point + (det(lines[j].direction,
+                                             lines[i].point - lines[j].point) /
+                                         determinant) *
+                                            lines[i].direction;
+        }
+
+        line.direction = normalize(lines[j].direction - lines[i].direction);
+        projLines.push_back(line);
+      }
+
+      const Vector2 tempResult = result;
+
+      if (linearProgram2(
+              projLines, radius,
+              Vector2(-lines[i].direction.y(), lines[i].direction.x()), true,
+              result) < projLines.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 = det(lines[i].direction, lines[i].point - result);
+    }
+  }
+}
+} /* namespace */
+
+Agent2D::Agent2D()
+    : id_(0U),
+      maxNeighbors_(0U),
+      maxSpeed_(0.0F),
+      neighborDist_(0.0F),
+      radius_(0.0F),
+      timeHorizon_(0.0F),
+      timeHorizonObst_(0.0F) {}
+
+Agent2D::~Agent2D() {}
+
+void Agent2D::computeNeighbors(const KdTree2D *kdTree) {
+  obstacleNeighbors_.clear();
+  const float range = timeHorizonObst_ * maxSpeed_ + radius_;
+  kdTree->computeObstacleNeighbors(this, range * range);
+
+  agentNeighbors_.clear();
+
+  if (maxNeighbors_ > 0U) {
+    float rangeSq = neighborDist_ * neighborDist_;
+    kdTree->computeAgentNeighbors(this, rangeSq);
+  }
+}
+
+/* Search for the best new velocity. */
+void Agent2D::computeNewVelocity(float timeStep) {
+  orcaLines_.clear();
+
+  const float invTimeHorizonObst = 1.0F / timeHorizonObst_;
+
+  /* Create obstacle ORCA lines. */
+  for (std::size_t i = 0U; i < obstacleNeighbors_.size(); ++i) {
+    const Obstacle2D *obstacle1 = obstacleNeighbors_[i].second;
+    const Obstacle2D *obstacle2 = obstacle1->next_;
+
+    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 (std::size_t j = 0U; j < orcaLines_.size(); ++j) {
+      if (det(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point,
+              orcaLines_[j].direction) -
+                  invTimeHorizonObst * radius_ >=
+              -RVO2D_EPSILON &&
+          det(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point,
+              orcaLines_[j].direction) -
+                  invTimeHorizonObst * radius_ >=
+              -RVO2D_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 = radius_ * 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.0F && distSq1 <= radiusSq) {
+      /* Collision with left vertex. Ignore if non-convex. */
+      if (obstacle1->isConvex_) {
+        line.point = Vector2(0.0F, 0.0F);
+        line.direction =
+            normalize(Vector2(-relativePosition1.y(), relativePosition1.x()));
+        orcaLines_.push_back(line);
+      }
+
+      continue;
+    }
+
+    if (s > 1.0F && 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->direction_) >= 0.0F) {
+        line.point = Vector2(0.0F, 0.0F);
+        line.direction =
+            normalize(Vector2(-relativePosition2.y(), relativePosition2.x()));
+        orcaLines_.push_back(line);
+      }
+
+      continue;
+    }
+
+    if (s >= 0.0F && s <= 1.0F && distSqLine <= radiusSq) {
+      /* Collision with obstacle segment. */
+      line.point = Vector2(0.0F, 0.0F);
+      line.direction = -obstacle1->direction_;
+      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;
+    Vector2 rightLegDirection;
+
+    if (s < 0.0F && distSqLine <= radiusSq) {
+      /* Obstacle2D 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.0F && distSqLine <= radiusSq) {
+      /* Obstacle2D 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->direction_;
+      }
+
+      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->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. */
+    const Obstacle2D *const leftNeighbor = obstacle1->previous_;
+
+    bool isLeftLegForeign = false;
+    bool isRightLegForeign = false;
+
+    if (obstacle1->isConvex_ &&
+        det(leftLegDirection, -leftNeighbor->direction_) >= 0.0F) {
+      /* Left leg points into obstacle. */
+      leftLegDirection = -leftNeighbor->direction_;
+      isLeftLegForeign = true;
+    }
+
+    if (obstacle2->isConvex_ &&
+        det(rightLegDirection, obstacle2->direction_) <= 0.0F) {
+      /* Right leg points into obstacle. */
+      rightLegDirection = obstacle2->direction_;
+      isRightLegForeign = true;
+    }
+
+    /* Compute cut-off centers. */
+    const Vector2 leftCutoff =
+        invTimeHorizonObst * (obstacle1->point_ - position_);
+    const Vector2 rightCutoff =
+        invTimeHorizonObst * (obstacle2->point_ - position_);
+    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) * cutoffVector / absSq(cutoffVector);
+    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;
+    }
+
+    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 * cutoffVector));
+    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->direction_;
+      line.point =
+          leftCutoff + radius_ * invTimeHorizonObst *
+                           Vector2(-line.direction.y(), line.direction.x());
+      orcaLines_.push_back(line);
+      continue;
+    }
+
+    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;
+    }
+
+    /* 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);
+  }
+
+  const std::size_t numObstLines = orcaLines_.size();
+
+  const float invTimeHorizon = 1.0F / timeHorizon_;
+
+  /* Create agent ORCA lines. */
+  for (std::size_t i = 0U; i < agentNeighbors_.size(); ++i) {
+    const Agent2D *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 = combinedRadius * 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 dotProduct = w * relativePosition;
+
+      if (dotProduct < 0.0F &&
+          dotProduct * dotProduct > 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;
+        }
+
+        u = (relativeVelocity * line.direction) * line.direction -
+            relativeVelocity;
+      }
+    } else {
+      /* Collision. Project on cut-off circle of time timeStep. */
+      const float invTimeStep = 1.0F / 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);
+  }
+
+  const std::size_t lineFail =
+      linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocity_);
+
+  if (lineFail < orcaLines_.size()) {
+    linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, newVelocity_);
+  }
+}
+
+void Agent2D::insertAgentNeighbor(const Agent2D *agent, float &rangeSq) {
+	// no point processing same agent
+	if (this == agent) {
+		return;
+	}
+	// ignore other agent if layers/mask bitmasks have no matching bit
+	if ((avoidance_mask_ & agent->avoidance_layers_) == 0) {
+		return;
+	}
+	// ignore other agent if this agent is below or above
+	if ((elevation_ > agent->elevation_ + agent->height_) || (elevation_ + height_ < agent->elevation_)) {
+		return;
+	}
+
+	if (avoidance_priority_ > agent->avoidance_priority_) {
+		return;
+	}
+  const float distSq = absSq(position_ - agent->position_);
+
+  if (distSq < rangeSq) {
+    if (agentNeighbors_.size() < maxNeighbors_) {
+      agentNeighbors_.push_back(std::make_pair(distSq, agent));
+    }
+
+    std::size_t i = agentNeighbors_.size() - 1U;
+
+    while (i != 0U && distSq < agentNeighbors_[i - 1U].first) {
+      agentNeighbors_[i] = agentNeighbors_[i - 1U];
+      --i;
+    }
+
+    agentNeighbors_[i] = std::make_pair(distSq, agent);
+
+    if (agentNeighbors_.size() == maxNeighbors_) {
+      rangeSq = agentNeighbors_.back().first;
+    }
+  }
+}
+
+void Agent2D::insertObstacleNeighbor(const Obstacle2D *obstacle, float rangeSq) {
+  const Obstacle2D *const nextObstacle = obstacle->next_;
+
+  float distSq = 0.0F;
+  const float r = ((position_ - obstacle->point_) *
+                   (nextObstacle->point_ - obstacle->point_)) /
+                  absSq(nextObstacle->point_ - obstacle->point_);
+
+  if (r < 0.0F) {
+    distSq = absSq(position_ - obstacle->point_);
+  } else if (r > 1.0F) {
+    distSq = absSq(position_ - nextObstacle->point_);
+  } else {
+    distSq = absSq(position_ - (obstacle->point_ +
+                                r * (nextObstacle->point_ - obstacle->point_)));
+  }
+
+  if (distSq < rangeSq) {
+    obstacleNeighbors_.push_back(std::make_pair(distSq, obstacle));
+
+    std::size_t i = obstacleNeighbors_.size() - 1U;
+
+    while (i != 0U && distSq < obstacleNeighbors_[i - 1U].first) {
+      obstacleNeighbors_[i] = obstacleNeighbors_[i - 1U];
+      --i;
+    }
+
+    obstacleNeighbors_[i] = std::make_pair(distSq, obstacle);
+  }
+}
+
+void Agent2D::update(float timeStep) {
+  velocity_ = newVelocity_;
+  position_ += velocity_ * timeStep;
+}
+} /* namespace RVO2D */

thirdparty/rvo2/rvo2_2d/Agent2d.cpp

@@ -1,594 +0,0 @@
-/*
- * Agent2d.cpp
- * RVO2 Library
- *
- * Copyright 2008 University of North Carolina at Chapel Hill
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
- * Please send all bug reports to <[email protected]>.
- *
- * The authors may be contacted via:
- *
- * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
- * Dept. of Computer Science
- * 201 S. Columbia St.
- * Frederick P. Brooks, Jr. Computer Science Bldg.
- * Chapel Hill, N.C. 27599-3175
- * United States of America
- *
- * <http://gamma.cs.unc.edu/RVO2/>
- */
-
-#include "Agent2d.h"
-
-#include "KdTree2d.h"
-#include "Obstacle2d.h"
-
-namespace RVO2D {
-	Agent2D::Agent2D() : maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f), timeHorizonObst_(0.0f), id_(0) { }
-
-	void Agent2D::computeNeighbors(RVOSimulator2D *sim_)
-	{
-		obstacleNeighbors_.clear();
-		float rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_);
-		sim_->kdTree_->computeObstacleNeighbors(this, rangeSq);
-
-		agentNeighbors_.clear();
-
-		if (maxNeighbors_ > 0) {
-			rangeSq = sqr(neighborDist_);
-			sim_->kdTree_->computeAgentNeighbors(this, rangeSq);
-		}
-	}
-
-	/* Search for the best new velocity. */
-	void Agent2D::computeNewVelocity(RVOSimulator2D *sim_)
-	{
-		orcaLines_.clear();
-
-		const float invTimeHorizonObst = 1.0f / timeHorizonObst_;
-
-		/* Create obstacle ORCA lines. */
-		for (size_t i = 0; i < obstacleNeighbors_.size(); ++i) {
-
-			const Obstacle2D *obstacle1 = obstacleNeighbors_[i].second;
-			const Obstacle2D *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.0f && distSq1 <= radiusSq) {
-				/* Collision with left vertex. Ignore if non-convex. */
-				if (obstacle1->isConvex_) {
-					line.point = Vector2(0.0f, 0.0f);
-					line.direction = normalize(Vector2(-relativePosition1.y(), relativePosition1.x()));
-					orcaLines_.push_back(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 neighoring obstace */
-				if (obstacle2->isConvex_ && det(relativePosition2, obstacle2->unitDir_) >= 0.0f) {
-					line.point = Vector2(0.0f, 0.0f);
-					line.direction = normalize(Vector2(-relativePosition2.y(), relativePosition2.x()));
-					orcaLines_.push_back(line);
-				}
-
-				continue;
-			}
-			else if (s >= 0.0f && s < 1.0f && distSqLine <= radiusSq) {
-				/* Collision with obstacle segment. */
-				line.point = Vector2(0.0f, 0.0f);
-				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.0f && 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.0f && 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 Obstacle2D *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 Agent2D *const other = agentNeighbors_[i].second;
-
-			//const float timeHorizon_mod = (avoidance_priority_ - other->avoidance_priority_ + 1.0f) * 0.5f;
-			//const float invTimeHorizon = (1.0f / timeHorizon_) * timeHorizon_mod;
-
-			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_);
-		}
-	}
-
-	void Agent2D::insertAgentNeighbor(const Agent2D *agent, float &rangeSq)
-	{
-		// no point processing same agent
-		if (this == agent) {
-			return;
-		}
-		// ignore other agent if layers/mask bitmasks have no matching bit
-		if ((avoidance_mask_ & agent->avoidance_layers_) == 0) {
-			return;
-		}
-		// ignore other agent if this agent is below or above
-		if ((elevation_ > agent->elevation_ + agent->height_) || (elevation_ + height_ < agent->elevation_)) {
-			return;
-		}
-
-		if (avoidance_priority_ > agent->avoidance_priority_) {
-			return;
-		}
-
-		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 Agent2D::insertObstacleNeighbor(const Obstacle2D *obstacle, float rangeSq)
-	{
-		const Obstacle2D *const nextObstacle = obstacle->nextObstacle_;
-		
-		// ignore obstacle if no matching layer/mask
-		if ((avoidance_mask_ & nextObstacle->avoidance_layers_) == 0) {
-			return;
-		}
-		// ignore obstacle if below or above
-		if ((elevation_ > obstacle->elevation_ + obstacle->height_) || (elevation_ + height_ < obstacle->elevation_)) {
-			return;
-		}
-
-		const float distSq = distSqPointLineSegment(obstacle->point_, nextObstacle->point_, position_);
-
-		if (distSq < rangeSq) {
-			obstacleNeighbors_.push_back(std::make_pair(distSq, obstacle));
-
-			size_t i = obstacleNeighbors_.size() - 1;
-
-			while (i != 0 && distSq < obstacleNeighbors_[i - 1].first) {
-				obstacleNeighbors_[i] = obstacleNeighbors_[i - 1];
-				--i;
-			}
-
-			obstacleNeighbors_[i] = std::make_pair(distSq, obstacle);
-		}
-		//}
-	}
-
-	void Agent2D::update(RVOSimulator2D *sim_)
-	{
-		velocity_ = newVelocity_;
-		position_ += velocity_ * sim_->timeStep_;
-	}
-
-	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;
-	}
-
-	size_t linearProgram2(const std::vector<Line> &lines, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &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 < lines.size(); ++i) {
-			if (det(lines[i].direction, lines[i].point - result) > 0.0f) {
-				/* Result does not satisfy constraint i. Compute new optimal result. */
-				const Vector2 tempResult = result;
-
-				if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) {
-					result = tempResult;
-					return i;
-				}
-			}
-		}
-
-		return lines.size();
-	}
-
-	void linearProgram3(const std::vector<Line> &lines, size_t numObstLines, size_t beginLine, float radius, Vector2 &result)
-	{
-		float distance = 0.0f;
-
-		for (size_t i = beginLine; i < lines.size(); ++i) {
-			if (det(lines[i].direction, lines[i].point - result) > distance) {
-				/* Result does not satisfy constraint of line i. */
-				std::vector<Line> projLines(lines.begin(), lines.begin() + static_cast<ptrdiff_t>(numObstLines));
-
-				for (size_t j = numObstLines; j < i; ++j) {
-					Line line;
-
-					float determinant = det(lines[i].direction, lines[j].direction);
-
-					if (std::fabs(determinant) <= RVO_EPSILON) {
-						/* Line i and line j are parallel. */
-						if (lines[i].direction * lines[j].direction > 0.0f) {
-							/* Line i and line j point in the same direction. */
-							continue;
-						}
-						else {
-							/* Line i and line j point in opposite direction. */
-							line.point = 0.5f * (lines[i].point + lines[j].point);
-						}
-					}
-					else {
-						line.point = lines[i].point + (det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction;
-					}
-
-					line.direction = normalize(lines[j].direction - lines[i].direction);
-					projLines.push_back(line);
-				}
-
-				const Vector2 tempResult = result;
-
-				if (linearProgram2(projLines, radius, Vector2(-lines[i].direction.y(), lines[i].direction.x()), true, result) < projLines.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 = det(lines[i].direction, lines[i].point - result);
-			}
-		}
-	}
-}

thirdparty/rvo2/rvo2_2d/Agent2d.h

@@ -2,13 +2,14 @@
  * Agent2d.h
  * RVO2 Library
  *
- * Copyright 2008 University of North Carolina at Chapel Hill
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
- *     http://www.apache.org/licenses/LICENSE-2.0
+ *     https://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
@@ -27,134 +28,110 @@
  * Chapel Hill, N.C. 27599-3175
  * United States of America
  *
- * <http://gamma.cs.unc.edu/RVO2/>
+ * <https://gamma.cs.unc.edu/RVO2/>
  */
 
 #ifndef RVO2D_AGENT_H_
 #define RVO2D_AGENT_H_
 
 /**
- * \file       Agent2d.h
- * \brief      Contains the Agent class.
+ * @file  Agent2d.h
+ * @brief Declares the Agent2D class.
  */
 
-#include "Definitions.h"
-#include "RVOSimulator2d.h"
+#include <cstddef>
+#include <cstdint>
+#include <utility>
+#include <vector>
+
+#include "Line.h"
+#include "Vector2.h"
 
 namespace RVO2D {
-	/**
-	 * \brief      Defines an agent in the simulation.
-	 */
-	class Agent2D {
-	public:
-		/**
-		 * \brief      Constructs an agent instance.
-		 * \param      sim             The simulator instance.
-		 */
-		explicit Agent2D();
-
-		/**
-		 * \brief      Computes the neighbors of this agent.
-		 */
-		void computeNeighbors(RVOSimulator2D *sim_);
-
-		/**
-		 * \brief      Computes the new velocity of this agent.
-		 */
-		void computeNewVelocity(RVOSimulator2D *sim_);
-
-		/**
-		 * \brief      Inserts an agent neighbor into the set of neighbors of
-		 *             this agent.
-		 * \param      agent           A pointer to the agent to be inserted.
-		 * \param      rangeSq         The squared range around this agent.
-		 */
-		void insertAgentNeighbor(const Agent2D *agent, float &rangeSq);
-
-		/**
-		 * \brief      Inserts a static obstacle neighbor into the set of neighbors
-		 *             of this agent.
-		 * \param      obstacle        The number of the static obstacle to be
-		 *                             inserted.
-		 * \param      rangeSq         The squared range around this agent.
-		 */
-		void insertObstacleNeighbor(const Obstacle2D *obstacle, float rangeSq);
-
-		/**
-		 * \brief      Updates the two-dimensional position and two-dimensional
-		 *             velocity of this agent.
-		 */
-		void update(RVOSimulator2D *sim_);
-
-		std::vector<std::pair<float, const Agent2D *> > agentNeighbors_;
-		size_t maxNeighbors_;
-		float maxSpeed_;
-		float neighborDist_;
-		Vector2 newVelocity_;
-		std::vector<std::pair<float, const Obstacle2D *> > obstacleNeighbors_;
-		std::vector<Line> orcaLines_;
-		Vector2 position_;
-		Vector2 prefVelocity_;
-		float radius_;
-		float timeHorizon_;
-		float timeHorizonObst_;
-		Vector2 velocity_;
-		float height_ = 0.0;
-		float elevation_ = 0.0;
-		uint32_t avoidance_layers_ = 1;
-		uint32_t avoidance_mask_ = 1;
-		float avoidance_priority_ = 1.0;
-
-		size_t id_;
-
-		friend class KdTree2D;
-		friend class RVOSimulator2D;
-	};
-
-	/**
-	 * \relates    Agent
-	 * \brief      Solves a one-dimensional linear program on a specified line
-	 *             subject to linear constraints defined by lines and a circular
-	 *             constraint.
-	 * \param      lines         Lines defining the linear constraints.
-	 * \param      lineNo        The specified line constraint.
-	 * \param      radius        The radius of the circular constraint.
-	 * \param      optVelocity   The optimization velocity.
-	 * \param      directionOpt  True if the direction should be optimized.
-	 * \param      result        A reference to the result of the linear program.
-	 * \return     True if successful.
-	 */
-	bool linearProgram1(const std::vector<Line> &lines, size_t lineNo,
-						float radius, const Vector2 &optVelocity,
-						bool directionOpt, Vector2 &result);
-
-	/**
-	 * \relates    Agent
-	 * \brief      Solves a two-dimensional linear program subject to linear
-	 *             constraints defined by lines and a circular constraint.
-	 * \param      lines         Lines defining the linear constraints.
-	 * \param      radius        The radius of the circular constraint.
-	 * \param      optVelocity   The optimization velocity.
-	 * \param      directionOpt  True if the direction should be optimized.
-	 * \param      result        A reference to the result of the linear program.
-	 * \return     The number of the line it fails on, and the number of lines if successful.
-	 */
-	size_t linearProgram2(const std::vector<Line> &lines, float radius,
-						  const Vector2 &optVelocity, bool directionOpt,
-						  Vector2 &result);
-
-	/**
-	 * \relates    Agent
-	 * \brief      Solves a two-dimensional linear program subject to linear
-	 *             constraints defined by lines and a circular constraint.
-	 * \param      lines         Lines defining the linear constraints.
-	 * \param      numObstLines  Count of obstacle lines.
-	 * \param      beginLine     The line on which the 2-d linear program failed.
-	 * \param      radius        The radius of the circular constraint.
-	 * \param      result        A reference to the result of the linear program.
-	 */
-	void linearProgram3(const std::vector<Line> &lines, size_t numObstLines, size_t beginLine,
-						float radius, Vector2 &result);
-}
+class KdTree2D;
+class Obstacle2D;
+
+/**
+ * @brief Defines an agent in the simulation.
+ */
+class Agent2D {
+ public:
+  /**
+   * @brief Constructs an agent instance.
+   */
+  Agent2D();
+
+  /**
+   * @brief Destroys this agent instance.
+   */
+  ~Agent2D();
+
+  /**
+   * @brief     Computes the neighbors of this agent.
+   * @param[in] kdTree A pointer to the k-D trees for agents and static
+   *                   obstacles in the simulation.
+   */
+  void computeNeighbors(const KdTree2D *kdTree);
+
+  /**
+   * @brief     Computes the new velocity of this agent.
+   * @param[in] timeStep The time step of the simulation.
+   */
+  void computeNewVelocity(float timeStep);
+
+  /**
+   * @brief          Inserts an agent neighbor into the set of neighbors of this
+   *                 agent.
+   * @param[in]      agent   A pointer to the agent to be inserted.
+   * @param[in, out] rangeSq The squared range around this agent.
+   */
+  void insertAgentNeighbor(const Agent2D *agent,
+                           float &rangeSq); /* NOLINT(runtime/references) */
+
+  /**
+   * @brief          Inserts a static obstacle neighbor into the set of
+   *                 neighbors of this agent.
+   * @param[in]      obstacle The number of the static obstacle to be inserted.
+   * @param[in, out] rangeSq  The squared range around this agent.
+   */
+  void insertObstacleNeighbor(const Obstacle2D *obstacle, float rangeSq);
+
+  /**
+   * @brief     Updates the two-dimensional position and two-dimensional
+   *            velocity of this agent.
+   * @param[in] timeStep The time step of the simulation.
+   */
+  void update(float timeStep);
+
+  /* Not implemented. */
+  Agent2D(const Agent2D &other);
+
+  /* Not implemented. */
+  Agent2D &operator=(const Agent2D &other);
+
+  std::vector<std::pair<float, const Agent2D *> > agentNeighbors_;
+  std::vector<std::pair<float, const Obstacle2D *> > obstacleNeighbors_;
+  std::vector<Line> orcaLines_;
+  Vector2 newVelocity_;
+  Vector2 position_;
+  Vector2 prefVelocity_;
+  Vector2 velocity_;
+  std::size_t id_;
+  std::size_t maxNeighbors_;
+  float maxSpeed_;
+  float neighborDist_;
+  float radius_;
+  float timeHorizon_;
+  float timeHorizonObst_;
+	float height_ = 0.0;
+	float elevation_ = 0.0;
+	uint32_t avoidance_layers_ = 1;
+	uint32_t avoidance_mask_ = 1;
+	float avoidance_priority_ = 1.0;
+
+  friend class KdTree2D;
+  friend class RVOSimulator2D;
+};
+} /* namespace RVO */
 
 #endif /* RVO2D_AGENT_H_ */

thirdparty/rvo2/rvo2_2d/Definitions.h

@@ -1,110 +0,0 @@
-/*
- * Definitions.h
- * RVO2 Library
- *
- * Copyright 2008 University of North Carolina at Chapel Hill
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
- * Please send all bug reports to <[email protected]>.
- *
- * The authors may be contacted via:
- *
- * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
- * Dept. of Computer Science
- * 201 S. Columbia St.
- * Frederick P. Brooks, Jr. Computer Science Bldg.
- * Chapel Hill, N.C. 27599-3175
- * United States of America
- *
- * <http://gamma.cs.unc.edu/RVO2/>
- */
-
-#ifndef RVO2D_DEFINITIONS_H_
-#define RVO2D_DEFINITIONS_H_
-
-/**
- * \file       Definitions.h
- * \brief      Contains functions and constants used in multiple classes.
- */
-
-#include <algorithm>
-#include <cmath>
-#include <cstddef>
-#include <cstdint>
-#include <limits>
-#include <vector>
-
-#include "Vector2.h"
-
-/**
- * \brief       A sufficiently small positive number.
- */
-const float RVO_EPSILON = 0.00001f;
-
-namespace RVO2D {
-	class Agent2D;
-	class Obstacle2D;
-	class RVOSimulator2D;
-
-	/**
-	 * \brief      Computes the squared distance from a line segment with the
-	 *             specified endpoints to a specified point.
-	 * \param      a               The first endpoint of the line segment.
-	 * \param      b               The second endpoint of the line segment.
-	 * \param      c               The point to which the squared distance is to
-	 *                             be calculated.
-	 * \return     The squared distance from the line segment to the point.
-	 */
-	inline float distSqPointLineSegment(const Vector2 &a, const Vector2 &b,
-										const Vector2 &c)
-	{
-		const float r = ((c - a) * (b - a)) / absSq(b - a);
-
-		if (r < 0.0f) {
-			return absSq(c - a);
-		}
-		else if (r > 1.0f) {
-			return absSq(c - b);
-		}
-		else {
-			return absSq(c - (a + r * (b - a)));
-		}
-	}
-
-	/**
-	 * \brief      Computes the signed distance from a line connecting the
-	 *             specified points to a specified point.
-	 * \param      a               The first point on the line.
-	 * \param      b               The second point on the line.
-	 * \param      c               The point to which the signed distance is to
-	 *                             be calculated.
-	 * \return     Positive when the point c lies to the left of the line ab.
-	 */
-	inline float leftOf(const Vector2 &a, const Vector2 &b, const Vector2 &c)
-	{
-		return det(a - c, b - a);
-	}
-
-	/**
-	 * \brief      Computes the square of a float.
-	 * \param      a               The float to be squared.
-	 * \return     The square of the float.
-	 */
-	inline float sqr(float a)
-	{
-		return a * a;
-	}
-}
-
-#endif /* RVO2D_DEFINITIONS_H_ */

thirdparty/rvo2/rvo2_2d/KdTree2d.cc

@@ -0,0 +1,517 @@
+/*
+ * KdTree2d.cpp
+ * RVO2 Library
+ *
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     https://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Please send all bug reports to <[email protected]>.
+ *
+ * The authors may be contacted via:
+ *
+ * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
+ * Dept. of Computer Science
+ * 201 S. Columbia St.
+ * Frederick P. Brooks, Jr. Computer Science Bldg.
+ * Chapel Hill, N.C. 27599-3175
+ * United States of America
+ *
+ * <https://gamma.cs.unc.edu/RVO2/>
+ */
+
+/**
+ * @file  KdTree2d.cpp
+ * @brief Defines the KdTree2D class.
+ */
+
+#include "KdTree2d.h"
+
+#include <algorithm>
+#include <utility>
+
+#include "Agent2d.h"
+#include "Obstacle2d.h"
+#include "RVOSimulator2d.h"
+#include "Vector2.h"
+
+namespace RVO2D {
+namespace {
+/**
+ * @relates KdTree2D
+ * @brief   The maximum k-D tree node leaf size.
+ */
+const std::size_t RVO_MAX_LEAF_SIZE = 10U;
+} /* namespace */
+
+/**
+ * @brief Defines an agent k-D tree node.
+ */
+class KdTree2D::AgentTreeNode {
+ public:
+  /**
+   * @brief Constructs an agent k-D tree node instance.
+   */
+  AgentTreeNode();
+
+  /**
+   * @brief The beginning node number.
+   */
+  std::size_t begin;
+
+  /**
+   * @brief The ending node number.
+   */
+  std::size_t end;
+
+  /**
+   * @brief The left node number.
+   */
+  std::size_t left;
+
+  /**
+   * @brief The right node number.
+   */
+  std::size_t right;
+
+  /**
+   * @brief The maximum x-coordinate.
+   */
+  float maxX;
+
+  /**
+   * @brief The maximum y-coordinate.
+   */
+  float maxY;
+
+  /**
+   * @brief The minimum x-coordinate.
+   */
+  float minX;
+
+  /**
+   * @brief The minimum y-coordinate.
+   */
+  float minY;
+};
+
+KdTree2D::AgentTreeNode::AgentTreeNode()
+    : begin(0U),
+      end(0U),
+      left(0U),
+      right(0U),
+      maxX(0.0F),
+      maxY(0.0F),
+      minX(0.0F),
+      minY(0.0F) {}
+
+/**
+ * @brief Defines an obstacle k-D tree node.
+ */
+class KdTree2D::ObstacleTreeNode {
+ public:
+  /**
+   * @brief Constructs an obstacle k-D tree node instance.
+   */
+  ObstacleTreeNode();
+
+  /**
+   * @brief Destroys this obstacle k-D tree node instance.
+   */
+  ~ObstacleTreeNode();
+
+  /**
+   * @brief The obstacle number.
+   */
+  const Obstacle2D *obstacle;
+
+  /**
+   * @brief The left obstacle tree node.
+   */
+  ObstacleTreeNode *left;
+
+  /**
+   * @brief The right obstacle tree node.
+   */
+  ObstacleTreeNode *right;
+
+ private:
+  /* Not implemented. */
+  ObstacleTreeNode(const ObstacleTreeNode &other);
+
+  /* Not implemented. */
+  ObstacleTreeNode &operator=(const ObstacleTreeNode &other);
+};
+
+KdTree2D::ObstacleTreeNode::ObstacleTreeNode()
+    : obstacle(NULL), left(NULL), right(NULL) {}
+
+KdTree2D::ObstacleTreeNode::~ObstacleTreeNode() {}
+
+KdTree2D::KdTree2D(RVOSimulator2D *simulator)
+    : obstacleTree_(NULL), simulator_(simulator) {}
+
+KdTree2D::~KdTree2D() { deleteObstacleTree(obstacleTree_); }
+
+void KdTree2D::buildAgentTree(std::vector<Agent2D *> agents) {
+	agents_.swap(agents);
+
+	if (!agents_.empty()) {
+		agentTree_.resize(2 * agents_.size() - 1);
+		buildAgentTreeRecursive(0, agents_.size(), 0);
+	}
+}
+
+void KdTree2D::buildAgentTreeRecursive(std::size_t begin, std::size_t end,
+                                     std::size_t node) {
+  agentTree_[node].begin = begin;
+  agentTree_[node].end = end;
+  agentTree_[node].minX = agentTree_[node].maxX = agents_[begin]->position_.x();
+  agentTree_[node].minY = agentTree_[node].maxY = agents_[begin]->position_.y();
+
+  for (std::size_t i = begin + 1U; i < end; ++i) {
+    agentTree_[node].maxX =
+        std::max(agentTree_[node].maxX, agents_[i]->position_.x());
+    agentTree_[node].minX =
+        std::min(agentTree_[node].minX, agents_[i]->position_.x());
+    agentTree_[node].maxY =
+        std::max(agentTree_[node].maxY, agents_[i]->position_.y());
+    agentTree_[node].minY =
+        std::min(agentTree_[node].minY, agents_[i]->position_.y());
+  }
+
+  if (end - begin > RVO_MAX_LEAF_SIZE) {
+    /* No leaf node. */
+    const bool isVertical = agentTree_[node].maxX - agentTree_[node].minX >
+                            agentTree_[node].maxY - agentTree_[node].minY;
+    const float splitValue =
+        0.5F * (isVertical ? agentTree_[node].maxX + agentTree_[node].minX
+                           : agentTree_[node].maxY + agentTree_[node].minY);
+
+    std::size_t left = begin;
+    std::size_t right = end;
+
+    while (left < right) {
+      while (left < right &&
+             (isVertical ? agents_[left]->position_.x()
+                         : agents_[left]->position_.y()) < splitValue) {
+        ++left;
+      }
+
+      while (right > left &&
+             (isVertical ? agents_[right - 1U]->position_.x()
+                         : agents_[right - 1U]->position_.y()) >= splitValue) {
+        --right;
+      }
+
+      if (left < right) {
+        std::swap(agents_[left], agents_[right - 1U]);
+        ++left;
+        --right;
+      }
+    }
+
+    if (left == begin) {
+      ++left;
+      ++right;
+    }
+
+    agentTree_[node].left = node + 1U;
+    agentTree_[node].right = node + 2U * (left - begin);
+
+    buildAgentTreeRecursive(begin, left, agentTree_[node].left);
+    buildAgentTreeRecursive(left, end, agentTree_[node].right);
+  }
+}
+
+void KdTree2D::buildObstacleTree(std::vector<Obstacle2D *> obstacles) {
+	deleteObstacleTree(obstacleTree_);
+
+	obstacleTree_ = buildObstacleTreeRecursive(obstacles);
+}
+
+KdTree2D::ObstacleTreeNode *KdTree2D::buildObstacleTreeRecursive(
+    const std::vector<Obstacle2D *> &obstacles) {
+  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 Obstacle2D *const obstacleI1 = obstacles[i];
+      const Obstacle2D *const obstacleI2 = obstacleI1->next_;
+
+      /* Compute optimal split node. */
+      for (std::size_t j = 0U; j < obstacles.size(); ++j) {
+        if (i != j) {
+          const Obstacle2D *const obstacleJ1 = obstacles[j];
+          const Obstacle2D *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 >= -RVO2D_EPSILON && j2LeftOfI >= -RVO2D_EPSILON) {
+            ++leftSize;
+          } else if (j1LeftOfI <= RVO2D_EPSILON && j2LeftOfI <= RVO2D_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::make_pair(std::max(minLeft, minRight),
+                         std::min(minLeft, minRight))) {
+        minLeft = leftSize;
+        minRight = rightSize;
+        optimalSplit = i;
+      }
+    }
+
+    /* Build split node. */
+    std::vector<Obstacle2D *> leftObstacles(minLeft);
+    std::vector<Obstacle2D *> rightObstacles(minRight);
+
+    std::size_t leftCounter = 0U;
+    std::size_t rightCounter = 0U;
+    const std::size_t i = optimalSplit;
+
+    const Obstacle2D *const obstacleI1 = obstacles[i];
+    const Obstacle2D *const obstacleI2 = obstacleI1->next_;
+
+    for (std::size_t j = 0U; j < obstacles.size(); ++j) {
+      if (i != j) {
+        Obstacle2D *const obstacleJ1 = obstacles[j];
+        Obstacle2D *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 >= -RVO2D_EPSILON && j2LeftOfI >= -RVO2D_EPSILON) {
+          leftObstacles[leftCounter++] = obstacles[j];
+        } else if (j1LeftOfI <= RVO2D_EPSILON && j2LeftOfI <= RVO2D_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_);
+
+          Obstacle2D *const newObstacle = new Obstacle2D();
+          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);
+
+    return node;
+  }
+
+  return NULL;
+}
+
+void KdTree2D::computeAgentNeighbors(Agent2D *agent, float &rangeSq) const {
+  queryAgentTreeRecursive(agent, rangeSq, 0U);
+}
+
+void KdTree2D::computeObstacleNeighbors(Agent2D *agent, float rangeSq) const {
+  queryObstacleTreeRecursive(agent, rangeSq, obstacleTree_);
+}
+
+void KdTree2D::deleteObstacleTree(ObstacleTreeNode *node) {
+  if (node != NULL) {
+    deleteObstacleTree(node->left);
+    deleteObstacleTree(node->right);
+    delete node;
+  }
+}
+
+void KdTree2D::queryAgentTreeRecursive(Agent2D *agent, float &rangeSq,
+                                     std::size_t node) const {
+  if (agentTree_[node].end - agentTree_[node].begin <= RVO_MAX_LEAF_SIZE) {
+    for (std::size_t i = agentTree_[node].begin; i < agentTree_[node].end;
+         ++i) {
+      agent->insertAgentNeighbor(agents_[i], rangeSq);
+    }
+  } else {
+    const float distLeftMinX = std::max(
+        0.0F, agentTree_[agentTree_[node].left].minX - agent->position_.x());
+    const float distLeftMaxX = std::max(
+        0.0F, agent->position_.x() - agentTree_[agentTree_[node].left].maxX);
+    const float distLeftMinY = std::max(
+        0.0F, agentTree_[agentTree_[node].left].minY - agent->position_.y());
+    const float distLeftMaxY = std::max(
+        0.0F, agent->position_.y() - agentTree_[agentTree_[node].left].maxY);
+
+    const float distSqLeft =
+        distLeftMinX * distLeftMinX + distLeftMaxX * distLeftMaxX +
+        distLeftMinY * distLeftMinY + distLeftMaxY * distLeftMaxY;
+
+    const float distRightMinX = std::max(
+        0.0F, agentTree_[agentTree_[node].right].minX - agent->position_.x());
+    const float distRightMaxX = std::max(
+        0.0F, agent->position_.x() - agentTree_[agentTree_[node].right].maxX);
+    const float distRightMinY = std::max(
+        0.0F, agentTree_[agentTree_[node].right].minY - agent->position_.y());
+    const float distRightMaxY = std::max(
+        0.0F, agent->position_.y() - agentTree_[agentTree_[node].right].maxY);
+
+    const float distSqRight =
+        distRightMinX * distRightMinX + distRightMaxX * distRightMaxX +
+        distRightMinY * distRightMinY + distRightMaxY * distRightMaxY;
+
+    if (distSqLeft < distSqRight) {
+      if (distSqLeft < rangeSq) {
+        queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
+
+        if (distSqRight < rangeSq) {
+          queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
+        }
+      }
+    } else if (distSqRight < rangeSq) {
+      queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
+
+      if (distSqLeft < rangeSq) {
+        queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
+      }
+    }
+  }
+}
+
+void KdTree2D::queryObstacleTreeRecursive(Agent2D *agent, float rangeSq,
+                                        const ObstacleTreeNode *node) const {
+  if (node != NULL) {
+    const Obstacle2D *const obstacle1 = node->obstacle;
+    const Obstacle2D *const obstacle2 = obstacle1->next_;
+
+    const float agentLeftOfLine =
+        leftOf(obstacle1->point_, obstacle2->point_, agent->position_);
+
+    queryObstacleTreeRecursive(
+        agent, rangeSq, agentLeftOfLine >= 0.0F ? node->left : node->right);
+
+    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);
+      }
+
+      /* Try other side of line. */
+      queryObstacleTreeRecursive(
+          agent, rangeSq, agentLeftOfLine >= 0.0F ? node->right : node->left);
+    }
+  }
+}
+
+bool KdTree2D::queryVisibility(const Vector2 &vector1, const Vector2 &vector2,
+                             float radius) const {
+  return queryVisibilityRecursive(vector1, vector2, radius, obstacleTree_);
+}
+
+bool KdTree2D::queryVisibilityRecursive(const Vector2 &vector1,
+                                      const Vector2 &vector2, float radius,
+                                      const ObstacleTreeNode *node) const {
+  if (node != NULL) {
+    const Obstacle2D *const obstacle1 = node->obstacle;
+    const Obstacle2D *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));
+    }
+
+    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) {
+      /* One can see through obstacle from left to right. */
+      return 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 RVO2D */

thirdparty/rvo2/rvo2_2d/KdTree2d.cpp

@@ -1,357 +0,0 @@
-/*
- * KdTree2d.cpp
- * RVO2 Library
- *
- * Copyright 2008 University of North Carolina at Chapel Hill
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
- * Please send all bug reports to <[email protected]>.
- *
- * The authors may be contacted via:
- *
- * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
- * Dept. of Computer Science
- * 201 S. Columbia St.
- * Frederick P. Brooks, Jr. Computer Science Bldg.
- * Chapel Hill, N.C. 27599-3175
- * United States of America
- *
- * <http://gamma.cs.unc.edu/RVO2/>
- */
-
-#include "KdTree2d.h"
-
-#include "Agent2d.h"
-#include "RVOSimulator2d.h"
-#include "Obstacle2d.h"
-
-namespace RVO2D {
-	KdTree2D::KdTree2D(RVOSimulator2D *sim) : obstacleTree_(NULL), sim_(sim) { }
-
-	KdTree2D::~KdTree2D()
-	{
-		deleteObstacleTree(obstacleTree_);
-	}
-
-	void KdTree2D::buildAgentTree(std::vector<Agent2D *> agents)
-	{
-		agents_.swap(agents);
-
-		if (!agents_.empty()) {
-			agentTree_.resize(2 * agents_.size() - 1);
-			buildAgentTreeRecursive(0, agents_.size(), 0);
-		}
-	}
-
-	void KdTree2D::buildAgentTreeRecursive(size_t begin, size_t end, size_t node)
-	{
-		agentTree_[node].begin = begin;
-		agentTree_[node].end = end;
-		agentTree_[node].minX = agentTree_[node].maxX = agents_[begin]->position_.x();
-		agentTree_[node].minY = agentTree_[node].maxY = agents_[begin]->position_.y();
-
-		for (size_t i = begin + 1; i < end; ++i) {
-			agentTree_[node].maxX = std::max(agentTree_[node].maxX, agents_[i]->position_.x());
-			agentTree_[node].minX = std::min(agentTree_[node].minX, agents_[i]->position_.x());
-			agentTree_[node].maxY = std::max(agentTree_[node].maxY, agents_[i]->position_.y());
-			agentTree_[node].minY = std::min(agentTree_[node].minY, agents_[i]->position_.y());
-		}
-
-		if (end - begin > MAX_LEAF_SIZE) {
-			/* No leaf node. */
-			const bool isVertical = (agentTree_[node].maxX - agentTree_[node].minX > agentTree_[node].maxY - agentTree_[node].minY);
-			const float splitValue = (isVertical ? 0.5f * (agentTree_[node].maxX + agentTree_[node].minX) : 0.5f * (agentTree_[node].maxY + agentTree_[node].minY));
-
-			size_t left = begin;
-			size_t right = end;
-
-			while (left < right) {
-				while (left < right && (isVertical ? agents_[left]->position_.x() : agents_[left]->position_.y()) < splitValue) {
-					++left;
-				}
-
-				while (right > left && (isVertical ? agents_[right - 1]->position_.x() : agents_[right - 1]->position_.y()) >= splitValue) {
-					--right;
-				}
-
-				if (left < right) {
-					std::swap(agents_[left], agents_[right - 1]);
-					++left;
-					--right;
-				}
-			}
-
-			if (left == begin) {
-				++left;
-				++right;
-			}
-
-			agentTree_[node].left = node + 1;
-			agentTree_[node].right = node + 2 * (left - begin);
-
-			buildAgentTreeRecursive(begin, left, agentTree_[node].left);
-			buildAgentTreeRecursive(left, end, agentTree_[node].right);
-		}
-	}
-
-	void KdTree2D::buildObstacleTree(std::vector<Obstacle2D *> obstacles)
-	{
-		deleteObstacleTree(obstacleTree_);
-
-		obstacleTree_ = buildObstacleTreeRecursive(obstacles);
-	}
-
-
-	KdTree2D::ObstacleTreeNode *KdTree2D::buildObstacleTreeRecursive(const std::vector<Obstacle2D *> &obstacles)
-	{
-		if (obstacles.empty()) {
-			return NULL;
-		}
-		else {
-			ObstacleTreeNode *const node = new ObstacleTreeNode;
-
-			size_t optimalSplit = 0;
-			size_t minLeft = obstacles.size();
-			size_t minRight = obstacles.size();
-
-			for (size_t i = 0; i < obstacles.size(); ++i) {
-				size_t leftSize = 0;
-				size_t rightSize = 0;
-
-				const Obstacle2D *const obstacleI1 = obstacles[i];
-				const Obstacle2D *const obstacleI2 = obstacleI1->nextObstacle_;
-
-				/* Compute optimal split node. */
-				for (size_t j = 0; j < obstacles.size(); ++j) {
-					if (i == j) {
-						continue;
-					}
-
-					const Obstacle2D *const obstacleJ1 = obstacles[j];
-					const Obstacle2D *const obstacleJ2 = obstacleJ1->nextObstacle_;
-
-					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::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) {
-					minLeft = leftSize;
-					minRight = rightSize;
-					optimalSplit = i;
-				}
-			}
-
-			/* Build split node. */
-			std::vector<Obstacle2D *> leftObstacles(minLeft);
-			std::vector<Obstacle2D *> rightObstacles(minRight);
-
-			size_t leftCounter = 0;
-			size_t rightCounter = 0;
-			const size_t i = optimalSplit;
-
-			const Obstacle2D *const obstacleI1 = obstacles[i];
-			const Obstacle2D *const obstacleI2 = obstacleI1->nextObstacle_;
-
-			for (size_t j = 0; j < obstacles.size(); ++j) {
-				if (i == j) {
-					continue;
-				}
-
-				Obstacle2D *const obstacleJ1 = obstacles[j];
-				Obstacle2D *const obstacleJ2 = obstacleJ1->nextObstacle_;
-
-				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_);
-
-					Obstacle2D *const newObstacle = new Obstacle2D();
-					newObstacle->point_ = splitpoint;
-					newObstacle->prevObstacle_ = obstacleJ1;
-					newObstacle->nextObstacle_ = obstacleJ2;
-					newObstacle->isConvex_ = true;
-					newObstacle->unitDir_ = obstacleJ1->unitDir_;
-
-					newObstacle->id_ = sim_->obstacles_.size();
-
-					sim_->obstacles_.push_back(newObstacle);
-
-					obstacleJ1->nextObstacle_ = newObstacle;
-					obstacleJ2->prevObstacle_ = 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);
-			return node;
-		}
-	}
-
-	void KdTree2D::computeAgentNeighbors(Agent2D *agent, float &rangeSq) const
-	{
-		queryAgentTreeRecursive(agent, rangeSq, 0);
-	}
-
-	void KdTree2D::computeObstacleNeighbors(Agent2D *agent, float rangeSq) const
-	{
-		queryObstacleTreeRecursive(agent, rangeSq, obstacleTree_);
-	}
-
-	void KdTree2D::deleteObstacleTree(ObstacleTreeNode *node)
-	{
-		if (node != NULL) {
-			deleteObstacleTree(node->left);
-			deleteObstacleTree(node->right);
-			delete node;
-		}
-	}
-
-	void KdTree2D::queryAgentTreeRecursive(Agent2D *agent, float &rangeSq, size_t node) const
-	{
-		if (agentTree_[node].end - agentTree_[node].begin <= MAX_LEAF_SIZE) {
-			for (size_t i = agentTree_[node].begin; i < agentTree_[node].end; ++i) {
-				agent->insertAgentNeighbor(agents_[i], rangeSq);
-			}
-		}
-		else {
-			const float distSqLeft = sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minX - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].left].maxX)) + sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minY - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].left].maxY));
-
-			const float distSqRight = sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minX - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].right].maxX)) + sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minY - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].right].maxY));
-
-			if (distSqLeft < distSqRight) {
-				if (distSqLeft < rangeSq) {
-					queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
-
-					if (distSqRight < rangeSq) {
-						queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
-					}
-				}
-			}
-			else {
-				if (distSqRight < rangeSq) {
-					queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
-
-					if (distSqLeft < rangeSq) {
-						queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
-					}
-				}
-			}
-
-		}
-	}
-
-	void KdTree2D::queryObstacleTreeRecursive(Agent2D *agent, float rangeSq, const ObstacleTreeNode *node) const
-	{
-		if (node == NULL) {
-			return;
-		}
-		else {
-			const Obstacle2D *const obstacle1 = node->obstacle;
-			const Obstacle2D *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));
-
-			}
-		}
-	}
-
-	bool KdTree2D::queryVisibility(const Vector2 &q1, const Vector2 &q2, float radius) const
-	{
-		return queryVisibilityRecursive(q1, q2, radius, obstacleTree_);
-	}
-
-	bool KdTree2D::queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2, float radius, const ObstacleTreeNode *node) const
-	{
-		if (node == NULL) {
-			return true;
-		}
-		else {
-			const Obstacle2D *const obstacle1 = node->obstacle;
-			const Obstacle2D *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));
-			}
-		}
-	}
-}

thirdparty/rvo2/rvo2_2d/KdTree2d.h

@@ -2,13 +2,14 @@
  * KdTree2d.h
  * RVO2 Library
  *
- * Copyright 2008 University of North Carolina at Chapel Hill
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
- *     http://www.apache.org/licenses/LICENSE-2.0
+ *     https://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
@@ -27,177 +28,162 @@
  * Chapel Hill, N.C. 27599-3175
  * United States of America
  *
- * <http://gamma.cs.unc.edu/RVO2/>
+ * <https://gamma.cs.unc.edu/RVO2/>
  */
 
 #ifndef RVO2D_KD_TREE_H_
 #define RVO2D_KD_TREE_H_
 
 /**
- * \file       KdTree2d.h
- * \brief      Contains the KdTree class.
+ * @file  KdTree2d.h
+ * @brief Declares the KdTree2D class.
  */
 
-#include "Definitions.h"
+#include <cstddef>
+#include <vector>
 
 namespace RVO2D {
-	/**
-	 * \brief      Defines <i>k</i>d-trees for agents and static obstacles in the
-	 *             simulation.
-	 */
-	class KdTree2D {
-	public:
-		/**
-		 * \brief      Defines an agent <i>k</i>d-tree node.
-		 */
-		class AgentTreeNode {
-		public:
-			/**
-			 * \brief      The beginning node number.
-			 */
-			size_t begin;
-
-			/**
-			 * \brief      The ending node number.
-			 */
-			size_t end;
-
-			/**
-			 * \brief      The left node number.
-			 */
-			size_t left;
-
-			/**
-			 * \brief      The maximum x-coordinate.
-			 */
-			float maxX;
-
-			/**
-			 * \brief      The maximum y-coordinate.
-			 */
-			float maxY;
-
-			/**
-			 * \brief      The minimum x-coordinate.
-			 */
-			float minX;
-
-			/**
-			 * \brief      The minimum y-coordinate.
-			 */
-			float minY;
-
-			/**
-			 * \brief      The right node number.
-			 */
-			size_t right;
-		};
-
-		/**
-		 * \brief      Defines an obstacle <i>k</i>d-tree node.
-		 */
-		class ObstacleTreeNode {
-		public:
-			/**
-			 * \brief      The left obstacle tree node.
-			 */
-			ObstacleTreeNode *left;
-
-			/**
-			 * \brief      The obstacle number.
-			 */
-			const Obstacle2D *obstacle;
-
-			/**
-			 * \brief      The right obstacle tree node.
-			 */
-			ObstacleTreeNode *right;
-		};
-
-		/**
-		 * \brief      Constructs a <i>k</i>d-tree instance.
-		 * \param      sim             The simulator instance.
-		 */
-		explicit KdTree2D(RVOSimulator2D *sim);
-
-		/**
-		 * \brief      Destroys this kd-tree instance.
-		 */
-		~KdTree2D();
-
-		/**
-		 * \brief      Builds an agent <i>k</i>d-tree.
-		 */
-		void buildAgentTree(std::vector<Agent2D *> agents);
-
-		void buildAgentTreeRecursive(size_t begin, size_t end, size_t node);
-
-		/**
-		 * \brief      Builds an obstacle <i>k</i>d-tree.
-		 */
-		void buildObstacleTree(std::vector<Obstacle2D *> obstacles);
-
-		ObstacleTreeNode *buildObstacleTreeRecursive(const std::vector<Obstacle2D *> &
-													 obstacles);
-
-		/**
-		 * \brief      Computes the agent neighbors of the specified agent.
-		 * \param      agent           A pointer to the agent for which agent
-		 *                             neighbors are to be computed.
-		 * \param      rangeSq         The squared range around the agent.
-		 */
-		void computeAgentNeighbors(Agent2D *agent, float &rangeSq) const;
-
-		/**
-		 * \brief      Computes the obstacle neighbors of the specified agent.
-		 * \param      agent           A pointer to the agent for which obstacle
-		 *                             neighbors are to be computed.
-		 * \param      rangeSq         The squared range around the agent.
-		 */
-		void computeObstacleNeighbors(Agent2D *agent, float rangeSq) const;
-
-		/**
-		 * \brief      Deletes the specified obstacle tree node.
-		 * \param      node            A pointer to the obstacle tree node to be
-		 *                             deleted.
-		 */
-		void deleteObstacleTree(ObstacleTreeNode *node);
-
-		void queryAgentTreeRecursive(Agent2D *agent, float &rangeSq,
-									 size_t node) const;
-
-		void queryObstacleTreeRecursive(Agent2D *agent, float rangeSq,
-										const ObstacleTreeNode *node) const;
-
-		/**
-		 * \brief      Queries the visibility between two points within a
-		 *             specified radius.
-		 * \param      q1              The first point between which visibility is
-		 *                             to be tested.
-		 * \param      q2              The second point between which visibility is
-		 *                             to be tested.
-		 * \param      radius          The radius within which visibility is to be
-		 *                             tested.
-		 * \return     True if q1 and q2 are mutually visible within the radius;
-		 *             false otherwise.
-		 */
-		bool queryVisibility(const Vector2 &q1, const Vector2 &q2,
-							 float radius) const;
-
-		bool queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2,
-									  float radius,
-									  const ObstacleTreeNode *node) const;
-
-		std::vector<Agent2D *> agents_;
-		std::vector<AgentTreeNode> agentTree_;
-		ObstacleTreeNode *obstacleTree_;
-		RVOSimulator2D *sim_;
-
-		static const size_t MAX_LEAF_SIZE = 10;
-
-		friend class Agent2D;
-		friend class RVOSimulator2D;
-	};
-}
+class Agent2D;
+class Obstacle2D;
+class RVOSimulator2D;
+class Vector2;
+
+/**
+ * @brief Defines k-D trees for agents and static obstacles in the simulation.
+ */
+class KdTree2D {
+ public:
+  class AgentTreeNode;
+  class ObstacleTreeNode;
+
+  /**
+   * @brief     Constructs a k-D tree instance.
+   * @param[in] simulator The simulator instance.
+   */
+  explicit KdTree2D(RVOSimulator2D *simulator);
+
+  /**
+   * @brief Destroys this k-D tree instance.
+   */
+  ~KdTree2D();
+
+  /**
+   * @brief Builds an agent k-D tree.
+   */
+  void buildAgentTree(std::vector<Agent2D *> agents);
+
+  /**
+   * @brief     Recursive function to build an agent k-D tree.
+   * @param[in] begin The beginning agent k-D tree node.
+   * @param[in] end   The ending agent k-D tree node.
+   * @param[in] node  The current agent k-D tree node.
+   */
+  void buildAgentTreeRecursive(std::size_t begin, std::size_t end,
+                               std::size_t node);
+
+  /**
+   * @brief Builds an obstacle k-D tree.
+   */
+  void buildObstacleTree(std::vector<Obstacle2D *> obstacles);
+
+  /**
+   * @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.
+   */
+  ObstacleTreeNode *buildObstacleTreeRecursive(
+      const std::vector<Obstacle2D *> &obstacles);
+
+  /**
+   * @brief     Computes the agent neighbors of the specified agent.
+   * @param[in] agent        A pointer to the agent for which agent neighbors
+   *                         are to be computed.
+   * @param[in, out] rangeSq The squared range around the agent.
+   */
+  void computeAgentNeighbors(
+      Agent2D *agent, float &rangeSq) const; /* NOLINT(runtime/references) */
+
+  /**
+   * @brief     Computes the obstacle neighbors of the specified agent.
+   * @param[in] agent   A pointer to the agent for which obstacle neighbors are
+   *                    to be computed.
+   * @param[in] rangeSq The squared range around the agent.
+   */
+  void computeObstacleNeighbors(Agent2D *agent, float rangeSq) const;
+
+  /**
+   * @brief     Deletes the specified obstacle tree node.
+   * @param[in] node A pointer to the obstacle tree node to be deleted.
+   */
+  void deleteObstacleTree(ObstacleTreeNode *node);
+
+  /**
+   * @brief         Recursive function to compute the neighbors of the specified
+   *                agent.
+   * @param[in]     agent   A pointer to the agent for which neighbors are to be
+   *                        computed.
+   * @param[in,out] rangeSq The squared range around the agent.
+   * @param[in]     node    The current agent k-D tree node.
+   */
+  void queryAgentTreeRecursive(Agent2D *agent,
+                               float &rangeSq, /* NOLINT(runtime/references) */
+                               std::size_t node) const;
+
+  /**
+   * @brief         Recursive function to compute the neighbors of the specified
+   *                obstacle.
+   * @param[in]     agent   A pointer to the agent for which neighbors are to be
+   *                        computed.
+   * @param[in,out] rangeSq The squared range around the agent.
+   * @param[in]     node    The current obstacle k-D tree node.
+   */
+  void queryObstacleTreeRecursive(Agent2D *agent, float rangeSq,
+                                  const ObstacleTreeNode *node) const;
+
+  /**
+   * @brief     Queries the visibility between two points within a specified
+   *            radius.
+   * @param[in] vector1 The first point between which visibility is to be
+   * 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.
+   * @return    True if q1 and q2 are mutually visible within the radius; false
+   *            otherwise.
+   */
+  bool queryVisibility(const Vector2 &vector1, const Vector2 &vector2,
+                       float radius) const;
+
+  /**
+   * @brief     Recursive function to query the visibility between two points
+   *            within a specified radius.
+   * @param[in] vector1 The first point between which visibility is to be
+   *                    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.
+   * @param[in] node    The current obstacle k-D tree node.
+   * @return    True if q1 and q2 are mutually visible within the radius; false
+   *            otherwise.
+   */
+  bool queryVisibilityRecursive(const Vector2 &vector1, const Vector2 &vector2,
+                                float radius,
+                                const ObstacleTreeNode *node) const;
+
+  /* Not implemented. */
+  KdTree2D(const KdTree2D &other);
+
+  /* Not implemented. */
+  KdTree2D &operator=(const KdTree2D &other);
+
+  std::vector<Agent2D *> agents_;
+  std::vector<AgentTreeNode> agentTree_;
+  ObstacleTreeNode *obstacleTree_;
+  RVOSimulator2D *simulator_;
+
+  friend class Agent2D;
+  friend class RVOSimulator2D;
+};
+} /* namespace RVO2D */
 
 #endif /* RVO2D_KD_TREE_H_ */

thirdparty/rvo2/rvo2_2d/Obstacle2d.cpp → thirdparty/rvo2/rvo2_2d/Line.cc

@@ -1,14 +1,15 @@
 /*
- * Obstacle2d.cpp
+ * Line.cc
  * RVO2 Library
  *
- * Copyright 2008 University of North Carolina at Chapel Hill
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
- *     http://www.apache.org/licenses/LICENSE-2.0
+ *     https://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
@@ -27,12 +28,16 @@
  * Chapel Hill, N.C. 27599-3175
  * United States of America
  *
- * <http://gamma.cs.unc.edu/RVO2/>
+ * <https://gamma.cs.unc.edu/RVO2/>
  */
 
-#include "Obstacle2d.h"
-#include "RVOSimulator2d.h"
+/**
+ * @file  Line.cc
+ * @brief Defines the Line class.
+ */
+
+#include "Line.h"
 
 namespace RVO2D {
-	Obstacle2D::Obstacle2D() : isConvex_(false), nextObstacle_(NULL), prevObstacle_(NULL), id_(0) { }
-}
+Line::Line() {}
+} /* namespace RVO2D */

thirdparty/rvo2/rvo2_2d/Line.h

@@ -0,0 +1,67 @@
+/*
+ * Line.h
+ * RVO2 Library
+ *
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     https://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Please send all bug reports to <[email protected]>.
+ *
+ * The authors may be contacted via:
+ *
+ * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
+ * Dept. of Computer Science
+ * 201 S. Columbia St.
+ * Frederick P. Brooks, Jr. Computer Science Bldg.
+ * Chapel Hill, N.C. 27599-3175
+ * United States of America
+ *
+ * <https://gamma.cs.unc.edu/RVO2/>
+ */
+
+#ifndef RVO2D_LINE_H_
+#define RVO2D_LINE_H_
+
+/**
+ * @file  Line.h
+ * @brief Declares the Line class.
+ */
+
+#include "Vector2.h"
+
+namespace RVO2D {
+/**
+ * @brief Defines a directed line.
+ */
+class Line {
+ public:
+  /**
+   * @brief Constructs a directed line instance.
+   */
+  Line();
+
+  /**
+   * @brief The direction of the directed line.
+   */
+  Vector2 direction;
+
+  /**
+   * @brief A point on the directed line.
+   */
+  Vector2 point;
+};
+} /* namespace RVO2D */
+
+#endif /* RVO2D_LINE_H_ */

thirdparty/rvo2/rvo2_2d/Obstacle2d.cc

@@ -0,0 +1,46 @@
+/*
+ * Obstacle2d.cpp
+ * RVO2 Library
+ *
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     https://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Please send all bug reports to <[email protected]>.
+ *
+ * The authors may be contacted via:
+ *
+ * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
+ * Dept. of Computer Science
+ * 201 S. Columbia St.
+ * Frederick P. Brooks, Jr. Computer Science Bldg.
+ * Chapel Hill, N.C. 27599-3175
+ * United States of America
+ *
+ * <https://gamma.cs.unc.edu/RVO2/>
+ */
+
+/**
+ * @file  Obstacle2d.cpp
+ * @brief Defines the Obstacle2D class.
+ */
+
+#include "Obstacle2d.h"
+
+namespace RVO2D {
+Obstacle2D::Obstacle2D()
+    : next_(NULL), previous_(NULL), id_(0U), isConvex_(false) {}
+
+Obstacle2D::~Obstacle2D() {}
+} /* namespace RVO2D */

thirdparty/rvo2/rvo2_2d/Obstacle2d.h

@@ -2,13 +2,14 @@
  * Obstacle2d.h
  * RVO2 Library
  *
- * Copyright 2008 University of North Carolina at Chapel Hill
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
- *     http://www.apache.org/licenses/LICENSE-2.0
+ *     https://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
@@ -27,46 +28,59 @@
  * Chapel Hill, N.C. 27599-3175
  * United States of America
  *
- * <http://gamma.cs.unc.edu/RVO2/>
+ * <https://gamma.cs.unc.edu/RVO2/>
  */
 
 #ifndef RVO2D_OBSTACLE_H_
 #define RVO2D_OBSTACLE_H_
 
 /**
- * \file       Obstacle2d.h
- * \brief      Contains the Obstacle class.
+ * @file  Obstacle2d.h
+ * @brief Declares the Obstacle2D class.
  */
 
-#include "Definitions.h"
+#include <cstddef>
+#include <cstdint>
+
+#include "Vector2.h"
 
 namespace RVO2D {
-	/**
-	 * \brief      Defines static obstacles in the simulation.
-	 */
-	class Obstacle2D {
-	public:
-		/**
-		 * \brief      Constructs a static obstacle instance.
-		 */
-		Obstacle2D();
+/**
+ * @brief Defines static obstacles in the simulation.
+ */
+class Obstacle2D {
+ public:
+  /**
+   * @brief Constructs a static obstacle instance.
+   */
+  Obstacle2D();
+
+  /**
+   * @brief Destroys this static obstacle instance.
+   */
+  ~Obstacle2D();
+
+  /* Not implemented. */
+  Obstacle2D(const Obstacle2D &other);
 
-		bool isConvex_;
-		Obstacle2D *nextObstacle_;
-		Vector2 point_;
-		Obstacle2D *prevObstacle_;
-		Vector2 unitDir_;
+  /* Not implemented. */
+  Obstacle2D &operator=(const Obstacle2D &other);
 
-		float height_ = 1.0;
-		float elevation_ = 0.0;
-		uint32_t avoidance_layers_ = 1;
+  Vector2 direction_;
+  Vector2 point_;
+  Obstacle2D *next_;
+  Obstacle2D *previous_;
+  std::size_t id_;
+  bool isConvex_;
 
-		size_t id_;
+  float height_ = 1.0;
+	float elevation_ = 0.0;
+	uint32_t avoidance_layers_ = 1;
 
-		friend class Agent2D;
-		friend class KdTree2D;
-		friend class RVOSimulator2D;
-	};
-}
+  friend class Agent2D;
+  friend class KdTree2D;
+  friend class RVOSimulator2D;
+};
+} /* namespace RVO2D */
 
 #endif /* RVO2D_OBSTACLE_H_ */

thirdparty/rvo2/rvo2_2d/RVOSimulator2d.cc

@@ -0,0 +1,371 @@
+/*
+ * RVOSimulator2d.cpp
+ * RVO2 Library
+ *
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     https://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Please send all bug reports to <[email protected]>.
+ *
+ * The authors may be contacted via:
+ *
+ * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
+ * Dept. of Computer Science
+ * 201 S. Columbia St.
+ * Frederick P. Brooks, Jr. Computer Science Bldg.
+ * Chapel Hill, N.C. 27599-3175
+ * United States of America
+ *
+ * <https://gamma.cs.unc.edu/RVO2/>
+ */
+
+/**
+ * @file  RVOSimulator2d.cpp
+ * @brief Defines the RVOSimulator2D class.
+ */
+
+#include "RVOSimulator2d.h"
+
+#include <limits>
+#include <utility>
+
+#include "Agent2d.h"
+#include "KdTree2d.h"
+#include "Line.h"
+#include "Obstacle2d.h"
+#include "Vector2.h"
+
+#ifdef _OPENMP
+#include <omp.h>
+#endif /* _OPENMP */
+
+namespace RVO2D {
+const std::size_t RVO2D_ERROR = std::numeric_limits<std::size_t>::max();
+
+RVOSimulator2D::RVOSimulator2D()
+    : defaultAgent_(NULL),
+      kdTree_(new KdTree2D(this)),
+      globalTime_(0.0F),
+      timeStep_(0.0F) {}
+
+RVOSimulator2D::RVOSimulator2D(float timeStep, float neighborDist,
+                           std::size_t maxNeighbors, float timeHorizon,
+                           float timeHorizonObst, float radius, float maxSpeed)
+    : defaultAgent_(new Agent2D()),
+      kdTree_(new KdTree2D(this)),
+      globalTime_(0.0F),
+      timeStep_(timeStep) {
+  defaultAgent_->maxNeighbors_ = maxNeighbors;
+  defaultAgent_->maxSpeed_ = maxSpeed;
+  defaultAgent_->neighborDist_ = neighborDist;
+  defaultAgent_->radius_ = radius;
+  defaultAgent_->timeHorizon_ = timeHorizon;
+  defaultAgent_->timeHorizonObst_ = timeHorizonObst;
+}
+
+RVOSimulator2D::RVOSimulator2D(float timeStep, float neighborDist,
+                           std::size_t maxNeighbors, float timeHorizon,
+                           float timeHorizonObst, float radius, float maxSpeed,
+                           const Vector2 &velocity)
+    : defaultAgent_(new Agent2D()),
+      kdTree_(new KdTree2D(this)),
+      globalTime_(0.0F),
+      timeStep_(timeStep) {
+  defaultAgent_->velocity_ = velocity;
+  defaultAgent_->maxNeighbors_ = maxNeighbors;
+  defaultAgent_->maxSpeed_ = maxSpeed;
+  defaultAgent_->neighborDist_ = neighborDist;
+  defaultAgent_->radius_ = radius;
+  defaultAgent_->timeHorizon_ = timeHorizon;
+  defaultAgent_->timeHorizonObst_ = timeHorizonObst;
+}
+
+RVOSimulator2D::~RVOSimulator2D() {
+  delete defaultAgent_;
+  delete kdTree_;
+
+  for (std::size_t i = 0U; i < agents_.size(); ++i) {
+    delete agents_[i];
+  }
+
+  for (std::size_t i = 0U; i < obstacles_.size(); ++i) {
+    delete obstacles_[i];
+  }
+}
+
+std::size_t RVOSimulator2D::addAgent(const Vector2 &position) {
+  if (defaultAgent_ != NULL) {
+    Agent2D *const agent = new Agent2D();
+    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 RVO2D_ERROR;
+}
+
+std::size_t RVOSimulator2D::addAgent(const Vector2 &position, float neighborDist,
+                                   std::size_t maxNeighbors, float timeHorizon,
+                                   float timeHorizonObst, float radius,
+                                   float maxSpeed) {
+  return addAgent(position, neighborDist, maxNeighbors, timeHorizon,
+                  timeHorizonObst, radius, maxSpeed, Vector2());
+}
+
+std::size_t RVOSimulator2D::addAgent(const Vector2 &position, float neighborDist,
+                                   std::size_t maxNeighbors, float timeHorizon,
+                                   float timeHorizonObst, float radius,
+                                   float maxSpeed, const Vector2 &velocity) {
+  Agent2D *const agent = new Agent2D();
+  agent->position_ = position;
+  agent->velocity_ = velocity;
+  agent->id_ = agents_.size();
+  agent->maxNeighbors_ = maxNeighbors;
+  agent->maxSpeed_ = maxSpeed;
+  agent->neighborDist_ = neighborDist;
+  agent->radius_ = radius;
+  agent->timeHorizon_ = timeHorizon;
+  agent->timeHorizonObst_ = timeHorizonObst;
+  agents_.push_back(agent);
+
+  return agents_.size() - 1U;
+}
+
+std::size_t RVOSimulator2D::addObstacle(const std::vector<Vector2> &vertices) {
+  if (vertices.size() > 1U) {
+    const std::size_t obstacleNo = obstacles_.size();
+
+    for (std::size_t i = 0U; i < vertices.size(); ++i) {
+      Obstacle2D *const obstacle = new Obstacle2D();
+      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);
+    }
+
+    return obstacleNo;
+  }
+
+  return RVO2D_ERROR;
+}
+
+void RVOSimulator2D::doStep() {
+  kdTree_->buildAgentTree(agents_);
+
+#ifdef _OPENMP
+#pragma omp parallel for
+#endif /* _OPENMP */
+  for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
+    agents_[i]->computeNeighbors(kdTree_);
+    agents_[i]->computeNewVelocity(timeStep_);
+  }
+
+#ifdef _OPENMP
+#pragma omp parallel for
+#endif /* _OPENMP */
+  for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
+    agents_[i]->update(timeStep_);
+  }
+
+  globalTime_ += timeStep_;
+}
+
+std::size_t RVOSimulator2D::getAgentAgentNeighbor(std::size_t agentNo,
+                                                std::size_t neighborNo) const {
+  return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_;
+}
+
+std::size_t RVOSimulator2D::getAgentMaxNeighbors(std::size_t agentNo) const {
+  return agents_[agentNo]->maxNeighbors_;
+}
+
+float RVOSimulator2D::getAgentMaxSpeed(std::size_t agentNo) const {
+  return agents_[agentNo]->maxSpeed_;
+}
+
+float RVOSimulator2D::getAgentNeighborDist(std::size_t agentNo) const {
+  return agents_[agentNo]->neighborDist_;
+}
+
+std::size_t RVOSimulator2D::getAgentNumAgentNeighbors(std::size_t agentNo) const {
+  return agents_[agentNo]->agentNeighbors_.size();
+}
+
+std::size_t RVOSimulator2D::getAgentNumObstacleNeighbors(
+    std::size_t agentNo) const {
+  return agents_[agentNo]->obstacleNeighbors_.size();
+}
+
+std::size_t RVOSimulator2D::getAgentNumORCALines(std::size_t agentNo) const {
+  return agents_[agentNo]->orcaLines_.size();
+}
+
+std::size_t RVOSimulator2D::getAgentObstacleNeighbor(
+    std::size_t agentNo, std::size_t neighborNo) const {
+  return agents_[agentNo]->obstacleNeighbors_[neighborNo].second->id_;
+}
+
+const Line &RVOSimulator2D::getAgentORCALine(std::size_t agentNo,
+                                           std::size_t lineNo) const {
+  return agents_[agentNo]->orcaLines_[lineNo];
+}
+
+const Vector2 &RVOSimulator2D::getAgentPosition(std::size_t agentNo) const {
+  return agents_[agentNo]->position_;
+}
+
+const Vector2 &RVOSimulator2D::getAgentPrefVelocity(std::size_t agentNo) const {
+  return agents_[agentNo]->prefVelocity_;
+}
+
+float RVOSimulator2D::getAgentRadius(std::size_t agentNo) const {
+  return agents_[agentNo]->radius_;
+}
+
+float RVOSimulator2D::getAgentTimeHorizon(std::size_t agentNo) const {
+  return agents_[agentNo]->timeHorizon_;
+}
+
+float RVOSimulator2D::getAgentTimeHorizonObst(std::size_t agentNo) const {
+  return agents_[agentNo]->timeHorizonObst_;
+}
+
+const Vector2 &RVOSimulator2D::getAgentVelocity(std::size_t agentNo) const {
+  return agents_[agentNo]->velocity_;
+}
+
+const Vector2 &RVOSimulator2D::getObstacleVertex(std::size_t vertexNo) const {
+  return obstacles_[vertexNo]->point_;
+}
+
+std::size_t RVOSimulator2D::getNextObstacleVertexNo(std::size_t vertexNo) const {
+  return obstacles_[vertexNo]->next_->id_;
+}
+
+std::size_t RVOSimulator2D::getPrevObstacleVertexNo(std::size_t vertexNo) const {
+  return obstacles_[vertexNo]->previous_->id_;
+}
+
+void RVOSimulator2D::processObstacles() { kdTree_->buildObstacleTree(obstacles_); }
+
+bool RVOSimulator2D::queryVisibility(const Vector2 &point1,
+                                   const Vector2 &point2) const {
+  return kdTree_->queryVisibility(point1, point2, 0.0F);
+}
+
+bool RVOSimulator2D::queryVisibility(const Vector2 &point1, const Vector2 &point2,
+                                   float radius) const {
+  return kdTree_->queryVisibility(point1, point2, radius);
+}
+
+void RVOSimulator2D::setAgentDefaults(float neighborDist,
+                                    std::size_t maxNeighbors, float timeHorizon,
+                                    float timeHorizonObst, float radius,
+                                    float maxSpeed) {
+  setAgentDefaults(neighborDist, maxNeighbors, timeHorizon, timeHorizonObst,
+                   radius, maxSpeed, Vector2());
+}
+
+void RVOSimulator2D::setAgentDefaults(float neighborDist,
+                                    std::size_t maxNeighbors, float timeHorizon,
+                                    float timeHorizonObst, float radius,
+                                    float maxSpeed, const Vector2 &velocity) {
+  if (defaultAgent_ == NULL) {
+    defaultAgent_ = new Agent2D();
+  }
+
+  defaultAgent_->maxNeighbors_ = maxNeighbors;
+  defaultAgent_->maxSpeed_ = maxSpeed;
+  defaultAgent_->neighborDist_ = neighborDist;
+  defaultAgent_->radius_ = radius;
+  defaultAgent_->timeHorizon_ = timeHorizon;
+  defaultAgent_->timeHorizonObst_ = timeHorizonObst;
+  defaultAgent_->velocity_ = velocity;
+}
+
+void RVOSimulator2D::setAgentMaxNeighbors(std::size_t agentNo,
+                                        std::size_t maxNeighbors) {
+  agents_[agentNo]->maxNeighbors_ = maxNeighbors;
+}
+
+void RVOSimulator2D::setAgentMaxSpeed(std::size_t agentNo, float maxSpeed) {
+  agents_[agentNo]->maxSpeed_ = maxSpeed;
+}
+
+void RVOSimulator2D::setAgentNeighborDist(std::size_t agentNo,
+                                        float neighborDist) {
+  agents_[agentNo]->neighborDist_ = neighborDist;
+}
+
+void RVOSimulator2D::setAgentPosition(std::size_t agentNo,
+                                    const Vector2 &position) {
+  agents_[agentNo]->position_ = position;
+}
+
+void RVOSimulator2D::setAgentPrefVelocity(std::size_t agentNo,
+                                        const Vector2 &prefVelocity) {
+  agents_[agentNo]->prefVelocity_ = prefVelocity;
+}
+
+void RVOSimulator2D::setAgentRadius(std::size_t agentNo, float radius) {
+  agents_[agentNo]->radius_ = radius;
+}
+
+void RVOSimulator2D::setAgentTimeHorizon(std::size_t agentNo, float timeHorizon) {
+  agents_[agentNo]->timeHorizon_ = timeHorizon;
+}
+
+void RVOSimulator2D::setAgentTimeHorizonObst(std::size_t agentNo,
+                                           float timeHorizonObst) {
+  agents_[agentNo]->timeHorizonObst_ = timeHorizonObst;
+}
+
+void RVOSimulator2D::setAgentVelocity(std::size_t agentNo,
+                                    const Vector2 &velocity) {
+  agents_[agentNo]->velocity_ = velocity;
+}
+} /* namespace RVO2D */

thirdparty/rvo2/rvo2_2d/RVOSimulator2d.cpp

@@ -1,363 +0,0 @@
-/*
- * RVOSimulator2d.cpp
- * RVO2 Library
- *
- * Copyright 2008 University of North Carolina at Chapel Hill
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
- * Please send all bug reports to <[email protected]>.
- *
- * The authors may be contacted via:
- *
- * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
- * Dept. of Computer Science
- * 201 S. Columbia St.
- * Frederick P. Brooks, Jr. Computer Science Bldg.
- * Chapel Hill, N.C. 27599-3175
- * United States of America
- *
- * <http://gamma.cs.unc.edu/RVO2/>
- */
-
-#include "RVOSimulator2d.h"
-
-#include "Agent2d.h"
-#include "KdTree2d.h"
-#include "Obstacle2d.h"
-
-#ifdef _OPENMP
-#include <omp.h>
-#endif
-
-namespace RVO2D {
-	RVOSimulator2D::RVOSimulator2D() : defaultAgent_(NULL), globalTime_(0.0f), kdTree_(NULL), timeStep_(0.0f)
-	{
-		kdTree_ = new KdTree2D(this);
-	}
-
-	RVOSimulator2D::RVOSimulator2D(float timeStep, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity) : defaultAgent_(NULL), globalTime_(0.0f), kdTree_(NULL), timeStep_(timeStep)
-	{
-		kdTree_ = new KdTree2D(this);
-		defaultAgent_ = new Agent2D();
-
-		defaultAgent_->maxNeighbors_ = maxNeighbors;
-		defaultAgent_->maxSpeed_ = maxSpeed;
-		defaultAgent_->neighborDist_ = neighborDist;
-		defaultAgent_->radius_ = radius;
-		defaultAgent_->timeHorizon_ = timeHorizon;
-		defaultAgent_->timeHorizonObst_ = timeHorizonObst;
-		defaultAgent_->velocity_ = velocity;
-	}
-
-	RVOSimulator2D::~RVOSimulator2D()
-	{
-		if (defaultAgent_ != NULL) {
-			delete defaultAgent_;
-		}
-
-		for (size_t i = 0; i < agents_.size(); ++i) {
-			delete agents_[i];
-		}
-
-		for (size_t i = 0; i < obstacles_.size(); ++i) {
-			delete obstacles_[i];
-		}
-
-		delete kdTree_;
-	}
-
-	size_t RVOSimulator2D::addAgent(const Vector2 &position)
-	{
-		if (defaultAgent_ == NULL) {
-			return RVO2D_ERROR;
-		}
-
-		Agent2D *agent = new Agent2D();
-
-		agent->position_ = position;
-		agent->maxNeighbors_ = defaultAgent_->maxNeighbors_;
-		agent->maxSpeed_ = defaultAgent_->maxSpeed_;
-		agent->neighborDist_ = defaultAgent_->neighborDist_;
-		agent->radius_ = defaultAgent_->radius_;
-		agent->timeHorizon_ = defaultAgent_->timeHorizon_;
-		agent->timeHorizonObst_ = defaultAgent_->timeHorizonObst_;
-		agent->velocity_ = defaultAgent_->velocity_;
-
-		agent->id_ = agents_.size();
-
-		agents_.push_back(agent);
-
-		return agents_.size() - 1;
-	}
-
-	size_t RVOSimulator2D::addAgent(const Vector2 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity)
-	{
-		Agent2D *agent = new Agent2D();
-
-		agent->position_ = position;
-		agent->maxNeighbors_ = maxNeighbors;
-		agent->maxSpeed_ = maxSpeed;
-		agent->neighborDist_ = neighborDist;
-		agent->radius_ = radius;
-		agent->timeHorizon_ = timeHorizon;
-		agent->timeHorizonObst_ = timeHorizonObst;
-		agent->velocity_ = velocity;
-
-		agent->id_ = agents_.size();
-
-		agents_.push_back(agent);
-
-		return agents_.size() - 1;
-	}
-
-	size_t RVOSimulator2D::addObstacle(const std::vector<Vector2> &vertices)
-	{
-		if (vertices.size() < 2) {
-			return RVO2D_ERROR;
-		}
-
-		const size_t obstacleNo = obstacles_.size();
-
-		for (size_t i = 0; i < vertices.size(); ++i) {
-			Obstacle2D *obstacle = new Obstacle2D();
-			obstacle->point_ = vertices[i];
-
-			if (i != 0) {
-				obstacle->prevObstacle_ = obstacles_.back();
-				obstacle->prevObstacle_->nextObstacle_ = obstacle;
-			}
-
-			if (i == vertices.size() - 1) {
-				obstacle->nextObstacle_ = obstacles_[obstacleNo];
-				obstacle->nextObstacle_->prevObstacle_ = obstacle;
-			}
-
-			obstacle->unitDir_ = normalize(vertices[(i == vertices.size() - 1 ? 0 : i + 1)] - vertices[i]);
-
-			if (vertices.size() == 2) {
-				obstacle->isConvex_ = true;
-			}
-			else {
-				obstacle->isConvex_ = (leftOf(vertices[(i == 0 ? vertices.size() - 1 : i - 1)], vertices[i], vertices[(i == vertices.size() - 1 ? 0 : i + 1)]) >= 0.0f);
-			}
-
-			obstacle->id_ = obstacles_.size();
-
-			obstacles_.push_back(obstacle);
-		}
-
-		return obstacleNo;
-	}
-
-	void RVOSimulator2D::doStep()
-	{
-		kdTree_->buildAgentTree(agents_);
-
-		for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
-			agents_[i]->computeNeighbors(this);
-			agents_[i]->computeNewVelocity(this);
-		}
-
-		for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
-			agents_[i]->update(this);
-		}
-
-		globalTime_ += timeStep_;
-	}
-
-	size_t RVOSimulator2D::getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const
-	{
-		return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_;
-	}
-
-	size_t RVOSimulator2D::getAgentMaxNeighbors(size_t agentNo) const
-	{
-		return agents_[agentNo]->maxNeighbors_;
-	}
-
-	float RVOSimulator2D::getAgentMaxSpeed(size_t agentNo) const
-	{
-		return agents_[agentNo]->maxSpeed_;
-	}
-
-	float RVOSimulator2D::getAgentNeighborDist(size_t agentNo) const
-	{
-		return agents_[agentNo]->neighborDist_;
-	}
-
-	size_t RVOSimulator2D::getAgentNumAgentNeighbors(size_t agentNo) const
-	{
-		return agents_[agentNo]->agentNeighbors_.size();
-	}
-
-	size_t RVOSimulator2D::getAgentNumObstacleNeighbors(size_t agentNo) const
-	{
-		return agents_[agentNo]->obstacleNeighbors_.size();
-	}
-
-	size_t RVOSimulator2D::getAgentNumORCALines(size_t agentNo) const
-	{
-		return agents_[agentNo]->orcaLines_.size();
-	}
-
-	size_t RVOSimulator2D::getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const
-	{
-		return agents_[agentNo]->obstacleNeighbors_[neighborNo].second->id_;
-	}
-
-	const Line &RVOSimulator2D::getAgentORCALine(size_t agentNo, size_t lineNo) const
-	{
-		return agents_[agentNo]->orcaLines_[lineNo];
-	}
-
-	const Vector2 &RVOSimulator2D::getAgentPosition(size_t agentNo) const
-	{
-		return agents_[agentNo]->position_;
-	}
-
-	const Vector2 &RVOSimulator2D::getAgentPrefVelocity(size_t agentNo) const
-	{
-		return agents_[agentNo]->prefVelocity_;
-	}
-
-	float RVOSimulator2D::getAgentRadius(size_t agentNo) const
-	{
-		return agents_[agentNo]->radius_;
-	}
-
-	float RVOSimulator2D::getAgentTimeHorizon(size_t agentNo) const
-	{
-		return agents_[agentNo]->timeHorizon_;
-	}
-
-	float RVOSimulator2D::getAgentTimeHorizonObst(size_t agentNo) const
-	{
-		return agents_[agentNo]->timeHorizonObst_;
-	}
-
-	const Vector2 &RVOSimulator2D::getAgentVelocity(size_t agentNo) const
-	{
-		return agents_[agentNo]->velocity_;
-	}
-
-	float RVOSimulator2D::getGlobalTime() const
-	{
-		return globalTime_;
-	}
-
-	size_t RVOSimulator2D::getNumAgents() const
-	{
-		return agents_.size();
-	}
-
-	size_t RVOSimulator2D::getNumObstacleVertices() const
-	{
-		return obstacles_.size();
-	}
-
-	const Vector2 &RVOSimulator2D::getObstacleVertex(size_t vertexNo) const
-	{
-		return obstacles_[vertexNo]->point_;
-	}
-
-	size_t RVOSimulator2D::getNextObstacleVertexNo(size_t vertexNo) const
-	{
-		return obstacles_[vertexNo]->nextObstacle_->id_;
-	}
-
-	size_t RVOSimulator2D::getPrevObstacleVertexNo(size_t vertexNo) const
-	{
-		return obstacles_[vertexNo]->prevObstacle_->id_;
-	}
-
-	float RVOSimulator2D::getTimeStep() const
-	{
-		return timeStep_;
-	}
-
-	void RVOSimulator2D::processObstacles()
-	{
-		kdTree_->buildObstacleTree(obstacles_);
-	}
-
-	bool RVOSimulator2D::queryVisibility(const Vector2 &point1, const Vector2 &point2, float radius) const
-	{
-		return kdTree_->queryVisibility(point1, point2, radius);
-	}
-
-	void RVOSimulator2D::setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity)
-	{
-		if (defaultAgent_ == NULL) {
-			defaultAgent_ = new Agent2D();
-		}
-
-		defaultAgent_->maxNeighbors_ = maxNeighbors;
-		defaultAgent_->maxSpeed_ = maxSpeed;
-		defaultAgent_->neighborDist_ = neighborDist;
-		defaultAgent_->radius_ = radius;
-		defaultAgent_->timeHorizon_ = timeHorizon;
-		defaultAgent_->timeHorizonObst_ = timeHorizonObst;
-		defaultAgent_->velocity_ = velocity;
-	}
-
-	void RVOSimulator2D::setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors)
-	{
-		agents_[agentNo]->maxNeighbors_ = maxNeighbors;
-	}
-
-	void RVOSimulator2D::setAgentMaxSpeed(size_t agentNo, float maxSpeed)
-	{
-		agents_[agentNo]->maxSpeed_ = maxSpeed;
-	}
-
-	void RVOSimulator2D::setAgentNeighborDist(size_t agentNo, float neighborDist)
-	{
-		agents_[agentNo]->neighborDist_ = neighborDist;
-	}
-
-	void RVOSimulator2D::setAgentPosition(size_t agentNo, const Vector2 &position)
-	{
-		agents_[agentNo]->position_ = position;
-	}
-
-	void RVOSimulator2D::setAgentPrefVelocity(size_t agentNo, const Vector2 &prefVelocity)
-	{
-		agents_[agentNo]->prefVelocity_ = prefVelocity;
-	}
-
-	void RVOSimulator2D::setAgentRadius(size_t agentNo, float radius)
-	{
-		agents_[agentNo]->radius_ = radius;
-	}
-
-	void RVOSimulator2D::setAgentTimeHorizon(size_t agentNo, float timeHorizon)
-	{
-		agents_[agentNo]->timeHorizon_ = timeHorizon;
-	}
-
-	void RVOSimulator2D::setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst)
-	{
-		agents_[agentNo]->timeHorizonObst_ = timeHorizonObst;
-	}
-
-	void RVOSimulator2D::setAgentVelocity(size_t agentNo, const Vector2 &velocity)
-	{
-		agents_[agentNo]->velocity_ = velocity;
-	}
-
-	void RVOSimulator2D::setTimeStep(float timeStep)
-	{
-		timeStep_ = timeStep;
-	}
-}

thirdparty/rvo2/rvo2_2d/RVOSimulator2d.h

@@ -1,14 +1,15 @@
 /*
- * RVOSimulator2d.h
+ * RVOSimulator.h
  * RVO2 Library
  *
- * Copyright 2008 University of North Carolina at Chapel Hill
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
- *     http://www.apache.org/licenses/LICENSE-2.0
+ *     https://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
@@ -27,566 +28,638 @@
  * Chapel Hill, N.C. 27599-3175
  * United States of America
  *
- * <http://gamma.cs.unc.edu/RVO2/>
+ * <https://gamma.cs.unc.edu/RVO2/>
  */
 
 #ifndef RVO2D_RVO_SIMULATOR_H_
 #define RVO2D_RVO_SIMULATOR_H_
 
 /**
- * \file       RVOSimulator2d.h
- * \brief      Contains the RVOSimulator2D class.
+ * @file  RVOSimulator2d.h
+ * @brief Declares and defines the RVOSimulator2D class.
  */
 
 #include <cstddef>
-#include <limits>
 #include <vector>
 
-#include "Vector2.h"
 
 namespace RVO2D {
-	/**
-	 * \brief       Error value.
-	 *
-	 * A value equal to the largest unsigned integer that is returned in case
-	 * of an error by functions in RVO2D::RVOSimulator2D.
-	 */
-	const size_t RVO2D_ERROR = std::numeric_limits<size_t>::max();
-
-	/**
-	 * \brief      Defines a directed line.
-	 */
-	class Line {
-	public:
-		/**
-		 * \brief     A point on the directed line.
-		 */
-		Vector2 point;
-
-		/**
-		 * \brief     The direction of the directed line.
-		 */
-		Vector2 direction;
-	};
-
-	class Agent2D;
-	class KdTree2D;
-	class Obstacle2D;
-
-	/**
-	 * \brief      Defines the simulation.
-	 *
-	 * The main class of the library that contains all simulation functionality.
-	 */
-	class RVOSimulator2D {
-	public:
-		/**
-		 * \brief      Constructs a simulator instance.
-		 */
-		RVOSimulator2D();
-
-		/**
-		 * \brief      Constructs a simulator instance and sets the default
-		 *             properties for any new agent that is added.
-		 * \param      timeStep        The time step of the simulation.
-		 *                             Must be positive.
-		 * \param      neighborDist    The default maximum distance (center point
-		 *                             to center point) to other agents a new agent
-		 *                             takes into account in the navigation. The
-		 *                             larger this number, the longer he running
-		 *                             time of the simulation. If the number is too
-		 *                             low, the simulation will not be safe. Must be
-		 *                             non-negative.
-		 * \param      maxNeighbors    The default maximum number of other agents a
-		 *                             new agent takes into account in the
-		 *                             navigation. The larger this number, the
-		 *                             longer the running time of the simulation.
-		 *                             If the number is too low, the simulation
-		 *                             will not be safe.
-		 * \param      timeHorizon     The default minimal amount of time for which
-		 *                             a new agent's velocities that are computed
-		 *                             by the simulation are safe with respect to
-		 *                             other agents. The larger this number, the
-		 *                             sooner an agent will respond to the presence
-		 *                             of other agents, but the less freedom the
-		 *                             agent has in choosing its velocities.
-		 *                             Must be positive.
-		 * \param      timeHorizonObst The default minimal amount of time for which
-		 *                             a new agent's velocities that are computed
-		 *                             by the simulation are safe with respect to
-		 *                             obstacles. The larger this number, the
-		 *                             sooner an agent will respond to the presence
-		 *                             of obstacles, but the less freedom the agent
-		 *                             has in choosing its velocities.
-		 *                             Must be positive.
-		 * \param      radius          The default radius of a new agent.
-		 *                             Must be non-negative.
-		 * \param      maxSpeed        The default maximum speed of a new agent.
-		 *                             Must be non-negative.
-		 * \param      velocity        The default initial two-dimensional linear
-		 *                             velocity of a new agent (optional).
-		 */
-		RVOSimulator2D(float timeStep, float neighborDist, size_t maxNeighbors,
-					 float timeHorizon, float timeHorizonObst, float radius,
-					 float maxSpeed, const Vector2 &velocity = Vector2());
-
-		/**
-		 * \brief      Destroys this simulator instance.
-		 */
-		~RVOSimulator2D();
-
-		/**
-		 * \brief      Adds a new agent with default properties to the
-		 *             simulation.
-		 * \param      position        The two-dimensional starting position of
-		 *                             this agent.
-		 * \return     The number of the agent, or RVO2D::RVO2D_ERROR when the agent
-		 *             defaults have not been set.
-		 */
-		size_t addAgent(const Vector2 &position);
-
-		/**
-		 * \brief      Adds a new agent to the simulation.
-		 * \param      position        The two-dimensional starting position of
-		 *                             this agent.
-		 * \param      neighborDist    The maximum distance (center point to
-		 *                             center point) to other agents this agent
-		 *                             takes into account in the navigation. The
-		 *                             larger this number, the longer the running
-		 *                             time of the simulation. If the number is too
-		 *                             low, the simulation will not be safe.
-		 *                             Must be non-negative.
-		 * \param      maxNeighbors    The maximum number of other agents this
-		 *                             agent takes into account in the navigation.
-		 *                             The larger this number, the longer the
-		 *                             running time of the simulation. If the
-		 *                             number is too low, the simulation will not
-		 *                             be safe.
-		 * \param      timeHorizon     The minimal amount of time for which this
-		 *                             agent's velocities that are computed by the
-		 *                             simulation are safe with respect to other
-		 *                             agents. The larger this number, the sooner
-		 *                             this agent will respond to the presence of
-		 *                             other agents, but the less freedom this
-		 *                             agent has in choosing its velocities.
-		 *                             Must be positive.
-		 * \param      timeHorizonObst The minimal amount of time for which this
-		 *                             agent's velocities that are computed by the
-		 *                             simulation are safe with respect to
-		 *                             obstacles. The larger this number, the
-		 *                             sooner this agent will respond to the
-		 *                             presence of obstacles, but the less freedom
-		 *                             this agent has in choosing its velocities.
-		 *                             Must be positive.
-		 * \param      radius          The radius of this agent.
-		 *                             Must be non-negative.
-		 * \param      maxSpeed        The maximum speed of this agent.
-		 *                             Must be non-negative.
-		 * \param      velocity        The initial two-dimensional linear velocity
-		 *                             of this agent (optional).
-		 * \return     The number of the agent.
-		 */
-		size_t addAgent(const Vector2 &position, float neighborDist,
-						size_t maxNeighbors, float timeHorizon,
-						float timeHorizonObst, float radius, float maxSpeed,
-						const Vector2 &velocity = Vector2());
-
-		/**
-		 * \brief      Adds a new obstacle to the simulation.
-		 * \param      vertices        List of the vertices of the polygonal
-		 *             obstacle in counterclockwise order.
-		 * \return     The number of the first vertex of the obstacle,
-		 *             or RVO2D::RVO2D_ERROR when the number of vertices is less than two.
-		 * \note       To add a "negative" obstacle, e.g. a bounding polygon around
-		 *             the environment, the vertices should be listed in clockwise
-		 *             order.
-		 */
-		size_t addObstacle(const std::vector<Vector2> &vertices);
-
-		/**
-		 * \brief      Lets the simulator perform a simulation step and updates the
-		 *             two-dimensional position and two-dimensional velocity of
-		 *             each agent.
-		 */
-		void doStep();
-
-		/**
-		 * \brief      Returns the specified agent neighbor of the specified
-		 *             agent.
-		 * \param      agentNo         The number of the agent whose agent
-		 *                             neighbor is to be retrieved.
-		 * \param      neighborNo      The number of the agent neighbor to be
-		 *                             retrieved.
-		 * \return     The number of the neighboring agent.
-		 */
-		size_t getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const;
-
-		/**
-		 * \brief      Returns the maximum neighbor count of a specified agent.
-		 * \param      agentNo         The number of the agent whose maximum
-		 *                             neighbor count is to be retrieved.
-		 * \return     The present maximum neighbor count of the agent.
-		 */
-		size_t getAgentMaxNeighbors(size_t agentNo) const;
-
-		/**
-		 * \brief      Returns the maximum speed of a specified agent.
-		 * \param      agentNo         The number of the agent whose maximum speed
-		 *                             is to be retrieved.
-		 * \return     The present maximum speed of the agent.
-		 */
-		float getAgentMaxSpeed(size_t agentNo) const;
-
-		/**
-		 * \brief      Returns the maximum neighbor distance of a specified
-		 *             agent.
-		 * \param      agentNo         The number of the agent whose maximum
-		 *                             neighbor distance is to be retrieved.
-		 * \return     The present maximum neighbor distance of the agent.
-		 */
-		float getAgentNeighborDist(size_t agentNo) const;
-
-		/**
-		 * \brief      Returns the count of agent neighbors taken into account to
-		 *             compute the current velocity for the specified agent.
-		 * \param      agentNo         The number of the agent whose count of agent
-		 *                             neighbors is to be retrieved.
-		 * \return     The count of agent neighbors taken into account to compute
-		 *             the current velocity for the specified agent.
-		 */
-		size_t getAgentNumAgentNeighbors(size_t agentNo) const;
-
-		/**
-		 * \brief      Returns the count of obstacle neighbors taken into account
-		 *             to compute the current velocity for the specified agent.
-		 * \param      agentNo         The number of the agent whose count of
-		 *                             obstacle neighbors is to be retrieved.
-		 * \return     The count of obstacle neighbors taken into account to
-		 *             compute the current velocity for the specified agent.
-		 */
-		size_t getAgentNumObstacleNeighbors(size_t agentNo) const;
-
-
-		/**
-		 * \brief      Returns the count of ORCA constraints used to compute
-		 *             the current velocity for the specified agent.
-		 * \param      agentNo         The number of the agent whose count of ORCA
-		 *                             constraints is to be retrieved.
-		 * \return     The count of ORCA constraints used to compute the current
-		 *             velocity for the specified agent.
-		 */
-		size_t getAgentNumORCALines(size_t agentNo) const;
-
-		/**
-		 * \brief      Returns the specified obstacle neighbor of the specified
-		 *             agent.
-		 * \param      agentNo         The number of the agent whose obstacle
-		 *                             neighbor is to be retrieved.
-		 * \param      neighborNo      The number of the obstacle neighbor to be
-		 *                             retrieved.
-		 * \return     The number of the first vertex of the neighboring obstacle
-		 *             edge.
-		 */
-		size_t getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const;
-
-		/**
-		 * \brief      Returns the specified ORCA constraint of the specified
-		 *             agent.
-		 * \param      agentNo         The number of the agent whose ORCA
-		 *                             constraint is to be retrieved.
-		 * \param      lineNo          The number of the ORCA constraint to be
-		 *                             retrieved.
-		 * \return     A line representing the specified ORCA constraint.
-		 * \note       The halfplane to the left of the line is the region of
-		 *             permissible velocities with respect to the specified
-		 *             ORCA constraint.
-		 */
-		const Line &getAgentORCALine(size_t agentNo, size_t lineNo) const;
-
-		/**
-		 * \brief      Returns the two-dimensional position of a specified
-		 *             agent.
-		 * \param      agentNo         The number of the agent whose
-		 *                             two-dimensional position is to be retrieved.
-		 * \return     The present two-dimensional position of the (center of the)
-		 *             agent.
-		 */
-		const Vector2 &getAgentPosition(size_t agentNo) const;
-
-		/**
-		 * \brief      Returns the two-dimensional preferred velocity of a
-		 *             specified agent.
-		 * \param      agentNo         The number of the agent whose
-		 *                             two-dimensional preferred velocity is to be
-		 *                             retrieved.
-		 * \return     The present two-dimensional preferred velocity of the agent.
-		 */
-		const Vector2 &getAgentPrefVelocity(size_t agentNo) const;
-
-		/**
-		 * \brief      Returns the radius of a specified agent.
-		 * \param      agentNo         The number of the agent whose radius is to
-		 *                             be retrieved.
-		 * \return     The present radius of the agent.
-		 */
-		float getAgentRadius(size_t agentNo) const;
-
-		/**
-		 * \brief      Returns the time horizon of a specified agent.
-		 * \param      agentNo         The number of the agent whose time horizon
-		 *                             is to be retrieved.
-		 * \return     The present time horizon of the agent.
-		 */
-		float getAgentTimeHorizon(size_t agentNo) const;
-
-		/**
-		 * \brief      Returns the time horizon with respect to obstacles of a
-		 *             specified agent.
-		 * \param      agentNo         The number of the agent whose time horizon
-		 *                             with respect to obstacles is to be
-		 *                             retrieved.
-		 * \return     The present time horizon with respect to obstacles of the
-		 *             agent.
-		 */
-		float getAgentTimeHorizonObst(size_t agentNo) const;
-
-		/**
-		 * \brief      Returns the two-dimensional linear velocity of a
-		 *             specified agent.
-		 * \param      agentNo         The number of the agent whose
-		 *                             two-dimensional linear velocity is to be
-		 *                             retrieved.
-		 * \return     The present two-dimensional linear velocity of the agent.
-		 */
-		const Vector2 &getAgentVelocity(size_t agentNo) const;
-
-		/**
-		 * \brief      Returns the global time of the simulation.
-		 * \return     The present global time of the simulation (zero initially).
-		 */
-		float getGlobalTime() const;
-
-		/**
-		 * \brief      Returns the count of agents in the simulation.
-		 * \return     The count of agents in the simulation.
-		 */
-		size_t getNumAgents() const;
-
-		/**
-		 * \brief      Returns the count of obstacle vertices in the simulation.
-		 * \return     The count of obstacle vertices in the simulation.
-		 */
-		size_t getNumObstacleVertices() const;
-
-		/**
-		 * \brief      Returns the two-dimensional position of a specified obstacle
-		 *             vertex.
-		 * \param      vertexNo        The number of the obstacle vertex to be
-		 *                             retrieved.
-		 * \return     The two-dimensional position of the specified obstacle
-		 *             vertex.
-		 */
-		const Vector2 &getObstacleVertex(size_t vertexNo) const;
-
-		/**
-		 * \brief      Returns the number of the obstacle vertex succeeding the
-		 *             specified obstacle vertex in its polygon.
-		 * \param      vertexNo        The number of the obstacle vertex whose
-		 *                             successor is to be retrieved.
-		 * \return     The number of the obstacle vertex succeeding the specified
-		 *             obstacle vertex in its polygon.
-		 */
-		size_t getNextObstacleVertexNo(size_t vertexNo) const;
-
-		/**
-		 * \brief      Returns the number of the obstacle vertex preceding the
-		 *             specified obstacle vertex in its polygon.
-		 * \param      vertexNo        The number of the obstacle vertex whose
-		 *                             predecessor is to be retrieved.
-		 * \return     The number of the obstacle vertex preceding the specified
-		 *             obstacle vertex in its polygon.
-		 */
-		size_t getPrevObstacleVertexNo(size_t vertexNo) const;
-
-		/**
-		 * \brief      Returns the time step of the simulation.
-		 * \return     The present time step of the simulation.
-		 */
-		float getTimeStep() const;
-
-		/**
-		 * \brief      Processes the obstacles that have been added so that they
-		 *             are accounted for in the simulation.
-		 * \note       Obstacles added to the simulation after this function has
-		 *             been called are not accounted for in the simulation.
-		 */
-		void processObstacles();
-
-		/**
-		 * \brief      Performs a visibility query between the two specified
-		 *             points with respect to the obstacles
-		 * \param      point1          The first point of the query.
-		 * \param      point2          The second point of the query.
-		 * \param      radius          The minimal distance between the line
-		 *                             connecting the two points and the obstacles
-		 *                             in order for the points to be mutually
-		 *                             visible (optional). Must be non-negative.
-		 * \return     A boolean specifying whether the two points are mutually
-		 *             visible. Returns true when the obstacles have not been
-		 *             processed.
-		 */
-		bool queryVisibility(const Vector2 &point1, const Vector2 &point2,
-							 float radius = 0.0f) const;
-
-		/**
-		 * \brief      Sets the default properties for any new agent that is
-		 *             added.
-		 * \param      neighborDist    The default maximum distance (center point
-		 *                             to center point) to other agents a new agent
-		 *                             takes into account in the navigation. The
-		 *                             larger this number, the longer he running
-		 *                             time of the simulation. If the number is too
-		 *                             low, the simulation will not be safe.
-		 *                             Must be non-negative.
-		 * \param      maxNeighbors    The default maximum number of other agents a
-		 *                             new agent takes into account in the
-		 *                             navigation. The larger this number, the
-		 *                             longer the running time of the simulation.
-		 *                             If the number is too low, the simulation
-		 *                             will not be safe.
-		 * \param      timeHorizon     The default minimal amount of time for which
-		 *                             a new agent's velocities that are computed
-		 *                             by the simulation are safe with respect to
-		 *                             other agents. The larger this number, the
-		 *                             sooner an agent will respond to the presence
-		 *                             of other agents, but the less freedom the
-		 *                             agent has in choosing its velocities.
-		 *                             Must be positive.
-		 * \param      timeHorizonObst The default minimal amount of time for which
-		 *                             a new agent's velocities that are computed
-		 *                             by the simulation are safe with respect to
-		 *                             obstacles. The larger this number, the
-		 *                             sooner an agent will respond to the presence
-		 *                             of obstacles, but the less freedom the agent
-		 *                             has in choosing its velocities.
-		 *                             Must be positive.
-		 * \param      radius          The default radius of a new agent.
-		 *                             Must be non-negative.
-		 * \param      maxSpeed        The default maximum speed of a new agent.
-		 *                             Must be non-negative.
-		 * \param      velocity        The default initial two-dimensional linear
-		 *                             velocity of a new agent (optional).
-		 */
-		void setAgentDefaults(float neighborDist, size_t maxNeighbors,
-							  float timeHorizon, float timeHorizonObst,
-							  float radius, float maxSpeed,
-							  const Vector2 &velocity = Vector2());
-
-		/**
-		 * \brief      Sets the maximum neighbor count of a specified agent.
-		 * \param      agentNo         The number of the agent whose maximum
-		 *                             neighbor count is to be modified.
-		 * \param      maxNeighbors    The replacement maximum neighbor count.
-		 */
-		void setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors);
-
-		/**
-		 * \brief      Sets the maximum speed of a specified agent.
-		 * \param      agentNo         The number of the agent whose maximum speed
-		 *                             is to be modified.
-		 * \param      maxSpeed        The replacement maximum speed. Must be
-		 *                             non-negative.
-		 */
-		void setAgentMaxSpeed(size_t agentNo, float maxSpeed);
-
-		/**
-		 * \brief      Sets the maximum neighbor distance of a specified agent.
-		 * \param      agentNo         The number of the agent whose maximum
-		 *                             neighbor distance is to be modified.
-		 * \param      neighborDist    The replacement maximum neighbor distance.
-		 *                             Must be non-negative.
-		 */
-		void setAgentNeighborDist(size_t agentNo, float neighborDist);
-
-		/**
-		 * \brief      Sets the two-dimensional position of a specified agent.
-		 * \param      agentNo         The number of the agent whose
-		 *                             two-dimensional position is to be modified.
-		 * \param      position        The replacement of the two-dimensional
-		 *                             position.
-		 */
-		void setAgentPosition(size_t agentNo, const Vector2 &position);
-
-		/**
-		 * \brief      Sets the two-dimensional preferred velocity of a
-		 *             specified agent.
-		 * \param      agentNo         The number of the agent whose
-		 *                             two-dimensional preferred velocity is to be
-		 *                             modified.
-		 * \param      prefVelocity    The replacement of the two-dimensional
-		 *                             preferred velocity.
-		 */
-		void setAgentPrefVelocity(size_t agentNo, const Vector2 &prefVelocity);
-
-		/**
-		 * \brief      Sets the radius of a specified agent.
-		 * \param      agentNo         The number of the agent whose radius is to
-		 *                             be modified.
-		 * \param      radius          The replacement radius.
-		 *                             Must be non-negative.
-		 */
-		void setAgentRadius(size_t agentNo, float radius);
-
-		/**
-		 * \brief      Sets the time horizon of a specified agent with respect
-		 *             to other agents.
-		 * \param      agentNo         The number of the agent whose time horizon
-		 *                             is to be modified.
-		 * \param      timeHorizon     The replacement time horizon with respect
-		 *                             to other agents. Must be positive.
-		 */
-		void setAgentTimeHorizon(size_t agentNo, float timeHorizon);
-
-		/**
-		 * \brief      Sets the time horizon of a specified agent with respect
-		 *             to obstacles.
-		 * \param      agentNo         The number of the agent whose time horizon
-		 *                             with respect to obstacles is to be modified.
-		 * \param      timeHorizonObst The replacement time horizon with respect to
-		 *                             obstacles. Must be positive.
-		 */
-		void setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst);
-
-		/**
-		 * \brief      Sets the two-dimensional linear velocity of a specified
-		 *             agent.
-		 * \param      agentNo         The number of the agent whose
-		 *                             two-dimensional linear velocity is to be
-		 *                             modified.
-		 * \param      velocity        The replacement two-dimensional linear
-		 *                             velocity.
-		 */
-		void setAgentVelocity(size_t agentNo, const Vector2 &velocity);
-
-		/**
-		 * \brief      Sets the time step of the simulation.
-		 * \param      timeStep        The time step of the simulation.
-		 *                             Must be positive.
-		 */
-		void setTimeStep(float timeStep);
-
-	public:
-		std::vector<Agent2D *> agents_;
-		Agent2D *defaultAgent_;
-		float globalTime_;
-		KdTree2D *kdTree_;
-		std::vector<Obstacle2D *> obstacles_;
-		float timeStep_;
-
-		friend class Agent2D;
-		friend class KdTree2D;
-		friend class Obstacle2D;
-	};
-}
+class Agent2D;
+class KdTree2D;
+class Line;
+class Obstacle2D;
+class Vector2;
+
+/**
+ * @relates RVOSimulator2D
+ * @brief   Error value. A value equal to the largest unsigned integer that is
+ *          returned in case of an error by functions in RVO::RVOSimulator.
+ */
+extern const std::size_t RVO2D_ERROR;
+
+/**
+ * @brief Defines the simulation. The main class of the library that contains
+ *        all simulation functionality.
+ */
+class RVOSimulator2D {
+ public:
+  /**
+   * @brief Constructs a simulator instance.
+   */
+  RVOSimulator2D();
+
+  /**
+   * @brief     Constructs a simulator instance and sets the default
+   *            properties for any new agent that is added.
+   * @param[in] timeStep        The time step of the simulation. Must be
+   *                            positive.
+   * @param[in] neighborDist    The default maximum distance center-point to
+   *                            center-point to other agents a new agent takes
+   *                            into account in the navigation. The larger this
+   *                            number, the longer he running time of the
+   *                            simulation. If the number is too low, the
+   *                            simulation will not be safe. Must be
+   *                            non-negative.
+   * @param[in] maxNeighbors    The default maximum number of other agents a
+   *                            new agent takes into account in the navigation.
+   *                            The larger this number, the longer the running
+   *                            time of the simulation. If the number is too
+   *                            low, the simulation will not be safe.
+   * @param[in] timeHorizon     The default minimal amount of time for which a
+   *                            new agent's velocities that are computed by the
+   *                            simulation are safe with respect to other
+   *                            agents. The larger this number, the sooner an
+   *                            agent will respond to the presence of other
+   *                            agents, but the less freedom the agent  has in
+   *                            choosing its velocities. Must be positive.
+   * @param[in] timeHorizonObst The default minimal amount of time for which a
+   *                            new agent's velocities that are computed by the
+   *                            simulation are safe with respect to obstacles.
+   *                            The larger this number, the sooner an agent will
+   *                            respond to the presence of obstacles, but the
+   *                            less freedom the agent has in choosing its
+   *                            velocities. Must be positive.
+   * @param[in] radius          The default radius of a new agent. Must be
+   *                            non-negative.
+   * @param[in] maxSpeed        The default maximum speed of a new agent. Must
+   *                            be non-negative.
+   */
+  RVOSimulator2D(float timeStep, float neighborDist, std::size_t maxNeighbors,
+               float timeHorizon, float timeHorizonObst, float radius,
+               float maxSpeed);
+
+  /**
+   * @brief     Constructs a simulator instance and sets the default properties
+   *            for any new agent that is added.
+   * @param[in] timeStep        The time step of the simulation. Must be
+   *                            positive.
+   * @param[in] neighborDist    The default maximum distance center-point to
+   *                            center-point to other agents a new agent takes
+   *                            into account in the navigation. The larger this
+   *                            number, the longer he running time of the
+   *                            simulation. If the number is too low, the
+   *                            simulation will not be safe. Must be
+   *                            non-negative.
+   * @param[in] maxNeighbors    The default maximum number of other agents a new
+   *                            agent takes into account in the navigation. The
+   *                            larger this number, the longer the running time
+   *                            of the simulation. If the number is too low, the
+   *                            simulation will not be safe.
+   * @param[in] timeHorizon     The default minimal amount of time for which a
+   *                            new agent's velocities that are computed by the
+   *                            simulation are safe with respect to other
+   *                            agents. The larger this number, the sooner an
+   *                            agent will respond to the presence of other
+   *                            agents, but the less freedom the agent has in
+   *                            choosing its velocities. Must be positive.
+   * @param[in] timeHorizonObst The default minimal amount of time for which a
+   *                            new agent's velocities that are computed by the
+   *                            simulation are safe with respect to obstacles.
+   *                            The larger this number, the sooner an agent will
+   *                            respond to the presence of obstacles, but the
+   *                            less freedom the agent has in choosing its
+   *                            velocities. Must be positive.
+   * @param[in] radius          The default radius of a new agent. Must be
+   *                            non-negative.
+   * @param[in] maxSpeed        The default maximum speed of a new agent. Must
+   *                            be non-negative.
+   * @param[in] velocity        The default initial two-dimensional linear
+   *                            velocity of a new agent.
+   */
+  RVOSimulator2D(float timeStep, float neighborDist, std::size_t maxNeighbors,
+               float timeHorizon, float timeHorizonObst, float radius,
+               float maxSpeed, const Vector2 &velocity);
+
+  /**
+   * @brief Destroys this simulator instance.
+   */
+  ~RVOSimulator2D();
+
+  /**
+   * @brief     Adds a new agent with default properties to the simulation.
+   * @param[in] position The two-dimensional starting position of this agent.
+   * @return    The number of the agent, or RVO::RVO2D_ERROR when the agent
+   *            defaults have not been set.
+   */
+  std::size_t addAgent(const Vector2 &position);
+
+  /**
+   * @brief     Adds a new agent to the simulation.
+   * @param[in] position        The two-dimensional starting position of this
+   *                            agent.
+   * @param[in] neighborDist    The maximum distance center-point to
+   *                            center-point to other agents this agent takes
+   *                            into account in the navigation. The larger this
+   *                            number, the longer the running time of the
+   *                            simulation. If the number is too low, the
+   *                            simulation will not be safe. Must be
+   *                            non-negative.
+   * @param[in] maxNeighbors    The maximum number of other agents this agent
+   *                            takes into account in the navigation. The larger
+   *                            this number, the longer the running time of the
+   *                            simulation. If the number is too low, the
+   *                            simulation will not be safe.
+   * @param[in] timeHorizon     The minimal amount of time for which this
+   *                            agent's velocities that are computed by the
+   *                            simulation are safe with respect to other
+   *                            agents. The larger this number, the sooner this
+   *                            agent will respond to the presence of other
+   *                            agents, but the less freedom this agent has in
+   *                            choosing its velocities. Must be positive.
+   * @param[in] timeHorizonObst The minimal amount of time for which this
+   *                            agent's velocities that are computed by the
+   *                            simulation are safe with respect to obstacles
+   *                            The larger this number, the sooner this agent
+   *                            will respond to the presence of obstacles, but
+   *                            the less freedom this agent has in choosing its
+   *                            velocities. Must be positive.
+   * @param[in] radius          The radius of this agent. Must be non-negative.
+   * @param[in] maxSpeed        The maximum speed of this agent. Must be
+   *                            non-negative.
+   * @return    The number of the agent.
+   */
+  std::size_t addAgent(const Vector2 &position, float neighborDist,
+                       std::size_t maxNeighbors, float timeHorizon,
+                       float timeHorizonObst, float radius, float maxSpeed);
+
+  /**
+   * @brief     Adds a new agent to the simulation.
+   * @param[in] position        The two-dimensional starting position of this
+   *                            agent.
+   * @param[in] neighborDist    The maximum distance center-point to
+   *                            center-point to other agents this agent takes
+   *                            into account in the navigation. The larger this
+   *                            number, the longer the running time of the
+   *                            simulation. If the number is too low, the
+   *                            simulation will not be safe. Must be
+   *                            non-negative.
+   * @param[in] maxNeighbors    The maximum number of other agents this agent
+   *                            takes into account in the navigation. The larger
+   *                            this number, the longer the running time of the
+   *                            simulation. If the number is too low, the
+   *                            simulation will not be safe.
+   * @param[in] timeHorizon     The minimal amount of time for which this
+   *                            agent's velocities that are computed by the
+   *                            simulation are safe with respect to other
+   *                            agents. The larger this number, the sooner this
+   *                            agent will respond to the presence of other
+   *                            agents, but the less freedom this agent has in
+   *                            choosing its velocities. Must be positive.
+   * @param[in] timeHorizonObst The minimal amount of time for which this
+   *                            agent's velocities that are computed by the
+   *                            simulation are safe with respect to obstacles.
+   *                            The larger this number, the sooner this agent
+   *                            will respond to the presence of obstacles, but
+   *                            the less freedom this agent has in choosing its
+   *                            velocities. Must be positive.
+   * @param[in] radius          The radius of this agent. Must be non-negative.
+   * @param[in] maxSpeed        The maximum speed of this agent. Must be
+   *                            non-negative.
+   * @param[in] velocity        The initial two-dimensional linear velocity of
+   *                            this agent.
+   * @return    The number of the agent.
+   */
+  std::size_t addAgent(const Vector2 &position, float neighborDist,
+                       std::size_t maxNeighbors, float timeHorizon,
+                       float timeHorizonObst, float radius, float maxSpeed,
+                       const Vector2 &velocity);
+
+  /**
+   * @brief     Adds a new obstacle to the simulation.
+   * @param[in] vertices List of the vertices of the polygonal obstacle in
+   *                     counterclockwise order.
+   * @return    The number of the first vertex of the obstacle, or
+   *            RVO::RVO2D_ERROR when the number of vertices is less than two.
+   * @note      To add a "negative" obstacle, e.g., a bounding polygon around
+   *            the environment, the vertices should be listed in clockwise
+   *            order.
+   */
+  std::size_t addObstacle(const std::vector<Vector2> &vertices);
+
+  /**
+   * @brief Lets the simulator perform a simulation step and updates the
+   *        two-dimensional position and two-dimensional velocity of each agent.
+   */
+  void doStep();
+
+  /**
+   * @brief     Returns the specified agent neighbor of the specified agent.
+   * @param[in] agentNo    The number of the agent whose agent neighbor is to be
+   *                       retrieved.
+   * @param[in] neighborNo The number of the agent neighbor to be retrieved.
+   * @return    The number of the neighboring agent.
+   */
+  std::size_t getAgentAgentNeighbor(std::size_t agentNo,
+                                    std::size_t neighborNo) const;
+
+  /**
+   * @brief     Returns the maximum neighbor count of a specified agent.
+   * @param[in] agentNo The number of the agent whose maximum neighbor count is
+   *                    to be retrieved.
+   * @return    The present maximum neighbor count of the agent.
+   */
+  std::size_t getAgentMaxNeighbors(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the maximum speed of a specified agent.
+   * @param[in] agentNo The number of the agent whose maximum speed is to be
+   *                    retrieved.
+   * @return    The present maximum speed of the agent.
+   */
+  float getAgentMaxSpeed(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the maximum neighbor distance of a specified agent.
+   * @param[in] agentNo The number of the agent whose maximum neighbor distance
+   *                    is to be retrieved.
+   * @return    The present maximum neighbor distance of the agent.
+   */
+  float getAgentNeighborDist(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the count of agent neighbors taken into account to
+   *            compute the current velocity for the specified agent.
+   * @param[in] agentNo The number of the agent whose count of agent neighbors
+   *                    is to be retrieved.
+   * @return    The count of agent neighbors taken into account to compute the
+   *            current velocity for the specified agent.
+   */
+  std::size_t getAgentNumAgentNeighbors(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the count of obstacle neighbors taken into account to
+   *            compute the current velocity for the specified agent.
+   * @param[in] agentNo The number of the agent whose count of obstacle
+   *                    neighbors is to be retrieved.
+   * @return    The count of obstacle neighbors taken into account to compute
+   *            the current velocity for the specified agent.
+   */
+  std::size_t getAgentNumObstacleNeighbors(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the count of ORCA constraints used to compute the
+   *            current velocity for the specified agent.
+   * @param[in] agentNo The number of the agent whose count of ORCA constraints
+   *                    is to be retrieved.
+   * @return    The count of ORCA constraints used to compute the current
+   *            velocity for the specified agent.
+   */
+  std::size_t getAgentNumORCALines(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the specified obstacle neighbor of the specified agent.
+   * @param[in] agentNo    The number of the agent whose obstacle neighbor is to
+   *                       be retrieved.
+   * @param[in] neighborNo The number of the obstacle neighbor to be retrieved.
+   * @return    The number of the first vertex of the neighboring obstacle edge.
+   */
+  std::size_t getAgentObstacleNeighbor(std::size_t agentNo,
+                                       std::size_t neighborNo) const;
+
+  /**
+   * @brief     Returns the specified ORCA constraint of the specified agent.
+   * @param[in] agentNo The number of the agent whose ORCA constraint is to be
+   *                    retrieved.
+   * @param[in] lineNo  The number of the ORCA constraint to be retrieved.
+   * @return    A line representing the specified ORCA constraint.
+   * @note      The half-plane to the left of the line is the region of
+   *            permissible velocities with respect to the specified ORCA
+   *            constraint.
+   */
+  const Line &getAgentORCALine(std::size_t agentNo, std::size_t lineNo) const;
+
+  /**
+   * @brief     Returns the two-dimensional position of a specified agent.
+   * @param[in] agentNo The number of the agent whose two-dimensional position
+   *                    is to be retrieved.
+   * @return    The present two-dimensional position of the center of the agent.
+   */
+  const Vector2 &getAgentPosition(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the two-dimensional preferred velocity of a specified
+   *            agent.
+   * @param[in] agentNo The number of the agent whose two-dimensional preferred
+   *                    velocity is to be retrieved.
+   * @return    The present two-dimensional preferred velocity of the agent.
+   */
+  const Vector2 &getAgentPrefVelocity(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the radius of a specified agent.
+   * @param[in] agentNo The number of the agent whose radius is to be retrieved.
+   * @return    The present radius of the agent.
+   */
+  float getAgentRadius(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the time horizon of a specified agent.
+   * @param[in] agentNo The number of the agent whose time horizon is to be
+   *                    retrieved.
+   * @return    The present time horizon of the agent.
+   */
+  float getAgentTimeHorizon(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the time horizon with respect to obstacles of a
+   *            specified agent.
+   * @param[in] agentNo The number of the agent whose time horizon with respect
+   *                    to obstacles is to be retrieved.
+   * @return    The present time horizon with respect to obstacles of the agent.
+   */
+  float getAgentTimeHorizonObst(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the two-dimensional linear velocity of a specified
+   *            agent.
+   * @param[in] agentNo The number of the agent whose two-dimensional linear
+   *                    velocity is to be retrieved.
+   * @return    The present two-dimensional linear velocity of the agent.
+   */
+  const Vector2 &getAgentVelocity(std::size_t agentNo) const;
+
+  /**
+   * @brief  Returns the global time of the simulation.
+   * @return The present global time of the simulation (zero initially).
+   */
+  float getGlobalTime() const { return globalTime_; }
+
+  /**
+   * @brief  Returns the count of agents in the simulation.
+   * @return The count of agents in the simulation.
+   */
+  std::size_t getNumAgents() const { return agents_.size(); }
+
+  /**
+   * @brief  Returns the count of obstacle vertices in the simulation.
+   * @return The count of obstacle vertices in the simulation.
+   */
+  std::size_t getNumObstacleVertices() const { return obstacles_.size(); }
+
+  /**
+   * @brief     Returns the two-dimensional position of a specified obstacle
+   *            vertex.
+   * @param[in] vertexNo The number of the obstacle vertex to be retrieved.
+   * @return    The two-dimensional position of the specified obstacle vertex.
+   */
+  const Vector2 &getObstacleVertex(std::size_t vertexNo) const;
+
+  /**
+   * @brief     Returns the number of the obstacle vertex succeeding the
+   *            specified obstacle vertex in its polygon.
+   * @param[in] vertexNo The number of the obstacle vertex whose successor is to
+   *                     be retrieved.
+   * @return    The number of the obstacle vertex succeeding the specified
+   *            obstacle vertex in its polygon.
+   */
+  std::size_t getNextObstacleVertexNo(std::size_t vertexNo) const;
+
+  /**
+   * @brief     Returns the number of the obstacle vertex preceding the
+   *            specified obstacle vertex in its polygon.
+   * @param[in] vertexNo The number of the obstacle vertex whose predecessor is
+   *                     to be retrieved.
+   * @return    The number of the obstacle vertex preceding the specified
+   *            obstacle vertex in its polygon.
+   */
+  std::size_t getPrevObstacleVertexNo(std::size_t vertexNo) const;
+
+  /**
+   * @brief  Returns the time step of the simulation.
+   * @return The present time step of the simulation.
+   */
+  float getTimeStep() const { return timeStep_; }
+
+  /**
+   * @brief Processes the obstacles that have been added so that they are
+   *        accounted for in the simulation.
+   * @note  Obstacles added to the simulation after this function has been
+   *        called are not accounted for in the simulation.
+   */
+  void processObstacles();
+
+  /**
+   * @brief     Performs a visibility query between the two specified points
+   *            with respect to the obstacles
+   * @param[in] point1 The first point of the query.
+   * @param[in] point2 The second point of the query.
+   * @return    A boolean specifying whether the two points are mutually
+   *            visible. Returns true when the obstacles have not been
+   *            processed.
+   */
+  bool queryVisibility(const Vector2 &point1, const Vector2 &point2) const;
+
+  /**
+   * @brief     Performs a visibility query between the two specified points
+   *            with respect to the obstacles
+   * @param[in] point1 The first point of the query.
+   * @param[in] point2 The second point of the query.
+   * @param[in] radius The minimal distance between the line connecting the two
+   *                   points and the obstacles in order for the points to be
+   *                   mutually visible. Must be non-negative.
+   * @return    A boolean specifying whether the two points are mutually
+   *            visible. Returns true when the obstacles have not been
+   *            processed.
+   */
+  bool queryVisibility(const Vector2 &point1, const Vector2 &point2,
+                       float radius) const;
+
+  /**
+   * @brief     Sets the default properties for any new agent that is added.
+   * @param[in] neighborDist    The default maximum distance center-point to
+   *                            center-point to other agents a new agent takes
+   *                            into account in the navigation. The larger this
+   *                            number, the longer he running time of the
+   *                            simulation. If the number is too low, the
+   *                            simulation will not be safe. Must be
+   *                            non-negative.
+   * @param[in] maxNeighbors    The default maximum number of other agents a new
+   *                            agent takes into account in the navigation. The
+   *                            larger this number, the longer the running time
+   *                            of the simulation. If the number is too low, the
+   *                            simulation will not be safe.
+   * @param[in] timeHorizon     The default minimal amount of time for which a
+   *                            new agent's velocities that are computed by the
+   *                            simulation are safe with respect to other
+   *                            agents. The larger this number, the sooner an
+   *                            agent will respond to the presence of other
+   *                            agents, but the less freedom the agent has in
+   *                            choosing its velocities. Must be positive.
+   * @param[in] timeHorizonObst The default minimal amount of time for which a
+   *                            new agent's velocities that are computed by the
+   *                            simulation are safe with respect to obstacles.
+   *                            The larger this number, the sooner an agent will
+   *                            respond to the presence of obstacles, but the
+   *                            less freedom the agent has in  choosing its
+   *                            velocities. Must be positive.
+   * @param[in] radius          The default radius of a new agent. Must be
+   *                            non-negative.
+   * @param[in] maxSpeed        The default maximum speed of a new agent. Must
+   *                            be non-negative.
+   */
+  void setAgentDefaults(float neighborDist, std::size_t maxNeighbors,
+                        float timeHorizon, float timeHorizonObst, float radius,
+                        float maxSpeed);
+
+  /**
+   * @brief     Sets the default properties for any new agent that is added.
+   * @param[in] neighborDist    The default maximum distance center-point to
+   *                            center-point to other agents a new agent takes
+   *                            into account in the navigation. The larger this
+   *                            number, the longer he running time of the
+   *                            simulation. If the number is too low, the
+   *                            simulation will not be safe. Must be
+   *                            non-negative.
+   * @param[in] maxNeighbors    The default maximum number of other agents a new
+   *                            agent takes into account in the navigation. The
+   *                            larger this number, the longer the running time
+   *                            of the simulation. If the number is too low, the
+   *                            simulation will not be safe.
+   * @param[in] timeHorizon     The default minimal amount of time for which a
+   *                            new agent's velocities that are computed by the
+   *                            simulation are safe with respect to other
+   *                            agents. The larger this number, the sooner an
+   *                            agent will respond to the presence of other
+   *                            agents, but the less freedom the agent has in
+   *                            choosing its velocities. Must be positive.
+   * @param[in] timeHorizonObst The default minimal amount of time for which a
+   *                            new agent's velocities that are computed by the
+   *                            simulation are safe with respect to obstacles.
+   *                            The larger this number, the sooner an agent will
+   *                            respond to the presence of obstacles, but the
+   *                            less freedom the agent has in choosing its
+   *                            velocities. Must be positive.
+   * @param[in] radius          The default radius of a new agent. Must be
+   *                            non-negative.
+   * @param[in] maxSpeed        The default maximum speed of a new agent. Must
+   *                            be non-negative.
+   * @param[in] velocity        The default initial two-dimensional linear
+   *                            velocity of a new agent.
+   */
+  void setAgentDefaults(float neighborDist, std::size_t maxNeighbors,
+                        float timeHorizon, float timeHorizonObst, float radius,
+                        float maxSpeed, const Vector2 &velocity);
+
+  /**
+   * @brief     Sets the maximum neighbor count of a specified agent.
+   * @param[in] agentNo      The number of the agent whose maximum neighbor
+   *                         count is to be modified.
+   * @param[in] maxNeighbors The replacement maximum neighbor count.
+   */
+  void setAgentMaxNeighbors(std::size_t agentNo, std::size_t maxNeighbors);
+
+  /**
+   * @brief     Sets the maximum speed of a specified agent.
+   * @param[in] agentNo  The number of the agent whose maximum speed is to be
+   *                     modified.
+   * @param[in] maxSpeed The replacement maximum speed. Must be non-negative.
+   */
+  void setAgentMaxSpeed(std::size_t agentNo, float maxSpeed);
+
+  /**
+   * @brief     Sets the maximum neighbor distance of a specified agent.
+   * @param[in] agentNo      The number of the agent whose maximum neighbor
+   *                         distance is to be modified.
+   * @param[in] neighborDist The replacement maximum neighbor distance. Must be
+   *                         non-negative.
+   */
+  void setAgentNeighborDist(std::size_t agentNo, float neighborDist);
+
+  /**
+   * @brief     Sets the two-dimensional position of a specified agent.
+   * @param[in] agentNo  The number of the agent whose two-dimensional position
+   *                     is to be modified.
+   * @param[in] position The replacement of the two-dimensional position.
+   */
+  void setAgentPosition(std::size_t agentNo, const Vector2 &position);
+
+  /**
+   * @brief     Sets the two-dimensional preferred velocity of a specified
+   *            agent.
+   * @param[in] agentNo      The number of the agent whose two-dimensional
+   *                         preferred velocity is to be modified.
+   * @param[in] prefVelocity The replacement of the two-dimensional preferred
+   *                         velocity.
+   */
+  void setAgentPrefVelocity(std::size_t agentNo, const Vector2 &prefVelocity);
+
+  /**
+   * @brief     Sets the radius of a specified agent.
+   * @param[in] agentNo The number of the agent whose radius is to be modified.
+   * @param[in] radius  The replacement radius. Must be non-negative.
+   */
+  void setAgentRadius(std::size_t agentNo, float radius);
+
+  /**
+   * @brief     Sets the time horizon of a specified agent with respect to other
+   *            agents.
+   * @param[in] agentNo     The number of the agent whose time horizon is to be
+   *                        modified.
+   * @param[in] timeHorizon The replacement time horizon with respect to other
+   *                        agents. Must be positive.
+   */
+  void setAgentTimeHorizon(std::size_t agentNo, float timeHorizon);
+
+  /**
+   * @brief     Sets the time horizon of a specified agent with respect to
+   *            obstacles.
+   * @param[in] agentNo         The number of the agent whose time horizon with
+   *                            respect to obstacles is to be modified.
+   * @param[in] timeHorizonObst The replacement time horizon with respect to
+   *                            obstacles. Must be positive.
+   */
+  void setAgentTimeHorizonObst(std::size_t agentNo, float timeHorizonObst);
+
+  /**
+   * @brief     Sets the two-dimensional linear velocity of a specified agent.
+   * @param[in] agentNo  The number of the agent whose two-dimensional linear
+   *                     velocity is to be modified.
+   * @param[in] velocity The replacement two-dimensional linear velocity.
+   */
+  void setAgentVelocity(std::size_t agentNo, const Vector2 &velocity);
+
+  /**
+   * @brief     Sets the time step of the simulation.
+   * @param[in] timeStep The time step of the simulation. Must be positive.
+   */
+  void setTimeStep(float timeStep) { timeStep_ = timeStep; }
+
+ public:
+  /* Not implemented. */
+  RVOSimulator2D(const RVOSimulator2D &other);
+
+  /* Not implemented. */
+  RVOSimulator2D &operator=(const RVOSimulator2D &other);
+
+  std::vector<Agent2D *> agents_;
+  std::vector<Obstacle2D *> obstacles_;
+  Agent2D *defaultAgent_;
+  KdTree2D *kdTree_;
+  float globalTime_;
+  float timeStep_;
+
+  friend class KdTree2D;
+};
+} /* namespace RVO2D */
 
 #endif /* RVO2D_RVO_SIMULATOR_H_ */

thirdparty/rvo2/rvo2_2d/Vector2.cc

@@ -0,0 +1,136 @@
+/*
+ * Vector2.cpp
+ * RVO2 Library
+ *
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     https://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Please send all bug reports to <[email protected]>.
+ *
+ * The authors may be contacted via:
+ *
+ * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
+ * Dept. of Computer Science
+ * 201 S. Columbia St.
+ * Frederick P. Brooks, Jr. Computer Science Bldg.
+ * Chapel Hill, N.C. 27599-3175
+ * United States of America
+ *
+ * <https://gamma.cs.unc.edu/RVO2/>
+ */
+
+/**
+ * @file  Vector2.cc
+ * @brief Defines the Vector2 class.
+ */
+
+#include "Vector2.h"
+
+#include <cmath>
+#include <ostream>
+
+namespace RVO2D {
+const float RVO2D_EPSILON = 0.00001F;
+
+Vector2::Vector2() : x_(0.0F), y_(0.0F) {}
+
+Vector2::Vector2(float x, float y) : x_(x), y_(y) {}
+
+Vector2 Vector2::operator-() const { return Vector2(-x_, -y_); }
+
+float Vector2::operator*(const Vector2 &vector) const {
+  return x_ * vector.x_ + y_ * vector.y_;
+}
+
+Vector2 Vector2::operator*(float scalar) const {
+  return Vector2(x_ * scalar, y_ * scalar);
+}
+
+Vector2 Vector2::operator/(float scalar) const {
+  const float invScalar = 1.0F / scalar;
+
+  return Vector2(x_ * invScalar, y_ * invScalar);
+}
+
+Vector2 Vector2::operator+(const Vector2 &vector) const {
+  return Vector2(x_ + vector.x_, y_ + vector.y_);
+}
+
+Vector2 Vector2::operator-(const Vector2 &vector) const {
+  return Vector2(x_ - vector.x_, y_ - vector.y_);
+}
+
+bool Vector2::operator==(const Vector2 &vector) const {
+  return x_ == vector.x_ && y_ == vector.y_;
+}
+
+bool Vector2::operator!=(const Vector2 &vector) const {
+  return x_ != vector.x_ || y_ != vector.y_;
+}
+
+Vector2 &Vector2::operator*=(float scalar) {
+  x_ *= scalar;
+  y_ *= scalar;
+
+  return *this;
+}
+
+Vector2 &Vector2::operator/=(float scalar) {
+  const float invScalar = 1.0F / scalar;
+  x_ *= invScalar;
+  y_ *= invScalar;
+
+  return *this;
+}
+
+Vector2 &Vector2::operator+=(const Vector2 &vector) {
+  x_ += vector.x_;
+  y_ += vector.y_;
+
+  return *this;
+}
+
+Vector2 &Vector2::operator-=(const Vector2 &vector) {
+  x_ -= vector.x_;
+  y_ -= vector.y_;
+
+  return *this;
+}
+
+Vector2 operator*(float scalar, const Vector2 &vector) {
+  return Vector2(scalar * vector.x(), scalar * vector.y());
+}
+
+std::ostream &operator<<(std::ostream &stream, const Vector2 &vector) {
+  stream << "(" << vector.x() << "," << vector.y() << ")";
+
+  return stream;
+}
+
+float abs(const Vector2 &vector) { return std::sqrt(vector * vector); }
+
+float absSq(const Vector2 &vector) { return vector * vector; }
+
+float det(const Vector2 &vector1, const Vector2 &vector2) {
+  return vector1.x() * vector2.y() - vector1.y() * vector2.x();
+}
+
+float leftOf(const Vector2 &vector1, const Vector2 &vector2,
+             const Vector2 &vector3) {
+  return det(vector1 - vector3, vector2 - vector1);
+}
+
+Vector2 normalize(const Vector2 &vector) { return vector / abs(vector); }
+} /* namespace RVO */

thirdparty/rvo2/rvo2_2d/Vector2.h

@@ -2,13 +2,14 @@
  * Vector2.h
  * RVO2 Library
  *
- * Copyright 2008 University of North Carolina at Chapel Hill
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
- *     http://www.apache.org/licenses/LICENSE-2.0
+ *     https://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
@@ -27,320 +28,246 @@
  * Chapel Hill, N.C. 27599-3175
  * United States of America
  *
- * <http://gamma.cs.unc.edu/RVO2/>
+ * <https://gamma.cs.unc.edu/RVO2/>
  */
 
 #ifndef RVO_VECTOR2_H_
 #define RVO_VECTOR2_H_
 
 /**
- * \file       Vector2.h
- * \brief      Contains the Vector2 class.
+ * @file  Vector2.h
+ * @brief Declares and defines the Vector2 class.
  */
 
-#include <cmath>
-#include <ostream>
+#include <iosfwd>
 
 namespace RVO2D {
-	/**
-	 * \brief      Defines a two-dimensional vector.
-	 */
-	class Vector2 {
-	public:
-		/**
-		 * \brief      Constructs and initializes a two-dimensional vector instance
-		 *             to (0.0, 0.0).
-		 */
-		inline Vector2() : x_(0.0f), y_(0.0f) { }
-
-		/**
-		 * \brief      Constructs and initializes a two-dimensional vector from
-		 *             the specified xy-coordinates.
-		 * \param      x               The x-coordinate of the two-dimensional
-		 *                             vector.
-		 * \param      y               The y-coordinate of the two-dimensional
-		 *                             vector.
-		 */
-		inline Vector2(float x, float y) : x_(x), y_(y) { }
-
-		inline Vector2(const Vector2 &vector)
-		{
-			x_ = vector.x();
-			y_ = vector.y();
-		}
-
-		/**
-		 * \brief      Returns the x-coordinate of this two-dimensional vector.
-		 * \return     The x-coordinate of the two-dimensional vector.
-		 */
-		inline float x() const { return x_; }
-
-		/**
-		 * \brief      Returns the y-coordinate of this two-dimensional vector.
-		 * \return     The y-coordinate of the two-dimensional vector.
-		 */
-		inline float y() const { return y_; }
-
-		/**
-		 * \brief      Computes the negation of this two-dimensional vector.
-		 * \return     The negation of this two-dimensional vector.
-		 */
-		inline Vector2 operator-() const
-		{
-			return Vector2(-x_, -y_);
-		}
-
-		/**
-		 * \brief      Computes the dot product of this two-dimensional vector with
-		 *             the specified two-dimensional vector.
-		 * \param      vector          The two-dimensional vector with which the
-		 *                             dot product should be computed.
-		 * \return     The dot product of this two-dimensional vector with a
-		 *             specified two-dimensional vector.
-		 */
-		inline float operator*(const Vector2 &vector) const
-		{
-			return x_ * vector.x() + y_ * vector.y();
-		}
-
-		/**
-		 * \brief      Computes the scalar multiplication of this
-		 *             two-dimensional vector with the specified scalar value.
-		 * \param      s               The scalar value with which the scalar
-		 *                             multiplication should be computed.
-		 * \return     The scalar multiplication of this two-dimensional vector
-		 *             with a specified scalar value.
-		 */
-		inline Vector2 operator*(float s) const
-		{
-			return Vector2(x_ * s, y_ * s);
-		}
-
-		/**
-		 * \brief      Computes the scalar division of this two-dimensional vector
-		 *             with the specified scalar value.
-		 * \param      s               The scalar value with which the scalar
-		 *                             division should be computed.
-		 * \return     The scalar division of this two-dimensional vector with a
-		 *             specified scalar value.
-		 */
-		inline Vector2 operator/(float s) const
-		{
-			const float invS = 1.0f / s;
-
-			return Vector2(x_ * invS, y_ * invS);
-		}
-
-		/**
-		 * \brief      Computes the vector sum of this two-dimensional vector with
-		 *             the specified two-dimensional vector.
-		 * \param      vector          The two-dimensional vector with which the
-		 *                             vector sum should be computed.
-		 * \return     The vector sum of this two-dimensional vector with a
-		 *             specified two-dimensional vector.
-		 */
-		inline Vector2 operator+(const Vector2 &vector) const
-		{
-			return Vector2(x_ + vector.x(), y_ + vector.y());
-		}
-
-		/**
-		 * \brief      Computes the vector difference of this two-dimensional
-		 *             vector with the specified two-dimensional vector.
-		 * \param      vector          The two-dimensional vector with which the
-		 *                             vector difference should be computed.
-		 * \return     The vector difference of this two-dimensional vector with a
-		 *             specified two-dimensional vector.
-		 */
-		inline Vector2 operator-(const Vector2 &vector) const
-		{
-			return Vector2(x_ - vector.x(), y_ - vector.y());
-		}
-
-		/**
-		 * \brief      Tests this two-dimensional vector for equality with the
-		 *             specified two-dimensional vector.
-		 * \param      vector          The two-dimensional vector with which to
-		 *                             test for equality.
-		 * \return     True if the two-dimensional vectors are equal.
-		 */
-		inline bool operator==(const Vector2 &vector) const
-		{
-			return x_ == vector.x() && y_ == vector.y();
-		}
-
-		/**
-		 * \brief      Tests this two-dimensional vector for inequality with the
-		 *             specified two-dimensional vector.
-		 * \param      vector          The two-dimensional vector with which to
-		 *                             test for inequality.
-		 * \return     True if the two-dimensional vectors are not equal.
-		 */
-		inline bool operator!=(const Vector2 &vector) const
-		{
-			return x_ != vector.x() || y_ != vector.y();
-		}
-
-		/**
-		 * \brief      Sets the value of this two-dimensional vector to the scalar
-		 *             multiplication of itself with the specified scalar value.
-		 * \param      s               The scalar value with which the scalar
-		 *                             multiplication should be computed.
-		 * \return     A reference to this two-dimensional vector.
-		 */
-		inline Vector2 &operator*=(float s)
-		{
-			x_ *= s;
-			y_ *= s;
-
-			return *this;
-		}
-
-		/**
-		 * \brief      Sets the value of this two-dimensional vector to the scalar
-		 *             division of itself with the specified scalar value.
-		 * \param      s               The scalar value with which the scalar
-		 *                             division should be computed.
-		 * \return     A reference to this two-dimensional vector.
-		 */
-		inline Vector2 &operator/=(float s)
-		{
-			const float invS = 1.0f / s;
-			x_ *= invS;
-			y_ *= invS;
-
-			return *this;
-		}
-
-		/**
-		 * \brief      Sets the value of this two-dimensional vector to the vector
-		 *             sum of itself with the specified two-dimensional vector.
-		 * \param      vector          The two-dimensional vector with which the
-		 *                             vector sum should be computed.
-		 * \return     A reference to this two-dimensional vector.
-		 */
-		inline Vector2 &operator+=(const Vector2 &vector)
-		{
-			x_ += vector.x();
-			y_ += vector.y();
-
-			return *this;
-		}
-
-		/**
-		 * \brief      Sets the value of this two-dimensional vector to the vector
-		 *             difference of itself with the specified two-dimensional
-		 *             vector.
-		 * \param      vector          The two-dimensional vector with which the
-		 *                             vector difference should be computed.
-		 * \return     A reference to this two-dimensional vector.
-		 */
-		inline Vector2 &operator-=(const Vector2 &vector)
-		{
-			x_ -= vector.x();
-			y_ -= vector.y();
-
-			return *this;
-		}
-
-		inline Vector2 &operator=(const Vector2 &vector)
-		{
-			x_ = vector.x();
-			y_ = vector.y();
-
-			return *this;
-		}
+/**
+ * @brief A sufficiently small positive number.
+ */
+extern const float RVO2D_EPSILON;
 
-	private:
-		float x_;
-		float y_;
-	};
+/**
+ * @brief Defines a two-dimensional vector.
+ */
+class Vector2 {
+ public:
+  /**
+   * @brief Constructs and initializes a two-dimensional vector instance to
+   *        (0.0, 0.0).
+   */
+  Vector2();
+
+  /**
+   * @brief     Constructs and initializes a two-dimensional vector from the
+   *            specified xy-coordinates.
+   * @param[in] x The x-coordinate of the two-dimensional vector.
+   * @param[in] y The y-coordinate of the two-dimensional vector.
+   */
+  Vector2(float x, float y);
+
+  /**
+   * @brief  Returns the x-coordinate of this two-dimensional vector.
+   * @return The x-coordinate of the two-dimensional vector.
+   */
+  float x() const { return x_; }
+
+  /**
+   * @brief  Returns the y-coordinate of this two-dimensional vector.
+   * @return The y-coordinate of the two-dimensional vector.
+   */
+  float y() const { return y_; }
+
+  /**
+   * @brief  Computes the negation of this two-dimensional vector.
+   * @return The negation of this two-dimensional vector.
+   */
+  Vector2 operator-() const;
+
+  /**
+   * @brief     Computes the dot product of this two-dimensional vector with the
+   *            specified two-dimensional vector.
+   * @param[in] vector The two-dimensional vector with which the dot product
+   *                   should be computed.
+   * @return    The dot product of this two-dimensional vector with a specified
+   *            two-dimensional vector.
+   */
+  float operator*(const Vector2 &vector) const;
+
+  /**
+   * @brief     Computes the scalar multiplication of this two-dimensional
+   *            vector with the specified scalar value.
+   * @param[in] scalar The scalar value with which the scalar multiplication
+   *                   should be computed.
+   * @return    The scalar multiplication of this two-dimensional vector with a
+   *            specified scalar value.
+   */
+  Vector2 operator*(float scalar) const;
+
+  /**
+   * @brief     Computes the scalar division of this two-dimensional vector with
+   *            the specified scalar value.
+   * @param[in] scalar The scalar value with which the scalar division should be
+   *                   computed.
+   * @return    The scalar division of this two-dimensional vector with a
+   *            specified scalar value.
+   */
+  Vector2 operator/(float scalar) const;
+
+  /**
+   * @brief     Computes the vector sum of this two-dimensional vector with the
+   *            specified two-dimensional vector.
+   * @param[in] vector The two-dimensional vector with which the vector sum
+   *                   should be computed.
+   * @return    The vector sum of this two-dimensional vector with a specified
+   *            two-dimensional vector.
+   */
+  Vector2 operator+(const Vector2 &vector) const;
+
+  /**
+   * @brief     Computes the vector difference of this two-dimensional vector
+   *            with the specified two-dimensional vector.
+   * @param[in] vector The two-dimensional vector with which the vector
+   *                   difference should be computed.
+   * @return    The vector difference of this two-dimensional vector with a
+   *            specified two-dimensional vector.
+   */
+  Vector2 operator-(const Vector2 &vector) const;
+
+  /**
+   * @brief     Tests this two-dimensional vector for equality with the
+   *            specified two-dimensional vector.
+   * @param[in] vector The two-dimensional vector with which to test for
+   *                   equality.
+   * @return    True if the two-dimensional vectors are equal.
+   */
+  bool operator==(const Vector2 &vector) const;
+
+  /**
+   * @brief     Tests this two-dimensional vector for inequality with the
+   *            specified two-dimensional vector.
+   * @param[in] vector The two-dimensional vector with which to test for
+   *                   inequality.
+   * @return    True if the two-dimensional vectors are not equal.
+   */
+  bool operator!=(const Vector2 &vector) const;
+
+  /**
+   * @brief     Sets the value of this two-dimensional vector to the scalar
+   *            multiplication of itself with the specified scalar value.
+   * @param[in] scalar The scalar value with which the scalar multiplication
+   *                   should be computed.
+   * @return    A reference to this two-dimensional vector.
+   */
+  Vector2 &operator*=(float scalar);
+
+  /**
+   * @brief     Sets the value of this two-dimensional vector to the scalar
+   *            division of itself with the specified scalar value.
+   * @param[in] scalar The scalar value with which the scalar division should be
+   *                   computed.
+   * @return    A reference to this two-dimensional vector.
+   */
+  Vector2 &operator/=(float scalar);
+
+  /**
+   * @brief     Sets the value of this two-dimensional vector to the vector sum
+   *            of itself with the specified two-dimensional vector.
+   * @param[in] vector The two-dimensional vector with which the vector sum
+   *                   should be computed.
+   * @return    A reference to this two-dimensional vector.
+   */
+  Vector2 &operator+=(const Vector2 &vector);
+
+  /**
+   * @brief     Sets the value of this two-dimensional vector to the vector
+   *            difference of itself with the specified two-dimensional vector.
+   * @param[in] vector The two-dimensional vector with which the vector
+   *                   difference should be computed.
+   * @return    A reference to this two-dimensional vector.
+   */
+  Vector2 &operator-=(const Vector2 &vector);
+
+ private:
+  float x_;
+  float y_;
+};
 
-	/**
-	 * \relates    Vector2
-	 * \brief      Computes the scalar multiplication of the specified
-	 *             two-dimensional vector with the specified scalar value.
-	 * \param      s               The scalar value with which the scalar
-	 *                             multiplication should be computed.
-	 * \param      vector          The two-dimensional vector with which the scalar
-	 *                             multiplication should be computed.
-	 * \return     The scalar multiplication of the two-dimensional vector with the
-	 *             scalar value.
-	 */
-	inline Vector2 operator*(float s, const Vector2 &vector)
-	{
-		return Vector2(s * vector.x(), s * vector.y());
-	}
+/**
+ * @relates   Vector2
+ * @brief     Computes the scalar multiplication of the specified
+ *            two-dimensional vector with the specified scalar value.
+ * @param[in] scalar The scalar value with which the scalar multiplication
+ *                   should be computed.
+ * @param[in] vector The two-dimensional vector with which the scalar
+ *                   multiplication should be computed.
+ * @return    The scalar multiplication of the two-dimensional vector with the
+ *            scalar value.
+ */
+ Vector2 operator*(float scalar, const Vector2 &vector);
 
-	/**
-	 * \relates    Vector2
-	 * \brief      Inserts the specified two-dimensional vector into the specified
-	 *             output stream.
-	 * \param      os              The output stream into which the two-dimensional
-	 *                             vector should be inserted.
-	 * \param      vector          The two-dimensional vector which to insert into
-	 *                             the output stream.
-	 * \return     A reference to the output stream.
-	 */
-	inline std::ostream &operator<<(std::ostream &os, const Vector2 &vector)
-	{
-		os << "(" << vector.x() << "," << vector.y() << ")";
+/**
+ * @relates        Vector2
+ * @brief          Inserts the specified two-dimensional vector into the
+ *                 specified output stream.
+ * @param[in, out] stream The output stream into which the two-dimensional
+ *                        vector should be inserted.
+ * @param[in]      vector The two-dimensional vector which to insert into the
+ *                        output stream.
+ * @return         A reference to the output stream.
+ */
+ std::ostream &operator<<(std::ostream &stream,
+                                    const Vector2 &vector);
 
-		return os;
-	}
+/**
+ * @relates   Vector2
+ * @brief     Computes the length of a specified two-dimensional vector.
+ * @param[in] vector The two-dimensional vector whose length is to be computed.
+ * @return    The length of the two-dimensional vector.
+ */
+ float abs(const Vector2 &vector);
 
-	/**
-	 * \relates    Vector2
-	 * \brief      Computes the length of a specified two-dimensional vector.
-	 * \param      vector          The two-dimensional vector whose length is to be
-	 *                             computed.
-	 * \return     The length of the two-dimensional vector.
-	 */
-	inline float abs(const Vector2 &vector)
-	{
-		return std::sqrt(vector * vector);
-	}
+/**
+ * @relates   Vector2
+ * @brief     Computes the squared length of a specified two-dimensional vector.
+ * @param[in] vector The two-dimensional vector whose squared length is to be
+ *                   computed.
+ * @return    The squared length of the two-dimensional vector.
+ */
+ float absSq(const Vector2 &vector);
 
-	/**
-	 * \relates    Vector2
-	 * \brief      Computes the squared length of a specified two-dimensional
-	 *             vector.
-	 * \param      vector          The two-dimensional vector whose squared length
-	 *                             is to be computed.
-	 * \return     The squared length of the two-dimensional vector.
-	 */
-	inline float absSq(const Vector2 &vector)
-	{
-		return vector * vector;
-	}
+/**
+ * @relates   Vector2
+ * @brief     Computes the determinant of a two-dimensional square matrix with
+ *            rows consisting of the specified two-dimensional vectors.
+ * @param[in] vector1 The top row of the two-dimensional square matrix.
+ * @param[in] vector2 The bottom row of the two-dimensional square matrix.
+ * @return    The determinant of the two-dimensional square matrix.
+ */
+ float det(const Vector2 &vector1, const Vector2 &vector2);
 
-	/**
-	 * \relates    Vector2
-	 * \brief      Computes the determinant of a two-dimensional square matrix with
-	 *             rows consisting of the specified two-dimensional vectors.
-	 * \param      vector1         The top row of the two-dimensional square
-	 *                             matrix.
-	 * \param      vector2         The bottom row of the two-dimensional square
-	 *                             matrix.
-	 * \return     The determinant of the two-dimensional square matrix.
-	 */
-	inline float det(const Vector2 &vector1, const Vector2 &vector2)
-	{
-		return vector1.x() * vector2.y() - vector1.y() * vector2.x();
-	}
+/**
+ * @brief     Computes the signed distance from a line connecting th specified
+ *            points to a specified point.
+ * @param[in] vector1 The first point on the line.
+ * @param[in] vector2 The second point on the line.
+ * @param[in] vector3 The point to which the signed distance is to be
+ *                    calculated.
+ * @return    Positive when the point vector3 lies to the left of the line
+ *            vector1-vector2.
+ */
+ float leftOf(const Vector2 &vector1, const Vector2 &vector2,
+                        const Vector2 &vector3);
 
-	/**
-	 * \relates    Vector2
-	 * \brief      Computes the normalization of the specified two-dimensional
-	 *             vector.
-	 * \param      vector          The two-dimensional vector whose normalization
-	 *                             is to be computed.
-	 * \return     The normalization of the two-dimensional vector.
-	 */
-	inline Vector2 normalize(const Vector2 &vector)
-	{
-		return vector / abs(vector);
-	}
-}
+/**
+ * @relates   Vector2
+ * @brief     Computes the normalization of the specified two-dimensional
+ *            vector.
+ * @param[in] vector The two-dimensional vector whose normalization is to be
+ *                   computed.
+ * @return    The normalization of the two-dimensional vector.
+ */
+ Vector2 normalize(const Vector2 &vector);
+} /* namespace RVO2D */
 
 #endif /* RVO_VECTOR2_H_ */

thirdparty/rvo2/rvo2_3d/Agent3d.cc

@@ -0,0 +1,474 @@
+/*
+ * Agent3d.cc
+ * RVO2-3D Library
+ *
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     https://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Please send all bug reports to <[email protected]>.
+ *
+ * The authors may be contacted via:
+ *
+ * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
+ * Dept. of Computer Science
+ * 201 S. Columbia St.
+ * Frederick P. Brooks, Jr. Computer Science Bldg.
+ * Chapel Hill, N.C. 27599-3175
+ * United States of America
+ *
+ * <https://gamma.cs.unc.edu/RVO2/>
+ */
+
+#include "Agent3d.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include "KdTree3d.h"
+#include "RVOSimulator3d.h"
+
+namespace RVO3D {
+namespace {
+/**
+ * @brief A sufficiently small positive number.
+ */
+const float RVO3D_EPSILON = 0.00001F;
+
+/**
+ * @brief Defines a directed line.
+ */
+class Line3D {
+ public:
+  /**
+   * @brief Constructs a directed line.``
+   */
+  Line3D();
+
+  /**
+   * @brief The direction of the directed line.
+   */
+  Vector3 direction;
+
+  /**
+   * @brief A point on the directed line.
+   */
+  Vector3 point;
+};
+
+Line3D::Line3D() {}
+
+/**
+ * @brief     Solves a one-dimensional linear program on a specified line
+ *            subject to linear constraints defined by planes and a spherical
+ *            constraint.
+ * @param[in] planes       Planes defining the linear constraints.
+ * @param[in] planeNo      The plane on which the line lies.
+ * @param[in] line         The line on which the one-dimensional linear program
+ *                         is solved.
+ * @param[in] radius       The radius of the spherical constraint.
+ * @param[in] optVelocity  The optimization velocity.
+ * @param[in] directionOpt True if the direction should be optimized.
+ * @param[in] result       A reference to the result of the linear program.
+ * @return True if successful.
+ */
+bool linearProgram1(const std::vector<Plane> &planes, std::size_t planeNo,
+                    const Line3D &line, float radius, const Vector3 &optVelocity,
+                    bool directionOpt,
+                    Vector3 &result) { /* NOLINT(runtime/references) */
+  const float dotProduct = line.point * line.direction;
+  const float discriminant =
+      dotProduct * dotProduct + radius * 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 (std::size_t i = 0U; i < planeNo; ++i) {
+    const float numerator = (planes[i].point - line.point) * planes[i].normal;
+    const float denominator = line.direction * planes[i].normal;
+
+    if (denominator * denominator <= RVO3D_EPSILON) {
+      /* Lines line is (almost) parallel to plane i. */
+      if (numerator > 0.0F) {
+        return false;
+      }
+
+      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;
+}
+
+/**
+ * @brief      Solves a two-dimensional linear program on a specified plane
+ *             subject to linear constraints defined by planes and a spherical
+ *             constraint.
+ * @param[in]  planes       Planes defining the linear constraints.
+ * @param[in]  planeNo      The plane on which the two-dimensional linear
+ *                          program is solved.
+ * @param[in]  radius       The radius of the spherical constraint.
+ * @param[in]  optVelocity  The optimization velocity.
+ * @param[in]  directionOpt True if the direction should be optimized.
+ * @param[out] result       A reference to the result of the linear program.
+ * @return     True if successful.
+ */
+bool linearProgram2(const std::vector<Plane> &planes, std::size_t planeNo,
+                    float radius, const Vector3 &optVelocity, bool directionOpt,
+                    Vector3 &result) { /* NOLINT(runtime/references) */
+  const float planeDist = planes[planeNo].point * planes[planeNo].normal;
+  const float planeDistSq = planeDist * planeDist;
+  const float radiusSq = radius * 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 <= RVO3D_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 (std::size_t i = 0U; 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) <= RVO3D_EPSILON) {
+        /* Planes planeNo and i are (almost) parallel, and plane i fully
+         * invalidates plane planeNo.
+         */
+        return false;
+      }
+
+      Line3D 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;
+}
+
+/**
+ * @brief      Solves a three-dimensional linear program subject to linear
+ *             constraints defined by planes and a spherical constraint.
+ * @param[in]  planes       Planes defining the linear constraints.
+ * @param[in]  radius       The radius of the spherical constraint.
+ * @param[in]  optVelocity  The optimization velocity.
+ * @param[in]  directionOpt True if the direction should be optimized.
+ * @param[out] result       A reference to the result of the linear program.
+ * @return     The number of the plane it fails on, and the number of planes if
+ *             successful.
+ */
+std::size_t linearProgram3(const std::vector<Plane> &planes, float radius,
+                           const Vector3 &optVelocity, bool directionOpt,
+                           Vector3 &result) { /* NOLINT(runtime/references) */
+  if (directionOpt) {
+    /* Optimize direction. Note that the optimization velocity is of unit length
+     * in this case.
+     */
+    result = optVelocity * radius;
+  } else if (absSq(optVelocity) > radius * radius) {
+    /* Optimize closest point and outside circle. */
+    result = normalize(optVelocity) * radius;
+  } else {
+    /* Optimize closest point and inside circle. */
+    result = optVelocity;
+  }
+
+  for (std::size_t i = 0U; 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();
+}
+
+/**
+ * @brief      Solves a four-dimensional linear program subject to linear
+ *             constraints defined by planes and a spherical constraint.
+ * @param[in]  planes     Planes defining the linear constraints.
+ * @param[in]  beginPlane The plane on which the three-dimensional linear
+ *                        program failed.
+ * @param[in]  radius     The radius of the spherical constraint.
+ * @param[out] result     A reference to the result of the linear program.
+ */
+void linearProgram4(const std::vector<Plane> &planes, std::size_t beginPlane,
+                    float radius,
+                    Vector3 &result) { /* NOLINT(runtime/references) */
+  float distance = 0.0F;
+
+  for (std::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 (std::size_t j = 0U; j < i; ++j) {
+        Plane plane;
+
+        const Vector3 crossProduct = cross(planes[j].normal, planes[i].normal);
+
+        if (absSq(crossProduct) <= RVO3D_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;
+          }
+
+          /* 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);
+    }
+  }
+}
+} /* namespace */
+
+Agent3D::Agent3D()
+    : id_(0U),
+      maxNeighbors_(0U),
+      maxSpeed_(0.0F),
+      neighborDist_(0.0F),
+      radius_(0.0F),
+      timeHorizon_(0.0F) {}
+
+Agent3D::~Agent3D() {}
+
+void Agent3D::computeNeighbors(RVOSimulator3D *sim_) {
+	agentNeighbors_.clear();
+
+	if (maxNeighbors_ > 0) {
+		sim_->kdTree_->computeAgentNeighbors(this, neighborDist_ * neighborDist_);
+	}
+}
+
+void Agent3D::computeNewVelocity(RVOSimulator3D *sim_) {
+  orcaPlanes_.clear();
+  const float invTimeHorizon = 1.0F / timeHorizon_;
+
+  /* Create agent ORCA planes. */
+  for (std::size_t i = 0U; i < agentNeighbors_.size(); ++i) {
+    const Agent3D *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 = combinedRadius * 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 &&
+          dotProduct * 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(b * b - a * c)) / a;
+        const Vector3 ww = relativeVelocity - t * relativePosition;
+        const float wwLength = abs(ww);
+        const Vector3 unitWW = ww / wwLength;
+
+        plane.normal = unitWW;
+        u = (combinedRadius * t - wwLength) * unitWW;
+      }
+    } 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 std::size_t planeFail = linearProgram3(
+      orcaPlanes_, maxSpeed_, prefVelocity_, false, newVelocity_);
+
+  if (planeFail < orcaPlanes_.size()) {
+    linearProgram4(orcaPlanes_, planeFail, maxSpeed_, newVelocity_);
+  }
+}
+
+void Agent3D::insertAgentNeighbor(const Agent3D *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));
+      }
+
+      std::size_t i = agentNeighbors_.size() - 1U;
+
+      while (i != 0U && distSq < agentNeighbors_[i - 1U].first) {
+        agentNeighbors_[i] = agentNeighbors_[i - 1U];
+        --i;
+      }
+
+      agentNeighbors_[i] = std::make_pair(distSq, agent);
+
+      if (agentNeighbors_.size() == maxNeighbors_) {
+        rangeSq = agentNeighbors_.back().first;
+      }
+    }
+  }
+}
+
+void Agent3D::update(RVOSimulator3D *sim_) {
+  velocity_ = newVelocity_;
+  position_ += velocity_ * sim_->timeStep_;
+}
+} /* namespace RVO3D */

thirdparty/rvo2/rvo2_3d/Agent3d.cpp

@@ -1,449 +0,0 @@
-/*
- * Agent.cpp
- * RVO2-3D Library
- *
- * Copyright 2008 University of North Carolina at Chapel Hill
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     https://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
- * Please send all bug reports to <[email protected]>.
- *
- * The authors may be contacted via:
- *
- * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
- * Dept. of Computer Science
- * 201 S. Columbia St.
- * Frederick P. Brooks, Jr. Computer Science Bldg.
- * Chapel Hill, N.C. 27599-3175
- * United States of America
- *
- * <https://gamma.cs.unc.edu/RVO2/>
- */
-
-#include "Agent3d.h"
-
-#include <cmath>
-#include <algorithm>
-
-#include "Definitions.h"
-#include "KdTree3d.h"
-
-namespace RVO3D {
-	/**
-	 * \brief   A sufficiently small positive number.
-	 */
-	const float RVO3D_EPSILON = 0.00001f;
-
-	/**
-	 * \brief   Defines a directed line.
-	 */
-	class Line3D {
-	public:
-		/**
-		 * \brief   The direction of the directed line.
-		 */
-		Vector3 direction;
-
-		/**
-		 * \brief   A point on the directed line.
-		 */
-		Vector3 point;
-	};
-
-	/**
-	 * \brief   Solves a one-dimensional linear program on a specified line subject to linear constraints defined by planes and a spherical constraint.
-	 * \param   planes        Planes defining the linear constraints.
-	 * \param   planeNo       The plane on which the line lies.
-	 * \param   line          The line on which the 1-d linear program is solved
-	 * \param   radius        The radius of the spherical constraint.
-	 * \param   optVelocity   The optimization velocity.
-	 * \param   directionOpt  True if the direction should be optimized.
-	 * \param   result        A reference to the result of the linear program.
-	 * \return  True if successful.
-	 */
-	bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line3D &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result);
-
-	/**
-	 * \brief   Solves a two-dimensional linear program on a specified plane subject to linear constraints defined by planes and a spherical constraint.
-	 * \param   planes        Planes defining the linear constraints.
-	 * \param   planeNo       The plane on which the 2-d linear program is solved
-	 * \param   radius        The radius of the spherical constraint.
-	 * \param   optVelocity   The optimization velocity.
-	 * \param   directionOpt  True if the direction should be optimized.
-	 * \param   result        A reference to the result of the linear program.
-	 * \return  True if successful.
-	 */
-	bool linearProgram2(const std::vector<Plane> &planes, size_t planeNo, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result);
-
-	/**
-	 * \brief   Solves a three-dimensional linear program subject to linear constraints defined by planes and a spherical constraint.
-	 * \param   planes        Planes defining the linear constraints.
-	 * \param   radius        The radius of the spherical constraint.
-	 * \param   optVelocity   The optimization velocity.
-	 * \param   directionOpt  True if the direction should be optimized.
-	 * \param   result        A reference to the result of the linear program.
-	 * \return  The number of the plane it fails on, and the number of planes if successful.
-	 */
-	size_t linearProgram3(const std::vector<Plane> &planes, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result);
-
-	/**
-	 * \brief   Solves a four-dimensional linear program subject to linear constraints defined by planes and a spherical constraint.
-	 * \param   planes     Planes defining the linear constraints.
-	 * \param   beginPlane The plane on which the 3-d linear program failed.
-	 * \param   radius     The radius of the spherical constraint.
-	 * \param   result     A reference to the result of the linear program.
-	 */
-	void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float radius, Vector3 &result);
-
-	Agent3D::Agent3D() : id_(0), maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f) { }
-
-	void Agent3D::computeNeighbors(RVOSimulator3D *sim_)
-	{
-		agentNeighbors_.clear();
-
-		if (maxNeighbors_ > 0) {
-			sim_->kdTree_->computeAgentNeighbors(this, neighborDist_ * neighborDist_);
-		}
-	}
-
-	void Agent3D::computeNewVelocity(RVOSimulator3D *sim_)
-	{
-		orcaPlanes_.clear();
-
-		const float invTimeHorizon = 1.0f / timeHorizon_;
-
-		/* Create agent ORCA planes. */
-		for (size_t i = 0; i < agentNeighbors_.size(); ++i) {
-			const Agent3D *const other = agentNeighbors_[i].second;
-
-			//const float timeHorizon_mod = (avoidance_priority_ - other->avoidance_priority_ + 1.0f) * 0.5f;
-			//const float invTimeHorizon = (1.0f / timeHorizon_) * timeHorizon_mod;
-
-			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_);
-		}
-	}
-
-	void Agent3D::insertAgentNeighbor(const Agent3D *agent, float &rangeSq)
-	{
-		// no point processing same agent
-		if (this == agent) {
-			return;
-		}
-		// ignore other agent if layers/mask bitmasks have no matching bit
-		if ((avoidance_mask_ & agent->avoidance_layers_) == 0) {
-			return;
-		}
-
-		if (avoidance_priority_ > agent->avoidance_priority_) {
-			return;
-		}
-
-		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 Agent3D::update(RVOSimulator3D *sim_)
-	{
-		velocity_ = newVelocity_;
-		position_ += velocity_ * sim_->timeStep_;
-	}
-
-	bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line3D &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) <= RVO3D_EPSILON) {
-				/* Lines3D 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;
-	}
-
-	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 <= RVO3D_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) <= RVO3D_EPSILON) {
-					/* Planes planeNo and i are (almost) parallel, and plane i fully invalidates plane planeNo. */
-					return false;
-				}
-
-				Line3D 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;
-	}
-
-	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 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) <= RVO3D_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);
-			}
-		}
-	}
-}

thirdparty/rvo2/rvo2_3d/Agent3d.h

@@ -1,14 +1,15 @@
 /*
- * Agent.h
+ * Agent3d.h
  * RVO2-3D Library
  *
- * Copyright 2008 University of North Carolina at Chapel Hill
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
- *     http://www.apache.org/licenses/LICENSE-2.0
+ *     https://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
@@ -27,80 +28,97 @@
  * Chapel Hill, N.C. 27599-3175
  * United States of America
  *
- * <http://gamma.cs.unc.edu/RVO2/>
+ * <https://gamma.cs.unc.edu/RVO2/>
  */
 
-/**
- * \file    Agent.h
- * \brief   Contains the Agent class.
- */
 #ifndef RVO3D_AGENT_H_
 #define RVO3D_AGENT_H_
 
+/**
+ * @file  Agent3d.h
+ * @brief Contains the Agent3D class.
+ */
+
 #include <cstddef>
 #include <cstdint>
 #include <utility>
 #include <vector>
 
-#include "RVOSimulator3d.h"
+#include "Plane.h"
 #include "Vector3.h"
 
 namespace RVO3D {
-	/**
-	 * \brief   Defines an agent in the simulation.
-	 */
-	class Agent3D {
-	public:
-		/**
-		 * \brief   Constructs an agent instance.
-		 * \param   sim  The simulator instance.
-		 */
-		explicit Agent3D();
-
-		/**
-		 * \brief   Computes the neighbors of this agent.
-		 */
-		void computeNeighbors(RVOSimulator3D *sim_);
-
-		/**
-		 * \brief   Computes the new velocity of this agent.
-		 */
-		void computeNewVelocity(RVOSimulator3D *sim_);
-
-		/**
-		 * \brief   Inserts an agent neighbor into the set of neighbors of this agent.
-		 * \param   agent    A pointer to the agent to be inserted.
-		 * \param   rangeSq  The squared range around this agent.
-		 */
-		void insertAgentNeighbor(const Agent3D *agent, float &rangeSq);
-
-		/**
-		 * \brief   Updates the three-dimensional position and three-dimensional velocity of this agent.
-		 */
-		void update(RVOSimulator3D *sim_);
-
-		Vector3 newVelocity_;
-		Vector3 position_;
-		Vector3 prefVelocity_;
-		Vector3 velocity_;
-		RVOSimulator3D *sim_;
-		size_t id_;
-		size_t maxNeighbors_;
-		float maxSpeed_;
-		float neighborDist_;
-		float radius_;
-		float timeHorizon_;
-		float timeHorizonObst_;
-		std::vector<std::pair<float, const Agent3D *> > agentNeighbors_;
-		std::vector<Plane> orcaPlanes_;
-		float height_ = 1.0;
-		uint32_t avoidance_layers_ = 1;
-		uint32_t avoidance_mask_ = 1;
-		float avoidance_priority_ = 1.0;
-
-		friend class KdTree3D;
-		friend class RVOSimulator3D;
-	};
-}
+class RVOSimulator3D;
+
+/**
+ * @brief Defines an agent in the simulation.
+ */
+class Agent3D {
+ public:
+  /**
+   * @brief     Constructs an agent instance.
+   * @param[in] sim The simulator instance.
+   */
+  explicit Agent3D();
+
+  /**
+   * @brief Destroys this agent instance.
+   */
+  ~Agent3D();
+
+  /**
+   * @brief Computes the neighbors of this agent.
+   */
+  void computeNeighbors(RVOSimulator3D *sim_);
+
+  /**
+   * @brief Computes the new velocity of this agent.
+   */
+  void computeNewVelocity(RVOSimulator3D *sim_);
+
+  /**
+   * @brief     Inserts an agent neighbor into the set of neighbors of this
+   *            agent.
+   * @param[in] agent   A pointer to the agent to be inserted.
+   * @param[in] rangeSq The squared range around this agent.
+   */
+  void insertAgentNeighbor(const Agent3D *agent,
+                           float &rangeSq); /* NOLINT(runtime/references) */
+
+  /**
+   * @brief Updates the three-dimensional position and three-dimensional
+   *        velocity of this agent.
+   */
+  void update(RVOSimulator3D *sim_);;
+
+  /* Not implemented. */
+  Agent3D(const Agent3D &other);
+
+  /* Not implemented. */
+  Agent3D &operator=(const Agent3D &other);
+
+  Vector3 newVelocity_;
+  Vector3 position_;
+  Vector3 prefVelocity_;
+  Vector3 velocity_;
+  RVOSimulator3D *sim_;
+  std::size_t id_;
+  std::size_t maxNeighbors_;
+  float maxSpeed_;
+  float neighborDist_;
+  float radius_;
+  float timeHorizon_;
+  float timeHorizonObst_;
+  std::vector<std::pair<float, const Agent3D *> > agentNeighbors_;
+  std::vector<Plane> orcaPlanes_;
+	float height_ = 1.0;
+	uint32_t avoidance_layers_ = 1;
+	uint32_t avoidance_mask_ = 1;
+	float avoidance_priority_ = 1.0;
+
+  friend class KdTree3D;
+  friend class RVOSimulator3D;
+};
+} /* namespace RVO3D */
 
 #endif /* RVO3D_AGENT_H_ */

thirdparty/rvo2/rvo2_3d/KdTree3d.cc

@@ -0,0 +1,264 @@
+/*
+ * KdTree3d.cc
+ * RVO2-3D Library
+ *
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     https://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Please send all bug reports to <[email protected]>.
+ *
+ * The authors may be contacted via:
+ *
+ * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
+ * Dept. of Computer Science
+ * 201 S. Columbia St.
+ * Frederick P. Brooks, Jr. Computer Science Bldg.
+ * Chapel Hill, N.C. 27599-3175
+ * United States of America
+ *
+ * <https://gamma.cs.unc.edu/RVO2/>
+ */
+
+#include "KdTree3d.h"
+
+#include <algorithm>
+#include <utility>
+
+#include "Agent3d.h"
+#include "RVOSimulator3d.h"
+#include "Vector3.h"
+
+namespace RVO3D {
+namespace {
+/**
+ * @brief The maximum size of a k-D leaf node.
+ */
+const std::size_t RVO3D_MAX_LEAF_SIZE = 10U;
+} /* namespace */
+
+/**
+ * @brief Defines an agent k-D tree node.
+ */
+class KdTree3D::AgentTreeNode {
+ public:
+  /**
+   * @brief Constructs an agent k-D tree node.
+   */
+  AgentTreeNode();
+
+  /**
+   * @brief The beginning node number.
+   */
+  std::size_t begin;
+
+  /**
+   * @brief The ending node number.
+   */
+  std::size_t end;
+
+  /**
+   * @brief The left node number.
+   */
+  std::size_t left;
+
+  /**
+   * @brief The right node number.
+   */
+  std::size_t right;
+
+  /**
+   * @brief The maximum coordinates.
+   */
+  Vector3 maxCoord;
+
+  /**
+   * @brief The minimum coordinates.
+   */
+  Vector3 minCoord;
+};
+
+KdTree3D::AgentTreeNode::AgentTreeNode()
+    : begin(0U), end(0U), left(0U), right(0U) {}
+
+KdTree3D::KdTree3D(RVOSimulator3D *sim) : sim_(sim) {}
+
+KdTree3D::~KdTree3D() {}
+
+void KdTree3D::buildAgentTree(std::vector<Agent3D *> agents) {
+  agents_.swap(agents_);
+
+  if (!agents_.empty()) {
+    agentTree_.resize(2U * agents_.size() - 1U);
+    buildAgentTreeRecursive(0U, agents_.size(), 0U);
+  }
+}
+
+void KdTree3D::buildAgentTreeRecursive(std::size_t begin, std::size_t end,
+                                     std::size_t node) {
+  agentTree_[node].begin = begin;
+  agentTree_[node].end = end;
+  agentTree_[node].minCoord = agents_[begin]->position_;
+  agentTree_[node].maxCoord = agents_[begin]->position_;
+
+  for (std::size_t i = begin + 1U; i < end; ++i) {
+    agentTree_[node].maxCoord[0] =
+        std::max(agentTree_[node].maxCoord[0], agents_[i]->position_.x());
+    agentTree_[node].minCoord[0] =
+        std::min(agentTree_[node].minCoord[0], agents_[i]->position_.x());
+    agentTree_[node].maxCoord[1] =
+        std::max(agentTree_[node].maxCoord[1], agents_[i]->position_.y());
+    agentTree_[node].minCoord[1] =
+        std::min(agentTree_[node].minCoord[1], agents_[i]->position_.y());
+    agentTree_[node].maxCoord[2] =
+        std::max(agentTree_[node].maxCoord[2], agents_[i]->position_.z());
+    agentTree_[node].minCoord[2] =
+        std::min(agentTree_[node].minCoord[2], agents_[i]->position_.z());
+  }
+
+  if (end - begin > RVO3D_MAX_LEAF_SIZE) {
+    /* No leaf node. */
+    std::size_t coord = 0U;
+
+    if (agentTree_[node].maxCoord[0] - agentTree_[node].minCoord[0] >
+            agentTree_[node].maxCoord[1] - agentTree_[node].minCoord[1] &&
+        agentTree_[node].maxCoord[0] - agentTree_[node].minCoord[0] >
+            agentTree_[node].maxCoord[2] - agentTree_[node].minCoord[2]) {
+      coord = 0U;
+    } else if (agentTree_[node].maxCoord[1] - agentTree_[node].minCoord[1] >
+               agentTree_[node].maxCoord[2] - agentTree_[node].minCoord[2]) {
+      coord = 1U;
+    } else {
+      coord = 2U;
+    }
+
+    const float splitValue = 0.5F * (agentTree_[node].maxCoord[coord] +
+                                     agentTree_[node].minCoord[coord]);
+
+    std::size_t left = begin;
+
+    std::size_t right = end;
+
+    while (left < right) {
+      while (left < right && agents_[left]->position_[coord] < splitValue) {
+        ++left;
+      }
+
+      while (right > left &&
+             agents_[right - 1U]->position_[coord] >= splitValue) {
+        --right;
+      }
+
+      if (left < right) {
+        std::swap(agents_[left], agents_[right - 1U]);
+        ++left;
+        --right;
+      }
+    }
+
+    std::size_t leftSize = left - begin;
+
+    if (leftSize == 0U) {
+      ++leftSize;
+      ++left;
+    }
+
+    agentTree_[node].left = node + 1U;
+    agentTree_[node].right = node + 2U * leftSize;
+
+    buildAgentTreeRecursive(begin, left, agentTree_[node].left);
+    buildAgentTreeRecursive(left, end, agentTree_[node].right);
+  }
+}
+
+void KdTree3D::computeAgentNeighbors(Agent3D *agent, float rangeSq) const {
+  queryAgentTreeRecursive(agent, rangeSq, 0U);
+}
+
+void KdTree3D::queryAgentTreeRecursive(Agent3D *agent, float &rangeSq,
+                                     std::size_t node) const {
+  if (agentTree_[node].end - agentTree_[node].begin <= RVO3D_MAX_LEAF_SIZE) {
+    for (std::size_t i = agentTree_[node].begin; i < agentTree_[node].end;
+         ++i) {
+      agent->insertAgentNeighbor(agents_[i], rangeSq);
+    }
+  } else {
+    const float distSqLeftMinX =
+        std::max(0.0F, agentTree_[agentTree_[node].left].minCoord[0] -
+                           agent->position_.x());
+    const float distSqLeftMaxX =
+        std::max(0.0F, agent->position_.x() -
+                           agentTree_[agentTree_[node].left].maxCoord[0]);
+    const float distSqLeftMinY =
+        std::max(0.0F, agentTree_[agentTree_[node].left].minCoord[1] -
+                           agent->position_.y());
+    const float distSqLeftMaxY =
+        std::max(0.0F, agent->position_.y() -
+                           agentTree_[agentTree_[node].left].maxCoord[1]);
+    const float distSqLeftMinZ =
+        std::max(0.0F, agentTree_[agentTree_[node].left].minCoord[2] -
+                           agent->position_.z());
+    const float distSqLeftMaxZ =
+        std::max(0.0F, agent->position_.z() -
+                           agentTree_[agentTree_[node].left].maxCoord[2]);
+
+    const float distSqLeft =
+        distSqLeftMinX * distSqLeftMinX + distSqLeftMaxX * distSqLeftMaxX +
+        distSqLeftMinY * distSqLeftMinY + distSqLeftMaxY * distSqLeftMaxY +
+        distSqLeftMinZ * distSqLeftMinZ + distSqLeftMaxZ * distSqLeftMaxZ;
+
+    const float distSqRightMinX =
+        std::max(0.0F, agentTree_[agentTree_[node].right].minCoord[0] -
+                           agent->position_.x());
+    const float distSqRightMaxX =
+        std::max(0.0F, agent->position_.x() -
+                           agentTree_[agentTree_[node].right].maxCoord[0]);
+    const float distSqRightMinY =
+        std::max(0.0F, agentTree_[agentTree_[node].right].minCoord[1] -
+                           agent->position_.y());
+    const float distSqRightMaxY =
+        std::max(0.0F, agent->position_.y() -
+                           agentTree_[agentTree_[node].right].maxCoord[1]);
+    const float distSqRightMinZ =
+        std::max(0.0F, agentTree_[agentTree_[node].right].minCoord[2] -
+                           agent->position_.z());
+    const float distSqRightMaxZ =
+        std::max(0.0F, agent->position_.z() -
+                           agentTree_[agentTree_[node].right].maxCoord[2]);
+
+    const float distSqRight =
+        distSqRightMinX * distSqRightMinX + distSqRightMaxX * distSqRightMaxX +
+        distSqRightMinY * distSqRightMinY + distSqRightMaxY * distSqRightMaxY +
+        distSqRightMinZ * distSqRightMinZ + distSqRightMaxZ * distSqRightMaxZ;
+
+    if (distSqLeft < distSqRight) {
+      if (distSqLeft < rangeSq) {
+        queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
+
+        if (distSqRight < rangeSq) {
+          queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
+        }
+      }
+    } else {
+      if (distSqRight < rangeSq) {
+        queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
+
+        if (distSqLeft < rangeSq) {
+          queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
+        }
+      }
+    }
+  }
+}
+} /* namespace RVO3D */

thirdparty/rvo2/rvo2_3d/KdTree3d.cpp

@@ -1,161 +0,0 @@
-/*
- * KdTree.cpp
- * RVO2-3D Library
- *
- * Copyright 2008 University of North Carolina at Chapel Hill
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     https://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
- * Please send all bug reports to <[email protected]>.
- *
- * The authors may be contacted via:
- *
- * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
- * Dept. of Computer Science
- * 201 S. Columbia St.
- * Frederick P. Brooks, Jr. Computer Science Bldg.
- * Chapel Hill, N.C. 27599-3175
- * United States of America
- *
- * <https://gamma.cs.unc.edu/RVO2/>
- */
-
-#include "KdTree3d.h"
-
-#include <algorithm>
-
-#include "Agent3d.h"
-#include "Definitions.h"
-#include "RVOSimulator3d.h"
-
-namespace RVO3D {
-	const size_t RVO3D_MAX_LEAF_SIZE = 10;
-
-	KdTree3D::KdTree3D(RVOSimulator3D *sim) : sim_(sim) { }
-
-	void KdTree3D::buildAgentTree(std::vector<Agent3D *> agents)
-	{
-		agents_.swap(agents);
-
-		if (!agents_.empty()) {
-			agentTree_.resize(2 * agents_.size() - 1);
-			buildAgentTreeRecursive(0, agents_.size(), 0);
-		}
-	}
-
-	void KdTree3D::buildAgentTreeRecursive(size_t begin, size_t end, size_t node)
-	{
-		agentTree_[node].begin = begin;
-		agentTree_[node].end = end;
-		agentTree_[node].minCoord = agents_[begin]->position_;
-		agentTree_[node].maxCoord = agents_[begin]->position_;
-
-		for (size_t i = begin + 1; i < end; ++i) {
-			agentTree_[node].maxCoord[0] = std::max(agentTree_[node].maxCoord[0], agents_[i]->position_.x());
-			agentTree_[node].minCoord[0] = std::min(agentTree_[node].minCoord[0], agents_[i]->position_.x());
-			agentTree_[node].maxCoord[1] = std::max(agentTree_[node].maxCoord[1], agents_[i]->position_.y());
-			agentTree_[node].minCoord[1] = std::min(agentTree_[node].minCoord[1], agents_[i]->position_.y());
-			agentTree_[node].maxCoord[2] = std::max(agentTree_[node].maxCoord[2], agents_[i]->position_.z());
-			agentTree_[node].minCoord[2] = std::min(agentTree_[node].minCoord[2], agents_[i]->position_.z());
-		}
-
-		if (end - begin > RVO3D_MAX_LEAF_SIZE) {
-			/* No leaf node. */
-			size_t coord;
-
-			if (agentTree_[node].maxCoord[0] - agentTree_[node].minCoord[0] > agentTree_[node].maxCoord[1] - agentTree_[node].minCoord[1] && agentTree_[node].maxCoord[0] - agentTree_[node].minCoord[0] > agentTree_[node].maxCoord[2] - agentTree_[node].minCoord[2]) {
-				coord = 0;
-			}
-			else if (agentTree_[node].maxCoord[1] - agentTree_[node].minCoord[1] > agentTree_[node].maxCoord[2] - agentTree_[node].minCoord[2]) {
-				coord = 1;
-			}
-			else {
-				coord = 2;
-			}
-
-			const float splitValue = 0.5f * (agentTree_[node].maxCoord[coord] + agentTree_[node].minCoord[coord]);
-
-			size_t left = begin;
-
-			size_t right = end;
-
-			while (left < right) {
-				while (left < right && agents_[left]->position_[coord] < splitValue) {
-					++left;
-				}
-
-				while (right > left && agents_[right - 1]->position_[coord] >= splitValue) {
-					--right;
-				}
-
-				if (left < right) {
-					std::swap(agents_[left], agents_[right - 1]);
-					++left;
-					--right;
-				}
-			}
-
-			size_t leftSize = left - begin;
-
-			if (leftSize == 0) {
-				++leftSize;
-				++left;
-				++right;
-			}
-
-			agentTree_[node].left = node + 1;
-			agentTree_[node].right = node + 2 * leftSize;
-
-			buildAgentTreeRecursive(begin, left, agentTree_[node].left);
-			buildAgentTreeRecursive(left, end, agentTree_[node].right);
-		}
-	}
-
-	void KdTree3D::computeAgentNeighbors(Agent3D *agent, float rangeSq) const
-	{
-		queryAgentTreeRecursive(agent, rangeSq, 0);
-	}
-
-	void KdTree3D::queryAgentTreeRecursive(Agent3D *agent, float &rangeSq, size_t node) const
-	{
-		if (agentTree_[node].end - agentTree_[node].begin <= RVO3D_MAX_LEAF_SIZE) {
-			for (size_t i = agentTree_[node].begin; i < agentTree_[node].end; ++i) {
-				agent->insertAgentNeighbor(agents_[i], rangeSq);
-			}
-		}
-		else {
-			const float distSqLeft = sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minCoord[0] - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].left].maxCoord[0])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minCoord[1] - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].left].maxCoord[1])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minCoord[2] - agent->position_.z())) + sqr(std::max(0.0f, agent->position_.z() - agentTree_[agentTree_[node].left].maxCoord[2]));
-
-			const float distSqRight = sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minCoord[0] - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].right].maxCoord[0])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minCoord[1] - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].right].maxCoord[1])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minCoord[2] - agent->position_.z())) + sqr(std::max(0.0f, agent->position_.z() - agentTree_[agentTree_[node].right].maxCoord[2]));
-
-			if (distSqLeft < distSqRight) {
-				if (distSqLeft < rangeSq) {
-					queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
-
-					if (distSqRight < rangeSq) {
-						queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
-					}
-				}
-			}
-			else {
-				if (distSqRight < rangeSq) {
-					queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
-
-					if (distSqLeft < rangeSq) {
-						queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
-					}
-				}
-			}
-		}
-	}
-}

thirdparty/rvo2/rvo2_3d/KdTree3d.h

@@ -1,8 +1,9 @@
 /*
- * KdTree.h
+ * KdTree3d.h
  * RVO2-3D Library
  *
- * Copyright 2008 University of North Carolina at Chapel Hill
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -29,92 +30,88 @@
  *
  * <https://gamma.cs.unc.edu/RVO2/>
  */
-/**
- * \file    KdTree.h
- * \brief   Contains the KdTree class.
- */
+
 #ifndef RVO3D_KD_TREE_H_
 #define RVO3D_KD_TREE_H_
 
+/**
+ * @file  KdTree3d.h
+ * @brief Contains the KdTree3D class.
+ */
+
 #include <cstddef>
 #include <vector>
 
-#include "Vector3.h"
-
 namespace RVO3D {
-	class Agent3D;
-	class RVOSimulator3D;
-
-	/**
-	 * \brief   Defines <i>k</i>d-trees for agents in the simulation.
-	 */
-	class KdTree3D {
-	public:
-		/**
-		 * \brief   Defines an agent <i>k</i>d-tree node.
-		 */
-		class AgentTreeNode3D {
-		public:
-			/**
-			 * \brief   The beginning node number.
-			 */
-			size_t begin;
-
-			/**
-			 * \brief   The ending node number.
-			 */
-			size_t end;
-
-			/**
-			 * \brief   The left node number.
-			 */
-			size_t left;
-
-			/**
-			 * \brief   The right node number.
-			 */
-			size_t right;
-
-			/**
-			 * \brief   The maximum coordinates.
-			 */
-			Vector3 maxCoord;
-
-			/**
-			 * \brief   The minimum coordinates.
-			 */
-			Vector3 minCoord;
-		};
-
-		/**
-		 * \brief   Constructs a <i>k</i>d-tree instance.
-		 * \param   sim  The simulator instance.
-		 */
-		explicit KdTree3D(RVOSimulator3D *sim);
-
-		/**
-		 * \brief   Builds an agent <i>k</i>d-tree.
-		 */
-		void buildAgentTree(std::vector<Agent3D *> agents);
-
-		void buildAgentTreeRecursive(size_t begin, size_t end, size_t node);
-
-		/**
-		 * \brief   Computes the agent neighbors of the specified agent.
-		 * \param   agent    A pointer to the agent for which agent neighbors are to be computed.
-		 * \param   rangeSq  The squared range around the agent.
-		 */
-		void computeAgentNeighbors(Agent3D *agent, float rangeSq) const;
-
-		void queryAgentTreeRecursive(Agent3D *agent, float &rangeSq, size_t node) const;
-
-		std::vector<Agent3D *> agents_;
-		std::vector<AgentTreeNode3D> agentTree_;
-		RVOSimulator3D *sim_;
-
-		friend class Agent3D;
-		friend class RVOSimulator3D;
-	};
-}
+class Agent3D;
+class RVOSimulator3D;
+
+/**
+ * @brief Defines a k-D tree for agents in the simulation.
+ */
+class KdTree3D {
+ public:
+  class AgentTreeNode;
+
+  /**
+   * @brief     Constructs a k-D tree instance.
+   * @param[in] sim The simulator instance.
+   */
+  explicit KdTree3D(RVOSimulator3D *sim);
+
+  /**
+   * @brief Destroys this k-D tree instance.
+   */
+  ~KdTree3D();
+
+  /**
+   * @brief Builds an agent k-D tree.
+   */
+  void buildAgentTree(std::vector<Agent3D *> agents);
+
+  /**
+   * @brief     Recursive function to build a k-D tree.
+   * @param[in] begin The beginning k-D tree node.
+   * @param[in] end   The ending k-D tree node.
+   * @param[in] node  The current k-D tree node.
+   */
+  void buildAgentTreeRecursive(std::size_t begin, std::size_t end,
+                               std::size_t node);
+
+  /**
+   * @brief     Computes the agent neighbors of the specified agent.
+   * @param[in] agent   A pointer to the agent for which agent neighbors are to
+   *                    be computed.
+   * @param[in] rangeSq The squared range around the agent.
+   */
+  void computeAgentNeighbors(Agent3D *agent, float rangeSq) const;
+
+  /**
+   * @brief         Recursive function to compute the neighbors of the specified
+   *                agent.
+   * @param[in]     agent   A pointer to the agent for which neighbors are to be
+   *                        computed.
+   * @param[in,out] rangeSq The squared range around the agent.
+   * @param[in]     node    The current k-D tree node.
+   */
+
+  void queryAgentTreeRecursive(Agent3D *agent,
+                               float &rangeSq, /* NOLINT(runtime/references) */
+                               std::size_t node) const;
+
+  /* Not implemented. */
+  KdTree3D(const KdTree3D &other);
+
+  /* Not implemented. */
+  KdTree3D &operator=(const KdTree3D &other);
+
+  std::vector<Agent3D *> agents_;
+  std::vector<AgentTreeNode> agentTree_;
+  RVOSimulator3D *sim_;
+
+  friend class Agent3D;
+  friend class RVOSimulator3D;
+};
+} /* namespace RVO3D */
 
 #endif /* RVO3D_KD_TREE_H_ */

thirdparty/rvo2/rvo2_3d/Definitions.h → thirdparty/rvo2/rvo2_3d/Plane.cc

@@ -1,8 +1,9 @@
 /*
- * Definitions.h
+ * Plane.cc
  * RVO2-3D Library
  *
- * Copyright 2008 University of North Carolina at Chapel Hill
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -30,24 +31,8 @@
  * <https://gamma.cs.unc.edu/RVO2/>
  */
 
-/**
- * \file   Definitions.h
- * \brief  Contains functions and constants used in multiple classes.
- */
-
-#ifndef RVO3D_DEFINITIONS_H_
-#define RVO3D_DEFINITIONS_H_
+#include "Plane.h"
 
 namespace RVO3D {
-	/**
-	 * \brief   Computes the square of a float.
-	 * \param   scalar  The float to be squared.
-	 * \return  The square of the float.
-	 */
-	inline float sqr(float scalar)
-	{
-		return scalar * scalar;
-	}
-}
-
-#endif /* RVO3D_DEFINITIONS_H_ */
+Plane::Plane() {}
+} /* namespace RVO3D */

thirdparty/rvo2/rvo2_3d/Plane.h

@@ -0,0 +1,67 @@
+/*
+ * Plane.h
+ * RVO2-3D Library
+ *
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     https://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Please send all bug reports to <[email protected]>.
+ *
+ * The authors may be contacted via:
+ *
+ * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
+ * Dept. of Computer Science
+ * 201 S. Columbia St.
+ * Frederick P. Brooks, Jr. Computer Science Bldg.
+ * Chapel Hill, N.C. 27599-3175
+ * United States of America
+ *
+ * <https://gamma.cs.unc.edu/RVO2/>
+ */
+
+#ifndef RVO3D_PLANE_H_
+#define RVO3D_PLANE_H_
+
+/**
+ * @file  Plane.h
+ * @brief Contains the Plane class.
+ */
+
+#include "Vector3.h"
+
+namespace RVO3D {
+/**
+ * @brief Defines a plane.
+ */
+class Plane {
+ public:
+  /**
+   * @brief Constructs a plane.
+   */
+  Plane();
+
+  /**
+   * @brief A point on the plane.
+   */
+  Vector3 point;
+
+  /**
+   * @brief The normal to the plane.
+   */
+  Vector3 normal;
+};
+} /* namespace RVO3D */
+
+#endif /* RVO3D_PLANE_H_ */

thirdparty/rvo2/rvo2_3d/RVOSimulator3d.cc

@@ -0,0 +1,250 @@
+/*
+ * RVOSimulator3d.cc
+ * RVO2-3D Library
+ *
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     https://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Please send all bug reports to <[email protected]>.
+ *
+ * The authors may be contacted via:
+ *
+ * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
+ * Dept. of Computer Science
+ * 201 S. Columbia St.
+ * Frederick P. Brooks, Jr. Computer Science Bldg.
+ * Chapel Hill, N.C. 27599-3175
+ * United States of America
+ *
+ * <https://gamma.cs.unc.edu/RVO2/>
+ */
+
+#include "RVOSimulator3d.h"
+
+#include <utility>
+
+#ifdef _OPENMP
+#include <omp.h>
+#endif /* _OPENMP */
+
+#include "Agent3d.h"
+#include "KdTree3d.h"
+#include "Plane.h"
+
+namespace RVO3D {
+RVOSimulator3D::RVOSimulator3D()
+    : defaultAgent_(NULL),
+      kdTree_(new KdTree3D(this)),
+      globalTime_(0.0F),
+      timeStep_(0.0F) {}
+
+RVOSimulator3D::RVOSimulator3D(float timeStep, float neighborDist,
+                           std::size_t maxNeighbors, float timeHorizon,
+                           float radius, float maxSpeed,
+                           const Vector3 &velocity)
+    : defaultAgent_(new Agent3D()),
+      kdTree_(new KdTree3D(this)),
+      globalTime_(0.0F),
+      timeStep_(timeStep) {
+  defaultAgent_->maxNeighbors_ = maxNeighbors;
+  defaultAgent_->maxSpeed_ = maxSpeed;
+  defaultAgent_->neighborDist_ = neighborDist;
+  defaultAgent_->radius_ = radius;
+  defaultAgent_->timeHorizon_ = timeHorizon;
+  defaultAgent_->velocity_ = velocity;
+}
+
+RVOSimulator3D::~RVOSimulator3D() {
+  delete defaultAgent_;
+  delete kdTree_;
+
+  for (std::size_t i = 0U; i < agents_.size(); ++i) {
+    delete agents_[i];
+  }
+}
+
+std::size_t RVOSimulator3D::getAgentNumAgentNeighbors(std::size_t agentNo) const {
+  return agents_[agentNo]->agentNeighbors_.size();
+}
+
+std::size_t RVOSimulator3D::getAgentAgentNeighbor(std::size_t agentNo,
+                                                std::size_t neighborNo) const {
+  return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_;
+}
+
+std::size_t RVOSimulator3D::getAgentNumORCAPlanes(std::size_t agentNo) const {
+  return agents_[agentNo]->orcaPlanes_.size();
+}
+
+const Plane &RVOSimulator3D::getAgentORCAPlane(std::size_t agentNo,
+                                             std::size_t planeNo) const {
+  return agents_[agentNo]->orcaPlanes_[planeNo];
+}
+
+void RVOSimulator3D::removeAgent(std::size_t agentNo) {
+  delete agents_[agentNo];
+  agents_[agentNo] = agents_.back();
+  agents_.pop_back();
+}
+
+std::size_t RVOSimulator3D::addAgent(const Vector3 &position) {
+  if (defaultAgent_ == NULL) {
+    return RVO3D_ERROR;
+  }
+
+  Agent3D *agent = new Agent3D();
+
+  agent->position_ = position;
+  agent->maxNeighbors_ = defaultAgent_->maxNeighbors_;
+  agent->maxSpeed_ = defaultAgent_->maxSpeed_;
+  agent->neighborDist_ = defaultAgent_->neighborDist_;
+  agent->radius_ = defaultAgent_->radius_;
+  agent->timeHorizon_ = defaultAgent_->timeHorizon_;
+  agent->velocity_ = defaultAgent_->velocity_;
+
+  agent->id_ = agents_.size();
+
+  agents_.push_back(agent);
+
+  return agents_.size() - 1U;
+}
+
+std::size_t RVOSimulator3D::addAgent(const Vector3 &position, float neighborDist,
+                                   std::size_t maxNeighbors, float timeHorizon,
+                                   float radius, float maxSpeed,
+                                   const Vector3 &velocity) {
+  Agent3D *agent = new Agent3D();
+
+  agent->position_ = position;
+  agent->maxNeighbors_ = maxNeighbors;
+  agent->maxSpeed_ = maxSpeed;
+  agent->neighborDist_ = neighborDist;
+  agent->radius_ = radius;
+  agent->timeHorizon_ = timeHorizon;
+  agent->velocity_ = velocity;
+
+  agent->id_ = agents_.size();
+
+  agents_.push_back(agent);
+
+  return agents_.size() - 1U;
+}
+
+void RVOSimulator3D::doStep() {
+  kdTree_->buildAgentTree(agents_);
+
+#ifdef _OPENMP
+#pragma omp parallel for
+#endif /* _OPENMP */
+  for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
+    agents_[i]->computeNeighbors(this);
+    agents_[i]->computeNewVelocity(this);
+  }
+
+#ifdef _OPENMP
+#pragma omp parallel for
+#endif /* _OPENMP */
+  for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
+    agents_[i]->update(this);
+  }
+
+  globalTime_ += timeStep_;
+}
+
+std::size_t RVOSimulator3D::getAgentMaxNeighbors(std::size_t agentNo) const {
+  return agents_[agentNo]->maxNeighbors_;
+}
+
+float RVOSimulator3D::getAgentMaxSpeed(std::size_t agentNo) const {
+  return agents_[agentNo]->maxSpeed_;
+}
+
+float RVOSimulator3D::getAgentNeighborDist(std::size_t agentNo) const {
+  return agents_[agentNo]->neighborDist_;
+}
+
+const Vector3 &RVOSimulator3D::getAgentPosition(std::size_t agentNo) const {
+  return agents_[agentNo]->position_;
+}
+
+const Vector3 &RVOSimulator3D::getAgentPrefVelocity(std::size_t agentNo) const {
+  return agents_[agentNo]->prefVelocity_;
+}
+
+float RVOSimulator3D::getAgentRadius(std::size_t agentNo) const {
+  return agents_[agentNo]->radius_;
+}
+
+float RVOSimulator3D::getAgentTimeHorizon(std::size_t agentNo) const {
+  return agents_[agentNo]->timeHorizon_;
+}
+
+const Vector3 &RVOSimulator3D::getAgentVelocity(std::size_t agentNo) const {
+  return agents_[agentNo]->velocity_;
+}
+
+void RVOSimulator3D::setAgentDefaults(float neighborDist,
+                                    std::size_t maxNeighbors, float timeHorizon,
+                                    float radius, float maxSpeed,
+                                    const Vector3 &velocity) {
+  if (defaultAgent_ == NULL) {
+    defaultAgent_ = new Agent3D();
+  }
+
+  defaultAgent_->maxNeighbors_ = maxNeighbors;
+  defaultAgent_->maxSpeed_ = maxSpeed;
+  defaultAgent_->neighborDist_ = neighborDist;
+  defaultAgent_->radius_ = radius;
+  defaultAgent_->timeHorizon_ = timeHorizon;
+  defaultAgent_->velocity_ = velocity;
+}
+
+void RVOSimulator3D::setAgentMaxNeighbors(std::size_t agentNo,
+                                        std::size_t maxNeighbors) {
+  agents_[agentNo]->maxNeighbors_ = maxNeighbors;
+}
+
+void RVOSimulator3D::setAgentMaxSpeed(std::size_t agentNo, float maxSpeed) {
+  agents_[agentNo]->maxSpeed_ = maxSpeed;
+}
+
+void RVOSimulator3D::setAgentNeighborDist(std::size_t agentNo,
+                                        float neighborDist) {
+  agents_[agentNo]->neighborDist_ = neighborDist;
+}
+
+void RVOSimulator3D::setAgentPosition(std::size_t agentNo,
+                                    const Vector3 &position) {
+  agents_[agentNo]->position_ = position;
+}
+
+void RVOSimulator3D::setAgentPrefVelocity(std::size_t agentNo,
+                                        const Vector3 &prefVelocity) {
+  agents_[agentNo]->prefVelocity_ = prefVelocity;
+}
+
+void RVOSimulator3D::setAgentRadius(std::size_t agentNo, float radius) {
+  agents_[agentNo]->radius_ = radius;
+}
+
+void RVOSimulator3D::setAgentTimeHorizon(std::size_t agentNo, float timeHorizon) {
+  agents_[agentNo]->timeHorizon_ = timeHorizon;
+}
+
+void RVOSimulator3D::setAgentVelocity(std::size_t agentNo,
+                                    const Vector3 &velocity) {
+  agents_[agentNo]->velocity_ = velocity;
+}
+} /* namespace RVO3D */

thirdparty/rvo2/rvo2_3d/RVOSimulator3d.cpp

@@ -1,274 +0,0 @@
-/*
- * RVOSimulator.cpp
- * RVO2-3D Library
- *
- * Copyright 2008 University of North Carolina at Chapel Hill
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
- * Please send all bug reports to <[email protected]>.
- *
- * The authors may be contacted via:
- *
- * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
- * Dept. of Computer Science
- * 201 S. Columbia St.
- * Frederick P. Brooks, Jr. Computer Science Bldg.
- * Chapel Hill, N.C. 27599-3175
- * United States of America
- *
- * <http://gamma.cs.unc.edu/RVO2/>
- */
-
-#include "RVOSimulator3d.h"
-
-#ifdef _OPENMP
-#include <omp.h>
-#endif
-
-#include "Agent3d.h"
-#include "KdTree3d.h"
-
-namespace RVO3D {
-	RVOSimulator3D::RVOSimulator3D() : defaultAgent_(NULL), kdTree_(NULL), globalTime_(0.0f), timeStep_(0.0f)
-	{
-		kdTree_ = new KdTree3D(this);
-	}
-
-	RVOSimulator3D::RVOSimulator3D(float timeStep, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity) : defaultAgent_(NULL), kdTree_(NULL), globalTime_(0.0f), timeStep_(timeStep)
-	{
-		kdTree_ = new KdTree3D(this);
-		defaultAgent_ = new Agent3D();
-
-		defaultAgent_->maxNeighbors_ = maxNeighbors;
-		defaultAgent_->maxSpeed_ = maxSpeed;
-		defaultAgent_->neighborDist_ = neighborDist;
-		defaultAgent_->radius_ = radius;
-		defaultAgent_->timeHorizon_ = timeHorizon;
-		defaultAgent_->velocity_ = velocity;
-	}
-
-	RVOSimulator3D::~RVOSimulator3D()
-	{
-		if (defaultAgent_ != NULL) {
-			delete defaultAgent_;
-		}
-
-		for (size_t i = 0; i < agents_.size(); ++i) {
-			delete agents_[i];
-		}
-
-		if (kdTree_ != NULL) {
-			delete kdTree_;
-		}
-	}
-
-	size_t RVOSimulator3D::getAgentNumAgentNeighbors(size_t agentNo) const
-	{
-		return agents_[agentNo]->agentNeighbors_.size();
-	}
-
-	size_t RVOSimulator3D::getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const
-	{
-		return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_;
-	}
-
-	size_t RVOSimulator3D::getAgentNumORCAPlanes(size_t agentNo) const
-	{
-		return agents_[agentNo]->orcaPlanes_.size();
-	}
-
-	const Plane &RVOSimulator3D::getAgentORCAPlane(size_t agentNo, size_t planeNo) const
-	{
-		return agents_[agentNo]->orcaPlanes_[planeNo];
-	}
-
-	void RVOSimulator3D::removeAgent(size_t agentNo)
-	{
-		delete agents_[agentNo];
-		agents_[agentNo] = agents_.back();
-		agents_.pop_back();
-	}
-
-	size_t RVOSimulator3D::addAgent(const Vector3 &position)
-	{
-		if (defaultAgent_ == NULL) {
-			return RVO3D_ERROR;
-		}
-
-		Agent3D *agent = new Agent3D();
-
-		agent->position_ = position;
-		agent->maxNeighbors_ = defaultAgent_->maxNeighbors_;
-		agent->maxSpeed_ = defaultAgent_->maxSpeed_;
-		agent->neighborDist_ = defaultAgent_->neighborDist_;
-		agent->radius_ = defaultAgent_->radius_;
-		agent->timeHorizon_ = defaultAgent_->timeHorizon_;
-		agent->velocity_ = defaultAgent_->velocity_;
-
-		agent->id_ = agents_.size();
-
-		agents_.push_back(agent);
-
-		return agents_.size() - 1;
-	}
-
-	size_t RVOSimulator3D::addAgent(const Vector3 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity)
-	{
-		Agent3D *agent = new Agent3D();
-
-		agent->position_ = position;
-		agent->maxNeighbors_ = maxNeighbors;
-		agent->maxSpeed_ = maxSpeed;
-		agent->neighborDist_ = neighborDist;
-		agent->radius_ = radius;
-		agent->timeHorizon_ = timeHorizon;
-		agent->velocity_ = velocity;
-
-		agent->id_ = agents_.size();
-
-		agents_.push_back(agent);
-
-		return agents_.size() - 1;
-	}
-
-	void RVOSimulator3D::doStep()
-	{
-		kdTree_->buildAgentTree(agents_);
-
-		for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
-			agents_[i]->computeNeighbors(this);
-			agents_[i]->computeNewVelocity(this);
-		}
-
-		for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
-			agents_[i]->update(this);
-		}
-
-		globalTime_ += timeStep_;
-	}
-
-	size_t RVOSimulator3D::getAgentMaxNeighbors(size_t agentNo) const
-	{
-		return agents_[agentNo]->maxNeighbors_;
-	}
-
-	float RVOSimulator3D::getAgentMaxSpeed(size_t agentNo) const
-	{
-		return agents_[agentNo]->maxSpeed_;
-	}
-
-	float RVOSimulator3D::getAgentNeighborDist(size_t agentNo) const
-	{
-		return agents_[agentNo]->neighborDist_;
-	}
-
-	const Vector3 &RVOSimulator3D::getAgentPosition(size_t agentNo) const
-	{
-		return agents_[agentNo]->position_;
-	}
-
-	const Vector3 &RVOSimulator3D::getAgentPrefVelocity(size_t agentNo) const
-	{
-		return agents_[agentNo]->prefVelocity_;
-	}
-
-	float RVOSimulator3D::getAgentRadius(size_t agentNo) const
-	{
-		return agents_[agentNo]->radius_;
-	}
-
-	float RVOSimulator3D::getAgentTimeHorizon(size_t agentNo) const
-	{
-		return agents_[agentNo]->timeHorizon_;
-	}
-
-	const Vector3 &RVOSimulator3D::getAgentVelocity(size_t agentNo) const
-	{
-		return agents_[agentNo]->velocity_;
-	}
-
-	float RVOSimulator3D::getGlobalTime() const
-	{
-		return globalTime_;
-	}
-
-	size_t RVOSimulator3D::getNumAgents() const
-	{
-		return agents_.size();
-	}
-
-	float RVOSimulator3D::getTimeStep() const
-	{
-		return timeStep_;
-	}
-
-	void RVOSimulator3D::setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity)
-	{
-		if (defaultAgent_ == NULL) {
-			defaultAgent_ = new Agent3D();
-		}
-
-		defaultAgent_->maxNeighbors_ = maxNeighbors;
-		defaultAgent_->maxSpeed_ = maxSpeed;
-		defaultAgent_->neighborDist_ = neighborDist;
-		defaultAgent_->radius_ = radius;
-		defaultAgent_->timeHorizon_ = timeHorizon;
-		defaultAgent_->velocity_ = velocity;
-	}
-
-	void RVOSimulator3D::setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors)
-	{
-		agents_[agentNo]->maxNeighbors_ = maxNeighbors;
-	}
-
-	void RVOSimulator3D::setAgentMaxSpeed(size_t agentNo, float maxSpeed)
-	{
-		agents_[agentNo]->maxSpeed_ = maxSpeed;
-	}
-
-	void RVOSimulator3D::setAgentNeighborDist(size_t agentNo, float neighborDist)
-	{
-		agents_[agentNo]->neighborDist_ = neighborDist;
-	}
-
-	void RVOSimulator3D::setAgentPosition(size_t agentNo, const Vector3 &position)
-	{
-		agents_[agentNo]->position_ = position;
-	}
-
-	void RVOSimulator3D::setAgentPrefVelocity(size_t agentNo, const Vector3 &prefVelocity)
-	{
-		agents_[agentNo]->prefVelocity_ = prefVelocity;
-	}
-
-	void RVOSimulator3D::setAgentRadius(size_t agentNo, float radius)
-	{
-		agents_[agentNo]->radius_ = radius;
-	}
-
-	void RVOSimulator3D::setAgentTimeHorizon(size_t agentNo, float timeHorizon)
-	{
-		agents_[agentNo]->timeHorizon_ = timeHorizon;
-	}
-
-	void RVOSimulator3D::setAgentVelocity(size_t agentNo, const Vector3 &velocity)
-	{
-		agents_[agentNo]->velocity_ = velocity;
-	}
-
-	void RVOSimulator3D::setTimeStep(float timeStep)
-	{
-		timeStep_ = timeStep;
-	}
-}

thirdparty/rvo2/rvo2_3d/RVOSimulator3d.h

@@ -1,14 +1,15 @@
 /*
- * RVOSimulator.h
+ * RVOSimulator3d.h
  * RVO2-3D Library
  *
- * Copyright 2008 University of North Carolina at Chapel Hill
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
- *     http://www.apache.org/licenses/LICENSE-2.0
+ *     https://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
@@ -27,16 +28,17 @@
  * Chapel Hill, N.C. 27599-3175
  * United States of America
  *
- * <http://gamma.cs.unc.edu/RVO2/>
+ * <https://gamma.cs.unc.edu/RVO2/>
  */
 
-/**
- * \file    RVOSimulator.h
- * \brief   Contains the RVOSimulator class.
- */
 #ifndef RVO3D_RVO_SIMULATOR_H_
 #define RVO3D_RVO_SIMULATOR_H_
 
+/**
+ * @file  RVOSimulator3d.h
+ * @brief Contains the RVOSimulator3D class.
+ */
+
 #include <cstddef>
 #include <limits>
 #include <vector>
@@ -44,281 +46,369 @@
 #include "Vector3.h"
 
 namespace RVO3D {
-	class Agent3D;
-	class KdTree3D;
-
-	/**
-	 * \brief   Error value.
-	 *
-	 * A value equal to the largest unsigned integer, which is returned in case of an error by functions in RVO3D::RVOSimulator.
-	 */
-	const size_t RVO3D_ERROR = std::numeric_limits<size_t>::max();
-
-	/**
-	 * \brief   Defines a plane.
-	 */
-	class Plane {
-	public:
-		/**
-		 * \brief   A point on the plane.
-		 */
-		Vector3 point;
-
-		/**
-		 * \brief   The normal to the plane.
-		 */
-		Vector3 normal;
-	};
-
-	/**
-	 * \brief  Defines the simulation.
-	 *
-	 * The main class of the library that contains all simulation functionality.
-	 */
-	class RVOSimulator3D {
-	public:
-		/**
-		 * \brief   Constructs a simulator instance.
-		 */
-		RVOSimulator3D();
-
-		/**
-		 * \brief   Constructs a simulator instance and sets the default properties for any new agent that is added.
-		 * \param   timeStep      The time step of the simulation. Must be positive.
-		 * \param   neighborDist  The default maximum distance (center point to center point) to other agents a new agent takes into account in the navigation. The larger this number, the longer he running time of the simulation. If the number is too low, the simulation will not be safe. Must be non-negative.
-		 * \param   maxNeighbors  The default maximum number of other agents a new agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe.
-		 * \param   timeHorizon   The default minimum amount of time for which a new agent's velocities that are computed by the simulation are safe with respect to other agents. The larger this number, the sooner an agent will respond to the presence of other agents, but the less freedom the agent has in choosing its velocities. Must be positive.
-		 * \param   radius        The default radius of a new agent. Must be non-negative.
-		 * \param   maxSpeed      The default maximum speed of a new agent. Must be non-negative.
-		 * \param   velocity      The default initial three-dimensional linear velocity of a new agent (optional).
-		 */
-		RVOSimulator3D(float timeStep, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity = Vector3());
-
-		/**
-		 * \brief   Destroys this simulator instance.
-		 */
-		~RVOSimulator3D();
-
-		/**
-		 * \brief   Adds a new agent with default properties to the simulation.
-		 * \param   position  The three-dimensional starting position of this agent.
-		 * \return  The number of the agent, or RVO3D::RVO3D_ERROR when the agent defaults have not been set.
-		 */
-		size_t addAgent(const Vector3 &position);
-
-		/**
-		 * \brief   Adds a new agent to the simulation.
-		 * \param   position      The three-dimensional starting position of this agent.
-		 * \param   neighborDist  The maximum distance (center point to center point) to other agents this agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe. Must be non-negative.
-		 * \param   maxNeighbors  The maximum number of other agents this agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe.
-		 * \param   timeHorizon   The minimum amount of time for which this agent's velocities that are computed by the simulation are safe with respect to other agents. The larger this number, the sooner this agent will respond to the presence of other agents, but the less freedom this agent has in choosing its velocities. Must be positive.
-		 * \param   radius        The radius of this agent. Must be non-negative.
-		 * \param   maxSpeed      The maximum speed of this agent. Must be non-negative.
-		 * \param   velocity      The initial three-dimensional linear velocity of this agent (optional).
-		 * \return  The number of the agent.
-		 */
-		size_t addAgent(const Vector3 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity = Vector3());
-
-		/**
-		 * \brief   Lets the simulator perform a simulation step and updates the three-dimensional position and three-dimensional velocity of each agent.
-		 */
-		void doStep();
-
-		/**
-		 * \brief   Returns the specified agent neighbor of the specified agent.
-		 * \param   agentNo     The number of the agent whose agent neighbor is to be retrieved.
-		 * \param   neighborNo  The number of the agent neighbor to be retrieved.
-		 * \return  The number of the neighboring agent.
-		 */
-		size_t getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const;
-
-		/**
-		 * \brief   Returns the maximum neighbor count of a specified agent.
-		 * \param   agentNo  The number of the agent whose maximum neighbor count is to be retrieved.
-		 * \return  The present maximum neighbor count of the agent.
-		 */
-		size_t getAgentMaxNeighbors(size_t agentNo) const;
-
-		/**
-		 * \brief   Returns the maximum speed of a specified agent.
-		 * \param   agentNo  The number of the agent whose maximum speed is to be retrieved.
-		 * \return  The present maximum speed of the agent.
-		 */
-		float getAgentMaxSpeed(size_t agentNo) const;
-
-		/**
-		 * \brief   Returns the maximum neighbor distance of a specified agent.
-		 * \param   agentNo  The number of the agent whose maximum neighbor distance is to be retrieved.
-		 * \return  The present maximum neighbor distance of the agent.
-		 */
-		float getAgentNeighborDist(size_t agentNo) const;
-
-		/**
-		 * \brief   Returns the count of agent neighbors taken into account to compute the current velocity for the specified agent.
-		 * \param   agentNo  The number of the agent whose count of agent neighbors is to be retrieved.
-		 * \return  The count of agent neighbors taken into account to compute the current velocity for the specified agent.
-		 */
-		size_t getAgentNumAgentNeighbors(size_t agentNo) const;
-
-		/**
-		 * \brief   Returns the count of ORCA constraints used to compute the current velocity for the specified agent.
-		 * \param   agentNo  The number of the agent whose count of ORCA constraints is to be retrieved.
-		 * \return  The count of ORCA constraints used to compute the current velocity for the specified agent.
-		 */
-		size_t getAgentNumORCAPlanes(size_t agentNo) const;
-
-		/**
-		 * \brief   Returns the specified ORCA constraint of the specified agent.
-		 * \param   agentNo  The number of the agent whose ORCA constraint is to be retrieved.
-		 * \param   planeNo  The number of the ORCA constraint to be retrieved.
-		 * \return  A plane representing the specified ORCA constraint.
-		 * \note    The halfspace to which the normal of the plane points is the region of permissible velocities with respect to the specified ORCA constraint.
-		 */
-		const Plane &getAgentORCAPlane(size_t agentNo, size_t planeNo) const;
-
-		/**
-		 * \brief   Returns the three-dimensional position of a specified agent.
-		 * \param   agentNo  The number of the agent whose three-dimensional position is to be retrieved.
-		 * \return  The present three-dimensional position of the (center of the) agent.
-		 */
-		const Vector3 &getAgentPosition(size_t agentNo) const;
-
-		/**
-		 * \brief   Returns the three-dimensional preferred velocity of a specified agent.
-		 * \param   agentNo  The number of the agent whose three-dimensional preferred velocity is to be retrieved.
-		 * \return  The present three-dimensional preferred velocity of the agent.
-		 */
-		const Vector3 &getAgentPrefVelocity(size_t agentNo) const;
-
-		/**
-		 * \brief   Returns the radius of a specified agent.
-		 * \param   agentNo  The number of the agent whose radius is to be retrieved.
-		 * \return  The present radius of the agent.
-		 */
-		float getAgentRadius(size_t agentNo) const;
-
-		/**
-		 * \brief   Returns the time horizon of a specified agent.
-		 * \param   agentNo  The number of the agent whose time horizon is to be retrieved.
-		 * \return  The present time horizon of the agent.
-		 */
-		float getAgentTimeHorizon(size_t agentNo) const;
-
-		/**
-		 * \brief   Returns the three-dimensional linear velocity of a specified agent.
-		 * \param   agentNo  The number of the agent whose three-dimensional linear velocity is to be retrieved.
-		 * \return  The present three-dimensional linear velocity of the agent.
-		 */
-		const Vector3 &getAgentVelocity(size_t agentNo) const;
-
-		/**
-		 * \brief   Returns the global time of the simulation.
-		 * \return  The present global time of the simulation (zero initially).
-		 */
-		float getGlobalTime() const;
-
-		/**
-		 * \brief   Returns the count of agents in the simulation.
-		 * \return  The count of agents in the simulation.
-		 */
-		size_t getNumAgents() const;
-
-		/**
-		 * \brief   Returns the time step of the simulation.
-		 * \return  The present time step of the simulation.
-		 */
-		float getTimeStep() const;
-
-		/**
-		 * \brief   Removes an agent from the simulation.
-		 * \param   agentNo  The number of the agent that is to be removed.
-		 * \note    After the removal of the agent, the agent that previously had number getNumAgents() - 1 will now have number agentNo.
-		 */
-		void removeAgent(size_t agentNo);
-
-		/**
-		 * \brief   Sets the default properties for any new agent that is added.
-		 * \param   neighborDist  The default maximum distance (center point to center point) to other agents a new agent takes into account in the navigation. The larger this number, the longer he running time of the simulation. If the number is too low, the simulation will not be safe. Must be non-negative.
-		 * \param   maxNeighbors  The default maximum number of other agents a new agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe.
-		 * \param   timeHorizon   The default minimum amount of time for which a new agent's velocities that are computed by the simulation are safe with respect to other agents. The larger this number, the sooner an agent will respond to the presence of other agents, but the less freedom the agent has in choosing its velocities. Must be positive.
-		 * \param   radius        The default radius of a new agent. Must be non-negative.
-		 * \param   maxSpeed      The default maximum speed of a new agent. Must be non-negative.
-		 * \param   velocity      The default initial three-dimensional linear velocity of a new agent (optional).
-		 */
-		void setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity = Vector3());
-
-		/**
-		 * \brief   Sets the maximum neighbor count of a specified agent.
-		 * \param   agentNo       The number of the agent whose maximum neighbor count is to be modified.
-		 * \param   maxNeighbors  The replacement maximum neighbor count.
-		 */
-		void setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors);
-
-		/**
-		 * \brief   Sets the maximum speed of a specified agent.
-		 * \param   agentNo   The number of the agent whose maximum speed is to be modified.
-		 * \param   maxSpeed  The replacement maximum speed. Must be non-negative.
-		 */
-		void setAgentMaxSpeed(size_t agentNo, float maxSpeed);
-
-		/**
-		 * \brief   Sets the maximum neighbor distance of a specified agent.
-		 * \param   agentNo       The number of the agent whose maximum neighbor distance is to be modified.
-		 * \param   neighborDist  The replacement maximum neighbor distance. Must be non-negative.
-		 */
-		void setAgentNeighborDist(size_t agentNo, float neighborDist);
-
-		/**
-		 * \brief   Sets the three-dimensional position of a specified agent.
-		 * \param   agentNo   The number of the agent whose three-dimensional position is to be modified.
-		 * \param   position  The replacement of the three-dimensional position.
-		 */
-		void setAgentPosition(size_t agentNo, const Vector3 &position);
-
-		/**
-		 * \brief   Sets the three-dimensional preferred velocity of a specified agent.
-		 * \param   agentNo       The number of the agent whose three-dimensional preferred velocity is to be modified.
-		 * \param   prefVelocity  The replacement of the three-dimensional preferred velocity.
-		 */
-		void setAgentPrefVelocity(size_t agentNo, const Vector3 &prefVelocity);
-
-		/**
-		 * \brief   Sets the radius of a specified agent.
-		 * \param   agentNo  The number of the agent whose radius is to be modified.
-		 * \param   radius   The replacement radius. Must be non-negative.
-		 */
-		void setAgentRadius(size_t agentNo, float radius);
-
-		/**
-		 * \brief   Sets the time horizon of a specified agent with respect to other agents.
-		 * \param   agentNo      The number of the agent whose time horizon is to be modified.
-		 * \param   timeHorizon  The replacement time horizon with respect to other agents. Must be positive.
-		 */
-		void setAgentTimeHorizon(size_t agentNo, float timeHorizon);
-
-		/**
-		 * \brief   Sets the three-dimensional linear velocity of a specified agent.
-		 * \param   agentNo   The number of the agent whose three-dimensional linear velocity is to be modified.
-		 * \param   velocity  The replacement three-dimensional linear velocity.
-		 */
-		void setAgentVelocity(size_t agentNo, const Vector3 &velocity);
-
-		/**
-		 * \brief   Sets the time step of the simulation.
-		 * \param   timeStep  The time step of the simulation. Must be positive.
-		 */
-		void setTimeStep(float timeStep);
-
-	public:
-		Agent3D *defaultAgent_;
-		KdTree3D *kdTree_;
-		float globalTime_;
-		float timeStep_;
-		std::vector<Agent3D *> agents_;
-
-		friend class Agent3D;
-		friend class KdTree3D;
-	};
-}
-
-#endif
+class Agent3D;
+class KdTree3D;
+class Plane;
+
+/**
+ * @brief Error value. A value equal to the largest unsigned integer, which is
+ *        returned in case of an error by functions in RVO::RVOSimulator.
+ */
+const std::size_t RVO3D_ERROR = std::numeric_limits<std::size_t>::max();
+
+/**
+ * @brief Defines the simulation. The main class of the library that contains
+ *        all simulation functionality.
+ */
+class RVOSimulator3D {
+ public:
+  /**
+   * @brief Constructs a simulator instance.
+   */
+  RVOSimulator3D();
+
+  /**
+   * @brief     Constructs a simulator instance and sets the default properties
+   *            for any new agent that is added.
+   * @param[in] timeStep     The time step of the simulation. Must be positive.
+   * @param[in] neighborDist The default maximum distance (center point to
+   *                         center point) to other agents a new agent takes
+   *                         into account in the navigation. The larger this
+   *                         number, the longer the running time of the
+   *                         simulation. If the number is too low, the
+   *                         simulation will not be safe. Must be non-negative.
+   * @param[in] maxNeighbors The default maximum number of other agents a new
+   *                         agent takes into account in the navigation. The
+   *                         larger this number, the longer the running time of
+   *                         the simulation. If the number is too low, the
+   *                         simulation will not be safe.
+   * @param[in] timeHorizon  The default minimum amount of time for which a new
+   *                         agent's velocities that are computed by the
+   *                         simulation are safe with respect to other agents.
+   *                         The larger this number, the sooner an agent will
+   *                         respond to the presence of other agents, but the
+   *                         less freedom the agent has in choosing its
+   *                         velocities. Must be positive.
+   * @param[in] radius       The default radius of a new agent. Must be
+   *                         non-negative.
+   * @param[in] maxSpeed     The default maximum speed of a new agent. Must be
+   *                         non-negative.
+   * @param[in] velocity     The default initial three-dimensional linear
+   *                         velocity of a new agent (optional).
+   */
+  RVOSimulator3D(float timeStep, float neighborDist, std::size_t maxNeighbors,
+               float timeHorizon, float radius, float maxSpeed,
+               const Vector3 &velocity = Vector3());
+
+  /**
+   * @brief Destroys this simulator instance.
+   */
+  ~RVOSimulator3D();
+
+  /**
+   * @brief     Adds a new agent with default properties to the simulation.
+   * @param[in] position The three-dimensional starting position of this agent.
+   * @return    The number of the agent or RVO::RVO3D_ERROR when the agent
+   *            defaults have not been set.
+   */
+  std::size_t addAgent(const Vector3 &position);
+
+  /**
+   * @brief      Adds a new agent to the simulation.
+   * @param[in] position     The three-dimensional starting position of this
+   *                          agent.
+   * @param[in] neighborDist The maximum distance (center point to center
+   *                         point) to other agents this agent takes into
+   *                         account in the navigation. The larger this number,
+   *                         the longer the running time of the simulation. If
+   *                         the number is too low, the simulation will not be
+   *                         safe. Must be non-negative.
+   * @param[in] maxNeighbors The maximum number of other agents this agent takes
+   *                         into account in the navigation. The larger this
+   *                         number, the longer the running time of the
+   *                         simulation. If the number is too low, the
+   *                         simulation will not be safe.
+   * @param[in] timeHorizon  The minimum amount of time for which this agent's
+   *                         velocities that are computed by the simulation are
+   *                         safe with respect to other agents. The larger this
+   *                         number, the sooner this agent will respond to the
+   *                         presence of other agents, but the less freedom this
+   *                         agent has in choosing its velocities. Must be
+   *                         positive.
+   * @param[in] radius       The radius of this agent. Must be non-negative.
+   * @param[in] maxSpeed     The maximum speed of this agent. Must be
+   *                         non-negative.
+   * @param[in] velocity     The initial three-dimensional linear velocity of
+   *                         this agent (optional).
+   * @return    The number of the agent.
+   */
+  std::size_t addAgent(const Vector3 &position, float neighborDist,
+                       std::size_t maxNeighbors, float timeHorizon,
+                       float radius, float maxSpeed,
+                       const Vector3 &velocity = Vector3());
+
+  /**
+   * @brief Lets the simulator perform a simulation step and updates the
+   *        three-dimensional position and three-dimensional velocity of each
+   *        agent.
+   */
+  void doStep();
+
+  /**
+   * @brief     Returns the specified agent neighbor of the specified agent.
+   * @param[in] agentNo    The number of the agent whose agent neighbor is to
+   *                       be retrieved.
+   * @param[in] neighborNo The number of the agent neighbor to be retrieved.
+   * @return    The number of the neighboring agent.
+   */
+  std::size_t getAgentAgentNeighbor(std::size_t agentNo,
+                                    std::size_t neighborNo) const;
+
+  /**
+   * @brief     Returns the maximum neighbor count of a specified agent.
+   * @param[in] agentNo The number of the agent whose maximum neighbor count is
+   *                    to be retrieved.
+   * @return    The present maximum neighbor count of the agent.
+   */
+  std::size_t getAgentMaxNeighbors(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the maximum speed of a specified agent.
+   * @param[in] agentNo The number of the agent whose maximum speed is to be
+   *                    retrieved.
+   * @return    The present maximum speed of the agent.
+   */
+  float getAgentMaxSpeed(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the maximum neighbor distance of a specified agent.
+   * @param[in] agentNo The number of the agent whose maximum neighbor distance
+   *                    is to be retrieved.
+   * @return    The present maximum neighbor distance of the agent.
+   */
+  float getAgentNeighborDist(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the count of agent neighbors taken into account to
+   *            compute the current velocity for the specified agent.
+   * @param[in] agentNo The number of the agent whose count of agent neighbors
+   *                    is to be retrieved.
+   * @return    The count of agent neighbors taken into account to compute the
+   *            current velocity for the specified agent.
+   */
+  std::size_t getAgentNumAgentNeighbors(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the count of ORCA constraints used to compute the
+   *            current velocity for the specified agent.
+   * @param[in] agentNo The number of the agent whose count of ORCA constraints
+   *                    i to be retrieved.
+   * @return    The count of ORCA constraints used to compute the current
+   *            velocity for the specified agent.
+   */
+  std::size_t getAgentNumORCAPlanes(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the specified ORCA constraint of the specified agent.
+   * @param[in] agentNo The number of the agent whose ORCA constraint is to be
+   *                    retrieved.
+   * @param[in] planeNo The number of the ORCA constraint to be retrieved.
+   * @return    A plane representing the specified ORCA constraint.
+   * @note      The halfspace to which the normal of the plane points is the
+   *            region of permissible velocities with respect to the specified
+   *            ORCA constraint.
+   */
+  const Plane &getAgentORCAPlane(std::size_t agentNo,
+                                 std::size_t planeNo) const;
+
+  /**
+   * @brief     Returns the three-dimensional position of a specified agent.
+   * @param[in] agentNo The number of the agent whose three-dimensional position
+   *                    is to be retrieved.
+   * @return    The present three-dimensional position of the (center of the)
+   *            agent.
+   */
+  const Vector3 &getAgentPosition(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the three-dimensional preferred velocity of a specified
+   *            agent.
+   * @param[in] agentNo The number of the agent whose three-dimensional
+   *                    preferred velocity is to be retrieved.
+   * @return    The present three-dimensional preferred velocity of the agent.
+   */
+  const Vector3 &getAgentPrefVelocity(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the radius of a specified agent.
+   * @param[in] agentNo The number of the agent whose radius is to be retrieved.
+   * @return    The present radius of the agent.
+   */
+  float getAgentRadius(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the time horizon of a specified agent.
+   * @param[in] agentNo The number of the agent whose time horizon is to be
+   *                    retrieved.
+   * @return    The present time horizon of the agent.
+   */
+  float getAgentTimeHorizon(std::size_t agentNo) const;
+
+  /**
+   * @brief     Returns the three-dimensional linear velocity of a specified
+   *            agent.
+   * @param[in] agentNo The number of the agent whose three-dimensional linear
+   *                    velocity is to be retrieved.
+   * @return    The present three-dimensional linear velocity of the agent.
+   */
+  const Vector3 &getAgentVelocity(std::size_t agentNo) const;
+
+  /**
+   * @brief  Returns the global time of the simulation.
+   * @return The present global time of the simulation (zero initially).
+   */
+  float getGlobalTime() const { return globalTime_; }
+  /**
+   * @brief  Returns the count of agents in the simulation.
+   * @return The count of agents in the simulation.
+   */
+  std::size_t getNumAgents() const { return agents_.size(); }
+
+  /**
+   * @brief  Returns the time step of the simulation.
+   * @return The present time step of the simulation.
+   */
+  float getTimeStep() const { return timeStep_; }
+
+  /**
+   * @brief     Removes an agent from the simulation.
+   * @param[in] agentNo The number of the agent that is to be removed.
+   * @note      After the removal of the agent, the agent that previously had
+   *            number getNumAgents() - 1 will now have number agentNo.
+   */
+  void removeAgent(std::size_t agentNo);
+
+  /**
+   * @brief     Sets the default properties for any new agent that is added.
+   * @param[in] neighborDist The default maximum distance (center point to
+   *                         center point) to other agents a new agent takes
+   *                         into account in the navigation. The larger this
+   *                         number, the longer he running time of the
+   *                         simulation. If the number is too low, the
+   *                         simulation will not be safe. Must be non-negative.
+   * @param[in] maxNeighbors The default maximum number of other agents a new
+   *                         agent takes into account in the navigation. The
+   *                         larger this number, the longer the running time of
+   *                         the simulation. If the number is too low, the
+   *                         simulation will not be safe.
+   * @param[in] timeHorizon  The default minimum amount of time for which a new
+   *                         agent's velocities that are computed by the
+   *                         simulation are safe with respect to other agents.
+   *                         The larger this number, the sooner an agent will
+   *                         respond to the presence of other agents, but the
+   *                         less freedom the agent has in choosing its
+   *                         velocities. Must be positive.
+   * @param[in] radius       The default radius of a new agent. Must be
+   *                         non-negative.
+   * @param[in] maxSpeed     The default maximum speed of a new agent. Must be
+   *                         non-negative.
+   * @param[in] velocity     The default initial three-dimensional linear
+   *                         velocity of a new agent (optional).
+   */
+  void setAgentDefaults(float neighborDist, std::size_t maxNeighbors,
+                        float timeHorizon, float radius, float maxSpeed,
+                        const Vector3 &velocity = Vector3());
+
+  /**
+   * @brief     Sets the maximum neighbor count of a specified agent.
+   * @param[in] agentNo      The number of the agent whose maximum neighbor
+   *                         count is to be modified.
+   * @param[in] maxNeighbors The replacement maximum neighbor count.
+   */
+  void setAgentMaxNeighbors(std::size_t agentNo, std::size_t maxNeighbors);
+
+  /**
+   * @brief     Sets the maximum speed of a specified agent.
+   * @param[in] agentNo  The number of the agent whose maximum speed is to be
+   *                     modified.
+   * @param[in] maxSpeed The replacement maximum speed. Must be non-negative.
+   */
+  void setAgentMaxSpeed(std::size_t agentNo, float maxSpeed);
+
+  /**
+   * @brief     Sets the maximum neighbor distance of a specified agent.
+   * @param[in] agentNo      The number of the agent whose maximum neighbor
+   *                         distance is to be modified.
+   * @param[in] neighborDist The replacement maximum neighbor distance. Must be
+   *                         non-negative.
+   */
+  void setAgentNeighborDist(std::size_t agentNo, float neighborDist);
+
+  /**
+   * @brief     Sets the three-dimensional position of a specified agent.
+   * @param[in] agentNo  The number of the agent whose three-dimensional
+   *                     position is to be modified.
+   * @param[in] position The replacement of the three-dimensional position.
+   */
+  void setAgentPosition(std::size_t agentNo, const Vector3 &position);
+
+  /**
+   * @brief     Sets the three-dimensional preferred velocity of a specified
+   *            agent.
+   * @param[in] agentNo      The number of the agent whose three-dimensional
+   *                         preferred velocity is to be modified.
+   * @param[in] prefVelocity The replacement of the three-dimensional preferred
+   *                         velocity.
+   */
+  void setAgentPrefVelocity(std::size_t agentNo, const Vector3 &prefVelocity);
+
+  /**
+   * @brief     Sets the radius of a specified agent.
+   * @param[in] agentNo The number of the agent whose radius is to be modified.
+   * @param[in] radius  The replacement radius. Must be non-negative.
+   */
+  void setAgentRadius(std::size_t agentNo, float radius);
+
+  /**
+   * @brief     Sets the time horizon of a specified agent with respect to other
+   *            agents.
+   * @param[in] agentNo     The number of the agent whose time horizon is to be
+   *                        modified.
+   * @param[in] timeHorizon The replacement time horizon with respect to other
+   *                        agents. Must be positive.
+   */
+  void setAgentTimeHorizon(std::size_t agentNo, float timeHorizon);
+
+  /**
+   * @brief     Sets the three-dimensional linear velocity of a specified agent.
+   * @param[in] agentNo  The number of the agent whose three-dimensional linear
+   *                     velocity is to be modified.
+   * @param[in] velocity The replacement three-dimensional linear velocity.
+   */
+  void setAgentVelocity(std::size_t agentNo, const Vector3 &velocity);
+
+  /**
+   * @brief     Sets the time step of the simulation.
+   * @param[in] timeStep The time step of the simulation. Must be positive.
+   */
+  void setTimeStep(float timeStep) { timeStep_ = timeStep; }
+
+ public:
+  /* Not implemented. */
+  RVOSimulator3D(const RVOSimulator3D &other);
+
+  /* Not implemented. */
+  RVOSimulator3D &operator=(const RVOSimulator3D &other);
+
+  Agent3D *defaultAgent_;
+  KdTree3D *kdTree_;
+  float globalTime_;
+  float timeStep_;
+  std::vector<Agent3D *> agents_;
+
+  friend class Agent3D;
+  friend class KdTree3D;
+};
+} /* namespace RVO3D */
+
+#endif /* RVO3D_RVO_SIMULATOR_H_ */

thirdparty/rvo2/rvo2_3d/Vector3.cc

@@ -0,0 +1,170 @@
+/*
+ * Vector3.cc
+ * RVO2-3D Library
+ *
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     https://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Please send all bug reports to <[email protected]>.
+ *
+ * The authors may be contacted via:
+ *
+ * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
+ * Dept. of Computer Science
+ * 201 S. Columbia St.
+ * Frederick P. Brooks, Jr. Computer Science Bldg.
+ * Chapel Hill, N.C. 27599-3175
+ * United States of America
+ *
+ * <https://gamma.cs.unc.edu/RVO2/>
+ */
+
+#include "Vector3.h"
+
+#include <cmath>
+#include <ostream>
+
+namespace RVO3D {
+Vector3::Vector3() : val_() {
+  val_[0] = 0.0F;
+  val_[1] = 0.0F;
+  val_[2] = 0.0F;
+}
+
+Vector3::Vector3(const Vector3 &vector) : val_() {
+  val_[0] = vector[0];
+  val_[1] = vector[1];
+  val_[2] = vector[2];
+}
+
+Vector3::Vector3(const float val[3]) : val_() {
+  val_[0] = val[0];
+  val_[1] = val[1];
+  val_[2] = val[2];
+}
+
+Vector3::Vector3(float x, float y, float z) : val_() {
+  val_[0] = x;
+  val_[1] = y;
+  val_[2] = z;
+}
+
+Vector3::~Vector3() {}
+
+Vector3 &Vector3::operator=(const Vector3 &vector) {
+  if (this != &vector) {
+    val_[0] = vector[0];
+    val_[1] = vector[1];
+    val_[2] = vector[2];
+  }
+
+  return *this;
+}
+
+float Vector3::operator[](std::size_t i) const { return val_[i]; }
+
+float &Vector3::operator[](std::size_t i) { return val_[i]; }
+
+Vector3 Vector3::operator-() const {
+  return Vector3(-val_[0], -val_[1], -val_[2]);
+}
+
+float Vector3::operator*(const Vector3 &vector) const {
+  return val_[0] * vector[0] + val_[1] * vector[1] + val_[2] * vector[2];
+}
+
+Vector3 Vector3::operator*(float scalar) const {
+  return Vector3(val_[0] * scalar, val_[1] * scalar, val_[2] * scalar);
+}
+
+Vector3 Vector3::operator/(float scalar) const {
+  const float invScalar = 1.0F / scalar;
+
+  return Vector3(val_[0] * invScalar, val_[1] * invScalar, val_[2] * invScalar);
+}
+
+Vector3 Vector3::operator+(const Vector3 &vector) const {
+  return Vector3(val_[0] + vector[0], val_[1] + vector[1], val_[2] + vector[2]);
+}
+
+Vector3 Vector3::operator-(const Vector3 &vector) const {
+  return Vector3(val_[0] - vector[0], val_[1] - vector[1], val_[2] - vector[2]);
+}
+
+bool Vector3::operator==(const Vector3 &vector) const {
+  return val_[0] == vector[0] && val_[1] == vector[1] && val_[2] == vector[2];
+}
+
+bool Vector3::operator!=(const Vector3 &vector) const {
+  return val_[0] != vector[0] || val_[1] != vector[1] || val_[2] != vector[2];
+}
+
+Vector3 &Vector3::operator*=(float scalar) {
+  val_[0] *= scalar;
+  val_[1] *= scalar;
+  val_[2] *= scalar;
+
+  return *this;
+}
+
+Vector3 &Vector3::operator/=(float scalar) {
+  const float invScalar = 1.0F / scalar;
+
+  val_[0] *= invScalar;
+  val_[1] *= invScalar;
+  val_[2] *= invScalar;
+
+  return *this;
+}
+
+Vector3 &Vector3::operator+=(const Vector3 &vector) {
+  val_[0] += vector[0];
+  val_[1] += vector[1];
+  val_[2] += vector[2];
+
+  return *this;
+}
+
+Vector3 &Vector3::operator-=(const Vector3 &vector) {
+  val_[0] -= vector[0];
+  val_[1] -= vector[1];
+  val_[2] -= vector[2];
+
+  return *this;
+}
+
+Vector3 operator*(float scalar, const Vector3 &vector) {
+  return Vector3(scalar * vector[0], scalar * vector[1], scalar * vector[2]);
+}
+
+std::ostream &operator<<(std::ostream &stream, const Vector3 &vector) {
+  stream << "(" << vector[0] << "," << vector[1] << "," << vector[2] << ")";
+
+  return stream;
+}
+
+float abs(const Vector3 &vector) { return std::sqrt(vector * vector); }
+
+float absSq(const Vector3 &vector) { return vector * vector; }
+
+Vector3 cross(const Vector3 &vector1, const Vector3 &vector2) {
+  return Vector3(vector1[1] * vector2[2] - vector1[2] * vector2[1],
+                 vector1[2] * vector2[0] - vector1[0] * vector2[2],
+                 vector1[0] * vector2[1] - vector1[1] * vector2[0]);
+}
+
+Vector3 normalize(const Vector3 &vector) { return vector / abs(vector); }
+
+} /* namespace RVO3D */

thirdparty/rvo2/rvo2_3d/Vector3.h

@@ -2,7 +2,8 @@
  * Vector3.h
  * RVO2-3D Library
  *
- * Copyright 2008 University of North Carolina at Chapel Hill
+ * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
+ * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -30,324 +31,284 @@
  * <https://gamma.cs.unc.edu/RVO2/>
  */
 
-/**
- * \file    Vector3.h
- * \brief   Contains the Vector3 class.
- */
 #ifndef RVO3D_VECTOR3_H_
 #define RVO3D_VECTOR3_H_
 
-#include <cmath>
+/**
+ * @file  Vector3.h
+ * @brief Contains the Vector3 class.
+ */
+
 #include <cstddef>
-#include <ostream>
+#include <iosfwd>
 
 namespace RVO3D {
-	/**
-	 * \brief  Defines a three-dimensional vector.
-	 */
-	class Vector3 {
-	public:
-		/**
-		 * \brief   Constructs and initializes a three-dimensional vector instance to zero.
-		 */
-		inline Vector3()
-		{
-			val_[0] = 0.0f;
-			val_[1] = 0.0f;
-			val_[2] = 0.0f;
-		}
-
-		/**
-		 * \brief   Constructs and initializes a three-dimensional vector from the specified three-dimensional vector.
-		 * \param   vector  The three-dimensional vector containing the xyz-coordinates.
-		 */
-		inline Vector3(const Vector3 &vector)
-		{
-			val_[0] = vector[0];
-			val_[1] = vector[1];
-			val_[2] = vector[2];
-		}
-
-		/**
-		 * \brief   Constructs and initializes a three-dimensional vector from the specified three-element array.
-		 * \param   val  The three-element array containing the xyz-coordinates.
-		 */
-		inline explicit Vector3(const float val[3])
-		{
-			val_[0] = val[0];
-			val_[1] = val[1];
-			val_[2] = val[2];
-		}
-
-		/**
-		 * \brief   Constructs and initializes a three-dimensional vector from the specified xyz-coordinates.
-		 * \param   x  The x-coordinate of the three-dimensional vector.
-		 * \param   y  The y-coordinate of the three-dimensional vector.
-		 * \param   z  The z-coordinate of the three-dimensional vector.
-		 */
-		inline Vector3(float x, float y, float z)
-		{
-			val_[0] = x;
-			val_[1] = y;
-			val_[2] = z;
-		}
-
-		/**
-		 * \brief   Returns the x-coordinate of this three-dimensional vector.
-		 * \return  The x-coordinate of the three-dimensional vector.
-		 */
-		inline float x() const { return val_[0]; }
-
-		/**
-		 * \brief   Returns the y-coordinate of this three-dimensional vector.
-		 * \return  The y-coordinate of the three-dimensional vector.
-		 */
-		inline float y() const { return val_[1]; }
-
-		/**
-		 * \brief   Returns the z-coordinate of this three-dimensional vector.
-		 * \return  The z-coordinate of the three-dimensional vector.
-		 */
-		inline float z() const { return val_[2]; }
-
-		/**
-		 * \brief   Returns the specified coordinate of this three-dimensional vector.
-		 * \param   i  The coordinate that should be returned (0 <= i < 3).
-		 * \return  The specified coordinate of the three-dimensional vector.
-		 */
-		inline float operator[](size_t i) const { return val_[i]; }
-
-		/**
-		 * \brief   Returns a reference to the specified coordinate of this three-dimensional vector.
-		 * \param   i  The coordinate to which a reference should be returned (0 <= i < 3).
-		 * \return  A reference to the specified coordinate of the three-dimensional vector.
-		 */
-		inline float &operator[](size_t i) { return val_[i]; }
-
-		/**
-		 * \brief   Computes the negation of this three-dimensional vector.
-		 * \return  The negation of this three-dimensional vector.
-		 */
-		inline Vector3 operator-() const
-		{
-			return Vector3(-val_[0], -val_[1], -val_[2]);
-		}
-
-		/**
-		 * \brief   Computes the dot product of this three-dimensional vector with the specified three-dimensional vector.
-		 * \param   vector  The three-dimensional vector with which the dot product should be computed.
-		 * \return  The dot product of this three-dimensional vector with a specified three-dimensional vector.
-		 */
-		inline float operator*(const Vector3 &vector) const
-		{
-			return val_[0] * vector[0] + val_[1] * vector[1] + val_[2] * vector[2];
-		}
-
-		/**
-		 * \brief   Computes the scalar multiplication of this three-dimensional vector with the specified scalar value.
-		 * \param   scalar  The scalar value with which the scalar multiplication should be computed.
-		 * \return  The scalar multiplication of this three-dimensional vector with a specified scalar value.
-		 */
-		inline Vector3 operator*(float scalar) const
-		{
-			return Vector3(val_[0] * scalar, val_[1] * scalar, val_[2] * scalar);
-		}
-
-		/**
-		 * \brief   Computes the scalar division of this three-dimensional vector with the specified scalar value.
-		 * \param   scalar  The scalar value with which the scalar division should be computed.
-		 * \return  The scalar division of this three-dimensional vector with a specified scalar value.
-		 */
-		inline Vector3 operator/(float scalar) const
-		{
-			const float invScalar = 1.0f / scalar;
-
-			return Vector3(val_[0] * invScalar, val_[1] * invScalar, val_[2] * invScalar);
-		}
-
-		/**
-		 * \brief   Computes the vector sum of this three-dimensional vector with the specified three-dimensional vector.
-		 * \param   vector  The three-dimensional vector with which the vector sum should be computed.
-		 * \return 	The vector sum of this three-dimensional vector with a specified three-dimensional vector.
-		 */
-		inline Vector3 operator+(const Vector3 &vector) const
-		{
-			return Vector3(val_[0] + vector[0], val_[1] + vector[1], val_[2] + vector[2]);
-		}
-
-		/**
-		 * \brief   Computes the vector difference of this three-dimensional vector with the specified three-dimensional vector.
-		 * \param   vector  The three-dimensional vector with which the vector difference should be computed.
-		 * \return  The vector difference of this three-dimensional vector with a specified three-dimensional vector.
-		 */
-		inline Vector3 operator-(const Vector3 &vector) const
-		{
-			return Vector3(val_[0] - vector[0], val_[1] - vector[1], val_[2] - vector[2]);
-		}
-
-		/**
-		 * \brief   Tests this three-dimensional vector for equality with the specified three-dimensional vector.
-		 * \param   vector  The three-dimensional vector with which to test for equality.
-		 * \return  True if the three-dimensional vectors are equal.
-		 */
-		inline bool operator==(const Vector3 &vector) const
-		{
-			return val_[0] == vector[0] && val_[1] == vector[1] && val_[2] == vector[2];
-		}
-
-		/**
-		 * \brief   Tests this three-dimensional vector for inequality with the specified three-dimensional vector.
-		 * \param   vector  The three-dimensional vector with which to test for inequality.
-		 * \return  True if the three-dimensional vectors are not equal.
-		 */
-		inline bool operator!=(const Vector3 &vector) const
-		{
-			return val_[0] != vector[0] || val_[1] != vector[1] || val_[2] != vector[2];
-		}
-
-		/**
-		 * \brief   Sets the value of this three-dimensional vector to the scalar multiplication of itself with the specified scalar value.
-		 * \param   scalar  The scalar value with which the scalar multiplication should be computed.
-		 * \return  A reference to this three-dimensional vector.
-		 */
-		inline Vector3 &operator*=(float scalar)
-		{
-			val_[0] *= scalar;
-			val_[1] *= scalar;
-			val_[2] *= scalar;
-
-			return *this;
-		}
-
-		/**
-		 * \brief   Sets the value of this three-dimensional vector to the scalar division of itself with the specified scalar value.
-		 * \param   scalar  The scalar value with which the scalar division should be computed.
-		 * \return  A reference to this three-dimensional vector.
-		 */
-		inline Vector3 &operator/=(float scalar)
-		{
-			const float invScalar = 1.0f / scalar;
-
-			val_[0] *= invScalar;
-			val_[1] *= invScalar;
-			val_[2] *= invScalar;
-
-			return *this;
-		}
-
-		/**
-		 * \brief   Sets the value of this three-dimensional vector to the vector
-		 *             sum of itself with the specified three-dimensional vector.
-		 * \param   vector  The three-dimensional vector with which the vector sum should be computed.
-		 * \return  A reference to this three-dimensional vector.
-		 */
-		inline Vector3 &operator+=(const Vector3 &vector)
-		{
-			val_[0] += vector[0];
-			val_[1] += vector[1];
-			val_[2] += vector[2];
-
-			return *this;
-		}
-
-		/**
-		 * \brief   Sets the value of this three-dimensional vector to the vector difference of itself with the specified three-dimensional vector.
-		 * \param   vector  The three-dimensional vector with which the vector difference should be computed.
-		 * \return  A reference to this three-dimensional vector.
-		 */
-		inline Vector3 &operator-=(const Vector3 &vector)
-		{
-			val_[0] -= vector[0];
-			val_[1] -= vector[1];
-			val_[2] -= vector[2];
-
-			return *this;
-		}
-
-		inline Vector3 &operator=(const Vector3 &vector)
-		{
-			val_[0] = vector[0];
-			val_[1] = vector[1];
-			val_[2] = vector[2];
-
-			return *this;
-		}
-
-	private:
-		float val_[3];
-	};
-
-
-	/**
-	 * \relates  Vector3
-	 * \brief    Computes the scalar multiplication of the specified three-dimensional vector with the specified scalar value.
-	 * \param    scalar  The scalar value with which the scalar multiplication should be computed.
-	 * \param    vector  The three-dimensional vector with which the scalar multiplication should be computed.
-	 * \return   The scalar multiplication of the three-dimensional vector with the scalar value.
-	 */
-	inline Vector3 operator*(float scalar, const Vector3 &vector)
-	{
-		return Vector3(scalar * vector[0], scalar * vector[1], scalar * vector[2]);
-	}
-
-	/**
-	 * \relates  Vector3
-	 * \brief    Computes the cross product of the specified three-dimensional vectors.
-	 * \param    vector1  The first vector with which the cross product should be computed.
-	 * \param    vector2  The second vector with which the cross product should be computed.
-	 * \return   The cross product of the two specified vectors.
-	 */
-	inline Vector3 cross(const Vector3 &vector1, const Vector3 &vector2)
-	{
-		return Vector3(vector1[1] * vector2[2] - vector1[2] * vector2[1], vector1[2] * vector2[0] - vector1[0] * vector2[2], vector1[0] * vector2[1] - vector1[1] * vector2[0]);
-	}
-
-	/**
-	 * \relates  Vector3
-	 * \brief    Inserts the specified three-dimensional vector into the specified output stream.
-	 * \param    os      The output stream into which the three-dimensional vector should be inserted.
-	 * \param    vector  The three-dimensional vector which to insert into the output stream.
-	 * \return   A reference to the output stream.
-	 */
-	inline std::ostream &operator<<(std::ostream &os, const Vector3 &vector)
-	{
-		os << "(" << vector[0] << "," << vector[1] << "," << vector[2] << ")";
-
-		return os;
-	}
-
-	/**
-	 * \relates  Vector3
-	 * \brief    Computes the length of a specified three-dimensional vector.
-	 * \param    vector  The three-dimensional vector whose length is to be computed.
-	 * \return   The length of the three-dimensional vector.
-	 */
-	inline float abs(const Vector3 &vector)
-	{
-		return std::sqrt(vector * vector);
-	}
-
-	/**
-	 * \relates  Vector3
-	 * \brief    Computes the squared length of a specified three-dimensional vector.
-	 * \param    vector  The three-dimensional vector whose squared length is to be computed.
-	 * \return   The squared length of the three-dimensional vector.
-	 */
-	inline float absSq(const Vector3 &vector)
-	{
-		return vector * vector;
-	}
-
-	/**
-	 * \relates  Vector3
-	 * \brief    Computes the normalization of the specified three-dimensional vector.
-	 * \param    vector  The three-dimensional vector whose normalization is to be computed.
-	 * \return   The normalization of the three-dimensional vector.
-	 */
-	inline Vector3 normalize(const Vector3 &vector)
-	{
-		return vector / abs(vector);
-	}
-}
-
-#endif
+/**
+ * @brief Defines a three-dimensional vector.
+ */
+class Vector3 {
+ public:
+  /**
+   * @brief Constructs and initializes a three-dimensional vector instance to
+   *        zero.
+   */
+  Vector3();
+
+  /**
+   * @brief     Constructs and initializes a three-dimensional vector from the
+   *            specified three-dimensional vector.
+   * @param[in] vector The three-dimensional vector containing the
+   *                   xyz-coordinates.
+   */
+  Vector3(const Vector3 &vector);
+
+  /**
+   * @brief     Constructs and initializes a three-dimensional vector from the
+   *            specified three-element array.
+   * @param[in] val The three-element array containing the xyz-coordinates.
+   */
+  explicit Vector3(const float val[3]);
+
+  /**
+   * @brief     Constructs and initializes a three-dimensional vector from the
+   *            specified xyz-coordinates.
+   * @param[in] x The x-coordinate of the three-dimensional vector.
+   * @param[in] y The y-coordinate of the three-dimensional vector.
+   * @param[in] z The z-coordinate of the three-dimensional vector.
+   */
+  Vector3(float x, float y, float z);
+
+  /**
+   * @brief Destroys this three-dimensional vector instance.
+   */
+  ~Vector3();
+
+  /**
+   * @brief  Returns the x-coordinate of this three-dimensional vector.
+   * @return The x-coordinate of the three-dimensional vector.
+   */
+  float x() const { return val_[0]; }
+
+  /**
+   * @brief  Returns the y-coordinate of this three-dimensional vector.
+   * @return The y-coordinate of the three-dimensional vector.
+   */
+  float y() const { return val_[1]; }
+
+  /**
+   * @brief  Returns the z-coordinate of this three-dimensional vector.
+   * @return The z-coordinate of the three-dimensional vector.
+   */
+  float z() const { return val_[2]; }
+
+  /**
+   * @brief     Assigns a copy of the specified three-dimensional vector to
+   *            this three-dimensional vector instance.
+   * @param[in] vector The three-dimensional vector containing the
+   *                   xyz-coordinates.
+   * @return    A reference to this three-dimensional vector instance.
+   */
+  Vector3 &operator=(const Vector3 &vector);
+
+  /**
+   * @brief     Returns the specified coordinate of this three-dimensional
+   *            vector.
+   * @param[in] i The coordinate that should be returned (0 <= i < 3).
+   * @return    The specified coordinate of the three-dimensional vector.
+   */
+  float operator[](std::size_t i) const;
+
+  /**
+   * @brief     Returns a reference to the specified coordinate of this
+   *            three-dimensional vector.
+   * @param[in] i The coordinate to which a reference should be returned
+   *              (0 <= i < 3).
+   * @return    A reference to the specified coordinate of the three-dimensional
+   *            vector.
+   */
+  float &operator[](std::size_t i);
+
+  /**
+   * @brief  Computes the negation of this three-dimensional vector.
+   * @return The negation of this three-dimensional vector.
+   */
+  Vector3 operator-() const;
+
+  /**
+   * @brief     Computes the dot product of this three-dimensional vector with
+   *            the specified three-dimensional vector.
+   * @param[in] vector The three-dimensional vector with which the dot product
+   *                   should be computed.
+   * @return    The dot product of this three-dimensional vector with a
+   *            specified three-dimensional vector.
+   */
+  float operator*(const Vector3 &vector) const;
+
+  /**
+   * @brief     Computes the scalar multiplication of this three-dimensional
+   *            vector with the specified scalar value.
+   * @param[in] scalar The scalar value with which the scalar multiplication
+   *                   should be computed.
+   * @return    The scalar multiplication of this three-dimensional vector with
+   *            a specified scalar value.
+   */
+  Vector3 operator*(float scalar) const;
+
+  /**
+   * @brief     Computes the scalar division of this three-dimensional vector
+   *            with the specified scalar value.
+   * @param[in] scalar The scalar value with which the scalar division should be
+   *                   computed.
+   * @return    The scalar division of this three-dimensional vector with a
+   *            specified scalar value.
+   */
+  Vector3 operator/(float scalar) const;
+
+  /**
+   * @brief     Computes the vector sum of this three-dimensional vector with
+   *            the specified three-dimensional vector.
+   * @param[in] vector The three-dimensional vector with which the vector sum
+   *                   should be computed.
+   * @return    The vector sum of this three-dimensional vector with a specified
+   *            three-dimensional vector.
+   */
+  Vector3 operator+(const Vector3 &vector) const;
+
+  /**
+   * @brief      Computes the vector difference of this three-dimensional vector
+   *             with the specified three-dimensional vector.
+   * @param[in]  vector The three-dimensional vector with which the vector
+   *                    difference should be computed.
+   * @return     The vector difference of this three-dimensional vector with a
+   *             specified three-dimensional vector.
+   */
+  Vector3 operator-(const Vector3 &vector) const;
+
+  /**
+   * @brief     Tests this three-dimensional vector for equality with the
+   *            specified three-dimensional vector.
+   * @param[in] vector The three-dimensional vector with which to test for
+   *                   equality.
+   * @return    True if the three-dimensional vectors are equal.
+   */
+  bool operator==(const Vector3 &vector) const;
+
+  /**
+   * @brief     Tests this three-dimensional vector for inequality with the
+   *            specified three-dimensional vector
+   * @param[in] vector The three-dimensional vector with which to test for
+   *                   inequality.
+   * @return    True if the three-dimensional vectors are not equal.
+   */
+  bool operator!=(const Vector3 &vector) const;
+
+  /**
+   * @brief     Sets the value of this three-dimensional vector to the scalar
+   *            multiplication of itself with the specified scalar value.
+   * @param[in] scalar The scalar value with which the scalar multiplication
+   *                   should be computed.
+   * @return    A reference to this three-dimensional vector.
+   */
+  Vector3 &operator*=(float scalar);
+
+  /**
+   * @brief     Sets the value of this three-dimensional vector to the scalar
+   *            division of itself with the specified scalar value.
+   * @param[in] scalar The scalar value with which the scalar division should be
+   *                   computed.
+   * @return    A reference to this three-dimensional vector.
+   */
+  Vector3 &operator/=(float scalar);
+
+  /**
+   * @brief     Sets the value of this three-dimensional vector to the vector
+   *            sum of itself with the specified three-dimensional vector.
+   * @param[in] vector The three-dimensional vector with which the vector sum
+   *                   should be computed.
+   * @return    A reference to this three-dimensional vector.
+   */
+  Vector3 &operator+=(const Vector3 &vector);
+
+  /**
+   * @brief     Sets the value of this three-dimensional vector to the vector
+   *            difference of itself with the specified three-dimensional
+   *            vector.
+   * @param[in] vector The three-dimensional vector with which the vector
+   *                   difference should be computed.
+   * @return    A reference to this three-dimensional vector.
+   */
+  Vector3 &operator-=(const Vector3 &vector);
+
+ private:
+  float val_[3];
+};
+
+/**
+ * @relates   Vector3
+ * @brief     Computes the scalar multiplication of the specified
+ *            three-dimensional vector with the specified scalar value.
+ * @param[in] scalar The scalar value with which the scalar multiplication
+ *                   should be computed.
+ * @param[in] vector The three-dimensional vector with which the scalar
+ *                   multiplication should be computed.
+ * @return    The scalar multiplication of the three-dimensional vector with the
+ *            scalar value.
+ */
+Vector3 operator*(float scalar, const Vector3 &vector);
+
+/**
+ * @relates       Vector3
+ * @brief         Inserts the specified three-dimensional vector into the
+ *                specified output stream.
+ * @param[in,out] os     The output stream into which the three-dimensional
+ *                       vector should be inserted.
+ * @param[in]     vector The three-dimensional vector which to insert into the
+ *                       output stream.
+ * @return        A reference to the output stream.
+ */
+std::ostream &operator<<(std::ostream &stream,
+                                      const Vector3 &vector);
+
+/**
+ * @relates   Vector3
+ * @brief     Computes the length of a specified three-dimensional vector.
+ * @param[in] vector The three-dimensional vector whose length is to be
+ *            computed.
+ * @return    The length of the three-dimensional vector.
+ */
+float abs(const Vector3 &vector);
+
+/**
+ * @relates   Vector3
+ * @brief     Computes the squared length of a specified three-dimensional
+ *            vector.
+ * @param[in] vector The three-dimensional vector whose squared length is to be
+ *                   computed.
+ * @return    The squared length of the three-dimensional vector.
+ */
+float absSq(const Vector3 &vector);
+
+/**
+ * @relates   Vector3
+ * @brief     Computes the cross product of the specified three-dimensional
+ *            vectors.
+ * @param[in] vector1 The first vector with which the cross product should be
+ *                    computed.
+ * @param[in] vector2 The second vector with which the cross product should be
+ *                    computed.
+ * @return    The cross product of the two specified vectors.
+ */
+Vector3 cross(const Vector3 &vector1, const Vector3 &vector2);
+
+/**
+ * @relates   Vector3
+ * @brief     Computes the normalization of the specified three-dimensiona
+ *            vector.
+ * @param[in] vector The three-dimensional vector whose normalization is to be
+ *                   computed.
+ * @return    The normalization of the three-dimensional vector.
+ */
+Vector3 normalize(const Vector3 &vector);
+} /* namespace RVO3D */
+
+#endif /* RVO3D_VECTOR3_H_ */