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.

(cherry picked from commit 6c78170d8c)

thirdparty/README.md

@@ -505,12 +505,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

thirdparty/rvo2/API.h

@@ -38,8 +38,6 @@
 #ifndef RVO_API_H_
 #define RVO_API_H_
 
-// -- GODOT start --
 #define RVO_API
-// -- GODOT end --
 
 #endif /* RVO_API_H_ */

thirdparty/rvo2/Agent.cpp

@@ -32,8 +32,8 @@
 
 #include "Agent.h"
 
-#include <algorithm>
 #include <cmath>
+#include <algorithm>
 
 #include "Definitions.h"
 #include "KdTree.h"
@@ -105,18 +105,18 @@ size_t linearProgram3(const std::vector<Plane> &planes, float radius, const Vect
 	 */
 	void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float radius, Vector3 &result);
 
-Agent::Agent() :
+	Agent::Agent() : id_(0), maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f), ignore_y_(false) { }
-        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;
@@ -200,7 +203,8 @@ void Agent::computeNewVelocity(float timeStep) {
 		}
 	}
 
-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_);
 
@@ -225,7 +229,8 @@ 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,19 +278,23 @@ 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;
 			}
 		}
@@ -291,7 +302,8 @@ bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line
 		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;
 
@@ -352,14 +366,17 @@ 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;
 		}
@@ -379,7 +396,8 @@ 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;
@@ -422,4 +442,4 @@ void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float r
 			}
 		}
 	}
-} // namespace RVO
+}

thirdparty/rvo2/Agent.h

@@ -73,7 +73,6 @@ public:
 	 * \brief   Defines an agent in the simulation.
 	 */
 	class Agent {
-
 	public:
 		/**
 		 * \brief   Constructs an agent instance.
@@ -116,6 +115,6 @@ public:
 
 		friend class KdTree;
 	};
-} // namespace RVO
+}
 
 #endif /* RVO_AGENT_H_ */

thirdparty/rvo2/KdTree.cpp

@@ -42,7 +42,8 @@ const size_t RVO_MAX_LEAF_SIZE = 10;
 
 	KdTree::KdTree() { }
 
-void KdTree::buildAgentTree(std::vector<Agent *> agents) {
+	void KdTree::buildAgentTree(std::vector<Agent *> agents)
+	{
 		agents_.swap(agents);
 
 		if (!agents_.empty()) {
@@ -51,7 +52,8 @@ void KdTree::buildAgentTree(std::vector<Agent *> agents) {
 		}
 	}
 
-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;
 			}
 
@@ -116,16 +120,19 @@ void KdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node) {
 		}
 	}
 
-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);
 
@@ -149,4 +157,4 @@ void KdTree::queryAgentTreeRecursive(Agent *agent, float &rangeSq, size_t node)
 			}
 		}
 	}
-} // namespace RVO
+}

thirdparty/rvo2/KdTree.h

@@ -119,6 +119,6 @@ public:
 		friend class Agent;
 		friend class RVOSimulator;
 	};
-} // namespace RVO
+}
 
 #endif /* RVO_KD_TREE_H_ */

thirdparty/rvo2/patches/rvo2-godot-changes.patch

@@ -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.