1 files changed, 474 insertions, 0 deletions
diff --git a/thirdparty/rvo2/rvo2_3d/Agent3d.cc b/thirdparty/rvo2/rvo2_3d/Agent3d.cc
new file mode 100644
index 0000000000..860ebdc58c
--- /dev/null
+++ b/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 <geom@cs.unc.edu>.
+ *
+ * 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 */