Rework Navigation Avoidance

Rework Navigation Avoidance.

10 files changed, 2834 insertions, 0 deletions

diff --git a/thirdparty/rvo2/rvo2_2d/Agent2d.cpp b/thirdparty/rvo2/rvo2_2d/Agent2d.cpp
new file mode 100644
index 0000000000..3ff95a4922
--- /dev/null
+++ b/thirdparty/rvo2/rvo2_2d/Agent2d.cpp
@@ -0,0 +1,594 @@
+/*
+ * Agent2d.cpp
+ * RVO2 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 "Agent2d.h"
+
+#include "KdTree2d.h"
+#include "Obstacle2d.h"
+
+namespace RVO2D {
+	Agent2D::Agent2D() : maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f), timeHorizonObst_(0.0f), id_(0) { }
+
+	void Agent2D::computeNeighbors(RVOSimulator2D *sim_)
+	{
+		obstacleNeighbors_.clear();
+		float rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_);
+		sim_->kdTree_->computeObstacleNeighbors(this, rangeSq);
+
+		agentNeighbors_.clear();
+
+		if (maxNeighbors_ > 0) {
+			rangeSq = sqr(neighborDist_);
+			sim_->kdTree_->computeAgentNeighbors(this, rangeSq);
+		}
+	}
+
+	/* Search for the best new velocity. */
+	void Agent2D::computeNewVelocity(RVOSimulator2D *sim_)
+	{
+		orcaLines_.clear();
+
+		const float invTimeHorizonObst = 1.0f / timeHorizonObst_;
+
+		/* Create obstacle ORCA lines. */
+		for (size_t i = 0; i < obstacleNeighbors_.size(); ++i) {
+
+			const Obstacle2D *obstacle1 = obstacleNeighbors_[i].second;
+			const Obstacle2D *obstacle2 = obstacle1->nextObstacle_;
+
+			const Vector2 relativePosition1 = obstacle1->point_ - position_;
+			const Vector2 relativePosition2 = obstacle2->point_ - position_;
+
+			/*
+			 * Check if velocity obstacle of obstacle is already taken care of by
+			 * previously constructed obstacle ORCA lines.
+			 */
+			bool alreadyCovered = false;
+
+			for (size_t j = 0; j < orcaLines_.size(); ++j) {
+				if (det(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON && det(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >=  -RVO_EPSILON) {
+					alreadyCovered = true;
+					break;
+				}
+			}
+
+			if (alreadyCovered) {
+				continue;
+			}
+
+			/* Not yet covered. Check for collisions. */
+
+			const float distSq1 = absSq(relativePosition1);
+			const float distSq2 = absSq(relativePosition2);
+
+			const float radiusSq = sqr(radius_);
+
+			const Vector2 obstacleVector = obstacle2->point_ - obstacle1->point_;
+			const float s = (-relativePosition1 * obstacleVector) / absSq(obstacleVector);
+			const float distSqLine = absSq(-relativePosition1 - s * obstacleVector);
+
+			Line line;
+
+			if (s < 0.0f && distSq1 <= radiusSq) {
+				/* Collision with left vertex. Ignore if non-convex. */
+				if (obstacle1->isConvex_) {
+					line.point = Vector2(0.0f, 0.0f);
+					line.direction = normalize(Vector2(-relativePosition1.y(), relativePosition1.x()));
+					orcaLines_.push_back(line);
+				}
+
+				continue;
+			}
+			else if (s > 1.0f && distSq2 <= radiusSq) {
+				/* Collision with right vertex. Ignore if non-convex
+				 * or if it will be taken care of by neighoring obstace */
+				if (obstacle2->isConvex_ && det(relativePosition2, obstacle2->unitDir_) >= 0.0f) {
+					line.point = Vector2(0.0f, 0.0f);
+					line.direction = normalize(Vector2(-relativePosition2.y(), relativePosition2.x()));
+					orcaLines_.push_back(line);
+				}
+
+				continue;
+			}
+			else if (s >= 0.0f && s < 1.0f && distSqLine <= radiusSq) {
+				/* Collision with obstacle segment. */
+				line.point = Vector2(0.0f, 0.0f);
+				line.direction = -obstacle1->unitDir_;
+				orcaLines_.push_back(line);
+				continue;
+			}
+
+			/*
+			 * No collision.
+			 * Compute legs. When obliquely viewed, both legs can come from a single
+			 * vertex. Legs extend cut-off line when nonconvex vertex.
+			 */
+
+			Vector2 leftLegDirection, rightLegDirection;
+
+			if (s < 0.0f && distSqLine <= radiusSq) {
+				/*
+				 * Obstacle viewed obliquely so that left vertex
+				 * defines velocity obstacle.
+				 */
+				if (!obstacle1->isConvex_) {
+					/* Ignore obstacle. */
+					continue;
+				}
+
+				obstacle2 = obstacle1;
+
+				const float leg1 = std::sqrt(distSq1 - radiusSq);
+				leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
+				rightLegDirection = Vector2(relativePosition1.x() * leg1 + relativePosition1.y() * radius_, -relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
+			}
+			else if (s > 1.0f && distSqLine <= radiusSq) {
+				/*
+				 * Obstacle viewed obliquely so that
+				 * right vertex defines velocity obstacle.
+				 */
+				if (!obstacle2->isConvex_) {
+					/* Ignore obstacle. */
+					continue;
+				}
+
+				obstacle1 = obstacle2;
+
+				const float leg2 = std::sqrt(distSq2 - radiusSq);
+				leftLegDirection = Vector2(relativePosition2.x() * leg2 - relativePosition2.y() * radius_, relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
+				rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
+			}
+			else {
+				/* Usual situation. */
+				if (obstacle1->isConvex_) {
+					const float leg1 = std::sqrt(distSq1 - radiusSq);
+					leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
+				}
+				else {
+					/* Left vertex non-convex; left leg extends cut-off line. */
+					leftLegDirection = -obstacle1->unitDir_;
+				}
+
+				if (obstacle2->isConvex_) {
+					const float leg2 = std::sqrt(distSq2 - radiusSq);
+					rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
+				}
+				else {
+					/* Right vertex non-convex; right leg extends cut-off line. */
+					rightLegDirection = obstacle1->unitDir_;
+				}
+			}
+
+			/*
+			 * Legs can never point into neighboring edge when convex vertex,
+			 * take cutoff-line of neighboring edge instead. If velocity projected on
+			 * "foreign" leg, no constraint is added.
+			 */
+
+			const Obstacle2D *const leftNeighbor = obstacle1->prevObstacle_;
+
+			bool isLeftLegForeign = false;
+			bool isRightLegForeign = false;
+
+			if (obstacle1->isConvex_ && det(leftLegDirection, -leftNeighbor->unitDir_) >= 0.0f) {
+				/* Left leg points into obstacle. */
+				leftLegDirection = -leftNeighbor->unitDir_;
+				isLeftLegForeign = true;
+			}
+
+			if (obstacle2->isConvex_ && det(rightLegDirection, obstacle2->unitDir_) <= 0.0f) {
+				/* Right leg points into obstacle. */
+				rightLegDirection = obstacle2->unitDir_;
+				isRightLegForeign = true;
+			}
+
+			/* Compute cut-off centers. */
+			const Vector2 leftCutoff = invTimeHorizonObst * (obstacle1->point_ - position_);
+			const Vector2 rightCutoff = invTimeHorizonObst * (obstacle2->point_ - position_);
+			const Vector2 cutoffVec = rightCutoff - leftCutoff;
+
+			/* Project current velocity on velocity obstacle. */
+
+			/* Check if current velocity is projected on cutoff circles. */
+			const float t = (obstacle1 == obstacle2 ? 0.5f : ((velocity_ - leftCutoff) * cutoffVec) / absSq(cutoffVec));
+			const float tLeft = ((velocity_ - leftCutoff) * leftLegDirection);
+			const float tRight = ((velocity_ - rightCutoff) * rightLegDirection);
+
+			if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f)) {
+				/* Project on left cut-off circle. */
+				const Vector2 unitW = normalize(velocity_ - leftCutoff);
+
+				line.direction = Vector2(unitW.y(), -unitW.x());
+				line.point = leftCutoff + radius_ * invTimeHorizonObst * unitW;
+				orcaLines_.push_back(line);
+				continue;
+			}
+			else if (t > 1.0f && tRight < 0.0f) {
+				/* Project on right cut-off circle. */
+				const Vector2 unitW = normalize(velocity_ - rightCutoff);
+
+				line.direction = Vector2(unitW.y(), -unitW.x());
+				line.point = rightCutoff + radius_ * invTimeHorizonObst * unitW;
+				orcaLines_.push_back(line);
+				continue;
+			}
+
+			/*
+			 * Project on left leg, right leg, or cut-off line, whichever is closest
+			 * to velocity.
+			 */
+			const float distSqCutoff = ((t < 0.0f || t > 1.0f || obstacle1 == obstacle2) ? std::numeric_limits<float>::infinity() : absSq(velocity_ - (leftCutoff + t * cutoffVec)));
+			const float distSqLeft = ((tLeft < 0.0f) ? std::numeric_limits<float>::infinity() : absSq(velocity_ - (leftCutoff + tLeft * leftLegDirection)));
+			const float distSqRight = ((tRight < 0.0f) ? std::numeric_limits<float>::infinity() : absSq(velocity_ - (rightCutoff + tRight * rightLegDirection)));
+
+			if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) {
+				/* Project on cut-off line. */
+				line.direction = -obstacle1->unitDir_;
+				line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x());
+				orcaLines_.push_back(line);
+				continue;
+			}
+			else if (distSqLeft <= distSqRight) {
+				/* Project on left leg. */
+				if (isLeftLegForeign) {
+					continue;
+				}
+
+				line.direction = leftLegDirection;
+				line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x());
+				orcaLines_.push_back(line);
+				continue;
+			}
+			else {
+				/* Project on right leg. */
+				if (isRightLegForeign) {
+					continue;
+				}
+
+				line.direction = -rightLegDirection;
+				line.point = rightCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x());
+				orcaLines_.push_back(line);
+				continue;
+			}
+		}
+
+		const size_t numObstLines = orcaLines_.size();
+
+		const float invTimeHorizon = 1.0f / timeHorizon_;
+
+		/* Create agent ORCA lines. */
+		for (size_t i = 0; i < agentNeighbors_.size(); ++i) {
+			const Agent2D *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 Vector2 relativePosition = other->position_ - position_;
+			const Vector2 relativeVelocity = velocity_ - other->velocity_;
+			const float distSq = absSq(relativePosition);
+			const float combinedRadius = radius_ + other->radius_;
+			const float combinedRadiusSq = sqr(combinedRadius);
+
+			Line line;
+			Vector2 u;
+
+			if (distSq > combinedRadiusSq) {
+				/* No collision. */
+				const Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;
+				/* Vector from cutoff center to relative velocity. */
+				const float wLengthSq = absSq(w);
+
+				const float dotProduct1 = w * relativePosition;
+
+				if (dotProduct1 < 0.0f && sqr(dotProduct1) > combinedRadiusSq * wLengthSq) {
+					/* Project on cut-off circle. */
+					const float wLength = std::sqrt(wLengthSq);
+					const Vector2 unitW = w / wLength;
+
+					line.direction = Vector2(unitW.y(), -unitW.x());
+					u = (combinedRadius * invTimeHorizon - wLength) * unitW;
+				}
+				else {
+					/* Project on legs. */
+					const float leg = std::sqrt(distSq - combinedRadiusSq);
+
+					if (det(relativePosition, w) > 0.0f) {
+						/* Project on left leg. */
+						line.direction = Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
+					}
+					else {
+						/* Project on right leg. */
+						line.direction = -Vector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
+					}
+
+					const float dotProduct2 = relativeVelocity * line.direction;
+
+					u = dotProduct2 * line.direction - relativeVelocity;
+				}
+			}
+			else {
+				/* Collision. Project on cut-off circle of time timeStep. */
+				const float invTimeStep = 1.0f / sim_->timeStep_;
+
+				/* Vector from cutoff center to relative velocity. */
+				const Vector2 w = relativeVelocity - invTimeStep * relativePosition;
+
+				const float wLength = abs(w);
+				const Vector2 unitW = w / wLength;
+
+				line.direction = Vector2(unitW.y(), -unitW.x());
+				u = (combinedRadius * invTimeStep - wLength) * unitW;
+			}
+
+			line.point = velocity_ + 0.5f * u;
+			orcaLines_.push_back(line);
+		}
+
+		size_t lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocity_);
+
+		if (lineFail < orcaLines_.size()) {
+			linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, newVelocity_);
+		}
+	}
+
+	void Agent2D::insertAgentNeighbor(const Agent2D *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;
+		}
+		// ignore other agent if this agent is below or above
+		if ((elevation_ > agent->elevation_ + agent->height_) || (elevation_ + height_ < agent->elevation_)) {
+			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 Agent2D::insertObstacleNeighbor(const Obstacle2D *obstacle, float rangeSq)
+	{
+		const Obstacle2D *const nextObstacle = obstacle->nextObstacle_;
+		
+		// ignore obstacle if no matching layer/mask
+		if ((avoidance_mask_ & nextObstacle->avoidance_layers_) == 0) {
+			return;
+		}
+		// ignore obstacle if below or above
+		if ((elevation_ > obstacle->elevation_ + obstacle->height_) || (elevation_ + height_ < obstacle->elevation_)) {
+			return;
+		}
+
+		const float distSq = distSqPointLineSegment(obstacle->point_, nextObstacle->point_, position_);
+
+		if (distSq < rangeSq) {
+			obstacleNeighbors_.push_back(std::make_pair(distSq, obstacle));
+
+			size_t i = obstacleNeighbors_.size() - 1;
+
+			while (i != 0 && distSq < obstacleNeighbors_[i - 1].first) {
+				obstacleNeighbors_[i] = obstacleNeighbors_[i - 1];
+				--i;
+			}
+
+			obstacleNeighbors_[i] = std::make_pair(distSq, obstacle);
+		}
+		//}
+	}
+
+	void Agent2D::update(RVOSimulator2D *sim_)
+	{
+		velocity_ = newVelocity_;
+		position_ += velocity_ * sim_->timeStep_;
+	}
+
+	bool linearProgram1(const std::vector<Line> &lines, size_t lineNo, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result)
+	{
+		const float dotProduct = lines[lineNo].point * lines[lineNo].direction;
+		const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(lines[lineNo].point);
+
+		if (discriminant < 0.0f) {
+			/* Max speed circle fully invalidates line lineNo. */
+			return false;
+		}
+
+		const float sqrtDiscriminant = std::sqrt(discriminant);
+		float tLeft = -dotProduct - sqrtDiscriminant;
+		float tRight = -dotProduct + sqrtDiscriminant;
+
+		for (size_t i = 0; i < lineNo; ++i) {
+			const float denominator = det(lines[lineNo].direction, lines[i].direction);
+			const float numerator = det(lines[i].direction, lines[lineNo].point - lines[i].point);
+
+			if (std::fabs(denominator) <= RVO_EPSILON) {
+				/* Lines lineNo and i are (almost) parallel. */
+				if (numerator < 0.0f) {
+					return false;
+				}
+				else {
+					continue;
+				}
+			}
+
+			const float t = numerator / denominator;
+
+			if (denominator >= 0.0f) {
+				/* Line i bounds line lineNo on the right. */
+				tRight = std::min(tRight, t);
+			}
+			else {
+				/* Line i bounds line lineNo on the left. */
+				tLeft = std::max(tLeft, t);
+			}
+
+			if (tLeft > tRight) {
+				return false;
+			}
+		}
+
+		if (directionOpt) {
+			/* Optimize direction. */
+			if (optVelocity * lines[lineNo].direction > 0.0f) {
+				/* Take right extreme. */
+				result = lines[lineNo].point + tRight * lines[lineNo].direction;
+			}
+			else {
+				/* Take left extreme. */
+				result = lines[lineNo].point + tLeft * lines[lineNo].direction;
+			}
+		}
+		else {
+			/* Optimize closest point. */
+			const float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point);
+
+			if (t < tLeft) {
+				result = lines[lineNo].point + tLeft * lines[lineNo].direction;
+			}
+			else if (t > tRight) {
+				result = lines[lineNo].point + tRight * lines[lineNo].direction;
+			}
+			else {
+				result = lines[lineNo].point + t * lines[lineNo].direction;
+			}
+		}
+
+		return true;
+	}
+
+	size_t linearProgram2(const std::vector<Line> &lines, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &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 < lines.size(); ++i) {
+			if (det(lines[i].direction, lines[i].point - result) > 0.0f) {
+				/* Result does not satisfy constraint i. Compute new optimal result. */
+				const Vector2 tempResult = result;
+
+				if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) {
+					result = tempResult;
+					return i;
+				}
+			}
+		}
+
+		return lines.size();
+	}
+
+	void linearProgram3(const std::vector<Line> &lines, size_t numObstLines, size_t beginLine, float radius, Vector2 &result)
+	{
+		float distance = 0.0f;
+
+		for (size_t i = beginLine; i < lines.size(); ++i) {
+			if (det(lines[i].direction, lines[i].point - result) > distance) {
+				/* Result does not satisfy constraint of line i. */
+				std::vector<Line> projLines(lines.begin(), lines.begin() + static_cast<ptrdiff_t>(numObstLines));
+
+				for (size_t j = numObstLines; j < i; ++j) {
+					Line line;
+
+					float determinant = det(lines[i].direction, lines[j].direction);
+
+					if (std::fabs(determinant) <= RVO_EPSILON) {
+						/* Line i and line j are parallel. */
+						if (lines[i].direction * lines[j].direction > 0.0f) {
+							/* Line i and line j point in the same direction. */
+							continue;
+						}
+						else {
+							/* Line i and line j point in opposite direction. */
+							line.point = 0.5f * (lines[i].point + lines[j].point);
+						}
+					}
+					else {
+						line.point = lines[i].point + (det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction;
+					}
+
+					line.direction = normalize(lines[j].direction - lines[i].direction);
+					projLines.push_back(line);
+				}
+
+				const Vector2 tempResult = result;
+
+				if (linearProgram2(projLines, radius, Vector2(-lines[i].direction.y(), lines[i].direction.x()), true, result) < projLines.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 = det(lines[i].direction, lines[i].point - result);
+			}
+		}
+	}
+}
diff --git a/thirdparty/rvo2/rvo2_2d/Agent2d.h b/thirdparty/rvo2/rvo2_2d/Agent2d.h
new file mode 100644
index 0000000000..c666c2de7b
--- /dev/null
+++ b/thirdparty/rvo2/rvo2_2d/Agent2d.h
@@ -0,0 +1,160 @@
+/*
+ * Agent2d.h
+ * RVO2 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/>
+ */
+
+#ifndef RVO2D_AGENT_H_
+#define RVO2D_AGENT_H_
+
+/**
+ * \file       Agent2d.h
+ * \brief      Contains the Agent class.
+ */
+
+#include "Definitions.h"
+#include "RVOSimulator2d.h"
+
+namespace RVO2D {
+	/**
+	 * \brief      Defines an agent in the simulation.
+	 */
+	class Agent2D {
+	public:
+		/**
+		 * \brief      Constructs an agent instance.
+		 * \param      sim             The simulator instance.
+		 */
+		explicit Agent2D();
+
+		/**
+		 * \brief      Computes the neighbors of this agent.
+		 */
+		void computeNeighbors(RVOSimulator2D *sim_);
+
+		/**
+		 * \brief      Computes the new velocity of this agent.
+		 */
+		void computeNewVelocity(RVOSimulator2D *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 Agent2D *agent, float &rangeSq);
+
+		/**
+		 * \brief      Inserts a static obstacle neighbor into the set of neighbors
+		 *             of this agent.
+		 * \param      obstacle        The number of the static obstacle to be
+		 *                             inserted.
+		 * \param      rangeSq         The squared range around this agent.
+		 */
+		void insertObstacleNeighbor(const Obstacle2D *obstacle, float rangeSq);
+
+		/**
+		 * \brief      Updates the two-dimensional position and two-dimensional
+		 *             velocity of this agent.
+		 */
+		void update(RVOSimulator2D *sim_);
+
+		std::vector<std::pair<float, const Agent2D *> > agentNeighbors_;
+		size_t maxNeighbors_;
+		float maxSpeed_;
+		float neighborDist_;
+		Vector2 newVelocity_;
+		std::vector<std::pair<float, const Obstacle2D *> > obstacleNeighbors_;
+		std::vector<Line> orcaLines_;
+		Vector2 position_;
+		Vector2 prefVelocity_;
+		float radius_;
+		float timeHorizon_;
+		float timeHorizonObst_;
+		Vector2 velocity_;
+		float height_ = 0.0;
+		float elevation_ = 0.0;
+		uint32_t avoidance_layers_ = 1;
+		uint32_t avoidance_mask_ = 1;
+		float avoidance_priority_ = 1.0;
+
+		size_t id_;
+
+		friend class KdTree2D;
+		friend class RVOSimulator2D;
+	};
+
+	/**
+	 * \relates    Agent
+	 * \brief      Solves a one-dimensional linear program on a specified line
+	 *             subject to linear constraints defined by lines and a circular
+	 *             constraint.
+	 * \param      lines         Lines defining the linear constraints.
+	 * \param      lineNo        The specified line constraint.
+	 * \param      radius        The radius of the circular 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<Line> &lines, size_t lineNo,
+						float radius, const Vector2 &optVelocity,
+						bool directionOpt, Vector2 &result);
+
+	/**
+	 * \relates    Agent
+	 * \brief      Solves a two-dimensional linear program subject to linear
+	 *             constraints defined by lines and a circular constraint.
+	 * \param      lines         Lines defining the linear constraints.
+	 * \param      radius        The radius of the circular 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 line it fails on, and the number of lines if successful.
+	 */
+	size_t linearProgram2(const std::vector<Line> &lines, float radius,
+						  const Vector2 &optVelocity, bool directionOpt,
+						  Vector2 &result);
+
+	/**
+	 * \relates    Agent
+	 * \brief      Solves a two-dimensional linear program subject to linear
+	 *             constraints defined by lines and a circular constraint.
+	 * \param      lines         Lines defining the linear constraints.
+	 * \param      numObstLines  Count of obstacle lines.
+	 * \param      beginLine     The line on which the 2-d linear program failed.
+	 * \param      radius        The radius of the circular constraint.
+	 * \param      result        A reference to the result of the linear program.
+	 */
+	void linearProgram3(const std::vector<Line> &lines, size_t numObstLines, size_t beginLine,
+						float radius, Vector2 &result);
+}
+
+#endif /* RVO2D_AGENT_H_ */
diff --git a/thirdparty/rvo2/rvo2_2d/Definitions.h b/thirdparty/rvo2/rvo2_2d/Definitions.h
new file mode 100644
index 0000000000..4b986fc991
--- /dev/null
+++ b/thirdparty/rvo2/rvo2_2d/Definitions.h
@@ -0,0 +1,109 @@
+/*
+ * Definitions.h
+ * RVO2 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/>
+ */
+
+#ifndef RVO2D_DEFINITIONS_H_
+#define RVO2D_DEFINITIONS_H_
+
+/**
+ * \file       Definitions.h
+ * \brief      Contains functions and constants used in multiple classes.
+ */
+
+#include <algorithm>
+#include <cmath>
+#include <cstddef>
+#include <limits>
+#include <vector>
+
+#include "Vector2.h"
+
+/**
+ * \brief       A sufficiently small positive number.
+ */
+const float RVO_EPSILON = 0.00001f;
+
+namespace RVO2D {
+	class Agent2D;
+	class Obstacle2D;
+	class RVOSimulator2D;
+
+	/**
+	 * \brief      Computes the squared distance from a line segment with the
+	 *             specified endpoints to a specified point.
+	 * \param      a               The first endpoint of the line segment.
+	 * \param      b               The second endpoint of the line segment.
+	 * \param      c               The point to which the squared distance is to
+	 *                             be calculated.
+	 * \return     The squared distance from the line segment to the point.
+	 */
+	inline float distSqPointLineSegment(const Vector2 &a, const Vector2 &b,
+										const Vector2 &c)
+	{
+		const float r = ((c - a) * (b - a)) / absSq(b - a);
+
+		if (r < 0.0f) {
+			return absSq(c - a);
+		}
+		else if (r > 1.0f) {
+			return absSq(c - b);
+		}
+		else {
+			return absSq(c - (a + r * (b - a)));
+		}
+	}
+
+	/**
+	 * \brief      Computes the signed distance from a line connecting the
+	 *             specified points to a specified point.
+	 * \param      a               The first point on the line.
+	 * \param      b               The second point on the line.
+	 * \param      c               The point to which the signed distance is to
+	 *                             be calculated.
+	 * \return     Positive when the point c lies to the left of the line ab.
+	 */
+	inline float leftOf(const Vector2 &a, const Vector2 &b, const Vector2 &c)
+	{
+		return det(a - c, b - a);
+	}
+
+	/**
+	 * \brief      Computes the square of a float.
+	 * \param      a               The float to be squared.
+	 * \return     The square of the float.
+	 */
+	inline float sqr(float a)
+	{
+		return a * a;
+	}
+}
+
+#endif /* RVO2D_DEFINITIONS_H_ */
diff --git a/thirdparty/rvo2/rvo2_2d/KdTree2d.cpp b/thirdparty/rvo2/rvo2_2d/KdTree2d.cpp
new file mode 100644
index 0000000000..184bc74fe2
--- /dev/null
+++ b/thirdparty/rvo2/rvo2_2d/KdTree2d.cpp
@@ -0,0 +1,357 @@
+/*
+ * KdTree2d.cpp
+ * RVO2 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 "KdTree2d.h"
+
+#include "Agent2d.h"
+#include "RVOSimulator2d.h"
+#include "Obstacle2d.h"
+
+namespace RVO2D {
+	KdTree2D::KdTree2D(RVOSimulator2D *sim) : obstacleTree_(NULL), sim_(sim) { }
+
+	KdTree2D::~KdTree2D()
+	{
+		deleteObstacleTree(obstacleTree_);
+	}
+
+	void KdTree2D::buildAgentTree(std::vector<Agent2D *> agents)
+	{
+		agents_.swap(agents);
+
+		if (!agents_.empty()) {
+			agentTree_.resize(2 * agents_.size() - 1);
+			buildAgentTreeRecursive(0, agents_.size(), 0);
+		}
+	}
+
+	void KdTree2D::buildAgentTreeRecursive(size_t begin, size_t end, size_t node)
+	{
+		agentTree_[node].begin = begin;
+		agentTree_[node].end = end;
+		agentTree_[node].minX = agentTree_[node].maxX = agents_[begin]->position_.x();
+		agentTree_[node].minY = agentTree_[node].maxY = agents_[begin]->position_.y();
+
+		for (size_t i = begin + 1; i < end; ++i) {
+			agentTree_[node].maxX = std::max(agentTree_[node].maxX, agents_[i]->position_.x());
+			agentTree_[node].minX = std::min(agentTree_[node].minX, agents_[i]->position_.x());
+			agentTree_[node].maxY = std::max(agentTree_[node].maxY, agents_[i]->position_.y());
+			agentTree_[node].minY = std::min(agentTree_[node].minY, agents_[i]->position_.y());
+		}
+
+		if (end - begin > MAX_LEAF_SIZE) {
+			/* No leaf node. */
+			const bool isVertical = (agentTree_[node].maxX - agentTree_[node].minX > agentTree_[node].maxY - agentTree_[node].minY);
+			const float splitValue = (isVertical ? 0.5f * (agentTree_[node].maxX + agentTree_[node].minX) : 0.5f * (agentTree_[node].maxY + agentTree_[node].minY));
+
+			size_t left = begin;
+			size_t right = end;
+
+			while (left < right) {
+				while (left < right && (isVertical ? agents_[left]->position_.x() : agents_[left]->position_.y()) < splitValue) {
+					++left;
+				}
+
+				while (right > left && (isVertical ? agents_[right - 1]->position_.x() : agents_[right - 1]->position_.y()) >= splitValue) {
+					--right;
+				}
+
+				if (left < right) {
+					std::swap(agents_[left], agents_[right - 1]);
+					++left;
+					--right;
+				}
+			}
+
+			if (left == begin) {
+				++left;
+				++right;
+			}
+
+			agentTree_[node].left = node + 1;
+			agentTree_[node].right = node + 2 * (left - begin);
+
+			buildAgentTreeRecursive(begin, left, agentTree_[node].left);
+			buildAgentTreeRecursive(left, end, agentTree_[node].right);
+		}
+	}
+
+	void KdTree2D::buildObstacleTree(std::vector<Obstacle2D *> obstacles)
+	{
+		deleteObstacleTree(obstacleTree_);
+
+		obstacleTree_ = buildObstacleTreeRecursive(obstacles);
+	}
+
+
+	KdTree2D::ObstacleTreeNode *KdTree2D::buildObstacleTreeRecursive(const std::vector<Obstacle2D *> &obstacles)
+	{
+		if (obstacles.empty()) {
+			return NULL;
+		}
+		else {
+			ObstacleTreeNode *const node = new ObstacleTreeNode;
+
+			size_t optimalSplit = 0;
+			size_t minLeft = obstacles.size();
+			size_t minRight = obstacles.size();
+
+			for (size_t i = 0; i < obstacles.size(); ++i) {
+				size_t leftSize = 0;
+				size_t rightSize = 0;
+
+				const Obstacle2D *const obstacleI1 = obstacles[i];
+				const Obstacle2D *const obstacleI2 = obstacleI1->nextObstacle_;
+
+				/* Compute optimal split node. */
+				for (size_t j = 0; j < obstacles.size(); ++j) {
+					if (i == j) {
+						continue;
+					}
+
+					const Obstacle2D *const obstacleJ1 = obstacles[j];
+					const Obstacle2D *const obstacleJ2 = obstacleJ1->nextObstacle_;
+
+					const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_);
+					const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_);
+
+					if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) {
+						++leftSize;
+					}
+					else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) {
+						++rightSize;
+					}
+					else {
+						++leftSize;
+						++rightSize;
+					}
+
+					if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) >= std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) {
+						break;
+					}
+				}
+
+				if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) < std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) {
+					minLeft = leftSize;
+					minRight = rightSize;
+					optimalSplit = i;
+				}
+			}
+
+			/* Build split node. */
+			std::vector<Obstacle2D *> leftObstacles(minLeft);
+			std::vector<Obstacle2D *> rightObstacles(minRight);
+
+			size_t leftCounter = 0;
+			size_t rightCounter = 0;
+			const size_t i = optimalSplit;
+
+			const Obstacle2D *const obstacleI1 = obstacles[i];
+			const Obstacle2D *const obstacleI2 = obstacleI1->nextObstacle_;
+
+			for (size_t j = 0; j < obstacles.size(); ++j) {
+				if (i == j) {
+					continue;
+				}
+
+				Obstacle2D *const obstacleJ1 = obstacles[j];
+				Obstacle2D *const obstacleJ2 = obstacleJ1->nextObstacle_;
+
+				const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_);
+				const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_);
+
+				if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) {
+					leftObstacles[leftCounter++] = obstacles[j];
+				}
+				else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) {
+					rightObstacles[rightCounter++] = obstacles[j];
+				}
+				else {
+					/* Split obstacle j. */
+					const float t = det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleI1->point_) / det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleJ2->point_);
+
+					const Vector2 splitpoint = obstacleJ1->point_ + t * (obstacleJ2->point_ - obstacleJ1->point_);
+
+					Obstacle2D *const newObstacle = new Obstacle2D();
+					newObstacle->point_ = splitpoint;
+					newObstacle->prevObstacle_ = obstacleJ1;
+					newObstacle->nextObstacle_ = obstacleJ2;
+					newObstacle->isConvex_ = true;
+					newObstacle->unitDir_ = obstacleJ1->unitDir_;
+
+					newObstacle->id_ = sim_->obstacles_.size();
+
+					sim_->obstacles_.push_back(newObstacle);
+
+					obstacleJ1->nextObstacle_ = newObstacle;
+					obstacleJ2->prevObstacle_ = newObstacle;
+
+					if (j1LeftOfI > 0.0f) {
+						leftObstacles[leftCounter++] = obstacleJ1;
+						rightObstacles[rightCounter++] = newObstacle;
+					}
+					else {
+						rightObstacles[rightCounter++] = obstacleJ1;
+						leftObstacles[leftCounter++] = newObstacle;
+					}
+				}
+			}
+
+			node->obstacle = obstacleI1;
+			node->left = buildObstacleTreeRecursive(leftObstacles);
+			node->right = buildObstacleTreeRecursive(rightObstacles);
+			return node;
+		}
+	}
+
+	void KdTree2D::computeAgentNeighbors(Agent2D *agent, float &rangeSq) const
+	{
+		queryAgentTreeRecursive(agent, rangeSq, 0);
+	}
+
+	void KdTree2D::computeObstacleNeighbors(Agent2D *agent, float rangeSq) const
+	{
+		queryObstacleTreeRecursive(agent, rangeSq, obstacleTree_);
+	}
+
+	void KdTree2D::deleteObstacleTree(ObstacleTreeNode *node)
+	{
+		if (node != NULL) {
+			deleteObstacleTree(node->left);
+			deleteObstacleTree(node->right);
+			delete node;
+		}
+	}
+
+	void KdTree2D::queryAgentTreeRecursive(Agent2D *agent, float &rangeSq, size_t node) const
+	{
+		if (agentTree_[node].end - agentTree_[node].begin <= 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].minX - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].left].maxX)) + sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minY - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].left].maxY));
+
+			const float distSqRight = sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minX - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].right].maxX)) + sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minY - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].right].maxY));
+
+			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);
+					}
+				}
+			}
+
+		}
+	}
+
+	void KdTree2D::queryObstacleTreeRecursive(Agent2D *agent, float rangeSq, const ObstacleTreeNode *node) const
+	{
+		if (node == NULL) {
+			return;
+		}
+		else {
+			const Obstacle2D *const obstacle1 = node->obstacle;
+			const Obstacle2D *const obstacle2 = obstacle1->nextObstacle_;
+
+			const float agentLeftOfLine = leftOf(obstacle1->point_, obstacle2->point_, agent->position_);
+
+			queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->left : node->right));
+
+			const float distSqLine = sqr(agentLeftOfLine) / absSq(obstacle2->point_ - obstacle1->point_);
+
+			if (distSqLine < rangeSq) {
+				if (agentLeftOfLine < 0.0f) {
+					/*
+					 * Try obstacle at this node only if agent is on right side of
+					 * obstacle (and can see obstacle).
+					 */
+					agent->insertObstacleNeighbor(node->obstacle, rangeSq);
+				}
+
+				/* Try other side of line. */
+				queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->right : node->left));
+
+			}
+		}
+	}
+
+	bool KdTree2D::queryVisibility(const Vector2 &q1, const Vector2 &q2, float radius) const
+	{
+		return queryVisibilityRecursive(q1, q2, radius, obstacleTree_);
+	}
+
+	bool KdTree2D::queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2, float radius, const ObstacleTreeNode *node) const
+	{
+		if (node == NULL) {
+			return true;
+		}
+		else {
+			const Obstacle2D *const obstacle1 = node->obstacle;
+			const Obstacle2D *const obstacle2 = obstacle1->nextObstacle_;
+
+			const float q1LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q1);
+			const float q2LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q2);
+			const float invLengthI = 1.0f / absSq(obstacle2->point_ - obstacle1->point_);
+
+			if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f) {
+				return queryVisibilityRecursive(q1, q2, radius, node->left) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->right));
+			}
+			else if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f) {
+				return queryVisibilityRecursive(q1, q2, radius, node->right) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->left));
+			}
+			else if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f) {
+				/* One can see through obstacle from left to right. */
+				return queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right);
+			}
+			else {
+				const float point1LeftOfQ = leftOf(q1, q2, obstacle1->point_);
+				const float point2LeftOfQ = leftOf(q1, q2, obstacle2->point_);
+				const float invLengthQ = 1.0f / absSq(q2 - q1);
+
+				return (point1LeftOfQ * point2LeftOfQ >= 0.0f && sqr(point1LeftOfQ) * invLengthQ > sqr(radius) && sqr(point2LeftOfQ) * invLengthQ > sqr(radius) && queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right));
+			}
+		}
+	}
+}
diff --git a/thirdparty/rvo2/rvo2_2d/KdTree2d.h b/thirdparty/rvo2/rvo2_2d/KdTree2d.h
new file mode 100644
index 0000000000..c7159eab97
--- /dev/null
+++ b/thirdparty/rvo2/rvo2_2d/KdTree2d.h
@@ -0,0 +1,203 @@
+/*
+ * KdTree2d.h
+ * RVO2 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/>
+ */
+
+#ifndef RVO2D_KD_TREE_H_
+#define RVO2D_KD_TREE_H_
+
+/**
+ * \file       KdTree2d.h
+ * \brief      Contains the KdTree class.
+ */
+
+#include "Definitions.h"
+
+namespace RVO2D {
+	/**
+	 * \brief      Defines <i>k</i>d-trees for agents and static obstacles in the
+	 *             simulation.
+	 */
+	class KdTree2D {
+	public:
+		/**
+		 * \brief      Defines an agent <i>k</i>d-tree node.
+		 */
+		class AgentTreeNode {
+		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 maximum x-coordinate.
+			 */
+			float maxX;
+
+			/**
+			 * \brief      The maximum y-coordinate.
+			 */
+			float maxY;
+
+			/**
+			 * \brief      The minimum x-coordinate.
+			 */
+			float minX;
+
+			/**
+			 * \brief      The minimum y-coordinate.
+			 */
+			float minY;
+
+			/**
+			 * \brief      The right node number.
+			 */
+			size_t right;
+		};
+
+		/**
+		 * \brief      Defines an obstacle <i>k</i>d-tree node.
+		 */
+		class ObstacleTreeNode {
+		public:
+			/**
+			 * \brief      The left obstacle tree node.
+			 */
+			ObstacleTreeNode *left;
+
+			/**
+			 * \brief      The obstacle number.
+			 */
+			const Obstacle2D *obstacle;
+
+			/**
+			 * \brief      The right obstacle tree node.
+			 */
+			ObstacleTreeNode *right;
+		};
+
+		/**
+		 * \brief      Constructs a <i>k</i>d-tree instance.
+		 * \param      sim             The simulator instance.
+		 */
+		explicit KdTree2D(RVOSimulator2D *sim);
+
+		/**
+		 * \brief      Destroys this kd-tree instance.
+		 */
+		~KdTree2D();
+
+		/**
+		 * \brief      Builds an agent <i>k</i>d-tree.
+		 */
+		void buildAgentTree(std::vector<Agent2D *> agents);
+
+		void buildAgentTreeRecursive(size_t begin, size_t end, size_t node);
+
+		/**
+		 * \brief      Builds an obstacle <i>k</i>d-tree.
+		 */
+		void buildObstacleTree(std::vector<Obstacle2D *> obstacles);
+
+		ObstacleTreeNode *buildObstacleTreeRecursive(const std::vector<Obstacle2D *> &
+													 obstacles);
+
+		/**
+		 * \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(Agent2D *agent, float &rangeSq) const;
+
+		/**
+		 * \brief      Computes the obstacle neighbors of the specified agent.
+		 * \param      agent           A pointer to the agent for which obstacle
+		 *                             neighbors are to be computed.
+		 * \param      rangeSq         The squared range around the agent.
+		 */
+		void computeObstacleNeighbors(Agent2D *agent, float rangeSq) const;
+
+		/**
+		 * \brief      Deletes the specified obstacle tree node.
+		 * \param      node            A pointer to the obstacle tree node to be
+		 *                             deleted.
+		 */
+		void deleteObstacleTree(ObstacleTreeNode *node);
+
+		void queryAgentTreeRecursive(Agent2D *agent, float &rangeSq,
+									 size_t node) const;
+
+		void queryObstacleTreeRecursive(Agent2D *agent, float rangeSq,
+										const ObstacleTreeNode *node) const;
+
+		/**
+		 * \brief      Queries the visibility between two points within a
+		 *             specified radius.
+		 * \param      q1              The first point between which visibility is
+		 *                             to be tested.
+		 * \param      q2              The second point between which visibility is
+		 *                             to be tested.
+		 * \param      radius          The radius within which visibility is to be
+		 *                             tested.
+		 * \return     True if q1 and q2 are mutually visible within the radius;
+		 *             false otherwise.
+		 */
+		bool queryVisibility(const Vector2 &q1, const Vector2 &q2,
+							 float radius) const;
+
+		bool queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2,
+									  float radius,
+									  const ObstacleTreeNode *node) const;
+
+		std::vector<Agent2D *> agents_;
+		std::vector<AgentTreeNode> agentTree_;
+		ObstacleTreeNode *obstacleTree_;
+		RVOSimulator2D *sim_;
+
+		static const size_t MAX_LEAF_SIZE = 10;
+
+		friend class Agent2D;
+		friend class RVOSimulator2D;
+	};
+}
+
+#endif /* RVO2D_KD_TREE_H_ */
diff --git a/thirdparty/rvo2/rvo2_2d/Obstacle2d.cpp b/thirdparty/rvo2/rvo2_2d/Obstacle2d.cpp
new file mode 100644
index 0000000000..a80c8af136
--- /dev/null
+++ b/thirdparty/rvo2/rvo2_2d/Obstacle2d.cpp
@@ -0,0 +1,38 @@
+/*
+ * Obstacle2d.cpp
+ * RVO2 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 "Obstacle2d.h"
+#include "RVOSimulator2d.h"
+
+namespace RVO2D {
+	Obstacle2D::Obstacle2D() : isConvex_(false), nextObstacle_(NULL), prevObstacle_(NULL), id_(0) { }
+}
diff --git a/thirdparty/rvo2/rvo2_2d/Obstacle2d.h b/thirdparty/rvo2/rvo2_2d/Obstacle2d.h
new file mode 100644
index 0000000000..9ba5937053
--- /dev/null
+++ b/thirdparty/rvo2/rvo2_2d/Obstacle2d.h
@@ -0,0 +1,72 @@
+/*
+ * Obstacle2d.h
+ * RVO2 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/>
+ */
+
+#ifndef RVO2D_OBSTACLE_H_
+#define RVO2D_OBSTACLE_H_
+
+/**
+ * \file       Obstacle2d.h
+ * \brief      Contains the Obstacle class.
+ */
+
+#include "Definitions.h"
+
+namespace RVO2D {
+	/**
+	 * \brief      Defines static obstacles in the simulation.
+	 */
+	class Obstacle2D {
+	public:
+		/**
+		 * \brief      Constructs a static obstacle instance.
+		 */
+		Obstacle2D();
+
+		bool isConvex_;
+		Obstacle2D *nextObstacle_;
+		Vector2 point_;
+		Obstacle2D *prevObstacle_;
+		Vector2 unitDir_;
+
+		float height_ = 1.0;
+		float elevation_ = 0.0;
+		uint32_t avoidance_layers_ = 1;
+
+		size_t id_;
+
+		friend class Agent2D;
+		friend class KdTree2D;
+		friend class RVOSimulator2D;
+	};
+}
+
+#endif /* RVO2D_OBSTACLE_H_ */
diff --git a/thirdparty/rvo2/rvo2_2d/RVOSimulator2d.cpp b/thirdparty/rvo2/rvo2_2d/RVOSimulator2d.cpp
new file mode 100644
index 0000000000..9fb1555ebc
--- /dev/null
+++ b/thirdparty/rvo2/rvo2_2d/RVOSimulator2d.cpp
@@ -0,0 +1,363 @@
+/*
+ * RVOSimulator2d.cpp
+ * RVO2 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 "RVOSimulator2d.h"
+
+#include "Agent2d.h"
+#include "KdTree2d.h"
+#include "Obstacle2d.h"
+
+#ifdef _OPENMP
+#include <omp.h>
+#endif
+
+namespace RVO2D {
+	RVOSimulator2D::RVOSimulator2D() : defaultAgent_(NULL), globalTime_(0.0f), kdTree_(NULL), timeStep_(0.0f)
+	{
+		kdTree_ = new KdTree2D(this);
+	}
+
+	RVOSimulator2D::RVOSimulator2D(float timeStep, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity) : defaultAgent_(NULL), globalTime_(0.0f), kdTree_(NULL), timeStep_(timeStep)
+	{
+		kdTree_ = new KdTree2D(this);
+		defaultAgent_ = new Agent2D();
+
+		defaultAgent_->maxNeighbors_ = maxNeighbors;
+		defaultAgent_->maxSpeed_ = maxSpeed;
+		defaultAgent_->neighborDist_ = neighborDist;
+		defaultAgent_->radius_ = radius;
+		defaultAgent_->timeHorizon_ = timeHorizon;
+		defaultAgent_->timeHorizonObst_ = timeHorizonObst;
+		defaultAgent_->velocity_ = velocity;
+	}
+
+	RVOSimulator2D::~RVOSimulator2D()
+	{
+		if (defaultAgent_ != NULL) {
+			delete defaultAgent_;
+		}
+
+		for (size_t i = 0; i < agents_.size(); ++i) {
+			delete agents_[i];
+		}
+
+		for (size_t i = 0; i < obstacles_.size(); ++i) {
+			delete obstacles_[i];
+		}
+
+		delete kdTree_;
+	}
+
+	size_t RVOSimulator2D::addAgent(const Vector2 &position)
+	{
+		if (defaultAgent_ == NULL) {
+			return RVO2D_ERROR;
+		}
+
+		Agent2D *agent = new Agent2D();
+
+		agent->position_ = position;
+		agent->maxNeighbors_ = defaultAgent_->maxNeighbors_;
+		agent->maxSpeed_ = defaultAgent_->maxSpeed_;
+		agent->neighborDist_ = defaultAgent_->neighborDist_;
+		agent->radius_ = defaultAgent_->radius_;
+		agent->timeHorizon_ = defaultAgent_->timeHorizon_;
+		agent->timeHorizonObst_ = defaultAgent_->timeHorizonObst_;
+		agent->velocity_ = defaultAgent_->velocity_;
+
+		agent->id_ = agents_.size();
+
+		agents_.push_back(agent);
+
+		return agents_.size() - 1;
+	}
+
+	size_t RVOSimulator2D::addAgent(const Vector2 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity)
+	{
+		Agent2D *agent = new Agent2D();
+
+		agent->position_ = position;
+		agent->maxNeighbors_ = maxNeighbors;
+		agent->maxSpeed_ = maxSpeed;
+		agent->neighborDist_ = neighborDist;
+		agent->radius_ = radius;
+		agent->timeHorizon_ = timeHorizon;
+		agent->timeHorizonObst_ = timeHorizonObst;
+		agent->velocity_ = velocity;
+
+		agent->id_ = agents_.size();
+
+		agents_.push_back(agent);
+
+		return agents_.size() - 1;
+	}
+
+	size_t RVOSimulator2D::addObstacle(const std::vector<Vector2> &vertices)
+	{
+		if (vertices.size() < 2) {
+			return RVO2D_ERROR;
+		}
+
+		const size_t obstacleNo = obstacles_.size();
+
+		for (size_t i = 0; i < vertices.size(); ++i) {
+			Obstacle2D *obstacle = new Obstacle2D();
+			obstacle->point_ = vertices[i];
+
+			if (i != 0) {
+				obstacle->prevObstacle_ = obstacles_.back();
+				obstacle->prevObstacle_->nextObstacle_ = obstacle;
+			}
+
+			if (i == vertices.size() - 1) {
+				obstacle->nextObstacle_ = obstacles_[obstacleNo];
+				obstacle->nextObstacle_->prevObstacle_ = obstacle;
+			}
+
+			obstacle->unitDir_ = normalize(vertices[(i == vertices.size() - 1 ? 0 : i + 1)] - vertices[i]);
+
+			if (vertices.size() == 2) {
+				obstacle->isConvex_ = true;
+			}
+			else {
+				obstacle->isConvex_ = (leftOf(vertices[(i == 0 ? vertices.size() - 1 : i - 1)], vertices[i], vertices[(i == vertices.size() - 1 ? 0 : i + 1)]) >= 0.0f);
+			}
+
+			obstacle->id_ = obstacles_.size();
+
+			obstacles_.push_back(obstacle);
+		}
+
+		return obstacleNo;
+	}
+
+	void RVOSimulator2D::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 RVOSimulator2D::getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const
+	{
+		return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_;
+	}
+
+	size_t RVOSimulator2D::getAgentMaxNeighbors(size_t agentNo) const
+	{
+		return agents_[agentNo]->maxNeighbors_;
+	}
+
+	float RVOSimulator2D::getAgentMaxSpeed(size_t agentNo) const
+	{
+		return agents_[agentNo]->maxSpeed_;
+	}
+
+	float RVOSimulator2D::getAgentNeighborDist(size_t agentNo) const
+	{
+		return agents_[agentNo]->neighborDist_;
+	}
+
+	size_t RVOSimulator2D::getAgentNumAgentNeighbors(size_t agentNo) const
+	{
+		return agents_[agentNo]->agentNeighbors_.size();
+	}
+
+	size_t RVOSimulator2D::getAgentNumObstacleNeighbors(size_t agentNo) const
+	{
+		return agents_[agentNo]->obstacleNeighbors_.size();
+	}
+
+	size_t RVOSimulator2D::getAgentNumORCALines(size_t agentNo) const
+	{
+		return agents_[agentNo]->orcaLines_.size();
+	}
+
+	size_t RVOSimulator2D::getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const
+	{
+		return agents_[agentNo]->obstacleNeighbors_[neighborNo].second->id_;
+	}
+
+	const Line &RVOSimulator2D::getAgentORCALine(size_t agentNo, size_t lineNo) const
+	{
+		return agents_[agentNo]->orcaLines_[lineNo];
+	}
+
+	const Vector2 &RVOSimulator2D::getAgentPosition(size_t agentNo) const
+	{
+		return agents_[agentNo]->position_;
+	}
+
+	const Vector2 &RVOSimulator2D::getAgentPrefVelocity(size_t agentNo) const
+	{
+		return agents_[agentNo]->prefVelocity_;
+	}
+
+	float RVOSimulator2D::getAgentRadius(size_t agentNo) const
+	{
+		return agents_[agentNo]->radius_;
+	}
+
+	float RVOSimulator2D::getAgentTimeHorizon(size_t agentNo) const
+	{
+		return agents_[agentNo]->timeHorizon_;
+	}
+
+	float RVOSimulator2D::getAgentTimeHorizonObst(size_t agentNo) const
+	{
+		return agents_[agentNo]->timeHorizonObst_;
+	}
+
+	const Vector2 &RVOSimulator2D::getAgentVelocity(size_t agentNo) const
+	{
+		return agents_[agentNo]->velocity_;
+	}
+
+	float RVOSimulator2D::getGlobalTime() const
+	{
+		return globalTime_;
+	}
+
+	size_t RVOSimulator2D::getNumAgents() const
+	{
+		return agents_.size();
+	}
+
+	size_t RVOSimulator2D::getNumObstacleVertices() const
+	{
+		return obstacles_.size();
+	}
+
+	const Vector2 &RVOSimulator2D::getObstacleVertex(size_t vertexNo) const
+	{
+		return obstacles_[vertexNo]->point_;
+	}
+
+	size_t RVOSimulator2D::getNextObstacleVertexNo(size_t vertexNo) const
+	{
+		return obstacles_[vertexNo]->nextObstacle_->id_;
+	}
+
+	size_t RVOSimulator2D::getPrevObstacleVertexNo(size_t vertexNo) const
+	{
+		return obstacles_[vertexNo]->prevObstacle_->id_;
+	}
+
+	float RVOSimulator2D::getTimeStep() const
+	{
+		return timeStep_;
+	}
+
+	void RVOSimulator2D::processObstacles()
+	{
+		kdTree_->buildObstacleTree(obstacles_);
+	}
+
+	bool RVOSimulator2D::queryVisibility(const Vector2 &point1, const Vector2 &point2, float radius) const
+	{
+		return kdTree_->queryVisibility(point1, point2, radius);
+	}
+
+	void RVOSimulator2D::setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity)
+	{
+		if (defaultAgent_ == NULL) {
+			defaultAgent_ = new Agent2D();
+		}
+
+		defaultAgent_->maxNeighbors_ = maxNeighbors;
+		defaultAgent_->maxSpeed_ = maxSpeed;
+		defaultAgent_->neighborDist_ = neighborDist;
+		defaultAgent_->radius_ = radius;
+		defaultAgent_->timeHorizon_ = timeHorizon;
+		defaultAgent_->timeHorizonObst_ = timeHorizonObst;
+		defaultAgent_->velocity_ = velocity;
+	}
+
+	void RVOSimulator2D::setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors)
+	{
+		agents_[agentNo]->maxNeighbors_ = maxNeighbors;
+	}
+
+	void RVOSimulator2D::setAgentMaxSpeed(size_t agentNo, float maxSpeed)
+	{
+		agents_[agentNo]->maxSpeed_ = maxSpeed;
+	}
+
+	void RVOSimulator2D::setAgentNeighborDist(size_t agentNo, float neighborDist)
+	{
+		agents_[agentNo]->neighborDist_ = neighborDist;
+	}
+
+	void RVOSimulator2D::setAgentPosition(size_t agentNo, const Vector2 &position)
+	{
+		agents_[agentNo]->position_ = position;
+	}
+
+	void RVOSimulator2D::setAgentPrefVelocity(size_t agentNo, const Vector2 &prefVelocity)
+	{
+		agents_[agentNo]->prefVelocity_ = prefVelocity;
+	}
+
+	void RVOSimulator2D::setAgentRadius(size_t agentNo, float radius)
+	{
+		agents_[agentNo]->radius_ = radius;
+	}
+
+	void RVOSimulator2D::setAgentTimeHorizon(size_t agentNo, float timeHorizon)
+	{
+		agents_[agentNo]->timeHorizon_ = timeHorizon;
+	}
+
+	void RVOSimulator2D::setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst)
+	{
+		agents_[agentNo]->timeHorizonObst_ = timeHorizonObst;
+	}
+
+	void RVOSimulator2D::setAgentVelocity(size_t agentNo, const Vector2 &velocity)
+	{
+		agents_[agentNo]->velocity_ = velocity;
+	}
+
+	void RVOSimulator2D::setTimeStep(float timeStep)
+	{
+		timeStep_ = timeStep;
+	}
+}
diff --git a/thirdparty/rvo2/rvo2_2d/RVOSimulator2d.h b/thirdparty/rvo2/rvo2_2d/RVOSimulator2d.h
new file mode 100644
index 0000000000..e074e0fe0e
--- /dev/null
+++ b/thirdparty/rvo2/rvo2_2d/RVOSimulator2d.h
@@ -0,0 +1,592 @@
+/*
+ * RVOSimulator2d.h
+ * RVO2 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/>
+ */
+
+#ifndef RVO2D_RVO_SIMULATOR_H_
+#define RVO2D_RVO_SIMULATOR_H_
+
+/**
+ * \file       RVOSimulator2d.h
+ * \brief      Contains the RVOSimulator2D class.
+ */
+
+#include <cstddef>
+#include <limits>
+#include <vector>
+
+#include "Vector2.h"
+
+namespace RVO2D {
+	/**
+	 * \brief       Error value.
+	 *
+	 * A value equal to the largest unsigned integer that is returned in case
+	 * of an error by functions in RVO2D::RVOSimulator2D.
+	 */
+	const size_t RVO2D_ERROR = std::numeric_limits<size_t>::max();
+
+	/**
+	 * \brief      Defines a directed line.
+	 */
+	class Line {
+	public:
+		/**
+		 * \brief     A point on the directed line.
+		 */
+		Vector2 point;
+
+		/**
+		 * \brief     The direction of the directed line.
+		 */
+		Vector2 direction;
+	};
+
+	class Agent2D;
+	class KdTree2D;
+	class Obstacle2D;
+
+	/**
+	 * \brief      Defines the simulation.
+	 *
+	 * The main class of the library that contains all simulation functionality.
+	 */
+	class RVOSimulator2D {
+	public:
+		/**
+		 * \brief      Constructs a simulator instance.
+		 */
+		RVOSimulator2D();
+
+		/**
+		 * \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 minimal 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      timeHorizonObst The default minimal amount of time for which
+		 *                             a new agent's velocities that are computed
+		 *                             by the simulation are safe with respect to
+		 *                             obstacles. The larger this number, the
+		 *                             sooner an agent will respond to the presence
+		 *                             of obstacles, 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 two-dimensional linear
+		 *                             velocity of a new agent (optional).
+		 */
+		RVOSimulator2D(float timeStep, float neighborDist, size_t maxNeighbors,
+					 float timeHorizon, float timeHorizonObst, float radius,
+					 float maxSpeed, const Vector2 &velocity = Vector2());
+
+		/**
+		 * \brief      Destroys this simulator instance.
+		 */
+		~RVOSimulator2D();
+
+		/**
+		 * \brief      Adds a new agent with default properties to the
+		 *             simulation.
+		 * \param      position        The two-dimensional starting position of
+		 *                             this agent.
+		 * \return     The number of the agent, or RVO2D::RVO2D_ERROR when the agent
+		 *             defaults have not been set.
+		 */
+		size_t addAgent(const Vector2 &position);
+
+		/**
+		 * \brief      Adds a new agent to the simulation.
+		 * \param      position        The two-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 minimal 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      timeHorizonObst The minimal amount of time for which this
+		 *                             agent's velocities that are computed by the
+		 *                             simulation are safe with respect to
+		 *                             obstacles. The larger this number, the
+		 *                             sooner this agent will respond to the
+		 *                             presence of obstacles, 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 two-dimensional linear velocity
+		 *                             of this agent (optional).
+		 * \return     The number of the agent.
+		 */
+		size_t addAgent(const Vector2 &position, float neighborDist,
+						size_t maxNeighbors, float timeHorizon,
+						float timeHorizonObst, float radius, float maxSpeed,
+						const Vector2 &velocity = Vector2());
+
+		/**
+		 * \brief      Adds a new obstacle to the simulation.
+		 * \param      vertices        List of the vertices of the polygonal
+		 *             obstacle in counterclockwise order.
+		 * \return     The number of the first vertex of the obstacle,
+		 *             or RVO2D::RVO2D_ERROR when the number of vertices is less than two.
+		 * \note       To add a "negative" obstacle, e.g. a bounding polygon around
+		 *             the environment, the vertices should be listed in clockwise
+		 *             order.
+		 */
+		size_t addObstacle(const std::vector<Vector2> &vertices);
+
+		/**
+		 * \brief      Lets the simulator perform a simulation step and updates the
+		 *             two-dimensional position and two-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 obstacle neighbors taken into account
+		 *             to compute the current velocity for the specified agent.
+		 * \param      agentNo         The number of the agent whose count of
+		 *                             obstacle neighbors is to be retrieved.
+		 * \return     The count of obstacle neighbors taken into account to
+		 *             compute the current velocity for the specified agent.
+		 */
+		size_t getAgentNumObstacleNeighbors(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 getAgentNumORCALines(size_t agentNo) const;
+
+		/**
+		 * \brief      Returns the specified obstacle neighbor of the specified
+		 *             agent.
+		 * \param      agentNo         The number of the agent whose obstacle
+		 *                             neighbor is to be retrieved.
+		 * \param      neighborNo      The number of the obstacle neighbor to be
+		 *                             retrieved.
+		 * \return     The number of the first vertex of the neighboring obstacle
+		 *             edge.
+		 */
+		size_t getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) 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      lineNo          The number of the ORCA constraint to be
+		 *                             retrieved.
+		 * \return     A line representing the specified ORCA constraint.
+		 * \note       The halfplane to the left of the line is the region of
+		 *             permissible velocities with respect to the specified
+		 *             ORCA constraint.
+		 */
+		const Line &getAgentORCALine(size_t agentNo, size_t lineNo) const;
+
+		/**
+		 * \brief      Returns the two-dimensional position of a specified
+		 *             agent.
+		 * \param      agentNo         The number of the agent whose
+		 *                             two-dimensional position is to be retrieved.
+		 * \return     The present two-dimensional position of the (center of the)
+		 *             agent.
+		 */
+		const Vector2 &getAgentPosition(size_t agentNo) const;
+
+		/**
+		 * \brief      Returns the two-dimensional preferred velocity of a
+		 *             specified agent.
+		 * \param      agentNo         The number of the agent whose
+		 *                             two-dimensional preferred velocity is to be
+		 *                             retrieved.
+		 * \return     The present two-dimensional preferred velocity of the agent.
+		 */
+		const Vector2 &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 time horizon with respect to obstacles of a
+		 *             specified agent.
+		 * \param      agentNo         The number of the agent whose time horizon
+		 *                             with respect to obstacles is to be
+		 *                             retrieved.
+		 * \return     The present time horizon with respect to obstacles of the
+		 *             agent.
+		 */
+		float getAgentTimeHorizonObst(size_t agentNo) const;
+
+		/**
+		 * \brief      Returns the two-dimensional linear velocity of a
+		 *             specified agent.
+		 * \param      agentNo         The number of the agent whose
+		 *                             two-dimensional linear velocity is to be
+		 *                             retrieved.
+		 * \return     The present two-dimensional linear velocity of the agent.
+		 */
+		const Vector2 &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 count of obstacle vertices in the simulation.
+		 * \return     The count of obstacle vertices in the simulation.
+		 */
+		size_t getNumObstacleVertices() const;
+
+		/**
+		 * \brief      Returns the two-dimensional position of a specified obstacle
+		 *             vertex.
+		 * \param      vertexNo        The number of the obstacle vertex to be
+		 *                             retrieved.
+		 * \return     The two-dimensional position of the specified obstacle
+		 *             vertex.
+		 */
+		const Vector2 &getObstacleVertex(size_t vertexNo) const;
+
+		/**
+		 * \brief      Returns the number of the obstacle vertex succeeding the
+		 *             specified obstacle vertex in its polygon.
+		 * \param      vertexNo        The number of the obstacle vertex whose
+		 *                             successor is to be retrieved.
+		 * \return     The number of the obstacle vertex succeeding the specified
+		 *             obstacle vertex in its polygon.
+		 */
+		size_t getNextObstacleVertexNo(size_t vertexNo) const;
+
+		/**
+		 * \brief      Returns the number of the obstacle vertex preceding the
+		 *             specified obstacle vertex in its polygon.
+		 * \param      vertexNo        The number of the obstacle vertex whose
+		 *                             predecessor is to be retrieved.
+		 * \return     The number of the obstacle vertex preceding the specified
+		 *             obstacle vertex in its polygon.
+		 */
+		size_t getPrevObstacleVertexNo(size_t vertexNo) const;
+
+		/**
+		 * \brief      Returns the time step of the simulation.
+		 * \return     The present time step of the simulation.
+		 */
+		float getTimeStep() const;
+
+		/**
+		 * \brief      Processes the obstacles that have been added so that they
+		 *             are accounted for in the simulation.
+		 * \note       Obstacles added to the simulation after this function has
+		 *             been called are not accounted for in the simulation.
+		 */
+		void processObstacles();
+
+		/**
+		 * \brief      Performs a visibility query between the two specified
+		 *             points with respect to the obstacles
+		 * \param      point1          The first point of the query.
+		 * \param      point2          The second point of the query.
+		 * \param      radius          The minimal distance between the line
+		 *                             connecting the two points and the obstacles
+		 *                             in order for the points to be mutually
+		 *                             visible (optional). Must be non-negative.
+		 * \return     A boolean specifying whether the two points are mutually
+		 *             visible. Returns true when the obstacles have not been
+		 *             processed.
+		 */
+		bool queryVisibility(const Vector2 &point1, const Vector2 &point2,
+							 float radius = 0.0f) const;
+
+		/**
+		 * \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 minimal 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      timeHorizonObst The default minimal amount of time for which
+		 *                             a new agent's velocities that are computed
+		 *                             by the simulation are safe with respect to
+		 *                             obstacles. The larger this number, the
+		 *                             sooner an agent will respond to the presence
+		 *                             of obstacles, 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 two-dimensional linear
+		 *                             velocity of a new agent (optional).
+		 */
+		void setAgentDefaults(float neighborDist, size_t maxNeighbors,
+							  float timeHorizon, float timeHorizonObst,
+							  float radius, float maxSpeed,
+							  const Vector2 &velocity = Vector2());
+
+		/**
+		 * \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 two-dimensional position of a specified agent.
+		 * \param      agentNo         The number of the agent whose
+		 *                             two-dimensional position is to be modified.
+		 * \param      position        The replacement of the two-dimensional
+		 *                             position.
+		 */
+		void setAgentPosition(size_t agentNo, const Vector2 &position);
+
+		/**
+		 * \brief      Sets the two-dimensional preferred velocity of a
+		 *             specified agent.
+		 * \param      agentNo         The number of the agent whose
+		 *                             two-dimensional preferred velocity is to be
+		 *                             modified.
+		 * \param      prefVelocity    The replacement of the two-dimensional
+		 *                             preferred velocity.
+		 */
+		void setAgentPrefVelocity(size_t agentNo, const Vector2 &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 time horizon of a specified agent with respect
+		 *             to obstacles.
+		 * \param      agentNo         The number of the agent whose time horizon
+		 *                             with respect to obstacles is to be modified.
+		 * \param      timeHorizonObst The replacement time horizon with respect to
+		 *                             obstacles. Must be positive.
+		 */
+		void setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst);
+
+		/**
+		 * \brief      Sets the two-dimensional linear velocity of a specified
+		 *             agent.
+		 * \param      agentNo         The number of the agent whose
+		 *                             two-dimensional linear velocity is to be
+		 *                             modified.
+		 * \param      velocity        The replacement two-dimensional linear
+		 *                             velocity.
+		 */
+		void setAgentVelocity(size_t agentNo, const Vector2 &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:
+		std::vector<Agent2D *> agents_;
+		Agent2D *defaultAgent_;
+		float globalTime_;
+		KdTree2D *kdTree_;
+		std::vector<Obstacle2D *> obstacles_;
+		float timeStep_;
+
+		friend class Agent2D;
+		friend class KdTree2D;
+		friend class Obstacle2D;
+	};
+}
+
+#endif /* RVO2D_RVO_SIMULATOR_H_ */
diff --git a/thirdparty/rvo2/rvo2_2d/Vector2.h b/thirdparty/rvo2/rvo2_2d/Vector2.h
new file mode 100644
index 0000000000..24353e09f3
--- /dev/null
+++ b/thirdparty/rvo2/rvo2_2d/Vector2.h
@@ -0,0 +1,346 @@
+/*
+ * Vector2.h
+ * RVO2 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/>
+ */
+
+#ifndef RVO_VECTOR2_H_
+#define RVO_VECTOR2_H_
+
+/**
+ * \file       Vector2.h
+ * \brief      Contains the Vector2 class.
+ */
+
+#include <cmath>
+#include <ostream>
+
+namespace RVO2D {
+	/**
+	 * \brief      Defines a two-dimensional vector.
+	 */
+	class Vector2 {
+	public:
+		/**
+		 * \brief      Constructs and initializes a two-dimensional vector instance
+		 *             to (0.0, 0.0).
+		 */
+		inline Vector2() : x_(0.0f), y_(0.0f) { }
+
+		/**
+		 * \brief      Constructs and initializes a two-dimensional vector from
+		 *             the specified xy-coordinates.
+		 * \param      x               The x-coordinate of the two-dimensional
+		 *                             vector.
+		 * \param      y               The y-coordinate of the two-dimensional
+		 *                             vector.
+		 */
+		inline Vector2(float x, float y) : x_(x), y_(y) { }
+
+		inline Vector2(const Vector2 &vector)
+		{
+			x_ = vector.x();
+			y_ = vector.y();
+		}
+
+		/**
+		 * \brief      Returns the x-coordinate of this two-dimensional vector.
+		 * \return     The x-coordinate of the two-dimensional vector.
+		 */
+		inline float x() const { return x_; }
+
+		/**
+		 * \brief      Returns the y-coordinate of this two-dimensional vector.
+		 * \return     The y-coordinate of the two-dimensional vector.
+		 */
+		inline float y() const { return y_; }
+
+		/**
+		 * \brief      Computes the negation of this two-dimensional vector.
+		 * \return     The negation of this two-dimensional vector.
+		 */
+		inline Vector2 operator-() const
+		{
+			return Vector2(-x_, -y_);
+		}
+
+		/**
+		 * \brief      Computes the dot product of this two-dimensional vector with
+		 *             the specified two-dimensional vector.
+		 * \param      vector          The two-dimensional vector with which the
+		 *                             dot product should be computed.
+		 * \return     The dot product of this two-dimensional vector with a
+		 *             specified two-dimensional vector.
+		 */
+		inline float operator*(const Vector2 &vector) const
+		{
+			return x_ * vector.x() + y_ * vector.y();
+		}
+
+		/**
+		 * \brief      Computes the scalar multiplication of this
+		 *             two-dimensional vector with the specified scalar value.
+		 * \param      s               The scalar value with which the scalar
+		 *                             multiplication should be computed.
+		 * \return     The scalar multiplication of this two-dimensional vector
+		 *             with a specified scalar value.
+		 */
+		inline Vector2 operator*(float s) const
+		{
+			return Vector2(x_ * s, y_ * s);
+		}
+
+		/**
+		 * \brief      Computes the scalar division of this two-dimensional vector
+		 *             with the specified scalar value.
+		 * \param      s               The scalar value with which the scalar
+		 *                             division should be computed.
+		 * \return     The scalar division of this two-dimensional vector with a
+		 *             specified scalar value.
+		 */
+		inline Vector2 operator/(float s) const
+		{
+			const float invS = 1.0f / s;
+
+			return Vector2(x_ * invS, y_ * invS);
+		}
+
+		/**
+		 * \brief      Computes the vector sum of this two-dimensional vector with
+		 *             the specified two-dimensional vector.
+		 * \param      vector          The two-dimensional vector with which the
+		 *                             vector sum should be computed.
+		 * \return     The vector sum of this two-dimensional vector with a
+		 *             specified two-dimensional vector.
+		 */
+		inline Vector2 operator+(const Vector2 &vector) const
+		{
+			return Vector2(x_ + vector.x(), y_ + vector.y());
+		}
+
+		/**
+		 * \brief      Computes the vector difference of this two-dimensional
+		 *             vector with the specified two-dimensional vector.
+		 * \param      vector          The two-dimensional vector with which the
+		 *                             vector difference should be computed.
+		 * \return     The vector difference of this two-dimensional vector with a
+		 *             specified two-dimensional vector.
+		 */
+		inline Vector2 operator-(const Vector2 &vector) const
+		{
+			return Vector2(x_ - vector.x(), y_ - vector.y());
+		}
+
+		/**
+		 * \brief      Tests this two-dimensional vector for equality with the
+		 *             specified two-dimensional vector.
+		 * \param      vector          The two-dimensional vector with which to
+		 *                             test for equality.
+		 * \return     True if the two-dimensional vectors are equal.
+		 */
+		inline bool operator==(const Vector2 &vector) const
+		{
+			return x_ == vector.x() && y_ == vector.y();
+		}
+
+		/**
+		 * \brief      Tests this two-dimensional vector for inequality with the
+		 *             specified two-dimensional vector.
+		 * \param      vector          The two-dimensional vector with which to
+		 *                             test for inequality.
+		 * \return     True if the two-dimensional vectors are not equal.
+		 */
+		inline bool operator!=(const Vector2 &vector) const
+		{
+			return x_ != vector.x() || y_ != vector.y();
+		}
+
+		/**
+		 * \brief      Sets the value of this two-dimensional vector to the scalar
+		 *             multiplication of itself with the specified scalar value.
+		 * \param      s               The scalar value with which the scalar
+		 *                             multiplication should be computed.
+		 * \return     A reference to this two-dimensional vector.
+		 */
+		inline Vector2 &operator*=(float s)
+		{
+			x_ *= s;
+			y_ *= s;
+
+			return *this;
+		}
+
+		/**
+		 * \brief      Sets the value of this two-dimensional vector to the scalar
+		 *             division of itself with the specified scalar value.
+		 * \param      s               The scalar value with which the scalar
+		 *                             division should be computed.
+		 * \return     A reference to this two-dimensional vector.
+		 */
+		inline Vector2 &operator/=(float s)
+		{
+			const float invS = 1.0f / s;
+			x_ *= invS;
+			y_ *= invS;
+
+			return *this;
+		}
+
+		/**
+		 * \brief      Sets the value of this two-dimensional vector to the vector
+		 *             sum of itself with the specified two-dimensional vector.
+		 * \param      vector          The two-dimensional vector with which the
+		 *                             vector sum should be computed.
+		 * \return     A reference to this two-dimensional vector.
+		 */
+		inline Vector2 &operator+=(const Vector2 &vector)
+		{
+			x_ += vector.x();
+			y_ += vector.y();
+
+			return *this;
+		}
+
+		/**
+		 * \brief      Sets the value of this two-dimensional vector to the vector
+		 *             difference of itself with the specified two-dimensional
+		 *             vector.
+		 * \param      vector          The two-dimensional vector with which the
+		 *                             vector difference should be computed.
+		 * \return     A reference to this two-dimensional vector.
+		 */
+		inline Vector2 &operator-=(const Vector2 &vector)
+		{
+			x_ -= vector.x();
+			y_ -= vector.y();
+
+			return *this;
+		}
+
+		inline Vector2 &operator=(const Vector2 &vector)
+		{
+			x_ = vector.x();
+			y_ = vector.y();
+
+			return *this;
+		}
+
+	private:
+		float x_;
+		float y_;
+	};
+
+	/**
+	 * \relates    Vector2
+	 * \brief      Computes the scalar multiplication of the specified
+	 *             two-dimensional vector with the specified scalar value.
+	 * \param      s               The scalar value with which the scalar
+	 *                             multiplication should be computed.
+	 * \param      vector          The two-dimensional vector with which the scalar
+	 *                             multiplication should be computed.
+	 * \return     The scalar multiplication of the two-dimensional vector with the
+	 *             scalar value.
+	 */
+	inline Vector2 operator*(float s, const Vector2 &vector)
+	{
+		return Vector2(s * vector.x(), s * vector.y());
+	}
+
+	/**
+	 * \relates    Vector2
+	 * \brief      Inserts the specified two-dimensional vector into the specified
+	 *             output stream.
+	 * \param      os              The output stream into which the two-dimensional
+	 *                             vector should be inserted.
+	 * \param      vector          The two-dimensional vector which to insert into
+	 *                             the output stream.
+	 * \return     A reference to the output stream.
+	 */
+	inline std::ostream &operator<<(std::ostream &os, const Vector2 &vector)
+	{
+		os << "(" << vector.x() << "," << vector.y() << ")";
+
+		return os;
+	}
+
+	/**
+	 * \relates    Vector2
+	 * \brief      Computes the length of a specified two-dimensional vector.
+	 * \param      vector          The two-dimensional vector whose length is to be
+	 *                             computed.
+	 * \return     The length of the two-dimensional vector.
+	 */
+	inline float abs(const Vector2 &vector)
+	{
+		return std::sqrt(vector * vector);
+	}
+
+	/**
+	 * \relates    Vector2
+	 * \brief      Computes the squared length of a specified two-dimensional
+	 *             vector.
+	 * \param      vector          The two-dimensional vector whose squared length
+	 *                             is to be computed.
+	 * \return     The squared length of the two-dimensional vector.
+	 */
+	inline float absSq(const Vector2 &vector)
+	{
+		return vector * vector;
+	}
+
+	/**
+	 * \relates    Vector2
+	 * \brief      Computes the determinant of a two-dimensional square matrix with
+	 *             rows consisting of the specified two-dimensional vectors.
+	 * \param      vector1         The top row of the two-dimensional square
+	 *                             matrix.
+	 * \param      vector2         The bottom row of the two-dimensional square
+	 *                             matrix.
+	 * \return     The determinant of the two-dimensional square matrix.
+	 */
+	inline float det(const Vector2 &vector1, const Vector2 &vector2)
+	{
+		return vector1.x() * vector2.y() - vector1.y() * vector2.x();
+	}
+
+	/**
+	 * \relates    Vector2
+	 * \brief      Computes the normalization of the specified two-dimensional
+	 *             vector.
+	 * \param      vector          The two-dimensional vector whose normalization
+	 *                             is to be computed.
+	 * \return     The normalization of the two-dimensional vector.
+	 */
+	inline Vector2 normalize(const Vector2 &vector)
+	{
+		return vector / abs(vector);
+	}
+}
+
+#endif /* RVO_VECTOR2_H_ */