Relintai/pandemonium_engine
1
0
Fork 0
mirror of https://github.com/Relintai/pandemonium_engine.git synced 2025-02-11 10:30:06 +01:00
Code Issues Packages Projects Releases Wiki Activity

Ported: rvo2: Re-sync with upstream, properly document Godot-specific changes

Browse Source 
Still tracking the `v1.0.1` tag for now, just reverting all the unnecessary
style changes that created a diff with upstream.
- akien-mga
e317b7efbb
This commit is contained in:
Relintai 2022-07-27 19:53:21 +02:00
parent 9281e140dd
commit fd84415f3b
9 changed files with 833 additions and 585 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
thirdparty/README.md vendored
View File

@ -442,12 +442,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.
## squish

47
thirdparty/rvo2/API.h vendored
View File

@ -1,47 +0,0 @@
#ifndef RVO_API_H_
#define RVO_API_H_
/*
* API.h
* 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 <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
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
/**
* \file API.h
* \brief Contains definitions related to Microsoft Windows.
*/
// -- PANDEMONIUM start --
#define RVO_API
// -- PANDEMONIUM end --
#endif /* RVO_API_H_ */

136
thirdparty/rvo2/Agent.cpp vendored
View File

@ -8,7 +8,7 @@
* 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,28 +27,28 @@
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
* <https://gamma.cs.unc.edu/RVO2/>
*/
#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 RVO3D_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_;
@ -128,10 +128,11 @@ void Agent::computeNewVelocity(float timeStep) {
Vector3 relativeVelocity = velocity_ - other->velocity_;
const float combinedRadius = radius_ + other->radius_;
// This is a Pandemonium feature that allow the agents to avoid the collision
// 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;
}
@ -160,20 +161,22 @@ 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;
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;
const Vector3 ww = relativeVelocity - t * relativePosition;
const float wwLength = abs(ww);
const Vector3 unitWW = ww / wwLength;
plane.normal = unitW;
u = (combinedRadius * t - wLength) * unitW;
plane.normal = unitWW;
u = (combinedRadius * t - wwLength) * unitWW;
}
} 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);
@ -242,11 +247,12 @@ bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line
const float numerator = (planes[i].point - line.point) * planes[i].normal;
const float denominator = line.direction * planes[i].normal;
if (sqr(denominator) <= RVO_EPSILON) {
if (sqr(denominator) <= RVO3D_EPSILON) {
/* 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);
@ -310,12 +322,14 @@ bool linearProgram2(const std::vector<Plane> &planes, size_t planeNo, float radi
const Vector3 planeOptVelocity = optVelocity - (optVelocity * planes[planeNo].normal) * planes[planeNo].normal;
const float planeOptVelocityLengthSq = absSq(planeOptVelocity);
if (planeOptVelocityLengthSq <= RVO_EPSILON) {
if (planeOptVelocityLengthSq <= RVO3D_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;
@ -333,7 +347,7 @@ bool linearProgram2(const std::vector<Plane> &planes, size_t planeNo, float radi
/* Compute intersection line of plane i and plane planeNo. */
Vector3 crossProduct = cross(planes[i].normal, planes[planeNo].normal);
if (absSq(crossProduct) <= RVO_EPSILON) {
if (absSq(crossProduct) <= RVO3D_EPSILON) {
/* Planes planeNo and i are (almost) parallel, and plane i fully invalidates plane planeNo. */
return false;
}
@ -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) {
@ -392,16 +410,18 @@ void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float r
const Vector3 crossProduct = cross(planes[j].normal, planes[i].normal);
if (absSq(crossProduct) <= RVO_EPSILON) {
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 {
}
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

39
thirdparty/rvo2/Agent.h vendored
View File

@ -1,5 +1,3 @@
#ifndef RVO_AGENT_H_
#define RVO_AGENT_H_
/*
* Agent.h
* RVO2-3D Library
@ -10,7 +8,7 @@
* 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,
@ -29,17 +27,15 @@
* 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.
*/
#include "API.h"
#ifndef RVO3D_AGENT_H_
#define RVO3D_AGENT_H_
#include <cstddef>
#include <utility>
@ -47,19 +43,19 @@
#include "Vector3.h"
// Note: Slightly modified to work better in Pandemonium.
// 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 pandemonium feature that allows to avoid collisions by moving on the horizontal plane.
// - 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.
*/
@ -69,14 +65,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.
@ -112,12 +107,12 @@ public:
float timeHorizon_;
std::vector<std::pair<float, const Agent *> > agentNeighbors_;
std::vector<Plane> orcaPlanes_;
/// This is a pandemonium feature that allows the Agent to avoid collision by mooving
/// This is a godot feature that allows the Agent to avoid collision by mooving
/// on the horizontal plane.
bool ignore_y_;
friend class KdTree;
};
} // namespace RVO
};
}
#endif /* RVO_AGENT_H_ */
#endif /* RVO3D_AGENT_H_ */

