├── .gitignore ├── simulation ├── worlds │ ├── multi_agents20_complex.world │ ├── can_population.world │ ├── cross_obstacles.world │ ├── multi_agents.world │ └── multi_agents6.world ├── rvo_sim.gif ├── model │ ├── cross │ │ ├── model.config │ │ └── model.sdf │ └── mossy_block │ │ ├── model.config │ │ └── model.sdf ├── plugins │ ├── CMakeLists.txt │ └── world_multi_obs.cc └── rvo_gazebo_agent.launch ├── srv └── SetGoals.srv ├── configure ├── formation.yaml ├── config.yaml ├── config_diff.yaml ├── config_exp.yaml ├── config_can.yaml └── config_complex.yaml ├── rvo_lib ├── rvo_test.h ├── Obstacle.cpp ├── nav_rvo.h ├── CMakeLists.txt ├── RVO.md ├── Obstacle.h ├── rvo_test.cpp ├── Definitions.h ├── KdTree.h ├── Agent.h ├── nav_rvo.cpp ├── RVOSimulator.cpp ├── Vector2.h ├── KdTree.cpp ├── Agent.cpp └── RVO.h ├── sustech_goal.sh ├── launch ├── rvo_test_can.launch └── rvo_test_cross.launch ├── src ├── rvo_node.h ├── formation.cpp ├── set_goals_client.cpp └── rvo_node.cpp ├── package.xml ├── CMakeLists.txt └── readme.md /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode -------------------------------------------------------------------------------- /simulation/worlds/multi_agents20_complex.world: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /simulation/rvo_sim.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/rvo_ros/HEAD/simulation/rvo_sim.gif -------------------------------------------------------------------------------- /srv/SetGoals.srv: -------------------------------------------------------------------------------- 1 | string model 2 | geometry_msgs/Point[] coordinates 3 | --- 4 | int64 num_goal 5 | 6 | 7 | -------------------------------------------------------------------------------- /configure/formation.yaml: -------------------------------------------------------------------------------- 1 | goals: [ [4, 7], [4, 6], [4, 5], [4, 4], [4, 3], [4, 2], [4, 1], [5, 1], [6, 1], [7, 2], [8, 2] ] -------------------------------------------------------------------------------- /configure/config.yaml: -------------------------------------------------------------------------------- 1 | # parameters of rvo ros 2 | neighborDist: 4 3 | maxNeighbors: 10 4 | timeHorizon: 10 5 | timeHorizonObst: 5 6 | radius: 0.3 7 | maxSpeed: 0.2 8 | goal_threshold: 0.001 -------------------------------------------------------------------------------- /configure/config_diff.yaml: -------------------------------------------------------------------------------- 1 | # parameters of rvo ros 2 | neighborDist: 4 3 | maxNeighbors: 10 4 | timeHorizon: 10 5 | timeHorizonObst: 5 6 | radius: 0.3 7 | maxSpeed: 0.2 8 | goal_threshold: 0.05 -------------------------------------------------------------------------------- /configure/config_exp.yaml: -------------------------------------------------------------------------------- 1 | # parameters of rvo ros for experiment 2 | 3 | neighborDist: 4 4 | maxNeighbors: 10 5 | timeHorizon: 10 6 | timeHorizonObst: 5 7 | radius: 0.2 8 | maxSpeed: 0.2 9 | goal_threshold: 0.02 -------------------------------------------------------------------------------- /configure/config_can.yaml: -------------------------------------------------------------------------------- 1 | # parameters of rvo ros 2 | neighborDist: 4 3 | maxNeighbors: 10 4 | timeHorizon: 10 5 | timeHorizonObst: 5 6 | radius: 0.3 7 | maxSpeed: 0.2 8 | goal_threshold: 0.005 9 | agent_name: 'can1_clone_' -------------------------------------------------------------------------------- /configure/config_complex.yaml: -------------------------------------------------------------------------------- 1 | # parameters of rvo ros 2 | neighborDist: 4 3 | maxNeighbors: 10 4 | timeHorizon: 10 5 | timeHorizonObst: 5 6 | radius: 0.3 7 | maxSpeed: 0.2 8 | goal_threshold: 0.005 9 | agent_name: 'cross_clone_' -------------------------------------------------------------------------------- /simulation/model/cross/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | cross 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /simulation/model/mossy_block/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | block 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /rvo_lib/rvo_test.h: -------------------------------------------------------------------------------- 1 | #ifndef NAV_ROV_H 2 | #define NAV_ROV_H 3 | 4 | #include "RVO.h" 5 | #include "iostream" 6 | 7 | void setupScenario(RVO::RVOSimulator* sim); 8 | void updateVisualization(RVO::RVOSimulator* sim); 9 | bool reachedGoal(RVO::RVOSimulator* sim); 10 | void setPreferredVelocities(RVO::RVOSimulator* sim); 11 | 12 | #endif -------------------------------------------------------------------------------- /simulation/plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(${GAZEBO_INCLUDE_DIRS}) 2 | link_directories(${GAZEBO_LIBRARY_DIRS}) 3 | list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}") 4 | 5 | add_library(multi_obs SHARED world_multi_obs.cc) 6 | target_link_libraries(multi_obs ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES}) 7 | # add_dependencies(multi_obs rvo_ros_gencpp ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 8 | -------------------------------------------------------------------------------- /sustech_goal.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | rosrun rvo_ros set_goals_client default 4 3 5 3 6 3 6 4 6 5 5 5 4 5 4 6 4 7 5 7 6 7 # S 3 | 4 | sleep 10 5 | 6 | rosrun rvo_ros set_goals_client default 4 3 5 3 6 3 6 4 6 5 4 4 4 5 4 6 4 7 6 6 6 7 # U 7 | 8 | sleep 4 9 | 10 | rosrun rvo_ros set_goals_client default 4 3 5 3 6 3 6 4 6 5 5 5 4 5 4 6 4 7 5 7 6 7 # S 11 | 12 | sleep 4 13 | 14 | rosrun rvo_ros set_goals_client default 5 2 5 3 5 4 5 5 5 6 5 7 5 8 3 8 4 8 6 8 7 8 # T 15 | 16 | sleep 6 17 | 18 | rosrun rvo_ros set_goals_client default 4 3 5 3 6 3 4 6 6 5 5 5 4 5 4 4 4 7 5 7 6 7 # E 19 | 20 | sleep 8 21 | 22 | rosrun rvo_ros set_goals_client default 4 3 5 3 6 3 4 6 7 3 7 7 4 5 4 4 4 7 5 7 6 7 # C 23 | 24 | sleep 6 25 | 26 | rosrun rvo_ros set_goals_client default 4 3 5 5 6 3 4 6 6 4 6 5 4 5 4 4 4 7 6 6 6 7 # H 27 | 28 | -------------------------------------------------------------------------------- /launch/rvo_test_can.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /simulation/rvo_gazebo_agent.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /launch/rvo_test_cross.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /simulation/worlds/can_population.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | model://sun 8 | 9 | 10 | 11 | 12 | model://ground_plane 13 | 14 | 15 | 16 | 17 | 18 | 19 | false 20 | model://coke_can 21 | 22 | 23 | 24 | 0 0 0 0 0 0 25 | 26 | 2 2 0.01 27 | 28 | 10 29 | 30 | random 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /src/rvo_node.h: -------------------------------------------------------------------------------- 1 | #ifndef RVO_NODE__GAZEBO_H 2 | #define RVO_NODE__GAZEBO_H 3 | 4 | #include "ros/ros.h" 5 | #include "geometry_msgs/Twist.h" 6 | #include "geometry_msgs/Pose.h" 7 | #include "geometry_msgs/Point.h" 8 | #include "std_msgs/Header.h" 9 | #include "gazebo_msgs/ModelStates.h" 10 | #include "gazebo_msgs/WorldState.h" 11 | #include 12 | #include "../rvo_lib/nav_rvo.h" 13 | #include "rvo_ros/SetGoals.h" 14 | #include "gazebo_msgs/WorldState.h" 15 | #include "gazebo_msgs/ModelStates.h" 16 | #include 17 | #include 18 | 19 | const int num_max = 30; 20 | int num_agent = 0; 21 | int copy_num_agent = 1; 22 | bool arrived = false; 23 | float vel_ratio(float vel, float lo, float hi); 24 | // geometry_msgs::Twist *list_obs_twist = new geometry_msgs::Twist(); 25 | ros::Publisher rvo_node_pub; 26 | gazebo_msgs::WorldState msg_pub; 27 | std::vector rvo_goals; 28 | 29 | RVO::RVOPlanner* rvo; 30 | std::string motion_model = "default"; 31 | 32 | void rvo_velCallback(const gazebo_msgs::ModelStates::ConstPtr& sub_msg); 33 | 34 | bool set_goals(rvo_ros::SetGoals::Request &req, rvo_ros::SetGoals::Response &res); 35 | void rvo_goals_init(); 36 | float limit_goal[4]; 37 | 38 | #endif -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rvo_ros 4 | 0.0.0 5 | The rvo package for robot planning 6 | 7 | hanrobot 8 | 9 | TODO 10 | 11 | catkin 12 | geometry_msgs 13 | std_msgs 14 | nav_msgs 15 | roscpp 16 | rospy 17 | tf 18 | geometry_msgs 19 | nav_msgs 20 | roscpp 21 | rospy 22 | tf 23 | message_generation 24 | message_runtime 25 | 26 | geometry_msgs 27 | std_msgs 28 | nav_msgs 29 | roscpp 30 | rospy 31 | tf 32 | message_runtime 33 | 34 | 35 | -------------------------------------------------------------------------------- /rvo_lib/Obstacle.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Obstacle.cpp 3 | * RVO2 Library 4 | * 5 | * Copyright 2008 University of North Carolina at Chapel Hill 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * Please send all bug reports to . 20 | * 21 | * The authors may be contacted via: 22 | * 23 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 24 | * Dept. of Computer Science 25 | * 201 S. Columbia St. 26 | * Frederick P. Brooks, Jr. Computer Science Bldg. 27 | * Chapel Hill, N.C. 27599-3175 28 | * United States of America 29 | * 30 | * 31 | */ 32 | 33 | #include "Obstacle.h" 34 | #include "RVOSimulator.h" 35 | 36 | namespace RVO { 37 | Obstacle::Obstacle() : isConvex_(false), nextObstacle_(NULL), prevObstacle_(NULL), id_(0) { } 38 | } 39 | -------------------------------------------------------------------------------- /rvo_lib/nav_rvo.h: -------------------------------------------------------------------------------- 1 | #ifndef NAV_ROV_H_ 2 | #define NAV_ROV_H_ 3 | 4 | #include "Agent.h" 5 | #include "KdTree.h" 6 | #include "Definitions.h" 7 | #include "Obstacle.h" 8 | #include "gazebo_msgs/ModelStates.h" 9 | #include "geometry_msgs/Point.h" 10 | #include "RVOSimulator.h" 11 | #include 12 | #include 13 | 14 | namespace RVO { 15 | 16 | class Agent; 17 | class Obstacle; 18 | class KdTree; 19 | 20 | class RVOPlanner{ 21 | public: 22 | RVOPlanner(std::string simulation); 23 | 24 | void setupScenario(float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed); 25 | 26 | void updateState_gazebo(gazebo_msgs::ModelStates::ConstPtr model_msg, std::string agent_name); 27 | 28 | void setGoal(); 29 | void randGoal(const float limit_goal[4], const std::string &model="default"); 30 | void randomOnceGoal(const float limit_goal[4]); 31 | bool arrived(); 32 | void setGoal(std::vector set_goals); 33 | void setInitial(); 34 | void setPreferredVelocities(); 35 | 36 | std::vector step(); 37 | float goal_threshold = 0.03; 38 | 39 | 40 | 41 | private: 42 | 43 | RVO::RVOSimulator* sim; 44 | std::string simulator; 45 | std::vector goals; 46 | bool IfInitial = false; 47 | std::vector newVelocities; 48 | 49 | 50 | friend class Agent; 51 | friend class KdTree; 52 | friend class Obstacle; 53 | }; 54 | } 55 | 56 | #endif -------------------------------------------------------------------------------- /rvo_lib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # src/CMakeLists.txt 3 | # RVO2 Library 4 | # 5 | # Copyright 2008 University of North Carolina at Chapel Hill 6 | # 7 | # Licensed under the Apache License, Version 2.0 (the "License"); 8 | # you may not use this file except in compliance with the License. 9 | # You may obtain a copy of the License at 10 | # 11 | # http://www.apache.org/licenses/LICENSE-2.0 12 | # 13 | # Unless required by applicable law or agreed to in writing, software 14 | # distributed under the License is distributed on an "AS IS" BASIS, 15 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | # See the License for the specific language governing permissions and 17 | # limitations under the License. 18 | # 19 | # Please send all bug reports to . 20 | # 21 | # The authors may be contacted via: 22 | # 23 | # Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 24 | # Dept. of Computer Science 25 | # 201 S. Columbia St. 26 | # Frederick P. Brooks, Jr. Computer Science Bldg. 27 | # Chapel Hill, N.C. 27599-3175 28 | # United States of America 29 | # 30 | # 31 | # 32 | 33 | set(RVO_HEADERS 34 | RVO.h 35 | RVOSimulator.h 36 | Vector2.h) 37 | 38 | set(RVO_SOURCES 39 | Agent.cpp 40 | Agent.h 41 | Definitions.h 42 | KdTree.cpp 43 | KdTree.h 44 | Obstacle.cpp 45 | Obstacle.h 46 | RVOSimulator.cpp) 47 | 48 | add_library(RVO ${RVO_HEADERS} ${RVO_SOURCES}) 49 | 50 | if(WIN32) 51 | set_target_properties(RVO PROPERTIES COMPILE_DEFINITIONS NOMINMAX) 52 | endif() 53 | 54 | install(FILES ${RVO_HEADERS} DESTINATION include) 55 | install(TARGETS RVO DESTINATION lib) 56 | -------------------------------------------------------------------------------- /rvo_lib/RVO.md: -------------------------------------------------------------------------------- 1 | # Optimal Reciprocal Collision Avoidance 2 | 3 | [[paper]](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.162.265&rep=rep1&type=pdf) 4 | 5 | ## Introduction 6 | 7 | The approach for reciprocal n-body collision avoidance, where multiple mobile robots need to avoid collisions with each other while moving in a common workspace. 8 | 9 | Assumption: 10 | 11 | 1. Each robot is assumed to have a simple shape (circular or convex polygon) moving in a two-dimensional workspace. 12 | 2. The robot is holonomic, i.e. it can move in any direction, such that the control input of each robot is simply given by a two-dimensional velocity vector. 13 | 3. Each robot has perfect sensing, and is able to infer the exact shape, position and velocity of obstacles and other robots in the environment. 14 | 15 | Advantage: 16 | 17 | 1. Do not need communication among robots. 18 | 2. Can tackle the static obstacles. 19 | 3. Can guarantee local collision-free motion for a large number of robots in a cluttered workspace. 20 | 21 | Limitation: 22 | 23 | 1. The assumption of perfect sensing is hard to perform in real world because of the uncertainties. 24 | 2. Too many parameters to construct the complex model. 25 | 26 | ## Step 27 | 28 | ### Initialization 29 | 30 | 1. environment: 31 | - time horizon(float): 32 | 2. robot: 33 | - position (vector): current position in 2d environment. 34 | - current velocity(vector, x,y): current velocity respect to x and y axis. 35 | 36 | ### set scenario 37 | 38 | 1. set time step 39 | 2. set default parameter of robots: 40 | - preferred velocity 41 | - 42 | 3. set robot initial position 43 | 4. 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(rvo_ros) 3 | 4 | set(CMAKE_CXX_STANDARD 11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | rospy 9 | geometry_msgs 10 | std_msgs 11 | message_generation 12 | ) 13 | find_package(gazebo REQUIRED) 14 | include_directories( 15 | include 16 | ${catkin_INCLUDE_DIRS} 17 | ) 18 | 19 | link_directories(${catkin_LIBRARY_DIRS}) 20 | 21 | add_service_files( 22 | DIRECTORY srv 23 | FILES SetGoals.srv 24 | ) 25 | 26 | # add_message_files( 27 | # FILES 28 | # rvo_vel.msg 29 | # ) 30 | 31 | generate_messages( 32 | DEPENDENCIES 33 | geometry_msgs 34 | std_msgs 35 | ) 36 | 37 | catkin_package( 38 | # INCLUDE_DIRS include 39 | # LIBRARIES robohan 40 | # CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy tf 41 | CATKIN_DEPENDS roscpp message_runtime geometry_msgs std_msgs 42 | ) 43 | 44 | # file(GLOB RVO_LIB_DIRS "rvo_lib/nav_rvo.cpp") 45 | 46 | add_subdirectory(simulation/plugins) 47 | add_library(rvolib SHARED rvo_lib/nav_rvo.cpp rvo_lib/RVOSimulator.cpp rvo_lib/Agent.cpp rvo_lib/KdTree.cpp rvo_lib/Obstacle.cpp) 48 | 49 | add_executable(rvo_node src/rvo_node.cpp) 50 | target_link_libraries(rvo_node ${catkin_LIBRARIES} rvolib) 51 | add_dependencies(rvo_node ${${PROJECT_NAME}_EXPORTED_TARGETS}) 52 | 53 | add_executable(set_goals_client src/set_goals_client.cpp) 54 | target_link_libraries(set_goals_client ${catkin_LIBRARIES}) 55 | add_dependencies(set_goals_client ${${PROJECT_NAME}_EXPORTED_TARGETS}) 56 | # add_dependencies(set_goals_client ) 57 | 58 | add_executable(formation src/formation.cpp) 59 | target_link_libraries(formation ${catkin_LIBRARIES}) 60 | add_dependencies(formation ${${PROJECT_NAME}_EXPORTED_TARGETS}) 61 | 62 | #target_link_libraries(rvo_test RVO) 63 | -------------------------------------------------------------------------------- /simulation/model/cross/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 0 0 0.048275 0 -0 0 8 | 0.5 9 | 10 | 0.00455435 11 | 0 12 | 0 13 | 0.00455435 14 | 0 15 | 0.00833187 16 | 17 | 18 | 19 | 0 0 0.048275 1.5707 -0 0 20 | 21 | 22 | 0.3162 23 | 0.0483 24 | 25 | 26 | 27 | 28 | 0 0 0.048275 0 1.5707 0 29 | 30 | 31 | 0.3162 32 | 0.0483 33 | 34 | 35 | 36 | 37 | 38 | 39 | model://cross_joint_part/meshes/cross_joint.dae 40 | 41 | 42 | 43 | 48 | 49 | 50 | 51 | -4e-06 -0 0 0 -0 0 52 | 53 | 0 54 | 1 55 | 56 | 57 | -------------------------------------------------------------------------------- /rvo_lib/Obstacle.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Obstacle.h 3 | * RVO2 Library 4 | * 5 | * Copyright 2008 University of North Carolina at Chapel Hill 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * Please send all bug reports to . 20 | * 21 | * The authors may be contacted via: 22 | * 23 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 24 | * Dept. of Computer Science 25 | * 201 S. Columbia St. 26 | * Frederick P. Brooks, Jr. Computer Science Bldg. 27 | * Chapel Hill, N.C. 27599-3175 28 | * United States of America 29 | * 30 | * 31 | */ 32 | 33 | #ifndef RVO_OBSTACLE_H_ 34 | #define RVO_OBSTACLE_H_ 35 | 36 | /** 37 | * \file Obstacle.h 38 | * \brief Contains the Obstacle class. 39 | */ 40 | 41 | #include "Definitions.h" 42 | 43 | namespace RVO { 44 | /** 45 | * \brief Defines static obstacles in the simulation. 46 | */ 47 | class Obstacle { 48 | private: 49 | /** 50 | * \brief Constructs a static obstacle instance. 51 | */ 52 | Obstacle(); 53 | 54 | bool isConvex_; 55 | Obstacle *nextObstacle_; 56 | Vector2 point_; 57 | Obstacle *prevObstacle_; 58 | Vector2 unitDir_; 59 | 60 | size_t id_; 61 | 62 | friend class Agent; 63 | friend class KdTree; 64 | friend class RVOSimulator; 65 | }; 66 | } 67 | 68 | #endif /* RVO_OBSTACLE_H_ */ 69 | -------------------------------------------------------------------------------- /src/formation.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "geometry_msgs/Point.h" 3 | #include "rvo_ros/SetGoals.h" 4 | #include 5 | 6 | int main(int argc, char **argv) 7 | { 8 | ros::init(argc, argv, "set_goals_client"); 9 | ros::NodeHandle n; 10 | ros::ServiceClient client = n.serviceClient("set_rvo_goals"); 11 | 12 | rvo_ros::SetGoals srv; 13 | 14 | srv.request.model = argv[1]; 15 | int args_num = argc - 1; 16 | int coordinate_num = argc - 2; 17 | 18 | std::string goals; 19 | n.getParam("goals", goals); 20 | std::cout< goals; 4 | 5 | int main() 6 | { 7 | // Create a new simulator instance. 8 | RVO::RVOSimulator* sim = new RVO::RVOSimulator(); 9 | 10 | // Set up the scenario. 11 | setupScenario(sim); 12 | 13 | // Perform (and manipulate) the simulation. 14 | do { 15 | updateVisualization(sim); 16 | setPreferredVelocities(sim); 17 | sim->doStep(); 18 | } while (!reachedGoal(sim)); 19 | 20 | delete sim; 21 | } 22 | 23 | void setupScenario(RVO::RVOSimulator* sim){ 24 | sim->setTimeStep(0.25f); 25 | 26 | sim->setAgentDefaults(15.0f, 10, 10.0f, 5.0f, 2.0f, 2.0f); 27 | 28 | sim->addAgent(RVO::Vector2(-50.0f, -50.0f)); 29 | sim->addAgent(RVO::Vector2(50.0f, -50.0f)); 30 | sim->addAgent(RVO::Vector2(50.0f, 50.0f)); 31 | sim->addAgent(RVO::Vector2(-50.0f, 50.0f)); 32 | 33 | for (size_t i=0; i < sim->getNumAgents(); ++i){ 34 | goals.push_back(-sim->getAgentPosition(i)); 35 | } 36 | 37 | std::vector vertices; 38 | 39 | vertices.push_back(RVO::Vector2(-7.0f, -20.0f)); 40 | vertices.push_back(RVO::Vector2(7.0f, -20.0f)); 41 | vertices.push_back(RVO::Vector2(7.0f, 20.0f)); 42 | vertices.push_back(RVO::Vector2(-7.0f, 20.0f)); 43 | 44 | sim->addObstacle(vertices); 45 | 46 | sim->processObstacles(); 47 | } 48 | 49 | void updateVisualization(RVO::RVOSimulator* sim){ 50 | 51 | std::cout<< sim->getGlobalTime()<<""; 52 | 53 | for (size_t i=0; igetNumAgents(); ++i){ 54 | std::cout<getAgentPosition(i)<<""; 55 | } 56 | 57 | std::cout<getNumAgents(); ++i) { 62 | if (absSq(goals[i] - sim->getAgentPosition(i)) > sim->getAgentRadius(i) * sim->getAgentRadius(i)) { 63 | // Agent is further away from its goal than one radius. 64 | return false; 65 | } 66 | } 67 | return true; 68 | } 69 | 70 | void setPreferredVelocities(RVO::RVOSimulator* sim) { 71 | // Set the preferred velocity for each agent. 72 | for (size_t i = 0; i < sim->getNumAgents(); ++i) { 73 | if (absSq(goals[i] - sim->getAgentPosition(i)) < sim->getAgentRadius(i) * sim->getAgentRadius(i) ) { 74 | // Agent is within one radius of its goal, set preferred velocity to zero 75 | sim->setAgentPrefVelocity(i, RVO::Vector2(0.0f, 0.0f)); 76 | } else { 77 | // Agent is far away from its goal, set preferred velocity as unit vector towards agent's goal. 78 | sim->setAgentPrefVelocity(i, normalize(goals[i] - sim->getAgentPosition(i))); 79 | } 80 | } 81 | } 82 | 83 | -------------------------------------------------------------------------------- /simulation/plugins/world_multi_obs.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "gazebo_msgs/ModelStates.h" 9 | #include "gazebo_msgs/WorldState.h" 10 | 11 | namespace gazebo 12 | { 13 | 14 | class ROSControl : public ModelPlugin 15 | { 16 | public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) 17 | { 18 | this->model = _parent; 19 | this->model_name = _parent->GetName(); 20 | 21 | if (!ros::isInitialized()) 22 | { 23 | int argc = 0; 24 | char** argv = NULL; 25 | ros::init(argc, argv, "obstacle_control", ros::init_options::NoSigintHandler); 26 | } 27 | 28 | this->rosNode.reset(new ros::NodeHandle()); 29 | 30 | this->rosSub = this->rosNode->subscribe("/obstacles_velocity", 1000, &ROSControl::obstacleCallback, this); 31 | 32 | // Store the pointer to the model 33 | // Listen to the update event. This event is broadcast every 34 | // simulation iteration. 35 | this->updateConnection = event::Events::ConnectWorldUpdateBegin( 36 | std::bind(&ROSControl::OnUpdate, this)); 37 | // ros::spin(); 38 | } 39 | 40 | // Called by the world update start event 41 | public: void OnUpdate() 42 | { 43 | // Apply a small linear velocity to the model. 44 | this->model->SetLinearVel(ignition::math::Vector3d(this->linear_x, this->linear_y, 0)); 45 | this->model->SetAngularVel(ignition::math::Vector3d(0, 0, 0)); 46 | } 47 | 48 | void obstacleCallback(const gazebo_msgs::WorldState::ConstPtr& message) 49 | { 50 | auto name = this->model_name; 51 | int num = message->name.size(); 52 | 53 | auto iter_name = message->name.begin(); 54 | auto iter_twist = message->twist.begin(); 55 | 56 | for (int i = 0; i < num; i++) 57 | { 58 | if (name == *iter_name) 59 | { 60 | this->linear_x = (*iter_twist).linear.x; 61 | this->linear_y = (*iter_twist).linear.y; 62 | } 63 | else 64 | { 65 | iter_name++; 66 | iter_twist++; 67 | } 68 | } 69 | 70 | } 71 | 72 | // Pointer to the model 73 | private: physics::ModelPtr model; 74 | 75 | // Pointer to the update event connection 76 | private: event::ConnectionPtr updateConnection; 77 | 78 | private: std::unique_ptr rosNode; 79 | 80 | private: float linear_x{}; 81 | private: float linear_y{}; 82 | private: ros::Subscriber rosSub; 83 | private: std::string model_name; 84 | }; 85 | 86 | // Register this plugin with the simulator 87 | GZ_REGISTER_MODEL_PLUGIN(ROSControl) 88 | } -------------------------------------------------------------------------------- /simulation/worlds/cross_obstacles.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | model://sun 8 | 9 | 10 | 11 | 12 | model://ground_plane 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 0 0 0.048275 0 -0 0 21 | 0.5 22 | 23 | 0.00455435 24 | 0 25 | 0 26 | 0.00455435 27 | 0 28 | 0.00833187 29 | 30 | 31 | 32 | 0 0 0.048275 1.5707 -0 0 33 | 34 | 35 | 0.3162 36 | 0.0483 37 | 38 | 39 | 10 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 0 0 0.048275 0 1.5707 0 55 | 56 | 57 | 0.3162 58 | 0.0483 59 | 60 | 61 | 10 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | model://cross_joint_part/meshes/cross_joint.dae 79 | 80 | 81 | 82 | 87 | 88 | 89 | 0 90 | 0 91 | 0 92 | 93 | -4e-06 -0 0 0 -0 0 94 | 95 | 96 | 0 0 0 0 0 0 97 | 98 | 5 5 0.01 99 | 100 | 10 101 | 102 | random 103 | 104 | 105 | 106 | 107 | -------------------------------------------------------------------------------- /rvo_lib/Definitions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Definitions.h 3 | * RVO2 Library 4 | * 5 | * Copyright 2008 University of North Carolina at Chapel Hill 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * Please send all bug reports to . 20 | * 21 | * The authors may be contacted via: 22 | * 23 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 24 | * Dept. of Computer Science 25 | * 201 S. Columbia St. 26 | * Frederick P. Brooks, Jr. Computer Science Bldg. 27 | * Chapel Hill, N.C. 27599-3175 28 | * United States of America 29 | * 30 | * 31 | */ 32 | 33 | #ifndef RVO_DEFINITIONS_H_ 34 | #define RVO_DEFINITIONS_H_ 35 | 36 | /** 37 | * \file Definitions.h 38 | * \brief Contains functions and constants used in multiple classes. 39 | */ 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include "Vector2.h" 48 | 49 | /** 50 | * \brief A sufficiently small positive number. 51 | */ 52 | const float RVO_EPSILON = 0.00001f; 53 | 54 | namespace RVO { 55 | class Agent; 56 | class Obstacle; 57 | class RVOSimulator; 58 | 59 | /** 60 | * \brief Computes the squared distance from a line segment with the 61 | * specified endpoints to a specified point. 62 | * \param a The first endpoint of the line segment. 63 | * \param b The second endpoint of the line segment. 64 | * \param c The point to which the squared distance is to 65 | * be calculated. 66 | * \return The squared distance from the line segment to the point. 67 | */ 68 | inline float distSqPointLineSegment(const Vector2 &a, const Vector2 &b, 69 | const Vector2 &c) 70 | { 71 | const float r = ((c - a) * (b - a)) / absSq(b - a); 72 | 73 | if (r < 0.0f) { 74 | return absSq(c - a); 75 | } 76 | else if (r > 1.0f) { 77 | return absSq(c - b); 78 | } 79 | else { 80 | return absSq(c - (a + r * (b - a))); 81 | } 82 | } 83 | 84 | /** 85 | * \brief Computes the signed distance from a line connecting the 86 | * specified points to a specified point. 87 | * \param a The first point on the line. 88 | * \param b The second point on the line. 89 | * \param c The point to which the signed distance is to 90 | * be calculated. 91 | * \return Positive when the point c lies to the left of the line ab. 92 | */ 93 | inline float leftOf(const Vector2 &a, const Vector2 &b, const Vector2 &c) 94 | { 95 | return det(a - c, b - a); 96 | } 97 | 98 | /** 99 | * \brief Computes the square of a float. 100 | * \param a The float to be squared. 101 | * \return The square of the float. 102 | */ 103 | inline float sqr(float a) 104 | { 105 | return a * a; 106 | } 107 | } 108 | 109 | #endif /* RVO_DEFINITIONS_H_ */ 110 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # reciprocal velocity obstacle ros library 2 | 3 | ## Introduction 4 | 5 | This ros package is derived from the ORCA library ([lib](http://gamma.cs.unc.edu/RVO2/)). 6 | 7 | ![](https://github.com/hanruihua/rvo_ros/blob/master/simulation/rvo_sim.gif) 8 | 9 | ## Environment 10 | 11 | - Ubuntu 18.04 12 | - ros Melodic 13 | 14 | ## Install 15 | 16 | > git clone https://github.com/hanruihua/rvo_ros.git 17 | > cd ~/catkin_ws 18 | > catkin_make 19 | 20 | ## set environment parameter 21 | 22 | > export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:~/catkin_ws/devel/lib 23 | 24 | Please write this line in the file .bashrc or .zshrc 25 | 26 | ## Usage 27 | 28 | > rosrun rvo_ros rvo_node args 29 | 30 | args: the coordinates of init point. default 0,1 0,2 ...0 10 31 | 32 | for example: 33 | 34 | > rosrun rvo_ros rvo_node 0 1 0 2 0 3 35 | 36 | ## test with simulation 37 | 38 | > roslaunch rvo_ros rvo_gazebo_agent.launch 39 | 40 | **Note**: Using service to set the model and goals. 41 | 42 | ## Service 43 | 44 | > rosrun rvo_ros set_goals_client 45 | 46 | - arguments: 47 | - model: 48 | - "default": specify a series of point as goals for the agents. The number of goals should be same as the number of agents: 1 1 2 3 4 2. 49 | - "random": allocate the goals randomly with limit along x and y, only for number: min_x, max_x, min_y, max_y. 50 | - "circle": allocate the goals with circle shape: circle_point_x, circle_point_y, radius, flag. flag is to set the reverse mode 51 | 52 | - example: 53 | >rosrun rvo_ros set_goals_client default 1 1 1 4 4 4 4 1 54 | >rosrun rvo_ros set_goals_client random 0 5 1 4 55 | >rosrun rvo_ros set_goals_client circle 4 4 4 0 56 | 57 | ## Topics 58 | 59 | - Subscribed Topic 60 | 61 | /rvo/model_states ([gazebo_msgs/ModelStates](http://docs.ros.org/jade/api/gazebo_msgs/html/msg/ModelStates.html)) 62 | 63 | **attention**: To avoid the model confusion, only the model name which is like the 'agent+num' style, for example, agent1, agent2, can be regarded as the agent model. 64 | 65 | - Published Topic 66 | 67 | /rvo_vel ([gazebo_msgs/WorldStates](http://docs.ros.org/jade/api/gazebo_msgs/html/msg/WorldState.html)) 68 | 69 | **Note**: only the speed in x, y direction of each agent calculated from the rvo are set in the WorldStates twist part. 70 | 71 | 72 | ## author 73 | 74 | **Han** - [Han](https://github.com/hanruihua) 75 | 76 | ## License 77 | 78 | This project is licensed under the MIT License 79 | 80 | ## Overview ([[paper]](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.162.265&rep=rep1&type=pdf)) 81 | 82 | The approach for reciprocal n-body collision avoidance, where multiple mobile robots need to avoid collisions with each other while moving in a common workspace. 83 | 84 | Assumption: 85 | 86 | 1. Each robot is assumed to have a simple shape (circular or convex polygon) moving in a two-dimensional workspace. 87 | 2. The robot is holonomic, i.e. it can move in any direction, such that the control input of each robot is simply given by a two-dimensional velocity vector. 88 | 3. Each robot has perfect sensing, and is able to infer the exact shape, position and velocity of obstacles and other robots in the environment. 89 | 90 | Advantage: 91 | 92 | 1. Do not need communication among robots. 93 | 2. Can tackle the static obstacles. 94 | 3. Can guarantee local collision-free motion for a large number of robots in a cluttered workspace. 95 | 96 | Limitation: 97 | 98 | 1. The assumption of perfect sensing is hard to perform in real world because of the uncertainties. 99 | 2. Too many parameters to construct the complex model. 100 | 101 | 102 | 103 | 104 | 105 | 106 | -------------------------------------------------------------------------------- /src/set_goals_client.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "geometry_msgs/Point.h" 3 | #include "rvo_ros/SetGoals.h" 4 | 5 | 6 | int main(int argc, char **argv) 7 | { 8 | ros::init(argc, argv, "set_goals_client"); 9 | ros::NodeHandle n; 10 | ros::ServiceClient client = n.serviceClient("set_rvo_goals"); 11 | 12 | rvo_ros::SetGoals srv; 13 | 14 | srv.request.model = argv[1]; 15 | int args_num = argc - 1; 16 | int coordinate_num = argc - 2; 17 | 18 | std::cout<<"arguments input:"< 2 | 3 | 4 | 5 | 6 | 7 | 0 0 0.1016 0 -0 0 8 | 5 9 | 10 | 0.028705 11 | 0 12 | 0.073708 13 | 0 14 | 0 15 | 0.06908 16 | 17 | 18 | 19 | 20 | 21 | 1.1 1.1 1.1 22 | https://fuel.ignitionrobotics.org/1.0/yuhang/models/mossy_cinder_block/1/files/meshes/mossy_cinder_block.dae 23 | 24 | 25 | 26 | 27 | 0 -0.25395 0.3048 0 -0 0 28 | 29 | 30 | 1.30485 0.1017 0.6096 31 | 32 | 33 | 34 | 35 | 36 | 0.1 37 | 0.001 38 | 39 | 40 | 41 | 42 | 1 43 | 1 44 | 45 | 46 | 47 | 48 | 49 | 0 0.25395 0.3048 0 -0 0 50 | 51 | 52 | 1.30485 0.1017 0.6096 53 | 54 | 55 | 56 | 57 | 58 | 0.1 59 | 0.001 60 | 61 | 62 | 63 | 64 | 1 65 | 1 66 | 67 | 68 | 69 | 70 | 71 | 0.55875 0 0.3048 0 -0 0 72 | 73 | 74 | 0.1017 0.4062 0.609 75 | 76 | 77 | 78 | 79 | 80 | 0.1 81 | 0.001 82 | 83 | 84 | 85 | 86 | 1 87 | 1 88 | 89 | 90 | 91 | 92 | 93 | 0 0 0.3048 0 -0 0 94 | 95 | 96 | 0.1017 0.4062 0.609 97 | 98 | 99 | 100 | 101 | 102 | 0.1 103 | 0.001 104 | 105 | 106 | 107 | 108 | 1 109 | 1 110 | 111 | 112 | 113 | 114 | 115 | -0.55875 0 0.3048 0 -0 0 116 | 117 | 118 | 0.1017 0.4062 0.609 119 | 120 | 121 | 122 | 123 | 124 | 0.1 125 | 0.001 126 | 127 | 128 | 129 | 130 | 1 131 | 1 132 | 133 | 134 | 135 | 136 | 137 | -2.46537 5.40454 0 0 -0 0 138 | 139 | 0 140 | 1 141 | 142 | 143 | -------------------------------------------------------------------------------- /rvo_lib/KdTree.h: -------------------------------------------------------------------------------- 1 | /* 2 | * KdTree.h 3 | * RVO2 Library 4 | * 5 | * Copyright 2008 University of North Carolina at Chapel Hill 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * Please send all bug reports to . 20 | * 21 | * The authors may be contacted via: 22 | * 23 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 24 | * Dept. of Computer Science 25 | * 201 S. Columbia St. 26 | * Frederick P. Brooks, Jr. Computer Science Bldg. 27 | * Chapel Hill, N.C. 27599-3175 28 | * United States of America 29 | * 30 | * 31 | */ 32 | 33 | #ifndef RVO_KD_TREE_H_ 34 | #define RVO_KD_TREE_H_ 35 | 36 | /** 37 | * \file KdTree.h 38 | * \brief Contains the KdTree class. 39 | */ 40 | 41 | #include "Definitions.h" 42 | 43 | namespace RVO { 44 | /** 45 | * \brief Defines kd-trees for agents and static obstacles in the 46 | * simulation. 47 | */ 48 | class KdTree { 49 | private: 50 | /** 51 | * \brief Defines an agent kd-tree node. 52 | */ 53 | class AgentTreeNode { 54 | public: 55 | /** 56 | * \brief The beginning node number. 57 | */ 58 | size_t begin; 59 | 60 | /** 61 | * \brief The ending node number. 62 | */ 63 | size_t end; 64 | 65 | /** 66 | * \brief The left node number. 67 | */ 68 | size_t left; 69 | 70 | /** 71 | * \brief The maximum x-coordinate. 72 | */ 73 | float maxX; 74 | 75 | /** 76 | * \brief The maximum y-coordinate. 77 | */ 78 | float maxY; 79 | 80 | /** 81 | * \brief The minimum x-coordinate. 82 | */ 83 | float minX; 84 | 85 | /** 86 | * \brief The minimum y-coordinate. 87 | */ 88 | float minY; 89 | 90 | /** 91 | * \brief The right node number. 92 | */ 93 | size_t right; 94 | }; 95 | 96 | /** 97 | * \brief Defines an obstacle kd-tree node. 98 | */ 99 | class ObstacleTreeNode { 100 | public: 101 | /** 102 | * \brief The left obstacle tree node. 103 | */ 104 | ObstacleTreeNode *left; 105 | 106 | /** 107 | * \brief The obstacle number. 108 | */ 109 | const Obstacle *obstacle; 110 | 111 | /** 112 | * \brief The right obstacle tree node. 113 | */ 114 | ObstacleTreeNode *right; 115 | }; 116 | 117 | /** 118 | * \brief Constructs a kd-tree instance. 119 | * \param sim The simulator instance. 120 | */ 121 | explicit KdTree(RVOSimulator *sim); 122 | 123 | /** 124 | * \brief Destroys this kd-tree instance. 125 | */ 126 | ~KdTree(); 127 | 128 | /** 129 | * \brief Builds an agent kd-tree. 130 | */ 131 | void buildAgentTree(); 132 | 133 | void buildAgentTreeRecursive(size_t begin, size_t end, size_t node); 134 | 135 | /** 136 | * \brief Builds an obstacle kd-tree. 137 | */ 138 | void buildObstacleTree(); 139 | 140 | ObstacleTreeNode *buildObstacleTreeRecursive(const std::vector & 141 | obstacles); 142 | 143 | /** 144 | * \brief Computes the agent neighbors of the specified agent. 145 | * \param agent A pointer to the agent for which agent 146 | * neighbors are to be computed. 147 | * \param rangeSq The squared range around the agent. 148 | */ 149 | void computeAgentNeighbors(Agent *agent, float &rangeSq) const; 150 | 151 | /** 152 | * \brief Computes the obstacle neighbors of the specified agent. 153 | * \param agent A pointer to the agent for which obstacle 154 | * neighbors are to be computed. 155 | * \param rangeSq The squared range around the agent. 156 | */ 157 | void computeObstacleNeighbors(Agent *agent, float rangeSq) const; 158 | 159 | /** 160 | * \brief Deletes the specified obstacle tree node. 161 | * \param node A pointer to the obstacle tree node to be 162 | * deleted. 163 | */ 164 | void deleteObstacleTree(ObstacleTreeNode *node); 165 | 166 | void queryAgentTreeRecursive(Agent *agent, float &rangeSq, 167 | size_t node) const; 168 | 169 | void queryObstacleTreeRecursive(Agent *agent, float rangeSq, 170 | const ObstacleTreeNode *node) const; 171 | 172 | /** 173 | * \brief Queries the visibility between two points within a 174 | * specified radius. 175 | * \param q1 The first point between which visibility is 176 | * to be tested. 177 | * \param q2 The second point between which visibility is 178 | * to be tested. 179 | * \param radius The radius within which visibility is to be 180 | * tested. 181 | * \return True if q1 and q2 are mutually visible within the radius; 182 | * false otherwise. 183 | */ 184 | bool queryVisibility(const Vector2 &q1, const Vector2 &q2, 185 | float radius) const; 186 | 187 | bool queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2, 188 | float radius, 189 | const ObstacleTreeNode *node) const; 190 | 191 | std::vector agents_; 192 | std::vector agentTree_; 193 | ObstacleTreeNode *obstacleTree_; 194 | RVOSimulator *sim_; 195 | 196 | static const size_t MAX_LEAF_SIZE = 10; 197 | 198 | friend class Agent; 199 | friend class RVOSimulator; 200 | friend class RVOPlanner; 201 | }; 202 | } 203 | 204 | #endif /* RVO_KD_TREE_H_ */ 205 | -------------------------------------------------------------------------------- /rvo_lib/Agent.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Agent.h 3 | * RVO2 Library 4 | * 5 | * Copyright 2008 University of North Carolina at Chapel Hill 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * Please send all bug reports to . 20 | * 21 | * The authors may be contacted via: 22 | * 23 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 24 | * Dept. of Computer Science 25 | * 201 S. Columbia St. 26 | * Frederick P. Brooks, Jr. Computer Science Bldg. 27 | * Chapel Hill, N.C. 27599-3175 28 | * United States of America 29 | * 30 | * 31 | */ 32 | 33 | #ifndef RVO_AGENT_H_ 34 | #define RVO_AGENT_H_ 35 | 36 | /** 37 | * \file Agent.h 38 | * \brief Contains the Agent class. 39 | */ 40 | 41 | #include "Definitions.h" 42 | #include "RVOSimulator.h" 43 | #include "nav_rvo.h" 44 | 45 | 46 | namespace RVO { 47 | 48 | // class Line { 49 | // public: 50 | // /** 51 | // * \brief A point on the directed line. 52 | // */ 53 | // Vector2 point; 54 | 55 | // /** 56 | // * \brief The direction of the directed line. 57 | // */ 58 | // Vector2 direction; 59 | // }; 60 | 61 | /** 62 | * \brief Defines an agent in the simulation. 63 | */ 64 | class Agent { 65 | private: 66 | /** 67 | * \brief Constructs an agent instance. 68 | * \param sim The simulator instance. 69 | */ 70 | explicit Agent(RVOSimulator *sim); 71 | 72 | /** 73 | * \brief Computes the neighbors of this agent. 74 | */ 75 | void computeNeighbors(); 76 | 77 | /** 78 | * \brief Computes the new velocity of this agent. 79 | */ 80 | void computeNewVelocity(); 81 | 82 | /** 83 | * \brief Inserts an agent neighbor into the set of neighbors of 84 | * this agent. 85 | * \param agent A pointer to the agent to be inserted. 86 | * \param rangeSq The squared range around this agent. 87 | */ 88 | void insertAgentNeighbor(const Agent *agent, float &rangeSq); 89 | 90 | /** 91 | * \brief Inserts a static obstacle neighbor into the set of neighbors 92 | * of this agent. 93 | * \param obstacle The number of the static obstacle to be 94 | * inserted. 95 | * \param rangeSq The squared range around this agent. 96 | */ 97 | void insertObstacleNeighbor(const Obstacle *obstacle, float rangeSq); 98 | 99 | /** 100 | * \brief Updates the two-dimensional position and two-dimensional 101 | * velocity of this agent. 102 | */ 103 | void update(); 104 | 105 | void update_gazebo(Vector2 position_gazebo); 106 | 107 | std::vector > agentNeighbors_; 108 | size_t maxNeighbors_; 109 | float maxSpeed_; 110 | float neighborDist_; 111 | Vector2 newVelocity_; 112 | std::vector > obstacleNeighbors_; 113 | std::vector orcaLines_; 114 | Vector2 position_; 115 | Vector2 prefVelocity_; 116 | float radius_; 117 | RVOSimulator *sim_; 118 | float timeHorizon_; 119 | float timeHorizonObst_; 120 | Vector2 velocity_; 121 | 122 | 123 | size_t id_; 124 | 125 | friend class KdTree; 126 | friend class RVOSimulator; 127 | friend class RVOPlanner; 128 | friend class Line; 129 | }; 130 | 131 | /** 132 | * \relates Agent 133 | * \brief Solves a one-dimensional linear program on a specified line 134 | * subject to linear constraints defined by lines and a circular 135 | * constraint. 136 | * \param lines Lines defining the linear constraints. 137 | * \param lineNo The specified line constraint. 138 | * \param radius The radius of the circular constraint. 139 | * \param optVelocity The optimization velocity. 140 | * \param directionOpt True if the direction should be optimized. 141 | * \param result A reference to the result of the linear program. 142 | * \return True if successful. 143 | */ 144 | bool linearProgram1(const std::vector &lines, size_t lineNo, 145 | float radius, const Vector2 &optVelocity, 146 | bool directionOpt, Vector2 &result); 147 | 148 | /** 149 | * \relates Agent 150 | * \brief Solves a two-dimensional linear program subject to linear 151 | * constraints defined by lines and a circular constraint. 152 | * \param lines Lines defining the linear constraints. 153 | * \param radius The radius of the circular constraint. 154 | * \param optVelocity The optimization velocity. 155 | * \param directionOpt True if the direction should be optimized. 156 | * \param result A reference to the result of the linear program. 157 | * \return The number of the line it fails on, and the number of lines if successful. 158 | */ 159 | size_t linearProgram2(const std::vector &lines, float radius, 160 | const Vector2 &optVelocity, bool directionOpt, 161 | Vector2 &result); 162 | 163 | /** 164 | * \relates Agent 165 | * \brief Solves a two-dimensional linear program subject to linear 166 | * constraints defined by lines and a circular constraint. 167 | * \param lines Lines defining the linear constraints. 168 | * \param numObstLines Count of obstacle lines. 169 | * \param beginLine The line on which the 2-d linear program failed. 170 | * \param radius The radius of the circular constraint. 171 | * \param result A reference to the result of the linear program. 172 | */ 173 | void linearProgram3(const std::vector &lines, size_t numObstLines, size_t beginLine, 174 | float radius, Vector2 &result); 175 | } 176 | 177 | #endif /* RVO_AGENT_H_ */ 178 | -------------------------------------------------------------------------------- /rvo_lib/nav_rvo.cpp: -------------------------------------------------------------------------------- 1 | #include "nav_rvo.h" 2 | 3 | #include 4 | 5 | namespace RVO 6 | { 7 | 8 | RVOPlanner::RVOPlanner(std::string simulation) : simulator(std::move(simulation)) 9 | { 10 | sim = new RVO::RVOSimulator(); 11 | }; 12 | 13 | void RVOPlanner::setupScenario(float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed) 14 | { 15 | sim->setAgentDefaults(neighborDist, maxNeighbors, timeHorizon, timeHorizonObst, radius, maxSpeed); 16 | } 17 | 18 | // set the goal manually 19 | // default: invert direction 20 | // random: change the goal every step 21 | 22 | void RVOPlanner::setGoal() 23 | { 24 | for (size_t i = 0; i < sim->getNumAgents(); ++i) 25 | { 26 | if (absSq(goals[i] - sim->getAgentPosition(i)) < goal_threshold) 27 | { 28 | // goals[i] = -sim->getAgentPosition(i); 29 | goals[i] = -goals[i]; 30 | } 31 | } 32 | } 33 | 34 | void RVOPlanner::setGoal(std::vector set_goals) 35 | { 36 | goals.clear(); 37 | int num_agent = sim->agents_.size(); 38 | if (set_goals.size() < num_agent) 39 | std::cout << "error:The num of goals is less than agents" << std::endl; 40 | else 41 | { 42 | for (int i = 0; i < num_agent; i++) 43 | { 44 | float x = set_goals[i].x; 45 | float y = set_goals[i].y; 46 | 47 | goals.emplace_back(Vector2(x, y)); 48 | std::cout<<"goal"+ std::to_string(i+1) + ":["< ux(x_min, x_max); 64 | std::uniform_real_distribution uy(y_min, y_max); 65 | std::uniform_int_distribution ur(0, 10); 66 | 67 | for (size_t i = 0; i < sim->getNumAgents(); ++i) 68 | { 69 | 70 | float x = ux(e); 71 | float y = uy(e); 72 | int rand = ur(e); 73 | 74 | goals[i] = (Vector2(x, y)); 75 | 76 | std::cout<< "random once successfully" < ux(x_min, x_max); 92 | std::uniform_real_distribution uy(y_min, y_max); 93 | std::uniform_int_distribution ur(0, 10); 94 | 95 | for (size_t i = 0; i < sim->getNumAgents(); ++i) 96 | { 97 | 98 | float x = ux(e); 99 | float y = uy(e); 100 | int rand = ur(e); 101 | 102 | if (!IfInitial) 103 | goals.emplace_back(x, y); 104 | 105 | else if (model == "default") 106 | { 107 | if (absSq(goals[i] - sim->getAgentPosition(i)) < goal_threshold) 108 | goals[i] = (Vector2(x, y)); 109 | } 110 | else if (model == "random") 111 | { 112 | if (rand > 8) 113 | goals[i] = (Vector2(x, y)); 114 | } 115 | } 116 | } 117 | 118 | bool RVOPlanner::arrived() 119 | { 120 | bool reach = true; 121 | 122 | for (size_t i = 0; i < sim->getNumAgents(); ++i) 123 | { 124 | if (absSq(goals[i] - sim->getAgentPosition(i)) >= goal_threshold) 125 | reach = false; 126 | } 127 | 128 | return reach; 129 | } 130 | 131 | 132 | 133 | void RVOPlanner::setInitial() 134 | { 135 | IfInitial = (!goals.empty()) && (!sim->agents_.empty()); 136 | } 137 | 138 | void RVOPlanner::setPreferredVelocities() 139 | { 140 | for (size_t i = 0; i < sim->getNumAgents(); ++i) 141 | { 142 | if (absSq(goals[i] - sim->getAgentPosition(i)) < goal_threshold) 143 | { 144 | // Agent is within one radius of its goal, set preferred velocity to zero 145 | sim->setAgentPrefVelocity(i, RVO::Vector2(0.0f, 0.0f)); 146 | } 147 | else 148 | { 149 | sim->setAgentPrefVelocity(i, normalize(goals[i] - sim->getAgentPosition(i))); 150 | } 151 | } 152 | } 153 | 154 | void RVOPlanner::updateState_gazebo(gazebo_msgs::ModelStates::ConstPtr model_msg, std::string agent_name) 155 | { 156 | if (simulator == "gazebo") 157 | { 158 | auto models_name = model_msg->name; 159 | int num = models_name.size(); 160 | int count = 0; 161 | 162 | // sim->agents_.clear(); 163 | 164 | for (int i = 0; i < num; i++) 165 | { 166 | // std::string full_agent_name = agent_name + std::to_string(i+1); 167 | std::string full_agent_name = agent_name + std::to_string(i); 168 | 169 | auto iter_agent = std::find(models_name.begin(), models_name.end(), full_agent_name); 170 | int agent_index = iter_agent - models_name.begin(); 171 | 172 | if (iter_agent != models_name.end()) 173 | { 174 | float obs_x = model_msg->pose[agent_index].position.x; 175 | float obs_y = model_msg->pose[agent_index].position.y; 176 | float vel_x = model_msg->twist[agent_index].linear.x; 177 | float vel_y = model_msg->twist[agent_index].linear.y; 178 | 179 | if (IfInitial) 180 | { 181 | sim->agents_[count]->position_ = RVO::Vector2(obs_x, obs_y); 182 | sim->agents_[count]->velocity_ = RVO::Vector2(vel_x, vel_y); 183 | } 184 | 185 | else 186 | sim->addAgent(RVO::Vector2(obs_x, obs_y)); 187 | 188 | count++; 189 | // sim->agents_[i]->quater = model_msg->pose[agent_index].orientation; 190 | } 191 | } 192 | } 193 | else 194 | std::cout << "error: please check the simulator" << std::endl; 195 | } 196 | 197 | std::vector RVOPlanner::step() 198 | { 199 | sim->kdTree_->buildAgentTree(); 200 | newVelocities.clear(); 201 | 202 | for (auto & agent : sim->agents_) 203 | { 204 | agent->computeNeighbors(); 205 | agent->computeNewVelocity(); 206 | auto *new_vel = new Vector2(agent->newVelocity_.x(), agent->newVelocity_.y()); 207 | newVelocities.push_back(new_vel); 208 | } 209 | 210 | return newVelocities; 211 | } 212 | 213 | }; // namespace RVO -------------------------------------------------------------------------------- /src/rvo_node.cpp: -------------------------------------------------------------------------------- 1 | #include "rvo_node.h" 2 | 3 | uint64_t seq = 0; 4 | std::string agent_name; 5 | 6 | int main(int argc, char **argv) 7 | { 8 | 9 | ros::init(argc, argv, "rvo_node"); 10 | ros::NodeHandle n; 11 | rvo_node_pub = n.advertise("rvo_vel", 1000); 12 | ros::Subscriber sub = n.subscribe("/rvo/model_states", 100, rvo_velCallback); 13 | ros::ServiceServer service = n.advertiseService("set_rvo_goals", set_goals); 14 | ros::Rate loop_rate(50); 15 | 16 | if ((argc > 1) && (argc % 2 == 1)) 17 | { 18 | int num_init_point = argc - 1; 19 | for (int i = 1; i < num_init_point + 1; i = i + 2) 20 | { 21 | geometry_msgs::Point point; 22 | point.x = atof(argv[i]); 23 | point.y = atof(argv[i + 1]); 24 | rvo_goals.push_back(point); 25 | } 26 | } 27 | else 28 | { 29 | ROS_INFO("No input, Using default position 0 1 0 2 ....0 10 "); 30 | } 31 | 32 | double neighborDist, maxNeighbors, timeHorizon, timeHorizonObst, radius, maxSpeed, goal_threshold; 33 | 34 | n.param("neighborDist", neighborDist, 4); 35 | n.param("maxNeighbors", maxNeighbors, 10); 36 | n.param("timeHorizon", timeHorizon, 10); 37 | n.param("timeHorizonObst", timeHorizonObst, 5); 38 | n.param("radius", radius, 0.3); 39 | n.param("maxSpeed", maxSpeed, 0.2); 40 | n.param("goal_threshold", goal_threshold, 0.01); 41 | n.param("agent_name", agent_name, "agent"); 42 | 43 | rvo = new RVO::RVOPlanner("gazebo"); 44 | 45 | rvo->goal_threshold = goal_threshold; 46 | rvo->setupScenario(neighborDist, maxNeighbors, timeHorizon, timeHorizonObst, radius, maxSpeed); // for exp 47 | // rvo->setupScenario(4.0f, 10, 10.0f, 5.0f, 0.3f, 0.2f); //for gazebo 48 | rvo_goals_init(); 49 | 50 | std::cout<<"Configure completely"<randomOnceGoal(limit_goal); 176 | // std::cout << "Current number of agent: " << num_agent << std::endl; 177 | return true; 178 | } 179 | } 180 | 181 | std::cout << "The specific model is wrong" << std::endl; 182 | return false; 183 | } 184 | 185 | void rvo_goals_init() 186 | { 187 | 188 | if (rvo_goals.empty()) 189 | { 190 | for (int i = 0; i < num_max; i++) 191 | { 192 | geometry_msgs::Point point; 193 | point.x = float(i); 194 | point.y = 1.0; 195 | rvo_goals.push_back(point); 196 | } 197 | } 198 | } 199 | 200 | void rvo_velCallback(const gazebo_msgs::ModelStates::ConstPtr &sub_msg) 201 | { 202 | // std::cout<updateState_gazebo(sub_msg, agent_name); // read the message 206 | if (motion_model == "default") 207 | rvo->setGoal(rvo_goals); 208 | else if (motion_model == "random") 209 | rvo->randGoal(limit_goal, "default"); 210 | else if (motion_model == "circle") 211 | rvo->setGoal(rvo_goals); 212 | else if (motion_model == "circle_spin") 213 | rvo->setGoal(rvo_goals); 214 | 215 | rvo->setInitial(); 216 | rvo->setPreferredVelocities(); 217 | 218 | bool arrive_flag = rvo->arrived(); 219 | 220 | if (motion_model == "circle_spin") 221 | { 222 | if (arrive_flag == true) 223 | 224 | { 225 | geometry_msgs::Point temp_first = rvo_goals[0]; 226 | 227 | for (int i=0;i new_velocities = rvo->step(); 237 | 238 | auto models_name = sub_msg->name; 239 | int total_num = models_name.size(); 240 | 241 | std_msgs::Header header; 242 | header.seq = seq; 243 | header.stamp = ros::Time::now(); 244 | header.frame_id = "/world"; 245 | 246 | msg_pub.header = header; 247 | msg_pub.name.clear(); 248 | msg_pub.pose.clear(); 249 | msg_pub.twist.clear(); 250 | 251 | msg_pub.header = header; 252 | 253 | num_agent = new_velocities.size(); 254 | 255 | if (num_agent != copy_num_agent) 256 | { 257 | std::cout << "The num of agents is " + std::to_string(num_agent) << std::endl; 258 | copy_num_agent = num_agent; 259 | } 260 | 261 | for (int i = 0; i < total_num; i++) 262 | { 263 | geometry_msgs::Twist new_vel; 264 | geometry_msgs::Pose cur_pose; 265 | 266 | // std::string agent_name = "agent" + std::to_string(i + 1); 267 | // std::string full_agent_name = agent_name + std::to_string(i + 1); 268 | std::string full_agent_name = agent_name + std::to_string(i); 269 | 270 | auto iter_agent = std::find(models_name.begin(), models_name.end(), full_agent_name); 271 | int iter_index = iter_agent - models_name.begin(); 272 | 273 | 274 | if (iter_agent != models_name.end()) 275 | { 276 | 277 | float x = new_velocities[count_vel]->x(); 278 | float y = new_velocities[count_vel]->y(); 279 | 280 | new_vel.linear.x = x; 281 | new_vel.linear.y = y; 282 | 283 | cur_pose = sub_msg->pose[iter_index]; 284 | 285 | msg_pub.name.push_back(full_agent_name); 286 | msg_pub.twist.push_back(new_vel); 287 | msg_pub.pose.push_back(cur_pose); 288 | 289 | count_vel++; 290 | // std::cout << "Current " << full_agent_name << std::endl; 291 | } 292 | } 293 | rvo_node_pub.publish(msg_pub); 294 | } 295 | 296 | -------------------------------------------------------------------------------- /rvo_lib/RVOSimulator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * RVOSimulator.cpp 3 | * RVO2 Library 4 | * 5 | * Copyright 2008 University of North Carolina at Chapel Hill 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * Please send all bug reports to . 20 | * 21 | * The authors may be contacted via: 22 | * 23 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 24 | * Dept. of Computer Science 25 | * 201 S. Columbia St. 26 | * Frederick P. Brooks, Jr. Computer Science Bldg. 27 | * Chapel Hill, N.C. 27599-3175 28 | * United States of America 29 | * 30 | * 31 | */ 32 | 33 | #include "RVOSimulator.h" 34 | 35 | #include "Agent.h" 36 | #include "KdTree.h" 37 | #include "Obstacle.h" 38 | 39 | #ifdef _OPENMP 40 | #include 41 | #endif 42 | 43 | namespace RVO { 44 | RVOSimulator::RVOSimulator() : defaultAgent_(NULL), globalTime_(0.0f), kdTree_(NULL), timeStep_(0.0f) 45 | { 46 | kdTree_ = new KdTree(this); 47 | } 48 | 49 | RVOSimulator::RVOSimulator(float timeStep, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity) : defaultAgent_(NULL), globalTime_(0.0f), kdTree_(NULL), timeStep_(timeStep) 50 | { 51 | kdTree_ = new KdTree(this); 52 | defaultAgent_ = new Agent(this); 53 | 54 | defaultAgent_->maxNeighbors_ = maxNeighbors; 55 | defaultAgent_->maxSpeed_ = maxSpeed; 56 | defaultAgent_->neighborDist_ = neighborDist; 57 | defaultAgent_->radius_ = radius; 58 | defaultAgent_->timeHorizon_ = timeHorizon; 59 | defaultAgent_->timeHorizonObst_ = timeHorizonObst; 60 | defaultAgent_->velocity_ = velocity; 61 | } 62 | 63 | RVOSimulator::~RVOSimulator() 64 | { 65 | if (defaultAgent_ != NULL) { 66 | delete defaultAgent_; 67 | } 68 | 69 | for (size_t i = 0; i < agents_.size(); ++i) { 70 | delete agents_[i]; 71 | } 72 | 73 | for (size_t i = 0; i < obstacles_.size(); ++i) { 74 | delete obstacles_[i]; 75 | } 76 | 77 | delete kdTree_; 78 | } 79 | 80 | size_t RVOSimulator::addAgent(const Vector2 &position) 81 | { 82 | if (defaultAgent_ == NULL) { 83 | return RVO_ERROR; 84 | } 85 | 86 | Agent *agent = new Agent(this); 87 | 88 | agent->position_ = position; 89 | agent->maxNeighbors_ = defaultAgent_->maxNeighbors_; 90 | agent->maxSpeed_ = defaultAgent_->maxSpeed_; 91 | agent->neighborDist_ = defaultAgent_->neighborDist_; 92 | agent->radius_ = defaultAgent_->radius_; 93 | agent->timeHorizon_ = defaultAgent_->timeHorizon_; 94 | agent->timeHorizonObst_ = defaultAgent_->timeHorizonObst_; 95 | agent->velocity_ = defaultAgent_->velocity_; 96 | 97 | agent->id_ = agents_.size(); 98 | 99 | agents_.push_back(agent); 100 | 101 | return agents_.size() - 1; 102 | } 103 | 104 | size_t RVOSimulator::addAgent(const Vector2 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity) 105 | { 106 | Agent *agent = new Agent(this); 107 | 108 | agent->position_ = position; 109 | agent->maxNeighbors_ = maxNeighbors; 110 | agent->maxSpeed_ = maxSpeed; 111 | agent->neighborDist_ = neighborDist; 112 | agent->radius_ = radius; 113 | agent->timeHorizon_ = timeHorizon; 114 | agent->timeHorizonObst_ = timeHorizonObst; 115 | agent->velocity_ = velocity; 116 | 117 | agent->id_ = agents_.size(); 118 | 119 | agents_.push_back(agent); 120 | 121 | return agents_.size() - 1; 122 | } 123 | 124 | size_t RVOSimulator::addObstacle(const std::vector &vertices) 125 | { 126 | if (vertices.size() < 2) { 127 | return RVO_ERROR; 128 | } 129 | 130 | const size_t obstacleNo = obstacles_.size(); 131 | 132 | for (size_t i = 0; i < vertices.size(); ++i) { 133 | Obstacle *obstacle = new Obstacle(); 134 | obstacle->point_ = vertices[i]; 135 | 136 | if (i != 0) { 137 | obstacle->prevObstacle_ = obstacles_.back(); 138 | obstacle->prevObstacle_->nextObstacle_ = obstacle; 139 | } 140 | 141 | if (i == vertices.size() - 1) { 142 | obstacle->nextObstacle_ = obstacles_[obstacleNo]; 143 | obstacle->nextObstacle_->prevObstacle_ = obstacle; 144 | } 145 | 146 | obstacle->unitDir_ = normalize(vertices[(i == vertices.size() - 1 ? 0 : i + 1)] - vertices[i]); 147 | 148 | if (vertices.size() == 2) { 149 | obstacle->isConvex_ = true; 150 | } 151 | else { 152 | obstacle->isConvex_ = (leftOf(vertices[(i == 0 ? vertices.size() - 1 : i - 1)], vertices[i], vertices[(i == vertices.size() - 1 ? 0 : i + 1)]) >= 0.0f); 153 | } 154 | 155 | obstacle->id_ = obstacles_.size(); 156 | 157 | obstacles_.push_back(obstacle); 158 | } 159 | 160 | return obstacleNo; 161 | } 162 | 163 | void RVOSimulator::doStep() 164 | { 165 | kdTree_->buildAgentTree(); 166 | 167 | #ifdef _OPENMP 168 | #pragma omp parallel for 169 | #endif 170 | for (int i = 0; i < static_cast(agents_.size()); ++i) { 171 | agents_[i]->computeNeighbors(); 172 | agents_[i]->computeNewVelocity(); 173 | } 174 | 175 | #ifdef _OPENMP 176 | #pragma omp parallel for 177 | #endif 178 | for (int i = 0; i < static_cast(agents_.size()); ++i) { 179 | agents_[i]->update(); 180 | } 181 | 182 | globalTime_ += timeStep_; 183 | } 184 | 185 | size_t RVOSimulator::getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const 186 | { 187 | return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_; 188 | } 189 | 190 | size_t RVOSimulator::getAgentMaxNeighbors(size_t agentNo) const 191 | { 192 | return agents_[agentNo]->maxNeighbors_; 193 | } 194 | 195 | float RVOSimulator::getAgentMaxSpeed(size_t agentNo) const 196 | { 197 | return agents_[agentNo]->maxSpeed_; 198 | } 199 | 200 | float RVOSimulator::getAgentNeighborDist(size_t agentNo) const 201 | { 202 | return agents_[agentNo]->neighborDist_; 203 | } 204 | 205 | size_t RVOSimulator::getAgentNumAgentNeighbors(size_t agentNo) const 206 | { 207 | return agents_[agentNo]->agentNeighbors_.size(); 208 | } 209 | 210 | size_t RVOSimulator::getAgentNumObstacleNeighbors(size_t agentNo) const 211 | { 212 | return agents_[agentNo]->obstacleNeighbors_.size(); 213 | } 214 | 215 | size_t RVOSimulator::getAgentNumORCALines(size_t agentNo) const 216 | { 217 | return agents_[agentNo]->orcaLines_.size(); 218 | } 219 | 220 | size_t RVOSimulator::getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const 221 | { 222 | return agents_[agentNo]->obstacleNeighbors_[neighborNo].second->id_; 223 | } 224 | 225 | const Line &RVOSimulator::getAgentORCALine(size_t agentNo, size_t lineNo) const 226 | { 227 | return agents_[agentNo]->orcaLines_[lineNo]; 228 | } 229 | 230 | const Vector2 &RVOSimulator::getAgentPosition(size_t agentNo) const 231 | { 232 | return agents_[agentNo]->position_; 233 | } 234 | 235 | const Vector2 &RVOSimulator::getAgentPrefVelocity(size_t agentNo) const 236 | { 237 | return agents_[agentNo]->prefVelocity_; 238 | } 239 | 240 | float RVOSimulator::getAgentRadius(size_t agentNo) const 241 | { 242 | return agents_[agentNo]->radius_; 243 | } 244 | 245 | float RVOSimulator::getAgentTimeHorizon(size_t agentNo) const 246 | { 247 | return agents_[agentNo]->timeHorizon_; 248 | } 249 | 250 | float RVOSimulator::getAgentTimeHorizonObst(size_t agentNo) const 251 | { 252 | return agents_[agentNo]->timeHorizonObst_; 253 | } 254 | 255 | const Vector2 &RVOSimulator::getAgentVelocity(size_t agentNo) const 256 | { 257 | return agents_[agentNo]->velocity_; 258 | } 259 | 260 | float RVOSimulator::getGlobalTime() const 261 | { 262 | return globalTime_; 263 | } 264 | 265 | size_t RVOSimulator::getNumAgents() const 266 | { 267 | return agents_.size(); 268 | } 269 | 270 | size_t RVOSimulator::getNumObstacleVertices() const 271 | { 272 | return obstacles_.size(); 273 | } 274 | 275 | const Vector2 &RVOSimulator::getObstacleVertex(size_t vertexNo) const 276 | { 277 | return obstacles_[vertexNo]->point_; 278 | } 279 | 280 | size_t RVOSimulator::getNextObstacleVertexNo(size_t vertexNo) const 281 | { 282 | return obstacles_[vertexNo]->nextObstacle_->id_; 283 | } 284 | 285 | size_t RVOSimulator::getPrevObstacleVertexNo(size_t vertexNo) const 286 | { 287 | return obstacles_[vertexNo]->prevObstacle_->id_; 288 | } 289 | 290 | float RVOSimulator::getTimeStep() const 291 | { 292 | return timeStep_; 293 | } 294 | 295 | void RVOSimulator::processObstacles() 296 | { 297 | kdTree_->buildObstacleTree(); 298 | } 299 | 300 | bool RVOSimulator::queryVisibility(const Vector2 &point1, const Vector2 &point2, float radius) const 301 | { 302 | return kdTree_->queryVisibility(point1, point2, radius); 303 | } 304 | 305 | void RVOSimulator::setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity) 306 | { 307 | if (defaultAgent_ == NULL) { 308 | defaultAgent_ = new Agent(this); 309 | } 310 | 311 | defaultAgent_->maxNeighbors_ = maxNeighbors; 312 | defaultAgent_->maxSpeed_ = maxSpeed; 313 | defaultAgent_->neighborDist_ = neighborDist; 314 | defaultAgent_->radius_ = radius; 315 | defaultAgent_->timeHorizon_ = timeHorizon; 316 | defaultAgent_->timeHorizonObst_ = timeHorizonObst; 317 | defaultAgent_->velocity_ = velocity; 318 | } 319 | 320 | void RVOSimulator::setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors) 321 | { 322 | agents_[agentNo]->maxNeighbors_ = maxNeighbors; 323 | } 324 | 325 | void RVOSimulator::setAgentMaxSpeed(size_t agentNo, float maxSpeed) 326 | { 327 | agents_[agentNo]->maxSpeed_ = maxSpeed; 328 | } 329 | 330 | void RVOSimulator::setAgentNeighborDist(size_t agentNo, float neighborDist) 331 | { 332 | agents_[agentNo]->neighborDist_ = neighborDist; 333 | } 334 | 335 | void RVOSimulator::setAgentPosition(size_t agentNo, const Vector2 &position) 336 | { 337 | agents_[agentNo]->position_ = position; 338 | } 339 | 340 | void RVOSimulator::setAgentPrefVelocity(size_t agentNo, const Vector2 &prefVelocity) 341 | { 342 | agents_[agentNo]->prefVelocity_ = prefVelocity; 343 | } 344 | 345 | void RVOSimulator::setAgentRadius(size_t agentNo, float radius) 346 | { 347 | agents_[agentNo]->radius_ = radius; 348 | } 349 | 350 | void RVOSimulator::setAgentTimeHorizon(size_t agentNo, float timeHorizon) 351 | { 352 | agents_[agentNo]->timeHorizon_ = timeHorizon; 353 | } 354 | 355 | void RVOSimulator::setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst) 356 | { 357 | agents_[agentNo]->timeHorizonObst_ = timeHorizonObst; 358 | } 359 | 360 | void RVOSimulator::setAgentVelocity(size_t agentNo, const Vector2 &velocity) 361 | { 362 | agents_[agentNo]->velocity_ = velocity; 363 | } 364 | 365 | void RVOSimulator::setTimeStep(float timeStep) 366 | { 367 | timeStep_ = timeStep; 368 | } 369 | } 370 | -------------------------------------------------------------------------------- /rvo_lib/Vector2.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Vector2.h 3 | * RVO2 Library 4 | * 5 | * Copyright 2008 University of North Carolina at Chapel Hill 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * Please send all bug reports to . 20 | * 21 | * The authors may be contacted via: 22 | * 23 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 24 | * Dept. of Computer Science 25 | * 201 S. Columbia St. 26 | * Frederick P. Brooks, Jr. Computer Science Bldg. 27 | * Chapel Hill, N.C. 27599-3175 28 | * United States of America 29 | * 30 | * 31 | */ 32 | 33 | #ifndef RVO_VECTOR2_H_ 34 | #define RVO_VECTOR2_H_ 35 | 36 | /** 37 | * \file Vector2.h 38 | * \brief Contains the Vector2 class. 39 | */ 40 | 41 | #include 42 | #include 43 | 44 | namespace RVO { 45 | /** 46 | * \brief Defines a two-dimensional vector. 47 | */ 48 | class Vector2 { 49 | public: 50 | /** 51 | * \brief Constructs and initializes a two-dimensional vector instance 52 | * to (0.0, 0.0). 53 | */ 54 | inline Vector2() : x_(0.0f), y_(0.0f) { } 55 | 56 | /** 57 | * \brief Constructs and initializes a two-dimensional vector from 58 | * the specified xy-coordinates. 59 | * \param x The x-coordinate of the two-dimensional 60 | * vector. 61 | * \param y The y-coordinate of the two-dimensional 62 | * vector. 63 | */ 64 | inline Vector2(float x, float y) : x_(x), y_(y) { } 65 | 66 | /** 67 | * \brief Returns the x-coordinate of this two-dimensional vector. 68 | * \return The x-coordinate of the two-dimensional vector. 69 | */ 70 | inline float x() const { return x_; } 71 | 72 | /** 73 | * \brief Returns the y-coordinate of this two-dimensional vector. 74 | * \return The y-coordinate of the two-dimensional vector. 75 | */ 76 | inline float y() const { return y_; } 77 | 78 | /** 79 | * \brief Computes the negation of this two-dimensional vector. 80 | * \return The negation of this two-dimensional vector. 81 | */ 82 | inline Vector2 operator-() const 83 | { 84 | return Vector2(-x_, -y_); 85 | } 86 | 87 | /** 88 | * \brief Computes the dot product of this two-dimensional vector with 89 | * the specified two-dimensional vector. 90 | * \param vector The two-dimensional vector with which the 91 | * dot product should be computed. 92 | * \return The dot product of this two-dimensional vector with a 93 | * specified two-dimensional vector. 94 | */ 95 | inline float operator*(const Vector2 &vector) const 96 | { 97 | return x_ * vector.x() + y_ * vector.y(); 98 | } 99 | 100 | /** 101 | * \brief Computes the scalar multiplication of this 102 | * two-dimensional vector with the specified scalar value. 103 | * \param s The scalar value with which the scalar 104 | * multiplication should be computed. 105 | * \return The scalar multiplication of this two-dimensional vector 106 | * with a specified scalar value. 107 | */ 108 | inline Vector2 operator*(float s) const 109 | { 110 | return Vector2(x_ * s, y_ * s); 111 | } 112 | 113 | /** 114 | * \brief Computes the scalar division of this two-dimensional vector 115 | * with the specified scalar value. 116 | * \param s The scalar value with which the scalar 117 | * division should be computed. 118 | * \return The scalar division of this two-dimensional vector with a 119 | * specified scalar value. 120 | */ 121 | inline Vector2 operator/(float s) const 122 | { 123 | const float invS = 1.0f / s; 124 | 125 | return Vector2(x_ * invS, y_ * invS); 126 | } 127 | 128 | /** 129 | * \brief Computes the vector sum of this two-dimensional vector with 130 | * the specified two-dimensional vector. 131 | * \param vector The two-dimensional vector with which the 132 | * vector sum should be computed. 133 | * \return The vector sum of this two-dimensional vector with a 134 | * specified two-dimensional vector. 135 | */ 136 | inline Vector2 operator+(const Vector2 &vector) const 137 | { 138 | return Vector2(x_ + vector.x(), y_ + vector.y()); 139 | } 140 | 141 | /** 142 | * \brief Computes the vector difference of this two-dimensional 143 | * vector with the specified two-dimensional vector. 144 | * \param vector The two-dimensional vector with which the 145 | * vector difference should be computed. 146 | * \return The vector difference of this two-dimensional vector with a 147 | * specified two-dimensional vector. 148 | */ 149 | inline Vector2 operator-(const Vector2 &vector) const 150 | { 151 | return Vector2(x_ - vector.x(), y_ - vector.y()); 152 | } 153 | 154 | /** 155 | * \brief Tests this two-dimensional vector for equality with the 156 | * specified two-dimensional vector. 157 | * \param vector The two-dimensional vector with which to 158 | * test for equality. 159 | * \return True if the two-dimensional vectors are equal. 160 | */ 161 | inline bool operator==(const Vector2 &vector) const 162 | { 163 | return x_ == vector.x() && y_ == vector.y(); 164 | } 165 | 166 | /** 167 | * \brief Tests this two-dimensional vector for inequality with the 168 | * specified two-dimensional vector. 169 | * \param vector The two-dimensional vector with which to 170 | * test for inequality. 171 | * \return True if the two-dimensional vectors are not equal. 172 | */ 173 | inline bool operator!=(const Vector2 &vector) const 174 | { 175 | return x_ != vector.x() || y_ != vector.y(); 176 | } 177 | 178 | /** 179 | * \brief Sets the value of this two-dimensional vector to the scalar 180 | * multiplication of itself with the specified scalar value. 181 | * \param s The scalar value with which the scalar 182 | * multiplication should be computed. 183 | * \return A reference to this two-dimensional vector. 184 | */ 185 | inline Vector2 &operator*=(float s) 186 | { 187 | x_ *= s; 188 | y_ *= s; 189 | 190 | return *this; 191 | } 192 | 193 | /** 194 | * \brief Sets the value of this two-dimensional vector to the scalar 195 | * division of itself with the specified scalar value. 196 | * \param s The scalar value with which the scalar 197 | * division should be computed. 198 | * \return A reference to this two-dimensional vector. 199 | */ 200 | inline Vector2 &operator/=(float s) 201 | { 202 | const float invS = 1.0f / s; 203 | x_ *= invS; 204 | y_ *= invS; 205 | 206 | return *this; 207 | } 208 | 209 | /** 210 | * \brief Sets the value of this two-dimensional vector to the vector 211 | * sum of itself with the specified two-dimensional vector. 212 | * \param vector The two-dimensional vector with which the 213 | * vector sum should be computed. 214 | * \return A reference to this two-dimensional vector. 215 | */ 216 | inline Vector2 &operator+=(const Vector2 &vector) 217 | { 218 | x_ += vector.x(); 219 | y_ += vector.y(); 220 | 221 | return *this; 222 | } 223 | 224 | /** 225 | * \brief Sets the value of this two-dimensional vector to the vector 226 | * difference of itself with the specified two-dimensional 227 | * vector. 228 | * \param vector The two-dimensional vector with which the 229 | * vector difference should be computed. 230 | * \return A reference to this two-dimensional vector. 231 | */ 232 | inline Vector2 &operator-=(const Vector2 &vector) 233 | { 234 | x_ -= vector.x(); 235 | y_ -= vector.y(); 236 | 237 | return *this; 238 | } 239 | 240 | private: 241 | float x_; 242 | float y_; 243 | }; 244 | 245 | /** 246 | * \relates Vector2 247 | * \brief Computes the scalar multiplication of the specified 248 | * two-dimensional vector with the specified scalar value. 249 | * \param s The scalar value with which the scalar 250 | * multiplication should be computed. 251 | * \param vector The two-dimensional vector with which the scalar 252 | * multiplication should be computed. 253 | * \return The scalar multiplication of the two-dimensional vector with the 254 | * scalar value. 255 | */ 256 | inline Vector2 operator*(float s, const Vector2 &vector) 257 | { 258 | return Vector2(s * vector.x(), s * vector.y()); 259 | } 260 | 261 | /** 262 | * \relates Vector2 263 | * \brief Inserts the specified two-dimensional vector into the specified 264 | * output stream. 265 | * \param os The output stream into which the two-dimensional 266 | * vector should be inserted. 267 | * \param vector The two-dimensional vector which to insert into 268 | * the output stream. 269 | * \return A reference to the output stream. 270 | */ 271 | inline std::ostream &operator<<(std::ostream &os, const Vector2 &vector) 272 | { 273 | os << "(" << vector.x() << "," << vector.y() << ")"; 274 | 275 | return os; 276 | } 277 | 278 | /** 279 | * \relates Vector2 280 | * \brief Computes the length of a specified two-dimensional vector. 281 | * \param vector The two-dimensional vector whose length is to be 282 | * computed. 283 | * \return The length of the two-dimensional vector. 284 | */ 285 | inline float abs(const Vector2 &vector) 286 | { 287 | return std::sqrt(vector * vector); 288 | } 289 | 290 | /** 291 | * \relates Vector2 292 | * \brief Computes the squared length of a specified two-dimensional 293 | * vector. 294 | * \param vector The two-dimensional vector whose squared length 295 | * is to be computed. 296 | * \return The squared length of the two-dimensional vector. 297 | */ 298 | inline float absSq(const Vector2 &vector) 299 | { 300 | return vector * vector; 301 | } 302 | 303 | /** 304 | * \relates Vector2 305 | * \brief Computes the determinant of a two-dimensional square matrix with 306 | * rows consisting of the specified two-dimensional vectors. 307 | * \param vector1 The top row of the two-dimensional square 308 | * matrix. 309 | * \param vector2 The bottom row of the two-dimensional square 310 | * matrix. 311 | * \return The determinant of the two-dimensional square matrix. 312 | */ 313 | inline float det(const Vector2 &vector1, const Vector2 &vector2) 314 | { 315 | return vector1.x() * vector2.y() - vector1.y() * vector2.x(); 316 | } 317 | 318 | /** 319 | * \relates Vector2 320 | * \brief Computes the normalization of the specified two-dimensional 321 | * vector. 322 | * \param vector The two-dimensional vector whose normalization 323 | * is to be computed. 324 | * \return The normalization of the two-dimensional vector. 325 | */ 326 | inline Vector2 normalize(const Vector2 &vector) 327 | { 328 | return vector / abs(vector); 329 | } 330 | } 331 | 332 | #endif /* RVO_VECTOR2_H_ */ 333 | -------------------------------------------------------------------------------- /rvo_lib/KdTree.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * KdTree.cpp 3 | * RVO2 Library 4 | * 5 | * Copyright 2008 University of North Carolina at Chapel Hill 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * Please send all bug reports to . 20 | * 21 | * The authors may be contacted via: 22 | * 23 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 24 | * Dept. of Computer Science 25 | * 201 S. Columbia St. 26 | * Frederick P. Brooks, Jr. Computer Science Bldg. 27 | * Chapel Hill, N.C. 27599-3175 28 | * United States of America 29 | * 30 | * 31 | */ 32 | 33 | #include "KdTree.h" 34 | 35 | #include "Agent.h" 36 | #include "RVOSimulator.h" 37 | #include "Obstacle.h" 38 | #include "nav_rvo.h" 39 | 40 | namespace RVO { 41 | KdTree::KdTree(RVOSimulator *sim) : obstacleTree_(NULL), sim_(sim) { } 42 | 43 | KdTree::~KdTree() 44 | { 45 | deleteObstacleTree(obstacleTree_); 46 | } 47 | 48 | void KdTree::buildAgentTree() 49 | { 50 | if (agents_.size() < sim_->agents_.size()) { 51 | for (size_t i = agents_.size(); i < sim_->agents_.size(); ++i) { 52 | agents_.push_back(sim_->agents_[i]); 53 | } 54 | 55 | agentTree_.resize(2 * agents_.size() - 1); 56 | } 57 | 58 | if (!agents_.empty()) { 59 | buildAgentTreeRecursive(0, agents_.size(), 0); 60 | } 61 | } 62 | 63 | void KdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node) 64 | { 65 | agentTree_[node].begin = begin; 66 | agentTree_[node].end = end; 67 | agentTree_[node].minX = agentTree_[node].maxX = agents_[begin]->position_.x(); 68 | agentTree_[node].minY = agentTree_[node].maxY = agents_[begin]->position_.y(); 69 | 70 | for (size_t i = begin + 1; i < end; ++i) { 71 | agentTree_[node].maxX = std::max(agentTree_[node].maxX, agents_[i]->position_.x()); 72 | agentTree_[node].minX = std::min(agentTree_[node].minX, agents_[i]->position_.x()); 73 | agentTree_[node].maxY = std::max(agentTree_[node].maxY, agents_[i]->position_.y()); 74 | agentTree_[node].minY = std::min(agentTree_[node].minY, agents_[i]->position_.y()); 75 | } 76 | 77 | if (end - begin > MAX_LEAF_SIZE) { 78 | /* No leaf node. */ 79 | const bool isVertical = (agentTree_[node].maxX - agentTree_[node].minX > agentTree_[node].maxY - agentTree_[node].minY); 80 | const float splitValue = (isVertical ? 0.5f * (agentTree_[node].maxX + agentTree_[node].minX) : 0.5f * (agentTree_[node].maxY + agentTree_[node].minY)); 81 | 82 | size_t left = begin; 83 | size_t right = end; 84 | 85 | while (left < right) { 86 | while (left < right && (isVertical ? agents_[left]->position_.x() : agents_[left]->position_.y()) < splitValue) { 87 | ++left; 88 | } 89 | 90 | while (right > left && (isVertical ? agents_[right - 1]->position_.x() : agents_[right - 1]->position_.y()) >= splitValue) { 91 | --right; 92 | } 93 | 94 | if (left < right) { 95 | std::swap(agents_[left], agents_[right - 1]); 96 | ++left; 97 | --right; 98 | } 99 | } 100 | 101 | if (left == begin) { 102 | ++left; 103 | ++right; 104 | } 105 | 106 | agentTree_[node].left = node + 1; 107 | agentTree_[node].right = node + 2 * (left - begin); 108 | 109 | buildAgentTreeRecursive(begin, left, agentTree_[node].left); 110 | buildAgentTreeRecursive(left, end, agentTree_[node].right); 111 | } 112 | } 113 | 114 | void KdTree::buildObstacleTree() 115 | { 116 | deleteObstacleTree(obstacleTree_); 117 | 118 | std::vector obstacles(sim_->obstacles_.size()); 119 | 120 | for (size_t i = 0; i < sim_->obstacles_.size(); ++i) { 121 | obstacles[i] = sim_->obstacles_[i]; 122 | } 123 | 124 | obstacleTree_ = buildObstacleTreeRecursive(obstacles); 125 | } 126 | 127 | 128 | KdTree::ObstacleTreeNode *KdTree::buildObstacleTreeRecursive(const std::vector &obstacles) 129 | { 130 | if (obstacles.empty()) { 131 | return NULL; 132 | } 133 | else { 134 | ObstacleTreeNode *const node = new ObstacleTreeNode; 135 | 136 | size_t optimalSplit = 0; 137 | size_t minLeft = obstacles.size(); 138 | size_t minRight = obstacles.size(); 139 | 140 | for (size_t i = 0; i < obstacles.size(); ++i) { 141 | size_t leftSize = 0; 142 | size_t rightSize = 0; 143 | 144 | const Obstacle *const obstacleI1 = obstacles[i]; 145 | const Obstacle *const obstacleI2 = obstacleI1->nextObstacle_; 146 | 147 | /* Compute optimal split node. */ 148 | for (size_t j = 0; j < obstacles.size(); ++j) { 149 | if (i == j) { 150 | continue; 151 | } 152 | 153 | const Obstacle *const obstacleJ1 = obstacles[j]; 154 | const Obstacle *const obstacleJ2 = obstacleJ1->nextObstacle_; 155 | 156 | const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_); 157 | const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_); 158 | 159 | if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { 160 | ++leftSize; 161 | } 162 | else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { 163 | ++rightSize; 164 | } 165 | else { 166 | ++leftSize; 167 | ++rightSize; 168 | } 169 | 170 | if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) >= std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) { 171 | break; 172 | } 173 | } 174 | 175 | if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) < std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) { 176 | minLeft = leftSize; 177 | minRight = rightSize; 178 | optimalSplit = i; 179 | } 180 | } 181 | 182 | /* Build split node. */ 183 | std::vector leftObstacles(minLeft); 184 | std::vector rightObstacles(minRight); 185 | 186 | size_t leftCounter = 0; 187 | size_t rightCounter = 0; 188 | const size_t i = optimalSplit; 189 | 190 | const Obstacle *const obstacleI1 = obstacles[i]; 191 | const Obstacle *const obstacleI2 = obstacleI1->nextObstacle_; 192 | 193 | for (size_t j = 0; j < obstacles.size(); ++j) { 194 | if (i == j) { 195 | continue; 196 | } 197 | 198 | Obstacle *const obstacleJ1 = obstacles[j]; 199 | Obstacle *const obstacleJ2 = obstacleJ1->nextObstacle_; 200 | 201 | const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_); 202 | const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_); 203 | 204 | if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { 205 | leftObstacles[leftCounter++] = obstacles[j]; 206 | } 207 | else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { 208 | rightObstacles[rightCounter++] = obstacles[j]; 209 | } 210 | else { 211 | /* Split obstacle j. */ 212 | const float t = det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleI1->point_) / det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleJ2->point_); 213 | 214 | const Vector2 splitpoint = obstacleJ1->point_ + t * (obstacleJ2->point_ - obstacleJ1->point_); 215 | 216 | Obstacle *const newObstacle = new Obstacle(); 217 | newObstacle->point_ = splitpoint; 218 | newObstacle->prevObstacle_ = obstacleJ1; 219 | newObstacle->nextObstacle_ = obstacleJ2; 220 | newObstacle->isConvex_ = true; 221 | newObstacle->unitDir_ = obstacleJ1->unitDir_; 222 | 223 | newObstacle->id_ = sim_->obstacles_.size(); 224 | 225 | sim_->obstacles_.push_back(newObstacle); 226 | 227 | obstacleJ1->nextObstacle_ = newObstacle; 228 | obstacleJ2->prevObstacle_ = newObstacle; 229 | 230 | if (j1LeftOfI > 0.0f) { 231 | leftObstacles[leftCounter++] = obstacleJ1; 232 | rightObstacles[rightCounter++] = newObstacle; 233 | } 234 | else { 235 | rightObstacles[rightCounter++] = obstacleJ1; 236 | leftObstacles[leftCounter++] = newObstacle; 237 | } 238 | } 239 | } 240 | 241 | node->obstacle = obstacleI1; 242 | node->left = buildObstacleTreeRecursive(leftObstacles); 243 | node->right = buildObstacleTreeRecursive(rightObstacles); 244 | return node; 245 | } 246 | } 247 | 248 | void KdTree::computeAgentNeighbors(Agent *agent, float &rangeSq) const 249 | { 250 | queryAgentTreeRecursive(agent, rangeSq, 0); 251 | } 252 | 253 | void KdTree::computeObstacleNeighbors(Agent *agent, float rangeSq) const 254 | { 255 | queryObstacleTreeRecursive(agent, rangeSq, obstacleTree_); 256 | } 257 | 258 | void KdTree::deleteObstacleTree(ObstacleTreeNode *node) 259 | { 260 | if (node != NULL) { 261 | deleteObstacleTree(node->left); 262 | deleteObstacleTree(node->right); 263 | delete node; 264 | } 265 | } 266 | 267 | void KdTree::queryAgentTreeRecursive(Agent *agent, float &rangeSq, size_t node) const 268 | { 269 | if (agentTree_[node].end - agentTree_[node].begin <= MAX_LEAF_SIZE) { 270 | for (size_t i = agentTree_[node].begin; i < agentTree_[node].end; ++i) { 271 | agent->insertAgentNeighbor(agents_[i], rangeSq); 272 | } 273 | } 274 | else { 275 | 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)); 276 | 277 | 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)); 278 | 279 | if (distSqLeft < distSqRight) { 280 | if (distSqLeft < rangeSq) { 281 | queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left); 282 | 283 | if (distSqRight < rangeSq) { 284 | queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right); 285 | } 286 | } 287 | } 288 | else { 289 | if (distSqRight < rangeSq) { 290 | queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right); 291 | 292 | if (distSqLeft < rangeSq) { 293 | queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left); 294 | } 295 | } 296 | } 297 | 298 | } 299 | } 300 | 301 | void KdTree::queryObstacleTreeRecursive(Agent *agent, float rangeSq, const ObstacleTreeNode *node) const 302 | { 303 | if (node == NULL) { 304 | return; 305 | } 306 | else { 307 | const Obstacle *const obstacle1 = node->obstacle; 308 | const Obstacle *const obstacle2 = obstacle1->nextObstacle_; 309 | 310 | const float agentLeftOfLine = leftOf(obstacle1->point_, obstacle2->point_, agent->position_); 311 | 312 | queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->left : node->right)); 313 | 314 | const float distSqLine = sqr(agentLeftOfLine) / absSq(obstacle2->point_ - obstacle1->point_); 315 | 316 | if (distSqLine < rangeSq) { 317 | if (agentLeftOfLine < 0.0f) { 318 | /* 319 | * Try obstacle at this node only if agent is on right side of 320 | * obstacle (and can see obstacle). 321 | */ 322 | agent->insertObstacleNeighbor(node->obstacle, rangeSq); 323 | } 324 | 325 | /* Try other side of line. */ 326 | queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->right : node->left)); 327 | 328 | } 329 | } 330 | } 331 | 332 | bool KdTree::queryVisibility(const Vector2 &q1, const Vector2 &q2, float radius) const 333 | { 334 | return queryVisibilityRecursive(q1, q2, radius, obstacleTree_); 335 | } 336 | 337 | bool KdTree::queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2, float radius, const ObstacleTreeNode *node) const 338 | { 339 | if (node == NULL) { 340 | return true; 341 | } 342 | else { 343 | const Obstacle *const obstacle1 = node->obstacle; 344 | const Obstacle *const obstacle2 = obstacle1->nextObstacle_; 345 | 346 | const float q1LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q1); 347 | const float q2LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q2); 348 | const float invLengthI = 1.0f / absSq(obstacle2->point_ - obstacle1->point_); 349 | 350 | if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f) { 351 | return queryVisibilityRecursive(q1, q2, radius, node->left) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->right)); 352 | } 353 | else if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f) { 354 | return queryVisibilityRecursive(q1, q2, radius, node->right) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->left)); 355 | } 356 | else if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f) { 357 | /* One can see through obstacle from left to right. */ 358 | return queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right); 359 | } 360 | else { 361 | const float point1LeftOfQ = leftOf(q1, q2, obstacle1->point_); 362 | const float point2LeftOfQ = leftOf(q1, q2, obstacle2->point_); 363 | const float invLengthQ = 1.0f / absSq(q2 - q1); 364 | 365 | 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)); 366 | } 367 | } 368 | } 369 | } 370 | -------------------------------------------------------------------------------- /simulation/worlds/multi_agents.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.1 -0.9 15 | 16 | 17 | 1 18 | 19 | 20 | 21 | 22 | 0 0 1 23 | 100 100 24 | 25 | 26 | 27 | 28 | 29 | 100 30 | 50 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 10 42 | 43 | 44 | 0 45 | 46 | 47 | 0 0 1 48 | 100 100 49 | 50 | 51 | 52 | 56 | 57 | 58 | 0 59 | 0 60 | 0 61 | 62 | 63 | 0 0 -9.8 64 | 6e-06 2.3e-05 -4.2e-05 65 | 66 | 67 | 0.005 68 | 1 69 | 1000 70 | 71 | 72 | 0.4 0.4 0.4 1 73 | 0.7 0.7 0.7 1 74 | 1 75 | 76 | 79 | 80 | 81 | EARTH_WGS84 82 | 0 83 | 0 84 | 0 85 | 0 86 | 87 | 88 | 89 | 90 | 1 91 | 92 | 0 93 | 0 94 | 0 95 | 0 96 | 0 97 | 0 98 | 99 | 100 | 0 0 0 0 -0 0 101 | 0 102 | 0 103 | 0 104 | 105 | 0 0 0 0 -0 0 106 | 107 | 108 | 0.2 109 | 0.4 110 | 111 | 112 | 113 | 1 114 | 118 | 119 | 0 120 | 1 121 | 122 | 123 | 0 124 | 10 125 | 0 0 0 0 -0 0 126 | 127 | 128 | 0.2 129 | 0.4 130 | 131 | 132 | 133 | 134 | 135 | 0 136 | 1 137 | 0 0 0 138 | 0 139 | 0 140 | 141 | 142 | 1 143 | 0 144 | 0 145 | 1 146 | 147 | 0 148 | 149 | 150 | 151 | 152 | 0 153 | 1e+06 154 | 155 | 156 | 0 157 | 1 158 | 1 159 | 160 | 0 161 | 0.2 162 | 1e+13 163 | 1 164 | 0.01 165 | 0 166 | 167 | 168 | 1 169 | -0.01 170 | 0 171 | 0.2 172 | 1e+13 173 | 1 174 | 175 | 176 | 177 | 178 | 1 179 | 180 | 0 181 | 1 182 | 183 | 0.66467 1.15989 0 0 -0 0 184 | 185 | 186 | 187 | 188 | 1 189 | 190 | 0 191 | 0 192 | 0 193 | 0 194 | 0 195 | 0 196 | 197 | 198 | 0 0 0 0 -0 0 199 | 0 200 | 0 201 | 0 202 | 203 | 0 0 0 0 -0 0 204 | 205 | 206 | 0.2 207 | 0.4 208 | 209 | 210 | 211 | 1 212 | 216 | 217 | 0 218 | 1 219 | 220 | 221 | 0 222 | 10 223 | 0 0 0 0 -0 0 224 | 225 | 226 | 0.2 227 | 0.4 228 | 229 | 230 | 231 | 232 | 233 | 0 234 | 1 235 | 0 0 0 236 | 0 237 | 0 238 | 239 | 240 | 1 241 | 0 242 | 0 243 | 1 244 | 245 | 0 246 | 247 | 248 | 249 | 250 | 0 251 | 1e+06 252 | 253 | 254 | 0 255 | 1 256 | 1 257 | 258 | 0 259 | 0.2 260 | 1e+13 261 | 1 262 | 0.01 263 | 0 264 | 265 | 266 | 1 267 | -0.01 268 | 0 269 | 0.2 270 | 1e+13 271 | 1 272 | 273 | 274 | 275 | 276 | 1 277 | 278 | 0 279 | 1 280 | 281 | 1.32776 2.29467 0 0 -0 0 282 | 283 | 284 | 285 | 286 | 1 287 | 288 | 0 289 | 0 290 | 0 291 | 0 292 | 0 293 | 0 294 | 295 | 296 | 0 0 0 0 -0 0 297 | 0 298 | 0 299 | 0 300 | 301 | 0 0 0 0 -0 0 302 | 303 | 304 | 0.2 305 | 0.4 306 | 307 | 308 | 309 | 1 310 | 314 | 315 | 0 316 | 1 317 | 318 | 319 | 0 320 | 10 321 | 0 0 0 0 -0 0 322 | 323 | 324 | 0.2 325 | 0.4 326 | 327 | 328 | 329 | 330 | 331 | 0 332 | 1 333 | 0 0 0 334 | 0 335 | 0 336 | 337 | 338 | 1 339 | 0 340 | 0 341 | 1 342 | 343 | 0 344 | 345 | 346 | 347 | 348 | 0 349 | 1e+06 350 | 351 | 352 | 0 353 | 1 354 | 1 355 | 356 | 0 357 | 0.2 358 | 1e+13 359 | 1 360 | 0.01 361 | 0 362 | 363 | 364 | 1 365 | -0.01 366 | 0 367 | 0.2 368 | 1e+13 369 | 1 370 | 371 | 372 | 373 | 374 | 1 375 | 376 | 0 377 | 1 378 | 379 | 1.95747 2.31785 0 0 -0 0 380 | 381 | 382 | 383 | 384 | 1 385 | 386 | 0 387 | 0 388 | 0 389 | 0 390 | 0 391 | 0 392 | 393 | 394 | 0 0 0 0 -0 0 395 | 0 396 | 0 397 | 0 398 | 399 | 0 0 0 0 -0 0 400 | 401 | 402 | 0.2 403 | 0.4 404 | 405 | 406 | 407 | 1 408 | 412 | 413 | 0 414 | 1 415 | 416 | 417 | 0 418 | 10 419 | 0 0 0 0 -0 0 420 | 421 | 422 | 0.2 423 | 0.4 424 | 425 | 426 | 427 | 428 | 429 | 0 430 | 1 431 | 0 0 0 432 | 0 433 | 0 434 | 435 | 436 | 1 437 | 0 438 | 0 439 | 1 440 | 441 | 0 442 | 443 | 444 | 445 | 446 | 0 447 | 1e+06 448 | 449 | 450 | 0 451 | 1 452 | 1 453 | 454 | 0 455 | 0.2 456 | 1e+13 457 | 1 458 | 0.01 459 | 0 460 | 461 | 462 | 1 463 | -0.01 464 | 0 465 | 0.2 466 | 1e+13 467 | 1 468 | 469 | 470 | 471 | 472 | 1 473 | 474 | 0 475 | 1 476 | 477 | 3.38122 2.314 0 0 -0 0 478 | 479 | 480 | 822 118000000 481 | 74 773115257 482 | 1573796281 541897907 483 | 74280 484 | 485 | 486 | 0 0 0 0 -0 0 487 | 1 1 1 488 | 489 | 0 0 0 0 -0 0 490 | 0 0 0 0 -0 0 491 | 0 0 0 0 -0 0 492 | 0 0 0 0 -0 0 493 | 494 | 495 | 496 | 1 2 0.199805 0 -0 1.2e-05 497 | 1 1 1 498 | 499 | 1 2 0.199805 0 -0 1.2e-05 500 | 0 0 0 0 -0 0 501 | 0 0 -9.8 0 -0 0 502 | 0 0 -9.8 0 -0 0 503 | 504 | 505 | 506 | 2 1.9749 0.199805 0 -0 5e-06 507 | 1 1 1 508 | 509 | 2 1.9749 0.199805 0 -0 5e-06 510 | 0 0 0 0 -0 0 511 | 0 0 -9.8 0 -0 0 512 | 0 0 -9.8 0 -0 0 513 | 514 | 515 | 516 | 3 2.00619 0.199805 0 0 -0.00026 517 | 1 1 1 518 | 519 | 3 2.00619 0.199805 0 0 -0.00026 520 | 0 0 0 0 -0 0 521 | 0 0 -9.8 0 -0 0 522 | 0 0 -9.8 0 -0 0 523 | 524 | 525 | 526 | 4 2 0.199805 0 0 -2e-06 527 | 1 1 1 528 | 529 | 4 2 0.199805 0 0 -2e-06 530 | 0 0 0 0 -0 0 531 | 0 0 -9.8 0 -0 0 532 | 0 0 -9.8 0 -0 0 533 | 534 | 535 | 536 | 0 0 10 0 -0 0 537 | 538 | 539 | 540 | 541 | 3.46139 -3.00246 9.56198 0 1.01963 1.4642 542 | orbit 543 | perspective 544 | 545 | 546 | 547 | 548 | -------------------------------------------------------------------------------- /rvo_lib/Agent.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Agent.cpp 3 | * RVO2 Library 4 | * 5 | * Copyright 2008 University of North Carolina at Chapel Hill 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * Please send all bug reports to . 20 | * 21 | * The authors may be contacted via: 22 | * 23 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 24 | * Dept. of Computer Science 25 | * 201 S. Columbia St. 26 | * Frederick P. Brooks, Jr. Computer Science Bldg. 27 | * Chapel Hill, N.C. 27599-3175 28 | * United States of America 29 | * 30 | * 31 | */ 32 | 33 | #include "Agent.h" 34 | 35 | #include "KdTree.h" 36 | #include "Obstacle.h" 37 | 38 | namespace RVO { 39 | Agent::Agent(RVOSimulator *sim) : maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), sim_(sim), timeHorizon_(0.0f), timeHorizonObst_(0.0f), id_(0) { } 40 | 41 | void Agent::computeNeighbors() 42 | { 43 | obstacleNeighbors_.clear(); 44 | float rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_); 45 | sim_->kdTree_->computeObstacleNeighbors(this, rangeSq); 46 | 47 | agentNeighbors_.clear(); 48 | 49 | if (maxNeighbors_ > 0) { 50 | rangeSq = sqr(neighborDist_); 51 | sim_->kdTree_->computeAgentNeighbors(this, rangeSq); 52 | } 53 | } 54 | 55 | /* Search for the best new velocity. */ 56 | void Agent::computeNewVelocity() 57 | { 58 | orcaLines_.clear(); 59 | 60 | const float invTimeHorizonObst = 1.0f / timeHorizonObst_; 61 | 62 | /* Create obstacle ORCA lines. */ 63 | for (size_t i = 0; i < obstacleNeighbors_.size(); ++i) { 64 | 65 | const Obstacle *obstacle1 = obstacleNeighbors_[i].second; 66 | const Obstacle *obstacle2 = obstacle1->nextObstacle_; 67 | 68 | const Vector2 relativePosition1 = obstacle1->point_ - position_; 69 | const Vector2 relativePosition2 = obstacle2->point_ - position_; 70 | 71 | /* 72 | * Check if velocity obstacle of obstacle is already taken care of by 73 | * previously constructed obstacle ORCA lines. 74 | */ 75 | bool alreadyCovered = false; 76 | 77 | for (size_t j = 0; j < orcaLines_.size(); ++j) { 78 | 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) { 79 | alreadyCovered = true; 80 | break; 81 | } 82 | } 83 | 84 | if (alreadyCovered) { 85 | continue; 86 | } 87 | 88 | /* Not yet covered. Check for collisions. */ 89 | 90 | const float distSq1 = absSq(relativePosition1); 91 | const float distSq2 = absSq(relativePosition2); 92 | 93 | const float radiusSq = sqr(radius_); 94 | 95 | const Vector2 obstacleVector = obstacle2->point_ - obstacle1->point_; 96 | const float s = (-relativePosition1 * obstacleVector) / absSq(obstacleVector); 97 | const float distSqLine = absSq(-relativePosition1 - s * obstacleVector); 98 | 99 | Line line; 100 | 101 | if (s < 0.0f && distSq1 <= radiusSq) { 102 | /* Collision with left vertex. Ignore if non-convex. */ 103 | if (obstacle1->isConvex_) { 104 | line.point = Vector2(0.0f, 0.0f); 105 | line.direction = normalize(Vector2(-relativePosition1.y(), relativePosition1.x())); 106 | orcaLines_.push_back(line); 107 | } 108 | 109 | continue; 110 | } 111 | else if (s > 1.0f && distSq2 <= radiusSq) { 112 | /* Collision with right vertex. Ignore if non-convex 113 | * or if it will be taken care of by neighoring obstace */ 114 | if (obstacle2->isConvex_ && det(relativePosition2, obstacle2->unitDir_) >= 0.0f) { 115 | line.point = Vector2(0.0f, 0.0f); 116 | line.direction = normalize(Vector2(-relativePosition2.y(), relativePosition2.x())); 117 | orcaLines_.push_back(line); 118 | } 119 | 120 | continue; 121 | } 122 | else if (s >= 0.0f && s < 1.0f && distSqLine <= radiusSq) { 123 | /* Collision with obstacle segment. */ 124 | line.point = Vector2(0.0f, 0.0f); 125 | line.direction = -obstacle1->unitDir_; 126 | orcaLines_.push_back(line); 127 | continue; 128 | } 129 | 130 | /* 131 | * No collision. 132 | * Compute legs. When obliquely viewed, both legs can come from a single 133 | * vertex. Legs extend cut-off line when nonconvex vertex. 134 | */ 135 | 136 | Vector2 leftLegDirection, rightLegDirection; 137 | 138 | if (s < 0.0f && distSqLine <= radiusSq) { 139 | /* 140 | * Obstacle viewed obliquely so that left vertex 141 | * defines velocity obstacle. 142 | */ 143 | if (!obstacle1->isConvex_) { 144 | /* Ignore obstacle. */ 145 | continue; 146 | } 147 | 148 | obstacle2 = obstacle1; 149 | 150 | const float leg1 = std::sqrt(distSq1 - radiusSq); 151 | leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1; 152 | rightLegDirection = Vector2(relativePosition1.x() * leg1 + relativePosition1.y() * radius_, -relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1; 153 | } 154 | else if (s > 1.0f && distSqLine <= radiusSq) { 155 | /* 156 | * Obstacle viewed obliquely so that 157 | * right vertex defines velocity obstacle. 158 | */ 159 | if (!obstacle2->isConvex_) { 160 | /* Ignore obstacle. */ 161 | continue; 162 | } 163 | 164 | obstacle1 = obstacle2; 165 | 166 | const float leg2 = std::sqrt(distSq2 - radiusSq); 167 | leftLegDirection = Vector2(relativePosition2.x() * leg2 - relativePosition2.y() * radius_, relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2; 168 | rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2; 169 | } 170 | else { 171 | /* Usual situation. */ 172 | if (obstacle1->isConvex_) { 173 | const float leg1 = std::sqrt(distSq1 - radiusSq); 174 | leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1; 175 | } 176 | else { 177 | /* Left vertex non-convex; left leg extends cut-off line. */ 178 | leftLegDirection = -obstacle1->unitDir_; 179 | } 180 | 181 | if (obstacle2->isConvex_) { 182 | const float leg2 = std::sqrt(distSq2 - radiusSq); 183 | rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2; 184 | } 185 | else { 186 | /* Right vertex non-convex; right leg extends cut-off line. */ 187 | rightLegDirection = obstacle1->unitDir_; 188 | } 189 | } 190 | 191 | /* 192 | * Legs can never point into neighboring edge when convex vertex, 193 | * take cutoff-line of neighboring edge instead. If velocity projected on 194 | * "foreign" leg, no constraint is added. 195 | */ 196 | 197 | const Obstacle *const leftNeighbor = obstacle1->prevObstacle_; 198 | 199 | bool isLeftLegForeign = false; 200 | bool isRightLegForeign = false; 201 | 202 | if (obstacle1->isConvex_ && det(leftLegDirection, -leftNeighbor->unitDir_) >= 0.0f) { 203 | /* Left leg points into obstacle. */ 204 | leftLegDirection = -leftNeighbor->unitDir_; 205 | isLeftLegForeign = true; 206 | } 207 | 208 | if (obstacle2->isConvex_ && det(rightLegDirection, obstacle2->unitDir_) <= 0.0f) { 209 | /* Right leg points into obstacle. */ 210 | rightLegDirection = obstacle2->unitDir_; 211 | isRightLegForeign = true; 212 | } 213 | 214 | /* Compute cut-off centers. */ 215 | const Vector2 leftCutoff = invTimeHorizonObst * (obstacle1->point_ - position_); 216 | const Vector2 rightCutoff = invTimeHorizonObst * (obstacle2->point_ - position_); 217 | const Vector2 cutoffVec = rightCutoff - leftCutoff; 218 | 219 | /* Project current velocity on velocity obstacle. */ 220 | 221 | /* Check if current velocity is projected on cutoff circles. */ 222 | const float t = (obstacle1 == obstacle2 ? 0.5f : ((velocity_ - leftCutoff) * cutoffVec) / absSq(cutoffVec)); 223 | const float tLeft = ((velocity_ - leftCutoff) * leftLegDirection); 224 | const float tRight = ((velocity_ - rightCutoff) * rightLegDirection); 225 | 226 | if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f)) { 227 | /* Project on left cut-off circle. */ 228 | const Vector2 unitW = normalize(velocity_ - leftCutoff); 229 | 230 | line.direction = Vector2(unitW.y(), -unitW.x()); 231 | line.point = leftCutoff + radius_ * invTimeHorizonObst * unitW; 232 | orcaLines_.push_back(line); 233 | continue; 234 | } 235 | else if (t > 1.0f && tRight < 0.0f) { 236 | /* Project on right cut-off circle. */ 237 | const Vector2 unitW = normalize(velocity_ - rightCutoff); 238 | 239 | line.direction = Vector2(unitW.y(), -unitW.x()); 240 | line.point = rightCutoff + radius_ * invTimeHorizonObst * unitW; 241 | orcaLines_.push_back(line); 242 | continue; 243 | } 244 | 245 | /* 246 | * Project on left leg, right leg, or cut-off line, whichever is closest 247 | * to velocity. 248 | */ 249 | const float distSqCutoff = ((t < 0.0f || t > 1.0f || obstacle1 == obstacle2) ? std::numeric_limits::infinity() : absSq(velocity_ - (leftCutoff + t * cutoffVec))); 250 | const float distSqLeft = ((tLeft < 0.0f) ? std::numeric_limits::infinity() : absSq(velocity_ - (leftCutoff + tLeft * leftLegDirection))); 251 | const float distSqRight = ((tRight < 0.0f) ? std::numeric_limits::infinity() : absSq(velocity_ - (rightCutoff + tRight * rightLegDirection))); 252 | 253 | if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) { 254 | /* Project on cut-off line. */ 255 | line.direction = -obstacle1->unitDir_; 256 | line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x()); 257 | orcaLines_.push_back(line); 258 | continue; 259 | } 260 | else if (distSqLeft <= distSqRight) { 261 | /* Project on left leg. */ 262 | if (isLeftLegForeign) { 263 | continue; 264 | } 265 | 266 | line.direction = leftLegDirection; 267 | line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x()); 268 | orcaLines_.push_back(line); 269 | continue; 270 | } 271 | else { 272 | /* Project on right leg. */ 273 | if (isRightLegForeign) { 274 | continue; 275 | } 276 | 277 | line.direction = -rightLegDirection; 278 | line.point = rightCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x()); 279 | orcaLines_.push_back(line); 280 | continue; 281 | } 282 | } 283 | 284 | const size_t numObstLines = orcaLines_.size(); 285 | 286 | const float invTimeHorizon = 1.0f / timeHorizon_; 287 | 288 | /* Create agent ORCA lines. */ 289 | for (size_t i = 0; i < agentNeighbors_.size(); ++i) { 290 | const Agent *const other = agentNeighbors_[i].second; 291 | 292 | const Vector2 relativePosition = other->position_ - position_; 293 | const Vector2 relativeVelocity = velocity_ - other->velocity_; 294 | const float distSq = absSq(relativePosition); 295 | const float combinedRadius = radius_ + other->radius_; 296 | const float combinedRadiusSq = sqr(combinedRadius); 297 | 298 | Line line; 299 | Vector2 u; 300 | 301 | if (distSq > combinedRadiusSq) { 302 | /* No collision. */ 303 | const Vector2 w = relativeVelocity - invTimeHorizon * relativePosition; 304 | /* Vector from cutoff center to relative velocity. */ 305 | const float wLengthSq = absSq(w); 306 | 307 | const float dotProduct1 = w * relativePosition; 308 | 309 | if (dotProduct1 < 0.0f && sqr(dotProduct1) > combinedRadiusSq * wLengthSq) { 310 | /* Project on cut-off circle. */ 311 | const float wLength = std::sqrt(wLengthSq); 312 | const Vector2 unitW = w / wLength; 313 | 314 | line.direction = Vector2(unitW.y(), -unitW.x()); 315 | u = (combinedRadius * invTimeHorizon - wLength) * unitW; 316 | } 317 | else { 318 | /* Project on legs. */ 319 | const float leg = std::sqrt(distSq - combinedRadiusSq); 320 | 321 | if (det(relativePosition, w) > 0.0f) { 322 | /* Project on left leg. */ 323 | line.direction = Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq; 324 | } 325 | else { 326 | /* Project on right leg. */ 327 | line.direction = -Vector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq; 328 | } 329 | 330 | const float dotProduct2 = relativeVelocity * line.direction; 331 | 332 | u = dotProduct2 * line.direction - relativeVelocity; 333 | } 334 | } 335 | else { 336 | /* Collision. Project on cut-off circle of time timeStep. */ 337 | const float invTimeStep = 1.0f / sim_->timeStep_; 338 | 339 | /* Vector from cutoff center to relative velocity. */ 340 | const Vector2 w = relativeVelocity - invTimeStep * relativePosition; 341 | 342 | const float wLength = abs(w); 343 | const Vector2 unitW = w / wLength; 344 | 345 | line.direction = Vector2(unitW.y(), -unitW.x()); 346 | u = (combinedRadius * invTimeStep - wLength) * unitW; 347 | } 348 | 349 | line.point = velocity_ + 0.5f * u; 350 | orcaLines_.push_back(line); 351 | } 352 | 353 | size_t lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocity_); 354 | 355 | if (lineFail < orcaLines_.size()) { 356 | linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, newVelocity_); 357 | } 358 | } 359 | 360 | void Agent::insertAgentNeighbor(const Agent *agent, float &rangeSq) 361 | { 362 | if (this != agent) { 363 | const float distSq = absSq(position_ - agent->position_); 364 | 365 | if (distSq < rangeSq) { 366 | if (agentNeighbors_.size() < maxNeighbors_) { 367 | agentNeighbors_.push_back(std::make_pair(distSq, agent)); 368 | } 369 | 370 | size_t i = agentNeighbors_.size() - 1; 371 | 372 | while (i != 0 && distSq < agentNeighbors_[i - 1].first) { 373 | agentNeighbors_[i] = agentNeighbors_[i - 1]; 374 | --i; 375 | } 376 | 377 | agentNeighbors_[i] = std::make_pair(distSq, agent); 378 | 379 | if (agentNeighbors_.size() == maxNeighbors_) { 380 | rangeSq = agentNeighbors_.back().first; 381 | } 382 | } 383 | } 384 | } 385 | 386 | void Agent::insertObstacleNeighbor(const Obstacle *obstacle, float rangeSq) 387 | { 388 | const Obstacle *const nextObstacle = obstacle->nextObstacle_; 389 | 390 | const float distSq = distSqPointLineSegment(obstacle->point_, nextObstacle->point_, position_); 391 | 392 | if (distSq < rangeSq) { 393 | obstacleNeighbors_.push_back(std::make_pair(distSq, obstacle)); 394 | 395 | size_t i = obstacleNeighbors_.size() - 1; 396 | 397 | while (i != 0 && distSq < obstacleNeighbors_[i - 1].first) { 398 | obstacleNeighbors_[i] = obstacleNeighbors_[i - 1]; 399 | --i; 400 | } 401 | 402 | obstacleNeighbors_[i] = std::make_pair(distSq, obstacle); 403 | } 404 | } 405 | 406 | void Agent::update() 407 | { 408 | velocity_ = newVelocity_; 409 | position_ += velocity_ * sim_->timeStep_; 410 | } 411 | 412 | void Agent::update_gazebo(Vector2 position_gazebo) 413 | { 414 | position_ = position_gazebo; 415 | } 416 | 417 | bool linearProgram1(const std::vector &lines, size_t lineNo, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result) 418 | { 419 | const float dotProduct = lines[lineNo].point * lines[lineNo].direction; 420 | const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(lines[lineNo].point); 421 | 422 | if (discriminant < 0.0f) { 423 | /* Max speed circle fully invalidates line lineNo. */ 424 | return false; 425 | } 426 | 427 | const float sqrtDiscriminant = std::sqrt(discriminant); 428 | float tLeft = -dotProduct - sqrtDiscriminant; 429 | float tRight = -dotProduct + sqrtDiscriminant; 430 | 431 | for (size_t i = 0; i < lineNo; ++i) { 432 | const float denominator = det(lines[lineNo].direction, lines[i].direction); 433 | const float numerator = det(lines[i].direction, lines[lineNo].point - lines[i].point); 434 | 435 | if (std::fabs(denominator) <= RVO_EPSILON) { 436 | /* Lines lineNo and i are (almost) parallel. */ 437 | if (numerator < 0.0f) { 438 | return false; 439 | } 440 | else { 441 | continue; 442 | } 443 | } 444 | 445 | const float t = numerator / denominator; 446 | 447 | if (denominator >= 0.0f) { 448 | /* Line i bounds line lineNo on the right. */ 449 | tRight = std::min(tRight, t); 450 | } 451 | else { 452 | /* Line i bounds line lineNo on the left. */ 453 | tLeft = std::max(tLeft, t); 454 | } 455 | 456 | if (tLeft > tRight) { 457 | return false; 458 | } 459 | } 460 | 461 | if (directionOpt) { 462 | /* Optimize direction. */ 463 | if (optVelocity * lines[lineNo].direction > 0.0f) { 464 | /* Take right extreme. */ 465 | result = lines[lineNo].point + tRight * lines[lineNo].direction; 466 | } 467 | else { 468 | /* Take left extreme. */ 469 | result = lines[lineNo].point + tLeft * lines[lineNo].direction; 470 | } 471 | } 472 | else { 473 | /* Optimize closest point. */ 474 | const float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point); 475 | 476 | if (t < tLeft) { 477 | result = lines[lineNo].point + tLeft * lines[lineNo].direction; 478 | } 479 | else if (t > tRight) { 480 | result = lines[lineNo].point + tRight * lines[lineNo].direction; 481 | } 482 | else { 483 | result = lines[lineNo].point + t * lines[lineNo].direction; 484 | } 485 | } 486 | 487 | return true; 488 | } 489 | 490 | size_t linearProgram2(const std::vector &lines, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result) 491 | { 492 | if (directionOpt) { 493 | /* 494 | * Optimize direction. Note that the optimization velocity is of unit 495 | * length in this case. 496 | */ 497 | result = optVelocity * radius; 498 | } 499 | else if (absSq(optVelocity) > sqr(radius)) { 500 | /* Optimize closest point and outside circle. */ 501 | result = normalize(optVelocity) * radius; 502 | } 503 | else { 504 | /* Optimize closest point and inside circle. */ 505 | result = optVelocity; 506 | } 507 | 508 | for (size_t i = 0; i < lines.size(); ++i) { 509 | if (det(lines[i].direction, lines[i].point - result) > 0.0f) { 510 | /* Result does not satisfy constraint i. Compute new optimal result. */ 511 | const Vector2 tempResult = result; 512 | 513 | if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) { 514 | result = tempResult; 515 | return i; 516 | } 517 | } 518 | } 519 | 520 | return lines.size(); 521 | } 522 | 523 | void linearProgram3(const std::vector &lines, size_t numObstLines, size_t beginLine, float radius, Vector2 &result) 524 | { 525 | float distance = 0.0f; 526 | 527 | for (size_t i = beginLine; i < lines.size(); ++i) { 528 | if (det(lines[i].direction, lines[i].point - result) > distance) { 529 | /* Result does not satisfy constraint of line i. */ 530 | std::vector projLines(lines.begin(), lines.begin() + static_cast(numObstLines)); 531 | 532 | for (size_t j = numObstLines; j < i; ++j) { 533 | Line line; 534 | 535 | float determinant = det(lines[i].direction, lines[j].direction); 536 | 537 | if (std::fabs(determinant) <= RVO_EPSILON) { 538 | /* Line i and line j are parallel. */ 539 | if (lines[i].direction * lines[j].direction > 0.0f) { 540 | /* Line i and line j point in the same direction. */ 541 | continue; 542 | } 543 | else { 544 | /* Line i and line j point in opposite direction. */ 545 | line.point = 0.5f * (lines[i].point + lines[j].point); 546 | } 547 | } 548 | else { 549 | line.point = lines[i].point + (det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction; 550 | } 551 | 552 | line.direction = normalize(lines[j].direction - lines[i].direction); 553 | projLines.push_back(line); 554 | } 555 | 556 | const Vector2 tempResult = result; 557 | 558 | if (linearProgram2(projLines, radius, Vector2(-lines[i].direction.y(), lines[i].direction.x()), true, result) < projLines.size()) { 559 | /* This should in principle not happen. The result is by definition 560 | * already in the feasible region of this linear program. If it fails, 561 | * it is due to small floating point error, and the current result is 562 | * kept. 563 | */ 564 | result = tempResult; 565 | } 566 | 567 | distance = det(lines[i].direction, lines[i].point - result); 568 | } 569 | } 570 | } 571 | } 572 | -------------------------------------------------------------------------------- /rvo_lib/RVO.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RVO.h 3 | * RVO2 Library 4 | * 5 | * Copyright 2008 University of North Carolina at Chapel Hill 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * Please send all bug reports to . 20 | * 21 | * The authors may be contacted via: 22 | * 23 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 24 | * Dept. of Computer Science 25 | * 201 S. Columbia St. 26 | * Frederick P. Brooks, Jr. Computer Science Bldg. 27 | * Chapel Hill, N.C. 27599-3175 28 | * United States of America 29 | * 30 | * 31 | */ 32 | 33 | #ifndef RVO_RVO_H_ 34 | #define RVO_RVO_H_ 35 | 36 | #include "RVOSimulator.h" 37 | #include "Vector2.h" 38 | 39 | /** 40 | 41 | \file RVO.h 42 | \brief Includes all public headers in the library. 43 | 44 | \namespace RVO 45 | \brief Contains all classes, functions, and constants used in the library. 46 | 47 | \mainpage RVO2 Library 48 | 49 | \author Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, and 50 | Dinesh Manocha 51 | 52 | RVO2 Library is an easy-to-use C++ implementation of the 53 | Optimal Reciprocal Collision Avoidance 54 | (ORCA) formulation for multi-agent simulation. RVO2 Library automatically 55 | uses parallelism for computing the motion of the agents if your machine has 56 | multiple processors and your compiler supports 57 | OpenMP. 58 | 59 | Please follow the following steps to install and use RVO2 Library. 60 | 61 | - \subpage whatsnew 62 | - \subpage building 63 | - \subpage using 64 | - \subpage params 65 | 66 | See the documentation of the RVO::RVOSimulator class for an exhaustive list of 67 | public functions of RVO2 Library. 68 | 69 | RVO2 Library, accompanying example code, and this documentation is 70 | released for educational, research, and non-profit purposes under the following 71 | \subpage terms "terms and conditions". 72 | 73 | 74 | \page whatsnew What Is New in RVO2 Library 75 | 76 | \section localca Local Collision Avoidance 77 | 78 | The main difference between RVO2 Library and %RVO Library 1.x is the 79 | local collision avoidance technique used. RVO2 Library uses 80 | Optimal Reciprocal Collision Avoidance 81 | (ORCA), whereas %RVO Library 1.x uses 82 | Reciprocal Velocity Obstacles (%RVO). For legacy reasons, and since both 83 | techniques are based on the same principles of reciprocal collision avoidance 84 | and relative velocity, we did not change the name of the library. 85 | 86 | A major consequence of the change of local collision avoidance technique is that 87 | the simulation has become much faster in RVO2 Library. ORCA defines 88 | velocity constraints with respect to other agents as half-planes, and an optimal 89 | velocity is efficiently found using (two-dimensional) linear programming. In 90 | contrast, %RVO Library 1.x uses random sampling to find a good velocity. Also, 91 | the behavior of the agents is smoother in RVO2 Library. It is proven 92 | mathematically that ORCA lets the velocity of agents evolve continuously over 93 | time, whereas %RVO Library 1.x occasionally showed oscillations and reciprocal 94 | dances. Furthermore, ORCA provides stronger guarantees with respect to collision 95 | avoidance. 96 | 97 | \section global Global Path Planning 98 | 99 | Local collision avoidance as provided by RVO2 Library should in principle 100 | be accompanied by global path planning that determines the preferred velocity of 101 | each agent in each time step of the simulation. %RVO Library 1.x has a built-in 102 | roadmap infrastructure to guide agents around obstacles to fixed goals. 103 | However, besides roadmaps, other techniques for global planning, such as 104 | navigation fields, cell decompositions, etc. exist. Therefore, RVO2 105 | Library does not provide global planning infrastructure anymore. Instead, 106 | it is the responsibility of the external application to set the preferred 107 | velocity of each agent ahead of each time step of the simulation. This makes the 108 | library more flexible to use in varying application domains. In one of the 109 | example applications that comes with RVO2 Library, we show how a roadmap 110 | similar to %RVO Library 1.x is used externally to guide the global navigation of 111 | the agents. As a consequence of this change, RVO2 Library does not have a 112 | concept of a "goal position" or "preferred speed" for each 113 | agent, but only relies on the preferred velocities of the agents set by the 114 | external application. 115 | 116 | \section structure Structure of RVO2 Library 117 | 118 | The structure of RVO2 Library is similar to that of %RVO Library 1.x. 119 | Users familiar with %RVO Library 1.x should find little trouble in using RVO2 120 | Library. However, RVO2 Library is not backwards compatible with %RVO 121 | Library 1.x. The main reason for this is that the ORCA technique requires 122 | different (and fewer) parameters to be set than %RVO. Also, the way obstacles 123 | are represented is different. In %RVO Library 1.x, obstacles are represented by 124 | an arbitrary collection of line segments. In RVO2 Library, obstacles are 125 | non-intersecting polygons, specified by lists of vertices in counterclockwise 126 | order. Further, in %RVO Library 1.x agents cannot be added to the simulation 127 | after the simulation is initialized. In RVO2 Library this restriction is 128 | removed. Obstacles still need to be processed before the simulation starts, 129 | though. Lastly, in %RVO Library 1.x an instance of the simulator is a singleton. 130 | This restriction is removed in RVO2 Library. 131 | 132 | \section smaller Smaller Changes 133 | 134 | With RVO2 Library, we have adopted the philosophy that anything that is 135 | not part of the core local collision avoidance technique is to be stripped from 136 | the library. Therefore, besides the roadmap infrastructure, we have also removed 137 | acceleration constraints of agents, orientation of agents, and the unused 138 | "class" of agents. Each of these can be implemented external of the 139 | library if needed. We did maintain a kd-tree infrastructure for 140 | efficiently finding other agents and obstacles nearby each agent. 141 | 142 | Also, RVO2 Library allows accessing information about the simulation, 143 | such as the neighbors and the collision-avoidance constraints of each agent, 144 | that is hidden from the user in %RVO Library 1.x. 145 | 146 | 147 | \page building Building RVO2 Library 148 | 149 | We assume that you have downloaded RVO2 Library and unpacked the ZIP 150 | archive to a path $RVO_ROOT. 151 | 152 | \section xcode Apple Xcode 3.x 153 | 154 | Open $RVO_ROOT/RVO.xcodeproj and select the %RVO target and 155 | a configuration (Debug or Release). A framework 156 | RVO.framework will be built in the default build directory, e.g. 157 | $RVO_ROOT/build/Release. 158 | 159 | \section cmake CMake 160 | 161 | Create and switch to your chosen build directory, e.g. $RVO_ROOT/build. 162 | Run cmake inside the build directory on the source directory, e.g. 163 | cmake $RVO_ROOT/src. Build files for the default generator for your 164 | platform will be generated in the build directory. 165 | 166 | \section make GNU Make 167 | 168 | Switch to the source directory $RVO_ROOT/src and run make. 169 | Public header files (RVO.h, RVOSimulator.h, and 170 | Vector2.h) will be copied to the $RVO_ROOT/include directory 171 | and a static library libRVO.a will be compiled into the 172 | $RVO_ROOT/lib directory. 173 | 174 | \section visual Microsoft Visual Studio 2008 175 | 176 | Open $RVO_ROOT/RVO.sln and select the %RVO project and a 177 | configuration (Debug, ReleaseST, or ReleaseMT). 178 | Public header files (RVO.h, RVOSimulator.h, and 179 | Vector2.h) will be copied to the $RVO_ROOT/include directory 180 | and a static library, e.g. RVO.lib, will be compiled into the 181 | $RVO_ROOT/lib directory. 182 | 183 | 184 | \page using Using RVO2 Library 185 | 186 | \section structure Structure 187 | 188 | A program performing an RVO2 Library simulation has the following global 189 | structure. 190 | 191 | \code 192 | #include 193 | 194 | std::vector goals; 195 | 196 | int main() 197 | { 198 | // Create a new simulator instance. 199 | RVO::RVOSimulator* sim = new RVO::RVOSimulator(); 200 | 201 | // Set up the scenario. 202 | setupScenario(sim); 203 | 204 | // Perform (and manipulate) the simulation. 205 | do { 206 | updateVisualization(sim); 207 | setPreferredVelocities(sim); 208 | sim->doStep(); 209 | } while (!reachedGoal(sim)); 210 | 211 | delete sim; 212 | } 213 | \endcode 214 | 215 | In order to use RVO2 Library, the user needs to include RVO.h. The first 216 | step is then to create an instance of RVO::RVOSimulator. Then, the process 217 | consists of two stages. The first stage is specifying the simulation scenario 218 | and its parameters. In the above example program, this is done in the method 219 | setupScenario(...), which we will discuss below. The second stage is the actual 220 | performing of the simulation. 221 | 222 | In the above example program, simulation steps are taken until all 223 | the agents have reached some predefined goals. Prior to each simulation step, 224 | we set the preferred velocity for each agent, i.e. the 225 | velocity the agent would have taken if there were no other agents around, in the 226 | method setPreferredVelocities(...). The simulator computes the actual velocities 227 | of the agents and attempts to follow the preferred velocities as closely as 228 | possible while guaranteeing collision avoidance at the same time. During the 229 | simulation, the user may want to retrieve information from the simulation for 230 | instance to visualize the simulation. In the above example program, this is done 231 | in the method updateVisualization(...), which we will discuss below. It is also 232 | possible to manipulate the simulation during the simulation, for instance by 233 | changing positions, radii, velocities, etc. of the agents. 234 | 235 | \section spec Setting up the Simulation Scenario 236 | 237 | A scenario that is to be simulated can be set up as follows. A scenario consists 238 | of two types of objects: agents and obstacles. Each of them can be manually 239 | specified. Agents may be added anytime before or during the simulation. 240 | Obstacles, however, need to be defined prior to the simulation, and 241 | RVO::RVOSimulator::processObstacles() need to be called in order for the 242 | obstacles to be accounted for in the simulation. 243 | The user may also want to define goal positions of the agents, or a 244 | roadmap to guide the agents around obstacles. This is not done in RVO2 245 | Library, but needs to be taken care of in the user's external application. 246 | 247 | The following example creates a scenario with four agents exchanging positions 248 | around a rectangular obstacle in the middle. 249 | 250 | \code 251 | void setupScenario(RVO::RVOSimulator* sim) { 252 | // Specify global time step of the simulation. 253 | sim->setTimeStep(0.25f); 254 | 255 | // Specify default parameters for agents that are subsequently added. 256 | sim->setAgentDefaults(15.0f, 10, 10.0f, 5.0f, 2.0f, 2.0f); 257 | 258 | // Add agents, specifying their start position. 259 | sim->addAgent(RVO::Vector2(-50.0f, -50.0f)); 260 | sim->addAgent(RVO::Vector2(50.0f, -50.0f)); 261 | sim->addAgent(RVO::Vector2(50.0f, 50.0f)); 262 | sim->addAgent(RVO::Vector2(-50.0f, 50.0f)); 263 | 264 | // Create goals (simulator is unaware of these). 265 | for (size_t i = 0; i < sim->getNumAgents(); ++i) { 266 | goals.push_back(-sim->getAgentPosition(i)); 267 | } 268 | 269 | // Add (polygonal) obstacle(s), specifying vertices in counterclockwise order. 270 | std::vector vertices; 271 | vertices.push_back(RVO::Vector2(-7.0f, -20.0f)); 272 | vertices.push_back(RVO::Vector2(7.0f, -20.0f)); 273 | vertices.push_back(RVO::Vector2(7.0f, 20.0f)); 274 | vertices.push_back(RVO::Vector2(-7.0f, 20.0f)); 275 | 276 | sim->addObstacle(vertices); 277 | 278 | // Process obstacles so that they are accounted for in the simulation. 279 | sim->processObstacles(); 280 | } 281 | \endcode 282 | 283 | See the documentation on RVO::RVOSimulator for a full overview of the 284 | functionality to specify scenarios. 285 | 286 | \section ret Retrieving Information from the Simulation 287 | 288 | During the simulation, the user can extract information from the simulation for 289 | instance for visualization purposes, or to determine termination conditions of 290 | the simulation. In the example program above, visualization is done in the 291 | updateVisualization(...) method. Below we give an example that simply writes 292 | the positions of each agent in each time step to the standard output. The 293 | termination condition is checked in the reachedGoal(...) method. Here we give an 294 | example that returns true if all agents are within one radius of their goals. 295 | 296 | \code 297 | void updateVisualization(RVO::RVOSimulator* sim) { 298 | // Output the current global time. 299 | std::cout << sim->getGlobalTime() << " "; 300 | 301 | // Output the position for all the agents. 302 | for (size_t i = 0; i < sim->getNumAgents(); ++i) { 303 | std::cout << sim->getAgentPosition(i) << " "; 304 | } 305 | 306 | std::cout << std::endl; 307 | } 308 | \endcode 309 | 310 | \code 311 | bool reachedGoal(RVO::RVOSimulator* sim) { 312 | // Check whether all agents have arrived at their goals. 313 | for (size_t i = 0; i < sim->getNumAgents(); ++i) { 314 | if (absSq(goals[i] - sim->getAgentPosition(i)) > sim->getAgentRadius(i) * sim->getAgentRadius(i)) { 315 | // Agent is further away from its goal than one radius. 316 | return false; 317 | } 318 | } 319 | return true; 320 | } 321 | \endcode 322 | 323 | Using similar functions as the ones used in this example, the user can access 324 | information about other parameters of the agents, as well as the global 325 | parameters, and the obstacles. See the documentation of the class 326 | RVO::RVOSimulator for an exhaustive list of public functions for retrieving 327 | simulation information. 328 | 329 | \section manip Manipulating the Simulation 330 | 331 | During the simulation, the user can manipulate the simulation, for instance by 332 | changing the global parameters, or changing the parameters of the agents 333 | (potentially causing abrupt different behavior). It is also possible to give the 334 | agents a new position, which make them jump through the scene. 335 | New agents can be added to the simulation at any time, but it is not allowed to 336 | add obstacles to the simulation after they have been processed by calling 337 | RVO::RVOSimulator::processObstacles(). Also, it is impossible to change the 338 | position of the vertices of the obstacles. 339 | 340 | See the documentation of the class RVO::RVOSimulator for an exhaustive list of 341 | public functions for manipulating the simulation. 342 | 343 | To provide global guidance to the agents, the preferred velocities of the agents 344 | can be changed ahead of each simulation step. In the above example program, this 345 | happens in the method setPreferredVelocities(...). Here we give an example that 346 | simply sets the preferred velocity to the unit vector towards the agent's goal 347 | for each agent (i.e., the preferred speed is 1.0). Note that this may not give 348 | convincing results with respect to global navigation around the obstacles. For 349 | this a roadmap or other global planning techniques may be used (see one of the 350 | \ref example "example programs" that accompanies RVO2 Library). 351 | 352 | \code 353 | void setPreferredVelocities(RVO::RVOSimulator* sim) { 354 | // Set the preferred velocity for each agent. 355 | for (size_t i = 0; i < sim->getNumAgents(); ++i) { 356 | if (absSq(goals[i] - sim->getAgentPosition(i)) < sim->getAgentRadius(i) * sim->getAgentRadius(i) ) { 357 | // Agent is within one radius of its goal, set preferred velocity to zero 358 | sim->setAgentPrefVelocity(i, RVO::Vector2(0.0f, 0.0f)); 359 | } else { 360 | // Agent is far away from its goal, set preferred velocity as unit vector towards agent's goal. 361 | sim->setAgentPrefVelocity(i, normalize(goals[i] - sim->getAgentPosition(i))); 362 | } 363 | } 364 | } 365 | \endcode 366 | 367 | \section example Example Programs 368 | 369 | RVO2 Library is accompanied by three example programs, which can be found in the 370 | $RVO_ROOT/examples directory. The examples are named Blocks, Circle, and 371 | Roadmap, and contain the following demonstration scenarios: 372 | 373 | 374 | 375 | 379 | 380 | 381 | 382 | 385 | 386 | 387 | 388 | 390 | 391 |
BlocksA scenario in which 100 agents, split in four groups initially 376 | positioned in each of four corners of the environment, move to the 377 | other side of the environment through a narrow passage generated by four 378 | obstacles. There is no roadmap to guide the agents around the obstacles.
CircleA scenario in which 250 agents, initially positioned evenly 383 | distributed on a circle, move to the antipodal position on the 384 | circle. There are no obstacles.
RoadmapThe same scenario as ExampleBlocks, but now the preferred velocities 389 | of the agents are determined using a roadmap guiding the agents around the obstacles.
392 | 393 | 394 | \page params Parameter Overview 395 | 396 | \section globalp Global Parameters 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 |
ParameterType (unit)Meaning
timeStepfloat (time)The time step of the simulation. Must be positive.
410 | 411 | \section agent Agent Parameters 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | 448 | 450 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 471 | 472 | 473 | 474 | 475 | 481 | 482 | 483 | 484 | 486 | 487 | 488 |
ParameterType (unit)Meaning
maxNeighborssize_tThe maximum number of other agents the agent takes into 423 | account in the navigation. The larger this number, the 424 | longer the running time of the simulation. If the number is 425 | 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 436 | other agents the agent takes into account in the 437 | navigation. The larger this number, the longer the running 438 | time of the simulation. If the number is too low, the 439 | 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) 449 | The current preferred velocity of the agent. This is the 451 | velocity the agent would take if no other agents or 452 | obstacles were around. The simulator computes an actual 453 | velocity for the agent that follows the preferred velocity 454 | as closely as possible, but at the same time guarantees 455 | 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 466 | that are computed by the simulation are safe with respect 467 | to other agents. The larger this number, the sooner this 468 | agent will respond to the presence of other agents, but the 469 | less freedom the agent has in choosing its velocities. 470 | Must be positive.
timeHorizonObstfloat (time)The minimal amount of time for which the agent's velocities 476 | that are computed by the simulation are safe with respect 477 | to obstacles. The larger this number, the sooner this agent 478 | will respond to the presence of obstacles, but the less 479 | freedom the agent has in choosing its velocities. 480 | Must be positive.
velocityRVO::Vector2 (distance/time, distance/time) 485 | The (current) velocity of the agent.
489 | 490 | 491 | \page terms Terms and Conditions 492 | 493 | RVO2 Library 494 | 495 | Copyright 2008 University of North Carolina at Chapel Hill 496 | 497 | Licensed under the Apache License, Version 2.0 (the "License"); 498 | you may not use this file except in compliance with the License. 499 | You may obtain a copy of the License at 500 | 501 | http://www.apache.org/licenses/LICENSE-2.0 502 | 503 | Unless required by applicable law or agreed to in writing, software 504 | distributed under the License is distributed on an "AS IS" BASIS, 505 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 506 | See the License for the specific language governing permissions and 507 | limitations under the License. 508 | 509 | */ 510 | 511 | #endif /* RVO_RVO_H_ */ 512 | -------------------------------------------------------------------------------- /simulation/worlds/multi_agents6.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.1 -0.9 15 | 16 | 17 | 1 18 | 19 | 20 | 21 | 22 | 0 0 1 23 | 100 100 24 | 25 | 26 | 27 | 28 | 29 | 100 30 | 50 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 10 42 | 43 | 44 | 0 45 | 46 | 47 | 0 0 1 48 | 100 100 49 | 50 | 51 | 52 | 56 | 57 | 58 | 0 59 | 0 60 | 0 61 | 62 | 63 | 0 0 -9.8 64 | 6e-06 2.3e-05 -4.2e-05 65 | 66 | 67 | 0.001 68 | 1 69 | 1000 70 | 71 | 72 | 0.4 0.4 0.4 1 73 | 0.7 0.7 0.7 1 74 | 1 75 | 76 | 79 | 80 | 81 | EARTH_WGS84 82 | 0 83 | 0 84 | 0 85 | 0 86 | 87 | 88 | 89 | 90 | 1 91 | 92 | 0 93 | 0 94 | 0 95 | 0 96 | 0 97 | 0 98 | 99 | 0 0 0 0 -0 0 100 | 101 | 0 0 0 0 -0 0 102 | 0 103 | 0 104 | 0 105 | 106 | 0 0 0 0 -0 0 107 | 108 | 109 | 0.2 110 | 0.4 111 | 112 | 113 | 114 | 1 115 | 119 | 120 | 0 121 | 1 122 | 123 | 124 | 0 125 | 10 126 | 0 0 0 0 -0 0 127 | 128 | 129 | 0.2 130 | 0.4 131 | 132 | 133 | 134 | 135 | 136 | 0 137 | 1 138 | 0 0 0 139 | 0 140 | 0 141 | 142 | 143 | 1 144 | 0 145 | 0 146 | 1 147 | 148 | 0 149 | 150 | 151 | 152 | 153 | 0 154 | 1e+06 155 | 156 | 157 | 0 158 | 1 159 | 1 160 | 161 | 0 162 | 0.2 163 | 1e+13 164 | 1 165 | 0.01 166 | 0 167 | 168 | 169 | 1 170 | -0.01 171 | 0 172 | 0.2 173 | 1e+13 174 | 1 175 | 176 | 177 | 178 | 179 | 1 180 | 181 | 0 182 | 1 183 | 184 | 3 1 0 0 -0 0 185 | 186 | 187 | 188 | 189 | 1 190 | 191 | 0 192 | 0 193 | 0 194 | 0 195 | 0 196 | 0 197 | 198 | 0 0 0 0 -0 0 199 | 200 | 0 0 0 0 -0 0 201 | 0 202 | 0 203 | 0 204 | 205 | 0 0 0 0 -0 0 206 | 207 | 208 | 0.2 209 | 0.4 210 | 211 | 212 | 213 | 1 214 | 218 | 219 | 0 220 | 1 221 | 222 | 223 | 0 224 | 10 225 | 0 0 0 0 -0 0 226 | 227 | 228 | 0.2 229 | 0.4 230 | 231 | 232 | 233 | 234 | 235 | 0 236 | 1 237 | 0 0 0 238 | 0 239 | 0 240 | 241 | 242 | 1 243 | 0 244 | 0 245 | 1 246 | 247 | 0 248 | 249 | 250 | 251 | 252 | 0 253 | 1e+06 254 | 255 | 256 | 0 257 | 1 258 | 1 259 | 260 | 0 261 | 0.2 262 | 1e+13 263 | 1 264 | 0.01 265 | 0 266 | 267 | 268 | 1 269 | -0.01 270 | 0 271 | 0.2 272 | 1e+13 273 | 1 274 | 275 | 276 | 277 | 278 | 1 279 | 280 | 0 281 | 1 282 | 283 | 2 1 0 0 -0 0 284 | 285 | 286 | 287 | 288 | 1 289 | 290 | 0 291 | 0 292 | 0 293 | 0 294 | 0 295 | 0 296 | 297 | 0 0 0 0 -0 0 298 | 299 | 0 0 0 0 -0 0 300 | 0 301 | 0 302 | 0 303 | 304 | 0 0 0 0 -0 0 305 | 306 | 307 | 0.2 308 | 0.4 309 | 310 | 311 | 312 | 1 313 | 317 | 318 | 0 319 | 1 320 | 321 | 322 | 0 323 | 10 324 | 0 0 0 0 -0 0 325 | 326 | 327 | 0.2 328 | 0.4 329 | 330 | 331 | 332 | 333 | 334 | 0 335 | 1 336 | 0 0 0 337 | 0 338 | 0 339 | 340 | 341 | 1 342 | 0 343 | 0 344 | 1 345 | 346 | 0 347 | 348 | 349 | 350 | 351 | 0 352 | 1e+06 353 | 354 | 355 | 0 356 | 1 357 | 1 358 | 359 | 0 360 | 0.2 361 | 1e+13 362 | 1 363 | 0.01 364 | 0 365 | 366 | 367 | 1 368 | -0.01 369 | 0 370 | 0.2 371 | 1e+13 372 | 1 373 | 374 | 375 | 376 | 377 | 1 378 | 379 | 0 380 | 1 381 | 382 | 4 1 0 0 -0 0 383 | 384 | 385 | 386 | 387 | 388 | 1 389 | 390 | 0 391 | 0 392 | 0 393 | 0 394 | 0 395 | 0 396 | 397 | 0 0 0 0 -0 0 398 | 399 | 0 0 0 0 -0 0 400 | 0 401 | 0 402 | 0 403 | 404 | 0 0 0 0 -0 0 405 | 406 | 407 | 0.2 408 | 0.4 409 | 410 | 411 | 412 | 1 413 | 417 | 418 | 0 419 | 1 420 | 421 | 422 | 0 423 | 10 424 | 0 0 0 0 -0 0 425 | 426 | 427 | 0.2 428 | 0.4 429 | 430 | 431 | 432 | 433 | 434 | 0 435 | 1 436 | 0 0 0 437 | 0 438 | 0 439 | 440 | 441 | 1 442 | 0 443 | 0 444 | 1 445 | 446 | 0 447 | 448 | 449 | 450 | 451 | 0 452 | 1e+06 453 | 454 | 455 | 0 456 | 1 457 | 1 458 | 459 | 0 460 | 0.2 461 | 1e+13 462 | 1 463 | 0.01 464 | 0 465 | 466 | 467 | 1 468 | -0.01 469 | 0 470 | 0.2 471 | 1e+13 472 | 1 473 | 474 | 475 | 476 | 477 | 1 478 | 479 | 0 480 | 1 481 | 482 | 1 1 0 0 -0 0 483 | 484 | 485 | 1854 798000000 486 | 207 624069799 487 | 1576240629 817930782 488 | 206536 489 | 490 | 491 | 492 | 0 0 0 0 -0 0 493 | 1 1 1 494 | 495 | 0 0 0 0 -0 0 496 | 0 0 0 0 -0 0 497 | 0 0 0 0 -0 0 498 | 0 0 0 0 -0 0 499 | 500 | 501 | 502 | 0 0 10 0 -0 0 503 | 504 | 505 | 506 | 507 | 2.71908 -1.34602 9.38861 -0 1.05963 1.4362 508 | orbit 509 | perspective 510 | 511 | 512 | 513 | 514 | 515 | 1 516 | 517 | 0 518 | 0 519 | 0 520 | 0 521 | 0 522 | 0 523 | 524 | 0 0 0 0 -0 0 525 | 526 | 0 0 0 0 -0 0 527 | 0 528 | 0 529 | 0 530 | 531 | 0 0 0 0 -0 0 532 | 533 | 534 | 0.2 535 | 0.4 536 | 537 | 538 | 539 | 1 540 | 544 | 545 | 0 546 | 1 547 | 548 | 549 | 0 550 | 10 551 | 0 0 0 0 -0 0 552 | 553 | 554 | 0.2 555 | 0.4 556 | 557 | 558 | 559 | 560 | 561 | 0 562 | 1 563 | 0 0 0 564 | 0 565 | 0 566 | 567 | 568 | 1 569 | 0 570 | 0 571 | 1 572 | 573 | 0 574 | 575 | 576 | 577 | 578 | 0 579 | 1e+06 580 | 581 | 582 | 0 583 | 1 584 | 1 585 | 586 | 0 587 | 0.2 588 | 1e+13 589 | 1 590 | 0.01 591 | 0 592 | 593 | 594 | 1 595 | -0.01 596 | 0 597 | 0.2 598 | 1e+13 599 | 1 600 | 601 | 602 | 603 | 604 | 1 605 | 606 | 0 607 | 1 608 | 609 | 5 1 0 0 -0 0 610 | 611 | 612 | 613 | 614 | 1 615 | 616 | 0 617 | 0 618 | 0 619 | 0 620 | 0 621 | 0 622 | 623 | 0 0 0 0 -0 0 624 | 625 | 0 0 0 0 -0 0 626 | 0 627 | 0 628 | 0 629 | 630 | 0 0 0 0 -0 0 631 | 632 | 633 | 0.2 634 | 0.4 635 | 636 | 637 | 638 | 1 639 | 643 | 644 | 0 645 | 1 646 | 647 | 648 | 0 649 | 10 650 | 0 0 0 0 -0 0 651 | 652 | 653 | 0.2 654 | 0.4 655 | 656 | 657 | 658 | 659 | 660 | 0 661 | 1 662 | 0 0 0 663 | 0 664 | 0 665 | 666 | 667 | 1 668 | 0 669 | 0 670 | 1 671 | 672 | 0 673 | 674 | 675 | 676 | 677 | 0 678 | 1e+06 679 | 680 | 681 | 0 682 | 1 683 | 1 684 | 685 | 0 686 | 0.2 687 | 1e+13 688 | 1 689 | 0.01 690 | 0 691 | 692 | 693 | 1 694 | -0.01 695 | 0 696 | 0.2 697 | 1e+13 698 | 1 699 | 700 | 701 | 702 | 703 | 1 704 | 705 | 0 706 | 1 707 | 708 | 6 1 0 0 -0 0 709 | 710 | 711 | 712 | --------------------------------------------------------------------------------