Rework Navigation Avoidance

Rework Navigation Avoidance.

8 files changed, 1839 insertions, 0 deletions

diff --git a/thirdparty/rvo2/rvo2_3d/Agent3d.cpp b/thirdparty/rvo2/rvo2_3d/Agent3d.cpp
new file mode 100644
index 0000000000..bddf226db1
--- /dev/null
+++ b/thirdparty/rvo2/rvo2_3d/Agent3d.cpp
@@ -0,0 +1,449 @@
+/*
+ * Agent.cpp
+ * 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
+ *
+ *     https://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Please send all bug reports to <geom@cs.unc.edu>.
+ *
+ * The authors may be contacted via:
+ *
+ * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
+ * Dept. of Computer Science
+ * 201 S. Columbia St.
+ * Frederick P. Brooks, Jr. Computer Science Bldg.
+ * Chapel Hill, N.C. 27599-3175
+ * United States of America
+ *
+ * <https://gamma.cs.unc.edu/RVO2/>
+ */
+
+#include "Agent3d.h"
+
+#include <cmath>
+#include <algorithm>
+
+#include "Definitions.h"
+#include "KdTree3d.h"
+
+namespace RVO3D {
+	/**
+	 * \brief   A sufficiently small positive number.
+	 */
+	const float RVO3D_EPSILON = 0.00001f;
+
+	/**
+	 * \brief   Defines a directed line.
+	 */
+	class Line3D {
+	public:
+		/**
+		 * \brief   The direction of the directed line.
+		 */
+		Vector3 direction;
+
+		/**
+		 * \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.
+	 * \param   line          The line on which the 1-d linear program is solved
+	 * \param   radius        The radius of the spherical constraint.
+	 * \param   optVelocity   The optimization velocity.
+	 * \param   directionOpt  True if the direction should be optimized.
+	 * \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 Line3D &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
+	 * \param   radius        The radius of the spherical constraint.
+	 * \param   optVelocity   The optimization velocity.
+	 * \param   directionOpt  True if the direction should be optimized.
+	 * \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);
+
+	/**
+	 * \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.
+	 * \param   optVelocity   The optimization velocity.
+	 * \param   directionOpt  True if the direction should be optimized.
+	 * \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);
+
+	/**
+	 * \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);
+
+	Agent3D::Agent3D() : id_(0), maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f) { }
+
+	void Agent3D::computeNeighbors(RVOSimulator3D *sim_)
+	{
+		agentNeighbors_.clear();
+
+		if (maxNeighbors_ > 0) {
+			sim_->kdTree_->computeAgentNeighbors(this, neighborDist_ * neighborDist_);
+		}
+	}
+
+	void Agent3D::computeNewVelocity(RVOSimulator3D *sim_)
+	{
+		orcaPlanes_.clear();
+
+		const float invTimeHorizon = 1.0f / timeHorizon_;
+
+		/* Create agent ORCA planes. */
+		for (size_t i = 0; i < agentNeighbors_.size(); ++i) {
+			const Agent3D *const other = agentNeighbors_[i].second;
+
+			//const float timeHorizon_mod = (avoidance_priority_ - other->avoidance_priority_ + 1.0f) * 0.5f;
+			//const float invTimeHorizon = (1.0f / timeHorizon_) * timeHorizon_mod;
+
+			const Vector3 relativePosition = other->position_ - position_;
+			const Vector3 relativeVelocity = velocity_ - other->velocity_;
+			const float distSq = absSq(relativePosition);
+			const float combinedRadius = radius_ + other->radius_;
+			const float combinedRadiusSq = sqr(combinedRadius);
+
+			Plane plane;
+			Vector3 u;
+
+			if (distSq > combinedRadiusSq) {
+				/* No collision. */
+				const Vector3 w = relativeVelocity - invTimeHorizon * relativePosition;
+				/* Vector from cutoff center to relative velocity. */
+				const float wLengthSq = absSq(w);
+
+				const float dotProduct = w * relativePosition;
+
+				if (dotProduct < 0.0f && sqr(dotProduct) > combinedRadiusSq * wLengthSq) {
+					/* Project on cut-off circle. */
+					const float wLength = std::sqrt(wLengthSq);
+					const Vector3 unitW = w / wLength;
+
+					plane.normal = unitW;
+					u = (combinedRadius * invTimeHorizon - wLength) * unitW;
+				}
+				else {
+					/* Project on cone. */
+					const float a = distSq;
+					const float b = relativePosition * relativeVelocity;
+					const float c = absSq(relativeVelocity) - absSq(cross(relativePosition, relativeVelocity)) / (distSq - combinedRadiusSq);
+					const float t = (b + std::sqrt(sqr(b) - a * c)) / a;
+					const Vector3 w = relativeVelocity - t * relativePosition;
+					const float wLength = abs(w);
+					const Vector3 unitW = w / wLength;
+
+					plane.normal = unitW;
+					u = (combinedRadius * t - wLength) * unitW;
+				}
+			}
+			else {
+				/* Collision. */
+				const float invTimeStep = 1.0f / sim_->timeStep_;
+				const Vector3 w = relativeVelocity - invTimeStep * relativePosition;
+				const float wLength = abs(w);
+				const Vector3 unitW = w / wLength;
+
+				plane.normal = unitW;
+				u = (combinedRadius * invTimeStep - wLength) * unitW;
+			}
+
+			plane.point = velocity_ + 0.5f * u;
+			orcaPlanes_.push_back(plane);
+		}
+
+		const size_t planeFail = linearProgram3(orcaPlanes_, maxSpeed_, prefVelocity_, false, newVelocity_);
+
+		if (planeFail < orcaPlanes_.size()) {
+			linearProgram4(orcaPlanes_, planeFail, maxSpeed_, newVelocity_);
+		}
+	}
+
+	void Agent3D::insertAgentNeighbor(const Agent3D *agent, float &rangeSq)
+	{
+		// no point processing same agent
+		if (this == agent) {
+			return;
+		}
+		// ignore other agent if layers/mask bitmasks have no matching bit
+		if ((avoidance_mask_ & agent->avoidance_layers_) == 0) {
+			return;
+		}
+
+		if (avoidance_priority_ > agent->avoidance_priority_) {
+			return;
+		}
+
+		const float distSq = absSq(position_ - agent->position_);
+
+		if (distSq < rangeSq) {
+			if (agentNeighbors_.size() < maxNeighbors_) {
+				agentNeighbors_.push_back(std::make_pair(distSq, agent));
+			}
+
+			size_t i = agentNeighbors_.size() - 1;
+
+			while (i != 0 && distSq < agentNeighbors_[i - 1].first) {
+				agentNeighbors_[i] = agentNeighbors_[i - 1];
+				--i;
+			}
+
+			agentNeighbors_[i] = std::make_pair(distSq, agent);
+
+			if (agentNeighbors_.size() == maxNeighbors_) {
+				rangeSq = agentNeighbors_.back().first;
+			}
+		}
+	}
+
+	void Agent3D::update(RVOSimulator3D *sim_)
+	{
+		velocity_ = newVelocity_;
+		position_ += velocity_ * sim_->timeStep_;
+	}
+
+	bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line3D &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);
+
+		if (discriminant < 0.0f) {
+			/* Max speed sphere fully invalidates line. */
+			return false;
+		}
+
+		const float sqrtDiscriminant = std::sqrt(discriminant);
+		float tLeft = -dotProduct - sqrtDiscriminant;
+		float tRight = -dotProduct + sqrtDiscriminant;
+
+		for (size_t i = 0; i < planeNo; ++i) {
+			const float numerator = (planes[i].point - line.point) * planes[i].normal;
+			const float denominator = line.direction * planes[i].normal;
+
+			if (sqr(denominator) <= RVO3D_EPSILON) {
+				/* Lines3D line is (almost) parallel to plane i. */
+				if (numerator > 0.0f) {
+					return false;
+				}
+				else {
+					continue;
+				}
+			}
+
+			const float t = numerator / denominator;
+
+			if (denominator >= 0.0f) {
+				/* Plane i bounds line on the left. */
+				tLeft = std::max(tLeft, t);
+			}
+			else {
+				/* Plane i bounds line on the right. */
+				tRight = std::min(tRight, t);
+			}
+
+			if (tLeft > tRight) {
+				return false;
+			}
+		}
+
+		if (directionOpt) {
+			/* Optimize direction. */
+			if (optVelocity * line.direction > 0.0f) {
+				/* Take right extreme. */
+				result = line.point + tRight * line.direction;
+			}
+			else {
+				/* Take left extreme. */
+				result = line.point + tLeft * line.direction;
+			}
+		}
+		else {
+			/* Optimize closest point. */
+			const float t = line.direction * (optVelocity - line.point);
+
+			if (t < tLeft) {
+				result = line.point + tLeft * line.direction;
+			}
+			else if (t > tRight) {
+				result = line.point + tRight * line.direction;
+			}
+			else {
+				result = line.point + t * line.direction;
+			}
+		}
+
+		return true;
+	}
+
+	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);
+
+		if (planeDistSq > radiusSq) {
+			/* Max speed sphere fully invalidates plane planeNo. */
+			return false;
+		}
+
+		const float planeRadiusSq = radiusSq - planeDistSq;
+
+		const Vector3 planeCenter = planeDist * planes[planeNo].normal;
+
+		if (directionOpt) {
+			/* Project direction optVelocity on plane planeNo. */
+			const Vector3 planeOptVelocity = optVelocity - (optVelocity * planes[planeNo].normal) * planes[planeNo].normal;
+			const float planeOptVelocityLengthSq = absSq(planeOptVelocity);
+
+			if (planeOptVelocityLengthSq <= RVO3D_EPSILON) {
+				result = planeCenter;
+			}
+			else {
+				result = planeCenter + std::sqrt(planeRadiusSq / planeOptVelocityLengthSq) * planeOptVelocity;
+			}
+		}
+		else {
+			/* Project point optVelocity on plane planeNo. */
+			result = optVelocity + ((planes[planeNo].point - optVelocity) * planes[planeNo].normal) * planes[planeNo].normal;
+
+			/* If outside planeCircle, project on planeCircle. */
+			if (absSq(result) > radiusSq) {
+				const Vector3 planeResult = result - planeCenter;
+				const float planeResultLengthSq = absSq(planeResult);
+				result = planeCenter + std::sqrt(planeRadiusSq / planeResultLengthSq) * planeResult;
+			}
+		}
+
+		for (size_t i = 0; i < planeNo; ++i) {
+			if (planes[i].normal * (planes[i].point - result) > 0.0f) {
+				/* Result does not satisfy constraint i. Compute new optimal result. */
+				/* Compute intersection line of plane i and plane planeNo. */
+				Vector3 crossProduct = cross(planes[i].normal, planes[planeNo].normal);
+
+				if (absSq(crossProduct) <= RVO3D_EPSILON) {
+					/* Planes planeNo and i are (almost) parallel, and plane i fully invalidates plane planeNo. */
+					return false;
+				}
+
+				Line3D line;
+				line.direction = normalize(crossProduct);
+				const Vector3 lineNormal = cross(line.direction, planes[planeNo].normal);
+				line.point = planes[planeNo].point + (((planes[i].point - planes[planeNo].point) * planes[i].normal) / (lineNormal * planes[i].normal)) * lineNormal;
+
+				if (!linearProgram1(planes, i, line, radius, optVelocity, directionOpt, result)) {
+					return false;
+				}
+			}
+		}
+
+		return true;
+	}
+
+	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)) {
+			/* Optimize closest point and outside circle. */
+			result = normalize(optVelocity) * radius;
+		}
+		else {
+			/* Optimize closest point and inside circle. */
+			result = optVelocity;
+		}
+
+		for (size_t i = 0; i < planes.size(); ++i) {
+			if (planes[i].normal * (planes[i].point - result) > 0.0f) {
+				/* Result does not satisfy constraint i. Compute new optimal result. */
+				const Vector3 tempResult = result;
+
+				if (!linearProgram2(planes, i, radius, optVelocity, directionOpt, result)) {
+					result = tempResult;
+					return i;
+				}
+			}
+		}
+
+		return planes.size();
+	}
+
+	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) {
+			if (planes[i].normal * (planes[i].point - result) > distance) {
+				/* Result does not satisfy constraint of plane i. */
+				std::vector<Plane> projPlanes;
+
+				for (size_t j = 0; j < i; ++j) {
+					Plane plane;
+
+					const Vector3 crossProduct = cross(planes[j].normal, planes[i].normal);
+
+					if (absSq(crossProduct) <= RVO3D_EPSILON) {
+						/* Plane i and plane j are (almost) parallel. */
+						if (planes[i].normal * planes[j].normal > 0.0f) {
+							/* Plane i and plane j point in the same direction. */
+							continue;
+						}
+						else {
+							/* Plane i and plane j point in opposite direction. */
+							plane.point = 0.5f * (planes[i].point + planes[j].point);
+						}
+					}
+					else {
+						/* Plane.point is point on line of intersection between plane i and plane j. */
+						const Vector3 lineNormal = cross(crossProduct, planes[i].normal);
+						plane.point = planes[i].point + (((planes[j].point - planes[i].point) * planes[j].normal) / (lineNormal * planes[j].normal)) * lineNormal;
+					}
+
+					plane.normal = normalize(planes[j].normal - planes[i].normal);
+					projPlanes.push_back(plane);
+				}
+
+				const Vector3 tempResult = result;
+
+				if (linearProgram3(projPlanes, radius, planes[i].normal, true, result) < projPlanes.size()) {
+					/* This should in principle not happen.  The result is by definition already in the feasible region of this linear program. If it fails, it is due to small floating point error, and the current result is kept. */
+					result = tempResult;
+				}
+
+				distance = planes[i].normal * (planes[i].point - result);
+			}
+		}
+	}
+}
diff --git a/thirdparty/rvo2/rvo2_3d/Agent3d.h b/thirdparty/rvo2/rvo2_3d/Agent3d.h
new file mode 100644
index 0000000000..3e43646871
--- /dev/null
+++ b/thirdparty/rvo2/rvo2_3d/Agent3d.h
@@ -0,0 +1,105 @@
+/*
+ * Agent.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    Agent.h
+ * \brief   Contains the Agent class.
+ */
+#ifndef RVO3D_AGENT_H_
+#define RVO3D_AGENT_H_
+
+#include <cstddef>
+#include <utility>
+#include <vector>
+
+#include "RVOSimulator3d.h"
+#include "Vector3.h"
+
+namespace RVO3D {
+	/**
+	 * \brief   Defines an agent in the simulation.
+	 */
+	class Agent3D {
+	public:
+		/**
+		 * \brief   Constructs an agent instance.
+		 * \param   sim  The simulator instance.
+		 */
+		explicit Agent3D();
+
+		/**
+		 * \brief   Computes the neighbors of this agent.
+		 */
+		void computeNeighbors(RVOSimulator3D *sim_);
+
+		/**
+		 * \brief   Computes the new velocity of this agent.
+		 */
+		void computeNewVelocity(RVOSimulator3D *sim_);
+
+		/**
+		 * \brief   Inserts an agent neighbor into the set of neighbors of this agent.
+		 * \param   agent    A pointer to the agent to be inserted.
+		 * \param   rangeSq  The squared range around this agent.
+		 */
+		void insertAgentNeighbor(const Agent3D *agent, float &rangeSq);
+
+		/**
+		 * \brief   Updates the three-dimensional position and three-dimensional velocity of this agent.
+		 */
+		void update(RVOSimulator3D *sim_);
+
+		Vector3 newVelocity_;
+		Vector3 position_;
+		Vector3 prefVelocity_;
+		Vector3 velocity_;
+		RVOSimulator3D *sim_;
+		size_t id_;
+		size_t maxNeighbors_;
+		float maxSpeed_;
+		float neighborDist_;
+		float radius_;
+		float timeHorizon_;
+		float timeHorizonObst_;
+		std::vector<std::pair<float, const Agent3D *> > agentNeighbors_;
+		std::vector<Plane> orcaPlanes_;
+		float height_ = 1.0;
+		uint32_t avoidance_layers_ = 1;
+		uint32_t avoidance_mask_ = 1;
+		float avoidance_priority_ = 1.0;
+
+		friend class KdTree3D;
+		friend class RVOSimulator3D;
+	};
+}
+
+#endif /* RVO3D_AGENT_H_ */
diff --git a/thirdparty/rvo2/rvo2_3d/Definitions.h b/thirdparty/rvo2/rvo2_3d/Definitions.h
new file mode 100644
index 0000000000..34d1d06e0a
--- /dev/null
+++ b/thirdparty/rvo2/rvo2_3d/Definitions.h
@@ -0,0 +1,53 @@
+/*
+ * Definitions.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
+ *
+ *     https://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Please send all bug reports to <geom@cs.unc.edu>.
+ *
+ * The authors may be contacted via:
+ *
+ * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
+ * Dept. of Computer Science
+ * 201 S. Columbia St.
+ * Frederick P. Brooks, Jr. Computer Science Bldg.
+ * Chapel Hill, N.C. 27599-3175
+ * United States of America
+ *
+ * <https://gamma.cs.unc.edu/RVO2/>
+ */
+
+/**
+ * \file   Definitions.h
+ * \brief  Contains functions and constants used in multiple classes.
+ */
+
+#ifndef RVO3D_DEFINITIONS_H_
+#define RVO3D_DEFINITIONS_H_
+
+namespace RVO3D {
+	/**
+	 * \brief   Computes the square of a float.
+	 * \param   scalar  The float to be squared.
+	 * \return  The square of the float.
+	 */
+	inline float sqr(float scalar)
+	{
+		return scalar * scalar;
+	}
+}
+
+#endif /* RVO3D_DEFINITIONS_H_ */
diff --git a/thirdparty/rvo2/rvo2_3d/KdTree3d.cpp b/thirdparty/rvo2/rvo2_3d/KdTree3d.cpp
new file mode 100644
index 0000000000..2534871db1
--- /dev/null
+++ b/thirdparty/rvo2/rvo2_3d/KdTree3d.cpp
@@ -0,0 +1,161 @@
+/*
+ * KdTree.cpp
+ * 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
+ *
+ *     https://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Please send all bug reports to <geom@cs.unc.edu>.
+ *
+ * The authors may be contacted via:
+ *
+ * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
+ * Dept. of Computer Science
+ * 201 S. Columbia St.
+ * Frederick P. Brooks, Jr. Computer Science Bldg.
+ * Chapel Hill, N.C. 27599-3175
+ * United States of America
+ *
+ * <https://gamma.cs.unc.edu/RVO2/>
+ */
+
+#include "KdTree3d.h"
+
+#include <algorithm>
+
+#include "Agent3d.h"
+#include "Definitions.h"
+#include "RVOSimulator3d.h"
+
+namespace RVO3D {
+	const size_t RVO3D_MAX_LEAF_SIZE = 10;
+
+	KdTree3D::KdTree3D(RVOSimulator3D *sim) : sim_(sim) { }
+
+	void KdTree3D::buildAgentTree(std::vector<Agent3D *> agents)
+	{
+		agents_.swap(agents);
+
+		if (!agents_.empty()) {
+			agentTree_.resize(2 * agents_.size() - 1);
+			buildAgentTreeRecursive(0, agents_.size(), 0);
+		}
+	}
+
+	void KdTree3D::buildAgentTreeRecursive(size_t begin, size_t end, size_t node)
+	{
+		agentTree_[node].begin = begin;
+		agentTree_[node].end = end;
+		agentTree_[node].minCoord = agents_[begin]->position_;
+		agentTree_[node].maxCoord = agents_[begin]->position_;
+
+		for (size_t i = begin + 1; i < end; ++i) {
+			agentTree_[node].maxCoord[0] = std::max(agentTree_[node].maxCoord[0], agents_[i]->position_.x());
+			agentTree_[node].minCoord[0] = std::min(agentTree_[node].minCoord[0], agents_[i]->position_.x());
+			agentTree_[node].maxCoord[1] = std::max(agentTree_[node].maxCoord[1], agents_[i]->position_.y());
+			agentTree_[node].minCoord[1] = std::min(agentTree_[node].minCoord[1], agents_[i]->position_.y());
+			agentTree_[node].maxCoord[2] = std::max(agentTree_[node].maxCoord[2], agents_[i]->position_.z());
+			agentTree_[node].minCoord[2] = std::min(agentTree_[node].minCoord[2], agents_[i]->position_.z());
+		}
+
+		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]) {
+				coord = 1;
+			}
+			else {
+				coord = 2;
+			}
+
+			const float splitValue = 0.5f * (agentTree_[node].maxCoord[coord] + agentTree_[node].minCoord[coord]);
+
+			size_t left = begin;
+
+			size_t right = end;
+
+			while (left < right) {
+				while (left < right && agents_[left]->position_[coord] < splitValue) {
+					++left;
+				}
+
+				while (right > left && agents_[right - 1]->position_[coord] >= splitValue) {
+					--right;
+				}
+
+				if (left < right) {
+					std::swap(agents_[left], agents_[right - 1]);
+					++left;
+					--right;
+				}
+			}
+
+			size_t leftSize = left - begin;
+
+			if (leftSize == 0) {
+				++leftSize;
+				++left;
+				++right;
+			}
+
+			agentTree_[node].left = node + 1;
+			agentTree_[node].right = node + 2 * leftSize;
+
+			buildAgentTreeRecursive(begin, left, agentTree_[node].left);
+			buildAgentTreeRecursive(left, end, agentTree_[node].right);
+		}
+	}
+
+	void KdTree3D::computeAgentNeighbors(Agent3D *agent, float rangeSq) const
+	{
+		queryAgentTreeRecursive(agent, rangeSq, 0);
+	}
+
+	void KdTree3D::queryAgentTreeRecursive(Agent3D *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 {
+			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]));
+
+			if (distSqLeft < distSqRight) {
+				if (distSqLeft < rangeSq) {
+					queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
+
+					if (distSqRight < rangeSq) {
+						queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
+					}
+				}
+			}
+			else {
+				if (distSqRight < rangeSq) {
+					queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
+
+					if (distSqLeft < rangeSq) {
+						queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
+					}
+				}
+			}
+		}
+	}
+}
diff --git a/thirdparty/rvo2/rvo2_3d/KdTree3d.h b/thirdparty/rvo2/rvo2_3d/KdTree3d.h
new file mode 100644
index 0000000000..c018f98b23
--- /dev/null
+++ b/thirdparty/rvo2/rvo2_3d/KdTree3d.h
@@ -0,0 +1,120 @@
+/*
+ * KdTree.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
+ *
+ *     https://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Please send all bug reports to <geom@cs.unc.edu>.
+ *
+ * The authors may be contacted via:
+ *
+ * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
+ * Dept. of Computer Science
+ * 201 S. Columbia St.
+ * Frederick P. Brooks, Jr. Computer Science Bldg.
+ * Chapel Hill, N.C. 27599-3175
+ * United States of America
+ *
+ * <https://gamma.cs.unc.edu/RVO2/>
+ */
+/**
+ * \file    KdTree.h
+ * \brief   Contains the KdTree class.
+ */
+#ifndef RVO3D_KD_TREE_H_
+#define RVO3D_KD_TREE_H_
+
+#include <cstddef>
+#include <vector>
+
+#include "Vector3.h"
+
+namespace RVO3D {
+	class Agent3D;
+	class RVOSimulator3D;
+
+	/**
+	 * \brief   Defines <i>k</i>d-trees for agents in the simulation.
+	 */
+	class KdTree3D {
+	public:
+		/**
+		 * \brief   Defines an agent <i>k</i>d-tree node.
+		 */
+		class AgentTreeNode3D {
+		public:
+			/**
+			 * \brief   The beginning node number.
+			 */
+			size_t begin;
+
+			/**
+			 * \brief   The ending node number.
+			 */
+			size_t end;
+
+			/**
+			 * \brief   The left node number.
+			 */
+			size_t left;
+
+			/**
+			 * \brief   The right node number.
+			 */
+			size_t right;
+
+			/**
+			 * \brief   The maximum coordinates.
+			 */
+			Vector3 maxCoord;
+
+			/**
+			 * \brief   The minimum coordinates.
+			 */
+			Vector3 minCoord;
+		};
+
+		/**
+		 * \brief   Constructs a <i>k</i>d-tree instance.
+		 * \param   sim  The simulator instance.
+		 */
+		explicit KdTree3D(RVOSimulator3D *sim);
+
+		/**
+		 * \brief   Builds an agent <i>k</i>d-tree.
+		 */
+		void buildAgentTree(std::vector<Agent3D *> agents);
+
+		void buildAgentTreeRecursive(size_t begin, size_t end, size_t node);
+
+		/**
+		 * \brief   Computes the agent neighbors of the specified agent.
+		 * \param   agent    A pointer to the agent for which agent neighbors are to be computed.
+		 * \param   rangeSq  The squared range around the agent.
+		 */
+		void computeAgentNeighbors(Agent3D *agent, float rangeSq) const;
+
+		void queryAgentTreeRecursive(Agent3D *agent, float &rangeSq, size_t node) const;
+
+		std::vector<Agent3D *> agents_;
+		std::vector<AgentTreeNode3D> agentTree_;
+		RVOSimulator3D *sim_;
+
+		friend class Agent3D;
+		friend class RVOSimulator3D;
+	};
+}
+
+#endif /* RVO3D_KD_TREE_H_ */
diff --git a/thirdparty/rvo2/rvo2_3d/RVOSimulator3d.cpp b/thirdparty/rvo2/rvo2_3d/RVOSimulator3d.cpp
new file mode 100644
index 0000000000..71e5aea9e2
--- /dev/null
+++ b/thirdparty/rvo2/rvo2_3d/RVOSimulator3d.cpp
@@ -0,0 +1,274 @@
+/*
+ * RVOSimulator.cpp
+ * 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/>
+ */
+
+#include "RVOSimulator3d.h"
+
+#ifdef _OPENMP
+#include <omp.h>
+#endif
+
+#include "Agent3d.h"
+#include "KdTree3d.h"
+
+namespace RVO3D {
+	RVOSimulator3D::RVOSimulator3D() : defaultAgent_(NULL), kdTree_(NULL), globalTime_(0.0f), timeStep_(0.0f)
+	{
+		kdTree_ = new KdTree3D(this);
+	}
+
+	RVOSimulator3D::RVOSimulator3D(float timeStep, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity) : defaultAgent_(NULL), kdTree_(NULL), globalTime_(0.0f), timeStep_(timeStep)
+	{
+		kdTree_ = new KdTree3D(this);
+		defaultAgent_ = new Agent3D();
+
+		defaultAgent_->maxNeighbors_ = maxNeighbors;
+		defaultAgent_->maxSpeed_ = maxSpeed;
+		defaultAgent_->neighborDist_ = neighborDist;
+		defaultAgent_->radius_ = radius;
+		defaultAgent_->timeHorizon_ = timeHorizon;
+		defaultAgent_->velocity_ = velocity;
+	}
+
+	RVOSimulator3D::~RVOSimulator3D()
+	{
+		if (defaultAgent_ != NULL) {
+			delete defaultAgent_;
+		}
+
+		for (size_t i = 0; i < agents_.size(); ++i) {
+			delete agents_[i];
+		}
+
+		if (kdTree_ != NULL) {
+			delete kdTree_;
+		}
+	}
+
+	size_t RVOSimulator3D::getAgentNumAgentNeighbors(size_t agentNo) const
+	{
+		return agents_[agentNo]->agentNeighbors_.size();
+	}
+
+	size_t RVOSimulator3D::getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const
+	{
+		return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_;
+	}
+
+	size_t RVOSimulator3D::getAgentNumORCAPlanes(size_t agentNo) const
+	{
+		return agents_[agentNo]->orcaPlanes_.size();
+	}
+
+	const Plane &RVOSimulator3D::getAgentORCAPlane(size_t agentNo, size_t planeNo) const
+	{
+		return agents_[agentNo]->orcaPlanes_[planeNo];
+	}
+
+	void RVOSimulator3D::removeAgent(size_t agentNo)
+	{
+		delete agents_[agentNo];
+		agents_[agentNo] = agents_.back();
+		agents_.pop_back();
+	}
+
+	size_t RVOSimulator3D::addAgent(const Vector3 &position)
+	{
+		if (defaultAgent_ == NULL) {
+			return RVO3D_ERROR;
+		}
+
+		Agent3D *agent = new Agent3D();
+
+		agent->position_ = position;
+		agent->maxNeighbors_ = defaultAgent_->maxNeighbors_;
+		agent->maxSpeed_ = defaultAgent_->maxSpeed_;
+		agent->neighborDist_ = defaultAgent_->neighborDist_;
+		agent->radius_ = defaultAgent_->radius_;
+		agent->timeHorizon_ = defaultAgent_->timeHorizon_;
+		agent->velocity_ = defaultAgent_->velocity_;
+
+		agent->id_ = agents_.size();
+
+		agents_.push_back(agent);
+
+		return agents_.size() - 1;
+	}
+
+	size_t RVOSimulator3D::addAgent(const Vector3 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity)
+	{
+		Agent3D *agent = new Agent3D();
+
+		agent->position_ = position;
+		agent->maxNeighbors_ = maxNeighbors;
+		agent->maxSpeed_ = maxSpeed;
+		agent->neighborDist_ = neighborDist;
+		agent->radius_ = radius;
+		agent->timeHorizon_ = timeHorizon;
+		agent->velocity_ = velocity;
+
+		agent->id_ = agents_.size();
+
+		agents_.push_back(agent);
+
+		return agents_.size() - 1;
+	}
+
+	void RVOSimulator3D::doStep()
+	{
+		kdTree_->buildAgentTree(agents_);
+
+		for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
+			agents_[i]->computeNeighbors(this);
+			agents_[i]->computeNewVelocity(this);
+		}
+
+		for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
+			agents_[i]->update(this);
+		}
+
+		globalTime_ += timeStep_;
+	}
+
+	size_t RVOSimulator3D::getAgentMaxNeighbors(size_t agentNo) const
+	{
+		return agents_[agentNo]->maxNeighbors_;
+	}
+
+	float RVOSimulator3D::getAgentMaxSpeed(size_t agentNo) const
+	{
+		return agents_[agentNo]->maxSpeed_;
+	}
+
+	float RVOSimulator3D::getAgentNeighborDist(size_t agentNo) const
+	{
+		return agents_[agentNo]->neighborDist_;
+	}
+
+	const Vector3 &RVOSimulator3D::getAgentPosition(size_t agentNo) const
+	{
+		return agents_[agentNo]->position_;
+	}
+
+	const Vector3 &RVOSimulator3D::getAgentPrefVelocity(size_t agentNo) const
+	{
+		return agents_[agentNo]->prefVelocity_;
+	}
+
+	float RVOSimulator3D::getAgentRadius(size_t agentNo) const
+	{
+		return agents_[agentNo]->radius_;
+	}
+
+	float RVOSimulator3D::getAgentTimeHorizon(size_t agentNo) const
+	{
+		return agents_[agentNo]->timeHorizon_;
+	}
+
+	const Vector3 &RVOSimulator3D::getAgentVelocity(size_t agentNo) const
+	{
+		return agents_[agentNo]->velocity_;
+	}
+
+	float RVOSimulator3D::getGlobalTime() const
+	{
+		return globalTime_;
+	}
+
+	size_t RVOSimulator3D::getNumAgents() const
+	{
+		return agents_.size();
+	}
+
+	float RVOSimulator3D::getTimeStep() const
+	{
+		return timeStep_;
+	}
+
+	void RVOSimulator3D::setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity)
+	{
+		if (defaultAgent_ == NULL) {
+			defaultAgent_ = new Agent3D();
+		}
+
+		defaultAgent_->maxNeighbors_ = maxNeighbors;
+		defaultAgent_->maxSpeed_ = maxSpeed;
+		defaultAgent_->neighborDist_ = neighborDist;
+		defaultAgent_->radius_ = radius;
+		defaultAgent_->timeHorizon_ = timeHorizon;
+		defaultAgent_->velocity_ = velocity;
+	}
+
+	void RVOSimulator3D::setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors)
+	{
+		agents_[agentNo]->maxNeighbors_ = maxNeighbors;
+	}
+
+	void RVOSimulator3D::setAgentMaxSpeed(size_t agentNo, float maxSpeed)
+	{
+		agents_[agentNo]->maxSpeed_ = maxSpeed;
+	}
+
+	void RVOSimulator3D::setAgentNeighborDist(size_t agentNo, float neighborDist)
+	{
+		agents_[agentNo]->neighborDist_ = neighborDist;
+	}
+
+	void RVOSimulator3D::setAgentPosition(size_t agentNo, const Vector3 &position)
+	{
+		agents_[agentNo]->position_ = position;
+	}
+
+	void RVOSimulator3D::setAgentPrefVelocity(size_t agentNo, const Vector3 &prefVelocity)
+	{
+		agents_[agentNo]->prefVelocity_ = prefVelocity;
+	}
+
+	void RVOSimulator3D::setAgentRadius(size_t agentNo, float radius)
+	{
+		agents_[agentNo]->radius_ = radius;
+	}
+
+	void RVOSimulator3D::setAgentTimeHorizon(size_t agentNo, float timeHorizon)
+	{
+		agents_[agentNo]->timeHorizon_ = timeHorizon;
+	}
+
+	void RVOSimulator3D::setAgentVelocity(size_t agentNo, const Vector3 &velocity)
+	{
+		agents_[agentNo]->velocity_ = velocity;
+	}
+
+	void RVOSimulator3D::setTimeStep(float timeStep)
+	{
+		timeStep_ = timeStep;
+	}
+}
diff --git a/thirdparty/rvo2/rvo2_3d/RVOSimulator3d.h b/thirdparty/rvo2/rvo2_3d/RVOSimulator3d.h
new file mode 100644
index 0000000000..4ea093d74c
--- /dev/null
+++ b/thirdparty/rvo2/rvo2_3d/RVOSimulator3d.h
@@ -0,0 +1,324 @@
+/*
+ * RVOSimulator.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    RVOSimulator.h
+ * \brief   Contains the RVOSimulator class.
+ */
+#ifndef RVO3D_RVO_SIMULATOR_H_
+#define RVO3D_RVO_SIMULATOR_H_
+
+#include <cstddef>
+#include <limits>
+#include <vector>
+
+#include "Vector3.h"
+
+namespace RVO3D {
+	class Agent3D;
+	class KdTree3D;
+
+	/**
+	 * \brief   Error value.
+	 *
+	 * A value equal to the largest unsigned integer, which is returned in case of an error by functions in RVO3D::RVOSimulator.
+	 */
+	const size_t RVO3D_ERROR = std::numeric_limits<size_t>::max();
+
+	/**
+	 * \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 the simulation.
+	 *
+	 * The main class of the library that contains all simulation functionality.
+	 */
+	class RVOSimulator3D {
+	public:
+		/**
+		 * \brief   Constructs a simulator instance.
+		 */
+		RVOSimulator3D();
+
+		/**
+		 * \brief   Constructs a simulator instance and sets the default properties for any new agent that is added.
+		 * \param   timeStep      The time step of the simulation. Must be positive.
+		 * \param   neighborDist  The default maximum distance (center point to center point) to other agents a new agent takes into account in the navigation. The larger this number, the longer he running time of the simulation. If the number is too low, the simulation will not be safe. Must be non-negative.
+		 * \param   maxNeighbors  The default maximum number of other agents a new agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe.
+		 * \param   timeHorizon   The default minimum amount of time for which a new agent's velocities that are computed by the simulation are safe with respect to other agents. The larger this number, the sooner an agent will respond to the presence of other agents, but the less freedom the agent has in choosing its velocities. Must be positive.
+		 * \param   radius        The default radius of a new agent. Must be non-negative.
+		 * \param   maxSpeed      The default maximum speed of a new agent. Must be non-negative.
+		 * \param   velocity      The default initial three-dimensional linear velocity of a new agent (optional).
+		 */
+		RVOSimulator3D(float timeStep, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity = Vector3());
+
+		/**
+		 * \brief   Destroys this simulator instance.
+		 */
+		~RVOSimulator3D();
+
+		/**
+		 * \brief   Adds a new agent with default properties to the simulation.
+		 * \param   position  The three-dimensional starting position of this agent.
+		 * \return  The number of the agent, or RVO3D::RVO3D_ERROR when the agent defaults have not been set.
+		 */
+		size_t addAgent(const Vector3 &position);
+
+		/**
+		 * \brief   Adds a new agent to the simulation.
+		 * \param   position      The three-dimensional starting position of this agent.
+		 * \param   neighborDist  The maximum distance (center point to center point) to other agents this agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe. Must be non-negative.
+		 * \param   maxNeighbors  The maximum number of other agents this agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe.
+		 * \param   timeHorizon   The minimum amount of time for which this agent's velocities that are computed by the simulation are safe with respect to other agents. The larger this number, the sooner this agent will respond to the presence of other agents, but the less freedom this agent has in choosing its velocities. Must be positive.
+		 * \param   radius        The radius of this agent. Must be non-negative.
+		 * \param   maxSpeed      The maximum speed of this agent. Must be non-negative.
+		 * \param   velocity      The initial three-dimensional linear velocity of this agent (optional).
+		 * \return  The number of the agent.
+		 */
+		size_t addAgent(const Vector3 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity = Vector3());
+
+		/**
+		 * \brief   Lets the simulator perform a simulation step and updates the three-dimensional position and three-dimensional velocity of each agent.
+		 */
+		void doStep();
+
+		/**
+		 * \brief   Returns the specified agent neighbor of the specified agent.
+		 * \param   agentNo     The number of the agent whose agent neighbor is to be retrieved.
+		 * \param   neighborNo  The number of the agent neighbor to be retrieved.
+		 * \return  The number of the neighboring agent.
+		 */
+		size_t getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const;
+
+		/**
+		 * \brief   Returns the maximum neighbor count of a specified agent.
+		 * \param   agentNo  The number of the agent whose maximum neighbor count is to be retrieved.
+		 * \return  The present maximum neighbor count of the agent.
+		 */
+		size_t getAgentMaxNeighbors(size_t agentNo) const;
+
+		/**
+		 * \brief   Returns the maximum speed of a specified agent.
+		 * \param   agentNo  The number of the agent whose maximum speed is to be retrieved.
+		 * \return  The present maximum speed of the agent.
+		 */
+		float getAgentMaxSpeed(size_t agentNo) const;
+
+		/**
+		 * \brief   Returns the maximum neighbor distance of a specified agent.
+		 * \param   agentNo  The number of the agent whose maximum neighbor distance is to be retrieved.
+		 * \return  The present maximum neighbor distance of the agent.
+		 */
+		float getAgentNeighborDist(size_t agentNo) const;
+
+		/**
+		 * \brief   Returns the count of agent neighbors taken into account to compute the current velocity for the specified agent.
+		 * \param   agentNo  The number of the agent whose count of agent neighbors is to be retrieved.
+		 * \return  The count of agent neighbors taken into account to compute the current velocity for the specified agent.
+		 */
+		size_t getAgentNumAgentNeighbors(size_t agentNo) const;
+
+		/**
+		 * \brief   Returns the count of ORCA constraints used to compute the current velocity for the specified agent.
+		 * \param   agentNo  The number of the agent whose count of ORCA constraints is to be retrieved.
+		 * \return  The count of ORCA constraints used to compute the current velocity for the specified agent.
+		 */
+		size_t getAgentNumORCAPlanes(size_t agentNo) const;
+
+		/**
+		 * \brief   Returns the specified ORCA constraint of the specified agent.
+		 * \param   agentNo  The number of the agent whose ORCA constraint is to be retrieved.
+		 * \param   planeNo  The number of the ORCA constraint to be retrieved.
+		 * \return  A plane representing the specified ORCA constraint.
+		 * \note    The halfspace to which the normal of the plane points is the region of permissible velocities with respect to the specified ORCA constraint.
+		 */
+		const Plane &getAgentORCAPlane(size_t agentNo, size_t planeNo) const;
+
+		/**
+		 * \brief   Returns the three-dimensional position of a specified agent.
+		 * \param   agentNo  The number of the agent whose three-dimensional position is to be retrieved.
+		 * \return  The present three-dimensional position of the (center of the) agent.
+		 */
+		const Vector3 &getAgentPosition(size_t agentNo) const;
+
+		/**
+		 * \brief   Returns the three-dimensional preferred velocity of a specified agent.
+		 * \param   agentNo  The number of the agent whose three-dimensional preferred velocity is to be retrieved.
+		 * \return  The present three-dimensional preferred velocity of the agent.
+		 */
+		const Vector3 &getAgentPrefVelocity(size_t agentNo) const;
+
+		/**
+		 * \brief   Returns the radius of a specified agent.
+		 * \param   agentNo  The number of the agent whose radius is to be retrieved.
+		 * \return  The present radius of the agent.
+		 */
+		float getAgentRadius(size_t agentNo) const;
+
+		/**
+		 * \brief   Returns the time horizon of a specified agent.
+		 * \param   agentNo  The number of the agent whose time horizon is to be retrieved.
+		 * \return  The present time horizon of the agent.
+		 */
+		float getAgentTimeHorizon(size_t agentNo) const;
+
+		/**
+		 * \brief   Returns the three-dimensional linear velocity of a specified agent.
+		 * \param   agentNo  The number of the agent whose three-dimensional linear velocity is to be retrieved.
+		 * \return  The present three-dimensional linear velocity of the agent.
+		 */
+		const Vector3 &getAgentVelocity(size_t agentNo) const;
+
+		/**
+		 * \brief   Returns the global time of the simulation.
+		 * \return  The present global time of the simulation (zero initially).
+		 */
+		float getGlobalTime() const;
+
+		/**
+		 * \brief   Returns the count of agents in the simulation.
+		 * \return  The count of agents in the simulation.
+		 */
+		size_t getNumAgents() const;
+
+		/**
+		 * \brief   Returns the time step of the simulation.
+		 * \return  The present time step of the simulation.
+		 */
+		float getTimeStep() const;
+
+		/**
+		 * \brief   Removes an agent from the simulation.
+		 * \param   agentNo  The number of the agent that is to be removed.
+		 * \note    After the removal of the agent, the agent that previously had number getNumAgents() - 1 will now have number agentNo.
+		 */
+		void removeAgent(size_t agentNo);
+
+		/**
+		 * \brief   Sets the default properties for any new agent that is added.
+		 * \param   neighborDist  The default maximum distance (center point to center point) to other agents a new agent takes into account in the navigation. The larger this number, the longer he running time of the simulation. If the number is too low, the simulation will not be safe. Must be non-negative.
+		 * \param   maxNeighbors  The default maximum number of other agents a new agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe.
+		 * \param   timeHorizon   The default minimum amount of time for which a new agent's velocities that are computed by the simulation are safe with respect to other agents. The larger this number, the sooner an agent will respond to the presence of other agents, but the less freedom the agent has in choosing its velocities. Must be positive.
+		 * \param   radius        The default radius of a new agent. Must be non-negative.
+		 * \param   maxSpeed      The default maximum speed of a new agent. Must be non-negative.
+		 * \param   velocity      The default initial three-dimensional linear velocity of a new agent (optional).
+		 */
+		void setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity = Vector3());
+
+		/**
+		 * \brief   Sets the maximum neighbor count of a specified agent.
+		 * \param   agentNo       The number of the agent whose maximum neighbor count is to be modified.
+		 * \param   maxNeighbors  The replacement maximum neighbor count.
+		 */
+		void setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors);
+
+		/**
+		 * \brief   Sets the maximum speed of a specified agent.
+		 * \param   agentNo   The number of the agent whose maximum speed is to be modified.
+		 * \param   maxSpeed  The replacement maximum speed. Must be non-negative.
+		 */
+		void setAgentMaxSpeed(size_t agentNo, float maxSpeed);
+
+		/**
+		 * \brief   Sets the maximum neighbor distance of a specified agent.
+		 * \param   agentNo       The number of the agent whose maximum neighbor distance is to be modified.
+		 * \param   neighborDist  The replacement maximum neighbor distance. Must be non-negative.
+		 */
+		void setAgentNeighborDist(size_t agentNo, float neighborDist);
+
+		/**
+		 * \brief   Sets the three-dimensional position of a specified agent.
+		 * \param   agentNo   The number of the agent whose three-dimensional position is to be modified.
+		 * \param   position  The replacement of the three-dimensional position.
+		 */
+		void setAgentPosition(size_t agentNo, const Vector3 &position);
+
+		/**
+		 * \brief   Sets the three-dimensional preferred velocity of a specified agent.
+		 * \param   agentNo       The number of the agent whose three-dimensional preferred velocity is to be modified.
+		 * \param   prefVelocity  The replacement of the three-dimensional preferred velocity.
+		 */
+		void setAgentPrefVelocity(size_t agentNo, const Vector3 &prefVelocity);
+
+		/**
+		 * \brief   Sets the radius of a specified agent.
+		 * \param   agentNo  The number of the agent whose radius is to be modified.
+		 * \param   radius   The replacement radius. Must be non-negative.
+		 */
+		void setAgentRadius(size_t agentNo, float radius);
+
+		/**
+		 * \brief   Sets the time horizon of a specified agent with respect to other agents.
+		 * \param   agentNo      The number of the agent whose time horizon is to be modified.
+		 * \param   timeHorizon  The replacement time horizon with respect to other agents. Must be positive.
+		 */
+		void setAgentTimeHorizon(size_t agentNo, float timeHorizon);
+
+		/**
+		 * \brief   Sets the three-dimensional linear velocity of a specified agent.
+		 * \param   agentNo   The number of the agent whose three-dimensional linear velocity is to be modified.
+		 * \param   velocity  The replacement three-dimensional linear velocity.
+		 */
+		void setAgentVelocity(size_t agentNo, const Vector3 &velocity);
+
+		/**
+		 * \brief   Sets the time step of the simulation.
+		 * \param   timeStep  The time step of the simulation. Must be positive.
+		 */
+		void setTimeStep(float timeStep);
+
+	public:
+		Agent3D *defaultAgent_;
+		KdTree3D *kdTree_;
+		float globalTime_;
+		float timeStep_;
+		std::vector<Agent3D *> agents_;
+
+		friend class Agent3D;
+		friend class KdTree3D;
+	};
+}
+
+#endif
diff --git a/thirdparty/rvo2/rvo2_3d/Vector3.h b/thirdparty/rvo2/rvo2_3d/Vector3.h
new file mode 100644
index 0000000000..6fa4bb074c
--- /dev/null
+++ b/thirdparty/rvo2/rvo2_3d/Vector3.h
@@ -0,0 +1,353 @@
+/*
+ * Vector3.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
+ *
+ *     https://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Please send all bug reports to <geom@cs.unc.edu>.
+ *
+ * The authors may be contacted via:
+ *
+ * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
+ * Dept. of Computer Science
+ * 201 S. Columbia St.
+ * Frederick P. Brooks, Jr. Computer Science Bldg.
+ * Chapel Hill, N.C. 27599-3175
+ * United States of America
+ *
+ * <https://gamma.cs.unc.edu/RVO2/>
+ */
+
+/**
+ * \file    Vector3.h
+ * \brief   Contains the Vector3 class.
+ */
+#ifndef RVO3D_VECTOR3_H_
+#define RVO3D_VECTOR3_H_
+
+#include <cmath>
+#include <cstddef>
+#include <ostream>
+
+namespace RVO3D {
+	/**
+	 * \brief  Defines a three-dimensional vector.
+	 */
+	class Vector3 {
+	public:
+		/**
+		 * \brief   Constructs and initializes a three-dimensional vector instance to zero.
+		 */
+		inline Vector3()
+		{
+			val_[0] = 0.0f;
+			val_[1] = 0.0f;
+			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.
+		 */
+		inline explicit Vector3(const float val[3])
+		{
+			val_[0] = val[0];
+			val_[1] = val[1];
+			val_[2] = val[2];
+		}
+
+		/**
+		 * \brief   Constructs and initializes a three-dimensional vector from the specified xyz-coordinates.
+		 * \param   x  The x-coordinate of the three-dimensional vector.
+		 * \param   y  The y-coordinate of the three-dimensional vector.
+		 * \param   z  The z-coordinate of the three-dimensional vector.
+		 */
+		inline Vector3(float x, float y, float z)
+		{
+			val_[0] = x;
+			val_[1] = y;
+			val_[2] = z;
+		}
+
+		/**
+		 * \brief   Returns the x-coordinate of this three-dimensional vector.
+		 * \return  The x-coordinate of the three-dimensional vector.
+		 */
+		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.
+		 */
+		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.
+		 */
+		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.
+		 */
+		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.
+		 */
+		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.
+		 */
+		inline Vector3 operator-() const
+		{
+			return Vector3(-val_[0], -val_[1], -val_[2]);
+		}
+
+		/**
+		 * \brief   Computes the dot product of this three-dimensional vector with the specified three-dimensional vector.
+		 * \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.
+		 */
+		inline float operator*(const Vector3 &vector) const
+		{
+			return val_[0] * vector[0] + val_[1] * vector[1] + val_[2] * vector[2];
+		}
+
+		/**
+		 * \brief   Computes the scalar multiplication of this three-dimensional vector with the specified scalar value.
+		 * \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.
+		 */
+		inline Vector3 operator*(float scalar) const
+		{
+			return Vector3(val_[0] * scalar, val_[1] * scalar, val_[2] * scalar);
+		}
+
+		/**
+		 * \brief   Computes the scalar division of this three-dimensional vector with the specified scalar value.
+		 * \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.
+		 */
+		inline Vector3 operator/(float scalar) const
+		{
+			const float invScalar = 1.0f / scalar;
+
+			return Vector3(val_[0] * invScalar, val_[1] * invScalar, val_[2] * invScalar);
+		}
+
+		/**
+		 * \brief   Computes the vector sum of this three-dimensional vector with the specified three-dimensional vector.
+		 * \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.
+		 */
+		inline Vector3 operator+(const Vector3 &vector) const
+		{
+			return Vector3(val_[0] + vector[0], val_[1] + vector[1], val_[2] + vector[2]);
+		}
+
+		/**
+		 * \brief   Computes the vector difference of this three-dimensional vector with the specified three-dimensional vector.
+		 * \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.
+		 */
+		inline Vector3 operator-(const Vector3 &vector) const
+		{
+			return Vector3(val_[0] - vector[0], val_[1] - vector[1], val_[2] - vector[2]);
+		}
+
+		/**
+		 * \brief   Tests this three-dimensional vector for equality with the specified three-dimensional vector.
+		 * \param   vector  The three-dimensional vector with which to test for equality.
+		 * \return  True if the three-dimensional vectors are equal.
+		 */
+		inline bool operator==(const Vector3 &vector) const
+		{
+			return val_[0] == vector[0] && val_[1] == vector[1] && val_[2] == vector[2];
+		}
+
+		/**
+		 * \brief   Tests this three-dimensional vector for inequality with the specified three-dimensional vector.
+		 * \param   vector  The three-dimensional vector with which to test for inequality.
+		 * \return  True if the three-dimensional vectors are not equal.
+		 */
+		inline bool operator!=(const Vector3 &vector) const
+		{
+			return val_[0] != vector[0] || val_[1] != vector[1] || val_[2] != vector[2];
+		}
+
+		/**
+		 * \brief   Sets the value of this three-dimensional vector to the scalar multiplication of itself with the specified scalar value.
+		 * \param   scalar  The scalar value with which the scalar multiplication should be computed.
+		 * \return  A reference to this three-dimensional vector.
+		 */
+		inline Vector3 &operator*=(float scalar)
+		{
+			val_[0] *= scalar;
+			val_[1] *= scalar;
+			val_[2] *= scalar;
+
+			return *this;
+		}
+
+		/**
+		 * \brief   Sets the value of this three-dimensional vector to the scalar division of itself with the specified scalar value.
+		 * \param   scalar  The scalar value with which the scalar division should be computed.
+		 * \return  A reference to this three-dimensional vector.
+		 */
+		inline Vector3 &operator/=(float scalar)
+		{
+			const float invScalar = 1.0f / scalar;
+
+			val_[0] *= invScalar;
+			val_[1] *= invScalar;
+			val_[2] *= invScalar;
+
+			return *this;
+		}
+
+		/**
+		 * \brief   Sets the value of this three-dimensional vector to the vector
+		 *             sum of itself with the specified three-dimensional vector.
+		 * \param   vector  The three-dimensional vector with which the vector sum should be computed.
+		 * \return  A reference to this three-dimensional vector.
+		 */
+		inline Vector3 &operator+=(const Vector3 &vector)
+		{
+			val_[0] += vector[0];
+			val_[1] += vector[1];
+			val_[2] += vector[2];
+
+			return *this;
+		}
+
+		/**
+		 * \brief   Sets the value of this three-dimensional vector to the vector difference of itself with the specified three-dimensional vector.
+		 * \param   vector  The three-dimensional vector with which the vector difference should be computed.
+		 * \return  A reference to this three-dimensional vector.
+		 */
+		inline Vector3 &operator-=(const Vector3 &vector)
+		{
+			val_[0] -= vector[0];
+			val_[1] -= vector[1];
+			val_[2] -= vector[2];
+
+			return *this;
+		}
+
+		inline Vector3 &operator=(const Vector3 &vector)
+		{
+			val_[0] = vector[0];
+			val_[1] = vector[1];
+			val_[2] = vector[2];
+
+			return *this;
+		}
+
+	private:
+		float val_[3];
+	};
+
+
+	/**
+	 * \relates  Vector3
+	 * \brief    Computes the scalar multiplication of the specified three-dimensional vector with the specified scalar value.
+	 * \param    scalar  The scalar value with which the scalar multiplication should be computed.
+	 * \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)
+	{
+		return Vector3(scalar * vector[0], scalar * vector[1], scalar * vector[2]);
+	}
+
+	/**
+	 * \relates  Vector3
+	 * \brief    Computes the cross product of the specified three-dimensional vectors.
+	 * \param    vector1  The first vector with which the cross product should be computed.
+	 * \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)
+	{
+		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]);
+	}
+
+	/**
+	 * \relates  Vector3
+	 * \brief    Inserts the specified three-dimensional vector into the specified output stream.
+	 * \param    os      The output stream into which the three-dimensional vector should be inserted.
+	 * \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)
+	{
+		os << "(" << vector[0] << "," << vector[1] << "," << vector[2] << ")";
+
+		return os;
+	}
+
+	/**
+	 * \relates  Vector3
+	 * \brief    Computes the length of a specified three-dimensional vector.
+	 * \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)
+	{
+		return std::sqrt(vector * vector);
+	}
+
+	/**
+	 * \relates  Vector3
+	 * \brief    Computes the squared length of a specified three-dimensional vector.
+	 * \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)
+	{
+		return vector * vector;
+	}
+
+	/**
+	 * \relates  Vector3
+	 * \brief    Computes the normalization of the specified three-dimensional vector.
+	 * \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)
+	{
+		return vector / abs(vector);
+	}
+}
+
+#endif