14
thirdparty/rvo2/Definitions.h vendored
View File

@ -1,5 +1,3 @@
#ifndef RVO_DEFINITIONS_H_
#define RVO_DEFINITIONS_H_
/*
* Definitions.h
* RVO2-3D Library
@ -10,7 +8,7 @@
* 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,
@ -29,7 +27,7 @@
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
* <https://gamma.cs.unc.edu/RVO2/>
*/
/**
@ -37,10 +35,8 @@
* \brief Contains functions and constants used in multiple classes.
*/
#include "API.h"
#ifndef RVO3D_DEFINITIONS_H_
#define RVO3D_DEFINITIONS_H_
namespace RVO {
/**
@ -54,4 +50,4 @@ namespace RVO {
}
}
#endif /* RVO_DEFINITIONS_H_ */
#endif /* RVO3D_DEFINITIONS_H_ */

44
thirdparty/rvo2/KdTree.cpp vendored
View File

@ -8,7 +8,7 @@
* 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,7 +27,7 @@
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
* <https://gamma.cs.unc.edu/RVO2/>
*/
#include "KdTree.h"
@ -38,20 +38,22 @@
#include "Definitions.h"
namespace RVO {
const size_t RVO_MAX_LEAF_SIZE = 10;
const size_t RVO3D_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_;
@ -66,15 +68,17 @@ void KdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node) {
agentTree_[node].minCoord[2] = std::min(agentTree_[node].minCoord[2], agents_[i]->position_.z());
}
if (end - begin > RVO_MAX_LEAF_SIZE) {
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]) {
}
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 {
if (agentTree_[node].end - agentTree_[node].begin <= RVO_MAX_LEAF_SIZE) {
void KdTree::queryAgentTreeRecursive(Agent *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 {
}
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

30
thirdparty/rvo2/KdTree.h vendored
View File

@ -1,5 +1,3 @@
#ifndef RVO_KD_TREE_H_
#define RVO_KD_TREE_H_
/*
* KdTree.h
* RVO2-3D Library
@ -10,7 +8,7 @@
* 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,
@ -29,34 +27,32 @@
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
* <https://gamma.cs.unc.edu/RVO2/>
*/
/**
* \file KdTree.h
* \brief Contains the KdTree class.
*/
#include "API.h"
#ifndef RVO3D_KD_TREE_H_
#define RVO3D_KD_TREE_H_
#include <cstddef>
#include <vector>
#include "Vector3.h"
// Note: Slightly modified to work better with Pandemonium.
// Note: Slightly modified to work better with Godot.
// - 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.
*/
@ -120,7 +116,7 @@ public:
friend class Agent;
friend class RVOSimulator;
};
} // namespace RVO
};
}
#endif /* RVO_KD_TREE_H_ */
#endif /* RVO3D_KD_TREE_H_ */

68
thirdparty/rvo2/Vector3.h vendored
View File

