diff options
Diffstat (limited to 'thirdparty/rvo2/rvo2_3d/Agent3d.cc')
| -rw-r--r-- | thirdparty/rvo2/rvo2_3d/Agent3d.cc | 474 |
1 files changed, 0 insertions, 474 deletions
diff --git a/thirdparty/rvo2/rvo2_3d/Agent3d.cc b/thirdparty/rvo2/rvo2_3d/Agent3d.cc deleted file mode 100644 index 860ebdc58c..0000000000 --- a/thirdparty/rvo2/rvo2_3d/Agent3d.cc +++ /dev/null @@ -1,474 +0,0 @@ -/* - * 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 */ |