475 lines
16 KiB
C++
475 lines
16 KiB
C++
|
/*
|
||
|
|
* 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 */
|