@ -1,5 +1,3 @@
#ifndef RVO_VECTOR3_H_
#define RVO_VECTOR3_H_
/*
* Vector3.h
* RVO2-3D Library
@ -10,7 +8,7 @@
* 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,
@ -29,32 +27,32 @@
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
* <https://gamma.cs.unc.edu/RVO2/>
*/
/**
* \file Vector3.h
* \brief Contains the Vector3 class.
*/
#include "API.h"
#ifndef RVO3D_VECTOR3_H_
#define RVO3D_VECTOR3_H_
#include <cmath>
#include <cstddef>
#include <ostream>
#define RVO3D_EXPORT
namespace RVO {
/**
* \brief Defines a three-dimensional vector.
*/
class Vector3 {
class RVO3D_EXPORT Vector3 {
public:
/**
* \brief Constructs and initializes a three-dimensional vector instance to zero.
*/
RVO_API inline Vector3()
inline Vector3()
{
val_[0] = 0.0f;
val_[1] = 0.0f;
@ -65,7 +63,7 @@ namespace RVO {
* \brief Constructs and initializes a three-dimensional vector from the specified three-element array.
* \param val The three-element array containing the xyz-coordinates.
*/
RVO_API inline explicit Vector3(const float val[3])
inline explicit Vector3(const float val[3])
{
val_[0] = val[0];
val_[1] = val[1];
@ -78,7 +76,7 @@ namespace RVO {
* \param y The y-coordinate of the three-dimensional vector.
* \param z The z-coordinate of the three-dimensional vector.
*/
RVO_API inline Vector3(float x, float y, float z)
inline Vector3(float x, float y, float z)
{
val_[0] = x;
val_[1] = y;
@ -89,39 +87,39 @@ namespace RVO {
* \brief Returns the x-coordinate of this three-dimensional vector.
* \return The x-coordinate of the three-dimensional vector.
*/
RVO_API inline float x() const { return val_[0]; }
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.
*/
RVO_API inline float y() const { return val_[1]; }
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.
*/
RVO_API inline float z() const { return val_[2]; }
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.
*/
RVO_API inline float operator[](size_t i) const { return val_[i]; }
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.
*/
RVO_API inline float &operator[](size_t i) { return val_[i]; }
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.
*/
RVO_API inline Vector3 operator-() const
inline Vector3 operator-() const
{
return Vector3(-val_[0], -val_[1], -val_[2]);
}
@ -131,7 +129,7 @@ namespace RVO {
* \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.
*/
RVO_API inline float operator*(const Vector3 &vector) const
inline float operator*(const Vector3 &vector) const
{
return val_[0] * vector[0] + val_[1] * vector[1] + val_[2] * vector[2];
}
@ -141,7 +139,7 @@ namespace RVO {
* \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.
*/
RVO_API inline Vector3 operator*(float scalar) const
inline Vector3 operator*(float scalar) const
{
return Vector3(val_[0] * scalar, val_[1] * scalar, val_[2] * scalar);
}
@ -151,7 +149,7 @@ namespace RVO {
* \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.
*/
RVO_API inline Vector3 operator/(float scalar) const
inline Vector3 operator/(float scalar) const
{
const float invScalar = 1.0f / scalar;
@ -163,7 +161,7 @@ namespace RVO {
* \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.
*/
RVO_API inline Vector3 operator+(const Vector3 &vector) const
inline Vector3 operator+(const Vector3 &vector) const
{
return Vector3(val_[0] + vector[0], val_[1] + vector[1], val_[2] + vector[2]);
}
@ -173,7 +171,7 @@ namespace RVO {
* \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.
*/
RVO_API inline Vector3 operator-(const Vector3 &vector) const
inline Vector3 operator-(const Vector3 &vector) const
{
return Vector3(val_[0] - vector[0], val_[1] - vector[1], val_[2] - vector[2]);
}
@ -183,7 +181,7 @@ namespace RVO {
* \param vector The three-dimensional vector with which to test for equality.
* \return True if the three-dimensional vectors are equal.
*/
RVO_API inline bool operator==(const Vector3 &vector) const
inline bool operator==(const Vector3 &vector) const
{
return val_[0] == vector[0] && val_[1] == vector[1] && val_[2] == vector[2];
}
@ -193,7 +191,7 @@ namespace RVO {
* \param vector The three-dimensional vector with which to test for inequality.
* \return True if the three-dimensional vectors are not equal.
*/
RVO_API inline bool operator!=(const Vector3 &vector) const
inline bool operator!=(const Vector3 &vector) const
{
return val_[0] != vector[0] || val_[1] != vector[1] || val_[2] != vector[2];
}
@ -203,7 +201,7 @@ namespace RVO {
* \param scalar The scalar value with which the scalar multiplication should be computed.
* \return A reference to this three-dimensional vector.
*/
RVO_API inline Vector3 &operator*=(float scalar)
inline Vector3 &operator*=(float scalar)
{
val_[0] *= scalar;
val_[1] *= scalar;
@ -217,7 +215,7 @@ namespace RVO {
* \param scalar The scalar value with which the scalar division should be computed.
* \return A reference to this three-dimensional vector.
*/
RVO_API inline Vector3 &operator/=(float scalar)
inline Vector3 &operator/=(float scalar)
{
const float invScalar = 1.0f / scalar;
@ -234,7 +232,7 @@ namespace RVO {
* \param vector The three-dimensional vector with which the vector sum should be computed.
* \return A reference to this three-dimensional vector.
*/
RVO_API inline Vector3 &operator+=(const Vector3 &vector)
inline Vector3 &operator+=(const Vector3 &vector)
{
val_[0] += vector[0];
val_[1] += vector[1];
@ -248,7 +246,7 @@ namespace RVO {
* \param vector The three-dimensional vector with which the vector difference should be computed.
* \return A reference to this three-dimensional vector.
*/
RVO_API inline Vector3 &operator-=(const Vector3 &vector)
inline Vector3 &operator-=(const Vector3 &vector)
{
val_[0] -= vector[0];
val_[1] -= vector[1];
@ -269,7 +267,7 @@ namespace RVO {
* \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)
RVO3D_EXPORT inline Vector3 operator*(float scalar, const Vector3 &vector)
{
return Vector3(scalar * vector[0], scalar * vector[1], scalar * vector[2]);
}
@ -281,7 +279,7 @@ namespace RVO {
* \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)
RVO3D_EXPORT 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]);
}
@ -293,7 +291,7 @@ namespace RVO {
* \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)
RVO3D_EXPORT inline std::ostream &operator<<(std::ostream &os, const Vector3 &vector)
{
os << "(" << vector[0] << "," << vector[1] << "," << vector[2] << ")";
@ -306,7 +304,7 @@ namespace RVO {
* \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)
RVO3D_EXPORT inline float abs(const Vector3 &vector)
{
return std::sqrt(vector * vector);
}
@ -317,7 +315,7 @@ namespace RVO {
* \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)
RVO3D_EXPORT inline float absSq(const Vector3 &vector)
{
return vector * vector;
}
@ -328,7 +326,7 @@ namespace RVO {
* \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)
RVO3D_EXPORT inline Vector3 normalize(const Vector3 &vector)
{
return vector / abs(vector);
}

282
thirdparty/rvo2/patches/rvo2-godot-changes.patch vendored Normal file
View File

@ -0,0 +1,282 @@
diff --git a/thirdparty/rvo2/Agent.cpp b/thirdparty/rvo2/Agent.cpp
index 5e49a3554c..b35eee9c12 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 d3922ec645..45fbead2f5 100644
--- a/thirdparty/rvo2/Agent.h
+++ b/thirdparty/rvo2/Agent.h
@@ -41,30 +41,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.
@@ -73,16 +95,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_;
@@ -91,9 +107,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 5e9e9777a6..c857f299df 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 RVO3D_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 a09384c20f..69d8920ce0 100644
--- a/thirdparty/rvo2/KdTree.h
+++ b/thirdparty/rvo2/KdTree.h
@@ -41,6 +41,9 @@
#include "Vector3.h"
+// Note: Slightly modified to work better with Godot.
+// - Removed `sim_`.
+// - KdTree things are public
namespace RVO {
class Agent;
class RVOSimulator;
@@ -49,7 +52,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.
*/
@@ -90,12 +93,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);
@@ -110,7 +113,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 6c3223bb87..f44e311f29 100644
--- a/thirdparty/rvo2/Vector3.h
+++ b/thirdparty/rvo2/Vector3.h
@@ -41,7 +41,7 @@
#include <cstddef>
#include <ostream>
-#include "Export.h"
+#define RVO3D_EXPORT
namespace RVO {
/**
@@ -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.
- */
- 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.