Skip to content

Commit

Permalink
Add RVO collision avoidance
Browse files Browse the repository at this point in the history
  • Loading branch information
Nathan Merrill committed Oct 25, 2019
1 parent 763a4d8 commit 1e88985
Show file tree
Hide file tree
Showing 24 changed files with 3,349 additions and 0 deletions.
5 changes: 5 additions & 0 deletions COPYRIGHT.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions SConstruct
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand Down
25 changes: 25 additions & 0 deletions modules/orca/SCsub
View file
Original file line number Diff line number Diff line change
@@ -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")
201 changes: 201 additions & 0 deletions modules/orca/collision_avoidance_2d.cpp
View file
Original file line number Diff line number Diff line change
@@ -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 <Agent.h>
#include <Definitions.h>
#include <KdTree.h>
#include <Obstacle.h>
#include <Vector2.h>

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<Vector2> points = Vector<Vector2>();
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<Vector2> p_obstacle_points) {
remove_obstacle(p_obstacle);
_add_obstacle(p_obstacle_points, p_obstacle);
}

void CollisionAvoidance2D::_add_obstacle(Vector<Vector2> p_obstacle_points, int p_obstacle) {
Vector<RVO::Obstacle *> obstacles = Vector<RVO::Obstacle *>();
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<Vector2> 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<RVO::Obstacle *> 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<RVO::Obstacle *> obstacles = std::vector<RVO::Obstacle *>();
auto o = obstacles_map.front();
while (o != NULL) {
Vector<RVO::Obstacle *> 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<RVO::Agent *>();
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) {
}
77 changes: 77 additions & 0 deletions modules/orca/collision_avoidance_2d.h
View file
Original file line number Diff line number Diff line change
@@ -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 <KdTree.h>
#include <Obstacle.h>

class CollisionAvoidance2D : public Node2D {
GDCLASS(CollisionAvoidance2D, Node2D);

private:
bool processed_obstacles;
Map<int, CollisionAvoidanceActor2D *> actor_map;
Map<int, Vector<RVO::Obstacle *> > obstacles_map;
Rect2 bounds;
RVO::KdTree kd_tree;
int last_obstacle_id;
int last_actor_id;
bool obstacles_dirty;

void _add_obstacle(Vector<Vector2> 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<Vector2> p_obstacle_points);
void move_obstacle(int p_obstacle, Vector<Vector2> 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
Loading

0 comments on commit 1e88985

Please sign in to comment.