diff --git a/COPYRIGHT.txt b/COPYRIGHT.txt index 94e182e47e658..61009583fa3b6 100644 --- a/COPYRIGHT.txt +++ b/COPYRIGHT.txt @@ -344,6 +344,11 @@ Comment: Recast Copyright: 2009, Mikko Mononen License: Zlib +Files: ./thirdparty/rvo2/ +Comment: RVO2 +Copyright: 2016, University of North Carolina at Chapel Hill +License: Apache 2.0 + Files: ./thirdparty/squish/ Comment: libSquish Copyright: 2006, Simon Brown diff --git a/SConstruct b/SConstruct index 128c5b0e926ad..bb2bf523ce534 100644 --- a/SConstruct +++ b/SConstruct @@ -151,6 +151,7 @@ opts.Add(BoolVariable('builtin_miniupnpc', "Use the built-in miniupnpc library", opts.Add(BoolVariable('builtin_opus', "Use the built-in Opus library", True)) opts.Add(BoolVariable('builtin_pcre2', "Use the built-in PCRE2 library", True)) opts.Add(BoolVariable('builtin_recast', "Use the built-in Recast library", True)) +opts.Add(BoolVariable('builtin_rvo2', "Use the built-in RVO2 library", True)) opts.Add(BoolVariable('builtin_squish', "Use the built-in squish library", True)) opts.Add(BoolVariable('builtin_xatlas', "Use the built-in xatlas library", True)) opts.Add(BoolVariable('builtin_zlib', "Use the built-in zlib library", True)) diff --git a/modules/orca/SCsub b/modules/orca/SCsub new file mode 100644 index 0000000000000..bc5e9c57f270f --- /dev/null +++ b/modules/orca/SCsub @@ -0,0 +1,25 @@ +#!/usr/bin/env python + +Import('env') +Import('env_modules') + +env_orca = env_modules.Clone() + +# Thirdparty source files +if env['builtin_rvo2']: + thirdparty_dir = "#thirdparty/rvo2/" + thirdparty_sources = [ + "src/Agent.cpp", + "src/KdTree.cpp", + "src/Obstacle.cpp", + ] + thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources] + + env_orca.Prepend(CPPPATH=[thirdparty_dir + "/src"]) + + env_thirdparty = env_orca.Clone() + env_thirdparty.disable_warnings() + env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) + +# Godot source files +env_orca.add_source_files(env.modules_sources, "*.cpp") diff --git a/modules/orca/collision_avoidance_2d.cpp b/modules/orca/collision_avoidance_2d.cpp new file mode 100644 index 0000000000000..3ea3aebd2b03f --- /dev/null +++ b/modules/orca/collision_avoidance_2d.cpp @@ -0,0 +1,201 @@ +/*************************************************************************/ +/* collision_avoidance_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "collision_avoidance_2d.h" + +#include +#include +#include +#include +#include + +RVO::KdTree *CollisionAvoidance2D::get_tree() { + return &kd_tree; +} + +void CollisionAvoidance2D::remove_bounds() { + remove_obstacle(0); + bounds = Rect2(); +} + +void CollisionAvoidance2D::set_bounds(Rect2 p_bounds) { + bounds = p_bounds; + if (p_bounds.has_no_area()) { + remove_obstacle(0); + return; + } + Vector points = Vector(); + Vector2 top_left = p_bounds.get_position(); + Vector2 bottom_right = p_bounds.get_size() + p_bounds.get_position(); + if (top_left.x > bottom_right.x) { + float swap = top_left.x; + top_left.x = bottom_right.x; + bottom_right.x = swap; + } + if (top_left.y > bottom_right.y) { + float swap = top_left.y; + top_left.y = bottom_right.y; + bottom_right.y = swap; + } + + points.push_back(top_left); + points.push_back(Vector2(top_left.x, bottom_right.y)); + points.push_back(bottom_right); + points.push_back(Vector2(bottom_right.x, top_left.y)); + + _add_obstacle(points, 0); +} + +Rect2 CollisionAvoidance2D::get_bounds() { + return bounds; +} + +void CollisionAvoidance2D::move_obstacle(int p_obstacle, Vector p_obstacle_points) { + remove_obstacle(p_obstacle); + _add_obstacle(p_obstacle_points, p_obstacle); +} + +void CollisionAvoidance2D::_add_obstacle(Vector p_obstacle_points, int p_obstacle) { + Vector obstacles = Vector(); + obstacles.resize(p_obstacle_points.size()); + + for (int i = 0; i < p_obstacle_points.size(); i++) { + RVO::Obstacle *point = new RVO::Obstacle(); + point->point_ = RVO::Vector2(p_obstacle_points[i].x, p_obstacle_points[i].y); + + // RVO expects the points in counterclockwise order + obstacles.set(p_obstacle_points.size() - i - 1, point); + } + + for (int i = 0; i < obstacles.size(); i++) { + RVO::Obstacle *point = obstacles[i]; + point->id_ = p_obstacle; + point->prevObstacle_ = obstacles[i > 0 ? i - 1 : obstacles.size() - 1]; + point->nextObstacle_ = obstacles[i < obstacles.size() - 1 ? i + 1 : 0]; + point->unitDir_ = RVO::normalize(point->point_ - point->nextObstacle_->point_); + if (obstacles.size() > 2) { + point->isConvex_ = RVO::leftOf(point->prevObstacle_->point_, point->point_, point->nextObstacle_->point_); + } else { + point->isConvex_ = true; + } + } + + obstacles_dirty = true; + obstacles_map[p_obstacle] = obstacles; +} + +int CollisionAvoidance2D::add_obstacle(Vector p_obstacle_points) { + if (p_obstacle_points.size() < 2) { + return -1; + } + + ++last_obstacle_id; + + _add_obstacle(p_obstacle_points, last_obstacle_id); + + return last_obstacle_id; +} + +void CollisionAvoidance2D::remove_obstacle(int p_obstacle) { + Vector points = obstacles_map[p_obstacle]; + for (int i = 0; i < points.size(); i++) { + delete points[i]; + } + + obstacles_map.erase(p_obstacle); + obstacles_dirty = true; +} + +int CollisionAvoidance2D::add_actor(CollisionAvoidanceActor2D *p_actor) { + ++last_actor_id; + actor_map[last_actor_id] = p_actor; + return last_actor_id; +} + +void CollisionAvoidance2D::remove_actor(int p_id) { + actor_map.erase(p_id); +} + +void CollisionAvoidance2D::navigate_actors() { + auto e = actor_map.front(); + while (e != NULL) { + e->get()->sync_position(); + e = e->next(); + } + + if (obstacles_dirty) { + std::vector obstacles = std::vector(); + auto o = obstacles_map.front(); + while (o != NULL) { + Vector obstacle = o->get(); + for (int i = 0; i < obstacle.size(); i++) { + obstacles.push_back(obstacle[i]); + } + o = o->next(); + } + + int previous_size = obstacles.size(); + kd_tree.buildObstacleTree(obstacles); + for (long unsigned int i = previous_size; i < obstacles.size(); i++) { + obstacles_map[obstacles[i]->id_].push_back(obstacles[i]); + } + obstacles_dirty = false; + } + + auto agents = std::vector(); + e = actor_map.front(); + while (e != NULL) { + agents.push_back(e->get()->get_agent()); + e = e->next(); + } + kd_tree.buildAgentTree(agents); +} + +void CollisionAvoidance2D::_bind_methods() { + ClassDB::bind_method(D_METHOD("set_bounds", "bounds"), &CollisionAvoidance2D::set_bounds); + ClassDB::bind_method(D_METHOD("get_bounds"), &CollisionAvoidance2D::get_bounds); + ClassDB::bind_method(D_METHOD("remove_bounds"), &CollisionAvoidance2D::remove_bounds); + ClassDB::bind_method(D_METHOD("add_obstacle", "points"), &CollisionAvoidance2D::add_obstacle); + ClassDB::bind_method(D_METHOD("move_obstacle", "obstacle_id", "points"), &CollisionAvoidance2D::move_obstacle); + ClassDB::bind_method(D_METHOD("remove_obstacle", "obstacle_id"), &CollisionAvoidance2D::remove_obstacle); + ClassDB::bind_method(D_METHOD("navigate_actors"), &CollisionAvoidance2D::navigate_actors); + ADD_PROPERTY(PropertyInfo(Variant::RECT2, "bounds"), "set_bounds", "get_bounds"); +} + +CollisionAvoidance2D::CollisionAvoidance2D() : + processed_obstacles(false), + actor_map(), + obstacles_map(), + bounds(), + kd_tree(), + last_obstacle_id(0), + last_actor_id(0), + obstacles_dirty(false) { +} diff --git a/modules/orca/collision_avoidance_2d.h b/modules/orca/collision_avoidance_2d.h new file mode 100644 index 0000000000000..049f8ee1122ae --- /dev/null +++ b/modules/orca/collision_avoidance_2d.h @@ -0,0 +1,77 @@ +/*************************************************************************/ +/* collision_avoidance_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef COLLISION_AVOIDANCE_2D_H +#define COLLISION_AVOIDANCE_2D_H + +#include "collision_avoidance_actor_2d.h" +#include "scene/2d/node_2d.h" + +#include +#include + +class CollisionAvoidance2D : public Node2D { + GDCLASS(CollisionAvoidance2D, Node2D); + +private: + bool processed_obstacles; + Map actor_map; + Map > obstacles_map; + Rect2 bounds; + RVO::KdTree kd_tree; + int last_obstacle_id; + int last_actor_id; + bool obstacles_dirty; + + void _add_obstacle(Vector p_obstacle_points, int p_obstacle); + +protected: + static void _bind_methods(); + +public: + RVO::KdTree *get_tree(); + + void set_bounds(Rect2 p_bounds); + Rect2 get_bounds(); + void remove_bounds(); + + int add_obstacle(Vector p_obstacle_points); + void move_obstacle(int p_obstacle, Vector p_obstacle_points); + void remove_obstacle(int p_obstacle); + + int add_actor(CollisionAvoidanceActor2D *p_actor); + void remove_actor(int p_id); + + void navigate_actors(); + + CollisionAvoidance2D(); +}; + +#endif diff --git a/modules/orca/collision_avoidance_actor_2d.cpp b/modules/orca/collision_avoidance_actor_2d.cpp new file mode 100644 index 0000000000000..b0bbdfe9fdff4 --- /dev/null +++ b/modules/orca/collision_avoidance_actor_2d.cpp @@ -0,0 +1,176 @@ +/*************************************************************************/ +/* collision_avoidance_actor_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "collision_avoidance_actor_2d.h" +#include "collision_avoidance_2d.h" + +void CollisionAvoidanceActor2D::set_radius(float p_radius) { + agent.radius_ = p_radius; +} +float CollisionAvoidanceActor2D::get_radius() { + return agent.radius_; +} + +void CollisionAvoidanceActor2D::set_obstacle_time_horizon(float p_horizon) { + agent.timeHorizonObst_ = p_horizon; +} +float CollisionAvoidanceActor2D::get_obstacle_time_horizon() { + return agent.timeHorizonObst_; +} + +void CollisionAvoidanceActor2D::set_neighbor_time_horizon(float p_horizon) { + agent.timeHorizon_ = p_horizon; +} +float CollisionAvoidanceActor2D::get_neighbor_time_horizon() { + return agent.timeHorizon_; +} + +void CollisionAvoidanceActor2D::set_max_neighbors(int p_neighbors) { + agent.maxNeighbors_ = p_neighbors; +} +int CollisionAvoidanceActor2D::get_max_neighbors() { + return agent.maxNeighbors_; +} + +void CollisionAvoidanceActor2D::set_neighbor_search_distance(float p_search_distance) { + agent.neighborDist_ = p_search_distance; +} +float CollisionAvoidanceActor2D::get_neighbor_search_distance() { + return agent.neighborDist_; +} + +void CollisionAvoidanceActor2D::set_max_speed(float p_speed) { + agent.maxSpeed_ = p_speed; +} +float CollisionAvoidanceActor2D::get_max_speed() { + return agent.maxSpeed_; +} + +void CollisionAvoidanceActor2D::_notification(int p_what) { + switch (p_what) { + case NOTIFICATION_ENTER_TREE: { + Node2D *c = this; + while (c) { + + parent = Object::cast_to(c); + if (parent) { + id = parent->add_actor(this); + break; + } + + c = Object::cast_to(c->get_parent()); + Vector2 relative_position = get_relative_transform().get_origin(); + agent.position_ = RVO::Vector2(relative_position.x, relative_position.y); + } + + } break; + case NOTIFICATION_EXIT_TREE: { + if (parent != NULL) { + parent->remove_actor(id); + } + } + } +} + +Transform2D CollisionAvoidanceActor2D::get_relative_transform() { + if (parent == NULL) { + return Transform2D(); + } + Transform2D transform = get_transform(); + Node *c = get_parent(); + while (c != parent) { + c = c->get_parent(); + transform *= Object::cast_to(c)->get_transform(); + } + return transform; +} + +void CollisionAvoidanceActor2D::sync_position() { + Vector2 relative_position = get_relative_transform().get_origin(); + agent.position_ = RVO::Vector2(relative_position.x, relative_position.y); +} + +Vector2 CollisionAvoidanceActor2D::calculate_velocity(Vector2 p_preferred_velocity, float p_delta) { + if (parent == NULL) { + return p_preferred_velocity; + } + + agent.prefVelocity_ = RVO::Vector2(p_preferred_velocity.x, p_preferred_velocity.y); + agent.computeNeighbors(parent->get_tree()); + agent.computeNewVelocity(p_delta); + agent.velocity_ = agent.newVelocity_; + return Vector2(agent.newVelocity_.x(), agent.newVelocity_.y()); +} + +RVO::Agent *CollisionAvoidanceActor2D::get_agent() { + return &agent; +} + +void CollisionAvoidanceActor2D::_bind_methods() { + ClassDB::bind_method(D_METHOD("set_radius", "radius"), &CollisionAvoidanceActor2D::set_radius); + ClassDB::bind_method(D_METHOD("get_radius"), &CollisionAvoidanceActor2D::get_radius); + ClassDB::bind_method(D_METHOD("set_obstacle_time_horizon", "horizon"), &CollisionAvoidanceActor2D::set_obstacle_time_horizon); + ClassDB::bind_method(D_METHOD("get_obstacle_time_horizon"), &CollisionAvoidanceActor2D::get_obstacle_time_horizon); + ClassDB::bind_method(D_METHOD("set_neighbor_time_horizon", "horizon"), &CollisionAvoidanceActor2D::set_neighbor_time_horizon); + ClassDB::bind_method(D_METHOD("get_neighbor_time_horizon"), &CollisionAvoidanceActor2D::get_neighbor_time_horizon); + ClassDB::bind_method(D_METHOD("set_max_neighbors", "neighbors"), &CollisionAvoidanceActor2D::set_max_neighbors); + ClassDB::bind_method(D_METHOD("get_max_neighbors"), &CollisionAvoidanceActor2D::get_max_neighbors); + ClassDB::bind_method(D_METHOD("set_neighbor_search_distance", "distance"), &CollisionAvoidanceActor2D::set_neighbor_search_distance); + ClassDB::bind_method(D_METHOD("get_neighbor_search_distance"), &CollisionAvoidanceActor2D::get_neighbor_search_distance); + ClassDB::bind_method(D_METHOD("set_max_speed", "speed"), &CollisionAvoidanceActor2D::set_max_speed); + ClassDB::bind_method(D_METHOD("get_max_speed"), &CollisionAvoidanceActor2D::get_max_speed); + ClassDB::bind_method(D_METHOD("calculate_velocity", "preferred_velocity", "delta"), &CollisionAvoidanceActor2D::calculate_velocity); + + ADD_PROPERTY(PropertyInfo(Variant::REAL, "radius"), "set_radius", "get_radius"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "max_speed"), "set_max_speed", "get_max_speed"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "obstacle_time_horizon"), "set_obstacle_time_horizon", "get_obstacle_time_horizon"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "neighbor_time_horizon"), "set_neighbor_time_horizon", "get_neighbor_time_horizon"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "max_neighbors"), "set_max_neighbors", "get_max_neighbors"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "neighbor_search_distance"), "set_neighbor_search_distance", "get_neighbor_search_distance"); + + ADD_PROPERTY_DEFAULT("max_speed", 100); + ADD_PROPERTY_DEFAULT("radius", 5); + ADD_PROPERTY_DEFAULT("obstacle_time_horizon", 30); + ADD_PROPERTY_DEFAULT("neighbor_time_horizon", 10); + ADD_PROPERTY_DEFAULT("max_neighbors", 20); + ADD_PROPERTY_DEFAULT("neighbor_search_distance", 200); +} + +CollisionAvoidanceActor2D::CollisionAvoidanceActor2D() : + id(0), + agent(), + parent(NULL) { + agent.maxSpeed_ = 100; + agent.radius_ = 5; + agent.timeHorizonObst_ = 30; + agent.timeHorizon_ = 10; + agent.maxNeighbors_ = 20; + agent.neighborDist_ = 200; +} diff --git a/modules/orca/collision_avoidance_actor_2d.h b/modules/orca/collision_avoidance_actor_2d.h new file mode 100644 index 0000000000000..3d27d29d73fc0 --- /dev/null +++ b/modules/orca/collision_avoidance_actor_2d.h @@ -0,0 +1,82 @@ +/*************************************************************************/ +/* collision_avoidance_actor_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef COLLISION_AVOIDANCE_ACTOR_2D_H +#define COLLISION_AVOIDANCE_ACTOR_2D_H + +#include "scene/2d/node_2d.h" + +#include + +class CollisionAvoidance2D; + +class CollisionAvoidanceActor2D : public Node2D { + GDCLASS(CollisionAvoidanceActor2D, Node2D); + +private: + int id; + RVO::Agent agent; + CollisionAvoidance2D *parent; + +protected: + static void _bind_methods(); + +public: + void set_radius(float p_radius); + float get_radius(); + + void set_obstacle_time_horizon(float p_horizon); + float get_obstacle_time_horizon(); + + void set_neighbor_time_horizon(float p_horizon); + float get_neighbor_time_horizon(); + + void set_max_neighbors(int p_neighbors); + int get_max_neighbors(); + + void set_neighbor_search_distance(float p_search_distance); + float get_neighbor_search_distance(); + + void set_max_speed(float p_speed); + float get_max_speed(); + + Transform2D get_relative_transform(); + + void _notification(int p_what); + + void sync_position(); + Vector2 calculate_velocity(Vector2 p_preferred_velocity, float p_delta); + + RVO::Agent *get_agent(); + + CollisionAvoidanceActor2D(); +}; + +#endif diff --git a/modules/orca/config.py b/modules/orca/config.py new file mode 100644 index 0000000000000..f8dabe2fbdb0f --- /dev/null +++ b/modules/orca/config.py @@ -0,0 +1,14 @@ +def can_build(env, platform): + return True + +def configure(env): + pass + +def get_doc_classes(): + return [ + "CollisionAvoidance2D", + "CollisionAvoidanceActor2D", + ] + +def get_doc_path(): + return "doc_classes" \ No newline at end of file diff --git a/modules/orca/doc_classes/CollisionAvoidance2D.xml b/modules/orca/doc_classes/CollisionAvoidance2D.xml new file mode 100644 index 0000000000000..d8abc30ff3ac7 --- /dev/null +++ b/modules/orca/doc_classes/CollisionAvoidance2D.xml @@ -0,0 +1,67 @@ + + + + Helps CollisionAvoidanceActor2D actors move around without colliding into each other or into other obstacles. + + + This module implements Reciprical Collision Avoidance using the library RVO2: http://gamma.cs.unc.edu/RVO2/ + This module does not add pathfinding. Pathfinding determines the path an actor needs to take to reach it's goal. Collision avoidance will help the actor avoid moving (or static) obstacles along its way. + You can add actors by placing CollisionAvoidanceActor2D as a subnode. You can add obstacles by calling add_obstacle. + Each tick you should call `navigate_actors()`. otherwise, actors will be unable to identify nearby actors. + + + + + + + + + + + Add an obstacle for actors to avoid. The points should be given in clockwise order. + + + + + + + + + + + This is equivalent to calling remove_obstacle() then add_obstacle(), except the id remains the same. + Moving obstacles each tick will likely slow your game down, as it requires a full recalculation of all obstacles. + + + + + + + Generates the information needed for actors to navigate around other actors. This should be called in `_process()` each tick. + + + + + + + Removes the bounds. + + + + + + + + + Removes an obstacle from the scene. + + + + + + The rectangular bounds all actors should stay within. + + + + + diff --git a/modules/orca/doc_classes/CollisionAvoidanceActor2D.xml b/modules/orca/doc_classes/CollisionAvoidanceActor2D.xml new file mode 100644 index 0000000000000..f303a1d58ee26 --- /dev/null +++ b/modules/orca/doc_classes/CollisionAvoidanceActor2D.xml @@ -0,0 +1,53 @@ + + + + This actor will attempt to avoid collisions with other CollisionAvoidanceActor2D actors. + + + To use this, ensure it is a child of a CollisionAvoidance2D node. + Each tick, calculate_velocity() should be called to calculate the velocity that will avoid its neighbors + An actor's shape is determined by it's radius, not by other collision objects. Other collision objects are intended for physics collision, not collision avoidance. + + + + + + + + + + + + + Returns the velocity that will avoid other CollisionAvoidanceActor2D and obstacles. + This velocity will be as close as possible to preferred_velocity, but it can be anywhere between 0 and max_speed and in any direction. + This velocity should still be multipled by delta when moving an actor in the current tick. + + + + + + The maximum number of neighbors for each actor to consider. + Setting max_neighbors, neighbor_search_distance, and neighbor_time_horizon to be too low causes more collisions. + Setting max_neighbors, neighbor_search_distance, and neighbor_time_horizon to be too high causes actors to slow down or freeze, and may cause game lag. + + + The maximum speed an actor can move. + If this has the same magnitude as the preferred_velocity, actors are more likely to get stuck as they can only slow down to avoid collision. + + + The distance to search for other actors. + + + How far into the future to predict each neighbor's path (in relation to their own path). + + + How far into the future to predict the actor's collision with obstacles. The bigger this number is, the sooner it can respond to obstacles, but limits the actor's movement. + + + The radius of the actor. + + + + + diff --git a/modules/orca/register_types.cpp b/modules/orca/register_types.cpp new file mode 100644 index 0000000000000..9099da023e266 --- /dev/null +++ b/modules/orca/register_types.cpp @@ -0,0 +1,42 @@ +/*************************************************************************/ +/* register_types.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "register_types.h" + +#include "collision_avoidance_2d.h" +#include "core/class_db.h" + +void register_orca_types() { + ClassDB::register_class(); + ClassDB::register_class(); +} + +void unregister_orca_types() { +} diff --git a/modules/orca/register_types.h b/modules/orca/register_types.h new file mode 100644 index 0000000000000..180d48309bd7b --- /dev/null +++ b/modules/orca/register_types.h @@ -0,0 +1,32 @@ +/*************************************************************************/ +/* register_types.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +void register_orca_types(); +void unregister_orca_types(); diff --git a/thirdparty/README.md b/thirdparty/README.md index 3f2fc6d8f92c8..7b1e19f6ebb5f 100644 --- a/thirdparty/README.md +++ b/thirdparty/README.md @@ -454,6 +454,24 @@ Files extracted from upstream source: - License.txt +## rvo2 + +- Upstream: http://gamma.cs.unc.edu/RVO2/ +- version: 2.0.2 +- License: Apache 2.0 + +Files extracted from upstream source: + +- All .cpp and .h files in the `src/` folder except for RVOSimulator.cpp and RVOSimulator.h +- LICENSE +- README + +Important: Some files have Godot-made changes. Specifically: + +- Agent, Obstacle, and KdTree have been made public +- All references to RVOSimulator have been refactored out (introducing new method parameters) +- `Line` class has been moved from RVOSimulator.h to Definitions.h + ## squish - Upstream: https://sourceforge.net/projects/libsquish diff --git a/thirdparty/rvo2/LICENSE b/thirdparty/rvo2/LICENSE new file mode 100644 index 0000000000000..d645695673349 --- /dev/null +++ b/thirdparty/rvo2/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/thirdparty/rvo2/README b/thirdparty/rvo2/README new file mode 100644 index 0000000000000..6bccf4dc3d508 --- /dev/null +++ b/thirdparty/rvo2/README @@ -0,0 +1,18 @@ +RVO2 Library is an easy-to-use C++ implementation of the optimal reciprocal +collision avoidance (ORCA) formulation for +multi-agent simulation. RVO2 Library automatically uses parallelism for +computing the motion of the agents on machines with multiple processors and a +compiler supporting OpenMP . + +Please send all bug reports to . + +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 + + diff --git a/thirdparty/rvo2/src/Agent.cpp b/thirdparty/rvo2/src/Agent.cpp new file mode 100644 index 0000000000000..a3c2d5dd920da --- /dev/null +++ b/thirdparty/rvo2/src/Agent.cpp @@ -0,0 +1,561 @@ +/* + * Agent.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 . + * + * 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 + * + * + */ + +#include "Agent.h" + +#include "KdTree.h" +#include "Obstacle.h" + +namespace RVO { + + Agent::Agent() : maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f), timeHorizonObst_(0.0f), id_(0) { } + + void Agent::computeNeighbors(KdTree *kdTree_) + { + obstacleNeighbors_.clear(); + float rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_); + kdTree_->computeObstacleNeighbors(this, rangeSq); + + agentNeighbors_.clear(); + + if (maxNeighbors_ > 0) { + rangeSq = sqr(neighborDist_); + kdTree_->computeAgentNeighbors(this, rangeSq); + } + } + + /* Search for the best new velocity. */ + void Agent::computeNewVelocity(float timeStep) + { + orcaLines_.clear(); + + const float invTimeHorizonObst = 1.0f / timeHorizonObst_; + + /* Create obstacle ORCA lines. */ + for (size_t i = 0; i < obstacleNeighbors_.size(); ++i) { + + const Obstacle *obstacle1 = obstacleNeighbors_[i].second; + const Obstacle *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 Obstacle *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::infinity() : absSq(velocity_ - (leftCutoff + t * cutoffVec))); + const float distSqLeft = ((tLeft < 0.0f) ? std::numeric_limits::infinity() : absSq(velocity_ - (leftCutoff + tLeft * leftLegDirection))); + const float distSqRight = ((tRight < 0.0f) ? std::numeric_limits::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 Agent *const other = agentNeighbors_[i].second; + + 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 / 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 Agent::insertAgentNeighbor(const Agent *agent, float &rangeSq) + { + if (this != agent) { + 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 Agent::insertObstacleNeighbor(const Obstacle *obstacle, float rangeSq) + { + const Obstacle *const nextObstacle = obstacle->nextObstacle_; + + 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); + } + } + + bool linearProgram1(const std::vector &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 &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 &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 projLines(lines.begin(), lines.begin() + static_cast(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/src/Agent.h b/thirdparty/rvo2/src/Agent.h new file mode 100644 index 0000000000000..9793d542bb336 --- /dev/null +++ b/thirdparty/rvo2/src/Agent.h @@ -0,0 +1,155 @@ +/* + * Agent.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 . + * + * 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 + * + * + */ + +#ifndef RVO_AGENT_H_ +#define RVO_AGENT_H_ + +/** + * \file Agent.h + * \brief Contains the Agent class. + */ + +#include "Definitions.h" +#include "KdTree.h" + +namespace RVO { + /** + * \brief Defines an agent in the simulation. + */ + class Agent { + + // -- GODOT start -- + public: + /** + * \brief Constructs an agent instance. + * \param sim The simulator instance. + */ + explicit Agent(); + // -- GODOT end -- + + /** + * \brief Computes the neighbors of this agent. + */ + void computeNeighbors(KdTree *kdTree_); + + /** + * \brief Computes the new velocity of this agent. + */ + void computeNewVelocity(float timeStep); + + /** + * \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 Agent *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 Obstacle *obstacle, float rangeSq); + + /** + * \brief Updates the two-dimensional position and two-dimensional + * velocity of this agent. + */ + void update(); + + std::vector > agentNeighbors_; + size_t maxNeighbors_; + float maxSpeed_; + float neighborDist_; + Vector2 newVelocity_; + std::vector > obstacleNeighbors_; + std::vector orcaLines_; + Vector2 position_; + Vector2 prefVelocity_; + float radius_; + float timeHorizon_; + float timeHorizonObst_; + Vector2 velocity_; + + size_t id_; + }; + + /** + * \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 &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 &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 &lines, size_t numObstLines, size_t beginLine, + float radius, Vector2 &result); +} + +#endif /* RVO_AGENT_H_ */ diff --git a/thirdparty/rvo2/src/Definitions.h b/thirdparty/rvo2/src/Definitions.h new file mode 100644 index 0000000000000..f961efb784dce --- /dev/null +++ b/thirdparty/rvo2/src/Definitions.h @@ -0,0 +1,124 @@ +/* + * 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 . + * + * 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 + * + * + */ + +#ifndef RVO_DEFINITIONS_H_ +#define RVO_DEFINITIONS_H_ + +/** + * \file Definitions.h + * \brief Contains functions and constants used in multiple classes. + */ + +#include +#include +#include +#include +#include + +#include "Vector2.h" + +/** + * \brief A sufficiently small positive number. + */ +const float RVO_EPSILON = 0.00001f; + +namespace RVO { + class Agent; + class Obstacle; + + /** + * \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 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; + }; + + /** + * \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 /* RVO_DEFINITIONS_H_ */ diff --git a/thirdparty/rvo2/src/KdTree.cpp b/thirdparty/rvo2/src/KdTree.cpp new file mode 100644 index 0000000000000..75d14fe169b8f --- /dev/null +++ b/thirdparty/rvo2/src/KdTree.cpp @@ -0,0 +1,355 @@ +/* + * KdTree.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 . + * + * 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 + * + * + */ + +#include "KdTree.h" + +#include "Agent.h" +#include "Obstacle.h" + +namespace RVO { + KdTree::KdTree() : agents_(NULL), obstacleTree_(NULL) { } + + KdTree::~KdTree() + { + deleteObstacleTree(obstacleTree_); + } + + void KdTree::buildAgentTree(std::vector agents) { + agents_.swap(agents); + if (!agents_.empty()) { + agentTree_.resize(2 * agents_.size() - 1); + buildAgentTreeRecursive(0, agents_.size(), 0); + } + } + + void KdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node) + { + 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 KdTree::buildObstacleTree(std::vector obstacles) + { + deleteObstacleTree(obstacleTree_); + + obstacleTree_ = buildObstacleTreeRecursive(obstacles, obstacles); + } + + + KdTree::ObstacleTreeNode *KdTree::buildObstacleTreeRecursive(const std::vector &obstacles, std::vector root) + { + 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 Obstacle *const obstacleI1 = obstacles[i]; + const Obstacle *const obstacleI2 = obstacleI1->nextObstacle_; + + /* Compute optimal split node. */ + for (size_t j = 0; j < obstacles.size(); ++j) { + if (i == j) { + continue; + } + + const Obstacle *const obstacleJ1 = obstacles[j]; + const Obstacle *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 leftObstacles(minLeft); + std::vector rightObstacles(minRight); + + size_t leftCounter = 0; + size_t rightCounter = 0; + const size_t i = optimalSplit; + + const Obstacle *const obstacleI1 = obstacles[i]; + const Obstacle *const obstacleI2 = obstacleI1->nextObstacle_; + + for (size_t j = 0; j < obstacles.size(); ++j) { + if (i == j) { + continue; + } + + Obstacle *const obstacleJ1 = obstacles[j]; + Obstacle *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_); + + Obstacle * newObstacle = new Obstacle(); + newObstacle->point_ = splitpoint; + newObstacle->prevObstacle_ = obstacleJ1; + newObstacle->nextObstacle_ = obstacleJ2; + newObstacle->isConvex_ = true; + newObstacle->unitDir_ = obstacleJ1->unitDir_; + + + newObstacle->id_ = obstacleJ1->id_; + + root.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, root); + node->right = buildObstacleTreeRecursive(rightObstacles, root); + return node; + } + } + + void KdTree::computeAgentNeighbors(Agent *agent, float &rangeSq) const + { + queryAgentTreeRecursive(agent, rangeSq, 0); + } + + void KdTree::computeObstacleNeighbors(Agent *agent, float rangeSq) const + { + queryObstacleTreeRecursive(agent, rangeSq, obstacleTree_); + } + + void KdTree::deleteObstacleTree(ObstacleTreeNode *node) + { + if (node != NULL) { + deleteObstacleTree(node->left); + deleteObstacleTree(node->right); + delete node; + } + } + + void KdTree::queryAgentTreeRecursive(Agent *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 KdTree::queryObstacleTreeRecursive(Agent *agent, float rangeSq, const ObstacleTreeNode *node) const + { + if (node == NULL) { + return; + } + else { + const Obstacle *const obstacle1 = node->obstacle; + const Obstacle *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 KdTree::queryVisibility(const Vector2 &q1, const Vector2 &q2, float radius) const + { + return queryVisibilityRecursive(q1, q2, radius, obstacleTree_); + } + + bool KdTree::queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2, float radius, const ObstacleTreeNode *node) const + { + if (node == NULL) { + return true; + } + else { + const Obstacle *const obstacle1 = node->obstacle; + const Obstacle *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/src/KdTree.h b/thirdparty/rvo2/src/KdTree.h new file mode 100644 index 0000000000000..2d76e6d1499da --- /dev/null +++ b/thirdparty/rvo2/src/KdTree.h @@ -0,0 +1,198 @@ +/* + * KdTree.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 . + * + * 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 + * + * + */ + +#ifndef RVO_KD_TREE_H_ +#define RVO_KD_TREE_H_ + +/** + * \file KdTree.h + * \brief Contains the KdTree class. + */ + +#include "Definitions.h" + +namespace RVO { + /** + * \brief Defines kd-trees for agents and static obstacles in the + * simulation. + */ + class KdTree { + public: + /** + * \brief Defines an agent kd-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 kd-tree node. + */ + class ObstacleTreeNode { + public: + /** + * \brief The left obstacle tree node. + */ + ObstacleTreeNode *left; + + /** + * \brief The obstacle number. + */ + const Obstacle *obstacle; + + /** + * \brief The right obstacle tree node. + */ + ObstacleTreeNode *right; + }; + + /** + * \brief Constructs a kd-tree instance. + * \param sim The simulator instance. + */ + explicit KdTree(); + + /** + * \brief Destroys this kd-tree instance. + */ + ~KdTree(); + + /** + * \brief Builds an agent kd-tree. + */ + void buildAgentTree(std::vector agents); + + void buildAgentTreeRecursive(size_t begin, size_t end, size_t node); + + /** + * \brief Builds an obstacle kd-tree. + */ + void buildObstacleTree(std::vector obstacles); + + ObstacleTreeNode *buildObstacleTreeRecursive(const std::vector &obstacles, std::vector root); + + /** + * \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(Agent *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(Agent *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(Agent *agent, float &rangeSq, + size_t node) const; + + void queryObstacleTreeRecursive(Agent *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 agents_; + std::vector agentTree_; + ObstacleTreeNode *obstacleTree_; + + static const size_t MAX_LEAF_SIZE = 10; + }; +} + +#endif /* RVO_KD_TREE_H_ */ diff --git a/thirdparty/rvo2/src/Obstacle.cpp b/thirdparty/rvo2/src/Obstacle.cpp new file mode 100644 index 0000000000000..8d6e0e6928367 --- /dev/null +++ b/thirdparty/rvo2/src/Obstacle.cpp @@ -0,0 +1,37 @@ +/* + * Obstacle.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 . + * + * 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 + * + * + */ + +#include "Obstacle.h" + +namespace RVO { + Obstacle::Obstacle() : isConvex_(false), nextObstacle_(NULL), prevObstacle_(NULL), id_(0) { } +} diff --git a/thirdparty/rvo2/src/Obstacle.h b/thirdparty/rvo2/src/Obstacle.h new file mode 100644 index 0000000000000..2adab50f4c2a9 --- /dev/null +++ b/thirdparty/rvo2/src/Obstacle.h @@ -0,0 +1,64 @@ +/* + * Obstacle.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 . + * + * 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 + * + * + */ + +#ifndef RVO_OBSTACLE_H_ +#define RVO_OBSTACLE_H_ + +/** + * \file Obstacle.h + * \brief Contains the Obstacle class. + */ + +#include "Definitions.h" + +namespace RVO { + /** + * \brief Defines static obstacles in the simulation. + */ + class Obstacle { + public: + /** + * \brief Constructs a static obstacle instance. + */ + Obstacle(); + + bool isConvex_; + Obstacle *nextObstacle_; + Vector2 point_; + Obstacle *prevObstacle_; + Vector2 unitDir_; + + size_t id_; + }; +} + +#endif /* RVO_OBSTACLE_H_ */ diff --git a/thirdparty/rvo2/src/RVO.h b/thirdparty/rvo2/src/RVO.h new file mode 100644 index 0000000000000..c17de5779465b --- /dev/null +++ b/thirdparty/rvo2/src/RVO.h @@ -0,0 +1,510 @@ +/* + * RVO.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 . + * + * 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 + * + * + */ + +#ifndef RVO_RVO_H_ +#define RVO_RVO_H_ + +#include "Vector2.h" + +/** + + \file RVO.h + \brief Includes all public headers in the library. + + \namespace RVO + \brief Contains all classes, functions, and constants used in the library. + + \mainpage RVO2 Library + + \author Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, and + Dinesh Manocha + + RVO2 Library is an easy-to-use C++ implementation of the + Optimal Reciprocal Collision Avoidance + (ORCA) formulation for multi-agent simulation. RVO2 Library automatically + uses parallelism for computing the motion of the agents if your machine has + multiple processors and your compiler supports + OpenMP. + + Please follow the following steps to install and use RVO2 Library. + + - \subpage whatsnew + - \subpage building + - \subpage using + - \subpage params + + See the documentation of the RVO::RVOSimulator class for an exhaustive list of + public functions of RVO2 Library. + + RVO2 Library, accompanying example code, and this documentation is + released for educational, research, and non-profit purposes under the following + \subpage terms "terms and conditions". + + + \page whatsnew What Is New in RVO2 Library + + \section localca Local Collision Avoidance + + The main difference between RVO2 Library and %RVO Library 1.x is the + local collision avoidance technique used. RVO2 Library uses + Optimal Reciprocal Collision Avoidance + (ORCA), whereas %RVO Library 1.x uses + Reciprocal Velocity Obstacles (%RVO). For legacy reasons, and since both + techniques are based on the same principles of reciprocal collision avoidance + and relative velocity, we did not change the name of the library. + + A major consequence of the change of local collision avoidance technique is that + the simulation has become much faster in RVO2 Library. ORCA defines + velocity constraints with respect to other agents as half-planes, and an optimal + velocity is efficiently found using (two-dimensional) linear programming. In + contrast, %RVO Library 1.x uses random sampling to find a good velocity. Also, + the behavior of the agents is smoother in RVO2 Library. It is proven + mathematically that ORCA lets the velocity of agents evolve continuously over + time, whereas %RVO Library 1.x occasionally showed oscillations and reciprocal + dances. Furthermore, ORCA provides stronger guarantees with respect to collision + avoidance. + + \section global Global Path Planning + + Local collision avoidance as provided by RVO2 Library should in principle + be accompanied by global path planning that determines the preferred velocity of + each agent in each time step of the simulation. %RVO Library 1.x has a built-in + roadmap infrastructure to guide agents around obstacles to fixed goals. + However, besides roadmaps, other techniques for global planning, such as + navigation fields, cell decompositions, etc. exist. Therefore, RVO2 + Library does not provide global planning infrastructure anymore. Instead, + it is the responsibility of the external application to set the preferred + velocity of each agent ahead of each time step of the simulation. This makes the + library more flexible to use in varying application domains. In one of the + example applications that comes with RVO2 Library, we show how a roadmap + similar to %RVO Library 1.x is used externally to guide the global navigation of + the agents. As a consequence of this change, RVO2 Library does not have a + concept of a "goal position" or "preferred speed" for each + agent, but only relies on the preferred velocities of the agents set by the + external application. + + \section structure Structure of RVO2 Library + + The structure of RVO2 Library is similar to that of %RVO Library 1.x. + Users familiar with %RVO Library 1.x should find little trouble in using RVO2 + Library. However, RVO2 Library is not backwards compatible with %RVO + Library 1.x. The main reason for this is that the ORCA technique requires + different (and fewer) parameters to be set than %RVO. Also, the way obstacles + are represented is different. In %RVO Library 1.x, obstacles are represented by + an arbitrary collection of line segments. In RVO2 Library, obstacles are + non-intersecting polygons, specified by lists of vertices in counterclockwise + order. Further, in %RVO Library 1.x agents cannot be added to the simulation + after the simulation is initialized. In RVO2 Library this restriction is + removed. Obstacles still need to be processed before the simulation starts, + though. Lastly, in %RVO Library 1.x an instance of the simulator is a singleton. + This restriction is removed in RVO2 Library. + + \section smaller Smaller Changes + + With RVO2 Library, we have adopted the philosophy that anything that is + not part of the core local collision avoidance technique is to be stripped from + the library. Therefore, besides the roadmap infrastructure, we have also removed + acceleration constraints of agents, orientation of agents, and the unused + "class" of agents. Each of these can be implemented external of the + library if needed. We did maintain a kd-tree infrastructure for + efficiently finding other agents and obstacles nearby each agent. + + Also, RVO2 Library allows accessing information about the simulation, + such as the neighbors and the collision-avoidance constraints of each agent, + that is hidden from the user in %RVO Library 1.x. + + + \page building Building RVO2 Library + + We assume that you have downloaded RVO2 Library and unpacked the ZIP + archive to a path $RVO_ROOT. + + \section xcode Apple Xcode 3.x + + Open $RVO_ROOT/RVO.xcodeproj and select the %RVO target and + a configuration (Debug or Release). A framework + RVO.framework will be built in the default build directory, e.g. + $RVO_ROOT/build/Release. + + \section cmake CMake + + Create and switch to your chosen build directory, e.g. $RVO_ROOT/build. + Run cmake inside the build directory on the source directory, e.g. + cmake $RVO_ROOT/src. Build files for the default generator for your + platform will be generated in the build directory. + + \section make GNU Make + + Switch to the source directory $RVO_ROOT/src and run make. + Public header files (RVO.h, RVOSimulator.h, and + Vector2.h) will be copied to the $RVO_ROOT/include directory + and a static library libRVO.a will be compiled into the + $RVO_ROOT/lib directory. + + \section visual Microsoft Visual Studio 2008 + + Open $RVO_ROOT/RVO.sln and select the %RVO project and a + configuration (Debug, ReleaseST, or ReleaseMT). + Public header files (RVO.h, RVOSimulator.h, and + Vector2.h) will be copied to the $RVO_ROOT/include directory + and a static library, e.g. RVO.lib, will be compiled into the + $RVO_ROOT/lib directory. + + + \page using Using RVO2 Library + + \section structure Structure + + A program performing an RVO2 Library simulation has the following global + structure. + + \code + #include + + std::vector goals; + + int main() + { + // Create a new simulator instance. + RVO::RVOSimulator* sim = new RVO::RVOSimulator(); + + // Set up the scenario. + setupScenario(sim); + + // Perform (and manipulate) the simulation. + do { + updateVisualization(sim); + setPreferredVelocities(sim); + sim->doStep(); + } while (!reachedGoal(sim)); + + delete sim; + } + \endcode + + In order to use RVO2 Library, the user needs to include RVO.h. The first + step is then to create an instance of RVO::RVOSimulator. Then, the process + consists of two stages. The first stage is specifying the simulation scenario + and its parameters. In the above example program, this is done in the method + setupScenario(...), which we will discuss below. The second stage is the actual + performing of the simulation. + + In the above example program, simulation steps are taken until all + the agents have reached some predefined goals. Prior to each simulation step, + we set the preferred velocity for each agent, i.e. the + velocity the agent would have taken if there were no other agents around, in the + method setPreferredVelocities(...). The simulator computes the actual velocities + of the agents and attempts to follow the preferred velocities as closely as + possible while guaranteeing collision avoidance at the same time. During the + simulation, the user may want to retrieve information from the simulation for + instance to visualize the simulation. In the above example program, this is done + in the method updateVisualization(...), which we will discuss below. It is also + possible to manipulate the simulation during the simulation, for instance by + changing positions, radii, velocities, etc. of the agents. + + \section spec Setting up the Simulation Scenario + + A scenario that is to be simulated can be set up as follows. A scenario consists + of two types of objects: agents and obstacles. Each of them can be manually + specified. Agents may be added anytime before or during the simulation. + Obstacles, however, need to be defined prior to the simulation, and + RVO::RVOSimulator::processObstacles() need to be called in order for the + obstacles to be accounted for in the simulation. + The user may also want to define goal positions of the agents, or a + roadmap to guide the agents around obstacles. This is not done in RVO2 + Library, but needs to be taken care of in the user's external application. + + The following example creates a scenario with four agents exchanging positions + around a rectangular obstacle in the middle. + + \code + void setupScenario(RVO::RVOSimulator* sim) { + // Specify global time step of the simulation. + sim->setTimeStep(0.25f); + + // Specify default parameters for agents that are subsequently added. + sim->setAgentDefaults(15.0f, 10, 10.0f, 5.0f, 2.0f, 2.0f); + + // Add agents, specifying their start position. + sim->addAgent(RVO::Vector2(-50.0f, -50.0f)); + sim->addAgent(RVO::Vector2(50.0f, -50.0f)); + sim->addAgent(RVO::Vector2(50.0f, 50.0f)); + sim->addAgent(RVO::Vector2(-50.0f, 50.0f)); + + // Create goals (simulator is unaware of these). + for (size_t i = 0; i < sim->getNumAgents(); ++i) { + goals.push_back(-sim->getAgentPosition(i)); + } + + // Add (polygonal) obstacle(s), specifying vertices in counterclockwise order. + std::vector vertices; + vertices.push_back(RVO::Vector2(-7.0f, -20.0f)); + vertices.push_back(RVO::Vector2(7.0f, -20.0f)); + vertices.push_back(RVO::Vector2(7.0f, 20.0f)); + vertices.push_back(RVO::Vector2(-7.0f, 20.0f)); + + sim->addObstacle(vertices); + + // Process obstacles so that they are accounted for in the simulation. + sim->processObstacles(); + } + \endcode + + See the documentation on RVO::RVOSimulator for a full overview of the + functionality to specify scenarios. + + \section ret Retrieving Information from the Simulation + + During the simulation, the user can extract information from the simulation for + instance for visualization purposes, or to determine termination conditions of + the simulation. In the example program above, visualization is done in the + updateVisualization(...) method. Below we give an example that simply writes + the positions of each agent in each time step to the standard output. The + termination condition is checked in the reachedGoal(...) method. Here we give an + example that returns true if all agents are within one radius of their goals. + + \code + void updateVisualization(RVO::RVOSimulator* sim) { + // Output the current global time. + std::cout << sim->getGlobalTime() << " "; + + // Output the position for all the agents. + for (size_t i = 0; i < sim->getNumAgents(); ++i) { + std::cout << sim->getAgentPosition(i) << " "; + } + + std::cout << std::endl; + } + \endcode + + \code + bool reachedGoal(RVO::RVOSimulator* sim) { + // Check whether all agents have arrived at their goals. + for (size_t i = 0; i < sim->getNumAgents(); ++i) { + if (absSq(goals[i] - sim->getAgentPosition(i)) > sim->getAgentRadius(i) * sim->getAgentRadius(i)) { + // Agent is further away from its goal than one radius. + return false; + } + } + return true; + } + \endcode + + Using similar functions as the ones used in this example, the user can access + information about other parameters of the agents, as well as the global + parameters, and the obstacles. See the documentation of the class + RVO::RVOSimulator for an exhaustive list of public functions for retrieving + simulation information. + + \section manip Manipulating the Simulation + + During the simulation, the user can manipulate the simulation, for instance by + changing the global parameters, or changing the parameters of the agents + (potentially causing abrupt different behavior). It is also possible to give the + agents a new position, which make them jump through the scene. + New agents can be added to the simulation at any time, but it is not allowed to + add obstacles to the simulation after they have been processed by calling + RVO::RVOSimulator::processObstacles(). Also, it is impossible to change the + position of the vertices of the obstacles. + + See the documentation of the class RVO::RVOSimulator for an exhaustive list of + public functions for manipulating the simulation. + + To provide global guidance to the agents, the preferred velocities of the agents + can be changed ahead of each simulation step. In the above example program, this + happens in the method setPreferredVelocities(...). Here we give an example that + simply sets the preferred velocity to the unit vector towards the agent's goal + for each agent (i.e., the preferred speed is 1.0). Note that this may not give + convincing results with respect to global navigation around the obstacles. For + this a roadmap or other global planning techniques may be used (see one of the + \ref example "example programs" that accompanies RVO2 Library). + + \code + void setPreferredVelocities(RVO::RVOSimulator* sim) { + // Set the preferred velocity for each agent. + for (size_t i = 0; i < sim->getNumAgents(); ++i) { + if (absSq(goals[i] - sim->getAgentPosition(i)) < sim->getAgentRadius(i) * sim->getAgentRadius(i) ) { + // Agent is within one radius of its goal, set preferred velocity to zero + sim->setAgentPrefVelocity(i, RVO::Vector2(0.0f, 0.0f)); + } else { + // Agent is far away from its goal, set preferred velocity as unit vector towards agent's goal. + sim->setAgentPrefVelocity(i, normalize(goals[i] - sim->getAgentPosition(i))); + } + } + } + \endcode + + \section example Example Programs + + RVO2 Library is accompanied by three example programs, which can be found in the + $RVO_ROOT/examples directory. The examples are named ExampleBlocks, ExampleCircle, and + ExampleRoadmap, and contain the following demonstration scenarios: + + + + + + + + + + + + + +
ExampleBlocksA scenario in which 100 agents, split in four groups initially + positioned in each of four corners of the environment, move to the + other side of the environment through a narrow passage generated by four + obstacles. There is no roadmap to guide the agents around the obstacles.
ExampleCircleA scenario in which 250 agents, initially positioned evenly + distributed on a circle, move to the antipodal position on the + circle. There are no obstacles.
ExampleRoadmapThe same scenario as ExampleBlocks, but now the preferred velocities + of the agents are determined using a roadmap guiding the agents around the obstacles.
+ + + \page params Parameter Overview + + \section globalp Global Parameters + + + + + + + + + + + + +
ParameterType (unit)Meaning
timeStepfloat (time)The time step of the simulation. Must be positive.
+ + \section agent Agent Parameters + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
ParameterType (unit)Meaning
maxNeighborssize_tThe maximum number of other agents the 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.
maxSpeedfloat (distance/time)The maximum speed of the agent. Must be non-negative.
neighborDistfloat (distance)The maximum distance (center point to center point) to + other agents the 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.
positionRVO::Vector2 (distance, distance)The current position of the agent.
prefVelocityRVO::Vector2 (distance/time, distance/time) + The current preferred velocity of the agent. This is the + velocity the agent would take if no other agents or + obstacles were around. The simulator computes an actual + velocity for the agent that follows the preferred velocity + as closely as possible, but at the same time guarantees + collision avoidance.
radiusfloat (distance)The radius of the agent. Must be non-negative.
timeHorizonfloat (time)The minimal amount of time for which the 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 the agent has in choosing its velocities. + Must be positive.
timeHorizonObstfloat (time)The minimal amount of time for which the 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 the agent has in choosing its velocities. + Must be positive.
velocityRVO::Vector2 (distance/time, distance/time) + The (current) velocity of the agent.
+ + + \page terms Terms and Conditions + + 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. + + */ + +#endif /* RVO_RVO_H_ */ diff --git a/thirdparty/rvo2/src/Vector2.h b/thirdparty/rvo2/src/Vector2.h new file mode 100644 index 0000000000000..1d9c8f5951c76 --- /dev/null +++ b/thirdparty/rvo2/src/Vector2.h @@ -0,0 +1,332 @@ +/* + * 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 . + * + * 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 + * + * + */ + +#ifndef RVO_VECTOR2_H_ +#define RVO_VECTOR2_H_ + +/** + * \file Vector2.h + * \brief Contains the Vector2 class. + */ + +#include +#include + +namespace RVO { + /** + * \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) { } + + /** + * \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; + } + + 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_ */