rvo2: Re-sync with upstream, properly document Godot-specific changes
Still tracking the `v1.0.1` tag for now, just reverting all the unnecessary style changes that created a diff with upstream.
This commit is contained in:
Rémi Verschelde
parent
26393cc037
commit
6c78170d8c
|
|
@ -571,12 +571,12 @@ Files extracted from upstream source:
|
|||
|
||||
Files extracted from upstream source:
|
||||
|
||||
- All .cpp and .h files in the `src/` folder except for RVO.h, RVOSimulator.cpp and RVOSimulator.h
|
||||
- All .cpp and .h files in the `src/` folder except for Export.h, RVO.h, RVOSimulator.cpp and RVOSimulator.h
|
||||
- LICENSE
|
||||
|
||||
Important: Some files have Godot-made changes; so to enrich the features
|
||||
originally proposed by this library and better integrate this library with
|
||||
Godot. Please check the file to know what's new.
|
||||
Godot. See the patch in the `patches` folder for details.
|
||||
|
||||
|
||||
## spirv-reflect
|
||||
|
|
|
|||
|
|
@ -38,8 +38,6 @@
|
|||
#ifndef RVO_API_H_
|
||||
#define RVO_API_H_
|
||||
|
||||
// -- GODOT start --
|
||||
#define RVO_API
|
||||
// -- GODOT end --
|
||||
|
||||
#endif /* RVO_API_H_ */
|
||||
|
|
|
|||
|
|
@ -32,23 +32,23 @@
|
|||
|
||||
#include "Agent.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
#include "Definitions.h"
|
||||
#include "KdTree.h"
|
||||
|
||||
namespace RVO {
|
||||
/**
|
||||
/**
|
||||
* \brief A sufficiently small positive number.
|
||||
*/
|
||||
const float RVO_EPSILON = 0.00001f;
|
||||
const float RVO_EPSILON = 0.00001f;
|
||||
|
||||
/**
|
||||
/**
|
||||
* \brief Defines a directed line.
|
||||
*/
|
||||
class Line {
|
||||
public:
|
||||
class Line {
|
||||
public:
|
||||
/**
|
||||
* \brief The direction of the directed line.
|
||||
*/
|
||||
|
|
@ -58,9 +58,9 @@ public:
|
|||
* \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.
|
||||
|
|
@ -71,9 +71,9 @@ public:
|
|||
* \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 Line &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result);
|
||||
bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line &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
|
||||
|
|
@ -83,9 +83,9 @@ bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line
|
|||
* \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);
|
||||
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.
|
||||
|
|
@ -94,29 +94,29 @@ bool linearProgram2(const std::vector<Plane> &planes, size_t planeNo, float radi
|
|||
* \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);
|
||||
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);
|
||||
void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float radius, Vector3 &result);
|
||||
|
||||
Agent::Agent() :
|
||||
id_(0), maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f), ignore_y_(false) {}
|
||||
Agent::Agent() : id_(0), maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f), ignore_y_(false) { }
|
||||
|
||||
void Agent::computeNeighbors(KdTree *kdTree_) {
|
||||
void Agent::computeNeighbors(KdTree *kdTree_)
|
||||
{
|
||||
agentNeighbors_.clear();
|
||||
if (maxNeighbors_ > 0) {
|
||||
kdTree_->computeAgentNeighbors(this, neighborDist_ * neighborDist_);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#define ABS(m_v) (((m_v) < 0) ? (-(m_v)) : (m_v))
|
||||
void Agent::computeNewVelocity(float timeStep) {
|
||||
void Agent::computeNewVelocity(float timeStep)
|
||||
{
|
||||
orcaPlanes_.clear();
|
||||
const float invTimeHorizon = 1.0f / timeHorizon_;
|
||||
|
||||
|
|
@ -132,6 +132,7 @@ void Agent::computeNewVelocity(float timeStep) {
|
|||
// by moving only on the horizontal plane relative to the player velocity.
|
||||
if (ignore_y_) {
|
||||
// Skip if these are in two different heights
|
||||
#define ABS(m_v) (((m_v) < 0) ? (-(m_v)) : (m_v))
|
||||
if (ABS(relativePosition[1]) > combinedRadius * 2) {
|
||||
continue;
|
||||
}
|
||||
|
|
@ -160,7 +161,8 @@ void Agent::computeNewVelocity(float timeStep) {
|
|||
|
||||
plane.normal = unitW;
|
||||
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
/* Project on cone. */
|
||||
const float a = distSq;
|
||||
const float b = relativePosition * relativeVelocity;
|
||||
|
|
@ -173,7 +175,8 @@ void Agent::computeNewVelocity(float timeStep) {
|
|||
plane.normal = unitW;
|
||||
u = (combinedRadius * t - wLength) * unitW;
|
||||
}
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
/* Collision. */
|
||||
const float invTimeStep = 1.0f / timeStep;
|
||||
const Vector3 w = relativeVelocity - invTimeStep * relativePosition;
|
||||
|
|
@ -198,9 +201,10 @@ void Agent::computeNewVelocity(float timeStep) {
|
|||
// Not 100% necessary, but better to have.
|
||||
newVelocity_[1] = prefVelocity_[1];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Agent::insertAgentNeighbor(const Agent *agent, float &rangeSq) {
|
||||
void Agent::insertAgentNeighbor(const Agent *agent, float &rangeSq)
|
||||
{
|
||||
if (this != agent) {
|
||||
const float distSq = absSq(position_ - agent->position_);
|
||||
|
||||
|
|
@ -223,9 +227,10 @@ void Agent::insertAgentNeighbor(const Agent *agent, float &rangeSq) {
|
|||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result) {
|
||||
bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line &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);
|
||||
|
||||
|
|
@ -246,7 +251,8 @@ bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line
|
|||
/* Lines line is (almost) parallel to plane i. */
|
||||
if (numerator > 0.0f) {
|
||||
return false;
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
|
@ -256,7 +262,8 @@ bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line
|
|||
if (denominator >= 0.0f) {
|
||||
/* Plane i bounds line on the left. */
|
||||
tLeft = std::max(tLeft, t);
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
/* Plane i bounds line on the right. */
|
||||
tRight = std::min(tRight, t);
|
||||
}
|
||||
|
|
@ -271,27 +278,32 @@ bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line
|
|||
if (optVelocity * line.direction > 0.0f) {
|
||||
/* Take right extreme. */
|
||||
result = line.point + tRight * line.direction;
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
/* Take left extreme. */
|
||||
result = line.point + tLeft * line.direction;
|
||||
}
|
||||
} else {
|
||||
}
|
||||
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) {
|
||||
}
|
||||
else if (t > tRight) {
|
||||
result = line.point + tRight * line.direction;
|
||||
} else {
|
||||
}
|
||||
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) {
|
||||
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);
|
||||
|
|
@ -312,10 +324,12 @@ bool linearProgram2(const std::vector<Plane> &planes, size_t planeNo, float radi
|
|||
|
||||
if (planeOptVelocityLengthSq <= RVO_EPSILON) {
|
||||
result = planeCenter;
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
result = planeCenter + std::sqrt(planeRadiusSq / planeOptVelocityLengthSq) * planeOptVelocity;
|
||||
}
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
/* Project point optVelocity on plane planeNo. */
|
||||
result = optVelocity + ((planes[planeNo].point - optVelocity) * planes[planeNo].normal) * planes[planeNo].normal;
|
||||
|
||||
|
|
@ -350,16 +364,19 @@ bool linearProgram2(const std::vector<Plane> &planes, size_t planeNo, float radi
|
|||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
size_t linearProgram3(const std::vector<Plane> &planes, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result) {
|
||||
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)) {
|
||||
}
|
||||
else if (absSq(optVelocity) > sqr(radius)) {
|
||||
/* Optimize closest point and outside circle. */
|
||||
result = normalize(optVelocity) * radius;
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
/* Optimize closest point and inside circle. */
|
||||
result = optVelocity;
|
||||
}
|
||||
|
|
@ -377,9 +394,10 @@ size_t linearProgram3(const std::vector<Plane> &planes, float radius, const Vect
|
|||
}
|
||||
|
||||
return planes.size();
|
||||
}
|
||||
}
|
||||
|
||||
void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float radius, Vector3 &result) {
|
||||
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) {
|
||||
|
|
@ -397,11 +415,13 @@ void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float r
|
|||
if (planes[i].normal * planes[j].normal > 0.0f) {
|
||||
/* Plane i and plane j point in the same direction. */
|
||||
continue;
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
/* Plane i and plane j point in opposite direction. */
|
||||
plane.point = 0.5f * (planes[i].point + planes[j].point);
|
||||
}
|
||||
} else {
|
||||
}
|
||||
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;
|
||||
|
|
@ -421,5 +441,5 @@ void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float r
|
|||
distance = planes[i].normal * (planes[i].point - result);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
} // namespace RVO
|
||||
|
|
|
|||
|
|
@ -53,11 +53,11 @@
|
|||
// - Moved the `Plane` class here.
|
||||
// - Added a new parameter `ignore_y_` in the `Agent`. This parameter is used to control a godot feature that allows to avoid collisions by moving on the horizontal plane.
|
||||
namespace RVO {
|
||||
/**
|
||||
/**
|
||||
* \brief Defines a plane.
|
||||
*/
|
||||
class Plane {
|
||||
public:
|
||||
class Plane {
|
||||
public:
|
||||
/**
|
||||
* \brief A point on the plane.
|
||||
*/
|
||||
|
|
@ -67,14 +67,13 @@ public:
|
|||
* \brief The normal to the plane.
|
||||
*/
|
||||
Vector3 normal;
|
||||
};
|
||||
};
|
||||
|
||||
/**
|
||||
/**
|
||||
* \brief Defines an agent in the simulation.
|
||||
*/
|
||||
class Agent {
|
||||
|
||||
public:
|
||||
class Agent {
|
||||
public:
|
||||
/**
|
||||
* \brief Constructs an agent instance.
|
||||
* \param sim The simulator instance.
|
||||
|
|
@ -115,7 +114,7 @@ public:
|
|||
bool ignore_y_;
|
||||
|
||||
friend class KdTree;
|
||||
};
|
||||
} // namespace RVO
|
||||
};
|
||||
}
|
||||
|
||||
#endif /* RVO_AGENT_H_ */
|
||||
|
|
|
|||
|
|
@ -38,20 +38,22 @@
|
|||
#include "Definitions.h"
|
||||
|
||||
namespace RVO {
|
||||
const size_t RVO_MAX_LEAF_SIZE = 10;
|
||||
const size_t RVO_MAX_LEAF_SIZE = 10;
|
||||
|
||||
KdTree::KdTree() {}
|
||||
KdTree::KdTree() { }
|
||||
|
||||
void KdTree::buildAgentTree(std::vector<Agent *> agents) {
|
||||
void KdTree::buildAgentTree(std::vector<Agent *> agents)
|
||||
{
|
||||
agents_.swap(agents);
|
||||
|
||||
if (!agents_.empty()) {
|
||||
agentTree_.resize(2 * agents_.size() - 1);
|
||||
buildAgentTreeRecursive(0, agents_.size(), 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void KdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node) {
|
||||
void KdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node)
|
||||
{
|
||||
agentTree_[node].begin = begin;
|
||||
agentTree_[node].end = end;
|
||||
agentTree_[node].minCoord = agents_[begin]->position_;
|
||||
|
|
@ -72,9 +74,11 @@ void KdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node) {
|
|||
|
||||
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]) {
|
||||
}
|
||||
else if (agentTree_[node].maxCoord[1] - agentTree_[node].minCoord[1] > agentTree_[node].maxCoord[2] - agentTree_[node].minCoord[2]) {
|
||||
coord = 1;
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
coord = 2;
|
||||
}
|
||||
|
||||
|
|
@ -114,18 +118,21 @@ void KdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node) {
|
|||
buildAgentTreeRecursive(begin, left, agentTree_[node].left);
|
||||
buildAgentTreeRecursive(left, end, agentTree_[node].right);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void KdTree::computeAgentNeighbors(Agent *agent, float rangeSq) const {
|
||||
void KdTree::computeAgentNeighbors(Agent *agent, float rangeSq) const
|
||||
{
|
||||
queryAgentTreeRecursive(agent, rangeSq, 0);
|
||||
}
|
||||
}
|
||||
|
||||
void KdTree::queryAgentTreeRecursive(Agent *agent, float &rangeSq, size_t node) const {
|
||||
void KdTree::queryAgentTreeRecursive(Agent *agent, float &rangeSq, size_t node) const
|
||||
{
|
||||
if (agentTree_[node].end - agentTree_[node].begin <= RVO_MAX_LEAF_SIZE) {
|
||||
for (size_t i = agentTree_[node].begin; i < agentTree_[node].end; ++i) {
|
||||
agent->insertAgentNeighbor(agents_[i], rangeSq);
|
||||
}
|
||||
} else {
|
||||
}
|
||||
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]));
|
||||
|
|
@ -138,7 +145,8 @@ void KdTree::queryAgentTreeRecursive(Agent *agent, float &rangeSq, size_t node)
|
|||
queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
if (distSqRight < rangeSq) {
|
||||
queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
|
||||
|
||||
|
|
@ -148,5 +156,5 @@ void KdTree::queryAgentTreeRecursive(Agent *agent, float &rangeSq, size_t node)
|
|||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
} // namespace RVO
|
||||
|
|
|
|||
|
|
@ -47,14 +47,14 @@
|
|||
// - Removed `sim_`.
|
||||
// - KdTree things are public
|
||||
namespace RVO {
|
||||
class Agent;
|
||||
class RVOSimulator;
|
||||
class Agent;
|
||||
class RVOSimulator;
|
||||
|
||||
/**
|
||||
/**
|
||||
* \brief Defines <i>k</i>d-trees for agents in the simulation.
|
||||
*/
|
||||
class KdTree {
|
||||
public:
|
||||
class KdTree {
|
||||
public:
|
||||
/**
|
||||
* \brief Defines an agent <i>k</i>d-tree node.
|
||||
*/
|
||||
|
|
@ -118,7 +118,7 @@ public:
|
|||
|
||||
friend class Agent;
|
||||
friend class RVOSimulator;
|
||||
};
|
||||
} // namespace RVO
|
||||
};
|
||||
}
|
||||
|
||||
#endif /* RVO_KD_TREE_H_ */
|
||||
|
|
|
|||
|
|
@ -1,32 +0,0 @@
|
|||
Optimal Reciprocal Collision Avoidance in Three Dimensions
|
||||
==========================================================
|
||||
|
||||
<http://gamma.cs.unc.edu/RVO2/>
|
||||
|
||||
[](https://travis-ci.org/snape/RVO2-3D)
|
||||
[](https://ci.appveyor.com/project/snape/rvo2-3d)
|
||||
|
||||
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 [geom@cs.unc.edu](mailto:geom@cs.unc.edu).
|
||||
|
||||
The authors may be contacted via:
|
||||
|
||||
Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, and 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
|
||||
|
|
@ -0,0 +1,308 @@
|
|||
diff --git a/thirdparty/rvo2/API.h b/thirdparty/rvo2/API.h
|
||||
index afef140253..9d424a661b 100644
|
||||
--- a/thirdparty/rvo2/API.h
|
||||
+++ b/thirdparty/rvo2/API.h
|
||||
@@ -38,30 +38,6 @@
|
||||
#ifndef RVO_API_H_
|
||||
#define RVO_API_H_
|
||||
|
||||
-#ifdef _WIN32
|
||||
-#include <SDKDDKVer.h>
|
||||
-#define WIN32_LEAN_AND_MEAN
|
||||
-#define NOCOMM
|
||||
-#define NOIMAGE
|
||||
-#define NOIME
|
||||
-#define NOKANJI
|
||||
-#define NOMCX
|
||||
-#define NOMINMAX
|
||||
-#define NOPROXYSTUB
|
||||
-#define NOSERVICE
|
||||
-#define NOSOUND
|
||||
-#define NOTAPE
|
||||
-#define NORPC
|
||||
-#define _USE_MATH_DEFINES
|
||||
-#include <windows.h>
|
||||
-#endif
|
||||
-
|
||||
-#ifdef RVO_EXPORTS
|
||||
-#define RVO_API __declspec(dllexport)
|
||||
-#elif defined(RVO_IMPORTS)
|
||||
-#define RVO_API __declspec(dllimport)
|
||||
-#else
|
||||
#define RVO_API
|
||||
-#endif
|
||||
|
||||
#endif /* RVO_API_H_ */
|
||||
diff --git a/thirdparty/rvo2/Agent.cpp b/thirdparty/rvo2/Agent.cpp
|
||||
index 1a236c7831..49f14c4f7d 100644
|
||||
--- a/thirdparty/rvo2/Agent.cpp
|
||||
+++ b/thirdparty/rvo2/Agent.cpp
|
||||
@@ -105,18 +105,17 @@ namespace RVO {
|
||||
*/
|
||||
void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float radius, Vector3 &result);
|
||||
|
||||
- Agent::Agent(RVOSimulator *sim) : sim_(sim), id_(0), maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f) { }
|
||||
+ Agent::Agent() : id_(0), maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f), ignore_y_(false) { }
|
||||
|
||||
- void Agent::computeNeighbors()
|
||||
+ void Agent::computeNeighbors(KdTree *kdTree_)
|
||||
{
|
||||
agentNeighbors_.clear();
|
||||
-
|
||||
if (maxNeighbors_ > 0) {
|
||||
- sim_->kdTree_->computeAgentNeighbors(this, neighborDist_ * neighborDist_);
|
||||
+ kdTree_->computeAgentNeighbors(this, neighborDist_ * neighborDist_);
|
||||
}
|
||||
}
|
||||
|
||||
- void Agent::computeNewVelocity()
|
||||
+ void Agent::computeNewVelocity(float timeStep)
|
||||
{
|
||||
orcaPlanes_.clear();
|
||||
const float invTimeHorizon = 1.0f / timeHorizon_;
|
||||
@@ -124,10 +123,24 @@ namespace RVO {
|
||||
/* Create agent ORCA planes. */
|
||||
for (size_t i = 0; i < agentNeighbors_.size(); ++i) {
|
||||
const Agent *const other = agentNeighbors_[i].second;
|
||||
- const Vector3 relativePosition = other->position_ - position_;
|
||||
- const Vector3 relativeVelocity = velocity_ - other->velocity_;
|
||||
- const float distSq = absSq(relativePosition);
|
||||
+
|
||||
+ Vector3 relativePosition = other->position_ - position_;
|
||||
+ Vector3 relativeVelocity = velocity_ - other->velocity_;
|
||||
const float combinedRadius = radius_ + other->radius_;
|
||||
+
|
||||
+ // This is a Godot feature that allow the agents to avoid the collision
|
||||
+ // by moving only on the horizontal plane relative to the player velocity.
|
||||
+ if (ignore_y_) {
|
||||
+ // Skip if these are in two different heights
|
||||
+#define ABS(m_v) (((m_v) < 0) ? (-(m_v)) : (m_v))
|
||||
+ if (ABS(relativePosition[1]) > combinedRadius * 2) {
|
||||
+ continue;
|
||||
+ }
|
||||
+ relativePosition[1] = 0;
|
||||
+ relativeVelocity[1] = 0;
|
||||
+ }
|
||||
+
|
||||
+ const float distSq = absSq(relativePosition);
|
||||
const float combinedRadiusSq = sqr(combinedRadius);
|
||||
|
||||
Plane plane;
|
||||
@@ -165,7 +178,7 @@ namespace RVO {
|
||||
}
|
||||
else {
|
||||
/* Collision. */
|
||||
- const float invTimeStep = 1.0f / sim_->timeStep_;
|
||||
+ const float invTimeStep = 1.0f / timeStep;
|
||||
const Vector3 w = relativeVelocity - invTimeStep * relativePosition;
|
||||
const float wLength = abs(w);
|
||||
const Vector3 unitW = w / wLength;
|
||||
@@ -183,6 +196,11 @@ namespace RVO {
|
||||
if (planeFail < orcaPlanes_.size()) {
|
||||
linearProgram4(orcaPlanes_, planeFail, maxSpeed_, newVelocity_);
|
||||
}
|
||||
+
|
||||
+ if (ignore_y_) {
|
||||
+ // Not 100% necessary, but better to have.
|
||||
+ newVelocity_[1] = prefVelocity_[1];
|
||||
+ }
|
||||
}
|
||||
|
||||
void Agent::insertAgentNeighbor(const Agent *agent, float &rangeSq)
|
||||
@@ -211,12 +229,6 @@ namespace RVO {
|
||||
}
|
||||
}
|
||||
|
||||
- void Agent::update()
|
||||
- {
|
||||
- velocity_ = newVelocity_;
|
||||
- position_ += velocity_ * sim_->timeStep_;
|
||||
- }
|
||||
-
|
||||
bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result)
|
||||
{
|
||||
const float dotProduct = line.point * line.direction;
|
||||
diff --git a/thirdparty/rvo2/Agent.h b/thirdparty/rvo2/Agent.h
|
||||
index 238f2d31b7..fd0bf4d1d4 100644
|
||||
--- a/thirdparty/rvo2/Agent.h
|
||||
+++ b/thirdparty/rvo2/Agent.h
|
||||
@@ -43,30 +43,52 @@
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
-#include "RVOSimulator.h"
|
||||
#include "Vector3.h"
|
||||
|
||||
+// Note: Slightly modified to work better in Godot.
|
||||
+// - The agent can be created by anyone.
|
||||
+// - The simulator pointer is removed.
|
||||
+// - The update function is removed.
|
||||
+// - The compute velocity function now need the timeStep.
|
||||
+// - Moved the `Plane` class here.
|
||||
+// - Added a new parameter `ignore_y_` in the `Agent`. This parameter is used to control a godot feature that allows to avoid collisions by moving on the horizontal plane.
|
||||
namespace RVO {
|
||||
+ /**
|
||||
+ * \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 an agent in the simulation.
|
||||
*/
|
||||
class Agent {
|
||||
- private:
|
||||
+ public:
|
||||
/**
|
||||
* \brief Constructs an agent instance.
|
||||
* \param sim The simulator instance.
|
||||
*/
|
||||
- explicit Agent(RVOSimulator *sim);
|
||||
+ explicit Agent();
|
||||
|
||||
/**
|
||||
* \brief Computes the neighbors of this agent.
|
||||
*/
|
||||
- void computeNeighbors();
|
||||
+ void computeNeighbors(class KdTree *kdTree_);
|
||||
|
||||
/**
|
||||
* \brief Computes the new velocity of this agent.
|
||||
*/
|
||||
- void computeNewVelocity();
|
||||
+ void computeNewVelocity(float timeStep);
|
||||
|
||||
/**
|
||||
* \brief Inserts an agent neighbor into the set of neighbors of this agent.
|
||||
@@ -75,16 +97,10 @@ namespace RVO {
|
||||
*/
|
||||
void insertAgentNeighbor(const Agent *agent, float &rangeSq);
|
||||
|
||||
- /**
|
||||
- * \brief Updates the three-dimensional position and three-dimensional velocity of this agent.
|
||||
- */
|
||||
- void update();
|
||||
-
|
||||
Vector3 newVelocity_;
|
||||
Vector3 position_;
|
||||
Vector3 prefVelocity_;
|
||||
Vector3 velocity_;
|
||||
- RVOSimulator *sim_;
|
||||
size_t id_;
|
||||
size_t maxNeighbors_;
|
||||
float maxSpeed_;
|
||||
@@ -93,9 +109,11 @@ namespace RVO {
|
||||
float timeHorizon_;
|
||||
std::vector<std::pair<float, const Agent *> > agentNeighbors_;
|
||||
std::vector<Plane> orcaPlanes_;
|
||||
+ /// This is a godot feature that allows the Agent to avoid collision by mooving
|
||||
+ /// on the horizontal plane.
|
||||
+ bool ignore_y_;
|
||||
|
||||
friend class KdTree;
|
||||
- friend class RVOSimulator;
|
||||
};
|
||||
}
|
||||
|
||||
diff --git a/thirdparty/rvo2/KdTree.cpp b/thirdparty/rvo2/KdTree.cpp
|
||||
index 719fabdf34..c6d43ee415 100644
|
||||
--- a/thirdparty/rvo2/KdTree.cpp
|
||||
+++ b/thirdparty/rvo2/KdTree.cpp
|
||||
@@ -36,16 +36,15 @@
|
||||
|
||||
#include "Agent.h"
|
||||
#include "Definitions.h"
|
||||
-#include "RVOSimulator.h"
|
||||
|
||||
namespace RVO {
|
||||
const size_t RVO_MAX_LEAF_SIZE = 10;
|
||||
|
||||
- KdTree::KdTree(RVOSimulator *sim) : sim_(sim) { }
|
||||
+ KdTree::KdTree() { }
|
||||
|
||||
- void KdTree::buildAgentTree()
|
||||
+ void KdTree::buildAgentTree(std::vector<Agent *> agents)
|
||||
{
|
||||
- agents_ = sim_->agents_;
|
||||
+ agents_.swap(agents);
|
||||
|
||||
if (!agents_.empty()) {
|
||||
agentTree_.resize(2 * agents_.size() - 1);
|
||||
diff --git a/thirdparty/rvo2/KdTree.h b/thirdparty/rvo2/KdTree.h
|
||||
index 5dbc2b492f..e05a7f40d4 100644
|
||||
--- a/thirdparty/rvo2/KdTree.h
|
||||
+++ b/thirdparty/rvo2/KdTree.h
|
||||
@@ -43,6 +43,9 @@
|
||||
|
||||
#include "Vector3.h"
|
||||
|
||||
+// Note: Slightly modified to work better with Godot.
|
||||
+// - Removed `sim_`.
|
||||
+// - KdTree things are public
|
||||
namespace RVO {
|
||||
class Agent;
|
||||
class RVOSimulator;
|
||||
@@ -51,7 +54,7 @@ namespace RVO {
|
||||
* \brief Defines <i>k</i>d-trees for agents in the simulation.
|
||||
*/
|
||||
class KdTree {
|
||||
- private:
|
||||
+ public:
|
||||
/**
|
||||
* \brief Defines an agent <i>k</i>d-tree node.
|
||||
*/
|
||||
@@ -92,12 +95,12 @@ namespace RVO {
|
||||
* \brief Constructs a <i>k</i>d-tree instance.
|
||||
* \param sim The simulator instance.
|
||||
*/
|
||||
- explicit KdTree(RVOSimulator *sim);
|
||||
+ explicit KdTree();
|
||||
|
||||
/**
|
||||
* \brief Builds an agent <i>k</i>d-tree.
|
||||
*/
|
||||
- void buildAgentTree();
|
||||
+ void buildAgentTree(std::vector<Agent *> agents);
|
||||
|
||||
void buildAgentTreeRecursive(size_t begin, size_t end, size_t node);
|
||||
|
||||
@@ -112,7 +115,6 @@ namespace RVO {
|
||||
|
||||
std::vector<Agent *> agents_;
|
||||
std::vector<AgentTreeNode> agentTree_;
|
||||
- RVOSimulator *sim_;
|
||||
|
||||
friend class Agent;
|
||||
friend class RVOSimulator;
|
||||
diff --git a/thirdparty/rvo2/Vector3.h b/thirdparty/rvo2/Vector3.h
|
||||
index adf3382339..8c8835c865 100644
|
||||
--- a/thirdparty/rvo2/Vector3.h
|
||||
+++ b/thirdparty/rvo2/Vector3.h
|
||||
@@ -59,17 +59,6 @@ namespace RVO {
|
||||
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.
|
||||
- */
|
||||
- RVO_API 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.
|
||||
Loading…
Reference in New Issue