6c78170d8c
Still tracking the `v1.0.1` tag for now, just reverting all the unnecessary style changes that created a diff with upstream.
309 lines
8.7 KiB
Diff
309 lines
8.7 KiB
Diff
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.
|